六轴UR机械臂正逆运动学公式推导+代码验证C++

FuYongqing0412

已于 2025-02-18 18:02:30 修改

阅读量3.1w

点赞数 50

分类专栏： UR 文章标签： c++ 线性代数硬件工程矩阵经验分享

于 2021-12-24 16:51:25 首次发布

本文链接：https://blog.csdn.net/sinat_32804425/article/details/122125911

版权

UR 专栏收录该内容

2 篇文章

订阅专栏

最近做项目需要用到UR5的逆解，在网上找资料自己实现碰到一堆坑，现在终于完美解决。本文包含从末端位姿变换到关节角的全部过程，网上许多教程都是用正运动学算出的变换矩阵进行逆运动学求解，在我的项目中完全没有办法应用。其中UR末端姿态有两种表示方法，这也是排查了好久才找到的问题。现在将完整过程分享出来，希望可以帮到和我遇到同样问题的小伙伴。URsim使用及安装可参考我另一篇博客URSim+Unity联合仿真以及Socket通信_sinat_32804425的博客-CSDN博客

（改进DH参数方法还可参考UR3机械臂正逆运动学详解及c++完整代码_Scolin丶-CSDN博客_ur3机械臂)

DH参数介绍

改进DH

图源《机器人学导论》

参考Universial robot 运动学_Peace-CSDN博客，此处是为了方便对比。

第i个坐标系固连在第i个连杆的左端。
轴i固连于i-1杆，在i-1杆的右端。
i坐标系固定在i杆上，随i杆转动。

每个连杆有四个参数，第i个连杆：

ai = (沿着Xi轴，从Zi移动到Zi+1的距离) ，即连杆i的抽象长度。
αi= (从Zi旋转到Zi+1的角度，转轴为Xi，右手定则)，即连杆i与连杆i+1的夹角。
di = (沿着Zi轴，从Xi-1移动至Xi的距离)， Xi-1和Xi都是垂于Zi的。
θi = (绕Zi轴，从Xi-1旋转至Xi的角度)，即连杆i-1与连杆i的夹角。

前两个参数的作用是建立 i系与i+1系的关系；

后两个参数的作用是建立i-1系与i系的关系；

UR5的官方DH参数如下表所示，是改进DH参数。其他型号可以参考Universal Robots - DH Parameters for calculations of kinematics and dynamics

下图是官方给的UR机械臂坐标系位置

标准DH

图源https://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters

坐标系的建立：

z轴指向旋转轴。

基座的x轴可以自由选择，之后的x轴与两个连续z轴的公垂线共线。

原点选在公垂线与新z轴的交点，注意不在关节的中心，可以在空间的任何地方。

y轴由右手定则给出。

有了这些关节坐标系，就可以得到由以下四个变量决定的关节之间的转换：

di : 沿着Zi-1轴，原点Oi-1至公垂线的距离。
θi : 绕Zi-1轴，从Xi-1旋转至Xi的角度。
ai : 沿着Xi轴，从Zi-1移动至Zi的距离，即连杆Linki关于Zi-1的旋转半径。
αi : 绕Xi轴，从Zi-1旋转到Zi的角度。

特殊情况：如果Z轴是平行的，那选择最方便的那个di。此时的αi一定是0。

辅助理解动画

https://www.youtube.com/watch?v=rA9tm0gTln8https://www.youtube.com/watch?v=rA9tm0gTln8

正运动学

已知关节角度求变换矩阵T，根据标准DH参数法，可以得到坐标系i-1到i的变换矩阵 $eq?%7B%20%7D_%7Bi%7D%5E%7Bi-1%7D%20T%3D$

$eq?%5Cleft%5B%5Cbegin%7Barray%7D%7Bcccc%7D%20%5Ccos%20%5Ctheta_%7Bi%7D%20%26%20-%5Csin%20%5Ctheta_%7Bi%7D%20%5Ccos%20%5Calpha_%7Bi%7D%20%26%20%5Csin%20%5Ctheta_%7Bi%7D%20%5Csin%20%5Calpha_%7Bi%7D%20%26%20a_%7Bi%7D%20%5Ccos%20%5Ctheta_%7Bi%7D%20%5C%5C%20%5Csin%20%5Ctheta_%7Bi%7D%20%26%20%5Ccos%20%5Ctheta_%7Bi%7D%20%5Ccos%20%5Calpha_%7Bi%7D%20%26%20-%5Ccos%20%5Ctheta_%7Bi%7D%20%5Csin%20%5Calpha_%7Bi%7D%20%26%20a_%7Bi%7D%20%5Csin%20%5Ctheta_%7Bi%7D%20%5C%5C%200%20%26%20%5Csin%20%5Calpha_%7Bi%7D%20%26%20%5Ccos%20%5Calpha_%7Bi%7D%20%26%20d_%7Bi%7D%20%5C%5C%200%20%26%200%20%26%200%20%26%201%20%5Cend%7Barray%7D%5Cright%5D$

