同时标定激光和里程计

最新推荐文章于 2024-05-15 16:26:14 发布

Jaky_wang

最新推荐文章于 2024-05-15 16:26:14 发布

阅读量661

点赞数

分类专栏： ICP

本文链接：https://blog.csdn.net/WangQiang319670/article/details/108098840

版权

ICP 专栏收录该内容

2 篇文章 0 订阅

订阅专栏

标定参数：两轮半径 r_L,r_R ，两轮间距，传感器相对于机器人坐标系的位姿 $\mathbf{l} = (l_x, l_y, l_\theta)$

运动学模型：

$\frac{d}{dt}\begin{bmatrix} q_x(t)\\ q_y(t) \\ q_\theta(t) \end{bmatrix} = \begin{bmatrix} v(t)cosq_\theta(t)\\ v(t)sinq_\theta(t) \\ \omega (t) \end{bmatrix} \qquad\qquad (1)$

其中:

$\begin{bmatrix} v(t)\\ \omega (t) \end{bmatrix} = \boldsymbol{J}\begin{bmatrix} \omega _L(t)\\ \omega _R(t) \end{bmatrix}, \qquad J = \begin{bmatrix} J_{11} & J_{12} \\ J_{21} & J_{22} \end{bmatrix} = \begin{bmatrix} +r_L/2 & +r_R/2 \\ -r_L/b & +r_R/b \end{bmatrix} \qquad\qquad(2)$

假定在每次采样间隔内轮子速度恒定

$\upsilon (t) = J_{11}\omega _L^k + J_{12}\omega _R^k = \upsilon _0^k \qquad\qquad(3) \\ \omega (t) = J_{21}\omega _L^k + J_{22}\omega _R^k = \omega _0^k \qquad\qquad(4)$

假设 $\mathbf{q}^k = q(0) = \mathbf{0}$ ，通过积分即可得到每个采样时刻的位姿

$\mathbf{o}^k = \begin{bmatrix} o_x^k \\ o_y^k \\ o_\theta^k \end{bmatrix} = \begin{bmatrix} \upsilon _0^kT^k(sin\omega _0^kT^k)/(w_0^kT^k)\\ \upsilon _0^kT^k(1 - cos\omega _0^kT^k)/(w_0^kT^k) \\ \omega _0^kT^k \end{bmatrix} \qquad\qquad (5-7)$

估计 $J_{21}, J_{22}$ :

基于机器人旋转的角度 $o_\theta^{k}$ 和传感器旋转的角度 $s_\theta^k$ 相同的原理。由激光匹配得到的角度估计为 $\hat{s}_\theta^k$ ，里程计观测到的角度 $o_\theta^k = \omega _0^kT^k = (J_{21}\omega _L + J_{22}\omega _R)T^k$ 。

$\begin{bmatrix} \hat{\omega }_L^kT^k & \hat{\omega }_R^kT^k \end{bmatrix}\begin{bmatrix} J_{21}\\ J_{22} \end{bmatrix} = \hat{s}_\theta^k + errors \qquad\qquad (8)$

所有时刻整合：

$\begin{bmatrix} \vdots & \vdots \\ \hat{\omega }_L^kT^k & \hat{\omega }_R^kT^k \\ \vdots & \vdots \end{bmatrix}\begin{bmatrix} J_{21}\\J_{22} \end{bmatrix} = \begin{bmatrix} \vdots \\ \hat{s}_\theta^k \\ \vdots \end{bmatrix} + errors$

$J_{21},J_{22}$ 可以从上式中估计出来。

由于 $J_{21} = -r_L/b, \ J_{22} = -r_R/b$ ，因此还需估计 $b, l_x, l_y, l_\theta$ 。

估计 $b, l_x, l_y, l_\theta$ :

由图可知： $\mathbf{l}\oplus \mathbf{s}^k = \mathbf{o}^k \oplus\mathbf{l}$ ，展开：

