[转]十三届智能汽车竞赛信标对抗组思路参考程序参考（三）寻灯策略Part.①()

84 阅读 0 评论 56 点赞

我是靠谱客的博主甜甜大船，最近开发中收集的这篇文章主要介绍[转]十三届智能汽车竞赛信标对抗组思路参考程序参考（三）寻灯策略Part.①()，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

系列链接：

十三届智能汽车竞赛信标对抗组思路参考程序参考（一）模块初始化
十三届智能汽车竞赛信标对抗组思路参考程序参考（二）图像处理
十三届智能汽车竞赛信标对抗组思路参考程序参考（三）寻灯策略Part.①
十三届智能汽车竞赛信标对抗组思路参考程序参考（四）寻灯策略Part.②
十三届智能汽车竞赛信标对抗组思路参考程序参考（五）分段控速
十三届智能汽车竞赛信标对抗组思路参考程序参考（六）避障

正文开始：
上一篇的图像处理部分已经求出了信标灯在图像中的像素坐标，基于此，这一次分享一下我们的寻灯策略，最终是实现小车灭灯的最优路径。一样的，希望大佬轻喷。。。

先从简单开始，想要实现小车向信标灯跑过去较简单，只需要用信标灯的横坐标与图像画面中轴线的横坐标做差，得到小车车头朝向与信标灯方向的偏差，用此偏差放入PD控制器来控制舵机打角即可：

X_track = 40;                                                     //图像尺寸为80×60，图像画面中轴线的横坐标即为40

errorlight = x_MID - X_track ;                                    //x_MID为当前图像中的信标灯横坐标

NewPWMKP = (int)(errorlight * PWM_P);                             //P分量
NewPWMKD = (int)((errorlight - errorlightlast) * PWM_I);          //D分量

NewPWMValue = (int)(NewPWMKP + NewPWMKD);                         //PD输出

errorlightlast = errorlight;

这样的效果就是小车会直冲冲地向信标灯驶去，直至撞上信标灯，频繁的撞击肯定会拖慢灭灯任务的时间。因此我们需要规划一个路径，能让小车成功灭灯的同时顺滑地从侧面避开信标灯。我们假设图像平面中横向从左至右为x轴正方向，纵向从上至下为y轴正方向，坐标系原点在左上角，小车接近信标灯时y会逐渐变大，即y反映距离，x则反映的是小车相对信标灯的朝向。上面提到的简单寻灯方法中设置的做差对象都是图像画面中轴线，即为x=40，为一条直线，x不随y变化，即小车的目标方向不随小车与信标灯之间的距离而改变。如果我们想让小车在接近信标灯时向一侧避开，那可以设置一个y的阈值，比如y=45，当小于这个阈值时，做差对象是x=40，当大于等于这个阈值时，做差对象设为另一条直线，比如x=20，此时只需要调好一个比较合适的y阈值，即可实现接近信标灯之后侧方避开信标灯。而且只需要调节y阈值和两个做差对象，共三个参数，调参量不大，好调。

if(y_MID<45) X_track = 40;							   //做差对象x=40
else X_track = 20;                                     //做差对象x=20

errorlight = x_MID - X_track;                                     //x_MID为当前图像中的信标灯横坐标

NewPWMKP = (int)(errorlight * PWM_P);                             //P分量
NewPWMKD = (int)((errorlight - errorlightlast) * PWM_I);          //D分量

NewPWMValue = (int)(NewPWMKP + NewPWMKD);                         //PD输出

errorlightlast = errorlight;

然而这样的处理方式缺点很明显，就是由于做差对象在y阈值附近有很大的突变，经过实验就会发现小车寻灯并避开的过程中有一个很明显的切换动作，并不顺滑，路线也不好。针对这个问题，我们可以设置多个y阈值，将做差对象更加细分。比如，设置四个y阈值，即把y轴划分为五个区间，五个区间的做差对象分别设为五条不同的直线，比如x=40,30,20,10,0。通过实验来调节四个较好的y阈值和五条做差直线，就已经能够实现顿挫感不那么明显的寻灯避灯动作了。需要调节四个y阈值和五个做差对象，共九个参数，调参量增加了，稍微不那么好调了。

if(y_MID<30) X_track = 40;							       //做差对象x=40
else if(y_MID<35) X_track = 30;							   //做差对象x=30
else if(y_MID<45) X_track = 20;							   //做差对象x=20
else if(y_MID<50) X_track = 10;							   //做差对象x=10
else X_track = 0;                                          //做差对象x=0

errorlight = x_MID - X_track;                                     //x_MID为当前图像中的信标灯横坐标

NewPWMKP = (int)(errorlight * PWM_P);                             //P分量
NewPWMKD = (int)((errorlight - errorlightlast) * PWM_I);          //D分量

NewPWMValue = (int)(NewPWMKP + NewPWMKD);                         //PD输出

errorlightlast = errorlight;

想要达到更优的路径和动作，还可以继续细分做差对象，当设置的y阈值数量等于图像的行数（即每一行的做差对象都不同）时，理论上能够得到非常流畅顺滑的运行动作。如果图像有60行的话，那这样岂不是需要调节60个做差对象，共60个参数，这还能实现吗？对于这个问题，换个角度想，我们每一行设置的做差对象值不就是我们想让小车在与信标灯不同距离时所偏转的程度吗，因此可以使用一个很方便的方法来设置这60个值：做实验，从远处慢慢地手推小车接近信标灯，推出想让小车运行的轨迹，此过程中由远及近地记录下图像中每一行信标灯的（x，y）坐标，这60个坐标的x值正是要设置的60个做差对象。实验会发现，这60个做差对象并不是60个两两不等的值，尤其是距离信标灯比较远的时候，也就是y值较小的时候，x值的变化是非常细微的，当靠近信标灯后，x值的变化才会变得比较剧烈。当把这60个参数实装后，就会惊喜地发现，小车自动跑的路径正好就是刚刚自己手推的路径，因此手推记录时做的越准确，最终跑出的效果越好。

