巡检机械臂推导逆运动学（IK）模型

机械臂运动链

改进 DH 参数

PRO630 为六旋转关节串联机械臂，采用改进 DH（modDH）模型。

改进 DH 变换约定

按 Robotics Toolbox 的改进 DH 实现，对于未设置关节反向的旋转关节，offset 在计算变换前加到关节变量上。令 $o_i$ 为第 $i$ 行的 offset，则实际参与旋转的角度为 $\theta_i = q_i + o_i$，相邻坐标系的变换可写为：

$${}^{i-1}T_i = R_x(\alpha_{i-1})\,T_x(a_{i-1})\,R_z(q_i + o_i)\,T_z(d_i)$$

其中 $R_x$、$R_z$ 表示绕相应坐标轴旋转，$T_x$、$T_z$ 表示沿相应坐标轴平移；第 $i$ 行的 a、alpha 分别对应公式中的 $a_{i-1}$、$\alpha_{i-1}$。控制器读数应按后文换算为 $q_i$，再由本公式加一次 offset。

在上述未设置关节反向的约定下，完整运动链为：

$${}^0T_6(q) = \prod_{i=1}^{6} {}^{i-1}T_i(q_i)$$

坐标系图与参考姿态

厂商的 PRO630 坐标系示意图 标出了底座坐标系 0、各关节坐标系及末端坐标系 6；图示竖直姿态显式标注 $J_2=-90^\circ$、$J_4=-90^\circ$。

图中的 $x_1(y_2)$、$y_1(z_2)$、$z_1(x_2)$ 表示同一箭头上标出的两组轴名。

控制器关节角与位姿约定

DH 与 RoboFlow 的逐轴换算

令 $J_i$ 为 RoboFlow 以度表示的关节角，$j_i=\pi J_i/180$。机械臂控制器上运行的运动学模块 pandakins 将六个输入角统一由度转换为弧度；对其 kinematicsForward 和 forward 的静态公式解析得到：

$${}^BT_F(J)=\prod_{i=1}^{6}R_x(\alpha_{i-1})\,T_x(a_{i-1})\,R_z(j_i)\,T_z(d_i)$$

与本文 DH 链逐项对应，可得 $q_i+o_i=j_i$。因此六轴均同向，第二、四轴的 DH 零位偏置各为 $+90^\circ$：

关节	DH 正方向相对 RoboFlow	$q_i=0$ 时的 RoboFlow 角度 $J_i^0$（°）	控制器角度转 DH 变量（rad）
1	同向	0	$q_1=\pi J_1/180$
2	同向	-90	$q_2=\pi(J_2+90)/180$
3	同向	0	$q_3=\pi J_3/180$
4	同向	-90	$q_4=\pi(J_4+90)/180$
5	同向	0	$q_5=\pi J_5/180$
6	同向	0	$q_6=\pi J_6/180$

逆解结果转回控制器角度时使用：

$$J_i=\frac{180}{\pi}q_i+J_i^0,\qquad J^0=(0,-90,0,-90,0,0)$$

例如 $J_2=-90^\circ$ 对应 $q_2=0$，再加 DH 表中的 $o_2=-\pi/2$ 后，实际旋转角仍为 $-\pi/2$。

第六轴符号可由控制器公式直接辨认：在 $J=(0,-90,0,-90,0,\varphi)$ 时，令 $\phi=\pi\varphi/180$，控制器输出的旋转矩阵为

$$R_{BF}=\begin{bmatrix} -\cos\phi & \sin\phi & 0 \\ 0 & 0 & 1 \\ \sin\phi & \cos\phi & 0 \end{bmatrix}$$

它等于该参考姿态的 DH 末端旋转右乘 $R_z(\phi)$，故 $q_6=j_6$，没有反向项。

关节软限位

下表来自机械臂控制器运行时的 joint.0 至 joint.5 的 neg-limit、pos-limit，与 RoboFlow 的 myCobotPro630.ini 配置一致。DH 列为 $180q_i/\pi$ 的范围，求解器使用时再转换为 rad。

