loam详细代码解析与公式推导

loam详细代码解析与公式推导(基础理论知识)

一、基础坐标变换

loam中欧拉角解算都采用R P Y 的解算方式，即先左乘R， 再左乘P， 最后左乘Y，用矩阵表示为： R = Ry * Rp * Rr。用个很简单的示例表示，在坐标系Oxyz中经过旋转后得到坐标系O'xyz，可以把坐标系Oxyz当成全局坐标系{world_link}，坐标系O'xyz当成局部坐标系{local_link}；有局部坐标系上的点P_l，在全局坐标系下的位置Pw是：

P_w = R * Pl = （Ry * Rp * Rr）* P_l

 上式将局部坐标系上的点变换到全局坐标系即：

  R = Ry * Rp * Rr

{world_link} ------------------------> {link_link}

其中：Rr是agv的翻滚角，与loam坐标系中的rot_x对应

    rot_x = [1, 0, 0;
             0, crx, -srx;
             0 , srx, crx];

          Ry是agv的偏航角，与loam坐标系中的rot_y对应

       rot_y = [cry, 0, sry; 
           0, 1, 0; 
             -sry , 0, cry];

         Rp是agv的俯仰角，与loam坐标系中的rot_z对应

            rot_z = [crz, -srz, 0; 
              srz, crz, 0; 
             0 , 0, 1];

二、loam对应的欧拉变换顺序

       loam中坐标变换通常分为从{link_world}到{world_link}和从{world_link}到{link_world}的变换，其分别对应的欧拉拉变换方式为：Rzxy 和 其逆矩阵inv(Rzxy) = R-y-x-z，其中

     Rzxy = rot_z  * rot_x  * rot_y  =  [ cry*crz + srx*sry*srz, crz*srx*sry - cry*srz, crx*sry]
                          [crx*srz,           crx*crz,         -srx]
                           [ cry*srx*srz - crz*sry, sry*srz + cry*crz*srx, crx*cry]

inv(Rzxy) = -rot_y  * -rot_x  * -rot_z = [ cry*crz + srx*sry*srz, crx*srz, cry*srx*srz - crz*sry]
                             [ crz*srx*sry - cry*srz, crx*crz, sry*srz + cry*crz*srx]
                              [     crx*sry,    -srx,               crx*cry]

三、imu角度变换计算

loam中imu的坐标系方向是前左上，假设在current的imu坐标系下的各个方向加速度为

A = [ax; ay; az]

在前左上坐标系中欧拉角的解算方式是：Rxyz = rot_z  *  rot_x  * rot_y，现在需要得到重力加速度在current坐标系下各个方向的分量，应为imu开始坐标系下G=[0; 0; -9.8]；现在相当于求全局imu坐标系下的向量在局部imu坐标系下向量值G'，故

G'  =  inv(Rxyz) * G

其中

inv(Rxyz) =  [          cry*crz,        cry*srz,    -sry]
          [ crz*srx*sry - crx*srz, crx*crz + srx*sry*srz, cry*srx]
          [ srx*srz + crx*crz*sry, crx*sry*srz - crz*srx, crx*cry]

因此

A’= A  +  inv(Rxyz) *  G

到这里，关于坐标系变换的基础理论就讲明白了。