SLAM中移动机器人的全局位姿表示

本文探讨了在SLAM(Simultaneous Localization And Mapping)中,如何进行坐标系之间的点变换。通过旋转和平移矩阵,解释了如何将参考系中的点坐标转换到机器人当前坐标系,并指出这种变换并不直接代表机器人的位姿。文章通过几何意义反向推导,说明了如何从坐标变换得到机器人在参考系下的位姿,强调了位姿变换的正确理解与计算方法。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

首先,对于绕X,Y,Z坐标轴分别旋转θ角度对应的旋转矩阵R_{x}, R_{y}, R_{z}有:

R_{x}=\begin{bmatrix} 1 & 0 &0 \\ 0&cos\theta &-sin\theta \\ 0&sin\theta & cos\theta \end{bmatrix}

R_{y}=\begin{bmatrix} cos\theta & 0 & sin\theta \\ 0 & 1&0 \\ -sin\theta & 0 & cos\theta \end{bmatrix}

R_{z}=\begin{bmatrix} cos\theta & -sin\theta & 0\\ sin\theta &cos\theta &0 \\ 0 & 0 & 1 \end{bmatrix}

这里以机器人做二维平面运动举例,其只有绕Z轴的旋转。在SLAM中,我们一般将参考系(即机器人第一帧的坐标系,亦称世界系)观测的点坐标P(x, y),重投影到移动机器人当前坐标系对应的点坐标P(x_{r}, y_{r}),这两个坐标值虽然不一样,但它们都表示空间中同一个点P。假设,参考系到机器人当前坐标系的相对变换为R,t,然后有:

\begin{bmatrix} x_{r}\\y_{r} \end{bmatrix}=R\begin{bmatrix} x\\y \end{bmatrix}+t----------(1)

将上式(1)写成齐次坐标表示,令T=[R| t],由于只考虑二维平面情况,所以有:

\begin{bmatrix} x_{r}\\y_{r} \\1 \end{bmatrix}=\begin{bmatrix} R &t \\ 0&1 \end{bmatrix}\begin{bmatrix} x\\y \\ 1 \end{bmatrix}--------(2)

上式(2)展开后与(1)式效果是一样的。那么,这里的T是机器人在参考系下的位姿吗?刚开始我想当然的以为是,实际上不是的!!!

由上面的式子(1)可以看出,通过先旋转、再平移操作可以求得同一个空间点P在不同坐标系下的表示,下面,我们反其道而行之,从几何意义上计算(x_{r}, y_{r})(x, y)的变换,如下图所示:

设参考系O-XYZ到机器人当前坐标系的O_{r}-XYZ的位姿变换为[R| t],坐标系O'-XYZO_{r}-XYZ平移-t,使得其原点与O-XYZ重合得到的。

PO-XYZ的坐标为(x,y),在O_{r}-XYZ的坐标为(x_{r},y_{r});点pO-XYZ的坐标为(x_{o},y_{o}),在O'-XYZ的坐标为(x_{o}^{'},y_{o}^{'})。根据坐标定义可知:(x_{r},y_{r})=(x_{o}^{'},y_{o}^{'}),根据几何关系有:

x_{o}=-(y_{o}^{'}sin\theta-x_{o}^{'}cos\theta )

y_{o}=y_{o}^{'}cos\theta+x_{o}^{'}sin\theta

又因为(x_{r},y_{r})=(x_{o}^{'},y_{o}^{'}),所以上式可以改写为:

\begin{bmatrix} x_{o}\\y_{o} \end{bmatrix}=\begin{bmatrix} cos\theta &-sin\theta \\ sin\theta &cos\theta \end{bmatrix}\begin{bmatrix} x_{r}\\y_{r} \end{bmatrix}

又因为\begin{bmatrix} x\\y \end{bmatrix}=\begin{bmatrix} x_{o}\\y_{0} \end{bmatrix}+t, 最终用齐次坐标系表示可以得到:

\begin{bmatrix} x\\y \\ 1 \end{bmatrix}=\begin{bmatrix} cos\theta &-sin\theta &t_{x} \\ sin\theta& cos\theta &t_{y} \\ 0& 0 &1 \end{bmatrix}\begin{bmatrix} x_{r}\\y_{r} \\ 1 \end{bmatrix}, 恰好说明,当将机器人当前坐标系的点(x_{r},y_{r})投影到参考系点(x,y)的变换T^{-1}是机器人在参考系下的位姿。相反地T则不是。

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值