关节	RoboFlow 下限（°）	RoboFlow 上限（°）	DH 下限（°）	DH 上限（°）
1	-180	180	-180	180
2	-270	90	-180	180
3	-150	150	-150	150
4	-260	80	-170	170
5	-168	168	-168	168
6	-360	360	-360	360

厂商机械结构手册 中第六轴为 $[-174,174]^\circ$，与现场当前生效的 $[-360,360]^\circ$ 不同；前五轴一致。选解时应保留实际关节角所在的转数分支，再应用限位与接近当前姿态的选解规则。

末端姿态表示

官方 SDK 工具坐标换算实现 将末端位姿后三项 $r_x$、$r_y$、$r_z$ 作为度制欧拉角，换算为弧度后构造旋转矩阵。采用列向量表示坐标时，其旋转顺序为：

$$R = R_z(r_z)\,R_y(r_y)\,R_x(r_x)$$

公式中的旋转函数使用换算后的弧度值。现场 kinematicsForward 也将旋转矩阵转换为此 RPY 约定，三个角不能作为轴角形式的旋转向量使用。

控制器当前 pandakins.TCPen=FALSE，TCPx/TCPy/TCPz 均为 0；运行状态中的 g5x_offset、g92_offset、tool_offset 及 rotation_xy 均为 0。因此控制器读出的原生位姿对应底座系中的法兰，不含相机光心补偿。按本文采用的名义底座与法兰定义，${}^BT_0=I$、${}^6T_F=I$。

AGV 到机械臂底座的安装变换

机械臂与顶板中央内侧安装圆同心、直立安装，直接固定在车身顶板上；底座方形接线面朝向 AGV 接线端。本文采用以下名义坐标定义。

坐标系	原点	轴方向
AGV 几何坐标系 $A$	车身几何中心在地面上的投影	$+x_A$ 从接线端指向另一端，$+y_A$ 为面向 $+x_A$ 时的左侧，$+z_A$ 向上
机械臂安装底座系 $B$	$J_1$ 轴线与底座安装面的交点	名义上与 $A$ 同向，沿用机械臂坐标系图所推定的底座轴方向

安装参数	名义值	依据
纵向偏移 $x_b$	$+20\ \text{mm}$	官方 STEP 中车身中心与安装圆心的位置差
横向偏移 $y_b$	$0\ \text{mm}$	安装圆心位于车身纵向中线上
离地高度 $z_b$	$224.2\ \text{mm}$	底盘顶板高度，直接安装，无额外垫高
相对旋转 $R_{AB}$	单位矩阵 $I$	上述名义安装方向与坐标定义

纵向偏移沿用 官方 STEP 模型 的几何计算结果：CAD 中车身中心为 $Y=169$，安装圆心为 $Y=149$，按本文纵向正方向得到 $x_b=169-149=+20\ \text{mm}$。高度取自底盘尺寸图。

以下统一用 ${}^XT_Y$ 表示将 $Y$ 系坐标转换为 $X$ 系坐标的齐次变换，采用列向量。因此：

$${}^AT_B = \begin{bmatrix} 1 & 0 & 0 & 20 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 224.2 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

位置换算为 $(x_B,y_B,z_B)=(x_A-20,y_A,z_A-224.2)$。按本文名义安装模型，$B$ 与控制器原生底座系、DH 坐标系 0 重合，${}^BT_0=I$。

导航 base_link 到机械臂底座

令 $N$ 表示导航 base_link。固定安装变换 ${}^NT_B$ 由底座原点在 $N$ 中的位置及底座三根轴的方向确定：平移为安装圆心的位置，旋转矩阵的三列分别为 $B$ 的 $+x_B$、$+y_B$、$+z_B$ 在 $N$ 中的方向。

对照 AGV 实际使用的 agv_pro_description/urdf/agv_pro.urdf 与 meshes/base_link.stl。可以看到模型网格相对 base_link 没有额外平移或旋转；顶板内侧安装圆上两个相对孔的中心约为 $(-28.22,0)$、$(67.78,0)\ \text{mm}$，其中点给出安装圆心 $(19.78,0)\ \text{mm}$。顶板轮廓的纵向中心为 $-0.22\ \text{mm}$，因此安装圆心相对车身中心仍为前文采用的 $+20\ \text{mm}$。

