激光雷达和里程计内外参标定

参考文献
Simultaneous calibration of odometry and sensor parameters for mobile robots

里程计运动模型

假设左轮线速度为${\mathbf{v}}_L$，右轮角速度为${\mathbf{v}}_R$，两轮之间的距离为$b$，机器人的速度为$\mathbf{v}$，角速度为$\omega$，运动半径为$r$。
$$\frac{{\mathbf{v}}_L}{r-b/2}=\frac{{\mathbf{v}}_R}{r+b/2}$$
可解得
$$\left(\begin{array}{c}v\\ \omega \end{array}\right)=\mathbf{J}\left(\begin{array}{c}{\omega }_L\\ {\omega }_R\end{array}\right)=\left(\begin{array}{cc}\frac{r_L}{2}& \frac{r_R}{2}\\ -\frac{r_L}{b}& \frac{r_R}{b}\end{array}\right)\left(\begin{array}{c}{\omega }_L\\ {\omega }_R\end{array}\right)$$

位姿表换

记sensor frame相对于robot frame的位姿为$\mathbf{l}=(l_x,l_y,l_{\theta })$，记robot frame相对于world frame的位姿为$\mathbf{q}=(q_x,q_y,q_{\theta })$，则Sensor$t+1$时刻相对于$t$时刻的位姿${\mathbf{s}}^k=\boxminus ({\mathbf{q}}^k⊞\mathbf{l})⊞({\mathbf{q}}^{k+1}⊞\mathbf{l})$。
其中$⊞$和$\boxminus$的定义如下：
$$\left(\begin{array}{c}{a}_x\\ a_y\\ a_{\theta }\end{array}\right)⊞\left(\begin{array}{c}{b}_x\\ b_y\\ b_{\theta }\end{array}\right)=\left(\begin{array}{c}{a}_x+\cos (a_{\theta })b_x-sin(a_{\theta })b_y\\ a_y+\sin (a_{\theta })b_x+\cos (a_{\theta })b_y\\ a_{\theta }+b_{\theta }\end{array}\right)$$
$$\boxminus \left(\begin{array}{c}{a}_x\\ a_y\\ a_{\theta }\end{array}\right)=\left(\begin{array}{c}-a_x\cos (a_{\theta })-a_y\sin (a_{\theta })\\ a_x\sin (a_{\theta })-a_y\cos (a_{\theta })\\ -a_{\theta }\end{array}\right)$$

问题定义

在已知${\omega }_L$,${\omega }_R$以及sensor运动${\hat{\mathbf{s}}}^k$的情况下，求解$r_L,r_R,b,l_x,l_y,l_{\theta }$。最小化
$$||\mathbf{l}⊞{\mathbf{s}}^k-{\mathbf{r}}^k⊞\mathbf{l}|{|}_2$$其中${\mathbf{r}}^k=\boxminus {\mathbf{q}}^k⊞{\mathbf{q}}^{k+1}$

解法

由于角度的变化值与坐标系无关，
$$s_{\theta }={\int }_t^{t+\Delta t}\omega (t)dt=\omega (t)\Delta t=J_{21}{\omega }_L(t)+J_{22}{\omega }_R(t)$$
$$\left[\begin{array}{cc}{\omega }_{L0}\Delta T_0& {\omega }_{R0}\Delta T_0\\ {\omega }_{L1}\Delta T_1& {\omega }_{R1}\Delta T_1\\ ⋮& ⋮\\ {\omega }_{Ln}\Delta T_n& {\omega }_{Rn}\Delta T_n\end{array}\right]\left[\begin{array}{c}{J}_{21}\\ J_{22}\end{array}\right]=\left[\begin{array}{c}{s}_{\theta 0}\\ s_{\theta 1}\\ ⋮\\ s_{\theta n}\end{array}\right]$$
用最小二乘法即可求解
对于${\mathbf{r}}^k$，有
$$r_{\theta }(t)=\left(J_{21}{\omega }_L(t)+J_{22}{\omega }_R(t)\right)\Delta t$$
$$r_x(t)=v\cos (r_{\theta }(t))\Delta t=b(-\frac{1}{2}{J}_{21}+\frac{1}{2}{J}_{22})\cos (r_{\theta }(t))\Delta t$$
$$r_y(t)=v\sin (r_{\theta }(t))\Delta t=b(-\frac{1}{2}{J}_{21}+\frac{1}{2}{J}_{22})\sin (r_{\theta }(t))\Delta t$$
即可对$\mathbf{\varphi }=\left(\begin{array}{ccccc}b& l_x& l_y& \cos (l_{\theta })& \sin (l_{\theta })\end{array}\right)$使用二次型求解。

校准的限制条件

要求环境为二维平面