一、背景

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

可得
在这里插入图片描述
消去θ可得
在这里插入图片描述

在这里插入图片描述

即上式化为
在这里插入图片描述
解得
在这里插入图片描述
将上式带入
在这里插入图片描述
解得
在这里插入图片描述

所以
在这里插入图片描述

在这里插入图片描述

在这里插入图片描述
又由
在这里插入图片描述

可得
在这里插入图片描述
消去γ \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;