四足机器人：闭链五杆腿结构运动学分析

一、背景

在四足机器人的发展中，由于串联腿结构对关节驱动电机的要求很高，控制精度难以保证，因此逐渐出现了使用闭链五杆机构作为机器人腿结构的四足机器人，如宾夕法尼亚大学的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;