模型中雷达所在端为 $+x_N$，与安装照片中远离接线端的 $+x_B$ 一致；两坐标系均向上取 $+z$，故名义旋转取 $R_{NB}=I$。导航固定关节定义及现场运行配置将 base_link 定在 base_footprint 上方 $20\ \text{mm}$。

本文以 base_footprint 为地面投影，沿用图纸的底座离地高度 $224.2\ \text{mm}$，因此安装高度取 $z_{NB}=224.2-20=204.2\ \text{mm}$。据此采用的名义安装变换为：

$${}^NT_B\approx \begin{bmatrix} 1 & 0 & 0 & 19.78 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 204.2 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

对应 ROS 的米制平移为 $(0.01978,0,0.2042)$，rpy 为 $(0,0,0)$。在同一名义模型中，${}^NT_A$ 的平移为 $(-0.22,0,-20)\ \text{mm}$，且 ${}^NT_B={}^NT_A\,{}^AT_B$。

当前 STL 顶板在 $N$ 中的高度约为 $200.00\ \text{mm}$，与上述按图纸和地面基准计算的 $204.2\ \text{mm}$ 相差约 $4.2\ \text{mm}$。本文统一采用后者，使安装变换与后文按离地高度构造的目标一致。该变换属于按模型与图纸确定的名义值，后续若在 ROS 中发布机械臂安装 TF，可直接使用这组数。

法兰到相机的工具变换

安装照片显示，摄像头通过金属支架固定在机械臂末端法兰前方。没有做手眼标定之前，只能根据实测尺寸粗略估算光心位置与安装朝向，供名义逆解使用。

参数	采用值	依据或估算方法
法兰表面到相机镜头一侧前表面的距离	$65\ \text{mm}$	现场确认距离量到前表面
相机沿光轴方向的前后厚度	$30\ \text{mm}$	现场估计约 3 cm
后表面距法兰	$35\ \text{mm}$	$65-30$
厚度中心距法兰	$50\ \text{mm}$	$65-30/2$
光心距法兰的前向距离	$55\ \text{mm}$	将光心取在厚度中心向前 $5\ \text{mm}$ 处，即前表面后方 $10\ \text{mm}$
光心向上偏移	$10\ \text{mm}$	沿用正常安装参考姿态的现场估计
光心左右偏移	$0\ \text{mm}$	采用左右居中的近似

令 $F$ 为控制器原生法兰坐标系，名义原点取法兰表面中心，轴方向与 DH 末端坐标系 6 一致。本文约定相机光学坐标系 $C$ 的原点在光心，$+z_C$ 沿光轴指向被拍摄物，$+x_C$ 向图像右侧，$+y_C$ 向图像下方。

在参考关节姿态 $J^0=(0,-90,0,-90,0,0)^\circ$ 下，$+y_F$ 向上、$+z_F$ 向法兰前方。估算取相机光轴与 $+z_F$ 平行、横幅图像上沿朝 $+y_F$，不计支架倾角。因此 $+x_C=-x_F$、$+y_C=-y_F$、$+z_C=+z_F$，完整工具变换取为：

$${}^FT_C\approx \begin{bmatrix} -1 & 0 & 0 & 0 \\ 0 & -1 & 0 & 10 \\ 0 & 0 & 1 & 55 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

即 $t_{FC}=(0,10,55)\ \text{mm}$、$R_{FC}=R_z(\pi)$；ROS 米制平移为 $(0,0.010,0.055)$，rpy 为 $(0,0,\pi)$。这组平移和旋转均是估算模型，不是手眼标定结果，精度要求高时应以标定值替换。$65\ \text{mm}$ 已包含机身厚度所在的距离，不再额外加上 $30\ \text{mm}$。

给定底座系中的目标相机位姿 $(R_{BC},p_C)$，所需法兰位姿为：

$$\begin{aligned} R_{BF} &= R_{BC}R_{FC}^{\mathsf T} \\ p_F &= p_C-R_{BF}t_{FC} \end{aligned}$$

