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

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

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

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

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

正向运动学分析#

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

\begin{aligned} _{i}^{i - 1} T & = T_{{\hat{X}}_{i - 1}} (α_{i - 1}) T_{{\hat{X}}_{R}} (α_{i - 1}) T_{{\hat{Z}}_{Q}} (θ_{i}) T_{{\hat{Z}}_{P}} (d_{i}) \\ = [\begin{matrix} c θ_{i} & - s θ_{i} & 0 & a_{i - 1} \\ s θ_{i} c α_{i - 1} & c θ_{i} c α_{i - 1} & - s α_{i - 1} & - s α_{i - 1} d_{i} \\ s θ_{i} s α_{i - 1} & c θ_{i} s α_{i - 1} & c α_{i - 1} & c α_{i - 1} d_{i} \\ 0 & 0 & 0 & 1 \end{matrix}] \end{aligned}

$\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}$

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

_{1}^{0} T = [\begin{matrix} c θ_{1} & - s θ_{1} & 0 & 0 \\ s θ_{1} & c θ_{1} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]_{2}^{1} T = [\begin{matrix} c θ_{2} & - s θ_{2} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ - s θ_{2} & - c θ_{2} & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]_{3}^{2} T = [\begin{matrix} c θ_{3} & - s θ_{3} & 0 & a_{2} \\ s θ_{3} & c θ_{3} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]

${^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}$

_{4}^{3} T = [\begin{matrix} c θ_{4} & - s θ_{4} & 0 & a_{3} \\ s θ_{4} & c θ_{4} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]_{5}^{4} T = [\begin{matrix} c θ_{5} & - s θ_{5} & 0 & a_{4} \\ 0 & 0 & 0 & d_{5} \\ - s θ_{5} & - c θ_{5} & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]

${^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}$

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

_{5}^{0} T =_{1}^{0} T_{2}^{1} T_{3}^{2} T_{4}^{3} T_{5}^{4} T = [\begin{matrix} 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{matrix}]

${^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}$ 的乘积通过积化和差公式转化为一个简化的表达式，表达式形式如下所示

_{4}^{1} T =_{2}^{1} T_{3}^{2} T_{4}^{3} T = [\begin{matrix} c_{234} & - s_{234} & 0 & c_{23} a_{3} + c_{2} a_{2} \\ 0 & 0 & 1 & 0 \\ - s_{234} & - c_{234} & 0 & - s_{23} a_{3} - s_{2} a_{2} \\ 0 & 0 & 0 & 1 \end{matrix}]

${^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$
则可以将表达式简化为以下形式

_{4}^{1} T =_{2}^{1} T_{3}^{2} T_{4}^{3} T = [\begin{matrix} 1 & 0 & 0 & c_{23} a_{3} + c_{2} a_{2} \\ 0 & 0 & 1 & 0 \\ 0 & - 1 & 0 & - s_{23} a_{3} - s_{2} a_{2} \\ 0 & 0 & 0 & 1 \end{matrix}]

${^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} _{5}^{0} T =_{1}^{0} T_{2}^{1} T_{3}^{2} T_{4}^{3} T_{5}^{4} T =_{1}^{0} T_{4}^{1} T_{5}^{4} T & = [\begin{matrix} c_{1} c_{5} + s_{1} s_{5} & - c_{1} s_{5} + s_{1} c_{5} & 0 & c_{1} (c_{23} a_{3} + c_{2} a_{2} + a_{4}) \\ s_{1} c_{5} - c_{1} s_{5} & - s_{1} s_{5} - c_{1} c_{5} & 0 & s_{1} (c_{23} a_{3} + c_{2} a_{2} + a_{4}) \\ 0 & 0 & - 1 & - d_{5} - s_{23} a_{3} - s_{2} a_{2} \\ 0 & 0 & 0 & 1 \end{matrix}] \\ = [\begin{matrix} 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{matrix}] \end{aligned}

$\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_{2} a_{2})^{2} + (s_{23} a_{3} + s_{2} a_{2})^{2} = (p_{x} - c_{1} a_{4})^{2} + (p_{y} - s_{1} a_{4})^{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} - 2 a_{4} (c_{1} p_{x} + s_{1} p_{y}) + 2 p_{z} d_{5} + a_{4}^{2} + d_{5}^{2} - a_{2}^{2} - a_{3}^{2}}{2 a_{2} a_{3}}

$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_{3} a_{3} + a_{2})^{2} + (s_{3} a_{3})^{2}} s i n (θ_{2} + a r c t a n (\frac{s_{3} a_{3}}{c_{3} a_{3} + a_{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，可以看到两个齐次矩阵是一模一样的😄

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

posted @ 2022-05-25 10:03 Dragonet-Z 阅读(3208) 评论(1) 编辑收藏举报

刷新页面返回顶部

登录后才能查看或发表评论，立即登录或者逛逛博客园首页

相关博文：

· 毕设(1)——机械臂DH建模

· 毕设(3)—机械臂工作空间

· matlab练习程序（机械臂DH模型）

· 二维逆运动学 – 代码

· 三维逆向运动学

阅读排行：
· 全程不用写代码，我用AI程序员写了一个飞机大战
· DeepSeek 开源周回顾「GitHub 热点速览」
· 记一次.NET内存居高不下排查解决与启示
· 物流快递公司核心技术能力-地址解析分单基础技术分享
· .NET 10首个预览版发布：重大改进与新特性概览！

公告

昵称： Dragonet-Z
园龄： 3年10个月
粉丝： 23
关注： 4

+加关注

2025年3月

日

一

二

三

四

五

六

Loading

Dragonet-Z's Blog

感谢您看我的博客，欢迎欢迎！ Gitee地址：https://gitee.com/dragonet-Z

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

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

正向运动学分析#