毕设(2)——机械臂正逆运动学分析

目录

• 毕设(2)——机械臂正逆运动学分析
  • 正向运动学分析
  • 逆向运动学分析
    • 求\(\theta_1\)
    • 求\(\theta_3\)
    • 求\(\theta_2\)
    • 求\(\theta_4\)
    • 求\(\theta_5\)
    • 逆向运动学方程
  • Matlab代码验证
    • 正向运动学
    • 逆向运动学
    • 验证程序

毕设中用到了很多代码，其中一部分我通过看书和看论文学习并实现的代码，会通过Gitee仓库分享出来，这些代码仅用于学习使用，祝各位毕业生顺利完成毕设！

毕设系列内容：毕业设计——四自由度机械臂轨迹规划

---

毕设(2)——机械臂正逆运动学分析

建立好运动学模型之后，就可以开始对机械臂进行运动学分析，这也是机械臂轨迹规划中重要的基础之一，B站上我看的一个课程非常好，是一位台湾的教授讲的课，网址是机器人运动学—林沛群

正向运动学分析

改进型DH的齐次变换矩阵形式如下

\[\begin{aligned} {^{i-1}_iT} &= T_{\hat{X}_{i-1}}(\alpha_{i-1}) T_{\hat{X}_R}(\alpha_{i-1}) T_{\hat{Z}_Q}(\theta_i) T_{\hat{Z}_P}(d_i) \\ &= \begin{bmatrix} c\theta_i & -s\theta_i & 0 & a_{i-1} \\ s\theta_ic\alpha_{i-1} & c\theta_ic\alpha_{i-1} & -s\alpha_{i-1} & -s\alpha_{i-1}d_i \\ s\theta_is\alpha_{i-1} & c\theta_is\alpha_{i-1} & c\alpha_{i-1} & c\alpha_{i-1}d_i \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{aligned} \]

得到五个关节的齐次变换矩阵分别如下所示

\[{^0_1T}= \begin{bmatrix} c\theta_1 & -s\theta_1 & 0 & 0 \\ s\theta_1 & c\theta_1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} {^1_2T}= \begin{bmatrix} c\theta_2 & -s\theta_2 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ -s\theta_2 & -c\theta_2 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} {^2_3T}= \begin{bmatrix} c\theta_3 & -s\theta_3 & 0 & a_2 \\ s\theta_3 & c\theta_3 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \]

\[{^3_4T}= \begin{bmatrix} c\theta_4 & -s\theta_4 & 0 & a_3 \\ s\theta_4 & c\theta_4 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} {^4_5T}= \begin{bmatrix} c\theta_5 & -s\theta_5 & 0 & a_4 \\ 0 & 0 & 0 & d_5 \\ -s\theta_5 & -c\theta_5 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \]

然后将各个矩阵相乘即可得到机械臂的正向运动学方程

\[{^0_5T}={^0_1T}{^1_2T}{^2_3T}{^3_4T}{^4_5T}= \begin{bmatrix} n_x & o_x & a_x & p_x \\ n_y & o_y & a_y & p_y \\ n_z & o_z & a_z & p_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \]

但是通过观察机械臂结构可以发现第2、3、4关节均为平行的旋转关节，则可以将\({^1_2T}\)、\({^2_3T}\)、\({^3_4T}\)的乘积通过积化和差公式转化为一个简化的表达式，表达式形式如下所示

\[{^1_4T}={^1_2T}{^2_3T}{^3_4T}= \begin{bmatrix} c_{234} & -s_{234} & 0 & c_{23}a_3+c_2a_2 \\ 0 & 0 & 1 & 0 \\ -s_{234} & -c_{234} & 0 & -s_{23}a_3-s_2a_2 \\ 0 & 0 & 0 & 1 \end{bmatrix} \]

其中，\(c_{234}=cos(\theta_2+\theta_3+\theta_4)\)，\(s_{234}=sin(\theta_2+\theta_3+\theta_4)\)
机械臂的结构几何关系如下图所示

根据机械臂的结构几何关系，可以得到\(\theta_2+\theta_3+\theta_4\equiv0\)
则可以将表达式简化为以下形式