所以正运动学可以求得 $eq?%7B%20%7D_%7B6%7D%5E%7B0%7D%20T%3D%7B%20%7D_%7B1%7D%5E%7B0%7D%20T%20%5Ctimes%7B%20%7D_%7B2%7D%5E%7B1%7D%20T%20%5Ctimes%7B%20%7D_%7B3%7D%5E%7B2%7D%20T%20%5Ctimes%7B%20%7D_%7B4%7D%5E%7B3%7D%20T%20%5Ctimes%7B%20%7D_%7B5%7D%5E%7B4%7D%20T%20%5Ctimes%7B%20%7D_%7B6%7D%5E%7B5%7D%20T$ ，只要知道六个关节角，就可以求出变换矩阵

代码

可自行下载，链接如下：

https://download.csdn.net/download/sinat_32804425/70045943

测试

在URSim上打开UR5的虚拟机，进入到move界面，选择Base基座坐标系，随意设置一个姿态，将六个关节角输入，可以检验正运动学计算正确与否。

运行正运动学程序，输入：93.14 -62.68 108.27 -135.56 -66.46 15.59

将变换矩阵T前三行打印出来，可以看到最后的位置与URSim中TCP的三个位置相同。

逆运动学

逆运动学就是已知末端的位姿，求六个关节角的过程，一般都会求出八组解，但是只有一组是最优解，通常试两次就可确定哪一组。

变换矩阵

根据末端执行器的位姿（绕静轴的姿态），求解末端坐标系到基座坐标系的变换矩阵。这里推荐一个视频，可以帮助理解绕静轴和绕动轴两种变换方式【机械臂运动学教程】机械臂+旋转矩阵+变换矩阵+DH+逆解+轨迹规划+机器人+教程_哔哩哔哩_bilibili

在URSim中也有两种表示位姿的方式，一种是Rotation Vector，另一种是RPY。所谓RPY就是绕参考坐标系，绕定轴X（Roll）—Y（Pitch）—Z（Yaw）旋转，旋转矩阵左乘。我们计算变换矩阵一定要采用RPY[rad]这种模式下的姿态角，要不然始终会有问题。RPY是绕静轴的一种旋转方式，理解可参考机器人RPY角和Euler角 -- 基本公式_lyhbkz的博客-CSDN博客_rpy和【机器人运动学/姿态角】欧拉角和RPY角_Amanda1m的博客-CSDN博客_rpy角

我这里采用绕静轴RPY的公式来计算旋转矩阵

则位姿为 $eq?%5Cleft%5Bp_%7Bx%7D%2C%20p_%7By%7D%2C%20p_%7Bz%7D%2C%20%5Cgamma%2C%20%5Cbeta%2C%20%5Calpha%5Cright%5D$ 时，旋转矩阵可以由如下公式计算得出。

$eq?%7B%20%7D_%7B6%7D%5E%7B0%7D%20R%3D%5Cleft%5B%5Cbegin%7Barray%7D%7Blll%7D%20n_%7Bx%7D%20%26%20o_%7Bx%7D%20%26%20a_%7Bx%7D%20%5C%5C%20n_%7By%7D%20%26%20o_%7By%7D%20%26%20a_%7By%7D%20%5C%5C%20n_%7Bz%7D%20%26%20o_%7Bz%7D%20%26%20a_%7Bz%7D%20%5Cend%7Barray%7D%5Cright%5D$

