📌 导语

在机器人操作任务中，比如 开门（OpenDoor），我们往往需要获取平面（例如门板）的 法向量，从而让机械臂末端的姿态与其对齐。
本文将从 深度图获取法向量 讲起，并延伸到如何在 相机坐标系、世界坐标系、末端坐标系 之间转换，最后实现 末端 Z 轴朝向目标方向 的控制。

---

1. 背景

假设我们有一台相机（深度相机/ZED/RealSense 等），能够获取一个 60×60 的平面深度数据：

depth_data.shape
# (60, 60)

我们的目标是：

1. 拟合平面的法向量（法向量方向统一使用右手坐标系定义）
2. 将法向量从 相机坐标系 转换到 世界坐标系 或 末端坐标系
3. 控制机械臂末端的朝向，使其与法向量对齐

---

2. 理论基础

2.1 相机坐标系定义（右手系）

通常的深度相机坐标系是：

• 原点：相机光心
• Z 轴：沿镜头正前方
• X 轴：向右
• Y 轴：向下

如果平面正对相机，法向量应接近 [0, 0, 1]；如果是倾斜的平面，法向量会有明显的 X/Y 分量。

---

2.2 平面法向量的数学模型

平面的方程可以写作：

$$ax+by+cz+d=0$$

其中：

• $[a,b,c]$ 就是平面的法向量

我们可以通过 最小二乘拟合 或 SVD 从点云中求得这个向量。

---

3. 实现步骤

3.1 从深度图生成点云

深度图中每个像素 (u, v) 对应一个深度 Z（相机坐标系下的距离）。

精确的反投影公式：

$$X=(u-c_x)\cdot Z/f_x$$

$$Y=(v-c_y)\cdot Z/f_y$$

$$Z=\text{depth}(u,v)$$

其中 $f_x,f_y$ 是相机焦距（像素单位），$c_x,c_y$ 是主点坐标。

---

3.2 SVD 拟合法向量

import numpy as np

points = []

for v in range(depth_data.shape[0]):
    for u in range(depth_data.shape[1]):
        Z = depth_data[v, u]
        X = (u - cx) * Z / fx
        Y = (v - cy) * Z / fy
        points.append([X, Y, Z])

points = np.array(points)

# 去中心化
centroid = np.mean(points, axis=0)
points_centered = points - centroid

# SVD 分解
_, _, vh = np.linalg.svd(points_centered)
normal = vh[-1]  # 最小奇异值对应的方向

# 单位化
normal_unit = normal / np.linalg.norm(normal)
print("法向量(相机坐标系):", normal_unit)

如果平面正对相机，结果可能是 [0, 0, 1] 或 [0, 0, -1]（方向取决于深度数据分布）。

---

4. 坐标系转换

4.1 相机 → 世界坐标系

如果知道相机的外参矩阵 $T_{\text{world→camera}}$，其旋转部分 $R_{\text{world→camera}}$ 可用于转换法向量：

normal_world = R_camera_to_world @ normal_unit

注意：法向量只受旋转影响，不受平移影响。

---

4.2 相机 → 末端坐标系

如果相机安装在机械臂末端，需要用相机到末端的旋转矩阵进行转换：

normal_end = R_camera_to_end @ normal_unit

例如一个 90° 旋转的示例矩阵：

R_camera_to_end = np.array([
    [0,  1, 0],
    [-1, 0, 0],
    [0,  0, 1]
])
normal_end = R_camera_to_end @ normal_unit

---

5. 在机械臂控制中的应用

5.1 问题描述

我们现在已知：

• 当前末端的 pose → $T_{\text{base→ee}}$
• 当前末端相对于自身的目标方向 → ${\vec{v}}_{\text{ee}}$
• 想让末端坐标系旋转，使其新的 Z 轴方向 == 当前末端的这个目标方向，映射到基坐标系下。

因此我们：