完整向量随法兰姿态旋转，不能在所有姿态中固定沿底座竖直方向加减 $10\ \text{mm}$。

相机视场角

视场角决定了相机离机器多远才能把目标拍全。根据相机规格，标称视场角如下，水平与垂直方向以相机横幅拍摄时为准：

参数	截图标记	标称值
水平视场角	H	$84.5^\circ$
垂直视场角	V	$54^\circ$
对角视场角	D	$93.1^\circ$

截图中的 D 表示对角视场角，本文公式中的 $D$ 表示光心到机器表面的拍摄距离。计算画面宽、高覆盖范围时使用水平与垂直视场角即可。

全景拍摄时，相机绕光轴旋转 $90^\circ$ 成为竖幅，画面在机器表面上的水平方向对应 $54^\circ$，竖直方向对应 $84.5^\circ$。按光轴垂直于机器表面、拍摄模式保留上述视场角的理想透视模型估算，竖幅覆盖宽度为 $2D\tan(27^\circ)$，高度为 $2D\tan(42.25^\circ)$。在名义距离 $D=750\ \text{mm}$ 处，覆盖范围约为宽 $764\ \text{mm}$、高 $1363\ \text{mm}$；对居中的 $1200\ \text{mm}$ 高目标，上、下各留约 $81\ \text{mm}$ 余量。

现场目标位姿输入

有了上面的变换，就可以把巡检任务翻译成逆解目标。下面是构造目标所需的现场参数。机器表面按竖直、与车身纵向平行处理；AGV 停车时使机械臂底座与目标纵向对齐。

参数	取值或约定
AGV 纵向中线到机器表面的横向距离 $L$	$550\pm50\ \text{mm}$，范围为 $[500,600]\ \text{mm}$
机器所在侧 $s$	$+y_B$ 侧为 $+1$，$-y_B$ 侧为 $-1$
目标在 $A$ 中的纵向坐标 $x_t$	与底座对齐，$x_t=x_b=20\ \text{mm}$
全景目标中心离地高度 $z_t$	$600\ \text{mm}$（目标全高约 $1200\ \text{mm}$）
铭牌中心离地高度 $z_t$	名义 $600\ \text{mm}$，随实际位置在 $[500,700]\ \text{mm}$ 内调整
光心到表面的距离 $D$	全景名义 $750\ \text{mm}$；铭牌名义 $300\ \text{mm}$，均为目标设置

相机正对机器表面、光轴穿过目标点时，底座系中的目标光心位置为：

$$p_C=(x_t-x_b,\ s(L-D),\ z_t-z_b)$$

按 $L=550\ \text{mm}$ 构造的名义目标如下，实际求解时应采用本次目标位置。

任务	目标光心位置 $p_C$，底座系（mm）	姿态要求
机器正面或背面全景	$(0,-200s,375.8)$	相机转为竖幅，图像长边沿竖直方向
铭牌	$(0,250s,z_t-224.2)$	相机保持横幅，铭牌中心位于画面中心

两类目标的光轴方向均为 $(0,s,0)$。为给出完整的目标姿态，本文将横幅图像上沿取为底座 $+z_B$ 方向，竖幅取在此基础上绕相机光轴按右手定则旋转 $+90^\circ$：

$$R_{BC}^{\mathrm{横幅}}= \begin{bmatrix} s & 0 & 0 \\ 0 & 0 & s \\ 0 & -1 & 0 \end{bmatrix},\qquad R_{BC}^{\mathrm{竖幅}}=R_{BC}^{\mathrm{横幅}}R_z(\pi/2)$$

令纵向误差 $\Delta x=x_t-x_b$。将上述姿态和估算的 $t_{FC}$ 代入法兰换算，可直接得到适用于本节表面与朝向约定的目标法兰位姿。横幅铭牌目标为：

$$p_F^{\mathrm{横幅}}= \begin{bmatrix} \Delta x\\ s(L-D-55)\\ z_t-z_b-10 \end{bmatrix}, \qquad R_{BF}^{\mathrm{横幅}}= \begin{bmatrix} -s & 0 & 0\\ 0 & 0 & s\\ 0 & 1 & 0 \end{bmatrix}$$

