3. 传感器选型与标定：激光雷达、深度相机、IMU、编码器

各位同学，欢迎来到第三章。这一章我们聊聊机械臂地图构建的“眼睛”和“耳朵”——传感器。说实话，传感器选错了，后面算法再牛也白搭。我见过太多项目，花大价钱买了顶级传感器，结果因为没标定，数据一塌糊涂。

今天我们就来拆解一下：激光雷达、深度相机、IMU、编码器，到底怎么选？怎么标定？

3.1 传感器选型原则

选传感器，说白了就是找平衡。精度、成本、环境适应性、接口，这四样你得心里有数。我个人的习惯是：先定场景，再定参数，最后看预算。

3.1.1 激光雷达

激光雷达在机械臂上主要用来做高精度环境感知和障碍物检测。选型时我重点关注三个指标：

线数：16线够用，32线更好，但价格翻倍。室内静态场景，16线完全能打。
测距精度：±2cm以内是底线。我踩过坑，用了精度±5cm的雷达，建图直接歪了。
视场角：水平360°是标配，垂直视场角至少30°，不然机械臂上方有盲区。

我的经验： 如果你做的是桌面级机械臂，单线激光雷达配合编码器也能凑合。但要是做移动操作，老老实实上多线雷达。

3.1.2 深度相机

深度相机负责给机械臂提供RGB-D数据，用于物体识别和抓取。选型时我主要看：

深度范围：0.3m-3m是黄金区间。太近会过曝，太远精度下降。
分辨率：640×480是底线，1280×720更好。分辨率太低，小物体根本认不出来。
帧率：30fps够用，60fps更流畅。但帧率高了，CPU扛不住。

市面上常见的方案有结构光和ToF。结构光在室内表现好，但阳光下直接废掉。ToF抗干扰强，但精度略低。怎么选？看你环境。

3.1.3 IMU

IMU（惯性测量单元）提供加速度和角速度，用来做姿态估计和短时积分。选型原则很简单：

零偏稳定性：低于10°/h算合格，低于1°/h算优秀。零偏太大，积分几分钟就飘到天上去。
量程：加速度计±8g，陀螺仪±2000°/s，基本覆盖机械臂运动范围。
输出频率：至少200Hz，不然跟不上机械臂的快速运动。

注意： IMU不能单独用。它必须和视觉或激光数据融合，否则误差会随时间累积。我曾经试过纯IMU做位姿估计，10秒后误差就超过10厘米。

3.1.4 编码器

编码器装在电机上，直接测量关节角度。这是机械臂最可靠的传感器，没有之一。选型时我关注：

分辨率：至少1000线/圈，配合四倍频能达到4000脉冲/圈。精度不够，控制就抖。
输出类型：增量式便宜，式贵但掉电不丢位置。我建议关节用式，末端用增量式。
防护等级：IP54以上，不然灰尘和油污会要了它的命。

3.2 传感器内参标定

内参标定，说白了就是搞清楚传感器自己的“脾气”。比如相机的焦距、畸变系数，IMU的零偏和尺度因子。不标定，数据就是错的。

3.2.1 相机内参标定

相机内参包括焦距(fx, fy)、主点(cx, cy)和畸变系数(k1, k2, p1, p2)。标定方法我推荐张正友标定法，用棋盘格就行。

具体步骤：

打印一张棋盘格，贴在平面上。
从不同角度拍20-30张照片。
用OpenCV的calibrateCamera()函数计算内参。

import cv2
import numpy as np

# 棋盘格尺寸
pattern_size = (9, 6)
objp = np.zeros((pattern_size[0]*pattern_size[1], 3), np.float32)
objp[:,:2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1,2)

objpoints = []  # 世界坐标系点
imgpoints = []  # 图像坐标系点

# 遍历图片
for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
    if ret:
        objpoints.append(objp)
        imgpoints.append(corners)

