Press "Enter" to skip to content

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

本站内容均来自兴趣收集,如不慎侵害的您的相关权益,请留言告知,我们将尽快删除.谢谢.

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

 

一、基础坐标变换

 

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

,在全局坐标系下的位置Pw是:

 


Pw

= R * Pl = (Ry * Rp * Rr)*
Pl

 

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

 

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

 

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

Be First to Comment

发表回复

您的电子邮箱地址不会被公开。 必填项已用*标注