两、多个坐标系 统一

在机器人、自动驾驶或三维视觉中，经常需要将两个不同坐标系（如相机坐标系和世界坐标系）统一到同一个参考系下。以下是系统化的解决方案，涵盖数学原理、算法实现和实际应用。

一、问题描述

设两个坐标系：

• 坐标系A（如世界坐标系）

• 坐标系B（如相机坐标系）

已知：

• 坐标系B中的点 pB需要转换到坐标系A中。

• 

 目标：找到变换矩阵 R​，将所有点从坐标系B统一到坐标系A

二、数学原理

齐次变换矩阵：

四元数:

三、求解变换矩阵

方法1：已知对应点（最小二乘法）

适用场景

步骤：

案例 Python实现1：

import numpy as np

def solve_umeyama(points_A, points_B):
    # points_A 和 points_B 是 Nx3 的数组
    centroid_A = np.mean(points_A, axis=0)
    centroid_B = np.mean(points_B, axis=0)
    H = (points_B - centroid_B).T @ (points_A - centroid_A)
    U, S, Vt = np.linalg.svd(H)
    R = Vt.T @ U.T
    if np.linalg.det(R) < 0:
        Vt[-1, :] *= -1
        R = Vt.T @ U.T
    t = centroid_A - R @ centroid_B
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

 

案例 Python实现2： 

import numpy as np


def compute_transform(points_A, points_B):
    """
    使用最小二乘法计算变换矩阵 T_{AB}，使得 p_A = T_{AB} * p_B

    参数：
        points_A (np.ndarray): (N, 3) 的数组，坐标系A下的点集
        points_B (np.ndarray): (N, 3) 的数组，坐标系B下的点集

    返回：
        T_AB (np.ndarray): 4x4 齐次变换矩阵
    """
    # 计算质心
    centroid_A = np.mean(points_A, axis=0)
    centroid_B = np.mean(points_B, axis=0)

    # 去中心化坐标
    q_A = points_A - centroid_A
    q_B = points_B - centroid_B

    # 计算协方差矩阵 H
    H = q_B.T @ q_A

    # SVD 分解求旋转
    U, S, Vt = np.linalg.svd(H)
    R = Vt.T @ U.T

    # 处理反射情况
    if np.linalg.det(R) < 0:
        Vt[-1, :] *= -1
        R = Vt.T @ U.T

    # 计算平移
    t = centroid_A - R @ centroid_B

    # 构造齐次变换矩阵
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t

    return T


points_B = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
points_A = np.array([[2, 1, 0], [1, 2, 0], [1, 1, 1]])

T_AB = compute_transform(points_A, points_B)
print("变换矩阵 T_AB:\n", T_AB)

  案例 Python实现1和 案例 Python实现2 结果是一样的

 总结 

 方法2：手眼标定（AX=XB问题）

假设我们有一个机械臂（坐标系A）和一个固定相机（坐标系B），机械臂末端执行器移动了两次，记录的运动数据如下：

适用场景：

数学建模

变换矩阵分解

旋转部分求解 

 案例：

 

 结果：

import numpy as np

def solve_rotation(A_list, B_list):
    C = []
    for A, B in zip(A_list, B_list):
        RA = A[:3, :3]
        RB = B[:3, :3]
        C.append(np.kron(np.eye(3), RA) - np.kron(RB.T, np.eye(3)))
    C = np.vstack(C)
    _, _, V = np.linalg.svd(C)
    R_X = V[-1, :].reshape(3, 3)
    # 正交化
    U, S, Vt = np.linalg.svd(R_X)
    R_X = U @ Vt
    return R_X

平移部分求解

 

 结果：

 最终变换矩阵

验证 

 总结 Python 实现： 

def solve_hand_eye(A_list, B_list):
    # A_list 和 B_list 是多个运动变换对
    M = []
    for A, B in zip(A_list, B_list):
        RA = A[:3, :3]
        RB = B[:3, :3]
        M.append(np.kron(np.eye(3), RA) - np.kron(RB.T, np.eye(3)))
    M = np.vstack(M)
    _, _, V = np.linalg.svd(M)
    X = V[-1, :].reshape(3, 3)
    # 正交化旋转矩阵
    U, S, Vt = np.linalg.svd(X)
    R = U @ Vt
    return R

四、多坐标系统一

deepseek搜的，没有验证，只看思路

在机器人、自动驾驶或增强现实中，常需要将多个传感器（如相机、激光雷达、IMU）的坐标系统一到同一个全局坐标系。以下是系统化的解决方案，涵盖数学原理、算法选择和代码实现。

问题描述

假设我们有以下三个坐标系：

1. 世界坐标系（W）：全局参考系

2. 机器人基座坐标系（B）

3. 相机坐标系（C）

4. 激光雷达坐标系（L）

已知：

• 机器人基座到末端执行器的变换：T_BE（通过运动学计算）

• 相机到末端执行器的变换：T_EC（通过手眼标定获得）

• 激光雷达检测到的标定板位姿：T_LBoard

• 相机检测到的同一标定板位姿：T_CBoard

目标

将所有传感器数据统一到世界坐标系（W）下。

坐标系关系图

变换矩阵计算步骤

计算相机到激光雷达的变换 T_CL

import numpy as np

# 标定板在相机和激光雷达中的位姿
T_CBoard = np.array([...])  # 4x4齐次矩阵
T_LBoard = np.array([...])  # 4x4齐次矩阵

# 计算 T_CL
T_CL = T_CBoard @ np.linalg.inv(T_LBoard)

计算机器人基座到相机的变换 T_BC

T_BE = np.array([...])  # 机器人基座到末端的变换
T_EC = np.array([...])  # 末端到相机的变换（手眼标定结果）
T_BC = T_BE @ T_EC

 

 计算机器人基座到激光雷达的变换 T_BL

统一到世界坐标系