wordpress创建自己的地图,整站seo排名公司,wordpress优化宝塔,网站结构与导航设计简述
对任意两个传感器进行外参标定可以采用手眼标定算法来完成#xff0c;但是#xff0c;传统手眼标定算法对于运动具有一定的要求#xff0c;可以证明#xff0c;至少需要两个以上轴角方向不同的旋转运动才可以正确估计出外参旋转#xff0c;因此#xff0c;如果使用…简述
对任意两个传感器进行外参标定可以采用手眼标定算法来完成但是传统手眼标定算法对于运动具有一定的要求可以证明至少需要两个以上轴角方向不同的旋转运动才可以正确估计出外参旋转因此如果使用手眼标定算法标定外参例如标定imu与激光雷达、imu与相机、多激光雷达的外参… 那么需要充分的全方位空间的旋转和平移运动才可以成功完成标定。
但是如果机器人是处于平面运动的地面机器人呢那么这个时候很可能很多的外参都是不可观的准确来说依然存在一个特殊情况使得我们可以使用特定的算法标定出除了z轴平移外的全部参数即其中一个传感器旋转运动时的轴角方向和它的z轴刚好平行除此之外也就是说如果你的传感器安装角度不满足这个条件那么就别指望仅靠平面上运动就能完成标定。
简单来说对于平面运动机器人的传感器外参标定有两个结论 1、如果其中一个传感器的旋转运动的轴角方向与其传感器坐标z轴是平行的那么可以标定出除了z轴平移外的全部参数。 2、任意安装的两个传感器在平面运动下外参不可观。
例如平面运动机器人中轮速计、2d激光雷达等和车体平行安装的传感器都满足结论一的条件。下面将介绍如何通过算法求解出外参。
算法
分为两个步骤 1、先求解出旋转的pitch和roll。2、求解yaw、xy。
step1: 首先我们知道手眼标定的旋转关系 q R i R i 1 ⊗ q C R q C R ⊗ q C i C i 1 q^{R_{i1}}_{R_i}\otimes q^R_Cq^R_C \otimes q^{C_{i1}}_{C_i} qRiRi1⊗qCRqCR⊗qCiCi1其中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_{i1}}_{R_i}\otimes q^R_C-q^R_C \otimes q^{C_{i1}}_{C_i}0 \\ \begin{aligned} (Q_L(q^{R_{i1}}_{R_i})-Q_R(q^{C_{i1}}_{C_i}))q^R_C0 \end{aligned}\tag{1} qRiRi1⊗qCR−qCR⊗qCiCi10(QL(qRiRi1)−QR(qCiCi1))qCR0(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_Cq_z(\alpha)q_y(\beta)q_z(\gamma)\end{aligned}\tag{2} qCRqz(α)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_{i1}}_{R_i}\otimes q_z(\alpha)q_y(\beta)q_z(\gamma)\\-q_z(\alpha)q_y(\beta)q_z(\gamma)\otimes q^{C_{i1}}_{C_i}0 qRiRi1⊗qz(α)qy(β)qz(γ)−qz(α)qy(β)qz(γ)⊗qCiCi10关键来了由于传感器R的旋转轴与z轴平行因此可将 q R i R i 1 q^{R_{i1}}_{R_i} qRiRi1和 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_{i1}}_{R_i} q_y(\beta)q_z(\gamma)\\-q_z(\alpha)q_y(\beta)q_z(\gamma)\otimes q^{C_{i1}}_{C_i}\\q_z(\alpha)(q^{R_{i1}}_{R_i} q_y(\beta)q_z(\gamma)-q_y(\beta)q_z(\gamma)q^{C_{i1}}_{C_i}) δqz(α)⊗qRiRi1qy(β)qz(γ)−qz(α)qy(β)qz(γ)⊗qCiCi1qz(α)(qRiRi1qy(β)qz(γ)−qy(β)qz(γ)qCiCi1) 显然 q R i R i 1 q y ( β ) q z ( γ ) − q y ( β ) q z ( γ ) q C i C i 1 0 q^{R_{i1}}_{R_i} q_y(\beta)q_z(\gamma)-q_y(\beta)q_z(\gamma)q^{C_{i1}}_{C_i}0 qRiRi1qy(β)qz(γ)−qy(β)qz(γ)qCiCi10 将 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_{i1}}_{R_i})-R(q^{C_{i1}}_{C_i}) \\ .\\.\\L(q^{R_{1}}_{R_0})-R(q^{C_{1}}_{C_0})\end{bmatrix} qYZqYZargmin∣∣MqYZ∣∣2M L(qRiRi1)−R(qCiCi1)..L(qR0R1)−R(qC0C1) 可以证明这个矩阵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} qYZ1qYZ2qYZ3qYZ4另外表示旋转的四元数都是单位四元数 ∣ ∣ q Y Z ∣ ∣ 2 1 ||q_{YZ}||_21 ∣∣qYZ∣∣21对于上述齐次线性最小二乘问题其解为 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_1bt_2 qYZat1bt2根据上面给出的两个约束即可以求解出 a , b a,b a,b。 需要注意的是由于噪声影响 M T M M^TM MTM最终是满秩的并不会有两个真正为0的特征值因此只需要取2个最小的特征值对应的特征向量即可。
step2求解yawxy
根据手眼标定有关系 ( 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_{i1}}_{R_i})-I)p^R_C-R_Z(\alpha)R(q_{YZ})p^{C_{i1}}_{C_i}p^{R_{i1}}_{R_i}0 (R(qRiRi1)−I)pCR−RZ(α)R(qYZ)pCiCi1pRiRi10其中 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_{i1}}_{C_i} R(qYZ)pCiCi1即将传感器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_{i1}}_{C_i}\begin{bmatrix}p_{i_1}\\p_{i_2}\\p_{i_3}\end{bmatrix} R(qYZ)pCiCi1 pi1pi2pi3 带入上式并忽略第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_{i1}}_{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_{i1}}_{R_i}0 (R(qRiRi1)−I)2×2[pxpy]−[cosαsinα−sinαcosα][pi1pi2]pRiRi10 使用最小二乘法即可解得 p x , p y , α p_x,p_y,\alpha px,py,α具体细节见参考文献【2】。
reference
[1] 标定系列一 | 机器人手眼标定的基础理论分析 https://zhuanlan.zhihu.com/p/93183788?utm_psn1706381723099791360 [2] An Analytical Least-Squares Solution to the Odometer-Camera Extrinsic Calibration Problem
