VR遥操作机械臂系统：核心算法与数学方法全解析

本文是"基于Meta Quest 2的VR遥操作6-DOF机械臂系统"的算法总结，涵盖视觉感知、手眼标定、正逆运动学等全部核心数学方法，附完整推导过程。系统平台：Orange Pi 5 Plus（RK3588）+ ROS2 Humble + Orbbec Gemini 336L RGB-D相机。

---

目录

1. 针孔相机模型与深度图反投影
2. 主动双目结构光测深原理
3. RANSAC平面分割
4. 点云预处理管线
5. 圆柱体质心法拟合
6. [PCA OBB有向包围盒](#PCA OBB有向包围盒)
7. [手眼标定：Umeyama SVD算法](#手眼标定：Umeyama SVD算法)
8. ArUco位姿估计与IPPE双解消歧义
9. DH参数与正向运动学
10. 解析逆向运动学
11. ZYX欧拉角与旋转矩阵互转

---

一、针孔相机模型与深度图反投影

1.1 基本概念

针孔相机模型是计算机视觉中最基础的相机模型，用一个理想小孔代替透镜，描述三维世界到二维图像的投影关系。

坐标系约定（OpenCV标准）：

• 相机坐标系：X 轴向右，Y 轴向下，Z 轴向前（光轴方向）
• 图像像素坐标系：左上角为原点，u 向右，v 向下

1.2 正向投影推导

设空间点 P = ( X , Y , Z ) P = (X, Y, Z) P=(X,Y,Z)（相机坐标系，单位米），设物理焦距为 f f f（米），传感器像素物理尺寸为 p p p（米/像素）。

由相似三角形关系（物点、像点、光心三点共线）：

x i m g f = X Z \frac{x_{img}}{f} = \frac{X}{Z} fximg=ZX

其中 x i m g x_{img} ximg 是像平面上的物理偏移（米）。换算为像素坐标，加上主点偏移 ( c x , c y ) (c_x, c_y) (cx,cy)：

u = f p ⋅ X Z + c x = f x ⋅ X Z + c x u = \frac{f}{p} \cdot \frac{X}{Z} + c_x = f_x \cdot \frac{X}{Z} + c_x u=pf⋅ZX+cx=fx⋅ZX+cx

v = f p ⋅ Y Z + c y = f y ⋅ Y Z + c y v = \frac{f}{p} \cdot \frac{Y}{Z} + c_y = f_y \cdot \frac{Y}{Z} + c_y v=pf⋅ZY+cy=fy⋅ZY+cy

其中 f x = f / p f_x = f/p fx=f/p， f y = f / p f_y = f/p fy=f/p 称为像素焦距，单位为像素。写成矩阵形式：

s ( u v 1 ) = ( f x 0 c x 0 f y c y 0 0 1 ) ⏟ K ( X Y Z ) s \begin{pmatrix} u \\ v \\ 1 \end{pmatrix} = \underbrace{\begin{pmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{pmatrix}}_{K} \begin{pmatrix} X \\ Y \\ Z \end{pmatrix} s uv1 =K fx000fy0cxcy1 XYZ

K K K 称为相机内参矩阵 ，来自 camera_info 话题，由相机出厂特性唯一确定，与场景无关。

1.3 反投影：深度图还原三维点云

已知像素坐标 ( u , v ) (u, v) (u,v) 和深度值 d d d（单位 mm），反解上述方程：

Z = d × 0.001 （mm → m） Z = d \times 0.001 \quad \text{（mm → m）} Z=d×0.001（mm → m）

X = ( u − c x ) × Z f x X = (u - c_x) \times \frac{Z}{f_x} X=(u−cx)×fxZ

Y = ( v − c y ) × Z f y Y = (v - c_y) \times \frac{Z}{f_y} Y=(v−cy)×fyZ

关键理解 ：深度相机输出的 d d d 是点到图像平面的垂直距离（Z轴分量），而非斜线欧氏距离。因此反投影公式对图像任意位置（包括边缘）都精确成立，不存在"边缘角度误差"。

f x f_x fx 的几何意义：在 Z = 1 m Z=1\text{m} Z=1m 处， X X X 方向偏移 1 m 1\text{m} 1m 的点，在图像上偏移 f x f_x fx 个像素。 Z / f x Z/f_x Z/fx 即为深度 Z Z Z 处每像素对应的实际长度（米/像素）。

代码实现（point_cloud_fitter_node.cpp）：

float z = depth_img.at<uint16_t>(v, u) * 0.001f;   // mm → m
float x = (u - cx_) * z / fx_;
float y = (v - cy_) * z / fy_;
cloud->push_back({x, y, z});

---

二、主动双目结构光测深原理

2.1 为什么深度值是 Z 而非斜线距离

Orbbec Gemini 336L 使用主动双目红外结构光（Active Stereo Infrared Structured Light）测深，原理是三角测量，而非飞行时间（ToF）。

俯视示意图 （两摄像头水平排列，基线为 B B B）：

【插入图】

2.2 三角测量推导

对空间点 P P P，左摄像头像平面上的水平偏移为 x l x_l xl，右摄像头为 x r x_r xr（均为带符号像素值，向右为正）。

由相似三角形（左摄像头）：

x l = f ⋅ X l e f t Z x_l = f \cdot \frac{X_{left}}{Z} xl=f⋅ZXleft

右摄像头与左摄像头间距 B B B， P P P 相对于右摄像头的水平坐标为 X l e f t − B X_{left} - B Xleft−B：

x r = f ⋅ X l e f t − B Z x_r = f \cdot \frac{X_{left} - B}{Z} xr=f⋅ZXleft−B

两式相减，消去 X l e f t X_{left} Xleft：

x l − x r = f ⋅ B Z x_l - x_r = \frac{f \cdot B}{Z} xl−xr=Zf⋅B

解出深度：

Z = f ⋅ B x l − x r = f ⋅ B d \boxed{Z = \frac{f \cdot B}{x_l - x_r} = \frac{f \cdot B}{d}} Z=xl−xrf⋅B=df⋅B

其中 d = x l − x r d = x_l - x_r d=xl−xr 称为视差（disparity）。

关键 ：该公式推导过程中 Z 轴方向没有引入任何角度，因此输出的 Z Z Z 天然是垂直深度，而非斜线距离。

2.3 结构光的作用

普通双目对均匀纹理区域（白墙、桌面）无法找到对应点。结构光投射随机散斑图案，使每块区域具有唯一"指纹"，确保左右摄像头可靠匹配，从而在工业场景中获得稠密深度图。

---

三、RANSAC平面分割

3.1 问题背景

点云中包含大量桌面点，需要将其剔除，仅保留物体点云。朴素做法（拟合全部点）会被物体点干扰导致结果偏歪，RANSAC通过随机采样解决这个问题。

3.2 平面方程

三维空间中平面方程： a x + b y + c z + d = 0 ax + by + cz + d = 0 ax+by+cz+d=0，法向量为 ( a , b , c ) (a, b, c) (a,b,c)（需满足 a 2 + b 2 + c 2 = 1 \sqrt{a^2+b^2+c^2}=1 a2+b2+c2 =1 以使距离公式简化）。

点 P P P 到平面的距离：

dist ( P ) = ∣ a P x + b P y + c P z + d ∣ \text{dist}(P) = |aP_x + bP_y + cP_z + d| dist(P)=∣aPx+bPy+cPz+d∣

3.3 算法步骤

输入 ： n n n 个三维点，迭代次数 N N N，内点阈值 ε \varepsilon ε（本系统约 5mm）

步骤：

1. 随机采样 3 个不共线点，用这三个点计算出确定一个候选平面方程（法向量由叉积得到）
2. 计算采集到的所有点到该候选平面的距离
3. 距离 < ε < \varepsilon <ε 的点为内点（判定为桌面点） ，否则为外点（判定为物体点） ，这很正常，因为我们就是假设这个平面为桌面，这个假设的平米越平行于桌面，内点肯定是越来越多（注意这个方案必须要适用于桌面的面积大于物体占用的面积的情况，很显然如果物体更多，这个判定方法就有问题了）
4. 重复 N N N 次迭代，保留内点最多的平面
5. 移除该平面的内点（桌面），保留外点（物体）

3.4 为什么可靠

设桌面点占全部点云比例为 r r r（通常 r ≈ 0.7 ∼ 0.8 r \approx 0.7 \sim 0.8 r≈0.7∼0.8），则单次迭代恰好采到 3 个桌面点的概率为 r 3 r^3 r3。经过 N N N 次迭代，至少一次采到纯桌面点的概率为：

P = 1 − ( 1 − r 3 ) N P = 1 - (1 - r^3)^N P=1−(1−r3)N

r = 0.8 , N = 100 r=0.8, N=100 r=0.8,N=100 时， P = 1 − ( 1 − 0.512 ) 100 ≈ 1 − 10 − 31 ≈ 1 P = 1-(1-0.512)^{100} \approx 1 - 10^{-31} \approx 1 P=1−(1−0.512)100≈1−10−31≈1，几乎必然成功。

PCL实现：

pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.005);   // 5mm
seg.setMaxIterations(100);

---

四、点云预处理管线

完整预处理流程：

原始深度图
  → [反投影] 原始点云
  → [RANSAC] （全局的点云）去桌面平面
  → [欧氏聚类] 取最高Z簇（物体）
  → [VoxelGrid] 降采样
  → [SOR] 统计离群点滤除
  → [深度范围滤波] 限制z_near ~ z_near+130mm
  → [几何拟合] 圆柱质心法 / PCA OBB

4.1 欧氏聚类（EuclideanClusterExtraction）

用 KdTree 建立空间索引，将所有两两距离 < ε c l u s t e r < \varepsilon_{cluster} <εcluster 的点归为同一簇。

调试日志 ：原先选点数最多的簇，但桌面残留点比物体点多，导致经常选到桌面（center.z ≈ -28mm）。改为选相机坐标系下平均 Z 最小的簇（即距相机最近 = base_link 中最高 = 物体顶面）：

// 选camera_frame中平均Z最小的簇（最高物体）
float best_mean_z = std::numeric_limits<float>::max();
for (size_t i = 0; i < clusters.size(); ++i) {
    double sum_z = 0.0;
    for (int idx : clusters[i].indices)
        sum_z += (*cloud)[idx].z;
    float mean_z = sum_z / clusters[i].indices.size();
    if (mean_z < best_mean_z) { best_mean_z = mean_z; best = i; }
}

4.2 统计离群点滤除（Statistical Outlier Removal）

对每个点，计算其到最近 k k k 个邻居的平均距离 μ i \mu_i μi。设全局均值为 μ ˉ \bar{\mu} μˉ，标准差为 σ \sigma σ。若 μ i > μ ˉ + α σ \mu_i > \bar{\mu} + \alpha\sigma μi>μˉ+ασ，则该点为离群点并删除（ α \alpha α 通常取 1.0）。

4.3 VoxelGrid 降采样

将空间划分为边长 l l l 的正方体体素，每个体素内所有点被替换为其质心（内点的XYZ分别求均值），在保留空间结构的同时大幅减少点数（机器性能比较差）。

PS，这个是点云的质心，不是物体的质心，因为物体只能看到面向相机的一侧，看不到背面，所以质心肯定是更偏向于摄像头的，因此在代码中要做质心的补偿。下面会说这个问题

---

五、圆柱体质心法拟合

5.1 背景：为何替换RANSAC

RANSAC圆柱拟合（pcl::SACSegmentationFromNormals）在点云稀疏时随机性强：相同输入不同迭代结果可能差距数十毫米。

5.2 XY质心估计

对圆柱体点云的所有 n n n 个点 { ( x i , y i , z i ) } \{(x_i, y_i, z_i)\} {(xi,yi,zi)}，估计轴线在 XY 平面的投影位置：

x ˉ = 1 n ∑ i = 1 n x i , y ˉ = 1 n ∑ i = 1 n y i \bar{x} = \frac{1}{n}\sum_{i=1}^n x_i, \quad \bar{y} = \frac{1}{n}\sum_{i=1}^n y_i xˉ=n1i=1∑nxi,yˉ=n1i=1∑nyi

前提假设 ：深度相机只能看到圆柱朝向相机的半面，因此质心会向相机方向偏移约 2 π r \frac{2}{\pi} r π2r（理论值）。代码用经验系数补偿：

Eigen::Vector3f cam_dir = ctr.normalized();
ctr += cam_dir * cyl_bias_ratio_ * radius;

5.3 中位半径估计

计算每个点到 XY 质心的距离：

r i = ( x i − x ˉ ) 2 + ( y i − y ˉ ) 2 r_i = \sqrt{(x_i - \bar{x})^2 + (y_i - \bar{y})^2} ri=(xi−xˉ)2+(yi−yˉ)2

取所有 r i r_i ri 的中位数作为半径估计：

std::nth_element(dists.begin(), dists.begin() + n/2, dists.end());
float radius = dists[n/2];

使用中位数而非均值，对遮挡边缘和噪声点鲁棒（如果是均值会被少数极大值拉偏）。

5.4 高度估计

h = z m a x − z m i n h = z_{max} - z_{min} h=zmax−zmin

---

六、PCA OBB有向包围盒

6.1 用途

估计长方体（箱子）的朝向和三轴尺寸，为IK规划提供抓取方向。

6.2 惯量矩张量

对点云计算3×3惯量矩张量 I \mathbf{I} I：

I x x = ∑ i ( y i 2 + z i 2 ) , I x y = − ∑ i x i y i , ... I_{xx} = \sum_i (y_i^2 + z_i^2), \quad I_{xy} = -\sum_i x_i y_i, \quad \ldots Ixx=i∑(yi2+zi2),Ixy=−i∑xiyi,...

（假设每个点质量为 1）完整 3 × 3 3\times3 3×3 矩阵：

I = ( ∑ ( y 2 + z 2 ) − ∑ x y − ∑ x z − ∑ x y ∑ ( x 2 + z 2 ) − ∑ y z − ∑ x z − ∑ y z ∑ ( x 2 + y 2 ) ) \mathbf{I} = \begin{pmatrix} \sum(y^2+z^2) & -\sum xy & -\sum xz \\ -\sum xy & \sum(x^2+z^2) & -\sum yz \\ -\sum xz & -\sum yz & \sum(x^2+y^2) \end{pmatrix} I= ∑(y2+z2)−∑xy−∑xz−∑xy∑(x2+z2)−∑yz−∑xz−∑yz∑(x2+y2)

6.3 特征值分解

对 I \mathbf{I} I 做特征值分解：

I v k = λ k v k , k = 1 , 2 , 3 \mathbf{I} \mathbf{v}_k = \lambda_k \mathbf{v}_k, \quad k = 1, 2, 3 Ivk=λkvk,k=1,2,3

三个特征向量 v 1 , v 2 , v 3 \mathbf{v}_1, \mathbf{v}_2, \mathbf{v}_3 v1,v2,v3 即为物体的主轴方向：

• 最小特征值对应最长轴（最容易转动方向）
• 最大特征值对应最短轴

6.4 OBB构造

在主轴坐标系下，分别计算点云在三个主轴方向上的 min/max，即得有向包围盒的尺寸和中心。

中心位置需补偿相机遮挡偏置（可见面中心 ≠ 几何中心，差半个厚度）：

c g e o m e t r i c = c v i s i b l e + d m i n 2 ⋅ n ^ \mathbf{c}{geometric} = \mathbf{c}{visible} + \frac{d_{min}}{2} \cdot \hat{\mathbf{n}} cgeometric=cvisible+2dmin⋅n^

其中 d m i n d_{min} dmin 为最短轴尺寸， n ^ \hat{\mathbf{n}} n^ 为朝向相机的法向量。

---

七、手眼标定：Umeyama SVD算法

7.1 问题定义

本系统采用 Eye-to-Hand 配置（相机固定，机械臂运动），目标是求相机坐标系到机械臂底座坐标系（base_link）的刚体变换 T = { R , t } T = \{R, \mathbf{t}\} T={R,t}：

p b a s e = R ⋅ p c a m + t \mathbf{p}{base} = R \cdot \mathbf{p}{cam} + \mathbf{t} pbase=R⋅pcam+t

其中 R R R 是 3 × 3 3\times3 3×3 旋转矩阵（ R T R = I , det ⁡ ( R ) = 1 R^T R = I, \det(R)=1 RTR=I,det(R)=1）， t \mathbf{t} t 是平移向量。

7.2 数据采集

机械臂末端夹爪上贴 ArUco 码（ID4）。对第 i i i 个标定姿态，采集一对对应点：

坐标来源	获取方式	表示
相机系中 ID4 位置	solvePnP 检测 ArUco	p c a m ( i ) \mathbf{p}_{cam}^{(i)} pcam(i)
底座系中 ID4 位置	DH正运动学 + 偏置计算	p b a s e ( i ) \mathbf{p}_{base}^{(i)} pbase(i)

7.3 Umeyama SVD 推导

最优化目标：

min ⁡ R , t ∑ i = 1 n ∥ R ⋅ p c a m ( i ) + t − p b a s e ( i ) ∥ 2 \min_{R, \mathbf{t}} \sum_{i=1}^{n} \| R \cdot \mathbf{p}{cam}^{(i)} + \mathbf{t} - \mathbf{p}{base}^{(i)} \|^2 R,tmini=1∑n∥R⋅pcam(i)+t−pbase(i)∥2

Step 1：去质心

p ˉ c a m = 1 n ∑ i p c a m ( i ) , p ˉ b a s e = 1 n ∑ i p b a s e ( i ) \bar{\mathbf{p}}{cam} = \frac{1}{n}\sum_i \mathbf{p}{cam}^{(i)}, \quad \bar{\mathbf{p}}{base} = \frac{1}{n}\sum_i \mathbf{p}{base}^{(i)} pˉcam=n1i∑pcam(i),pˉbase=n1i∑pbase(i)

p ~ c a m ( i ) = p c a m ( i ) − p ˉ c a m , p ~ b a s e ( i ) = p b a s e ( i ) − p ˉ b a s e \tilde{\mathbf{p}}{cam}^{(i)} = \mathbf{p}{cam}^{(i)} - \bar{\mathbf{p}}{cam}, \quad \tilde{\mathbf{p}}{base}^{(i)} = \mathbf{p}{base}^{(i)} - \bar{\mathbf{p}}{base} p~cam(i)=pcam(i)−pˉcam,p~base(i)=pbase(i)−pˉbase

Step 2：构造协方差矩阵 （ 3 × 3 3\times3 3×3）

H = ∑ i = 1 n p ~ c a m ( i ) ⋅ ( p ~ b a s e ( i ) ) T H = \sum_{i=1}^{n} \tilde{\mathbf{p}}{cam}^{(i)} \cdot \left(\tilde{\mathbf{p}}{base}^{(i)}\right)^T H=i=1∑np~cam(i)⋅(p~base(i))T

Step 3：SVD 分解

H = U Σ V T H = U \Sigma V^T H=UΣVT

Step 4：构造最优旋转（关键：用行列式修正防止反射解）

D = diag ( 1 , 1 , det ⁡ ( V T U T ) ) D = \text{diag}\left(1,\ 1,\ \det(V^T U^T)\right) D=diag(1, 1, det(VTUT))

R = V ⋅ D ⋅ U T R = V \cdot D \cdot U^T R=V⋅D⋅UT

Step 5：求最优平移

t = p ˉ b a s e − R ⋅ p ˉ c a m \mathbf{t} = \bar{\mathbf{p}}{base} - R \cdot \bar{\mathbf{p}}{cam} t=pˉbase−R⋅pˉcam

数学直觉 ：去质心后，旋转最优化问题等价于最大化迹 tr ( R H ) \text{tr}(R H) tr(RH)，SVD给出其闭合解。对角矩阵 D D D 的 det ⁡ \det det 修正确保 R R R 是旋转矩阵而非反射矩阵（ det ⁡ = + 1 \det=+1 det=+1）。

7.4 残差评估

RMS残差 = 1 n ∑ i = 1 n ∥ R ⋅ p c a m ( i ) + t − p b a s e ( i ) ∥ 2 \text{RMS残差} = \sqrt{\frac{1}{n}\sum_{i=1}^{n} \| R \cdot \mathbf{p}{cam}^{(i)} + \mathbf{t} - \mathbf{p}{base}^{(i)} \|^2} RMS残差=n1i=1∑n∥R⋅pcam(i)+t−pbase(i)∥2

本系统最终标定残差：1.93mm。

---

八、ArUco位姿估计与IPPE双解消歧义

8.1 PnP问题

PnP（Perspective-n-Point） ：已知若干3D点在物体坐标系中的位置（obj_pts）及其在图像中的2D投影坐标（img_pts），求物体坐标系到相机坐标系的变换 { R , t } \{R, \mathbf{t}\} {R,t}。

投影方程（针孔模型）：

s ( u v 1 ) = K [ R ∣ t ] ( X Y Z 1 ) s\begin{pmatrix}u\\v\\1\end{pmatrix} = K [R\ |\ \mathbf{t}] \begin{pmatrix}X\\Y\\Z\\1\end{pmatrix} s uv1 =K[R ∣ t] XYZ1

ArUco码4个角点的 obj_pts（以码中心为原点，码边长 2 h 2h 2h）：

{ ( − h , h , 0 ) , ( h , h , 0 ) , ( h , − h , 0 ) , ( − h , − h , 0 ) } \{(-h, h, 0),\ (h, h, 0),\ (h, -h, 0),\ (-h, -h, 0)\} {(−h,h,0), (h,h,0), (h,−h,0), (−h,−h,0)}

结果 t \mathbf{t} t 即为码中心在相机坐标系中的3D位置 p c a m \mathbf{p}_{cam} pcam。

8.2 IPPE 双解问题

IPPE（Infinitesimal Plane-based Pose Estimation） 是专为平面标记设计的解析解法。从单应性矩阵 H H H（4点对唯一确定）分解姿态时，代数上存在两个精确解：

H = K [ R 1 ∣ t 1 ] 和 H = K [ R 2 ∣ t 2 ] H = K[R_1\ |\ \mathbf{t}_1] \quad \text{和} \quad H = K[R_2\ |\ \mathbf{t}_2] H=K[R1 ∣ t1]和H=K[R2 ∣ t2]

关键问题 ：两个解对同一组角点像素的重投影误差完全相同，无法通过最小重投影误差来区分。

几何直觉：如同从不同角度看一张卡片，存在两种现实都能产生完全相同的投影图像------一种是正面朝向相机，另一种是翻转后的"镜像"姿态。

8.3 消歧义判据：R[2,2] < 0

物理约束 ：ArUco码可见，说明码的正面朝向相机，即码的 Z 轴（垂直正面向外）在相机坐标系中应指向 − Z c a m -Z_{cam} −Zcam 方向（朝向相机）。

solvePnP 的旋转矩阵 R r v e c R_{rvec} Rrvec 将标记坐标系旋转到相机坐标系，标记 Z 轴在相机系中的方向为 R r v e c R_{rvec} Rrvec 的第3列 ，其 Z 分量即为 R r v e c [ 2 , 2 ] R_{rvec}[2,2] Rrvec[2,2]。

正确解满足： R r v e c [ 2 , 2 ] < 0 R_{rvec}[2,2] < 0 Rrvec[2,2]<0（标记正面朝向相机）

int nsols = cv::solvePnPGeneric(obj_pts, corners[i], cm, dc,
                                rvecs, tvecs, false,
                                cv::SOLVEPNP_IPPE_SQUARE);
int best = -1;
for (int s = 0; s < nsols; s++) {
    double z = tvecs[s].at<double>(2);
    if (z < 0.05 || z > 1.0) continue;        // 深度合理性检查
    cv::Mat R_sol; cv::Rodrigues(rvecs[s], R_sol);
    if (R_sol.at<double>(2,2) < 0 && best == -1) best = s;  // 正面朝向相机
}

判据	正确解	错误解
R [ 2 , 2 ] R[2,2] R[2,2]	< 0 < 0 <0（法向朝向相机）	> 0 > 0 >0（法向背离相机）
重投影误差	≈ 0	≈ 0（无法区分！）
物理可能性	✓ 可见面朝前	✗ 背面朝前

---

九、DH参数与正向运动学

9.1 DH参数约定

Denavit-Hartenberg（DH）参数是描述串联机器人关节坐标变换的标准方法。每个关节用四个参数描述：

参数	符号	含义
关节零位偏置	θ h o m e \theta_{home} θhome	零位时的角度偏置
连杆偏距	d d d	沿上一关节 Z 轴的平移
连杆长度	a a a	沿当前关节 X 轴的平移
连杆扭角	α \alpha α	绕 X 轴的扭转角

本项目实际DH参数：

关节	θ h o m e \theta_{home} θhome	d d d (mm)	a a a (mm)	α \alpha α
J1	0	131	35	− π / 2 -\pi/2 −π/2
J2	− π / 2 -\pi/2 −π/2	0	146	0
J3	+ π / 2 +\pi/2 +π/2	52	0	+ π / 2 +\pi/2 +π/2
J4	0	117	0	− π / 2 -\pi/2 −π/2
J5	0	0	0	+ π / 2 +\pi/2 +π/2
J6	0	75.5	0	0

9.2 单关节旋转矩阵

令 q i = θ i × π 180 + θ h o m e , i q_i = \theta_i \times \frac{\pi}{180} + \theta_{home,i} qi=θi×180π+θhome,i，第 i i i 关节的旋转矩阵：

R i = ( cos ⁡ q − cos ⁡ α sin ⁡ q sin ⁡ α sin ⁡ q sin ⁡ q cos ⁡ α cos ⁡ q − sin ⁡ α cos ⁡ q 0 sin ⁡ α cos ⁡ α ) R_i = \begin{pmatrix} \cos q & -\cos\alpha \sin q & \sin\alpha \sin q \\ \sin q & \cos\alpha \cos q & -\sin\alpha \cos q \\ 0 & \sin\alpha & \cos\alpha \end{pmatrix} Ri= cosqsinq0−cosαsinqcosαcosqsinαsinαsinq−sinαcosqcosα

9.3 链式乘法求末端位姿

累积旋转矩阵：

R 06 = R 0 ⋅ R 1 ⋅ R 2 ⋅ R 3 ⋅ R 4 ⋅ R 5 R_{06} = R_0 \cdot R_1 \cdot R_2 \cdot R_3 \cdot R_4 \cdot R_5 R06=R0⋅R1⋅R2⋅R3⋅R4⋅R5

末端（TCP）位置（各连杆贡献在 base_link 下的向量之和）：

P e n d = R 0 ⋅ L 1 + R 02 ⋅ L 2 + R 03 ⋅ L 3 + R 06 ⋅ L 6 \mathbf{P}_{end} = R_0 \cdot \mathbf{L}1 + R{02} \cdot \mathbf{L}2 + R{03} \cdot \mathbf{L}3 + R{06} \cdot \mathbf{L}_6 Pend=R0⋅L1+R02⋅L2+R03⋅L3+R06⋅L6

其中：

• L 1 = ( D B S , − L B S , 0 ) T = ( 35 , − 131 , 0 ) \mathbf{L}1 = (D{BS},\ -L_{BS},\ 0)^T = (35,\ -131,\ 0) L1=(DBS, −LBS, 0)T=(35, −131, 0) mm
• L 2 = ( L A M , 0 , 0 ) T = ( 146 , 0 , 0 ) \mathbf{L}2 = (L{AM},\ 0,\ 0)^T = (146,\ 0,\ 0) L2=(LAM, 0, 0)T=(146, 0, 0) mm
• L 3 = ( − D E W , 0 , L F A ) T = ( − 52 , 0 , 117 ) \mathbf{L}3 = (-D{EW},\ 0,\ L_{FA})^T = (-52,\ 0,\ 117) L3=(−DEW, 0, LFA)T=(−52, 0, 117) mm
• L 6 = ( 0 , 0 , L W T ) T = ( 0 , 0 , 75.5 ) \mathbf{L}6 = (0,\ 0,\ L{WT})^T = (0,\ 0,\ 75.5) L6=(0, 0, LWT)T=(0, 0, 75.5) mm

---

十、解析逆向运动学

10.1 解耦策略

6-DOF机械臂 + 球腕（J4/J5/J6轴交于一点）可将位置和姿态解耦：

• J1/J2/J3：确定腕点（球腕中心）的三维位置
• J4/J5/J6：确定末端姿态

每步有2个解，总计 2 × 2 × 2 = 8 2 \times 2 \times 2 = 8 2×2×2=8 组解。

10.2 第一步：求腕点坐标

已知目标末端位置 P e n d \mathbf{P}{end} Pend 和姿态 R 06 R{06} R06，去除末端连杆（ L W T L_{WT} LWT 沿末端 Z 轴）得腕点：

P w r i s t = P e n d − L W T ⋅ R 06 [ : , 2 ] \mathbf{P}{wrist} = \mathbf{P}{end} - L_{WT} \cdot R_{06}[:,2] Pwrist=Pend−LWT⋅R06[:,2]

其中 R 06 [ : , 2 ] R_{06}[:,2] R06[:,2] 是 R 06 R_{06} R06 的第3列（末端 Z 轴方向）。

10.3 第二步：求 J1

J1是底座绕 Z 轴的旋转，由腕点的 XY 坐标直接得出：

J 1 1 = atan2 ( P w , y , P w , x ) （肘朝外） J1_1 = \text{atan2}(P_{w,y},\ P_{w,x}) \quad \text{（肘朝外）} J11=atan2(Pw,y, Pw,x)（肘朝外）
J 1 2 = atan2 ( − P w , y , − P w , x ) （肘朝内） J1_2 = \text{atan2}(-P_{w,y},\ -P_{w,x}) \quad \text{（肘朝内）} J12=atan2(−Pw,y, −Pw,x)（肘朝内）

10.4 第三步：用余弦定理求 J2/J3

在J1坐标系下，将问题化为等效二连杆：

• 上臂： l s e = L A M = 146 l_{se} = L_{AM} = 146 lse=LAM=146 mm
• 前臂等效： l e w = L F A 2 + D E W 2 = 117 2 + 52 2 ≈ 128 l_{ew} = \sqrt{L_{FA}^2 + D_{EW}^2} = \sqrt{117^2 + 52^2} \approx 128 lew=LFA2+DEW2 =1172+522 ≈128 mm
• 肩到腕距离： l s w = ∣ L s w ∣ l_{sw} = |\mathbf{L}_{sw}| lsw=∣Lsw∣

由余弦定理：

cos ⁡ ( ∠ A ) = l s e 2 + l s w 2 − l e w 2 2 ⋅ l s e ⋅ l s w \cos(\angle A) = \frac{l_{se}^2 + l_{sw}^2 - l_{ew}^2}{2 \cdot l_{se} \cdot l_{sw}} cos(∠A)=2⋅lse⋅lswlse2+lsw2−lew2

cos ⁡ ( ∠ E ) = l s e 2 + l e w 2 − l s w 2 2 ⋅ l s e ⋅ l e w \cos(\angle E) = \frac{l_{se}^2 + l_{ew}^2 - l_{sw}^2}{2 \cdot l_{se} \cdot l_{ew}} cos(∠E)=2⋅lse⋅lewlse2+lew2−lsw2

肘朝上解：

J 2 = atan2 ( L s w , y , L s w , x ) − arccos ⁡ ( ∠ A ) + π 2 J2 = \text{atan2}(L_{sw,y},\ L_{sw,x}) - \arccos(\angle A) + \frac{\pi}{2} J2=atan2(Lsw,y, Lsw,x)−arccos(∠A)+2π

J 3 = θ e − arccos ⁡ ( ∠ E ) + π J3 = \theta_e - \arccos(\angle E) + \pi J3=θe−arccos(∠E)+π

其中 θ e = atan ( D E W / L F A ) \theta_e = \text{atan}(D_{EW}/L_{FA}) θe=atan(DEW/LFA) 是前臂偏置引入的角度修正。

10.5 第四步：R36分解求腕部 J4/J5/J6

已知 R 06 R_{06} R06（目标姿态）和 R 03 R_{03} R03（由J1/J2/J3确定），关节3到6的旋转：

R 36 = R 03 − 1 ⋅ R 06 = R 03 T ⋅ R 06 R_{36} = R_{03}^{-1} \cdot R_{06} = R_{03}^T \cdot R_{06} R36=R03−1⋅R06=R03T⋅R06

（旋转矩阵的逆等于其转置）

由ZYX腕部分解：

J 5 = arccos ⁡ ( R 36 [ 2 , 2 ] ) J5 = \arccos(R_{36}[2,2]) J5=arccos(R36[2,2])

J 4 = atan2 ( R 36 [ 1 , 2 ] , R 36 [ 0 , 2 ] ) J4 = \text{atan2}(R_{36}[1,2],\ R_{36}[0,2]) J4=atan2(R36[1,2], R36[0,2])

J 6 = atan2 ( R 36 [ 2 , 1 ] , − R 36 [ 2 , 0 ] ) J6 = \text{atan2}(R_{36}[2,1],\ -R_{36}[2,0]) J6=atan2(R36[2,1], −R36[2,0])

万向锁处理 ：当 J 5 = 0 ° J5 = 0° J5=0° 或 J 5 = 180 ° J5 = 180° J5=180°（ R 36 [ 2 , 2 ] = ± 1 R_{36}[2,2] = \pm1 R36[2,2]=±1），J4和J6无法独立确定，从种子关节角（seed）补全。

---

十一、ZYX欧拉角与旋转矩阵互转

11.1 ZYX约定

ZYX顺序：先绕Z轴转 A A A（偏航Yaw），再绕Y轴转 B B B（俯仰Pitch），再绕X轴转 C C C（横滚Roll）。

记 c A = cos ⁡ A , s A = sin ⁡ A c_A = \cos A,\ s_A = \sin A cA=cosA, sA=sinA，以此类推：

R = ( c A c B c A s B s C − s A c C c A s B c C + s A s C s A c B s A s B s C + c A c C s A s B c C − c A s C − s B c B s C c B c C ) R = \begin{pmatrix} c_A c_B & c_A s_B s_C - s_A c_C & c_A s_B c_C + s_A s_C \\ s_A c_B & s_A s_B s_C + c_A c_C & s_A s_B c_C - c_A s_C \\ -s_B & c_B s_C & c_B c_C \end{pmatrix} R= cAcBsAcB−sBcAsBsC−sAcCsAsBsC+cAcCcBsCcAsBcC+sAsCsAsBcC−cAsCcBcC

11.2 旋转矩阵 → ZYX欧拉角

从矩阵元素直接读出（行优先索引， R [ k ] = R ⌊ k / 3 ⌋ , k % 3 R[k] = R_{\lfloor k/3 \rfloor, k\%3} R[k]=R⌊k/3⌋,k%3）：

B = atan2 ( − R [ 6 ] , R [ 0 ] 2 + R [ 3 ] 2 ) B = \text{atan2}(-R[6],\ \sqrt{R[0]^2 + R[3]^2}) B=atan2(−R[6], R[0]2+R[3]2 )

A = atan2 ( R [ 3 ] / cos ⁡ B , R [ 0 ] / cos ⁡ B ) A = \text{atan2}(R[3]/\cos B,\ R[0]/\cos B) A=atan2(R[3]/cosB, R[0]/cosB)

C = atan2 ( R [ 7 ] / cos ⁡ B , R [ 8 ] / cos ⁡ B ) C = \text{atan2}(R[7]/\cos B,\ R[8]/\cos B) C=atan2(R[7]/cosB, R[8]/cosB)

万向锁 （ ∣ R [ 6 ] ∣ ≈ 1 |R[6]| \approx 1 ∣R[6]∣≈1，即 B = ± π / 2 B = \pm\pi/2 B=±π/2）： cos ⁡ B = 0 \cos B = 0 cosB=0 导致上式退化，此时令 A = 0 A=0 A=0，单独由 atan2 \text{atan2} atan2 求 C C C。

---

十二、整体系统数据流配合主要算法

Quest 2 手柄位姿（四元数）
        │ UDP 100Hz
        ▼
hand_servo_node ──→ SolveIK ──→ 关节角指令
        │                           │
        │ 视觉反馈                  │ 串口 115200bps
        ▼                           ▼
yolo_detector_node               STM32F4 + CAN 总线
(RKNN NPU, YOLOv8n)              → 6关节电机 + 夹爪
        │ BBox + track_id
        ▼
point_cloud_fitter_node
[深度反投影 → RANSAC去面 → 聚类 → 质心法/PCA OBB]
        │ 抓取位姿
        ▼
grasp_executor_node
[SolveIK + 6步状态机]
        ▲
aruco_calibrator_node
[IPPE solvePnP → Umeyama SVD → camera→base TF]

---

不定期更新本篇