# 标定
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
print("内参矩阵:\n", mtx)
print("畸变系数:\n", dist)

  避坑指南： 拍照时棋盘格要占画面1/3以上，角度变化要大。我曾经偷懒只拍了10张，结果标定出来的焦距偏差5%，建图直接歪了。

3.2.2 IMU内参标定

IMU内参包括加速度计和陀螺仪的零偏、尺度因子、交叉耦合。标定方法有六面法和旋转法。

六面法步骤：

将IMU静止放置，采集数据，计算零偏。
分别让IMU的每个轴朝上、朝下，采集数据，计算尺度因子。
用最小二乘法拟合出交叉耦合矩阵。

我习惯用开源工具imu_utils，它基于Allan方差分析，能自动标定零偏和噪声密度。

# 安装imu_utils
cd ~/catkin_ws/src
git clone https://github.com/gaowenliang/imu_utils
cd ..
catkin_make

# 采集静止数据，运行标定
rosrun imu_utils imu_an_node

小技巧： IMU标定前先预热10分钟，让温度稳定。不然零偏会随温度漂移，标定结果不准。

3.3 传感器外参标定

外参标定，就是搞清楚传感器之间的相对位置和姿态。比如相机装在激光雷达旁边，它们之间的旋转和平移是多少。不标定，数据融合就是鸡同鸭讲。

3.3.1 激光雷达与相机外参标定

激光雷达和相机的外参标定，核心是找3D点云和2D图像的对应关系。常用方法：

手动标定：选几个特征点，手动匹配，解算外参。精度一般，但快。
自动标定：用棋盘格或标定板，自动检测角点，优化外参。精度高，但麻烦。

我推荐用lidar_camera_calibration工具包。它支持棋盘格标定，自动匹配点云和图像。

# 安装工具包
cd ~/catkin_ws/src
git clone https://github.com/ankitdhall/lidar_camera_calibration
cd ..
catkin_make

# 运行标定
roslaunch lidar_camera_calibration calibration.launch

注意： 标定板要足够大，至少1m×1m，不然激光点云打上去点太少，匹配不准。我吃过这个亏，用了A4纸大小的板子，标定结果误差5cm。

3.3.2 相机与IMU外参标定

相机和IMU的外参标定，核心是找视觉和惯性数据的时间对齐和空间对齐。常用工具是Kalibr。

Kalibr标定步骤：

录制一段数据，包含相机图像和IMU数据。
运行Kalibr，输入相机内参、IMU内参、数据包。
输出相机到IMU的旋转矩阵和平移向量。

# 运行Kalibr
kalibr_calibrate_imu_camera \
  --target april_6x6_80x80cm.yaml \
  --cam camchain.yaml \
  --imu imu.yaml \
  --bag data.bag

  我的经验： 录制数据时，要尽量让传感器做六自由度运动，包括平移和旋转。只做平移，IMU的角速度信息用不上，标定结果会退化。

3.4 传感器选型与标定知识体系

下面这张图总结了本章的核心逻辑。你可以看到，从选型到内参标定，再到外参标定，每一步都环环相扣。

3.5 标定流程总结

最后，我把标定流程整理成一个表格，方便你对照执行。

标定类型	传感器	方法	工具	注意事项
内参标定	相机	张正友标定法	OpenCV	棋盘格占画面1/3以上
内参标定	IMU	六面法 / Allan方差	imu_utils	预热10分钟
外参标定	激光雷达-相机	棋盘格匹配	lidar_camera_calibration	标定板至少1m×1m
外参标定	相机-IMU	六自由度运动	Kalibr	包含平移和旋转

📚 课程目录

3. 传感器选型与标定：激光雷达、深度相机、IMU、编码器

3.1 传感器选型原则

3.1.1 激光雷达

3.1.2 深度相机

3.1.3 IMU

3.1.4 编码器

3.2 传感器内参标定

3.2.1 相机内参标定

3.2.2 IMU内参标定

3.3 传感器外参标定

3.3.1 激光雷达与相机外参标定

3.3.2 相机与IMU外参标定

3.4 传感器选型与标定知识体系

3.5 标定流程总结