Lego-Loam的通信框架和Aloam相同,都是基于ros,其各个节点的运行数据流图如下:
相比于Aloam,Lego-Loam的输入会多一个imu数据,这里首先介绍imu数据预处理
imu数据主要用于做lidar点云去畸变以及odometry位姿估计;
首先对输入的imu数据去重力,坐标系遵循常规欧拉角物理定义,imu在世界坐标系下面的角度为\tilde{r} = \left[\begin{matrix} roll & pitch & yaw \end{matrix}\right]^T , 常采用先进行实际意义偏航yaw,再进行实际意义俯仰pitch,最后进行实际意义滚转roll的规则顺序来进行组合旋转构成。仅从转轴顺序上进行对应,则对应了z、y、x的坐标轴顺序,即可表示为:
R^w_I 表示imu坐标系到world坐标系的变换矩阵,即为imu坐标在world坐标系的姿态,所以imu坐标系下面的加速度为
令roll = \alpha ,pitch = \beta , yaw = \gamma , 那么有
imu的原始坐标系为tx朝前、ty朝左、tz朝上,如下图所示
而重力减速度在world坐标系下面的坐标为
所以有
然后将imu测量的三轴加速度分别减去重力分量V_I ,得到去重力的加速度。
后来将传统的imu坐标系变换到和相机坐标轴朝向相同的坐标系,矫正后的坐标系为tz朝前、tx朝左、ty朝上,矫正后的坐标系如下图所示
显示在实车上如下图所示:
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);
// 这里的roll、pitch、yaw是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;
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();
}
最后通过函数AccumulateIMUShiftAndRotation实现将以上imu坐标系下面的加速度转换到world坐标系,并在world坐标系下面进行imu角速度、加速度的积分得到world坐标系下面的车体的平移和旋转,由于加速度是交换坐标轴后的加速度,因此
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];
// 先绕Z轴(原x轴)旋转,下方坐标系示意imuHandler()中加速度的坐标轴交换
// z->Y
// ^
// | ^ y->X
// | /
// | /
// | /
// -----> x->Z
//
// |cosrz -sinrz 0|
// Rz=|sinrz cosrz 0|
// |0 0 1|
// [x1,y1,z1]^T=Rz*[accX,accY,accZ]
// 因为在imuHandler中进行过坐标变换,
// 下面的roll其实已经对应于新坐标系中(X-Y-Z)的yaw
float x1 = cos(roll) * accX - sin(roll) * accY;
float y1 = sin(roll) * accX + cos(roll) * accY;
float z1 = accZ;
// 绕X轴(原y轴)旋转
// [x2,y2,z2]^T=Rx*[x1,y1,z1]
// |1 0 0|
// Rx=|0 cosrx -sinrx|
// |0 sinrx cosrx|
float x2 = x1;
float y2 = cos(pitch) * y1 - sin(pitch) * z1;
float z2 = sin(pitch) * y1 + cos(pitch) * z1;
// 最后再绕Y轴(原z轴)旋转
// |cosry 0 sinry|
// Ry=|0 1 0|
// |-sinry 0 cosry|
// 得到world坐标系下面的三个轴加速度 accX、accY、accZ
accX = cos(yaw) * x2 + sin(yaw) * z2;
accY = y2;
accZ = -sin(yaw) * x2 + cos(yaw) * z2;
// 进行位移,速度,角度量的累加
int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;
double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];
if (timeDiff < scanPeriod) {
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;
// 角速度积分得到imu在world坐标系下面相对初始imu坐标系的旋
imuAngularRotationX[imuPointerLast] = imuAngularRotationX[imuPointerBack] + imuAngularVeloX[imuPointerBack] * timeDiff;
imuAngularRotationY[imuPointerLast] = imuAngularRotationY[imuPointerBack] + imuAngularVeloY[imuPointerBack] * timeDiff;
imuAngularRotationZ[imuPointerLast] = imuAngularRotationZ[imuPointerBack] + imuAngularVeloZ[imuPointerBack] * timeDiff;
}
}
原创声明:本文系作者授权腾讯云开发者社区发表,未经许可,不得转载。
如有侵权,请联系 cloudcommunity@tencent.com 删除。
原创声明:本文系作者授权腾讯云开发者社区发表,未经许可,不得转载。
如有侵权,请联系 cloudcommunity@tencent.com 删除。