五杆機構正運動學公式、逆運動學公式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的角度值單位°。