1. 将 target_vector 从末端坐标系转换到基坐标系：

   $${\vec{v}}_{\text{base}}=R_{\text{base→ee}}\cdot {\vec{v}}_{\text{ee}}$$

2. 计算从当前 Z 轴指向该目标方向的旋转；

3. 应用该旋转到当前旋转矩阵上，得到新的旋转矩阵；

4. 输出新的 [x, y, z, rx, ry, rz]。

---

5.2 解法步骤

1. 将 target_vector 从末端坐标系转换到基坐标系：

$$v_{\text{base}}=R_{\text{base→ee}}\cdot v_{\text{ee}}$$

2. 计算从当前 Z 轴到 v_base 的旋转矩阵 $R_{\text{align}}$

3. 应用旋转：

$$R_{\text{new}}=R_{\text{align}}\cdot R_{\text{base→ee}}$$

4. 得到新的位姿，发送给机械臂控制器

---

5.3 Python 实现示例

from scipy.spatial.transform import Rotation as R
import numpy as np

def align_end_effector_z_to_vector(R_base_ee, target_vector_ee):
    # Step 1: 末端向量 → 基坐标系
    v_base = R_base_ee @ target_vector_ee
    v_base /= np.linalg.norm(v_base)

    # Step 2: 当前 Z 轴方向
    z_axis_base = R_base_ee @ np.array([0, 0, 1])

    # Step 3: 计算旋转
    axis = np.cross(z_axis_base, v_base)
    angle = np.arccos(np.clip(np.dot(z_axis_base, v_base), -1.0, 1.0))

    if np.linalg.norm(axis) < 1e-6:
        return R_base_ee  # 已对齐，无需旋转

    axis /= np.linalg.norm(axis)
    R_align = R.from_rotvec(axis * angle).as_matrix()

    # Step 4: 应用旋转
    R_new = R_align @ R_base_ee
    return R_new

---

6. 完整代码（Gemini 335 & RealMan RM-65B）

import numpy as np
from scipy.spatial.transform import Rotation as R
import cv2
import sys
from PIL import Image, ImageDraw
import serial
from pyorbbecsdk import *
from utils import frame_to_bgr_image
import time
from Robotic_Arm.rm_robot_interface import *
from scipy.spatial.transform import Rotation as R

robot = RoboticArm(rm_thread_mode_e.RM_TRIPLE_MODE_E)
handle = robot.rm_create_robot_arm("192.168.1.18", 8080)


def estimate_plane_normal(depth_plane_data):
    """
    输入：
        depth_plane_data: np.ndarray, shape (H, W)
            表示深度图（单位为 meter 或 millimeter，保持一致即可）
    输出：
        normal_unit: np.ndarray, shape (3,)
            平面法向量（单位向量），已转换到末端坐标系下
    """
    points = []

    for v in range(depth_plane_data.shape[0]):
        for u in range(depth_plane_data.shape[1]):
            z = depth_plane_data[v, u]
            x = u
            y = v
            points.append([x, y, z])

    points = np.array(points)

    # 平移到质心
    centroid = np.mean(points, axis=0)
    points_centered = points - centroid

    # SVD 求解法向量
    _, _, vh = np.linalg.svd(points_centered)
    normal = vh[-1]  # 最小奇异值对应的方向向量

    # 单位化
    normal_unit = normal / np.linalg.norm(normal)

    # 转换到末端坐标系
    trans2end = np.array([
        [0,  1, 0],
        [-1, 0, 0],
        [0,  0, 1]
    ])

    normal_unit = trans2end @ normal_unit

    if normal_unit[2] < 0:
        # 如果法向量指向下方，取反
        normal_unit = -normal_unit

    return normal_unit




