里程计系统误差的三个主要来源为“左右轮实际直径与标称直径的偏差”，“左右轮实际间距与标称间距的偏差”和“两轮子直径的实际平均值与标称平均值不相等”。“左右轮实际直径与标称直径的偏差”会导致直线运动的距离误差。“左右轮实际间距与标称间距的偏差”会导致旋转运动的方向误差。“两轮子直径的实际平均值与标称平均值不相等”将同时影响直线运动和旋转运动。

这里仅记录一下我了解到的里程计标定方法。

ROS机器人Diego 1#制作（四）base controller—线速度的标定 ROS机器人Diego 1#制作（五）base controller—角速度的标定 上面两篇博文描述了一种简单的标定轮子直径和轮间距的方法。这里假设左右两轮的直径是一样的。通过控制机器人直行一米，对比机器人实际走的距离与标称一米的偏差。然后通过修改轮子直径来尽量缩小偏差。控制机器人旋转，不断获取里程计计算出的角度。当角度达到360度时就停止机器人。对比实际转过的角度与标称360度的偏差，然后修改轮子间距来尽量缩小偏差。

基于线性最小二乘对里程计进行标定。该方法不直接标定轮子的间距和轮子直径。因为激光数据比码盘计算的里程要准。该方法使用ros中的csm包处理激光数据得到机器人在 △ t \triangle t △t时间间隔内机器人的位姿变化 ( u i x ∗ u i y ∗ u i θ ∗ ) \left(\begin{array}{c}{u_{i x}^{*}} \\ {u_{i y}^{*}} \\ {u_{i \theta}^{*}}\end{array}\right) ⎝⎛​uix∗​uiy∗​uiθ∗​​⎠⎞​。将该状态量用列向量 u i ∗ u_{i}^{*} ui∗​表示，即 u i ∗ = ( u i x ∗ u i y ∗ u i θ ∗ ) u_{i}^{*}=\left(\begin{array}{c}{u_{i x}^{*}} \\ {u_{i y}^{*}} \\ {u_{i \theta}^{*}}\end{array}\right) ui∗​=⎝⎛​uix∗​uiy∗​uiθ∗​​⎠⎞​。csm包可用以下命令获取

与激光数据对齐时间戳的情况下，我们同样能得到同一时间段内里程计计算出来的位姿变化。这里用 u i = ( u i x u i y u i θ ) u_{i}=\left(\begin{array}{c}{u_{i x}} \\ {u_{i y}} \\ {u_{i \theta}}\end{array}\right) ui​=⎝⎛​uix​uiy​uiθ​​⎠⎞​表示。我们可以得到线性转换方程： u i ∗ = X ∗ u i u_{i}^{*}=X * u_{i} ui∗​=X∗ui​ 令矩阵 X = [ x 11 x 12 x 13 x 21 x 22 x 23 x 31 x 32 x 33 ] X=\left[\begin{array}{lll}{x_{11}} & {x_{12}} & {x_{13}} \\ {x_{21}} & {x_{22}} & {x_{23}} \\ {x_{31}} & {x_{32}} & {x_{33}}\end{array}\right] X=⎣⎡​x11​x21​x31​​x12​x22​x32​​x13​x23​x33​​⎦⎤​ 则针对每一组数据可以得到下列方程组： u i x ∗ x 11 + u i y ∗ x 12 + u i θ ∗ x 13 = u i x ∗ u_{i x} * x_{11}+u_{i y} * x_{12}+u_{i \theta} * x_{13}=u_{i x}^{*} uix​∗x11​+uiy​∗x12​+uiθ​∗x13​=uix∗​ u i x ∗ x 21 + u i y ∗ x 22 + u i θ ∗ x 23 = u i y ∗ u_{i x} * x_{21}+u_{i y} * x_{22}+u_{i \theta} * x_{23}=u_{i y}^{*} uix​∗x21​+uiy​∗x22​+uiθ​∗x23​=uiy∗​ u i x ∗ x 31 + u i y ∗ x 32 + u i θ ∗ x 33 = u i θ ∗ u_{i x} * x_{31}+u_{i y} * x_{32}+u_{i \theta} * x_{33}=u_{i \theta}^{*} uix​∗x31​+uiy​∗x32​+uiθ​∗x33​=uiθ∗​

只要求出矩阵 X X X，我们就可以将每次的里程计算出的位姿增量转换程具有激光数据精度的里程计增量。将上面的方程组写成如下的矩阵形式： [ u i x u i y u i θ 0 0 0 0 0 0 0 0 0 u i x u i y u i θ 0 0 0 0 0 0 0 0 0 u i x u i y u i θ ] [ x 11 ⋮ x 33 ] = [ u i x ∗ u i y ∗ u i θ ∗ ] \left[\begin{array}{ccccccccc}{u_{i x}} & {u_{i y}} & {u_{i \theta}} & {0} & {0} & {0} & {0} & {0} & {0} \\ {0} & {0} & {0} & {u_{i x}} & {u_{i y}} & {u_{i \theta}} & {0} & {0} & {0} \\ {0} & {0} & {0} & {0} & {0} & {0} & {u_{i x}} & {u_{i y}} & {u_{i \theta}}\end{array}\right]\left[\begin{array}{c}{x_{11}} \\ {\vdots} \\ {x_{33}}\end{array}\right]=\left[\begin{array}{c}{u_{i x}^{*}} \\ {u_{i y}^{*}} \\ {u_{i \theta}^{*}}\end{array}\right] ⎣⎡​uix​00​uiy​00​uiθ​00​0uix​0​0uiy​0​0uiθ​0​00uix​​00uiy​​00uiθ​​⎦⎤​⎣⎢⎡​x11​⋮x33​​⎦⎥⎤​=⎣⎡​uix∗​uiy∗​uiθ∗​​⎦⎤​ 每一帧数据形成这样一个方程组，可以用 A i X ⃗ = b i A_{i} \vec{X}=b_{i} Ai​X =bi​表示。校准时会接收n帧数据形成一个超定方程组，可以用 A X ⃗ = b A \vec{X}=b AX =b表示。其中 A = A 1 ⋮ A n A=\begin{array}{c}{A_{1}} \\ {\vdots} \\ {A_{n}}\end{array} A=A1​⋮An​​， b = b 1 ⋮ b n b=\begin{array}{c}{b_{1}} \\ {\vdots} \\ {b_{n}}\end{array} b=b1​⋮bn​​。由于超定方程组是没有准确的解的，但可以用线性最小二乘得到一个近似解 X ⃗ \vec{X} X 。 X ⃗ \vec{X} X 是矩阵 X X X在向量空间 A A A上的投影。所以它们是最接近的。而且近似解 X ⃗ \vec{X} X 有如下通解形式: X ⃗ = ( A T A ) − 1 A T b \vec{X}=\left(A^{T} A\right)^{-1} A^{T} b X =(ATA)−1ATb 使用该方法需要注意以下两点： 1）里程计数据和激光数据需保持时间戳一致。 2）校准时需要在环境特征多的地方。如果在长的走廊里，激光数据计算得到的里程增量是不准的。

轮式里程计与2D激光联合标定及里程计内参校准 Simultaneous Calibration of Odometry and Sensor Parameters for Mobile Robots 论文的源码实现

关注公众号《首飞》回复“机器人”获取精心推荐的C/C++，Python，Docker，Qt，ROS1/2，机器人学等机器人行业常用技术资料。