【六足】关于六足机器人运动学正逆解

您所在的位置：网站首页 › 机器人运动学逆解的数目取决于 › 【六足】关于六足机器人运动学正逆解

【六足】关于六足机器人运动学正逆解

2024-07-03 19:42| 来源: 网络整理| 查看: 265

前言

干robocon的时候闲得慌，看看门狗军团里边那个小六足挺帅的，然后拍了一下大腿就决定开始做六足这个项目了，这篇文章主要分享一下关于六足的正运动学和逆运动学。

如何入门

在打算做六足的时候，我还只是一个单纯的做控制（也只是做全向轮底盘运动控制之类的）的学生，为了六足（毕竟自己设计的东西才帅，要是用了别人的机构和公式，总感觉有种ntr那味emm）自学了建模和matlab。

学习《机器人学导论》，了解DH参数和他们的意义以及应用学好线性代数，毕竟运算几乎都是矩阵运算会使用matlab（但是千万千万不要用他来求解析解！）

我这里的求解是针对一条腿的求解，因为如果以机器人中心为坐标原点，加上offset，整个公式会复杂很多倍，所以建议先把一条腿的做出来，怎么反推到绝对坐标系可以后期处理。

正运动学

首先是对着自己设计的机械模型，获取对应的dh参数，至于dh参数如何确定，建议自己百度。我这边是自己用sw画的全部结构，整体的dh参数就很好获取。

在这里插入图片描述

把这些参数代入matlab，基本就是这样

累积起来就可以得到末端的坐标和姿态，即

由于前面的旋转矩阵太长了，只截取了坐标这一列，于是可以得到正运动学解如下

$px = a1*cos(q1) + a2*cos(q1)*cos(q2) + a3*cos(q1)*cos(q2)*cos(q3) a3*cos(q1)*sin(q2)*sin(q3)$ $py = a1*sin(q1) + a2*cos(q2)*sin(q1) + a3*cos(q2)*cos(q3)*sin(q1) - a3*sin(q1)*sin(q2)*sin(q3)$

$pz = a2*sin(q2) + a3*cos(q2)*sin(q3) + a3*cos(q3)*sin(q2)$

以上就是一条腿的正运动学解析。

逆运动学

首先需要设置一个末端坐标矩阵，我设置为

（这样就可以把xyz列入公式了）

由上面求正运动学的公式可知

$T = T_{2}^{1}\cdot T_{3}^{2} \cdot T_{4}^{3}$

那么很容易得知

$(T_{2}^{1})^{-1}\cdot T = (T_{2}^{1})^{-1}\cdot T_{2}^{1}\cdot T_{3}^{2} \cdot T_{4}^{3} = T_{3}^{2} \cdot T_{4}^{3}$

从matlab上运算可得等式左右两边的矩阵

由等式第四列来列方程获取运动学逆解，有

$a3 \cdot cos(q2 + q3) = px \cdot cos(q1) - a1 + py \cdot sin(q1) - a2 \cdot cos(q2)$ （1-1）

$a3 \cdot sin(q2 + q3) = pz - a2 \cdot sin(q2)$ （1-2）

$px \cdot sin(q1) - py \cdot cos(q1) = 0$ （1-3）

首先由（1-3）可以得到

$q1 = atan(\frac{px}{py})$

（这个其实有限制条件，当px = 0时q1 = 90°，不过一般来说，六足是绝对扭不到这个角度的，故可以无视）

由（1-2）和（1-3）联立可得q2和q3，你可以看到我列方程的时候做了一些处理,接下来还有处理需要做，令

$m1 = px \cdot cos(q1) - a1 + py \cdot sin(q1)$

$m2 = pz$

方程可以化简为

$a3 \cdot cos(q2 + q3) == m1 - a2 \cdot cos(q2)$

$a3 \cdot sin(q2 + q3) == m2 - a2 \cdot sin(q2)$

方程两边同时平方，相加，有

$a3^{2} = a2^2 - 2\cdot cos(q2)\cdot a2\cdot m1 - 2\cdot sin(q2)\cdot a2\cdot m2 + m1^2 + m2^2$

令

$n1 = 2\cdot a2\cdot m1$

$n2 = 2\cdot a2\cdot m2$

$n3 = a2^2 + m1^2 + m2^2 -a3^2$

则可将方程化简为

$n1\cdot cos(q2) + n2\cdot sin(q2) = n3$

既然你都会上csdn了，那么我相信这个方程你一定能够解出来吧（笑）

最终求得的结果就是

$q1 = atan(\frac{px}{py})$

$q2 = acosd \frac{(n1*n3 + n2*(n1^2 + n2^2 - n3^2)^(1/2))}{(n1^2 + n2^2)}$

$q3 = acosd(\frac{(m1 - a2*cosd(q2))}{a3}) - q2$

是不是非常简单？这个公式你就已经可以放进你的单片机或者是别的系统里了，通过输入xyz的坐标就能自动推出逆解，达到对应的坐标。

结语

其实作为一个机电的学生，做这个还是很挑战自己的，写这篇文章就当写了个日记吧，求这个逆解确实是花了不少时间。从只会嵌入式编程，到自学solid works建模，用3D打印机打出自己的第一个模型，再到学习机器人学导论，用matlab仿真机械臂的运动，一路上感觉学到了不少。当第一次把正运动学求出的结果和solid works实际的的距离完全相等的时候，那种开心简直难以言喻，实在是非常有趣的过程。

一路上并不是学到的全部都用上，也走了不少歪路，学会用platformio和micropython写esp32和树莓派pico以后，发现还是stm32最好使，这引脚数什么的，做机器人这块果然还得是stm家的芯片好使啊（笑）

写这篇文章是因为当时求逆解碰了不少歪路，把各种视频全都看了一遍，网上也没人写怎么求这种三轴机械臂的逆解（可能是我太菜了，勿喷），希望对看到这篇文章的人有一些的帮助吧XD（附个自己设计的六足的图）

【本文地址】

【六足】关于六足机器人运动学正逆解

【六足】关于六足机器人运动学正逆解

今日新闻

推荐新闻