$eq?%3D%5Cleft%5B%5Cbegin%7Barray%7D%7Bccc%7D%20c%20%5Calpha%20c%20%5Cbeta%20%26%20c%20%5Calpha%20s%20%5Cbeta%20s%20%5Cgamma-s%20%5Calpha%20c%20%5Cgamma%20%26%20c%20%5Calpha%20s%20%5Cbeta%20c%20%5Cgamma+s%20%5Calpha%20s%20%5Cgamma%20%5C%5C%20s%20%5Calpha%20c%20%5Cbeta%20%26%20s%20%5Calpha%20s%20%5Cbeta%20s%20%5Cgamma+c%20%5Calpha%20c%20%5Cgamma%20%26%20s%20%5Calpha%20s%20%5Cbeta%20c%20%5Cgamma-c%20%5Calpha%20s%20%5Cgamma%20%5C%5C%20-s%20%5Cbeta%20%26%20c%20%5Cbeta%20s%20%5Cgamma%20%26%20c%20%5Cbeta%20c%20%5Cgamma%20%5Cend%7Barray%7D%5Cright%5D$

至此得到了变换矩阵T，下面开始逐步计算关节角。下列公式参考UR5型六轴机械臂运动学推导及效果演示 - 古月居

求逆解时一定要用atan2()来求解，Eigen库中自带这个函数。原理可参考UR机械臂正逆运动学求解_fengyu19930920的博客-CSDN博客_机械臂正逆运动学

关节1的求解

已知

$eq?%7B%20%7D_%7B6%7D%5E%7B0%7D%20T%3D%5Cleft%5B%5Cbegin%7Barray%7D%7Bcccc%7D%20n_%7Bx%7D%20%26%20o_%7Bx%7D%20%26%20a_%7Bx%7D%20%26%20p_%7Bx%7D%20%5C%5C%20n_%7By%7D%20%26%20o_%7By%7D%20%26%20a_%7By%7D%20%26%20p_%7By%7D%20%5C%5C%20n_%7Bz%7D%20%26%20o_%7Bz%7D%20%26%20a_%7Bz%7D%20%26%20p_%7Bz%7D%20%5C%5C%200%20%26%200%20%26%200%20%26%201%20%5Cend%7Barray%7D%5Cright%5D%3D%7B%20%7D_%7B1%7D%5E%7B0%7D%20T%20%5Ccdot%7B%20%7D_%7B2%7D%5E%7B1%7D%20T%20%5Ccdot%7B%20%7D_%7B3%7D%5E%7B2%7D%20T%20%5Ccdot%7B%20%7D_%7B4%7D%5E%7B3%7D%20T%20%5Ccdot%7B%20%7D_%7B5%7D%5E%7B4%7D%20T%20%5Ccdot%7B%20%7D_%7B6%7D%5E%7B5%7D%20T$

则 $eq?%7B%20%7D_%7B1%7D%5E%7B0%7D%20T%5E%7B-1%7D%20%5Ccdot%20T%20%5Ccdot%7B%20%7D_%7B6%7D%5E%7B5%7D%20T%5E%7B-1%7D%3D%7B%20%7D_%7B5%7D%5E%7B1%7D%20T%3D%7B%20%7D_%7B2%7D%5E%7B1%7D%20T%20%5Ccdot%7B%20%7D_%7B3%7D%5E%7B2%7D%20T%20%5Ccdot%7B%20%7D_%7B4%7D%5E%7B3%7D%20T%20%5Ccdot%7B%20%7D_%7B5%7D%5E%7B4%7D%20T$

$eq?%7B%20%7D_%7B1%7D%5E%7B0%7D%20T%5E%7B-1%7D%3D%5Cleft%5B%5Cbegin%7Barray%7D%7Bcccc%7D%20%5Ccos%20%5Ctheta_%7B1%7D%20%26%20%5Csin%20%5Ctheta_%7B1%7D%20%26%200%20%26%200%20%5C%5C%200%20%26%200%20%26%201%20%26%20-d_%7B1%7D%20%5C%5C%20%5Csin%20%5Ctheta_%7B1%7D%20%26%20-%5Ccos%20%5Ctheta_%7B1%7D%20%26%200%20%26%200%20%5C%5C%200%20%26%200%20%26%200%20%26%201%20%5Cend%7Barray%7D%5Cright%5D%2C%7B%20%7D_%7B6%7D%5E%7B5%7D%20T%5E%7B-1%7D%3D%5Cleft%5B%5Cbegin%7Barray%7D%7Bcccc%7D%20%5Ccos%20%5Ctheta_%7B6%7D%20%26%20%5Csin%20%5Ctheta_%7B6%7D%20%26%200%20%26%200%20%5C%5C%20-%5Csin%20%5Ctheta_%7B6%7D%20%26%20%5Ccos%20%5Ctheta_%7B6%7D%20%26%200%20%26%200%20%5C%5C%200%20%26%200%20%26%201%20%26%20-d_%7B6%7D%20%5C%5C%200%20%26%200%20%26%200%20%26%201%20%5Cend%7Barray%7D%5Cright%5D$

