当前位置：首页 > news >正文

用三个面中心点求解长方体位姿：从几何直觉到线性代数实现

news 2025/10/29 8:10:29

作者：白码思
日期：2025年10月28日

一、问题背景：为什么需要“三个点”来定位一个长方体？

在工业场景中，我们经常需要让机械臂精准抓取一个长方体工件（如电池、盒子、零件）。已知：

工件的尺寸（长、宽、高）
机械臂通过视觉或触觉测得工件三个正交面中心点的世界坐标

目标：计算工件在世界坐标系下的完整6DoF位姿（Pose） —— 即中心位置 O 和旋转矩阵 R。

这正是 Pose GT（Ground Truth）生成 的经典方法，广泛用于：

视觉系统标定
机械臂抓取对齐
仿真与真实数据配准
位姿估计算法验证

二、核心思想：从“三个已知点”反推“中心与姿态”

我们以长方体中心为原点建立局部坐标系，三个正交面中心在局部坐标为：

(+x面中心): (dx/2, 0, 0)
(+y面中心): (0, dy/2, 0)
(+z面中心): (0, 0, dz/2)

记：

A = pos_x, B = pos_y, C = pos_z（世界坐标）
ra = dx/2, rb = dy/2, rc = dz/2

则存在未知向量 Vx = A - O，满足：

||Vx|| = ra
||Vy|| = rb, ||Vz|| = rc
Vx ⊥ Vy, Vy ⊥ Vz, Vz ⊥ Vx

但我们只用 Vx 入手，利用正交性：

(A - O) · (B - A) = -ra²
(A - O) · (C - A) = -ra²

→ 得到 2个线性方程 + 1个二次约束（长度）

三、算法精髓：线性系统 + 零空间参数化

1. 线性系统建模

M @ Vx = b
# M = [Dab]
#     [Dac]    (2×3)
# b = [-ra², -ra²]

这是一个欠定系统（2方程3未知），解空间是一条直线。

2. 伪逆求最小范数特解 `V_part`

V_part = pinv(M) @ b

为什么是最小范数？
因为它是原点到解空间直线的垂足，天然垂直于零空间方向 N。

3. 零空间方向

N = np.cross(Dab, Dac)  # 垂直于 M 的行空间

所有解：Vx = V_part + k * N

4. 代入长度约束 → 二次方程

||V_part + k N||² = ra²
→ a k² + b k + c = 0

解出 k1, k2 → 两个可能的 O

5. 手性筛选：`det(R) > 0`

构造旋转矩阵：

R = [ (A-O)/ra, (B-O)/rb, (C-O)/rc ]

选择 det(R) ≈ 1 的解 → 排除镜像翻转

四、为什么这个算法在工业中如此重要？

场景	作用
机械臂抓取	视觉系统测得三个面中心 → 实时计算工件 Pose GT → 精确对齐抓手
数据集标注	人工或传感器标注三个点 → 自动生成高精度 6DoF 真值
算法验证	对比估计 Pose 与 GT，计算 APE/RPE 误差
仿真到现实（Sim-to-Real）	仿真中已知尺寸+点 → 真实场景中复现相同 GT

一句话：三个点 = 6DoF 位姿的“最小充分输入”

五、完整 Python 实现（附注释）

import numpy as npdef compute_cuboid_pose(dx, dy, dz, pos_x, pos_y, pos_z, tol=1e-6):ra, rb, rc = dx/2, dy/2, dz/2A, B, C = pos_x, pos_y, pos_zDab, Dac = B - A, C - A# 1. 距离一致性检查if not (np.isclose(np.linalg.norm(A-B)**2, ra**2 + rb**2, rtol=tol) andnp.isclose(np.linalg.norm(A-C)**2, ra**2 + rc**2, rtol=tol) andnp.isclose(np.linalg.norm(B-C)**2, rb**2 + rc**2, rtol=tol)):raise ValueError("输入点不满足正交面距离约束")# 2. 线性系统M = np.vstack([Dab, Dac])b = np.array([-ra**2, -ra**2])V_part = np.linalg.pinv(M) @ b# 3. 零空间N = np.cross(Dab, Dac)N = N / np.linalg.norm(N) if np.linalg.norm(N) > 1e-8 else N# 4. 二次方程a = np.dot(N, N)b_coef = 2 * np.dot(V_part, N)c = np.dot(V_part, V_part) - ra**2disc = b_coef**2 - 4*a*cif disc < 0: raise ValueError("无实数解")# 5. 两个候选解for sign in [1, -1]:k = (-b_coef + sign * np.sqrt(disc)) / (2*a)Vx = V_part + k * NO = A - Vxux = (A - O) / rauy = (B - O) / rbuz = (C - O) / rcR = np.column_stack((ux, uy, uz))if abs(np.linalg.det(R) - 1.0) < 1e-5:return O, Rraise ValueError("未找到右手系解")