在做定位过程中遇到一个比较简单也很基础的问题,那就是如何将相机(或者其他传感器)坐标系下得到的位置和姿态转换到在世界坐标系下机体的位姿。
上述表述多少有些绕,这是一个很有用的实际问题,比如说在做vins-mono的时候,将imu和camera作为一个独立的模块安装在机器人上,这个模块里程计部分输出的是Rwi和twi,然而这个w实际上是这个模块第一帧位姿所在的坐标系,(为了下面更好讲解,将上面的位姿替换成Rvm-i和tvm-i)而实际安装过程并不一定把这个模块安装在机体的中心位置,这个模块和车体中心存在一个旋转和平移,那么如何根据该模块计算出的旋转和平移转化成机体的位姿呢?
首先,由于定位模块和机器人是刚性连接,也就是说定位模块发生旋转机体也会发生同样的旋转因此,Rvm-i = Rwi这里的w代表的是真正的世界坐标系。关于世界坐标系的定义通常是机体中心最初始的位置为坐标原点,然后根据车体实际需求确定坐标轴。
位置的坐标可以由以下公式得到:
也可以通过几何方法得到关于位置的计算,如下图:
左边为机体最初始时刻所在坐标系,当做世界系,右上为机体运动一段时间后到达新的位置,b为机体坐标系,vm为定位模块所在坐标系。根据上图可以得到以下几何关系:
在上式中,计算出的OO'向量就是机体在世界坐标系下的位置,世界坐标系下的OA向量和集体坐标系下的O'A'向量各分量值相等,考虑一种常见的特殊情况:二维平面下运动,并且定位模块坐标系和机体坐标系重合,那么从vm系到w系坐标变换是个单位阵,而从b系到w系的坐标变换是只和偏航角相关的旋转矩阵。