def align_ee_z_to_local_vector(current_pose, target_vector_local):
    """
    current_pose: [x, y, z, rx, ry, rz] - 欧拉角 (xyz)
    target_vector_local: 目标方向单位向量，**在末端坐标系下表达**
    """

    # Step 1: 提取当前姿态
    x, y, z, rx, ry, rz = current_pose
    target_vector_local = target_vector_local / np.linalg.norm(target_vector_local)

    # 当前末端旋转矩阵
    R_current = R.from_euler('xyz', [rx, ry, rz])
    R_mat = R_current.as_matrix()

    # Step 2: 将目标向量从末端系 → 基坐标系
    target_vector_world = R_mat @ target_vector_local

    # 当前Z轴在基坐标系下方向（第三列）
    z_current = R_mat[:, 2]

    # Step 3: 计算旋转向量（轴+角度）
    dot_product = np.dot(z_current, target_vector_world)
    if np.isclose(dot_product, 1.0, atol=1e-4):
        print("Z 轴已与目标方向对齐，无需旋转")
        return current_pose

    rotation_axis = np.cross(z_current, target_vector_world)
    norm_axis = np.linalg.norm(rotation_axis)

    if norm_axis < 1e-6:
        # 反向情况
        rotation_axis = np.array([1.0, 0.0, 0.0])  # 任意垂直轴
        rotation_angle = np.pi
    else:
        rotation_axis /= norm_axis
        rotation_angle = np.arccos(np.clip(dot_product, -1.0, 1.0))

    # Step 4: 构造旋转，并更新当前姿态
    R_delta = R.from_rotvec(rotation_angle * rotation_axis)
    R_new = R_delta * R_current

    # Step 5: 转为欧拉角返回
    rx_new, ry_new, rz_new = R_new.as_euler('xyz')
    new_pose = [x, y, z, rx_new, ry_new, rz_new]

    return new_pose





if __name__ == "__main__":

    config = Config()
    pipeline = Pipeline()
    try:
        profile_list = pipeline.get_stream_profile_list(OBSensorType.COLOR_SENSOR)
        color_profile: VideoStreamProfile = profile_list.get_video_stream_profile(640, 0, OBFormat.RGB, 30)
        config.enable_stream(color_profile)

        # 启用 DEPTH 流
        depth_profile_list = pipeline.get_stream_profile_list(OBSensorType.DEPTH_SENSOR)
        depth_profile = depth_profile_list.get_default_video_stream_profile()
        config.enable_stream(depth_profile)

        pipeline.start(config)
    except Exception as e:
        print(f"Camera config error: {e}")
        sys.exit(1)


    while True:

        frames = pipeline.wait_for_frames(100)
        if not frames:
            continue

        # 判断 target_center 的深度信息
        depth_frame = frames.get_depth_frame()
        if not depth_frame:
            print("No depth frame.")
            continue

        depth_format = depth_frame.get_format()
        if depth_format != OBFormat.Y16:
            print("Unsupported depth format.")
            continue

        width = depth_frame.get_width()
        height = depth_frame.get_height()
        scale = depth_frame.get_depth_scale()  # 单位转换比例

        # 从 depth_frame 中提取 numpy 格式深度图
        depth_data = np.frombuffer(depth_frame.get_data(), dtype=np.uint16)
        depth_data = depth_data.reshape((height, width))


        depth_plane_data = depth_data[230:250, 420:440] * scale  # 转为米为单位
        normal_vector_local = estimate_plane_normal(depth_plane_data)

        current_pose = robot.rm_get_current_arm_state()[1]['pose']
        new_pose = align_ee_z_to_local_vector(current_pose, normal_vector_local)
        robot.rm_movej_p(new_pose, 20, 10, 0, 1)  # Move down slightly

        pipeline.stop()
        print("Pipeline stopped.")

7. 总结

本文完整介绍了：

• 如何从深度图拟合平面法向量（SVD 方法）
• 如何在相机、世界、末端坐标系之间进行法向量转换
• 如何将机械臂末端的 Z 轴对齐到任意目标方向

这套方法在 开门（OpenDoor） 等操作任务中非常有用，例如：

• 检测门板法向量
• 末端工具朝向与门板法向量一致
• 控制机械臂沿着法向量方向推/拉门

---