竖幅全景目标为：

$$p_F^{\mathrm{竖幅}}= \begin{bmatrix} \Delta x-10s\\ s(L-D-55)\\ z_t-z_b \end{bmatrix}, \qquad R_{BF}^{\mathrm{竖幅}}= \begin{bmatrix} 0 & s & 0\\ 0 & 0 & s\\ 1 & 0 & 0 \end{bmatrix}$$

这两组位姿已包含相机工具补偿，可以直接作为法兰逆解目标。取 $\Delta x=0$、$L=550\ \text{mm}$ 及各任务的名义拍摄距离后，得到：

任务	目标法兰位置 $p_F$，底座系（mm）	目标法兰旋转
机器正面或背面全景，竖幅	$(-10s,-255s,375.8)$	$R_{BF}^{\mathrm{竖幅}}$
铭牌，横幅	$(0,195s,z_t-224.2-10)$；名义高度为 $365.8$	$R_{BF}^{\mathrm{横幅}}$

竖幅姿态中原先向上的 $10\ \text{mm}$ 偏移转到沿底座 $s\times x_B$ 的方向，因此全景法兰的纵向坐标需要补偿 $-10s\ \text{mm}$，高度不再减去 $10\ \text{mm}$。

完整名义求解链

从安装底座系到相机光心的完整运动学关系为：

$${}^BT_C(q) = {}^BT_0\;{}^0T_6(q)\;{}^6T_F\;{}^FT_C$$

按已确定的控制器名义坐标定义，${}^BT_0={}^6T_F=I$，因此 ${}^BT_C(q)={}^0T_6(q)\,{}^FT_C$。$d_6=53\ \text{mm}$ 已计入 DH 运动链，不应再作为额外工具长度重复叠加。

给定目标相机位姿 ${}^BT_C^*$ 后，逆解的目标应转换为：

$${}^0T_6^* = {}^BT_C^*\;({}^FT_C)^{-1}$$

然后求满足 ${}^0T_6(q)={}^0T_6^*$ 且不超出换算后关节限位的 $q$。直接以物理法兰为目标时，使用 ${}^BT_F^*$，无需相机工具变换。

若目标相机位姿在导航 map 系 $W$ 中给出，停车后读取该时刻的 ${}^WT_N$，结合本次采用的固定安装变换，可得：

$$\begin{aligned} {}^WT_C(q)&={}^WT_N\;{}^NT_B\;{}^0T_6(q)\;{}^FT_C \\ {}^0T_6^*&=({}^WT_N\;{}^NT_B)^{-1}\;{}^WT_C^*\;({}^FT_C)^{-1} \end{aligned}$$

对任意输入坐标系 $X$，定义 $H_X={}^XT_B$，可将目标转换统一写为：

$${}^0T_6^*= \begin{cases} H_X^{-1}\;{}^XT_C^*\;({}^FT_C)^{-1}, & \text{相机目标}\\ H_X^{-1}\;{}^XT_F^*, & \text{法兰目标} \end{cases}$$

其中：

$$H_X= \begin{cases} I, & X=B\\ {}^AT_B, & X=A\\ {}^NT_B, & X=N\\ {}^WT_N\;{}^NT_B, & X=W \end{cases}$$

输入目标应具有有效的旋转矩阵，满足 $R^{\mathsf T}R=I$、$\det R=1$，并使用统一的毫米制平移；ROS 的米制平移应先乘以 $1000$。RPY 输入按前文约定转换为旋转矩阵后参与求解。

至此，名义模型所需的固定参数均已有采用值，其中相机外参为估算值，底盘安装变换按模型与图纸推导。解析枚举候选解只需要目标位姿；按当前姿态选解还需要当前关节角，导航目标还需要实时导航位姿。目标是否存在满足限位的逆解，由下文的分支求解与筛选确定。

解析逆运动学

求解变量与等价正运动学

解析求解直接使用实际参与各轴旋转的角度 $\theta_i$：

