LOAM中消除重力影响和预积分（R，V、P）的计算（超详细）-EW帮帮网

看了多篇博客，虽然列举出来了公式，但是整体看下来，还是看不到很清晰的东西。钻研了一下，主要是用的这个函数

  ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 50, imuHandler);

所以主要看imuHnadler的代码了。


//接收imu消息，imu坐标系为x轴向前，y轴向右，z轴向上的右手坐标系
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
  double roll, pitch, yaw;
  tf::Quaternion orientation;
  //convert Quaternion msg to Quaternion
  tf::quaternionMsgToTF(imuIn->orientation, orientation);
  //This will get the roll pitch and yaw from the matrix about fixed axes X, Y, Z respectively. That's R = Rz(yaw)*Ry(pitch)*Rx(roll).
  //Here roll pitch yaw is in the global frame
  tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

  //减去重力的影响,求出xyz方向的加速度实际值，并进行坐标轴交换，统一到z轴向前,x轴向左的右手坐标系, 交换过后RPY对应fixed axes ZXY(RPY---ZXY)。Now R = Ry(yaw)*Rx(pitch)*Rz(roll).
  float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
  float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
  float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;

  //循环移位效果，形成环形数组
  imuPointerLast = (imuPointerLast + 1) % imuQueLength;

  imuTime[imuPointerLast] = imuIn->header.stamp.toSec();
  imuRoll[imuPointerLast] = roll;
  imuPitch[imuPointerLast] = pitch;
  imuYaw[imuPointerLast] = yaw;
  imuAccX[imuPointerLast] = accX;
  imuAccY[imuPointerLast] = accY;
  imuAccZ[imuPointerLast] = accZ;

  AccumulateIMUShift();
}

其中这个函数调用了 AccumulateIMUShift();来作预积分。


//积分速度与位移
void AccumulateIMUShift()
{
  float roll = imuRoll[imuPointerLast];
  float pitch = imuPitch[imuPointerLast];
  float yaw = imuYaw[imuPointerLast];
  float accX = imuAccX[imuPointerLast];
  float accY = imuAccY[imuPointerLast];
  float accZ = imuAccZ[imuPointerLast];

  //将当前时刻的加速度值绕交换过的ZXY固定轴（原XYZ）分别旋转(roll, pitch, yaw)角，转换得到世界坐标系下的加速度值(right hand rule)
  //绕z轴旋转(roll)
  float x1 = cos(roll) * accX - sin(roll) * accY;
  float y1 = sin(roll) * accX + cos(roll) * accY;
  float z1 = accZ;
  //绕x轴旋转(pitch)
  float x2 = x1;
  float y2 = cos(pitch) * y1 - sin(pitch) * z1;
  float z2 = sin(pitch) * y1 + cos(pitch) * z1;
  //绕y轴旋转(yaw)
  accX = cos(yaw) * x2 + sin(yaw) * z2;
  accY = y2;
  accZ = -sin(yaw) * x2 + cos(yaw) * z2;

  //上一个imu点
  int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;
  //上一个点到当前点所经历的时间，即计算imu测量周期
  double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];
  //要求imu的频率至少比lidar高，这样的imu信息才使用，后面校正也才有意义
  if (timeDiff < scanPeriod) {//（隐含从静止开始运动）
    //求每个imu时间点的位移与速度,两点之间视为匀加速直线运动
    imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff 
                              + accX * timeDiff * timeDiff / 2;
    imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff 
                              + accY * timeDiff * timeDiff / 2;
    imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff 
                              + accZ * timeDiff * timeDiff / 2;

    imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;
    imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;
    imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;
  }
}

看着是不是有点绕，那么接下来来看解释

二、公式推导

1）首先要确定imu坐标轴指向

AccumulateIMUShift()函数中从imu当前坐标系变换到imu世界坐标系中，用的是roll(z)---pitch(x)---yaw(y)，所以imu的安置就是z轴指向前进方向，x轴指向左边，y轴指向上，也就是imu按照左(x)---上(y)---前(z)安置在载体上的。

2）要认清坐标系变换关系

AccumulateIMUShift()中按照 R = Ry(yaw) *Rx(pitch) * Rz(roll)构成的旋转矩阵，R就是从imu当前坐标系变换到imu世界坐标系的旋转矩阵，那么反过来R.inverse或者R的转置就是从世界坐标系变换到当前坐标系中。

3）接下来看imuHandler，里面计算出来了roll,pitch,yaw三个角度，那么这三个公式是怎么来的呢？

  float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
  float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
  float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;

重点推导来了。

首先我们知道imu安置是左(x)---上(y)---前(z)，所以重力在该坐标系下矢量是多少呢？当然是G = （0，-9.81, 0)对不对？所以imu当前坐标系下的重力矢量就是g= R.inverse() * G

然后我们来推导一下公式

$R = Ry(yaw) *Rx(pitch) * Rz(roll) = \begin{bmatrix} cos(yaw) & 0 & sin(yaw) \\ 0& 1& 0\\ -sin(yaw)& 0 & cos(yaw)) \end{bmatrix} \begin{bmatrix} 1 & 0& 0\\ 0& cos(pitch)& -sin(pitch) \\ 0& sin(pitch)& cos(pitch) \end{bmatrix} \begin{bmatrix} cos(roll)& -sin(roll) & 0\\ sin(roll) & cos(roll)& 0 \\ 0& 0 & 1 \end{bmatrix}$

= $\begin{bmatrix} cos(yaw)*cos(roll) + sin(yaw) * sin(pitch)* sin(roll)& cos(yaw)*sin(roll) +sin(yaw)*sin(pitch)*cos(roll) & sin(yaw)*cos(pitch))\\ cos(pitch)*sin(roll)& cos(pitch)*cos(roll) & -sin(pitch) \\ -sin(yaw)*cos(roll)+cos(yaw)*sin(pitch)*sin(roll)& sin(yaw)*sin(roll)) + cos(yaw)*sin(pitch)*cos(roll)& cor(yaw) * cos(pitch) \end{bmatrix}$

那么我们用R.inverse()*（0，-9.81, 0),是不是就可以求得：

$g = \begin{bmatrix} sin(roll) * cos(pitch) * 9.81\\ cos(roll) * cos(pitch) * 9.81\\ -sin(pitch) * 9.81 \end{bmatrix}$

那么用imu当前坐标系中的加速度减去g，就可以得到当前坐标系中的去除重力后的加速度。

也就得到了：

  float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
  float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
  float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;

所以这时候accx和accy和accz就是imu当前坐标系中的去除重力后的加速度

三、预积分生成每一个点的全局旋转矩阵、位移（位置）和速度

当然就是看AccumulateIMUShift();这个函数

首先用下面公式，把加速度变换到世界坐标系下，

$\begin{bmatrix} accx\\ accy\\ accz \end{bmatrix}_{imu_world} = Ry(yaw) *Rx(pitch) * Rz(roll) * \begin{bmatrix} accx\\ accy\\ accz \end{bmatrix}_{current}$

然后，计算世界坐标系下的位置，旋转矩阵和速度。

当然，旋转矩阵已经是有的了。位置就是根据s = s0+ v*t +1/2*a*t*t得到的，这个位置是imu世界坐标系下的位置。

    imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff 
                              + accX * timeDiff * timeDiff / 2;

速度 v = v0+ a* t,这个是imu世界坐标系下的v。

    imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;
    imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;
    imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;

到这里，就计算得到了每一个点的旋转量、平移量和速度。

LOAM中消除重力影响和预积分（R，V、P）的计算（超详细）

网站公告

今日签到

热门文章

最新发布