五杆机构正运动学公式、逆运动学公式MATLAB代码

在研究五杆机构的时候感觉网上的资料更多的在研究正运动学，包括机构顶端的运动轨迹、速度、加速度，缺少逆运动学的分析。当我们想用五杆机构做实际用途比如3D打印或轨迹描绘的时候逆运动学公式是必不可少的。所以我觉得写一篇汇总正逆运动学的文章还是有必要的。

一、正运动学：

正运动学公式推导主要参考这篇文章：平面五杆机构运动学和动力学特性分析

MATLAB正运动学代码：

function [xc,yc] = F(u1,u4,l1,l2,l3,l4,l5)
xb = l1 * cos(u1);
yb = l1 * sin(u1);
xd = l5 + l4 * cos(u4);
yd = l4 * sin(u4);
lbd = sqrt((xd - xb).^2 + (yd - yb).^2);
A0 = 2 * l2 * (xd - xb);
B0 = 2 * l2 * (yd - yb);
C0 = l2.^2 + lbd.^2 - l3.^2;
u2 = 2 * atan((B0 + sqrt(A0.^2 + B0.^2 - C0.^2))/(A0 + C0));
xc = xb + l2 * cos(u2);
yc = yb + l2 * sin(u2);
end

其中l1、l2、l3、l4、l5长度分别对应图1中的相应标号的杆子的长度。u1、u4就是φ1、φ4单位°，正运动学根据给定的7个参数求出C点相对于坐标原点（A点）的X\Y信息。

二、逆运动学公式

逆运动学主要参考David Tavkhelidze的PPT，由于文章被墙，这里给个百度云链接，或者博客下载地址。

主要逆运动学公式在PPT里面的公式14、15、17、18.

MATLAB逆运动学代码：

function [thta1,thta2] = inverseF(x,y,l1,l2,l3,l4,l0)
Xc = x; %Xc∈（35,80）
Yc = y; %Yc∈（75,115）
cosfoai_12 = (Xc.^2 + Yc.^2 - l1.^2 - l2.^2)/(2 * l1 * l2);
%foai_12∈（pi，2pi）
foai_12 = 2*pi - acos(cosfoai_12);
cosfoai_01 = (l2*Yc*sin(foai_12)+Xc*(l2*cos(foai_12)+l1))/((l2*cos(foai_12)+l1).^2+l2.^2*(sin(foai_12)).^2);
%foai_01∈（20,180）
foai_01 = acos(cosfoai_01);

cosfoai_34 = ((Xc-l0).^2+Yc.^2-l3.^2-l4.^2)/(2*l3*l4);
%foai_34∈（pi,2pi）
foai_34 = 2*pi - acos(cosfoai_34);
A = l0 - Xc;
B = l3 * sin(foai_34);
C = l4 + l3*cos(foai_34);
%foai_t = asin(C/sqrt(B.^2+C.^2));
foai_t = acos(B/sqrt(B.^2+C.^2));
%foai_40∈（0,pi）
foai_40 = foai_t - asin(A/sqrt(B.^2+C.^2));
thta1 = foai_01/pi*180;
thta2 = 180 - foai_40/pi*180;
thta1 = thta1/180*pi;
thta2 = thta2/180*pi;
end

l0代表l5,l1、l2、l3、l4同上，X\Y为C点相对于坐标原点（A点）的位置，返回的参数thta1，thta2为上面φ1，φ4的角度值单位°。