$$\theta_i=q_i+o_i=j_i=\frac{\pi}{180}J_i, \qquad o=\left(0,-\frac{\pi}{2},0,-\frac{\pi}{2},0,0\right)$$

求解后转换为：

$$q_i=\theta_i-o_i, \qquad J_i=\frac{180}{\pi}\theta_i$$

使用 $\theta_i$ 构造运动链时不再额外加偏置；使用 DH 变量 $q_i$ 时，仍由 DH 公式加一次 $o_i$。

为避免连杆标记与 DH 表格行号混淆，下文将两段主要臂长记为：

$$\ell_2=269,\qquad \ell_3=266$$

其余长度参数为：

$$d_1=205,\qquad d_4=88,\qquad d_5=95,\qquad d_6=53$$

将参数代入改进 DH 链，可得等价的显式乘积：

$$\begin{aligned} {}^BT_F(\theta)={}& [R_z(\theta_1)T_z(d_1)]\\ &[R_x(-\pi/2)R_z(\theta_2)]\\ &[T_x(\ell_2)R_z(\theta_3)]\\ &[T_x(\ell_3)R_z(\theta_4)T_z(d_4)]\\ &[R_x(-\pi/2)R_z(\theta_5)T_z(d_5)]\\ &[R_x(\pi/2)R_z(\theta_6)T_z(d_6)] \end{aligned}$$

第二、第三、第四轴平行，定义它们的角度和为：

$$h=\theta_2+\theta_3+\theta_4$$

下文保留 $d_4$、$d_5$、$d_6$ 的完整影响。该结构的后三轴不构成三轴共点的球腕，不能仅按末端轴向减去一个工具长度后，将位置和姿态完全独立求解。

非奇异解析逆解

将已转换到机械臂底座系的法兰目标记为：

$${}^BT_F^*={}^0T_6^*= \begin{bmatrix} R & p\\ 0 & 1 \end{bmatrix}, \qquad R=[\,\boldsymbol n\;\;\boldsymbol b\;\;\boldsymbol a\,]$$

其中 $\boldsymbol n$、$\boldsymbol b$、$\boldsymbol a$ 分别为目标法兰 $x$、$y$、$z$ 轴在底座系中的单位方向；加粗的 $\boldsymbol a$ 是方向向量，与 DH 表中的长度参数 $a$ 无关。

1. 求第一轴

沿法兰轴向去除 $d_6$，得到坐标系 5 原点在底座系中的位置：

$$w=p-d_6\boldsymbol a= \begin{bmatrix} w_x\\w_y\\w_z \end{bmatrix}, \qquad \rho=\sqrt{w_x^2+w_y^2}$$

第一轴满足：

$$-w_x\sin\theta_1+w_y\cos\theta_1=d_4$$

若 $\rho<d_4$，目标无几何逆解。否则，对 $\sigma_1\in\{-1,+1\}$：

$$\boxed{ \theta_1= \operatorname{atan2}(w_y,w_x) - \operatorname{atan2} \left(d_4,\;\sigma_1\sqrt{\rho^2-d_4^2}\right) }$$

对每个第一轴候选，定义底座系中的单位向量：

$$e_r= \begin{bmatrix} \cos\theta_1\\\sin\theta_1\\0 \end{bmatrix}, \qquad e_t= \begin{bmatrix} -\sin\theta_1\\\cos\theta_1\\0 \end{bmatrix}, \qquad e_z= \begin{bmatrix} 0\\0\\1 \end{bmatrix}$$

2. 求第五轴与角度和

由正运动学可得法兰轴向：

$$\boldsymbol a= \sin\theta_5(\cos h\,e_r-\sin h\,e_z) +\cos\theta_5\,e_t$$

因此：

$$c_5=e_t^{\mathsf T}\boldsymbol a, \qquad s_5=\sigma_5\sqrt{1-c_5^2}, \qquad \sigma_5\in\{-1,+1\}$$ $$\boxed{\theta_5=\operatorname{atan2}(s_5,c_5)}$$

在 $s_5\ne0$ 的非奇异情况下：

$$\boxed{ h= \operatorname{atan2} \left( -\frac{a_z}{s_5}, \frac{e_r^{\mathsf T}\boldsymbol a}{s_5} \right) }$$

