惯导算法讲解

最新推荐文章于 2026-04-03 09:20:24 发布

原创

最新推荐文章于 2026-04-03 09:20:24 发布 · 1.6k 阅读

标签

#算法

收录于

本文详细描述了一种使用传感器数据（如陀螺仪和速度编码器）进行滤波处理，获取车辆位置和偏航角的方法。文章介绍了如何通过坐标系转换、角度解算以及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