2d Laser 和 Odomter 内外参数标定工具原理及使用方法

前言

两轮差速轮式机器人可以基于码盘数据和两轮间距以及车轮半径进行航迹推演，得到机器人的轨迹。激光雷达也可以利用 icp 等算法计算出两时刻间机器人的相对运动量。因此，可以利用两者数据进行融合定位，本博客根据 Censi 2013 年发表在 TRO 上的论文《Simultaneous calibration of odometry and sensor parameters for mobile robots》，对如何标定里程计内参数 ( 轮子半径，两轮间距)，以及外参数（激光和里程计之间的坐标关系）进行理论推导，并将对应标定代码整理在了我们的网站上。

说明：这里的标定仅仅是 3 自由度的(激光坐标系和机器人坐标系都是平行于地面的，只能标定 yaw 和xy)，如果还想标定激光相对于水平面的（pitch,roll，主要是安装误差，激光安装的并不水平），可以参考如下两篇论文：

2007 年 bmvc, Fully Automated Laser Range Calibration
2016 年 TRO, Full-DOF Calibration of a Rotating 2-D LIDAR with a Simple Plane Measurement

标定原理

原理概述

如下图所示，由于机器人在平面上运动，机器人相对于固定坐标系的姿态可以用 SE(2) 表示 q=(qx,qy,qθ)∈SE(2)\boldsymbol{q}=\left(q_{x}, q_{y}, q_{\theta}\right) \in \mathrm{SE}(2)q=(qx,qy,qθ)∈SE(2). 考虑到水平上运动，2d 激光通过自身无法确定 roll 和 pitch , 也无法知道相对于地面的高度，因此假设机器人运动时，激光坐标系在一个平行于地面的平面上运动，激光坐标系 (sensor frame) 坐标系相对于机器人坐标系的姿态为 ℓ=(ℓx,ℓy,ℓθ)∈SE⁡(2)\boldsymbol{\ell}=\left(\ell_{x}, \ell_{y}, \ell_{\theta}\right) \in \operatorname{SE}(2)ℓ=(ℓx,ℓy,ℓθ)∈SE(2).

通常机器人两个轮子上各自装有一个编码器，能够计算左右轮的速度 ωL,ωR\omega_{\mathrm{L}}, \omega_{\mathrm{R}}ωL,ωR，利用左右轮速度以及轮半径和轮间距能够计算移动机器人前进速度 vvv 和旋转角速度 ω\omegaω
(vω)=J(ωLωR)\left( \begin{array}{l}{v} \\ {\omega}\end{array}\right)=\boldsymbol{J} \left( \begin{array}{c}{\omega_{\mathrm{L}}} \\ {\omega_{\mathrm{R}}}\end{array}\right) (vω)=J(ωLωR)
其中 J\boldsymbol{J}J 是一个关于左右轮半径及轮间距 rL,rR,br_{\mathrm{L}}, r_{\mathrm{R}}, brL,rR,b 的函数：
J=(J11J12J21J22)=(+rL/2+rR/2−rL/b+rR/b)\boldsymbol{J}=\left( \begin{array}{ll}{J_{11}} & {J_{12}} \\ {J_{21}} & {J_{22}}\end{array}\right)=\left( \begin{array}{cc}{+r_{\mathrm{L}} / 2} & {+r_{\mathrm{R}} / 2} \\ {-r_{\mathrm{L}} / b} & {+r_{\mathrm{R}} / b}\end{array}\right) J=(J11J21J12J22)=(+rL/2−rL/b+rR/2+rR/b)
当得到机器人前进速度和旋转速度以后，进行积分，就能得到车体姿态 q\boldsymbol{q}q. 具体推导过程可参看我之前的博客。

