IMU系统的运动学
惯性测量单元（IMU）已经非常普及了。我们在绝大多数电子设备中都能找到IMU：车辆、手机、手表、头盔，甚至足球当中都内置了IMU。它们的体积很小，安装在设备内部，可以提供有效的局部运动估计，实现一些有趣的功能。在自动驾驶中，惯性导航器件也是十分基础的定位装置。惯性导航提供的定位效果基本与外部环境和其它传感器数据无关，具有很高的泛用性和可靠性。
典型的六轴IMU由陀螺仪（Gyroscope）和加速度计（Accelerator）组成。虽然它们测量的目标都是物体的惯性，但其实现手段非常多样，从低成本的MEMS（Micro-electromechanical Systems，微0087机电）惯性导航到昂贵的光纤陀螺，它们的作用都是精确的测量物体的惯性。
IMU通常安装在一个运动的系统中。通过测量运动载体的惯性，推断物体本身的状态。这些与惯性相关的物理量通常不是直接的位置和旋转，而是微分之后的物理量。IMU的陀螺仪可以测量物体的角速度，而加速度计可以测量物体的加速度。它们内部可以根据受力或者时间等其它物理量来推算角速度和加速度，但从外部来看，只需要关心它们对角速度和加速度的测量是否精确，以及这些量和车辆位置、姿态之间的关系。
根据运动学，可以把连续时间的运动方程列出来。
\begin{matrix}
\dot{R} =R\omega^{\wedge }或 \dot{q} =\frac{1}{2} q\omega
\
\dot{p} =v
\
\dot{v} =a
\end{matrix}
其中旋转部分既可以用旋转矩阵表示，也可以用四元数来表示。
在不考虑地球自转时，也可以简单的将大地视为固定的世界坐标系。这时IMU的测量值$\tilde{\omega }$ ,$\tilde{a}$，就是车辆本身的角速度，以及车体坐标系下的加速度：
\begin{matrix}
\tilde{a} =R^{T} a
\
\tilde{\omega } =\omega
\end{matrix}
注意$R^{T}$ 带下标之后就是$R_{bw}$。它将世界坐标系下的物理量转换到车体坐标系。
实际的车辆或者机器人都在地球表面运行。这些系统受到重力的影响，所以应该把重力写在系统方程中。在绝大多数IMU系统中，可以忽略地球自转影响，从而把IMU测量值写成：
\begin{matrix}
\tilde{a} =R^{T} （a-g）
\
\tilde{\omega } =\omega
\end{matrix}
g为地球的重力。当前如果在无重力环境下测量物体的加速度，就不会出现重力项。
注意，这里g的符合和坐标系定义相关，我们的车体坐标系和世界坐标系都是Z轴向上，于是g通常取值$(0,0,-9.8)^T$，按照如上坐标系定义，测量方程中应该为a-g。
为了便于理解，可以试着想象一个水平放置的IMU。
假设IMU静止，由于物体加速度的测量实际上是通过测量受力情况得到的，这个IMU应该受到一个反向的支撑力，所以应该测到一个-g方向的重力。
如果把IMU颠倒过来，$R^{T}$ 就发生了改变，也可以读取到正的重力g。
如果IMU在空中做自由落体运动，那么传感器本身将测不到外力影响，此时a-g=0，加速度计应该输出一个零测量值。
前面的惯导测量值的方程是在无噪声的情况下列写的。如果想要写一个仿真系统，那么可以用这种不带噪声模型的方程。不过实际的IMU测量值通常都带有噪声，因此我们要考虑噪声的影响。
注意另一种情况：如果不把IMU放在车辆中心，当车辆方式旋转和平移时，IMU还应该测量到由车辆旋转导致的离心力、科氏力和角加速度，最后体现在加速度计的读数上。有些车辆还存在各种机械减震，如悬挂系统、车辆本身的运动部件，它们也会影响IMU的读数。
因此完整的方程还应该加如这些项，但是将这些小项全部写到后面的状态估计方程中，势必使得方程变得冗长。
一方面这些读数本身是小量；另一方面，可以通过尽量保证IMU的安装位置在车辆中心，避免由于IMU与载体不重合引来的问题。
IMU数据进行短时间航迹推算
代码
声明一个用与 IMU积分的类 ，来实现 短时间内的航迹推算
类的名字叫 IMUIntegration
构造函数 有三个变量进行私有变量初始化 重力、初始陀螺仪零偏、初始加速度零偏。
class IMUIntegration {
public:
IMUIntegration(const Vec3d& gravity, const Vec3d& init_bg, const Vec3d& init_ba)
: gravity_(gravity), bg_(init_bg), ba_(init_ba) {}
下面是在imu数据一帧一帧进来，如何实现状态的积分，在使用的时候则是来一帧IMU数据，调用一次这个函数
函数名称
void AddIMU(const IMU& imu) {
计算时间间隔，与上一帧IMU数据时间间隔
double dt = imu.timestamp_ - timestamp_;
时间间隔满足要求，间隔时间不能过长
if (dt > 0 && dt < 0.1) {
更新位置，用上一帧的速度与姿态
p_ = p_ + v_ * dt + 0.5 * gravity_ * dt * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt;
对应公式：
p_{j}=p_{k} +\sum_{k=i}^{j-1} [v_{k}\triangle t+\frac{1}{2} g\triangle t^{2} ] +\frac{1}{2}\sum_{k=i}^{j-1}R_{k}(\tilde{a}-b_{a,k})\triangle t^{2}
更新速度，用上一帧的姿态
v_ = v_ + R_ * (imu.acce_ - ba_) * dt + gravity_ * dt;
对应公式：
v_{j}=v_{k}+\sum_{k=i}^{j-1}[R_{k}(\tilde{a}-b_{a,k})\triangle t+g\triangle t]
更新姿态（旋转矩阵）
R_ = R_ * Sophus::SO3d::exp((imu.gyro_ - bg_) * dt);
R_{j}=R_{i}\prod_{k=i}^{j-1} \exp((\tilde{\omega } -b_{g,k})\triangle t)
更新时间，用于判断下一帧数据，在时间间隔上是否满足要求
timestamp_ = imu.timestamp_;
返回有IMU积分的各个航迹状态
/// 组成NavState
NavStated GetNavState() const { return NavStated(timestamp_, R_, p_, v_, bg_, ba_); }
SO3 GetR() const { return R_; }
Vec3d GetV() const { return v_; }
Vec3d GetP() const { return p_; }
下面是私有变量的声明
private:
// 累计量
SO3 R_;
Vec3d v_ = Vec3d::Zero();
Vec3d p_ = Vec3d::Zero();
double timestamp_ = 0.0;
// 零偏，由外部设定
Vec3d bg_ = Vec3d::Zero();
Vec3d ba_ = Vec3d::Zero();
Vec3d gravity_ = Vec3d(0, 0, -9.8); // 重力
gazebo中进行仿真测试
在gazebo中的IMU插件对惯导的噪声描述很简单，仅有一个高斯噪声可以设置。
其中xacro设置的一个例子如下：
<gazebo reference="imu_base_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>200</update_rate>
<visualize>true</visualize>
<topic>/jk/imu</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>/jk/imu</topicName>
<bodyName>imu_base_link</bodyName>
<updateRateHZ>200.0</updateRateHZ>
<gaussianNoise>0.00329</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_base_link</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
其中gaussianNoise噪声参数 可以进行手动设置。
与真实的IMU 噪声模型差别很大，用rotors的imu插件，可以尽可能模拟IMU噪声模型
<gazebo>
<plugin filename="librotors_gazebo_imu_plugin.so" name="rotors_gazebo_imu${imu_suffix}_plugin">
<robotNamespace>${namespace}</robotNamespace> <!-- (string, required): ros namespace in which the messages are published -->
<linkName>${namespace}/imu${imu_suffix}_link</linkName> <!-- (string, required): name of the body which holds the IMU sensor -->
<imuTopic>${imu_topic}</imuTopic> <!-- (string): name of the sensor output topic and prefix of service names (defaults to imu) -->
<gyroscopeNoiseDensity>${gyroscope_noise_density}</gyroscopeNoiseDensity> <!-- Gyroscope noise density (two-sided spectrum) [rad/s/sqrt(Hz)] -->
<gyroscopeRandomWalk>${gyroscope_random_walk}</gyroscopeRandomWalk> <!-- Gyroscope bias random walk [rad/s/s/sqrt(Hz)] -->
<gyroscopeBiasCorrelationTime>${gyroscope_bias_correlation_time}</gyroscopeBiasCorrelationTime> <!-- Gyroscope bias correlation time constant [s] -->
<gyroscopeTurnOnBiasSigma>${gyroscope_turn_on_bias_sigma}</gyroscopeTurnOnBiasSigma> <!-- Gyroscope turn on bias standard deviation [rad/s] -->
<accelerometerNoiseDensity>${accelerometer_noise_density}</accelerometerNoiseDensity> <!-- Accelerometer noise density (two-sided spectrum) [m/s^2/sqrt(Hz)] -->
<accelerometerRandomWalk>${accelerometer_random_walk}</accelerometerRandomWalk> <!-- Accelerometer bias random walk. [m/s^2/s/sqrt(Hz)] -->
<accelerometerBiasCorrelationTime>${accelerometer_bias_correlation_time}</accelerometerBiasCorrelationTime> <!-- Accelerometer bias correlation time constant [s] -->
<accelerometerTurnOnBiasSigma>${accelerometer_turn_on_bias_sigma}</accelerometerTurnOnBiasSigma> <!-- Accelerometer turn on bias standard deviation [m/s^2] -->
</plugin>
</gazebo>
其中关键参数包括：
- accelerometer/gyroscopeNoiseDensity 测量噪声
- accelerometer/gyroscope_random_walk 随机游走
其中发布出来的一包数据如下：
header:
seq: 128
stamp:
secs: 22
nsecs: 290000000
frame_id: “imu_base_link”
orientation:
x: 0.0016563453004138674
y: -0.0008542007888005784
z: 0.006514394955332766
w: 1.001055072733733
orientation_covariance: [1.08241e-05, 0.0, 0.0, 0.0, 1.08241e-05, 0.0, 0.0, 0.0, 1.08241e-05]
angular_velocity:
x: 0.0014246123123964744
y: 0.0027524592879204523
z: 0.0007163285834896606
angular_velocity_covariance: [1.08241e-05, 0.0, 0.0, 0.0, 1.08241e-05, 0.0, 0.0, 0.0, 1.08241e-05]
linear_acceleration:
x: -0.0029448312010755674
y: -0.00014593761997295991
z: 9.795575703789861
linear_acceleration_covariance: [1.08241e-05, 0.0, 0.0, 0.0, 1.08241e-05, 0.0, 0.0, 0.0, 1.08241e-05]
自定义的IMU数据结构体如下：
struct IMU {
IMU() = default;
IMU(double t, const Vec3d& gyro, const Vec3d& acce) : timestamp_(t), gyro_(gyro), acce_(acce) {}
double timestamp_ = 0.0;
Vec3d gyro_ = Vec3d::Zero();
Vec3d acce_ = Vec3d::Zero();
};
所以需要在IMU的回调函数里面，赋值上面的结构体数据
sad::IMU imu;
imu.timestamp_ = Imu_msg->header.stamp.toSec();
imu.gyro_ << Imu_msg->angular_velocity.x,Imu_msg->angular_velocity.y,Imu_msg->angular_velocity.z;
imu.acce_ << Imu_msg->linear_acceleration.x,Imu_msg->linear_acceleration.y,Imu_msg->linear_acceleration.z;
然后将数据加入，在该函数内部，即完成航迹的推算
AddIMU(imu);
发布一个里程计数据，来查看航迹推算的结果
nav_msgs::Odometry imu_Integration_Odom;//声明一个里程计 来记录推算的结果
imu_Integration_Odom.header = Imu_msg->header;
imu_Integration_Odom.pose.pose.position.x = p_[0];
imu_Integration_Odom.pose.pose.position.y = p_[1];
imu_Integration_Odom.pose.pose.position.z = p_[2];
imu_Integration_Odom.twist.twist.linear.x = v_[0];
imu_Integration_Odom.twist.twist.linear.y = v_[1];
imu_Integration_Odom.twist.twist.linear.z = v_[2];
将旋转矩阵转成欧拉角单位为度，方便直观查看
float yaw = atan2(R_.matrix()(1,0), R_.matrix()(0,0));
float roll = atan2(R_.matrix()(2,1), R_.matrix()(2,2));
float pitch = atan2(-R_.matrix()(2,0), sqrt(R_.matrix()(0,0) * R_.matrix()(0,0) + R_.matrix()(1,0) * R_.matrix()(1,0)));
imu_Integration_Odom.pose.pose.orientation.x = pitch/0.01745329252;
imu_Integration_Odom.pose.pose.orientation.y = roll/0.01745329252;
imu_Integration_Odom.pose.pose.orientation.z = yaw/0.01745329252;
发布里程计
imu_Integration_Odom_pub_.publish(imu_Integration_Odom);
无人机在地面静止不动情况下
曲线结果数据如下：
- 位置（很快就飘掉了）
- 速度（与位置类似，越来越大）
- 欧拉角度 （航向角度保持还可以）
评论（0）
您还未登录，请登录后发表或查看评论