LeGO LOAM坐标系问题的自我思考

IMU坐标系

在LeGO LOAM中IMU坐标系的形式采用前(x)-左(y)-上(z)的形式，IMU坐标系同时可以表示为载体坐标系(body frame)，世界坐标系表示的形式采用左(x)-上(y)-前(z)的形式，世界坐标系也可以称为world系，世界坐标系作为一种固定的坐标系，而IMU坐标系作为一种非固定坐标系。

下图展示为两种坐标系的表示形式：
(下图展示的坐标系为IMU坐标系)

(下图展示的坐标系为世界坐标系)

坐标系在实体汽车上的表示:

LeGO LOAM代码分析

代码

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn){double roll, pitch, yaw;tf::Quaternion orientation;tf::quaternionMsgToTF(imuIn->orientation, orientation);tf::Matrix3x3(orientation).getRPY(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;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;imuAngularVeloX[imuPointerLast] = imuIn->angular_velocity.x;imuAngularVeloY[imuPointerLast] = imuIn->angular_velocity.y;imuAngularVeloZ[imuPointerLast] = imuIn->angular_velocity.z;AccumulateIMUShiftAndRotation();}

上述代码为消除IMU测量的加速度数据受到重力加速度的影响(重力加速度存在于world坐标系)

1. 因为IMU坐标系与world坐标系是不对齐的，所以，下面这三行代码的用途如下：

#输出的为在IMU与world坐标系一致朝向的IMU坐标系下的新IMU坐标系下的加速度 测量数据值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坐标系，然后执行消除重力的影响：涉及的公式如下

同时因为IMU坐标系与world坐标系不对齐，所以第二部将X->y,Y->z,Z->x实则是考虑一个新的IMU坐标系，使得这个新的IMU坐标与world坐标系能够在朝向上一致！！！（但是需要区分的是此时新的IMU坐标系和world坐标系只是朝向一致，但是他们并不是同一个坐标系！！！）

6. 但是对于imu坐标系表示为前x->左y->上z，新的IMU坐标系(IMU’)表示为左x->上y->前z，所以 最终得到的accX，accY，accZ分别经过imu到IMU’的坐标系的变换，(所以需要经历的变换为如下图所示：)
   

对于IMU输出测量值的integration积分过程

因为accX,accY,accZ对应的都是IMU表示的新的坐标系(IMU’)下的测量数据值，因为IMU到world坐标系下的旋转满足yaw-pitch-roll旋转，同时是内旋，满足右乘，所以最终满足的公式为:

// rollfloat x1 = cos(roll) * accX - sin(roll) * accY;float y1 = sin(roll) * accX + cos(roll) * accY;float z1 = accZ;//z

// pitchfloat x2 = x1;//xfloat y2 = cos(pitch) * y1 - sin(pitch) * z1;float z2 = sin(pitch) * y1 + cos(pitch) * z1;

//yawaccX = cos(yaw) * x2 + sin(yaw) * z2;accY = y2;//yaccZ = -sin(yaw) * x2 + cos(yaw) * z2;

IMU坐标系转为机器人坐标系(左X(pitch)->上Y(yaw)->前Z(roll)),需要按照转换后的坐标系进行欧拉角的旋转yaw->pitch->roll的内旋右乘->表示为先✖roll(Z)->再✖pitch(x)->再✖yaw(Y)

    void AccumulateIMUShiftAndRotation(){float roll = imuRoll[imuPointerLast];float pitch = imuPitch[imuPointerLast];float yaw = imuYaw[imuPointerLast];float accX = imuAccX[imuPointerLast];float accY = imuAccY[imuPointerLast];float accZ = imuAccZ[imuPointerLast];
//将新的坐标系IMU'数据通过YAW->PITCH->ROLL转到world坐标系
// roll zfloat x1 = cos(roll) * accX - sin(roll) * accY;float y1 = sin(roll) * accX + cos(roll) * accY;float z1 = accZ;//z
//pitch xfloat x2 = x1;//xfloat y2 = cos(pitch) * y1 - sin(pitch) * z1;float z2 = sin(pitch) * y1 + cos(pitch) * z1;
//yaw yaccX = cos(yaw) * x2 + sin(yaw) * z2;accY = y2;//yaccZ = -sin(yaw) * x2 + cos(yaw) * z2;int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];if (timeDiff < scanPeriod) {
// 最终得到的 Shift | Velo | AngularRotation均表示为world坐标系下的数值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;imuAngularRotationX[imuPointerLast] = imuAngularRotationX[imuPointerBack] + imuAngularVeloX[imuPointerBack] * timeDiff;imuAngularRotationY[imuPointerLast] = imuAngularRotationY[imuPointerBack] + imuAngularVeloY[imuPointerBack] * timeDiff;imuAngularRotationZ[imuPointerLast] = imuAngularRotationZ[imuPointerBack] + imuAngularVeloZ[imuPointerBack] * timeDiff;}}

其最终得到的速度，偏移以及旋转的积分数据值表示的为在world坐标系下的数据

欧拉角的旋转矩阵

在adjustDistortion对点云数据执行运动去畸变的过程是这样的，首先将lidar点云同样与IMU数据统一的跟world坐标系同指向的新的lidar’坐标系(用来与world坐标系的朝向对齐)：执行如下代码的变换：

point.x = segmentedCloud->points[i].y;point.y = segmentedCloud->points[i].z;point.z = segmentedCloud->points[i].x;

然后，找到针对此刻点云对应的IMU数据信息，如果是第一帧，则不需要旋转到IMU对应的初始时刻 但是对于在scanperiod的时间间隔内，lidar360度旋转，非第一帧对应的初始时刻的IMU数据，则需要转换到初始时刻的IMU数据坐标系下也就是涉及的两个函数VeloToStartIMU()函数和TransformToStartIMU(point)函数

void adjustDistortion(){bool halfPassed = false;int cloudSize = segmentedCloud->points.size();PointType point;for (int i = 0; i < cloudSize; i++) {point.x = segmentedCloud->points[i].y;point.y = segmentedCloud->points[i].z;point.z = segmentedCloud->points[i].x;float ori = -atan2(point.x, point.z);if (!halfPassed) {if (ori < segInfo.startOrientation - M_PI / 2)ori += 2 * M_PI;else if (ori > segInfo.startOrientation + M_PI * 3 / 2)ori -= 2 * M_PI;if (ori - segInfo.startOrientation > M_PI)halfPassed = true;}else {ori += 2 * M_PI;if (ori < segInfo.endOrientation - M_PI * 3 / 2)ori += 2 * M_PI;else if (ori > segInfo.endOrientation + M_PI / 2)ori -= 2 * M_PI;}float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff;point.intensity = int(segmentedCloud->points[i].intensity) + scanPeriod * relTime;if (imuPointerLast >= 0) {//have imu datafloat pointTime = relTime * scanPeriod;imuPointerFront = imuPointerLastIteration;while (imuPointerFront != imuPointerLast) {if (timeScanCur + pointTime < imuTime[imuPointerFront]) {break;}imuPointerFront = (imuPointerFront + 1) % imuQueLength;}//find the imuPointerFront (imu time ) > current scan timeif (timeScanCur + pointTime > imuTime[imuPointerFront]) {//no imu dataimuRollCur = imuRoll[imuPointerFront];//imu data coverageimuPitchCur = imuPitch[imuPointerFront];imuYawCur = imuYaw[imuPointerFront];imuVeloXCur = imuVeloX[imuPointerFront];imuVeloYCur = imuVeloY[imuPointerFront];imuVeloZCur = imuVeloZ[imuPointerFront];imuShiftXCur = imuShiftX[imuPointerFront];imuShiftYCur = imuShiftY[imuPointerFront];imuShiftZCur = imuShiftZ[imuPointerFront];   } else {int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;//0-1 % imuQueLength xfloat ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;} else {imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;}imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;}if (i == 0) {//first point cloud initializationimuRollStart = imuRollCur;imuPitchStart = imuPitchCur;imuYawStart = imuYawCur;imuVeloXStart = imuVeloXCur;imuVeloYStart = imuVeloYCur;imuVeloZStart = imuVeloZCur;imuShiftXStart = imuShiftXCur;imuShiftYStart = imuShiftYCur;imuShiftZStart = imuShiftZCur;if (timeScanCur + pointTime > imuTime[imuPointerFront]) {imuAngularRotationXCur = imuAngularRotationX[imuPointerFront];imuAngularRotationYCur = imuAngularRotationY[imuPointerFront];imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront];}else{int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);imuAngularRotationXCur = imuAngularRotationX[imuPointerFront] * ratioFront + imuAngularRotationX[imuPointerBack] * ratioBack;imuAngularRotationYCur = imuAngularRotationY[imuPointerFront] * ratioFront + imuAngularRotationY[imuPointerBack] * ratioBack;imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront] * ratioFront + imuAngularRotationZ[imuPointerBack] * ratioBack;}imuAngularFromStartX = imuAngularRotationXCur - imuAngularRotationXLast;imuAngularFromStartY = imuAngularRotationYCur - imuAngularRotationYLast;imuAngularFromStartZ = imuAngularRotationZCur - imuAngularRotationZLast;imuAngularRotationXLast = imuAngularRotationXCur;imuAngularRotationYLast = imuAngularRotationYCur;imuAngularRotationZLast = imuAngularRotationZCur;updateImuRollPitchYawStartSinCos();} else {VeloToStartIMU();TransformToStartIMU(&point);}}segmentedCloud->points[i] = point;}imuPointerLastIteration = imuPointerLast;}

VeloToStartIMU()函数

==因为对应的velocity数据是在world坐标系下的，所以imuVeloFromStartXCur存在的坐标系也是world坐标系下的，因此，如果要转到start坐标系，需要先转移到IMU坐标系，然后在转移到start imu的坐标系 ==
**world坐标系到IMU坐标系的变换满足roll (Z)->pitch(X) ->yaw(Y) ,得到的变换如下图所示： **

    void VeloToStartIMU(){imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;//current time to start timeimuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;
//新的坐标系Y为yaw ->yaw的转置float x1 = cosImuYawStart * imuVeloFromStartXCur - sinImuYawStart * imuVeloFromStartZCur;float y1 = imuVeloFromStartYCur;float z1 = sinImuYawStart * imuVeloFromStartXCur + cosImuYawStart * imuVeloFromStartZCur;
//新的坐标系X为pitch->X的转置float x2 = x1;float y2 = cosImuPitchStart * y1 + sinImuPitchStart * z1;float z2 = -sinImuPitchStart * y1 + cosImuPitchStart * z1;
//新的坐标系Z为roll ->Z的转置imuVeloFromStartXCur = cosImuRollStart * x2 + sinImuRollStart * y2;imuVeloFromStartYCur = -sinImuRollStart * x2 + cosImuRollStart * y2;imuVeloFromStartZCur = z2;}

TransformToStartIMU(PointType *p)

函数首先通过yaw（Y）->pitch（X）->roll（Z）的右乘转到world坐标系，然后再进行world 到 start imu的转换(Yaw^T)->(Pitch^T)->(Roll^T)因为是固定坐标系的旋转所以执行的乘法为左乘（Y->X->Z）最终得到变换的结果

    void TransformToStartIMU(PointType *p){float x1 = cos(imuRollCur) * p->x - sin(imuRollCur) * p->y;float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y;float z1 = p->z;float x2 = x1;float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1;float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2;float y3 = y2;float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2;float x4 = cosImuYawStart * x3 - sinImuYawStart * z3;float y4 = y3;float z4 = sinImuYawStart * x3 + cosImuYawStart * z3;float x5 = x4;float y5 = cosImuPitchStart * y4 + sinImuPitchStart * z4;float z5 = -sinImuPitchStart * y4 + cosImuPitchStart * z4;p->x = cosImuRollStart * x5 + sinImuRollStart * y5 + imuShiftFromStartXCur;p->y = -sinImuRollStart * x5 + cosImuRollStart * y5 + imuShiftFromStartYCur;p->z = z5 + imuShiftFromStartZCur;}