car/arm_plane_calib

arm_plane_calib 使用说明

1. 当前机器的实际设备映射

根据现场枚举结果，当前机器建议按下面的映射理解和使用：

1.1 串口设备

• 机械臂控制口：/dev/ttyUSB1
• 机械臂默认波特率：9600

现场串口枚举：

root@ubuntu:~# ls /dev/ttyUSB*
/dev/ttyUSB0  /dev/ttyUSB1

说明：

• ttyUSB0 当前用于底盘
• ttyUSB1 当前用于机械臂

1.2 深度相机

• 深度相机型号：Orbbec Gemini 2
• 系统节点：/dev/video1

但本工程里的深度数据 不是直接按普通 UVC 摄像头去读 /dev/video1，而是通过 orbbec_camera 驱动发布 ROS 2 话题：

• /camera/color/image_raw
• /camera/depth/image_raw
• /camera/depth/camera_info

所以在做标定前，需要先把 Gemini 2 的 ROS 话题跑起来。

---

2. 目录说明

~/arm_plane_calib
├── biaodi.png                           # 标定示意图
├── collect_plane_calib.py               # 采样脚本
├── depth_click_to_learm_cmd_node.py     # 点击深度图 -> 预测舵机值 -> 可选自动发送
├── fit_plane_calib.py                   # 拟合脚本
├── learm_test.py                        # 机械臂串口测试脚本
├── model.yaml                           # 拟合后的模型参数
├── predict_plane_cmd.py                 # 点击图像预测舵机命令
├── readme.md                            # 原始说明
└── samples.yaml                         # 采样数据

---

3. 平面标定的目的

平面标定的核心目的是：

当你在相机画面中点击一个桌面目标点时，系统能够根据深度相机测得的三维信息，预测出一组机械臂舵机控制值，使机械臂末端运动到该目标点上方。

当前这套标定使用的是：

• 输入特征：x_cam, y_cam
• 拟合模型：二次多项式
• 特征顺序：[1, x, y, x², x*y, y²]

注意：当前拟合 不直接使用 z_cam 做建模，但会记录 z_cam，并在 model.yaml 中给出 z_ref_median 作为参考。

---

4. 标定前准备

4.1 先启动 Gemini 2

cd ~/dev_ws
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch origintank_bringup orbbec_gemini2.launch.py

4.2 再进入标定目录

cd ~/arm_plane_calib

---

5. 脚本说明

5.1 collect_plane_calib.py —— 采样脚本

作用：

• 订阅：
  • /camera/color/image_raw
  • /camera/depth/image_raw
  • /camera/depth/camera_info
• 鼠标点击图像中的标定点
• 自动计算该点的相机坐标 X_cam, Y_cam, Z_cam
• 终端手动输入对应的机械臂命令向量
• 自动保存到 samples.yaml
• 采满设定数量后自动退出

默认参数要点：

• save_path = samples.yaml
• depth_scale = 0.001
• min_depth_m = 0.05
• max_depth_m = 1.50
• median_kernel = 5
• target_count = 9

运行：

cd ~/arm_plane_calib
python3 collect_plane_calib.py

采样时终端输入示例：

520,610,430,500

这 4 维命令向量的具体含义，需要与你的模型输出顺序保持一致。

---

5.2 fit_plane_calib.py —— 拟合脚本

作用：

• 读取 samples.yaml
• 构造二次多项式特征
• 对机械臂命令向量做最小二乘拟合
• 输出 model.yaml
• 同时打印每一维的 MAE 和 RMSE

运行：

cd ~/arm_plane_calib
python3 fit_plane_calib.py samples.yaml model.yaml

要求：

• 至少需要 6 个样本点才能拟合
• 实际建议 9~16 个点
• 点位尽量铺满工作区，不要只集中在中间

---

5.3 predict_plane_cmd.py —— 点击图像预测舵机值

作用：

• 运行时点击图像任意位置
• 自动求取该像素点对应的相机三维坐标
• 调用 model.yaml 预测机械臂命令向量
• 在终端输出预测结果

运行：

cd ~/arm_plane_calib
python3 predict_plane_cmd.py

当前脚本中的关键约定：

• 模型输出顺序固定为：6、5、4、3
• 终端打印时会整理为：3、4、5、6
• 当前默认舵机限幅范围：125 ~ 875

示例输出：

点击像素: (694, 230)
相机坐标: X=0.0267, Y=-0.0681, Z=0.3600
预测机械臂命令向量(按模型顺序 6,5,4,3): 501, 874, 385, 393
舵机列表: 3:393,4:385,5:874,6:501

---

5.4 depth_click_to_learm_cmd_node.py —— 点击深度图后可直接发送控制

作用：

• 订阅深度图、彩色图和相机内参
• 点击图像后自动预测机械臂命令向量
• 可按参数决定是否自动通过串口发送给机械臂

关键默认参数：

• serial_port = /dev/ttyUSB0
• baud_rate = 9600
• move_time_ms = 800
• servo_ids = [6, 5, 4, 3]
• servo_min = [125, 125, 125, 125]
• servo_max = [875, 875, 875, 875]
• auto_send = True

注意：这个脚本源码默认串口仍然写的是 /dev/ttyUSB0，与你当前机器的机械臂实际口 /dev/ttyUSB1 不一致。

因此当前机器使用时，建议显式传参：

cd ~/arm_plane_calib
python3 depth_click_to_learm_cmd_node.py --ros-args -p serial_port:=/dev/ttyUSB1 -p baud_rate:=9600

---

5.5 learm_test.py —— 机械臂串口测试脚本

作用：

• 测试串口是否能连上机械臂
• 查询固件
• 回读舵机位置
• 发初始位姿命令
• 控制单个或多个舵机

当前脚本 main() 里默认写的是：

• port = /dev/ttyUSB0
• baudrate = 9600

但你当前机器的机械臂实际连接为 /dev/ttyUSB1，因此使用前建议先把源码最后一行改成：

arm = LeArmSerial(port="/dev/ttyUSB1", baudrate=9600)

再运行：

cd ~/arm_plane_calib
python3 learm_test.py

---

6. 九点标定推荐流程

6.1 布点建议

建议用 3×3 共 9 个点，尽量铺满机械臂实际抓取区域：

1   2   3
4   5   6
7   8   9

6.2 启动 Gemini 2

cd ~/dev_ws
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch origintank_bringup orbbec_gemini2.launch.py

6.3 运行采样脚本

cd ~/arm_plane_calib
python3 collect_plane_calib.py

6.4 对每个点重复以下动作

1. 鼠标点击图像中的桌面标定点
2. 手动把机械臂末端移动到该点正上方
3. 在终端输入这一点的机械臂命令向量
4. 脚本自动保存到 samples.yaml

例如输入：

520,610,430,500

说明：

• 当前标定默认以 4 维命令向量 为例
• 夹爪开合通常不参与平面标定
• 抓取时再单独附加夹爪动作即可

6.5 完成后拟合模型

cd ~/arm_plane_calib
python3 fit_plane_calib.py samples.yaml model.yaml

6.6 预测测试

cd ~/arm_plane_calib
python3 predict_plane_cmd.py