平面运动机器人的传感器外参标定

机器人的传感器外参标定"/>

平面运动机器人的传感器外参标定

简述

对任意两个传感器进行外参标定可以采用手眼标定算法来完成，但是，传统手眼标定算法对于运动具有一定的要求，可以证明，至少需要两个以上轴角方向不同的旋转运动才可以正确估计出外参旋转，因此，如果使用手眼标定算法标定外参，例如标定imu与激光雷达、imu与相机、多激光雷达的外参… 那么需要充分的全方位空间的旋转和平移运动，才可以成功完成标定。

但是如果机器人是处于平面运动的地面机器人呢？那么这个时候很可能很多的外参都是不可观的，准确来说，依然存在一个特殊情况，使得我们可以使用特定的算法标定出除了z轴平移外的全部参数，即其中一个传感器旋转运动时的轴角方向和它的z轴刚好平行，除此之外，也就是说如果你的传感器安装角度不满足这个条件，那么就别指望仅靠平面上运动就能完成标定。

简单来说，对于平面运动机器人的传感器外参标定，有两个结论：
1、如果其中一个传感器的旋转运动的轴角方向与其传感器坐标z轴是平行的，那么可以标定出除了z轴平移外的全部参数。
2、任意安装的两个传感器在平面运动下外参不可观。

例如，平面运动机器人中，轮速计、2d激光雷达等和车体平行安装的传感器都满足结论一的条件。下面将介绍如何通过算法求解出外参。

算法

分为两个步骤 1、先求解出旋转的pitch和roll。2、求解yaw、x，y。

step1:
首先我们知道手眼标定的旋转关系：
q R i R i + 1 ⊗ q C R = q C R ⊗ q C i C i + 1 q^{R_{i+1}}_{R_i}\otimes q^R_C=q^R_C \otimes q^{C_{i+1}}_{C_i} qRi​Ri+1​​⊗qCR​=qCR​⊗qCi​Ci+1​​其中R表示任何与车体水平安装的传感器(例如轮速计、2D激光…),C表示任意其他3D传感器如相机、3D激光雷达… 外参旋转为 q C R q^R_C qCR​。
由上式得：
q R i R i + 1 ⊗ q C R − q C R ⊗ q C i C i + 1 = 0 = > ( Q L ( q R i R i + 1 ) − Q R ( q C i C i + 1 ) ) q C R = 0 (1) q^{R_{i+1}}_{R_i}\otimes q^R_C-q^R_C \otimes q^{C_{i+1}}_{C_i}=0 \\ =>\begin{aligned} (Q_L(q^{R_{i+1}}_{R_i})-Q_R(q^{C_{i+1}}_{C_i}))q^R_C=0 \end{aligned}\tag{1} qRi​Ri+1​​⊗qCR​−qCR​⊗qCi​Ci+1​​=0=>(QL​(qRi​Ri+1​​)−QR​(qCi​Ci+1​​))qCR​=0​(1)

Q L , Q R Q_L,Q_R QL​,QR​即四元数的左乘、右乘矩阵。
因此转换为了一个齐次线性方程组，在给定约束 ∣ ∣ q C R ∣ ∣ = 1 ||q^R_C||=1 ∣∣qCR​∣∣=1下求解该方程的最小二乘解即可解得 q C R q^R_C qCR​。
但是由于我们的运动被限制为平面运动，上面方程的秩为2，存在多余的零空间维度，因此无法正确求解，因此我们采用别的技巧进行处理，首先将外参旋转表示为z-y-z 欧拉角形式：
q C R = q z ( α ) q y ( β ) q z ( γ ) (2) \begin{aligned} q^R_C=q_z(\alpha)q_y(\beta)q_z(\gamma)\end{aligned}\tag{2} qCR​=qz​(α)qy​(β)qz​(γ)​(2) q z ( α ) q_z(\alpha) qz​(α)即绕z轴旋转 α \alpha α角度 ， q y ( β ) q_y(\beta) qy​(β)为绕y轴旋转 β \beta β角度。
根据轴角到四元数的转换关系 q = [ c o s α 2 , u s i n α 2 ] q=[cos\frac{\alpha}{2},usin\frac{\alpha}{2}] q=[cos2α​,usin2α​],可得：
q z ( α ) = [ c o s α 2 , 0 , 0 , s i n α 2 ] T q_z(\alpha)=[cos\frac{\alpha}{2},0,0,sin\frac{\alpha}{2}]^T qz​(α)=[cos2α​,0,0,sin2α​]T q y ( β ) = [ c o s β 2 , 0 , s i n β 2 , 0 ] T q_y(\beta)=[cos\frac{\beta}{2},0,sin\frac{\beta}{2},0]^T qy​(β)=[cos2β​,0,sin2β​,0]T

