6轴机器人运动学（正解）_6轴机械臂运动学算法

理论部分

概念

运动学正解，简而言之，就是给出6个关节变量，求得机械臂末端的位置和姿态
即给出$j_1-j_6$，求$x,y,z,rx,ry,rz$

DH参数

只单一地给出关节值或直角坐标值，是不能直接互相转化的，还与具体的机器人有关，这部分有关的内容可以用DH参数表来表示，其描述了机器人各关节坐标系之间的关系

表中内容
连杆长度 (length) ：2个相邻关节轴线之间的距离
连杆扭角 (angle) ：2个相邻关节轴线之间的角度
连杆偏距 (d) ：2个关节坐标系的X轴之间的距离

eg：UR5e

• DH参数表

计算

根据DH参数表以及$j_1-j_6$，建立6个关节矩阵$A_1-A_6$，计算出转换矩阵$T_1-T_6$，计算$A_1-A_6$相乘得到矩阵R
R = [ r o t 3 ∗ 3 P 3 ∗ 1 0 1 ∗ 3 1 ] R=

$$\begin{bmatrix} {rot_{3*3}}&{P_{3*1}}\\ {0_{1*3}}&{1}\\ \end{bmatrix}$$ R=[rot3∗3​01∗3​​P3∗1​1​]
$P_{3\ast 1}=(x,y,z{)}^T$
则求出R即求出x,y,z
关节矩阵$A_i$由当前的关节的$j_i$和DH参数导出
设当前$j_i$为$\beta$,legth为$l$,d为$d$,angle为$\alpha$
$$A_i=\left[\begin{array}{cccc}cos\beta & -sin\beta cos\alpha & sin\beta sin\alpha & lcos\beta \\ sin\beta & cos\beta cos\alpha & -cos\beta sin\alpha & lsin\beta \\ 0& sin\alpha & cos\alpha & d\\ 0& 0& 0& 1\end{array}\right]$$
$R=A_1{A}_2{A}_3{A}_4{A}_5{A}_6$

然后再求rx,ry,rz
r o t 3 ∗ 3 = [ r 00 r 01 r 02 r 10 r 11 r 12 r 20 r 21 r 22 ] rot_{3*3}= \left[

$$\begin{matrix} r_{00} & r_{01} & r_{02} \\ r_{10} & r_{11} & r_{12} \\ r_{20} & r_{21} & r_{22} \\ \end{matrix}$$ \right] rot3∗3​= ​r00​r10​r20​​r01​r11​r21​​r02​r12​r22​​ ​
$rx=arctan(r[1][2],r[2][2])$
$ry=arctan(r[0][2],\sqrt{r[0][0{]}^2+r[0][1{]}^2})$
$rz=arctan(r[0][1],r[0][0])$

代码（C++）

可见https://github.com/jeekzhang/6DOF-KFS

验证(UR5e)

$j.txt:$

输出：

robodk C#API结果：

参考资料：

6轴机器人运动学正解,逆解1
机器人导论 学习笔记2 - 运动学（正解）
欧拉角，四元数，旋转矩阵相互转化（c++, python)