LeGO LOAM坐标系问题的自我思考
- IMU坐标系
- LeGO LOAM代码分析
- 代码
- 对于IMU输出测量值的integration积分过程
- 欧拉角的旋转矩阵
- VeloToStartIMU()函数
- TransformToStartIMU(PointType *p)
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坐标系)
- 因为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坐标系只是朝向一致,但是他们并不是同一个坐标系!!!)
- 但是对于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旋转,同时是内旋,满足右乘,所以最终满足的公式为:
// roll
float x1 = cos(roll) * accX - sin(roll) * accY;
float y1 = sin(roll) * accX + cos(roll) * accY;
float z1 = accZ;//z
// pitch
float x2 = x1;//x
float y2 = cos(pitch) * y1 - sin(pitch) * z1;
float z2 = sin(pitch) * y1 + cos(pitch) * z1;
//yaw
accX = cos(yaw) * x2 + sin(yaw) * z2;
accY = y2;//y
accZ = -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 z
float x1 = cos(roll) * accX - sin(roll) * accY;
float y1 = sin(roll) * accX + cos(roll) * accY;
float z1 = accZ;//z
//pitch x
float x2 = x1;//x
float y2 = cos(pitch) * y1 - sin(pitch) * z1;
float z2 = sin(pitch) * y1 + cos(pitch) * z1;
//yaw y
accX = cos(yaw) * x2 + sin(yaw) * z2;
accY = y2;//y
accZ = -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 data
float 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 time
if (timeScanCur + pointTime > imuTime[imuPointerFront]) {//no imu data
imuRollCur = imuRoll[imuPointerFront];//imu data coverage
imuPitchCur = 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 x
float 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 initialization
imuRollStart = 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 time
imuVeloFromStartYCur = 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的转换(YawT)->(PitchT)->(RollT)因为是固定坐标系的旋转所以执行的乘法为左乘(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;
}