确定机械臂D-H参数并在Matlab进行仿真（正逆运动学求解）

工训实践大赛智能+赛道的智能物流机器人包含机械臂，我们小组准备设计六自由度的机械臂。

想要控制这样一个机械臂可不简单，我们通过图像识别可以很容易的计算物块相对于小车在空间中的坐标，我们需要把机械臂移动到这个坐标。

机械臂的每个关节都是舵机，通过改变PWM波的占空比可以让舵机旋转成我们想要的角度。然而机械臂有那么多个关节，我们该怎么知道这些关节应该往什么方向旋转多少度呢？这就需要看我们的反向运动学（逆运动学）求解啦！

我们来明确一下术语

几个术语	是啥呢？能干啥？
平移向量	就是把点在右手笛卡尔系的向量坐标用xyz的顺序从上往下写哒！（下文有示例）
旋转矩阵	一个3*3的矩阵，我的理解是两两垂直的三个单位向量按照原来的坐标系旋转后得到的三个新向量按照xyz三个方向从左往右写下它们的坐标（此处难以形象地说明，下文详解）
齐次变换矩阵(位姿矩阵)	由旋转矩阵和平移向量组成，用来描述机械臂某个关节的位置和姿态
D-H参数	用来描述机械臂各个连杆的空间关系
正运动学	把每个关节的位姿矩阵相乘即可得到末端的位姿矩阵
逆运动学	通过末端的位姿矩阵计算出各个关节旋转的角度

数学知识

平移向量怎么求？

例如向量α(1,2,3)，那么它对应的平移向量就是

$\begin{bmatrix} 1\\2\\3 \end{bmatrix}$

旋转矩阵怎么求？

已知x(1,0,0) y(0,1,0) z(0,0,1)我们把黑色的xoy绕z轴旋转θ角得到绿色的x’oy’

如图，xoy旋转后得到x’oy’，根据中学的数学知识很容易得到x’(cosθ,sinθ,0)，y’(-sinθ,cosθ,0)，z没有改变，还是z(0,0,1)

我们把x’,y’,z按照平移向量的方法写出来，像这样

$\begin{bmatrix} cos\theta \\sin\theta\\0 \end{bmatrix} \begin{bmatrix} -sin\theta \\cos\theta\\0 \end{bmatrix} \begin{bmatrix} 0\\0\\1 \end{bmatrix}$

然后我们把他们合在一起写

$\begin{bmatrix} cos\theta & -sin\theta&0\\ sin\theta&cos\theta&0\\ 0&0&1 \end{bmatrix}$

这个就是绕z轴的旋转矩阵啦！

绕着其他轴的旋转矩阵也是类似的求法，这里就不赘述了

齐次变换矩阵

齐次变换矩阵可以用来描述一个物体（机械臂的关节）在空间中的位置和姿态，它的写法是这样的

$T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & p_{1} \\ r_{21} & r_{22} & r_{23} & p_{2} \\ r_{31} & r_{32} & r_{33} & p_{3} \\ 0 & 0 & 0 & 1 \end{bmatrix}$

其中，R是旋转矩阵，p是平移向量

我们把上文写的平移矩阵和旋转矩阵写成齐次变换矩阵就是这样的：

$\begin{bmatrix} cos\theta & -sin\theta&0&1\\ sin\theta&cos\theta&0&2\\ 0&0&1&3\\ 0&0&0&1 \end{bmatrix}$

使用Matlab将这些矩阵可视化

下面是我整理出来的一些指令

trotx(pi/6)齐次x 旋转六分之派

trotx(120,'deg') 旋转120度

a=transl(1,2,4) 平移点为(1,2,4)，这里得到的
齐次旋转矩阵的旋转矩阵部分是单位矩阵

trplot(ans) 画图

ans=ans*a 把平移作用到旋转上，实质是矩阵叉乘

tranimate(ans)动画

确定D-H参数

D-H参数共有4个，见表格

theta(θ)	关节转角
d	连杆偏移
a	连杆长度
alpha(α)	连杆扭角

这些参数在下图中有所指示

注意一下，不然有可能看不懂图：连杆i右边实际上还有关节，只是没画出来，其中θi那个箭头指的是下一个连杆到这一个连杆的公共法线

源：机器人学导论第三版译文53页

正运动学求解

本质上是将每个关节的齐次变换矩阵相乘求出末端关节的齐次变换矩阵

${}_{6}^{0}T = {}_{1}^{0}T{}_{2}^{1}T{}_{3}^{2}T{}_{4}^{3}T{}_{5}^{4}T{}_{6}^{5}T$

*逆运动学求解

根据末端关节的齐次变换矩阵求出所有关节的角度

这一部分比较复杂，详见论文P53推导过程

六自由度机械臂逆运动学求解.pdf