$l_x + s_x^kcosl_\theta - s_y^ksinl_\theta = o_x^k + l_x coso_\theta^k - l_ysino_\theta^k \qquad \qquad (10)$

$l_y + s_x^ksinl_\theta + s_y^kcosl_\theta = o_y^k + l_x sino_\theta^k + l_ycoso_\theta^k \qquad \qquad (11)$

其中 $o_\theta^k$ 已经估计出来。

将 o_x^k, o_y^k 用其他参数表示：

$J_{11} = -\frac{b}{2}J_{21}, J_{12} = \frac{b}{2}J_{22}$

$\upsilon _0^k = -\frac{b}{2}J_{21}\omega _L^k + \frac{b}{2}J_{22}\omega _R^k \qquad\qquad (12)$

将（12）代入（5-6）得到： $o_x^k = c_xb \qquad o_y^k = c_yb \qquad\qquad (13)$

其中:

$c_x = \frac{1}{2} T^k (-J_{21}\omega _L^k + J_{22}\omega _R^k)\frac{sin\omega _o^kT^k}{\omega _0^kT^k}$

$c_y = \frac{1}{2} T^k (-J_{21}\omega _L^k + J_{22}\omega _R^k)\frac{1 - cos\omega _o^kT^k}{\omega _0^kT^k}$

将（13）代入（10）（11）中：

$l_x + s_x^kcosl_\theta - s_y^ksinl_\theta = c_xb + l_x coso_\theta^k - l_ysino_\theta^k \qquad \qquad (14)$

$l_y + s_x^ksinl_\theta + s_y^kcosl_\theta = c_yb + l_x sino_\theta^k + l_ycoso_\theta^k \qquad \qquad (15)$

定义： $\mathbf{x} = \begin{bmatrix} b & l_x & l_y & cosl_\theta & sinl_\theta \end{bmatrix}^T$ ，额外约束： x_4^2 + x_5^2 = 1 。

那么（14）（15）写成矩阵形式 $\mathbf{L}_k\mathbf{x} = \mathbf{0}$ ：

$\begin{bmatrix} -c_x & (1-cos\hat{o}_\theta^k) & +sin\hat{o}_\theta^k & +\hat{s}_x^k & -\hat{s}_y^k\\ -c_y & -sin\hat{o}_\theta^k & (1-cos\hat{o}_\theta^k) & +\hat{s}_x^k & +\hat{s}_y^k \end{bmatrix}$

定义： $\mathbf{M} = \sum ^k \mathbf{L}_k^T\mathbf{L}^k$ ，解最小二乘问题：

$min \mathbf{x}^T\mathbf{M}\mathbf{x} \qquad\qquad (17) \\ subject to x_4^2 + x_5^2 = 1 \qquad\qquad(18)$

由于对于任意解 $\bar{\mathbf{x}}$ ， $-\bar{\mathbf{x}}$ 同样是解。因此增加额外约束：

$x_1 > 0 \qquad \qquad \qquad \qquad (19)$

求解带约束的最小二乘问题:

使用拉格朗日乘子，将（18）约束写为矩阵形式：

$\mathbf{x}^T\mathbf{W}\mathbf{x} = 1, \qquad with \qquad \mathbf{W}\doteq \begin{bmatrix} \mathbf{0}_3\times 3 & \mathbf{0}_3\times2 \\ \mathbf{0}_2\times3 & \mathbf{I}_3\times2 \end{bmatrix} \qquad \qquad \qquad \qquad (21)$

约束(19)暂时忽略，近采用当 $\hat{x}_1$ 出现负值时跳过。

使用拉格朗日乘子方法，有：

$(\mathbf{M} + \lambda \mathbf{W})\mathbf{x} = \mathbf{0}$

$\lambda$ 求解： $det(\mathbf{M}+\lambda \mathbf{M}) = 0$

$\mathbf{M}$ 有比较特殊的结构：

