欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

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

程序员文章站 2022-07-14 22:16:00
...

一、背景

在四足机器人的发展中,由于串联腿结构对关节驱动电机的要求很高,控制精度难以保证,因此逐渐出现了使用闭链五杆机构作为机器人腿结构的四足机器人,如宾夕法尼亚大学的Minitaur,斯坦福的开源机器狗Stanford Doggo,以及在2019年全国大学生机器人大赛Robocon赛事中以武汉大学的四足机器人为代表的一系列高校所采取的并联五杆结构四足机器人。

本文针对这一类机器人所采用的闭链五杆机构,对其运动学进行了简要的分析,并给出了其运动学的正反解过程,以及相应的Matlab源码。

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

二、建立数学模型

建立闭链五杆机构简化数学模型如图所示,其中OAABBCCDDOBE 长度分别为L1L_1L2L_2L3L_3L4L_4L5L_5L6L_6DO为机架,OD 两点为驱动。
四足机器人:闭链五杆腿结构运动学分析

三、正解

由图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;