\[{^1_4T}={^1_2T}{^2_3T}{^3_4T}= \begin{bmatrix} 1 & 0 & 0 & c_{23}a_3+c_2a_2 \\ 0 & 0 & 1 & 0 \\ 0 & -1 & 0 & -s_{23}a_3-s_2a_2 \\ 0 & 0 & 0 & 1 \end{bmatrix} \]

则可以得到正向运动学方程为以下形式

\[\begin{aligned} {^0_5T}={^0_1T}{^1_2T}{^2_3T}{^3_4T}{^4_5T}={^0_1T}{^1_4T}{^4_5T} &= \begin{bmatrix} c_1c_5+s_1s_5 & -c_1s_5+s_1c_5 & 0 & c_1(c_{23}a_3+c_2a_2+a_4) \\ s_1c_5-c_1s_5 & -s_1s_5-c_1c_5 & 0 & s_1(c_{23}a_3+c_2a_2+a_4) \\ 0 & 0 & -1 & -d_5-s_{23}a_3-s_2a_2 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ &= \begin{bmatrix} n_x & o_x & a_x & p_x \\ n_y & o_y & a_y & p_y \\ n_z & o_z & a_z & p_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{aligned} \]

逆向运动学分析

求\(\theta_1\)

\(\theta_1\)可以通过\(p_x\)和\(p_y\)求出

