机器人中手眼标定算法推导过程.docx

发布时间：2022-05-31 发布人：admin 分类：说明书资料大小：0.09M 资料格式：docx 举报版权申诉

lhjcsubupt-11004404-4744302543336806781.docx.pdf-第1页.png

第1页 / 共5页

lhjcsubupt-11004404-4744302543336806781.docx.pdf-第2页.png

第2页 / 共5页

lhjcsubupt-11004404-4744302543336806781.docx.pdf-第3页.png

第3页 / 共5页

lhjcsubupt-11004404-4744302543336806781.docx.pdf-第4页.png

第4页 / 共5页

lhjcsubupt-11004404-4744302543336806781.docx.pdf-第5页.png

第5页 / 共5页

一 SLAM坐标系与标定

1.1 SLAM中的坐标系

1.2 SLAM标定方程推导

1.3 四元数手眼标定

1.3.1 先标姿态再杆臂

1.3.2 姿态和杆臂联合求解

一 SLAM 坐标系与标定载体位姿：本质是坐标系之间的关系，包括旋转 R 和平移 T 两部分。而坐标系之间的关系使用坐标转换式来表达的。因此，载体位姿是指参考坐标系到载体坐标系的关系，即某个物理点在参考坐标系坐标转到转到载体坐标系下的坐标的转换关系。 p b  Rp w  t 或 p b  Tp w 所以推导 P 均表示坐标，下标均表示坐标系。 1.1 SLAM 中的坐标系相机参考坐标系： 0c 相机运动坐标系： tc IMU 参考坐标系： 0i IMU 运动坐标系： ti 世界坐标系： w 载体坐标系：b 为了简化模型，假设 IMU 放在载体坐标系原点，IMU 运动坐标系和载体坐标系两个坐标系一直重合，b 一直和 ti 重合；由于设计采用局部坐标，采用了 0i 作为世界坐标系 w 。

1.2 SLAM 标定方程推导注解： Tab, 表示 b 坐标系到 a 坐标系下的转换，使用了齐次坐标的形式，包含旋转和平移。 t 时刻相机位姿：都是以 0c 坐标系做参考，是空间点从 tc 坐标系到 0c 坐标系的转换关系，，（即 R c t t c t ）或者 T c t 。 T c t  T cc 0 t ， P c 0  PT c c t t t 时刻 IMU 位姿：都是以 0i 坐标系做参考，是 ti 坐标系到 0i 坐标系的转换关系，即，（ R ）或者 i t t i t T i t 。 T i t  T ii 0 t ， P i 0  PT i i t t 待标定参数：相机参考载体系（IMU）下的位姿，是 tc 坐标系到 ti 坐标系的转换关系，即，（ tR ）或者 T ，此值在任意时刻恒定。 P  i t TP c t 标定目标：通过一组 tcT 和 tiT ,求出T 。推导过程：对于任意空间点 P，推导如下，其中下标是 P 在对应坐标系下的表示。 P  i 0 TP c 0 PT i i t t  PTT c c t t

TPT i c t t  PTT c c t t 由于可以得出： TT i t  TT c t  1-1  从而得到了三者的关系式，采用最小二乘法即可标定参数 T。 1.3 四元数手眼标定标定问题描述：在获得一组 camera 和 IMU 对应时刻的位姿， T 和，求解 T。 i t T c t 四元数手眼标定方法可以分为如下两种方法，第一种是先标姿态再标定杆臂，第二种是姿态和杆臂联合标定求解。第二种使用了迭代，算法更复杂，但是迭代后精度更高。 1.3.1 先标姿态再杆臂 1.2 节得到的结论式子 1-1 可以写成两个式子： RR i  21 RR c  tR i  t i Rt c  t  31 1>先标定姿态：t 未知求 q 将上面的姿态用四元数表达，得到标定姿态的目标函数如下。此处运用了最小二乘法。 f 1    qq i || qq c 2 || i q 2  qWqW q c T () 写成四元数相乘矩阵形式：  f  1       ||    T ( WWq q q  T q Wq  T 1( qW q ）（   T T 1( qWq qq   sum  ) q c i i q c fmin 的求解结果就是  ( || qWqWqWqW q ) qWW T ()  q q q c c i i )  41 T qq ) qWsum  ，q 是 sumW 最小特征值对应的特征向量。标定的时候要 q 对每一对 iq 和 cq 对应的观测值 sumW ，再求特征向量。 2>求出姿态之后再标定杆臂:q 已知求 t

f 1       i c 2 ) ( t i  ||) 2 (  Rt t  ||) Rt c t   (|| tR   i  - (|| ) tER i  2 || || bAt   T ( () bAt bAt    T T T ( ( ) ( AtAt bbAt At    T T T ( ) (2 AtAt bbtAb   Ct t EDt    51 T bb 2  ) ) ) T T T ) fmin 的求解结果就是 TDC 1 1.3.2 姿态和杆臂联合求解 f  f 1  f 2 联合求解时，目标函数包含两部分，姿态误差部分和平移误差部分。 f 1  T qWq s  61 i i c i c 2 2 2 c t 1  || * Rt  ) (|| tER    i  2 || || Rt t kt  i  || || || qqt q tKt  c  2 * * || || * tqKt tqq    T ( * * ()* tqKt tqq   i c  T ( * ( ()) VqKt qV   il cR  T ( () ) Lq Vq Vq Lq    T T ( ) () VLq qVL    T () ( ( )) VL q qVL    T () ( ( )) VL qVL q   T qMq s 姿态误差部分由 1-4 式给出。 || f 2           联合式 1-5 和式 1-6，得到最终的目标函数是式 1-7。 f tqKt  i ( VqKt  il )* tqq  c )) qV  cR T ) qMWq T qMqqWq s  71 * * Sq      q ( T T s s T s T  81 式 1-8 可以解决 t 已知求 q 的问题。

结论：姿态和杆臂联合求解采用迭代思想，先假设姿态已知 q0,利用式 1-5 求出最优 t0,再利用 t0 和式 1-8 求出最优 q1,再利用 q1 和式 1-5 求出最优 t1。 q 1 q  ......  1 t 0  0 t q 2 t 2

分享到：

赞收藏

资料库

机器人中手眼标定算法推导过程.docx

相关推荐

人工智能

热门标签

最新资料