SLAM 中常见坐标系及转换关系

SLAM 中常见坐标系及转换关系

右手坐标系与yaw, pitch, roll

右手坐标系的旋转正方向：

• 从轴的正方向看向原点，逆时针方向即是旋转正向。
• 判断方法：伸出右手，拇指指向旋转轴正向，四指弯曲，四指指向的旋转方向就是正向。

世界坐标系：ENU与NED

如果IMU是按照前-右-下安装的，那么就是NED坐标系或者纬经高
如果IMU是按照右-前-上安装的，那么就是ENU坐标系或者经纬高
roll、potch、yaw对应关系

• roll_NED=pitch_ENU
• pitch_NED=roll_ENU
• yaw_NED=π/2-yaw_ENU
  旋转矩阵：（坐标点）

R = | 0   1     0    |
    | 1   0     0    |
    | 0   0    -1    |

• xy对调，z取反

自身坐标系：FRD和FLU

FRD：X轴向前，Y轴向右，Z轴向下，Front-Right-Down
FLU：X轴向前，Y轴向左，Z轴向上，Front-Left-Up
roll、potch、yaw对应关系

• roll_FLU=roll_FRD
• pitch_FLU=-pitch_FRD
• yaw_FLU=-yaw_FRD
  旋转矩阵：（坐标点）

R = | 1   0     0    |
    | 0  -1     0    |
    | 0   0    -1    |

x不变，yz取反

WGS84和ENU坐标系

ENU坐标系的优点：

• 设置的R阵更加贴近于真实的情况，因为单位是：米
• 计算两点之间的距离，可以直接进行计算
  ENU坐标系的缺点：
• 会稍复杂一点，需要确定一个初始的参考坐标
• 和真值比较时需要转化成WGS84，也无法直接加载到地图上去，需要对外输出时转化成WGS84
  WGS84坐标系的优点：
• 简单方便，绝对的坐标，非常的通用
• 可以直接将坐标直接进行绘图
  WGS84坐标系的缺点：
• 计算距离时需要引入卯酉圈半径
• R阵会非常小，如果是以弧度为单位的话，这个值会更加的小
• 计算P阵时会出现无法求逆的情况，需要对P的计算公式进行变形，采用5.3.31c的公式：
• 如果采用序惯滤波的方法，可以避免矩阵的求逆，从而避免发散的问题