其中 $a_z$ 为方向向量 $\boldsymbol a$ 的底座系竖直分量。

3. 求第六轴

定义：

$$\boldsymbol v=-\sin h\,e_r-\cos h\,e_z$$

该方向为坐标系 5 的 $z$ 轴方向，正运动学满足：

$$\boldsymbol v^{\mathsf T}\boldsymbol n=\sin\theta_6, \qquad \boldsymbol v^{\mathsf T}\boldsymbol b=\cos\theta_6$$

因此：

$$\boxed{ \theta_6= \operatorname{atan2} (\boldsymbol v^{\mathsf T}\boldsymbol n,\; \boldsymbol v^{\mathsf T}\boldsymbol b) }$$

4. 求第二、第三轴

去除 $d_6$ 后的位置满足：

$$\begin{aligned} w={}& [\ell_2\cos\theta_2+ \ell_3\cos(\theta_2+\theta_3)-d_5\sin h]\,e_r\\ &+d_4e_t\\ &+[d_1-\ell_2\sin\theta_2 -\ell_3\sin(\theta_2+\theta_3)-d_5\cos h]\,e_z \end{aligned}$$

补偿第五轴偏移，定义平面二连杆目标：

$$u=e_r^{\mathsf T}w+d_5\sin h, \qquad v_p=d_1-w_z-d_5\cos h$$

于是：

$$\begin{aligned} u&=\ell_2\cos\theta_2+\ell_3\cos(\theta_2+\theta_3)\\ v_p&=\ell_2\sin\theta_2+\ell_3\sin(\theta_2+\theta_3) \end{aligned}$$

由余弦定理：

$$c_3= \frac{u^2+v_p^2-\ell_2^2-\ell_3^2} {2\ell_2\ell_3}$$

若 $|c_3|>1$，舍弃当前第一轴、第五轴组合对应的分支。否则，对 $\sigma_3\in\{-1,+1\}$：

$$s_3=\sigma_3\sqrt{1-c_3^2}$$ $$\boxed{\theta_3=\operatorname{atan2}(s_3,c_3)}$$ $$\boxed{ \theta_2= \operatorname{atan2}(v_p,u) - \operatorname{atan2} (\ell_3s_3,\;\ell_2+\ell_3c_3) }$$

5. 求第四轴并汇总分支

$$\boxed{\theta_4=h-\theta_2-\theta_3}$$

枚举 $\sigma_1$、$\sigma_5$、$\sigma_3$，非奇异情况下最多得到 $2\times2\times2=8$ 组模 $2\pi$ 的几何解。关节转数展开、限位筛选和当前姿态选解在后续统一处理。

浮点实现应为可达性判断设置与量纲相符的数值容差。仅当越界来自容差内的舍入误差时，才将根号内的微小负值截断为零，或将 $c_3$、$c_5$ 截断到 $[-1,1]$；实际不可达的分支应舍弃。

奇异解与分支合并

当 $\sin\theta_5=0$ 时，不能使用上述除以 $s_5$ 的角度和公式。第五轴当前限位为 $[-168,168]^\circ$，因此允许范围内需要处理的这类奇异情况只有：

$$\theta_5=0,\qquad \boldsymbol a=e_t$$

若 $c_5=-1$，则第五轴角为 $\pi$ 模 $2\pi$，所有转数分支均超出当前限位，可直接舍弃。

对于 $\theta_5=0$ 分支，姿态只能确定：

$$h+\theta_6=\gamma\pmod{2\pi}$$

其中：

$$\gamma= \operatorname{atan2}(-n_z,\;e_r^{\mathsf T}\boldsymbol n)$$

可将 $h\in[-\pi,\pi)$ 作为一维自由参数，并取：

$$\theta_6=\gamma-h$$

对每个 $h$，使用非奇异求解第 4 步中的 $u$、$v_p$ 及平面二连杆公式求 $\theta_2$、$\theta_3$，再计算：

$$\theta_4=h-\theta_2-\theta_3$$