所以 $eq?%7B%20%7D_%7B1%7D%5E%7B0%7D%20T%5E%7B-1%7D%20%5Ccdot%20T%20%5Ccdot%7B%20%7D_%7B6%7D%5E%7B5%7D%20T%5E%7B-1%7D%3D$

$eq?%5Cleft%5B%5Cbegin%7Barray%7D%7Bcccc%7D%20c_%7B6%7D%5Cleft%28n_%7Bx%7D%20c_%7B1%7D+n_%7By%7D%20s_%7B1%7D%5Cright%29-s_%7B6%7D%5Cleft%28o_%7Bx%7D%20c_%7B1%7D+o_%7By%7D%20s_%7B1%7D%5Cright%29%20%26%20s_%7B6%7D%5Cleft%28n_%7Bx%7D%20c_%7B1%7D+n_%7By%7D%20s_%7B1%7D%5Cright%29+c_%7B6%7D%5Cleft%28o_%7Bx%7D%20c_%7B1%7D+o_%7By%7D%20s_%7B1%7D%5Cright%29%20%26%20a_%7Bx%7D%20c_%7B1%7D+a_%7By%7D%20s_%7B1%7D%20%26%20p_%7Bx%7D%20c_%7B1%7D-d_%7B6%7D%5Cleft%28a_%7Bx%7D%20c_%7B1%7D+a_%7By%7D%20s_%7B1%7D%5Cright%29+p_%7By%7D%20s_%7B1%7D%20%5C%5C%20n_%7Bx%7D%20c_%7B6%7D-o_%7Bz%7D%20s_%7B6%7D%20%26%20o_%7Bz%7D%20c_%7B6%7D+n_%7Bz%7D%20s_%7B6%7D%20%26%20a_%7Bz%7D%20%26%20p_%7Bz%7D-d_%7B1%7D-a_%7Bz%7D%20d_%7B6%7D%20%5C%5C%20s_%7B6%7D%5Cleft%28o_%7By%7D%20c_%7B1%7D-o_%7Bx%7D%20s_%7B1%7D%5Cright%29-c_%7B6%7D%5Cleft%28n_%7By%7D%20c_%7B1%7D-n_%7Bx%7D%20s_%7B1%7D%5Cright%29%20%26%20-s_%7B6%7D%5Cleft%28n_%7By%7D%20c_%7B1%7D-n_%7Bx%7D%20s_%7B1%7D%5Cright%29-c_%7B6%7D%5Cleft%28o_%7By%7D%20c_%7B1%7D-o_%7Bx%7D%20s_%7B1%7D%5Cright%29%20%26%20a_%7Bx%7D%20s_%7B1%7D-a_%7By%7D%20c_%7B1%7D%20%26%20-p_%7By%7D%20c_%7B1%7D+d_%7B6%7D%5Cleft%28a_%7By%7D%20c_%7B1%7D-a_%7Bx%7D%20s_%7B1%7D%5Cright%29+p_%7Bx%7D%20s_%7B1%7D%20%5C%5C%200%20%26%200%20%26%200%20%26%201%20%5Cend%7Barray%7D%5Cright%5D$

而

$eq?%7B%20%7D_%7B2%7D%5E%7B1%7D%20T%20%5Ccdot%7B%20%7D_%7B3%7D%5E%7B2%7D%20T%20%5Ccdot%7B%20%7D_%7B4%7D%5E%7B3%7D%20T%20%5Ccdot%7B%20%7D_%7B5%7D%5E%7B4%7D%20T%3D%5Cleft%5B%5Cbegin%7Barray%7D%7Bcccc%7D%20c_%7B234%7D%20c_%7B5%7D%20%26%20-s_%7B234%7D%20%26%20-c_%7B234%7D%20s_%7B5%7D%20%26%20a_%7B3%7D%20c_%7B23%7D+a_%7B2%7D%20c_%7B2%7D+d_%7B2%7D%20s_%7B234%7D%20%5C%5C%20s_%7B234%7D%20c_%7B5%7D%20%26%20c_%7B234%7D%20%26%20-s_%7B234%7D%20s_%7B5%7D%20%26%20a_%7B3%7D%20s_%7B23%7D+a_%7B2%7D%20s_%7B2%7D-d_%7B5%7D%20c_%7B234%7D%20%5C%5C%20s_%7B5%7D%20%26%200%20%26%20c_%7B5%7D%20%26%20d_%7B4%7D%20%5C%5C%200%20%26%200%20%26%200%20%26%201%20%5Cend%7Barray%7D%5Cright%5D$

