六轴机械臂运动学算法原理及其推导过程

        网站上关于六轴机械臂piper算法的讲解有很多，但其腕点姿态的推到较为模糊，故此写一篇关于六轴机械臂piper算法的推导讲解，供有缘人参考，如果您觉得有用，可以点个赞，吾将不胜感激，若是推导过程存在错误，大佬也可以帮忙指出，感激不尽。

        一、结构原型

 图1.机器人结构原型

如上图所示，菱形及圆形表示轴的不同旋转方向；

二、运动学正解

       串联机器人运动学正解较为简单，只是位置、姿态的一个转换，本次运用标准D-H算法（先绕z轴旋转），推到运动学正解。

                                                  (1)

       T为机器人末端位置的转换矩阵，为第i轴的转换矩阵。

表1.D-H参数列表

Ɵ	d	a	ɑ
Ɵ1	d1	a1	Pi/2
Ɵ2+pi/2	0	a2	0
Ɵ3	d3	a3	Pi/2
Ɵ4	d4	0	Pi/2
Ɵ5	0	0	-pi/2
Ɵ6	d6	0	0

其中：

                    (2)

三、运动学逆解

       六轴机器人运动学逆解较为复杂，由于机器人末端三轴的轴线相交于一点，满足piper算法，故此采用piper算法进行逆解的推导。

 

图2.机器人结构

如图2，可以看出机械臂末端三轴的轴线相交于轴5所在的位置，即腕点位置。从参考坐标系来看，倘若前三轴的角度确定，腕点的位置也将固定，后三轴的角度仅影响腕点的姿态，不影响腕点位置。

3.1 腕点位置坐标

已知：机器人末端位置为()，机械臂末端姿态为，末端到腕点的连杆长度为，求腕点坐标()。

由于连杆是末端姿态Z方向的量，且Z正向指向远离机器人的位置，故：

                                                         (3)

3.2 前三关节转到量

       已知腕点位置，求前三关节的转动量。

       对机器人进行XOY的平面投影：

如图，在机器人XOY的平面投影中，为机器人投影与初始位置的夹角，从机器人结构可以看出，无论其余五轴转动量如何，都不会影响到机器人投影与初始位置的夹角，故：

                                                                   (4)

如图2，在机器人XOZ平面上，可以看出：

                          (5)

                       (6)

故，根据公式（4~6），可以计算出前三关节的转动量，计算略。

3.3 后三关节转动量

       后三关节的转动量，需要结合机器人位姿来进行计算。

图4.机器人后三轴结构

       令：参考坐标系到末端坐标系的姿态变换：

                                              (7)

其中：为转换矩阵的旋转因子。

       前三轴的转动量已知，则从轴4开始的位姿态已知。仅从姿态角度考虑，从轴4开始，机器人的变换依次为：

绕轴旋转，绕轴旋转；

绕轴旋转，绕轴旋转；

                                                   绕轴旋转；

坐标系的旋转，虽然顺序不同，姿态也将大为不同，但旋转可以进行等效替换。

如图4所示的机械结构，从轴4到到末端的姿态变化，轴4的旋转反映到末端依旧为末端Z轴方向上的旋转，轴5的旋转反映到末端为Y轴方向的负向旋转；故末端三轴的旋转姿态可以等效为：

绕轴旋转0，绕轴旋转；

绕轴旋转，绕轴旋转；

                                                    绕轴旋转；

                                                    绕轴旋转；

                                                    绕轴旋转；

                                                    绕轴旋转；

利用MATLAB进行验证输出：

经MATLAB计算验证，等效后的姿态数据与等效前数据完全相同。故：

                              (8)

       (9)

                                 (10)

故此可以推出后三角的转动量，计算过程，略。