惯导算法讲解

基于惯性导航的车辆定位与路径调整：滤波算法与坐标计算

原创

于 2024-04-20 20:41:14 发布 · 1.4k 阅读

45 ·

CC 4.0 BY-SA版权

文章标签：

#算法

本文详细描述了一种使用传感器数据（如陀螺仪和速度编码器）进行滤波处理，获取车辆位置和偏航角的方法。文章介绍了如何通过坐标系转换、角度解算以及PID控制策略，实现车辆精确回库的导航过程。

对数据的获取进行了滤波和减去零飘的处理。

```c
void ICM_getValues() {
    // 一阶低通滤波，单位g/s
    icm_data_acc_x = (((float)imu660ra_acc_x) * alpha) + icm_data_acc_x * (1 - alpha);
    icm_data_acc_y = (((float)imu660ra_acc_y) * alpha) + icm_data_acc_y * (1 - alpha);
    icm_data_acc_z = (((float)imu660ra_acc_z) * alpha) + icm_data_acc_z * (1 - alpha);
    // 陀螺仪角速度转弧度
    icm_data_gyro_x = ((float)imu660ra_gyro_x - GyroOffset_Xdata) * M_PI / 180 / 16.4f;
    icm_data_gyro_y = ((float)imu660ra_gyro_y - GyroOffset_Ydata) * M_PI / 180 / 16.4f;
    icm_data_gyro_z = ((float)imu660ra_gyro_z - GyroOffset_Zdata) * M_PI / 180 / 16.4f;
}
```

`GyroOffset`的获取很简单，取车辆静止不动的一段时间内的数据平均值即可。

角度解算的数据获取中断周期为2MS。

速度编码器的数据获取中断周期为20MS。

通过四元数最后的到了车辆的偏航角。

# 2.车辆自身坐标获取

我们假设有两个按键，一个按键决定了车辆当前坐标系，一个按键决定了发车与否。

那我们将车放在车库中心，按下第一个按键，调用以下函数

```c
void Inertial_Navigation_Start(void) {
    //设置当前坐标,抓取当前坐标系为基准坐标系
    navigation.start_yaw = eulerAngle_yaw_total;
    navigation.x_cur = 0;
    navigation.y_cu

最低0.47元/天解锁文章