由\( \left\{ \begin{array}{l} c_1(c_{23}a_3+c_2a_2+a_4)=p_x \\ s_1(c_{23}a_3+c_2a_2+a_4)=p_y \end{array} \right. \)

求得\(\theta_1=arctan(\frac{p_y}{p_x})\)，得到的两个值可根据\(p_x\)和\(p_y\)的正负情况社区一个值，得到唯一解

求\(\theta_3\)

由\( \left\{ \begin{array}{l} c_1(c_{23}a_3+c_2a_2+a_4)=p_x \\ s_1(c_{23}a_3+c_2a_2+a_4)=p_y \end{array} \right. \)
平方相加可得\((c_{23}a_3+c_2a_2)^2=(p_x-c_1a_4)^2+(p_y-s_1a_4)^2\)
由\(-d_5-s_{23}a_3-s_2a_2=p_z\)
得\((s_{23}a_3+s_2a_2)^2=(p_z+d_5)^2\)
两式相加，可得

\[(c_{23}a_3+c_2a_2)^2+(s_{23}a_3+s_2a_2)^2=(p_x-c_1a_4)^2+(p_y-s_1a_4)^2+(p_z+d_5)^2 \]

\[c_3=\frac{p_x^2+p_y^2+p_z^2-2a_4(c_1p_x+s_1p_y)+2p_zd_5+a_4^2+d_5^2-a_2^2-a_3^2}{2a_2a_3} \]

则\(\theta_3=\pm arccos(\frac{p_x^2+p_y^2+p_z^2-2a_4(c_1p_x+s_1p_y)+2p_zd_5+a_4^2+d_5^2-a_2^2-a_3^2}{2a_2a_3})\)
求出来得\(\theta_3\)可能有两个值，可以根据"最小变化行程原则"选择一个合适得角度值

求\(\theta_2\)

由\(-d_5-s_{23}a_3-s_2a_2=p_z\)
将\(s_{23}\)通过和差化积公式拆开
得\((c_3a_3+a_2)s_2+(s_3a_3)c_2=-(p_z+d_5)\)
根据辅助角公式，可得

\[\sqrt{(c_3a_3+a_2)^2+(s_3a_3)^2}sin(\theta_2+arctan(\frac{s_3a_3}{c_3a_3+a_2}))=-(p_z+d_5) \]

则\(\theta_2=arcsin(\frac{-(p_z+d_5)}{\sqrt{(c_3a_3+a_2)^2+(s_3a_3)^2}})-arctan(\frac{s_3a_3}{c_3a_3+a_2})\)
根据\(\theta_2\)得范围可知\(\theta_2\)存在唯一解，只需要在求出的结果中筛选在范围内得角度值即可

求\(\theta_4\)

\(\theta_4\)可根据\(\theta_2+\theta_3+\theta_4\equiv0\)求出

\(\theta_4=-(\theta_2+\theta_3)\)

求\(\theta_5\)

由\( \left\{ \begin{array}{l} c_1c_5+s_1s_5=n_x \\ s_1c_5-c_1s_5=n_y \end{array} \right. \)
得\(\theta_5=\theta_1-arctan(\frac{n_y}{n_x})\)

同样可以求出两个值，需要根据"最小变化行程原则"筛选合适的角度值

逆向运动学方程

根据五个关节角度的求解，可以得到逆向运动学方程

\( \left\{ \begin{array}{l} \theta_1=arctan(\frac{p_y}{p_x}) \\ \theta_2=arcsin(\frac{-(p_z+d_5)}{\sqrt{(c_3a_3+a_2)^2+(s_3a_3)^2}})-arctan(\frac{s_3a_3}{c_3a_3+a_2}) \\ \theta_3=\pm arccos(\frac{p_x^2+p_y^2+p_z^2-2a_4(c_1p_x+s_1p_y)+2p_zd_5+a_4^2+d_5^2-a_2^2-a_3^2}{2a_2a_3}) \\ \theta_4=-(\theta_2+\theta_3) \\ \theta_5=\theta_1-arctan(\frac{n_y}{n_x}) \end{array} \right. \)

Matlab代码验证

正向运动学

原理搞清楚之后，代码方面就很简单了

myfkine.m

function [T05] = myfkine(Angle_T)
%运动学正解
theta1 = Angle_T(1, 1);
theta2 = Angle_T(1, 2);
theta3 = Angle_T(1, 3);
theta4 = Angle_T(1, 4);
theta5 = Angle_T(1, 5);
theta23 = theta2 + theta3;
a2 = 135; a3 = 147; a4 = 61; d5 = 131;
T01 = [cos(theta1) -sin(theta1) 0 0;
       sin(theta1) cos(theta1)  0 0;
       0           0            1 0;
       0           0            0 1];
T14 = [1 0  0 cos(theta23) * a3 + cos(theta2) * a2;
       0 0  1 0;
       0 -1 0 -sin(theta23) * a3 - sin(theta2) * a2;
       0 0  0 1];
T45 = [cos(theta5)  -sin(theta5) 0 a4;
       0            0            1 d5;
       -sin(theta5) -cos(theta5) 0 0;
       0            0            0 1];
T05 = T01 * T14 * T45;

逆向运动学

逆向运动学也是类似操作，最需要了解的内容是理论部分
代码较长，文中就不放出来了，你可以在代码仓库中找到它

验证程序

需要注意的是，机器人工具箱中的ikine函数，支持的是带有球形手腕的6轴机械臂，通常对6自由度以下的机械臂支持不好，会报错，所以逆运动学没法用工具箱来验证，只是将逆解输入到正解函数中再次求解，看看两次正解情况是否相同

test2.m

% Modified DH 建模dobot
clear, clc, close all;

%建立机器人模型
%           关节角   关节偏距  连杆长度   连杆转角   旋转关节   偏差
%           theta    d        a         alpha     sigma      offset
L(1) = Link([0       0        0         0         0          0], 'modified');
L(2) = Link([0       0        0         -pi / 2   0          0], 'modified');
L(3) = Link([0       0        135       0         0          0], 'modified');
L(4) = Link([0       0        147       0         0          0], 'modified');
L(5) = Link([0       131      61        -pi / 2   0          0], 'modified');

%连接连杆
robot = SerialLink(L, 'name', 'Dobot');

angle1 = [-pi / 6, -pi / 6, pi / 3, -pi / 6, pi / 4];
angle2 = [pi / 3, -pi / 60, pi / 30, -pi / 60, -pi / 4];

%运动学正解
fk1 = myfkine(angle1); %正解函数
fk2 = robot.fkine(angle1);%工具箱正解函数
%fk1 = myfkine(angle2); %正解函数
%fk2 = robot.fkine(angle2);%工具箱正解函数

%运动学逆解
ik1 = myikine(fk1);%逆解函数
%实际上robot.ikine函数并不可用
%ik2 = robot.ikine(fk1);%工具箱逆解函数
degik1 = rad2deg(ik1);

%再次运动学正解
ik1_fk = myfkine(ik1(1, :));

在matlab命令行中查看fk1和ik1_fk，可以看到两个齐次矩阵是一模一样的😄

本文到此结束，后续会继续更新的~😃