由此得到连续候选集合，在满足关节限位的可行集合上按下文目标函数选解。当前第六轴角可作为选解偏好，但不能通过任意固定 $\theta_6$ 来替代整个连续解族。单个固定 $h$ 或有限采样未命中可行解，也不能证明整个连续解族无解。

当 $\sin\theta_5$ 接近零但不为零时，逆解对姿态误差敏感。数值处理应保留原始目标，通过当前姿态保持分支连续，并用正运动学残差判断候选是否有效，不能直接将第五轴强制置零后接受不满足目标的解。

当 $\rho=d_4$ 时，两组第一轴分支合并；当 $\sin\theta_3=0$ 时，两组肘部几何分支合并。对应公式仍可使用，汇总时去除重复解。

候选解约束与选择

转数展开与关节限位

解析公式给出的角度按模 $2\pi$ 理解。对每组几何解 $\bar\theta$，应按各轴实际限位枚举：

$$\theta_i^{(k)}=\bar\theta_i+2\pi k_i,\qquad k_i\in\mathbb Z$$

令 $\theta_i^-$、$\theta_i^+$ 为前文 RoboFlow 软限位乘以 $\pi/180$ 后的弧度值。注意这里使用的是控制器角度范围，而非带零位偏置的 DH 变量范围。允许的整数范围为：

$$\left\lceil \frac{\theta_i^--\bar\theta_i}{2\pi} \right\rceil \le k_i\le \left\lfloor \frac{\theta_i^+-\bar\theta_i}{2\pi} \right\rfloor$$

任一轴的整数范围为空时，舍弃该组几何解；否则保留各轴允许转数的组合。第六轴范围为 $[-360,360]^\circ$，不能将所有候选统一压缩到 $[-180,180]^\circ$。重复解应按展开后的实际关节角去重，不能将相差整圈的控制器角度合并。

正运动学残差

将目标法兰位姿记为 $(R^*,p^*)=(R,p)$。将每个候选转换为 $q_i=\theta_i-o_i$，代回完整正运动学，记所得法兰位姿为 $(R(\theta),p(\theta))$。候选应满足：

$$e_p=\|p(\theta)-p^*\|\le\varepsilon_p$$ $$e_R= \arccos\!\left[ \operatorname{clip} \left( \frac{\operatorname{tr}(R^{*\mathsf T}R(\theta))-1}{2}, -1,1 \right) \right] \le\varepsilon_R$$

其中 $\varepsilon_p$ 使用 mm，$\varepsilon_R$ 使用 rad，分别为位置与旋转残差容差，应按求解精度要求设置。正运动学复核使用未舍入的候选角度。这些残差反映名义模型内的求解误差，不包含安装尺寸、相机外参及设备标定误差。

接近当前姿态的选解

记通过限位和残差约束的候选集合为 $\mathcal S$，其中包含奇异情况下的可行连续解。将当前控制器角度换算为：

$$\theta_i^{\mathrm{cur}}=\frac{\pi}{180}J_i^{\mathrm{cur}}$$

可按加权关节位移选择：

$$\boxed{ \theta^*= \arg\min_{\theta\in\mathcal S} \sum_{i=1}^{6} \lambda_i(\theta_i-\theta_i^{\mathrm{cur}})^2 }, \qquad \lambda_i>0$$

没有轴间偏好时可取 $\lambda_i=1$。这里使用展开后的实际角度差，不对差值取模，以正确计入跨转数分支的关节位移。若 $\mathcal S$ 为空，则目标在当前名义模型与限位下无解；若未提供当前关节角，则可返回全部候选，奇异时以参数化解族表示，不据此指定执行分支。

完整求解顺序为：

1. 统一输入单位和姿态表示，将目标转换为底座系中的法兰位姿。
2. 枚举第一轴、第五轴和肘部分支；第五轴奇异时求解对应的连续可行集合。
3. 展开各轴允许的转数，按现场生效的软限位筛选。
4. 用完整正运动学检查位置与姿态残差，去除实际角度相同的重复候选。
5. 根据当前关节角选解，输出 $q_i=\theta_i^*-o_i$ 或 RoboFlow 角度 $J_i=180\theta_i^*/\pi$。