根据两式第三行第四列相等，则可以得到

$eq?d_%7B4%7D%3D-p_%7By%7D%20c_%7B1%7D+d_%7B6%7D%5Cleft%28a_%7By%7D%20c_%7B1%7D-a_%7Bx%7D%20s_%7B1%7D%5Cright%29+p_%7Bx%7D%20s_%7B1%7D$

另 $eq?m%3Dd_%7B6%7D%20a_%7By%7D-p_%7By%7D%2C%20n%3Da_%7Bx%7D%20d_%7B6%7D-p_%7Bx%7D$ ，有

$eq?%5Ctheta_%7B1%7D%3Da%20%5Ctan%202%28m%2C%20n%29-a%20%5Ctan%202%5Cleft%28d_%7B4%7D%2C%20%5Cpm%20%5Csqrt%7Bm%5E%7B2%7D+n%5E%7B2%7D-d_%7B4%7D%5E%7B2%7D%7D%5Cright%29$

共两组解。

关节5的求解

根据两式第三行第三列相等，有 $eq?c_%7B5%7D%3Da_%7Bx%7D%20s_%7B1%7D-a_%7By%7D%20c_%7B1%7D$ ，则

$eq?%5Ctheta_%7B5%7D%3D%5Cpm%20%5Carccos%20%5Cleft%28a_%7Bx%7D%20s_%7B1%7D-a_%7By%7D%20c_%7B1%7D%5Cright%29$

至此共四组解。

关节6的求解

根据两式第三行第三列相等，有 $eq?s_%7B5%7D%3Ds_%7B6%7D%5Cleft%28o_%7By%7D%20c_%7B1%7D-o_%7Bx%7D%20s_%7B1%7D%5Cright%29-c_%7B6%7D%5Cleft%28n_%7By%7D%20c_%7B1%7D-n_%7Bx%7D%20s_%7B1%7D%5Cright%29$

令 $eq?m%20m%3Dn_%7Bx%7D%20s_%7B1%7D-n_%7By%7D%20c_%7B1%7D%2C%20%5Cquad%20n%20n%3Do_%7Bx%7D%20s_%7B1%7D-o_%7By%7D%20c_%7B1%7D$ ，有

$eq?%5Ctheta_%7B6%7D%3Da%20%5Ctan%202%28m%20m%2C%20n%20n%29-a%20%5Ctan%202%5Cleft%28s_%7B5%7D%2C%200%5Cright%29$

至此共四组解。

关节3的求解

由 $eq?%7B%20%7D_%7B1%7D%5E%7B0%7D%20T%5E%7B-1%7D%20%5Ccdot%20T%20%5Ccdot%7B%20%7D_%7B6%7D%5E%7B5%7D%20T%5E%7B-1%7D%20%5Ccdot%7B%20%7D_%7B5%7D%5E%7B4%7D%20T%5E%7B-1%7D%3D%7B%20%7D_%7B4%7D%5E%7B1%7D%20T%3D%7B%20%7D_%7B2%7D%5E%7B1%7D%20T%20%5Ccdot%7B%20%7D_%7B3%7D%5E%7B2%7D%20T%20%5Ccdot%7B%20%7D_%7B4%7D%5E%7B3%7D%20T$ ，而且

$eq?%7B%20%7D_%7B5%7D%5E%7B4%7D%20T%5E%7B-1%7D%3D%5Cleft%5B%5Cbegin%7Barray%7D%7Bcccc%7D%20%5Ccos%20%5Ctheta_%7B5%7D%20%26%20%5Csin%20%5Ctheta_%7B5%7D%20%26%200%20%26%200%20%5C%5C%200%20%26%200%20%26%20-1%20%26%20d_%7B5%7D%20%5C%5C%20-%5Csin%20%5Ctheta_%7B5%7D%20%26%20%5Ccos%20%5Ctheta_%7B5%7D%20%26%200%20%26%200%20%5C%5C%200%20%26%200%20%26%200%20%26%201%20%5Cend%7Barray%7D%5Cright%5D$

