第十七届智能车竞赛–极速越野组总结报告
任务简析
本次极速越野组要求小车沿着操场赛道运行一周。
根据赛题提供的可用传感器列表，可大致锁定使用 GPS 导航或采用摄像头巡线跑法。经过简单的验证后，选择采用 GPS 作为小车的运动导航。因为摄像头在强光环境下的巡线效果十分不明显，而 GPS 则受影响较小，虽然路线误差大于使用摄像头巡线，但是稳定可靠。
所用模块及功能
本次小车所用模块如下：
- 双频 GPS 卫星定位模块：用于获取经纬度等信息
- Lora 无线串口：用于小车紧急制动
- MPU6050 六轴陀螺仪：用于检测小车姿态信息
- LCD 显示屏：显示各种信息
- SD5 模拟舵机：用于转向控制
- 有感无刷电机：提供动力
整体框架和思路
整个小车程序设计的主要框架为多传感器数据联合导航。
先将导航点信息录入单片机的 Flash ，之后在运行过程中根据小车自身位置信息逐点寻迹，也就是通过算法计算出自身位置与导航点之间的方位角信息和距离信息，得到这些信息后由 CPU 进行判断，若与导航点之间的距离小于一定值则表示这个点已到达，再切换到下一个点，直至完成一圈的导航。
以上是最初的大体方案，也是整个小车控制代码的主体框架，再此之上还加入了丢点算法，后期也移植了 RT-Thread 操作系统。以下是最后参赛的代码框架流程：
整个小车是基于 RT-Thread 操作系统的，所以整个任务是由六个线程完成。
- 初始化线程：优先级最高，在此线程中完成各模块和传感器的初始化，初始化完后会将 Flash 中的导航点信息导入数组，之后等待GPS模块传回有效信息，定位成功后再初始化舵机和电机并释放信号量，使其余线程开始运行，执行完操作后，该线程会被销毁。
- 角度读取线程：该线程优先级较高，主要负责读取 MPU6050 返回的角度信息，即俯仰角、横滚角和航向角。其中航向角用于小车行驶方向纠正，横滚角和俯仰角负责判断小车姿态，若这两个角大于一定值，则代表小车翻车，应立即刹车。
- 角度控制线程：该线程优先级较低，作用为控制舵机的转向，根据 MPU6050 传回的航向角（yaw）和设置的转向值（Turn_set）做 PID 积分，积分限幅后得到的 PWM 值输入舵机控制，由此来控制小车的转向。
- GPS 解析线程：该线程优先级较高，主要负责解析 GPS 传回的信息，即解析经纬度等。
- 遥控刹车线程：该线程优先级较高，负责接收刹车信号，接收到后立即刹车。
- 小车控制线程：该线程优先级较低，主要负责计算各种导航信息，首先是计算小车自身位置和导航点位置的距离和角度，之后与小车航向角对齐后解算出设置的转向值（Turn_set），还要判断距离信息来进行导航点的切换。其中丢点算法也在其中，当判断到已经错过了该导航点，则会自动切换到下下个导航点，防止小车掉头回到丢掉的导航点。
这六个线程的优先级不同，其中各种信息的接收线程优先级较高，即角度读取、GPS 解析、遥控刹车这三个线程的优先级较高，主要原因是为了不错漏任何一条重要信息，确保信息的及时和小车的安全。角度控制和小车整体控制这两个线程的优先级较低，主要是为了有导航信息返回时能及时中断这两个控制线程，及时更新信息来确保导航的实时性。
部分源码分析
源码 GitHub 仓库：https://github.com/hg0720/Smart_car.git
小车控制线程入口函数：
void Car_thread_entry(void *parameter)
{
static rt_err_t result;
static int count=0;
float distance;
float azimuth_N;
float last_turn=0;
result = rt_sem_take(Car_sem, RT_WAITING_FOREVER);
while(result == RT_EOK)
{
distance = get_two_points_distance(gps_tau1201.latitude, gps_tau1201.longitude, test_latitude[count], test_longitude[count]);
azimuth_N = get_two_points_azimuth(gps_tau1201.latitude, gps_tau1201.longitude,test_latitude[count], test_longitude[count]);
if ((distance <= 2))
{
count++;
}
else if(((Turn_set - last_turn >= 35) || (Turn_set - last_turn <= -35)) && (distance <= 10))
{
Turn_set = last_turn;
count++;
}
else
{
last_turn = Turn_set;
Turn_set = F_Angle - azimuth_N;
}
if((count == Turn_Point1) || (count == Turn_Point2))
{
Servo_Turn_PID_Init();
Motor_Control(Turn_Speed);
}
if((count == Stright_Point1) || (count == Stright_Point2))
{
Servo__Stright_PID_Init();
Motor_Control(Stright_Speed);
}
if(count == Point_Num)
{
Motor_Control(0);
pwm_disable(Servo_PWM_TIM);
}
lcd_showint8(45, 7, count);
lcd_showfloat(45, 8, distance, 3, 3);
lcd_showfloat(45, 9, Turn_set, 3, 3);
systick_delay_ms(10);
}
}
PID 代码：
float PID_Loc(float SetValue, float ActualValue, PID_TypeDef *PID)
{
float err;
float PWM;
err = SetValue - ActualValue;
if(err <= -300)
{
err = 360 + err;
}
if(err >= 300)
{
err = err - 360;
}
PID->Ek = err;
PID->LocSum += PID->Ek;
PWM = PID->Kp * PID->Ek + (PID->Ki * PID->LocSum) + PID->Kd
* (PID->Ek1 - PID->Ek);
PID->Ek1 = PID->Ek;
return PWM;
}
踩坑指南
导航方案的选择
在选用 GPS 做导航时，还有一个非常重要的选择，就是小车自身姿态信息的获取。
这其中有两种途径来进行这个信息的获取，因为只需要进行角度的修正，所以可以用 GPS 传回来的方位角来做小车行驶方向；另一种就是利用 MPU6050 进行角速度积分来获取小车的航向角。
一开始选择的是利用 GPS 传回来的方位角，因为 MPU6050 的姿态解算过于复杂，且精度较低，噪声干扰太大。后来写出第一版程序后到操场实测，发现小车行驶轨迹诡异，而且反应很慢，之后研究发现，GPS 传回的方位角是由小车行驶的速度和经纬度信息的变化通过公式计算得出，这里面就存在一些问题，就是在小车具有一定速度之前，这个方位角晃动很大，不具有使用价值，再者便是 GPS 回传信息很慢，在这个过程中小车容易偏航。
最后还是采用了 GPS 加 MPU6050 来进行导航的方案，至于 MPU6050 角度的获取和滤波解算等，都可移植官方的 dmp 库来使用。
坐标系的对齐问题
一开始第一次下地实测的时候，发现小车总是漫无目的的乱跑，未按照导航点的轨迹行驶，后来通过解析导航点之间的角度发现，通过经纬度计算出来的角度是 WGS84 坐标系，而 MPU6050 是以车头方向为 0 度设置的载体坐标系，两个角度均不能直接使用，需要进行计算和转换。
后来翻阅了大量资料，大概了解了通过旋转矩阵来转换坐标系的方法：https://blog.csdn.net/m0_46430715/article/details/123298911?spm=1001.2014.3001.5501 ；之后经过实地操作，发现算法难度太大，于是简单粗暴的直接将 MPU6050 航向角加上小车行驶方向的方位角来对齐解算出来的方向角，发现效果还行，小车能正常行驶，但由于一些干扰会导致 GPS 信号，导致计算出的初始角度可能会有偏差，所以小车有些许摇摆或跑偏。
最后确定的方案为：小车向正北方向发车（标准操场直道均为南北朝向），这时小车自身与第一个导航点之间的方位角就为 0 度，直接与 MPU6050 的初始角度相同，以此完成坐标对齐。
MPU6050 角度问题
后续调试过程中发现，当小车行驶完半圈，在进入第二条直道时，总是会莫名开始转圈，一开始以为是丢掉了导航点，后来发现是 MPU6050 转到 180 度时，其航向角的差值过大，往左右轻微摇摆就能达到 360 度左右的差值，PID 计算出的 PWM 值传入舵机后会持续往一个方向打死，由此导致小车不停转圈。
最后将偏航角的范围改为 0-360 度，并在 PID 积分器中对较大的差值做了处理（参见上述 PID 代码），完美解决。
PID 调试问题
在 PID 参数调试过程中也是充满坎坷，在直道上跑的很稳的参数拿到弯道跑的一塌糊涂，同理，适合弯道的参数也不适合跑直道（因为刚开始是弯道和直道分开调试，主要原因是操场人太多，只能测一小段），在漫长的调试过后反而搞出了一套不伦不类的参数，效果仅仅是勉强跑完全程，而且速度很慢。
最后，调试了两套 PID 参数，一套用于直道行驶，一套用于弯道行驶，并给弯道和直道都设置了单独的速度，根据进入弯道或进入直道的导航点来进行切换，也算勉强解决了问题。
MPU6050 采样频率问题
一开始使用 MPU6050 时，都是使用的默认参数，当时的采样频率是 100Hz ，在低速时并未发现问题，后来提速过程中发现，只要转弯太快或是小车行驶过程中发生漂移，就会出现剧烈的摆尾或是大幅度震荡，直至冲出赛道。
最初出现这个问题依旧是怀疑 PID 的参数不对，经过一段时间的调参后还是会出现这个问题，于是将注意力转移到了 MPU6050 的身上，在不停改配置测试之后，终于发现，在采样值过高时，转弯过快会出现角度丢失的问题，低速时不是很明显，在转过 180 度后只会丢掉几度，但一旦速度过快，这个丢失的范围就越来越大，最严重的一次甚至丢失了 50 度！
这些丢失的角度就导致小车的行驶方向航向角错误，随着 PID 的误差积分，就会出现甩尾或是冲突赛道等现象。
最后将采样率下调到了 10Hz ，与 GPS 的采样率保持一致，小车终于能平稳运行。
小车遥控失控问题
在调试过程中常常遇到小车翻车或是撞墙的问题，于是便加入了遥控，用来控制小车紧急刹车。但是在使用过程中，发现遥控有很大概率失灵，且失灵时小车还有很大概率失控（为此付出惨痛代价）。
在一系列排查之后终于发现了问题，因为小车一开始的遥控停车是直接放在中断中进行，在接收到停车信号后会立即关闭电机和舵机的 PWM 输出，这一点从逻辑上并没有什么问题，但是小车上是移植了 RT-Thread 操作系统的，在操作系统中，中断是高于任何线程的，所以在中断中不能有过多操作，一般在中断中仅进行一些信号量的释放或是保存信息，直接在中断中执行过多操作可能会引起整个系统的混乱。
后来，仅在中断中进行了标志位置一的操作，并给停车单独创建了线程，当标志位为一就立即刹车，其余则不做处理，之后也解决了这个问题。
Flash 保存和读取经纬度
详见：https://blog.csdn.net/m0_46430715/article/details/123782816?spm=1001.2014.3001.5501
小车进弯侧翻
初步怀疑是小车自重不够且速度过快导致，可以适当增重或是减小转弯角度。
代码缺陷
由于当时是第一次使用 RT-Thread 操作系统，所以代码当中还保留了很多逻辑编程的习惯，这给整个系统带来了一些不稳定的安全隐患和隐形 BUG ：
- 在程序中使用了大量全局变量，且在写入和读取时未对资源做出保护，在使用系统时应当避免使用大量全局变量，就算使用了，也需要进行互斥来保护这些共享资源。
- 对整个系统线程的优先级和时间片划分还有待商榷，对信号量、互斥量等 IPC 机制的使用混乱。
总结
本次大赛取得了区二等奖的成绩，没能冲进国赛还是很遗憾的，主要还是水平不够吧，但是在做车的过程中学会了很多，也对嵌入式操作系统有了一定的概念，写下这篇文章算是对本次大赛的一次总结反思吧，进国赛的愿望就托付给各位学弟学妹了，大家共勉！
|