将(2)带入(1)可得：
q R i R i + 1 ⊗ q z ( α ) q y ( β ) q z ( γ ) − q z ( α ) q y ( β ) q z ( γ ) ⊗ q C i C i + 1 = 0 q^{R_{i+1}}_{R_i}\otimes q_z(\alpha)q_y(\beta)q_z(\gamma)\\-q_z(\alpha)q_y(\beta)q_z(\gamma)\otimes q^{C_{i+1}}_{C_i}=0 qRi​Ri+1​​⊗qz​(α)qy​(β)qz​(γ)−qz​(α)qy​(β)qz​(γ)⊗qCi​Ci+1​​=0关键来了，由于传感器R的旋转轴与z轴平行，因此可将 q R i R i + 1 q^{R_{i+1}}_{R_i} qRi​Ri+1​​和 q z ( α ) q_z(\alpha) qz​(α)的顺序交换，可得下式：
δ = q z ( α ) ⊗ q R i R i + 1 q y ( β ) q z ( γ ) − q z ( α ) q y ( β ) q z ( γ ) ⊗ q C i C i + 1 = q z ( α ) ( q R i R i + 1 q y ( β ) q z ( γ ) − q y ( β ) q z ( γ ) q C i C i + 1 ) \delta =q_z(\alpha) \otimes q^{R_{i+1}}_{R_i} q_y(\beta)q_z(\gamma)\\-q_z(\alpha)q_y(\beta)q_z(\gamma)\otimes q^{C_{i+1}}_{C_i}\\=q_z(\alpha)(q^{R_{i+1}}_{R_i} q_y(\beta)q_z(\gamma)-q_y(\beta)q_z(\gamma)q^{C_{i+1}}_{C_i}) δ=qz​(α)⊗qRi​Ri+1​​qy​(β)qz​(γ)−qz​(α)qy​(β)qz​(γ)⊗qCi​Ci+1​​=qz​(α)(qRi​Ri+1​​qy​(β)qz​(γ)−qy​(β)qz​(γ)qCi​Ci+1​​)
显然，
q R i R i + 1 q y ( β ) q z ( γ ) − q y ( β ) q z ( γ ) q C i C i + 1 = 0 q^{R_{i+1}}_{R_i} q_y(\beta)q_z(\gamma)-q_y(\beta)q_z(\gamma)q^{C_{i+1}}_{C_i}=0 qRi​Ri+1​​qy​(β)qz​(γ)−qy​(β)qz​(γ)qCi​Ci+1​​=0
将 q y ( β ) q z ( γ ) q_y(\beta)q_z(\gamma) qy​(β)qz​(γ)记为 q Y Z q_{YZ} qYZ​,
q Y Z = [ c o s ( β 2 ) c o s ( γ 2 ) s i n ( β 2 ) s i n ( γ 2 ) s i n ( β 2 ) c o s ( γ 2 ) c o s ( β 2 ) s i n ( γ 2 ) ] (3) \begin{aligned} q_{YZ}=\begin{bmatrix} cos(\frac{\beta}{2})cos(\frac{\gamma}{2})\\sin(\frac{\beta}{2})sin(\frac{\gamma}{2}) \\ sin(\frac{\beta}{2})cos(\frac{\gamma}{2})\\cos(\frac{\beta}{2})sin(\frac{\gamma}{2})\end{bmatrix}\end{aligned} \tag{3} qYZ​= ​cos(2β​)cos(2γ​)sin(2β​)sin(2γ​)sin(2β​)cos(2γ​)cos(2β​)sin(2γ​)​ ​​(3)