所以有 $eq?%7B%20%7D_%7B1%7D%5E%7B0%7D%20T%5E%7B-1%7D%20%5Ccdot%20T%20%5Ccdot%7B%20%7D_%7B6%7D%5E%7B5%7D%20T%5E%7B-1%7D%20%5Ccdot%7B%20%7D_%7B5%7D%5E%7B4%7D%20T%5E%7B-1%7D%3D$

$eq?%5Cleft%5B%5Cbegin%7Barray%7D%7Bcc%7D%20c_%7B5%7D%5Cleft%28c_%7B6%7D%5Cleft%28n_%7Bx%7D%20c_%7B1%7D+n_%7By%7D%20s_%7B1%7D%5Cright%29-s_%7B6%7D%5Cleft%28o_%7Bx%7D%20c_%7B1%7D+o_%7By%7D%20s_%7B1%7D%5Cright%29%5Cright%29-s_%7B5%7D%5Cleft%28a_%7Bx%7D%20c_%7B1%7D+a_%7By%7D%20s_%7B1%7D%5Cright%29%20%26%20s_%7B5%7D%5Cleft%28c_%7B6%7D%5Cleft%28n_%7Bx%7D%20c_%7B1%7D+n_%7By%7D%20s_%7B1%7D%5Cright%29-s_%7B6%7D%5Cleft%28o_%7Bx%7D%20c_%7B1%7D+o_%7By%7D%20s_%7B1%7D%5Cright%29%5Cright%29+c_%7B5%7D%5Cleft%28a_%7Bx%7D%20c_%7B1%7D+a_%7By%7D%20s_%7B1%7D%5Cright%29%20%5C%5C%20c_%7B5%7D%5Cleft%28n_%7Bx%7D%20c_%7B6%7D-o_%7Bz%7D%20s_%7B6%7D%5Cright%29-s_%7B5%7D%20a_%7Bz%7D%20%26%20s_%7B5%7D%5Cleft%28n_%7Bx%7D%20c_%7B6%7D-o_%7Bz%7D%20s_%7B6%7D%5Cright%29+c_%7B5%7D%20a_%7Bz%7D%20%5C%5C%20c_%7B5%7D%5Cleft%28s_%7B6%7D%5Cleft%28o_%7By%7D%20c_%7B1%7D-o_%7Bx%7D%20s_%7B1%7D%5Cright%29-c_%7B6%7D%5Cleft%28n_%7By%7D%20c_%7B1%7D-n_%7Bx%7D%20s_%7B1%7D%5Cright%29%5Cright%29-s_%7B5%7D%5Cleft%28a_%7Bx%7D%20s_%7B1%7D-a_%7By%7D%20c_%7B1%7D%5Cright%29%20%26%20s_%7B5%7D%5Cleft%28s_%7B6%7D%5Cleft%28o_%7By%7D%20c_%7B1%7D-o_%7Bx%7D%20s_%7B1%7D%5Cright%29-c_%7B6%7D%5Cleft%28n_%7By%7D%20c_%7B1%7D-n_%7Bx%7D%20s_%7B1%7D%5Cright%29%5Cright%29+c_%7B5%7D%5Cleft%28a_%7Bx%7D%20s_%7B1%7D-a_%7By%7D%20c_%7B1%7D%5Cright%29%20%5C%5C%200%20%26%200%20%5Cend%7Barray%7D%5Cright.$ $eq?%5Cleft.%5Cbegin%7Barray%7D%7Bcc%7D%20-%5Cleft%28s_%7B6%7D%5Cleft%28n_%7Bx%7D%20c_%7B1%7D+n_%7By%7D%20s_%7B1%7D%5Cright%29+c_%7B6%7D%5Cleft%28o_%7Bx%7D%20c_%7B1%7D+o_%7By%7D%20s_%7B1%7D%5Cright%29%5Cright%29%20%26%20d_%7B5%7D%5Cleft%28s_%7B6%7D%5Cleft%28n_%7Bx%7D%20c_%7B1%7D+n_%7By%7D%20s_%7B1%7D%5Cright%29+c_%7B6%7D%5Cleft%28o_%7Bx%7D%20c_%7B1%7D+o_%7By%7D%20s_%7B1%7D%5Cright%29%5Cright%29+p_%7Bx%7D%20c_%7B1%7D-d_%7B6%7D%5Cleft%28a_%7Bx%7D%20c_%7B1%7D+a_%7By%7D%20s_%7B1%7D%5Cright%29+p_%7By%7D%20s_%7B1%7D%20%5C%5C%20-%5Cleft%28o_%7Bz%7D%20c_%7B6%7D+n_%7Bz%7D%20s_%7B6%7D%5Cright%29%20%26%20d_%7B5%7D%5Cleft%28o_%7Bz%7D%20c_%7B6%7D+n_%7Bz%7D%20s_%7B6%7D%5Cright%29+p_%7Bz%7D-d_%7B1%7D-a_%7Bz%7D%20d_%7B6%7D%20%5C%5C%20s_%7B6%7D%5Cleft%28n_%7By%7D%20c_%7B1%7D-n_%7Bx%7D%20s_%7B1%7D%5Cright%29+c_%7B6%7D%5Cleft%28o_%7By%7D%20c_%7B1%7D-o_%7Bx%7D%20s_%7B1%7D%5Cright%29%20%26%20d_%7B5%7D%5Cleft%28-s_%7B6%7D%5Cleft%28n_%7By%7D%20c_%7B1%7D-n_%7Bx%7D%20s_%7B1%7D%5Cright%29-c_%7B6%7D%5Cleft%28o_%7By%7D%20c_%7B1%7D-o_%7Bx%7D%20s_%7B1%7D%5Cright%29%5Cright%29-p_%7By%7D%20c_%7B1%7D+d_%7B6%7D%5Cleft%28a_%7By%7D%20c_%7B1%7D-a_%7Bx%7D%20s_%7B1%7D%5Cright%29+p_%7Bx%7D%20s_%7B1%7D%20%5C%5C%200%20%26%201%20%5Cend%7Barray%7D%5Cright%5D$