考虑一段时间 t∈[tk,tk+1]t \in\left[t_{k}, t_{k+1}\right]t∈[tk,tk+1]，里程计推算得到的两个时刻机器人坐标系的姿态 qk\boldsymbol{q}^{k}qk 和 qk+1\boldsymbol{q}^{k+1}qk+1，利用外参数 ℓ\boldsymbol{\ell}ℓ 可以得到两时刻时间激光坐标系的相对变化量
sk=⊖(qk⊕ℓ)⊕(qk+1⊕ℓ)(1)\boldsymbol{s}^{k}=\ominus\left(\boldsymbol{q}^{k} \oplus \boldsymbol{\ell}\right) \oplus\left(\boldsymbol{q}^{k+1} \oplus \boldsymbol{\ell}\right) ~~~~~~~ (1) sk=⊖(qk⊕ℓ)⊕(qk+1⊕ℓ) (1)
其中 ⊕\oplus⊕ 表示 SE(2) 加法：
(axayaθ)⊕(bxbybθ)=(ax+bxcos⁡(aθ)−bysin⁡(aθ)ay+bxsin⁡(aθ)+bycos⁡(aθ)aθ+bθ)\left( \begin{array}{l}{a_{x}} \\ {a_{y}} \\ {a_{\theta}}\end{array}\right) \oplus \left( \begin{array}{l}{b_{x}} \\ {b_{y}} \\ {b_{\theta}}\end{array}\right)=\left( \begin{array}{c}{a_{x}+b_{x} \cos \left(a_{\theta}\right)-b_{y} \sin \left(a_{\theta}\right)} \\ {a_{y}+b_{x} \sin \left(a_{\theta}\right)+b_{y} \cos \left(a_{\theta}\right)} \\ {a_{\theta}+b_{\theta}}\end{array}\right) ⎝⎛axayaθ⎠⎞⊕⎝⎛bxbybθ⎠⎞=⎝⎛ax+bxcos(aθ)−bysin(aθ)ay+bxsin(aθ)+bycos(aθ)aθ+bθ⎠⎞
⊖\ominus⊖ 表示求逆：
⊖(axayaθ)=(−axcos⁡(aθ)−aysin⁡(aθ)+axsin⁡(aθ)−aycos⁡(aθ)−aθ)\ominus \left( \begin{array}{l}{a_{x}} \\ {a_{y}} \\ {a_{\theta}}\end{array}\right)=\left( \begin{array}{c}{-a_{x} \cos \left(a_{\theta}\right)-a_{y} \sin \left(a_{\theta}\right)} \\ {+a_{x} \sin \left(a_{\theta}\right)-a_{y} \cos \left(a_{\theta}\right)} \\ {-a_{\theta}}\end{array}\right) ⊖⎝⎛axayaθ⎠⎞=⎝⎛−axcos(aθ)−aysin(aθ)+axsin(aθ)−aycos(aθ)−aθ⎠⎞
对于上述两个运算，如果熟悉坐标系变换的同学，应该很容易理解。

为了简化，将两时刻间里程计得到的姿态记作 rk=⊖qk⊕qk+1=rk(rL,rR,b)\boldsymbol{r}^{k}=\ominus \boldsymbol{q}^{k} \oplus \boldsymbol{q}^{k+1}=\boldsymbol{r}^{k}\left(r_{\mathrm{L}}, r_{\mathrm{R}}, b\right)rk=⊖qk⊕qk+1=rk(rL,rR,b), 公式 (1) 可以简化为：
sk=⊖ℓ⊕rk(rL,rR,b)⊕ℓ(2)\boldsymbol{s}^{k}=\ominus \boldsymbol{\ell} \oplus \boldsymbol{r}^{k}\left(r_{\mathrm{L}}, r_{\mathrm{R}}, b\right) \oplus \boldsymbol{\ell}~~~~~~~(2) sk=⊖ℓ⊕rk(rL,rR,b)⊕ℓ (2)

实际上 , 两时刻间激光坐标系的运动量可以使用 icp 算法来估计，本文采用的是 pl-icp 来估计姿态 s^k\hat{\boldsymbol{s}}^{k}s^k。假设估计的相对运动服从高斯分布，就可以采用最小二乘来估计参数，把标定问题转化成优化问题。
J=−12∑k=1n∥s^k−⊖ℓ⊕rk(rL,rR,b)⊕ℓ∥Σk−12(3)\mathcal{J}=-\frac{1}{2} \sum_{k=1}^{n}\left\|\hat{\boldsymbol{s}}^{k}-\ominus \ell \oplus \boldsymbol{r}^{k}\left(r_{\mathrm{L}}, r_{\mathrm{R}}, b\right) \oplus \ell\right\|_{\mathbf{\Sigma}_{k}^{-1}}^{2} ~~~~~~~(3) J=−21k=1∑n∥∥∥s^k−⊖ℓ⊕rk(rL,rR,b)⊕ℓ∥∥∥Σk−12 (3)

标定算法的具体实现

针对公式（3），可能大家会直接使用 ceres 等工具直接优化，Censi 在论文提出了一个闭式求解方法。