于是，这个问题转换为一个求解 q Y Z q_{YZ} qYZ​的最小二乘问题：
q Y Z = arg min ⁡ q Y Z ∣ ∣ M q Y Z ∣ ∣ 2 M = [ L ( q R i R i + 1 ) − R ( q C i C i + 1 ) . . L ( q R 0 R 1 ) − R ( q C 0 C 1 ) ] q_{YZ}=\argmin \limits_{q_{YZ}}||Mq_{YZ}||^2\\ M=\begin{bmatrix} L(q^{R_{i+1}}_{R_i})-R(q^{C_{i+1}}_{C_i}) \\ .\\.\\L(q^{R_{1}}_{R_0})-R(q^{C_{1}}_{C_0})\end{bmatrix} qYZ​=qYZ​argmin​∣∣MqYZ​∣∣2M= ​L(qRi​Ri+1​​)−R(qCi​Ci+1​​)..L(qR0​R1​​)−R(qC0​C1​​)​ ​
可以证明，这个矩阵M的秩为2，证明过程见文献【2】，因此，上述最小二乘解的零空间维度为2，因此我们需要继续寻找约束，根据(3)式可知： q Y Z 1 q Y Z 2 = q Y Z 3 q Y Z 4 q_{YZ1}q_{YZ2}=q_{YZ3}q_{YZ4} qYZ1​qYZ2​=qYZ3​qYZ4​另外表示旋转的四元数都是单位四元数： ∣ ∣ q Y Z ∣ ∣ 2 = 1 ||q_{YZ}||_2=1 ∣∣qYZ​∣∣2​=1对于上述齐次线性最小二乘问题，其解为 M T M M^TM MTM最小特征值对应的特征向量，因为M的秩为2，因此有两个为0的最小特征值，这两个0特征值对应的两个线性无关的特征向量记为 t 1 , t 2 t_1,t_2 t1​,t2​,它们构成了零空间的一组基(实对称矩阵必能对角化，有n个相同的特征值必然存在n个不相关的特征向量)，因此解可以表示为： q Y Z = a t 1 + b t 2 q_{YZ}=at_1+bt_2 qYZ​=at1​+bt2​根据上面给出的两个约束即可以求解出 a , b a,b a,b。
需要注意的是，由于噪声影响， M T M M^TM MTM最终是满秩的，并不会有两个真正为0的特征值，因此只需要取2个最小的特征值对应的特征向量即可。

step2求解yaw，x，y

根据手眼标定，有关系：
( R ( q R i R i + 1 ) − I ) p C R − R Z ( α ) R ( q Y Z ) p C i C i + 1 + p R i R i + 1 = 0 (R(q^{R_{i+1}}_{R_i})-I)p^R_C-R_Z(\alpha)R(q_{YZ})p^{C_{i+1}}_{C_i}+p^{R_{i+1}}_{R_i}=0 (R(qRi​Ri+1​​)−I)pCR​−RZ​(α)R(qYZ​)pCi​Ci+1​​+pRi​Ri+1​​=0其中， q Y Z q_{YZ} qYZ​即step1求解的结果， R Z ( α ) R_Z(\alpha) RZ​(α)即外参旋转待求的一部分， p C R p^R_C pCR​为待求外参平移。
由于外参平移中,z发生变化时传感器的运动观测保持不变 ，因此z方向的平移是不可观的，因此将外参平移z设为0，并丢弃上式第3行，
R ( q Y Z ) p C i C i + 1 R(q_{YZ})p^{C_{i+1}}_{C_i} R(qYZ​)pCi​Ci+1​​即将传感器C的运动投影到运动平面，令： R ( q Y Z ) p C i C i + 1 = [ p i 1 p i 2 p i 3 ] R(q_{YZ})p^{C_{i+1}}_{C_i}=\begin{bmatrix}p_{i_1}\\p_{i_2}\\p_{i_3}\end{bmatrix} R(qYZ​)pCi​Ci+1​​= ​pi1​​pi2​​pi3​​​ ​
带入上式并忽略第3行，可得： ( R ( q R i R i + 1 ) − I ) 2 × 2 [ p x p y ] − [ c o s α − s i n α s i n α c o s α ] [ p i 1 p i 2 ] + p R i R i + 1 = 0 (R(q^{R_{i+1}}_{R_i})-I)_{2\times2}\begin{bmatrix}p_x\\p_y\end{bmatrix}-\\\begin{bmatrix}cos\alpha&-sin\alpha \\ sin\alpha & cos\alpha \end{bmatrix}\begin{bmatrix} p_{i_1}\\ p_{i_2}\end{bmatrix}+p^{R_{i+1}}_{R_i}=0 (R(qRi​Ri+1​​)−I)2×2​[px​py​​]−[cosαsinα​−sinαcosα​][pi1​​pi2​​​]+pRi​Ri+1​​=0
使用最小二乘法即可解得 p x , p y , α p_x,p_y,\alpha px​,py​,α，具体细节见参考文献【2】。

reference：

[1] 标定系列一 | 机器人手眼标定的基础理论分析 =1706381723099791360
[2] An Analytical Least-Squares Solution to the Odometer-Camera Extrinsic Calibration Problem