因为

$eq?%7B%20%7D_%7B2%7D%5E%7B1%7D%20T%20%5Ccdot%7B%20%7D_%7B3%7D%5E%7B2%7D%20T%20%5Ccdot%7B%20%7D_%7B4%7D%5E%7B3%7D%20T%3D%5Cleft%5B%5Cbegin%7Barray%7D%7Bcccc%7D%20c_%7B234%7D%20%26%200%20%26%20s_%7B234%7D%20%26%20a_%7B3%7D%20c_%7B23%7D+a_%7B2%7D%20c_%7B2%7D%20%5C%5C%20s_%7B234%7D%20%26%200%20%26%20-c_%7B234%7D%20%26%20a_%7B3%7D%20s_%7B23%7D+a_%7B2%7D%20s_%7B2%7D%20%5C%5C%200%20%26%201%20%26%200%20%26%20d_%7B4%7D%20%5C%5C%200%20%26%200%20%26%200%20%26%201%20%5Cend%7Barray%7D%5Cright%5D$

根据两式第一行第四列相等，第二行第四列相等，有

$eq?%5Cbegin%7Baligned%7D%20%26d_%7B5%7D%5Cleft%28s_%7B6%7D%5Cleft%28n_%7Bx%7D%20c_%7B1%7D+n_%7By%7D%20s_%7B1%7D%5Cright%29+c_%7B6%7D%5Cleft%28o_%7Bx%7D%20c_%7B1%7D+o_%7By%7D%20s_%7B1%7D%5Cright%29%5Cright%29+p_%7Bx%7D%20c_%7B1%7D-d_%7B6%7D%5Cleft%28a_%7Bx%7D%20c_%7B1%7D+a_%7By%7D%20s_%7B1%7D%5Cright%29+p_%7By%7D%20s_%7B1%7D%3Da_%7B3%7D%20c_%7B23%7D+a_%7B2%7D%20c_%7B2%7D%20%5C%5C%20%26d_%7B5%7D%5Cleft%28o_%7Bz%7D%20c_%7B6%7D+n_%7Bz%7D%20s_%7B6%7D%5Cright%29+p_%7Bz%7D-d_%7B1%7D-a_%7Bz%7D%20d_%7B6%7D%3Da_%7B3%7D%20s_%7B23%7D+a_%7B2%7D%20s_%7B2%7D%20%5Cend%7Baligned%7D$

令