float track_tab[60][2]={{0，40},
                        {1，40},
                        {2，39.8},
                           ...
                        {57，5.2},
                        {58，1.8},
                        {59，0}};                                 //第一列为行数0-59，第二列为每一行对应的做差对象

for(i=0; i<60; i++)                                               //取出对应做差对象
{
  if((int)y_MID=track_tab[i][0]) X_track = track_tab[i][1];     
}

errorlight = x_MID - X_track;                                     //x_MID为当前图像中的信标灯横坐标

NewPWMKP = (int)(errorlight * PWM_P);                             //P分量
NewPWMKD = (int)((errorlight - errorlightlast) * PWM_I);          //D分量

NewPWMValue = (int)(NewPWMKP + NewPWMKD);                         //PD输出

errorlightlast = errorlight;

上面这个方式相当于是一个查表的过程，在60个值中查到对应的做差对象。能够查表的前提是信标灯在图像中的y坐标必须是0~59的整数，如果图像处理中求得的信标灯y坐标包含小数的话只能转换为整形，这个取整的过程也就导致了精度损失。数量有限的查表方式想通过继续细分做差对象来提高精度显然不太现实，因此我们用到曲线拟合。将之前我们记录下来的60组（x，y）坐标使用Matlab拟合工具箱来拟合出一个连续的函数曲线，可以根据自己的需要选择高斯、傅里叶、幂次函数等等，我们最终选择了拟合一个二次函数曲线，效果足够优秀了。拟合出来大概长这样，依旧是80×60尺寸的图像平面坐标系。

函数曲线大概长这样

注意，y坐标是自变量，x坐标才是因变量，此时我们一样可以输入一个y值得到对应的做差对象x值，并且不需要取整，精度得到了保障。

X_track = (-0.012)*pow(y_MID,2) + 0.054*y_MID + 40;               //拟合出的函数曲线

errorlight = x_MID - X_track;                                     //x_MID为当前图像中的信标灯横坐标

NewPWMKP = (int)(errorlight * PWM_P);                             //P分量
NewPWMKD = (int)((errorlight - errorlightlast) * PWM_I);          //D分量

NewPWMValue = (int)(NewPWMKP + NewPWMKD);                         //PD输出

errorlightlast = errorlight;

此时的寻灯避灯过程就是非常顺滑且优质的了。然而实验中发现，虽然在一定速度下寻灯避灯过程很好，但一旦把速度提的比较快，这个寻灯避灯过程就会发生变化甚至失败，出现这个问题并不是因为之前记录的值不好，也不是函数拟合的有问题，而是因为单片机的处理时间限制：单片机的运行过程是离散的，处理两幅图像之间有时间差，尽管处理速度很快、时间差很短，但也会给快速运动的小车带来信标灯识别的滞后，举个夸张的例子，首先无论车速如何调节，单片机处理一个循环的时间几乎都是一致的，车速慢时单片机处理完一个循环小车只前进了10cm，而车速快时一个循环内小车就已经前进了50cm，此时处理好的这幅图像可能已经是50cm之前采集的图像了。
知道了原因，就可以针对它进行调整，前面提过，图像中信标灯的y坐标反映的是小车与信标灯之间的距离关系，要解决识别滞后问题，也就是解决此时处理的图像不是当前位置的图像的问题，那么可以把刚刚拟合出来的函数曲线进行y方向的上下平移，也就是把此时采集的图像认为是图像处理完后的小车位置采集的图像，也就是把与距离挂钩的信标灯y坐标当做更近时候的情况。（不知道这个部分我有没有表达清楚，有疑问的话可以评论提问）
那应该要把函数曲线在y方向上下平移多远呢？这就需要与当前的车速挂钩，我们测量的其实是后轮的转速，与小车的实际车速有些许偏差，但是问题不大，因此上下平移的距离就与后轮编码器的采集值挂钩了。即编码器值v为输入量，上下平移距离l为输出量，又可以构建一个函数关系。思路有了，可是这个函数关系似乎并不好找了，它不如刚才前面的做差对象可以一点点地取值，我们当时做的想到的办法就是实验：尝试不同的车速和不同的平移距离，速度快时把曲线向上（y负方向）平移，速度慢时向下（y正方向）平移，精心观察调节的效果，记录下几组编码器值-平移距离的组合，用记录下来的组合再进行Matlab拟合。
我们观察到编码器值-平移距离的关系只需要简单的一次函数就可以代表了，实装实验，效果非同凡响。

suduyiY=2.385-Speed_test*0.37;                                    //上下移动量
X_track = (-0.012)*pow((y_MID-suduyiY),2) + 0.054*(y_MID-suduyiY) + 40;    //加入考虑移动量的函数曲线

errorlight = x_MID - X_track;                                     //x_MID为当前图像中的信标灯横坐标

NewPWMKP = (int)(errorlight * PWM_P);                             //P分量
NewPWMKD = (int)((errorlight - errorlightlast) * PWM_I);          //D分量

NewPWMValue = (int)(NewPWMKP + NewPWMKD);                         //PD输出

errorlightlast = errorlight;

寻灯策略这块的内容比较多，我打算分几部分来分享，那就下次再继续更新啦。。。

---------------------
作者：ZZM丶
来源：CSDN
原文：https://blog.csdn.net/qq_41833526/article/details/109495021