LOAM代码中使用的是IMU进行运动补偿。这部分在思想上比较简单，即用IMU积分得到各个点相对开始时刻的位姿。但实际上涉及一些坐标系转换，积分，时间戳问题，因此对代码进一步阅读。
IMU数据处理
对于发布的IMU消息，首先还原加速度，然后利用加速度积分得到当前imu的速度，位置，而姿态可以直接使用，存成IMUState，加入队列。
IMU设置后一般可以直接输出四元数，由加速度积分得到。但不会输出速度，因为加速度不准，积分很容易发散。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
|
void ScanRegistration::handleIMUMessage(const sensor_msgs::Imu::ConstPtr& imuIn)
{
tf::Quaternion orientation;
tf::quaternionMsgToTF(imuIn->orientation, orientation);
double roll, pitch, yaw;
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
Vector3 acc;
acc.x() = float(imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81);
acc.y() = float(imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81);
acc.z() = float(imuIn->linear_acceleration.x + sin(pitch) * 9.81);
IMUState newState;
newState.stamp = fromROSTime( imuIn->header.stamp);
newState.roll = roll;
newState.pitch = pitch;
newState.yaw = yaw;
newState.acceleration = acc;
updateIMUData(acc, newState);
}
|
1 imu消息的结构
ROS中sensor_msgs::Imu的定义：包含一个四元数表示姿态，角速度和加速度为三维向量
1
2
3
4
5
6
7
8
9
10
| Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance
|
2 减去重力得到真实的加速度
由于IMU测量的是比力，在物体静止时会测量到一个垂直向上，大小为g的加速度，因此测量值需要减去g。但由于IMU在车辆运动中跟随车辆旋转，我们需要先将世界坐标系下的
\(\textbf{g}=[0,0,9.8]\)
用orientation转换到当前IMU坐标系下，即：
\[ a_ {true} '=a '_ {measure}
- R_ {ZYX} *\textbf{g} = \left [\begin{matrix}a_{x}'\\ a_{y}'
\\ a_{z}' \end{matrix} \right ] - \left [\begin{matrix}-sin(p)g \\
sin(r)cos(p)g \\ cos(r)cos(p)g \end{matrix} \right ]
\]
3 IMUState的结构
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
| typedef struct IMUState
{
Time stamp;
Angle roll;
Angle pitch;
Angle yaw;
Vector3 position;
Vector3 velocity;
Vector3 acceleration;
static void interpolate(const IMUState& start,
const IMUState& end,
const float& ratio,
IMUState& result)
{
float invRatio = 1 - ratio;
result.roll = start.roll.rad() * invRatio + end.roll.rad() * ratio;
result.pitch = start.pitch.rad() * invRatio + end.pitch.rad() * ratio;
if (start.yaw.rad() - end.yaw.rad() > M_PI)
{
result.yaw = start.yaw.rad() * invRatio + (end.yaw.rad() + 2 * M_PI) * ratio;
}
else if (start.yaw.rad() - end.yaw.rad() < -M_PI)
{
result.yaw = start.yaw.rad() * invRatio + (end.yaw.rad() - 2 * M_PI) * ratio;
}
else
{
result.yaw = start.yaw.rad() * invRatio + end.yaw.rad() * ratio;
}
result.velocity = start.velocity * invRatio + end.velocity * ratio;
result.position = start.position * invRatio + end.position * ratio;
};
} IMUState;
|
4
将新的IMU状态更新到全局信息中
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
| void BasicScanRegistration::updateIMUData(Vector3& acc, IMUState& newState)
{
if (_imuHistory.size() > 0) {
rotateZXY(acc, newState.roll, newState.pitch, newState.yaw);
const IMUState& prevState = _imuHistory.last();
float timeDiff = toSec(newState.stamp - prevState.stamp);
newState.position = prevState.position
+ (prevState.velocity * timeDiff)
+ (0.5 * acc * timeDiff * timeDiff);
newState.velocity = prevState.velocity
+ acc * timeDiff;
}
_imuHistory.push(newState);
}
|
这里根据当前的加速度，前一个时刻的速度对位置和时间做了积分
\[p_{new} = p_{pre} + v_{prev} * \Delta t
+ \frac{1}{2} a (\Delta t)^2\]
\[v_{new} = v_{pre} + a \Delta
t\]
问题1：对加速度做2次积分应该不太准吧，代码中有用反馈信息来纠正吗？
运动补偿
把点云投影到这一帧开始时刻:
1
2
3
4
5
6
7
8
| void BasicScanRegistration::projectPointToStartOfSweep(pcl::PointXYZI& point, float relTime)
{
if (hasIMUData())
{
setIMUTransformFor(relTime);
transformToStartIMU(point);
}
}
|
1
计算该点相对于这一sweep初始时刻的位姿
1
2
3
4
5
6
7
| void BasicScanRegistration::setIMUTransformFor(const float& relTime)
{
interpolateIMUStateFor(relTime, _imuCur);
float relSweepTime = toSec(_scanTime - _sweepStart) + relTime;
_imuPositionShift = _imuCur.position - _imuStart.position - _imuStart.velocity * relSweepTime;
}
|
获得relTime时间的IMU状态
找到当前时刻之后最近的一帧IMU，用这一帧IMU和前一帧插值得到relTime时间的IMU状态：
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
| void BasicScanRegistration::interpolateIMUStateFor(const float &relTime, IMUState &outputState)
{
double timeDiff = toSec(_scanTime - _imuHistory[_imuIdx].stamp) + relTime;
while (_imuIdx < _imuHistory.size() - 1 && timeDiff > 0) {
_imuIdx++;
timeDiff = toSec(_scanTime - _imuHistory[_imuIdx].stamp) + relTime;
}
if (_imuIdx == 0 || timeDiff > 0) {
outputState = _imuHistory[_imuIdx];
} else {
float ratio = -timeDiff / toSec(_imuHistory[_imuIdx].stamp - _imuHistory[_imuIdx - 1].stamp);
IMUState::interpolate(_imuHistory[_imuIdx], _imuHistory[_imuIdx - 1], ratio, outputState);
}
}
|
问题2：_imuPositionShift为什么是当前位置-(scan开始位置+开始速度*relTime)。_imuStart看代码scan开始时的imu状态。那_imuPositionShift就是由加速运动导致的点的位置偏移？
2 把该点投影到scan初始时刻
先把点转换到全局坐标系，只加入由加速度得到的偏移？再旋转回初始时刻的imu局部坐标系
1
2
3
4
5
6
7
8
9
10
11
12
13
| void BasicScanRegistration::transformToStartIMU(pcl::PointXYZI& point)
{
rotateZXY(point, _imuCur.roll, _imuCur.pitch, _imuCur.yaw);
point.x += _imuPositionShift.x();
point.y += _imuPositionShift.y();
point.z += _imuPositionShift.z();
rotateYXZ(point, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);
}
|
问题3：这里两次旋转相对于对姿态做了补偿，但是平移方向是只对加速度产生偏差做补偿？