$\mathbf{M} = \begin{bmatrix} m_{11} & 0 & m_{13} & m_{14} & m_{15} \\ * & m_{22} & 0 & m_{35} & -m_{34} \\ * & * & m_{22} & m_{34} & m_{35} \\ * & * & * & m_{44} & 0 \\ *& * & * & *& m_{44} \end{bmatrix}$

$det(\mathbf{M}+\lambda \mathbf{M})$ 是二次多项式 $(a\lambda ^2 +b\lambda +c)$ ，其中：

$a = m_{11}m_{22}^2 - m_{22}m_{13}^2$

$b = 2m_{13}m_{22}m_{35}m_{15} - m_{22}^2m_{15}^2 - 2m_{11}m_{22}m_{35}^2 \\ \qquad + 2m_{13}m_{22}m_{34}m_{14} - 2m_{22}m_{13}^2m_{44} - m_{22}^2m_{14}^2 \\ \qquad + 2m_{11}m_{22}^2m_{44} + m_{13}^2m_{35}^2 - 2m_{11}m_{22}m_{34}^2 \\ \qquad + m_{13}^2m_{34}^2$

$c = -2m_{13}m_{35}^3m_{15} - m_{22}m_{13}^2m_{44}^2 + m_{11}m_{22}^2m_{44}^2 \\ + m_{13}^2m_{35}^2m_{44} + 2m_{13}m_{22}m_{34}m_{14}m_{44} \\ + m_{13}^2m_{34}^2m_{44} - 2m_{11}m_{22}m_{34}^2m_{44} \\ - 2m_{13}m_{34}^3m_{14} - 2m_{11}m_{22}m_{35}^2m_{44} \\ +2m_{11}m_{35}^2m_{34}^2 + m_{22}m_{14}^2m_{35}^2 \\ - 2m_{13}m_{35}^2m_{34}m_{14} - 2m_{13}m_{34}^2m_{35}m_{15} \\ + m_{11}m_{34}^4 + m_{22}m_{15}^2m_{34}^2 \\ + m_{22}m_{35}^2m_{15}^2 + m_{11}m_{35}^4 \\ - m_{22}^2m_{14}^2m_{44} + 2m_{13}m_{22}m_{35}m_{15}m_{44} \\ + m_{22}m_{34}^2m_{14}^2 - m_{22}^2m_{15}^2m_{44}$

由此求得 $\bar{\lambda}$ 。

对于任意 $det(\mathbf{M}+\bar{\lambda} \mathbf{M})$ 核空间的任意向量 $\mathbf{\upsilon}$ ，考虑（18）（19）约束，得到：

$\hat{\mathbf{x}} = \frac{sign \ \upsilon _1}{\sqrt{\upsilon _4^2 + \upsilon _5^2}}\mathbf{\upsilon }$

最终标定参数为：

$\hat{b} = \hat{x}_1$

$\hat{r}_L = \hat{x}_1\hat{J}_{21}$

$\hat{r}_R = -\hat{x}_1\hat{J}_{22}$

$\hat{l}_x = \hat{x}_2$

$\hat{l}_y = \hat{x}_3$

$\hat{l}_\theta = atan2(\hat{x}_5,\hat{x}_4)$

参考链接：

https://blog.csdn.net/heyijia0327/article/details/85000943 贺博提供了代码

https://blog.csdn.net/qq_17032807/article/details/92613892 论文翻译

Jaky_wang

关注

0
点赞
踩
6

收藏

觉得还不错? 一键收藏
0
评论
同时标定激光和里程计

标定参数：两轮半径，两轮间距，传感器相对于机器人坐标系的位姿运动学模型：其中:假定在每次采样间隔内轮子速度恒定通过积分即可得到每个采样时刻的位姿估计:基于机器人旋转的角度和传感器旋转的角度相同的原理。由激光匹配得到的角度估计为，里程计观测到的角度...
复制链接

扫一扫

专栏目录