理论部分

概念

运动学正解，简而言之，就是给出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=\left[\begin{array}{cc}ro{t}_{3\ast 3}& P_{3\ast 1}\\ 0_{1\ast 3}& 1\end{array}\right]$$
$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
$$ro{t}_{3\ast 3}=\left[\begin{array}{ccc}{r}_{00}& r_{01}& r_{02}\\ r_{10}& r_{11}& r_{12}\\ r_{20}& r_{21}& r_{22}\end{array}\right]$$
$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)