先求线性部分：首先由于激光坐标系和机器人坐标系都是水平面运动，所以两个传感器得到的两个时刻之间的角度变化是相等的 sθk=rθk\boldsymbol{s}_{\theta}^{k}=\boldsymbol{r}_{\theta}^{k}sθk=rθk. 因此直接利用公式（2）中的对应项构建等式，用线性最小二乘直接求出轮速计的内参数矩阵的两个元素：
(J^21J^22)=[∑kLkTLk(σθk)2]−1∑kLkT(σθk)2s^θk\left( \begin{array}{c}{\hat{J}_{21}} \\ {\hat{J}_{22}}\end{array}\right)=\left[\sum_{k} \frac{\boldsymbol{L}_{k}^{T} \boldsymbol{L}_{k}}{\left(\sigma_{\theta}^{k}\right)^{2}}\right]^{-1} \sum_{k} \frac{\boldsymbol{L}_{k}^{T}}{\left(\sigma_{\theta}^{k}\right)^{2}} \hat{s}_{\theta}^{k} (J^21J^22)=[k∑(σθk)2LkTLk]−1k∑(σθk)2LkTs^θk
具体公式可以参照论文。
再闭式求解非线性部分：有了部分线性内参数，作者对公式（3）进行了简单的变形，变成了J=−12∑k∥ℓ⊕s^k−rk⊕ℓ∥Σk−12\mathcal{J}=-\frac{1}{2} \sum_{k}\left\|\boldsymbol{\ell} \oplus \hat{\boldsymbol{s}}^{k}-\boldsymbol{r}^{k} \oplus \boldsymbol{\ell}\right\|_{\mathbf{\Sigma}_{k}^{-1}}^{2}J=−21∑k∥∥∥ℓ⊕s^k−rk⊕ℓ∥∥∥Σk−12. 这样这个公式就能转化成带约束的二次型问题，然后利用拉格朗日乘数转化成闭式解。具体的推导请参看论文。
outlier remove : 采样的数据中可能有轮子打滑等无效数据在里面，会降低标定精度，因此标定过程反复迭代多次，每迭代一次，就计算一下每个采样数据的 Chi-2 误差。
χk=∥s^k−⊖ℓ^⊕rk(r^L,r^R,b^)⊕ℓ^∥Σk−1\chi^{k}=\left\|\hat{\boldsymbol{s}}^{k}-\ominus \hat{\ell} \oplus \boldsymbol{r}^{k}\left(\hat{r}_{\mathrm{L}}, \hat{r}_{\mathrm{R}}, \hat{b}\right) \oplus \hat{\ell}\right\|_{\mathbf{\Sigma}_{k}^{-1}} χk=∥∥∥s^k−⊖ℓ^⊕rk(r^L,r^R,b^)⊕ℓ^∥∥∥Σk−1
然后根据误差大小排序，丢弃掉一定百分比的数据。
协方差估计：协方差的估计直接采用信息矩阵的逆来计算。如果给定一个模型 yk=fk(x)+ϵk\boldsymbol{y}_{k}=\boldsymbol{f}_{k}(\boldsymbol{x})+\boldsymbol{\epsilon}_{k}yk=fk(x)+ϵk 其中 f\boldsymbol{f}f 是可导函数，ϵk\boldsymbol{\epsilon}_{k}ϵk 是协方差为 Σk\mathbf{\Sigma}_{k}Σk 的高斯噪声。协方差的下界 Cramer-Rao bound，cov⁡(x^)≥I(x)−1\operatorname{cov}(\hat{\boldsymbol{x}}) \geq \mathcal{I}(\boldsymbol{x})^{-1}cov(x^)≥I(x)−1. 在这个应用中，x=(rR,rL,b,ℓx,ℓy,ℓθ)\boldsymbol{x}=\left(r_{\mathrm{R}}, r_{\mathrm{L}}, b, \ell_{x}, \ell_{y}, \ell_{\theta}\right)x=(rR,rL,b,ℓx,ℓy,ℓθ)，yk=s^k\boldsymbol{y}_{k}=\hat{\boldsymbol{s}}^{k}yk=s^k ，观测函数 fk\boldsymbol{f}_{k}fk 为公式（2）所示。这个模型最小二乘估计的参数 x\boldsymbol{x}x 的信息矩阵为

I(x)=∑k∂fk∂xΣk−1∂fk∂x\mathcal{I}(\boldsymbol{x})=\sum_{k} \frac{\partial \boldsymbol{f}_{k}}{\partial \boldsymbol{x}} \boldsymbol{\Sigma}_{k}^{-\mathbf{1}} \frac{\partial \boldsymbol{f}_{k}}{\partial \boldsymbol{x}} I(x)=k∑∂x∂fkΣk−1∂x∂fk

工具使用

使用 ROS 录制下移动机器人的 odom 和激光 scan 数据，就能完成标定，只需要修改 launch 文件中的文件路径和文件名等信息。

   cd catkin_ws/srcgit clone https://github.com/MegviiRobot/OdomLaserCalibraToolcd ..catkin_makesource devel/setup.bashroslaunch example.launch

注意：代码可以标定车轮半径等内参数，但是需要你提供左右轮的速度，实际上左右轮速度都知道了，就不需要标定半径了。因此，我们对需求进行了改动，需要你提供 ros odom 数据格式（机器人前进速度和角速度），代码中会假定一个车轮半径和轮间距，从而给你算出一个码盘内参数。