惯导算法讲解

Dream_begin_
于 2024-04-20 20:41:14 发布
阅读量483
点赞数 3
文章标签：算法
本文链接：https://blog.csdn.net/dream_begin_/article/details/137246607
版权
对数据的获取进行了滤波和减去零飘的处理。

```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_cur = 0;
    navigation.x_start = 0 * 1; //mm
    navigation.y_start = 0 * 1; //mm
    navigation.x_set = 0 * 1;   //mm
    navigation.y_set = 0 * 1;   //mm
}
```
即可获得一个参考二维坐标系，这个坐标系以车库为中心，出车库右侧为X轴正方向，车辆前方为Y轴正方向。我们在中断中进行以下计算，周期为2MS。
```c
navigation.cur_yaw = eulerAngle_yaw_total - navigation.start_yaw;
navigation.x_cur = navigation.x_cur + sin(navigation.cur_yaw / 180 * 3.14159f) * (encoder_bdc / 150 * 50 * 0.002);
navigation.y_cur = navigation.y_cur + cos(navigation.cur_yaw / 180 * 3.14159f) * (encoder_bdc / 150 * 50 * 0.002);
```

我们车轮子行进一米大概会让编码器产生150000个脉冲，以20ms为编码器速度获取周期进行换算，得到的`x_cur`,`y_cur`即为车辆的坐标。

`eulerAngle_yaw_total`为yaw轴偏航累计角度，因为直接读取姿态解算的yaw值的话，它会有-180~180的限制，且在边界会出现跳变，故采用以下代码进行`eulerAngle_yaw_total`值的获取。

```c
float i = eulerAngle_yaw - eulerAngle_yaw_old;
if (i < -180) {
    i += 360;
}
else if (i > 180) {
    i -= 360;
}
eulerAngle_yaw_total += i;
eulerAngle_yaw_old = eulerAngle_yaw;
```

# 3.车库（目标点）方向计算

这部分思路很简单，既然得到了车辆自身的坐标，那么只需要进行atan计算即可获取车辆所需的偏航角。但是需要注意的是atan的返回值在-45~45°之间，需要我们针对车辆坐标系进行判断后才能知道准确的-180~180之间的偏航角。

```c
if (navigation.y_set - navigation.y_cur < 0 && navigation.x_set - navigation.x_cur > 0) {
    navigation.inertial_navigation_set_yaw_update = atan((navigation.x_set - navigation.x_cur) / (navigation.y_set - navigation.y_cur)) / 3.14159f * 180 + 180;
}
else if (navigation.y_set - navigation.y_cur < 0 && navigation.x_set - navigation.x_cur < 0) {
    navigation.inertial_navigation_set_yaw_update = atan((navigation.x_set - navigation.x_cur) / (navigation.y_set - navigation.y_cur)) / 3.14159f * 180 - 180;
}
else {
    navigation.inertial_navigation_set_yaw_update = atan((navigation.x_set - navigation.x_cur) / (navigation.y_set - navigation.y_cur)) / 3.14159f * 180;
}
```

获取到```navigation.inertial_navigation_set_yaw_update```后进行类似偏航累计的处理

```c
float i = navigation.inertial_navigation_set_yaw_update - navigation.inertial_navigation_set_yaw_old;
while (i > 180) {
    i = i - 360;
    if (i < 180) {
        break;
    }
}
while (i < -180) {
    i = i + 360;
    if (i > -180) {
        break;
    }
}
```

至此我们得到了目标偏航角的累加。我们再取一次余。

```c
if (navigation.inertial_navigation_set_yaw_up_total - navigation.cur_yaw > 360) {
    while (1) {
        navigation.inertial_navigation_set_yaw_up_total -= 360;
        if (navigation.inertial_navigation_set_yaw_up_total - navigation.cur_yaw < 360) {
            break;
        }
    }
}
else if (navigation.inertial_navigation_set_yaw_up_total - navigation.cur_yaw < -360) {
    while (1) {
        navigation.inertial_navigation_set_yaw_up_total += 360;
        if (navigation.inertial_navigation_set_yaw_up_total - navigation.cur_yaw > -360) {
            break;
        }
    }
}
```

然后我们车辆在惯导回车库的时候，实际上不需要车头朝向车库，旋转最近的旋转角度，使车头或者车尾朝向车库即可。

```c
if (navigation.inertial_navigation_set_yaw_up_total - navigation.cur_yaw < 90 && navigation.inertial_navigation_set_yaw_up_total - navigation.cur_yaw > -90) {
    speed_reversal_flag = 0;
}
if (navigation.inertial_navigation_set_yaw_up_total - navigation.cur_yaw > 90) {
    while (1) {
        //
        navigation.inertial_navigation_set_yaw_up_total -= 180;
        if (navigation.inertial_navigation_set_yaw_up_total - navigation.cur_yaw < 90) {
            speed_reversal_flag = 1;
            break;
        }
    }
}
if (navigation.inertial_navigation_set_yaw_up_total - navigation.cur_yaw < -90) {
    while (1) {
        navigation.inertial_navigation_set_yaw_up_total += 180;
        if (navigation.inertial_navigation_set_yaw_up_total - navigation.cur_yaw > -90) {
            speed_reversal_flag = 1;
            break;
        }
    }
}
```

在speed_reversal_flag == 1时，我们在行进轮速度控制的时候使车辆倒车即可。

# 4.回库流程

在出断路后，我们将pid计算中的角度环的偏差替换为`navigation.cur_yaw - navigation.inertial_navigation_set_yaw`,其中`navigation.inertial_navigation_set_yaw`为经过了斜坡函数获得的值。

```c
if (navigation.inertial_navigation_set_yaw < navigation.inertial_navigation_set_yaw_up_total) {
    navigation.inertial_navigation_set_yaw += 0.04f;
}
else if (navigation.inertial_navigation_set_yaw > navigation.inertial_navigation_set_yaw_up_total) {
    navigation.inertial_navigation_set_yaw -= 0.04f;
}
```

我们车辆基础性能有限，只能进行了这样的处理，希望大家可以多多交流控制方面的思路，互相进步。



在回库过程中，我们只需要设置目标值`navigation.x_set`与`navigation.y_set`即可让车回到设定点位。  

我们组先将车开回（0，0），再设置到（0，x），x值为一个常量，为计时器的触发地点，随后再将目标值设定为（0，0），车辆到达后，进行关闭输出处理。

需要注意的是，因为无法精确地到达目标点，我们可以在到达目标点半径一定范围后就将状态机切换为下一个状态。
更多参考paper佬的开源https://github.com/paper-tei/smart_car