$eq?m%20m%20m%3Dd_%7B5%7D%5Cleft%28s_%7B6%7D%5Cleft%28n_%7Bx%7D%20c_%7B1%7D+n_%7By%7D%20s_%7B1%7D%5Cright%29+c_%7B6%7D%5Cleft%28o_%7Bx%7D%20c_%7B1%7D+o_%7By%7D%20s_%7B1%7D%5Cright%29%5Cright%29+p_%7Bx%7D%20c_%7B1%7D-d_%7B6%7D%5Cleft%28a_%7Bx%7D%20c_%7B1%7D+a_%7By%7D%20s_%7B1%7D%5Cright%29+p_%7By%7D%20s_%7B1%7D$

$eq?%5Ctext%20%7B%20nnn%20%7D%3Dd_%7B5%7D%5Cleft%28o_%7Bz%7D%20c_%7B6%7D+n_%7Bz%7D%20s_%7B6%7D%5Cright%29+p_%7Bz%7D-d_%7B1%7D-a_%7Bz%7D%20d_%7B6%7D$

则有

$eq?%5Ctheta_%7B3%7D%3D%5Cpm%20%5Carccos%20%5Cleft%28%5Cfrac%7Bm%20m%20m%5E%7B2%7D+n%20n%20n%5E%7B2%7D-a_%7B2%7D%5E%7B2%7D-a_%7B3%7D%5E%7B2%7D%7D%7B2%20a_%7B2%7D%20a_%7B3%7D%7D%5Cright%29$

至此共八组解。

关节2的求解

由上述推导，可令

$eq?s_%7B2%7D%3D%5Cfrac%7B%5Cleft%28a_%7B3%7D%20c_%7B3%7D+a_%7B2%7D%5Cright%29%20n%20n%20n-a_%7B3%7D%20s_%7B3%7D%20m%20m%20m%7D%7Ba_%7B2%7D%5E%7B2%7D+a_%7B3%7D%5E%7B2%7D+2%20a_%7B2%7D%20a_%7B3%7D%20c_%7B3%7D%7D%2C%20%5Cquad%20c_%7B2%7D%3D%5Cfrac%7Bm%20m%20m+a_%7B3%7D%20s_%7B3%7D%20s_%7B2%7D%7D%7Ba_%7B3%7D%20c_%7B3%7D+a_%7B2%7D%7D$

所以

$eq?%5Ctheta_%7B2%7D%3Da%20%5Ctan%202%5Cleft%28s_%7B2%7D%2C%20c_%7B2%7D%5Cright%29$

至此共八组解。

关节4的求解

由 $eq?%7B%20%7D_%7B1%7D%5E%7B0%7D%20T%5E%7B-1%7D%20%5Ccdot%20T%20%5Ccdot%7B%20%7D_%7B6%7D%5E%7B5%7D%20T%5E%7B-1%7D%3D%7B%20%7D_%7B5%7D%5E%7B1%7D%20T%3D%7B%20%7D_%7B2%7D%5E%7B1%7D%20T%20%5Ccdot%7B%20%7D_%7B3%7D%5E%7B2%7D%20T%20%5Ccdot%7B%20%7D_%7B4%7D%5E%7B3%7D%20T%20%5Ccdot%7B%20%7D_%7B5%7D%5E%7B4%7D%20T$ ，可设

$eq?%5Cbegin%7Bgathered%7D%20-s_%7B234%7D%3Ds_%7B6%7D%5Cleft%28n_%7Bx%7D%20c_%7B1%7D+n_%7By%7D%20s_%7B1%7D%5Cright%29+c_%7B6%7D%5Cleft%28o_%7Bx%7D%20c_%7B1%7D+o_%7By%7D%20s_%7B1%7D%5Cright%29%20%5C%5C%20c_%7B234%7D%3Do_%7Bz%7D%20c_%7B6%7D+n_%7Bz%7D%20s_%7B6%7D%20%5Cend%7Bgathered%7D$

所以

$eq?%5Ctheta_%7B4%7D%3Da%20%5Ctan%202%5Cleft%28-s_%7B6%7D%5Cleft%28n_%7Bx%7D%20c_%7B1%7D+n_%7By%7D%20s_%7B1%7D%5Cright%29-c_%7B6%7D%5Cleft%28o_%7Bx%7D%20c_%7B1%7D+o_%7By%7D%20s_%7B1%7D%5Cright%29%2C%20o_%7Bz%7D%20c_%7B6%7D+n_%7Bz%7D%20s_%7B6%7D%5Cright%29-%5Ctheta_%7B2%7D-%5Ctheta_%7B3%7D$