四足機器人：閉鏈五杆腿結構運動學分析

一、背景

在四足機器人的發展中，由於串聯腿結構對關節驅動電機的要求很高，控制精度難以保證，因此逐漸出現了使用閉鏈五杆機構作爲機器人腿結構的四足機器人，如賓夕法尼亞大學的Minitaur，斯坦福的開源機器狗Stanford Doggo，以及在2019年全國大學生機器人大賽Robocon賽事中以武漢大學的四足機器人爲代表的一系列高校所採取的並聯五杆結構四足機器人。

本文針對這一類機器人所採用的閉鏈五杆機構，對其運動學進行了簡要的分析，並給出了其運動學的正反解過程，以及相應的Matlab源碼。

二、建立數學模型

建立閉鏈五杆機構簡化數學模型如圖所示，其中OA 、AB 、BC 、CD、 DO、BE 長度分別爲$L_1$、$L_2$、$L_3$、$L_4$、$L_5$、$L_6$，DO爲機架，O、D 兩點爲驅動。

三、正解

由圖1不難得到

由

得

又由

可得

由余弦定理可得

即

其中

代入可得

令

即上式變爲

帶入萬能公式可得

解得

又由

得

四、逆解

由圖1不難得到

又由

可得

消去$\theta$可得

令

即上式化爲

解得

將上式帶入

解得

所以

由

得

又由

可得

消去$\gamma$得

令

上式變爲

解得

即

五、Matlab源碼

1.正解

% 閉鏈五杆機構運動學正解M
%                y  |    / E
%                   |  /  L6
%                   | / B
%                   |/\ 
%                   /  \
%               L2 /|   \ L3
%                 / |    \
%              A /  |     \ C
%                \  |     /
%              L1 \ |    / L4
%          ---------|---------->
%                  O  L5 D      x
%
% L1=L4=70 L2=L3=140 L6=0 L5=0 菱形
 
[L1,L2,L3,L4,L5,L6]=deal(70,140,140,70,0,0);
Xa=L1.*cos(Alpha1);
Ya=L1.*sin(Alpha1);
Xc=L5+L4.*cos(Alpha2);
Yc=L4.*sin(Alpha2);
lengthAC=sqrt((Xc-Xa)^2+(Yc-Ya)^2);
A=2*L2.*(Xc-Xa);
B=2*L2.*(Yc-Ya);
C=L2^2+lengthAC^2-L3^2;
theta1=2*atan((B+sqrt(A^2+B^2-C^2))/(A+C));
% theta2=2*atan((B-sqrt(A^2+B^2-C^2))/(A+C));
% if isreal(theta1)
    x=Xa+(L2+L6)*cos(theta1);
    y=Ya+(L2+L6)*sin(theta1);
% else
%     x=Xa+(L2+L6)*cos(theta2);
%     y=Ya+(L2+L6)*sin(theta2);
end

2.逆解

function [Alpha1,Alpha2] = inverse_kinematic(x,y)
%閉鏈五杆機構運動學反解
%                 y |   / E
%                   |  /  L6
%                   | / B
%                   |/\ 
%                   /  \
%               L2 /|   \ L3
%                 / |    \
%              A /  |     \ C
%                \   |     /
%              L1 \ |    / L4
%          ---------|---------->
%                  O  L5 D      x
%
% L1=L4=70 L2=L3=140 L6=0 L5=0 菱形
 
[L1,L2,L3,L4,L5,L6]=deal(70,140,140,70,0,0);
 
a=2*x*L1;
b=2*y*L1;
c=x^2+y^2+L1^2-(L2+L6)^2;
 
Alpha10(1)=2*atan((b+sqrt(a^2+b^2-c^2))/(a+c));
Alpha10(2)=2*atan((b-sqrt(a^2+b^2-c^2))/(a+c));
 
Xb=x-L6*((x-L1*cos(Alpha10))/(L2+L6));
Yb=y-L6*((y-L1*sin(Alpha10))/(L2+L6));
 
d=2*L4*(Xb-L5);
e=2*L4*Yb;
f=[((Xb(1)-L5)^2+L4^2+(Yb(1))^2-L3^2),((Xb(2)-L5)^2+L4^2+(Yb(2))^2-L3^2)];
 
Alpha20(1)=2*atan((e(1)+sqrt((d(1))^2+(e(1))^2-(f(1))^2))/(d(1)+f(1)));
Alpha20(2)=2*atan((e(1)-sqrt((d(1))^2+(e(1))^2-(f(1))^2))/(d(1)+f(1)));
 
Alpha20(3)=2*atan((e(2)+sqrt((d(2))^2+(e(2))^2-(f(2))^2))/(d(2)+f(2)));
Alpha20(4)=2*atan((e(2)-sqrt((d(2))^2+(e(2))^2-(f(2))^2))/(d(2)+f(2)));
 
 
if (Alpha10(1))<0 
    Alpha10(1)=Alpha10(1)+2*pi;
end
if (Alpha10(2))<0 
    Alpha10(2)=Alpha10(2)+2*pi; 
end
 
if (Alpha20(1))<0 
    Alpha20(1)=Alpha20(1)+2*pi;
end
if (Alpha20(2))<0
    Alpha20(2)=Alpha20(2)+2*pi;
end
if (Alpha20(3))<0
    Alpha20(3)=Alpha20(3)+2*pi;
end
if (Alpha20(4))<0 
    Alpha20(4)=Alpha20(4)+2*pi; 
end
Alpha1=Alpha10;
Alpha2=Alpha20;
end

3.工作空間

%閉鏈五杆工作空間求解
%                 y |   / E
%                   |  /  L6
%                   | / B
%                   |/\ 
%                   /  \
%               L2 /|   \ L3
%                 / |    \
%              A /  |     \ C
%                \   |     /
%              L1 \ |    / L4
%          ---------|---------->
%                  O  L5 D      x
%
% L1=L4=70 L2=L3=140 L6=0 L5=0 菱形
%E點座標（x,y）
clear,clc;
i = 1;
for Alpha1 = 0:0.01:2*pi
    for Alpha2 = 0:0.01:2 * pi
        [x(i),y(i)] = forward_kinematic(Alpha1,Alpha2);
        i = i + 1;
    end
    figure(1);
    plot(x,y,'b.');
    hold on;
    i = 1;   
end
axis equal;