目录
- 简介
- 本文以第十六届双车组（直立）软件调试为主，第十七届部分主要讲通讯与控距
- 一、第十六届双车组：
- 1.机械结构搭建（以直立为例）
- 1.1 车模布局
- 1.2陀螺仪安装
- 2.软件控制部分
- 2.1姿态&速度控制
- 2.1.1数据处理
- 2.1.2姿态&速度串级PID
- 2.2转向控制
- 3.图像处理
- 3.1 获取图像数据
- 3.2 大津法与二值化
- 3.3 元素处理
- 4.补充部分
- 1、图像部分
- 1.1 大津法优化
- 1.2 图像处理
- 2、控制算法
- 2.1 姿态与转向的双串级PID
- 2.2 自适应P参数
- 3、UI菜单设计
- 二、十七届多车编队——协同编队方案
- 【完整工程代码】
简介
全国大学生智能车竞赛分为很多组别，大体的任务都是在组委会规定的车模基础上，自己设计电路板等硬件，通过编写程序来，再根据摄像头、陀螺仪、电感等传感器实时获取的信息，进行数据运算、图像处理、滤波等等操作后，输出一定占空比的PWM信号，控制电机的转速或者舵机的打角等操作，然后驱动小车按照规定的赛道行驶，并尽可能在最短时间内完成相应任务。
在第十六届的时候，我们队伍选择的组别是双车接力组，具体任务是需要做出两辆不同的车，一辆两轮的直立平衡车，另一辆是有一个从动轮的三轮车。需要实现接力任务。 我负责的主要是直立平衡车的软件任务，包括数据处理、控制算法和图像处理算法，然后也负责了双车的传接方案、参与了机械结构的搭建。
十七届我们选的是多车编队组，我除了负责控制和图像，还和队友设计了编队和通讯的方案。
本文以第十六届双车组（直立）软件调试为主，第十七届部分主要讲通讯与控距
数据处理：互补滤波、低通滤波、速度平滑滤波、串口数据处理、超声波红外传感器处理等等。
控制算法：传统PID、自适应P参数、双车组-在姿态控制和转向控制都使用了性能更好的串级PID，多车组-距离控制使用单P算法
图像算法：对大津法进行优化、二值化、对图像进行边界搜寻、并对赛道元素进行处理
传接方案：使用分体式超声波实现接力时的精准控距、设计传接装置。涉及的专业知识：
单片机嵌入式开发应用——编程、中断、定时器、串口通讯
C语言——函数、数组、指针等
传感器原理与应用——编码器、摄像头、电感、超声波
自控原理——PID、串级PID
计算机控制原理——互补滤波、低通滤波、数字离散控制
电力电子技术——PWM波，驱动控制
MATLAB——数据仿真、函数拟合
数电模电——电路搭建
一、第十六届双车组：
1.机械结构搭建（以直立为例）
1.1 车模布局
在赛题规定的电机和底盘基础上，为了使得车模重心尽可能低，我们选择底盘朝向相反的方向为行进的方向，这样使得各种路段都能更好的维持稳定性。
1.2陀螺仪安装
首先，在确定车模的大致机械结构后，车模x,y,z三轴的位置就已基本确定，安装陀螺仪（我们使用的是icm20602模块——一个六轴陀螺仪加速度模块，包含加速度计和陀螺仪，它能够获得车模在空间直角坐标系内关于车模自身x,y,z三轴的角速度，以及其加速度在x,y,z三周上的分量）时应尽量保证陀螺仪模块的三个坐标轴与车模的三个坐标轴重合，于是放到车轴中心，就可以使陀螺仪采集到的左右或者前后不同方向的数据具有对称性。
其次，为了最大限度地减少陀螺仪随车模振动引起的电信号误差，将陀螺仪安装的位置尽可能地放低。
2.软件控制部分
以直立为例，对车的基本控制主要分成姿态&速度控制、转向控制，三轮车转向部分也可做参考。
2.1姿态&速度控制
2.1.1数据处理
1、获取姿态数据
先搭好车的结构后，找到机械平衡点，使用开源库获取icm20602陀螺仪数据（这里使用SPI总线方式，速度快），这样就可以获得车模在空间直角坐标系内关于车模自身 x,y,z 三轴的角速度，以及其加速度在x,y,z三轴上的分量。
2、角度归一化
加速度计通过求反正切算出的车模倾角只是一个与车模角度一一对应的特征值，为了与车模的实际姿态关联，需要将车模躺倒时与静态平衡点状态下加速度计的数据划分为0~90°的区间。
（1）首先将车模放倒，测出此时加速度计Z轴的数据，记为ACCZ_Horizonal;
（2）然后将车模手扶至静态平衡点测出此时加速度计Z轴的数据，记为ACCZ_vertical,于是我们可以计算得加速度计角度归一化的比例:
ACCZ_ratio = 90/(ACCZ_Horizonal-ACCZ_vertical);
（3）车模在任意角度下都可以读取到相应加速度计的读数，记作icm_acc_z;由此就可以解算出滤波前车模的倾角：
Angle = (icm_acc_z - ACCZ_vertical)*ACCZ_ratio;
3、互补滤波
一般陀螺仪输出的信号往往包含有各种噪声，直接获得的角度值不能直接使用，所以必须要滤波。
虽然理论上来说卡尔曼滤波效果应该更好，但是代码比较麻烦，而且原理不太容易理解。于是在获取直立车模角度时我们选用一阶互补滤波的方式，该方式相较于卡尔曼滤波虽然精度不足，但存在代码简洁，计算速度快，占用内存少等特点。
- 互补滤波器的基本结构如下：
- 假设两个测量信号B(s),A(s)都包含了实际信号θ (s)以及噪声。在信号A(s)中的噪声主要分布在高频，而在信号B(s)中的噪声主要分布在低频。H(s)代表着低通滤波器，那么对应的1−H(s)就是互补的高通滤波器。它们分别提取)A(s),B(s)中的低频和高频成分，然后叠加在一起。其基本原理如图所示：
互补滤波器就是根据传感器特性不同，使信号通过不同的滤波器，然后再整合得到整个频带的信号，在陀螺仪的问题上：
- 加速度计静态稳定性好，但是对振动之类的噪声比较敏感。测倾角时，其动态响应较慢，在高频时信号不好，所以可通过低通抑制高频；
- 陀螺仪动态稳定性好，积分得到的角度不受小车加速度的影响，测倾角响应快，但是随着时间的增加积分漂移和温度漂移带来的误差比较大不过由于零漂等，在低频段信号不好。所以可通过高通滤波可抑制低频噪声。
互补滤波选择了切换的频率点，将两者结合，就将陀螺仪和加速度计的优点融合起来，得到在高频和低频都较好的信号，即高通和低通的频率。
从程序角度来说：互补滤波就是由上一次解出的角度，结合加速度计和陀螺仪数据的置信程度融合出一个新的角度， 主要调节的参数有：加速度计和陀螺仪计的比例，以及采样周期。
陀螺积分角度 += 角速度 * dt；
融合角度 = 陀螺仪权值 * 陀螺积分角度 + (1 - 陀螺仪权值) * 加速度角度实际一阶互补代码部分：
//--------------------------------------------------------------
// @brief 一阶互补滤波
// @param angle_m 加速度计数据
// @param gyro_m 陀螺仪数据
// @return float 数据融合后的角度
//--------------------------------------------------------------
float angle_calc(float angle_m, float gyro_m)
{
float temp_angle;
float error_angle;
static float last_angle;
static uint8 first_angle;
if(!first_angle)//判断是否为第一次运行本函数
{
//如果是第一次运行，则将上次角度值设置为与加速度值一致
first_angle = 1;
last_angle = angle_m;
}
error_angle = (angle_m - last_angle)*ACC_ratio;
//根据偏差与陀螺仪测量得到的角度值计算当前角度值
temp_angle = last_angle + (error_angle + gyro_m)*DT;
//保存当前角度值
last_angle = temp_angle;
return temp_angle;
}滤波的效果（红：滤波后、蓝：滤波前）：
由数据处理得到可用的角度之后，就是最重要的PID控制了。
2.1.2姿态&速度串级PID
在调试过程中我们发现，对于陀螺仪加速度计而言，其加速度计的响应远不如陀螺仪模块敏感。后者能在车模绕轮轴发生轻微扰动时返回较大的数值。在单独使用传统的单环角度环调试车模时，我们发现车模姿态较软，不够稳定，抗干扰能力较差。
为了提高姿态控制对扰动的相应灵敏度，我们决定使用串级PID的控制方案。
- 串级PID:当控制系统中存在多个扰动信号，且控制精度要求较高时，就可以采用串级控制方式，进而显著抑制内回路的扰动影响。
- 原理：两个控制器串联之后，以外环控制器为主导(定值控制系统)，而内环回路可看成是一个随动控制系统，两个控制器协调一致，互相配合，控制品质必然优于简单控制系统。在结构上，串级控制系统比单回路控制系统多了一个副回路，因而对进入副回路的二次波动有很强的抑制能力。总体优点：振荡周期减小，调节更加迅速。
就比如角速度是波动很明显的量，增加角速度环这样一个内环控制器，串级PID就可以很好的抑制内环的角速度值的干扰。像这样对于干扰，内环控制器首先进行“粗调”，外环控制器再进一步“细调”，就可以使得适应性稳定性都很好。
于是，在姿态调整与直线速度控制模块，我们引入三个控制闭环：
速度环->直立角度环->陀螺仪俯仰角速度环->电机前进pwm
具体思路而言，我画了一张流程图：
这张图表示的就是：首先需要设定一个期望速度，速度环的输出值作为直立环的输入，然后直立环的输出作为直立内环（即角速度环）的输入，最后内环输出值输送给电机驱动函数，然后直接控制电机即可。
值得注意的是，由于串级PID中，直立环是主要调节器，速度控制的思路是以角度来换取速度，例如当小车快向前倒下的时候，这个时候为了使小车角度回正，小车需要向前加速；同时这个的时候，直立环会使得车轮转速加快，然后导致速度输入大于速度期望，也就是速度环偏差为正，为了使速度回正，就需要“以退为进”，需要将速度环设计为正反馈。
//-------------------------------------------------------------------------------------------------------------------
// @brief 直立环pd实现函数
// @param expect 期望角度
// @param input 实际角度
// @return int32 直立环输出值（期望角速度）
// @author 2021山威海韵二队
//-------------------------------------------------------------------------------------------------------------------
int32 uppid_realize(int32 speed,int32 input)
{
up_pid_struct.Kp = Kp_up;
up_pid_struct.Kd = Kd_up;
up_pid_struct.SetAngle = 0;
up_pid_struct.ActualAngle = input;
up_pid_struct.err = up_pid_struct.ActualAngle - up_pid_struct.SetAngle;
up_pid_struct.duty = (int32)(up_pid_struct.Kp*(up_pid_struct.err + speed) + up_pid_struct.Kd*(up_pid_struct.err-up_pid_struct.err_last));
up_pid_struct.err_last = up_pid_struct.err;
if(up_pid_struct.duty > up_pwm_limit)
up_pid_struct.duty = up_pwm_limit;
else if(up_pid_struct.duty < -up_pwm_limit)
up_pid_struct.duty = -up_pwm_limit;
return up_pid_struct.duty;
}
//-------------------------------------------------------------------------------------------------------------------
// @brief 俯仰角角速度环pi实现函数
// @param expect 期望角速度
// @param input 实际角速度
// @return int32 给电机
// @author 2021山威海韵二队
//-------------------------------------------------------------------------------------------------------------------
int32 Av_pid_realize(int32 uppid,int32 input)
{
Av_pid_struct.Kp = Kp_Av;
Av_pid_struct.Ki = Ki_Av;
Av_pid_struct.SetAngle_v = uppid;
Av_pid_struct.ActualAngle_v = (int32)input;
Av_pid_struct.err = Av_pid_struct.SetAngle_v - Av_pid_struct.ActualAngle_v;
Av_pid_struct.integral += (Av_pid_struct.err) * Av_pid_struct.Ki;
if(Av_pid_struct.integral > 6000)//积分限幅
Av_pid_struct.integral = 6000;
else if(Av_pid_struct.integral < -6000)
Av_pid_struct.integral = -6000;
Av_pid_struct.duty = (int32)(Av_pid_struct.Kp*(Av_pid_struct.err) + Av_pid_struct.integral);
Av_pid_struct.err_last = Av_pid_struct.err;
if(Av_pid_struct.duty > Av_pwm_limit)
Av_pid_struct.duty = Av_pwm_limit;
else if(Av_pid_struct.duty < -Av_pwm_limit)
Av_pid_struct.duty = -Av_pwm_limit;
return Av_pid_struct.duty;
}
//-------------------------------------------------------------------------------------------------------------------
// @brief 速度pi实现函数
// @param expect 期望速度
// @param input 编码器获取的速度
// @return int32 期望角度
// @author 2021山威海韵二队
//-------------------------------------------------------------------------------------------------------------------
int32 speedpid_realize(int32 expect,int32 input)
{
double integral;
speed_pid_struct.Kp = Kp_speed;
speed_pid_struct.Ki = Ki_speed;
speed_pid_struct.err_last=input-expect; //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）
speed_pid_struct.err *= 0.8; //===一阶低通滤波器
speed_pid_struct.err += speed_pid_struct.err_last*0.2; //===一阶低通滤波器
if((speed_pid_struct.err >= 150) && (input <= 30)) integral = 0;//防止起步时积分饱和
else integral = speed_pid_struct.err * speed_pid_struct.Ki;
speed_pid_struct.integral += integral;
if(speed_pid_struct.integral > 2000)
speed_pid_struct.integral = 2000;
else if(speed_pid_struct.integral < -2000)
speed_pid_struct.integral = -2000;
speed_pid_struct.speedangle = (int32)(speed_pid_struct.Kp*speed_pid_struct.err + speed_pid_struct.integral);
if(speed_pid_struct.speedangle > speed_pwm_limit)
speed_pid_struct.speedangle = speed_pwm_limit;
else if(speed_pid_struct.speedangle < -speed_pwm_limit)
speed_pid_struct.speedangle = -speed_pwm_limit;
return speed_pid_struct.speedangle;
}此外，速度环的输出如果不够平滑，那么会对直立环其造成影响，需要对速度进行平滑输出，可参考这位大佬的博客
直立平衡车PID控制策略以及数据平滑算法：
/**************************************************************************
* 功能说明：直立车速度平滑输出函数
**************************************************************************/
void SpeedControlOutput(void) //
{
float fValue;
fValue = g_fSpeedControlOutNew - g_fSpeedControlOutOld;
duty_Speed = fValue * (cnt_speed + 1)/100 + g_fSpeedControlOutOld;
}2.2转向控制
在调试过程中我们发现，由双轮平衡车的固有机械结构决定直立车模的转向过于敏感，两个电机之间差速稍大一点就会导致超调，车模沿其路径大幅扭动，差速值稍有减小，车模在弯道处就会出现响应不够的情况，完全无法循迹。
倘若不再直接控制电机的差速，转而用电机差速去控制车模在弯道处运行的角速度，利用陀螺仪的灵敏性，①可以使车模在不同路径偏差下迅速找到其需要的转向角速度；②在由弯道切入直道时迅速将角速度期望降低至很低的数值进一步防止出弯抖动。受姿态部分控制思路的启发，我们决定对直立车模的转向同样采用串级控制方案。引入两个控制闭环：
图像中线偏差转向外环->陀螺仪偏航角速度内环->电机差速pwm
原理不再赘述。转向流程如图：
代码如下：
//-------------------------------------------------------------------------------------------------------------------
// @brief 偏航角角速度pi实现函数
// @author 2021山威海韵二队
//-------------------------------------------------------------------------------------------------------------------
int32 Hv_pid_realize(int32 adpid,int32 input)
{
Hv_pid_struct.Kp = Kp_Hv;
Hv_pid_struct.Ki = Ki_Hv;
Hv_pid_struct.SetAngle_v = adpid;//期望的值,可能要改
Hv_pid_struct.ActualAngle_v = (int32)input;//传入的值，当前角速度
Hv_pid_struct.err = Hv_pid_struct.SetAngle_v - Hv_pid_struct.ActualAngle_v;
Hv_pid_struct.integral += (Hv_pid_struct.err) * Hv_pid_struct.Ki;
/*位置式PID*/
if(Hv_pid_struct.integral > 1500)
Hv_pid_struct.integral = 1500;
else if(Hv_pid_struct.integral < -1500)
Hv_pid_struct.integral = -1500;
Hv_pid_struct.duty = (int32)(Hv_pid_struct.Kp*(Hv_pid_struct.err) + Hv_pid_struct.integral);
Hv_pid_struct.err_last = Hv_pid_struct.err;
if(Hv_pid_struct.duty > Hv_pwm_limit)
Hv_pid_struct.duty = Hv_pwm_limit;
else if(Hv_pid_struct.duty < -Hv_pwm_limit)
Hv_pid_struct.duty = -Hv_pwm_limit;
return Hv_pid_struct.duty;
}
//-------------------------------------------------------------------------------------------------------------------
// @brief 摄像头转向pd实现函数
// @param expect 期望中线
// @param input 实际中线
// @return int16 返回的偏差
// @author 2021山威海韵二队
//-------------------------------------------------------------------------------------------------------------------
int16 camerapid_realize(int16 expect,int16 input)
{
camera_pid_struct.Kp = Kp_camera;
camera_pid_struct.Kd = Kd_camera;
camera_pid_struct.A = A_camera;
camera_pid_struct.SetMiddle = expect;
camera_pid_struct.ActualMiddle = input;
camera_pid_struct.err = camera_pid_struct.ActualMiddle - camera_pid_struct.SetMiddle;
camera_pid_struct.Kp = (camera_pid_struct.A * camera_pid_struct.err * camera_pid_struct.err + Kp_camera);//二次p
camera_pid_struct.voltage = (int16)( camera_pid_struct.Kp*camera_pid_struct.err + camera_pid_struct.Kd*(camera_pid_struct.err-camera_pid_struct.err_last) );
camera_pid_struct.err_last = camera_pid_struct.err;
if(camera_pid_struct.voltage > camera_pwm_limit)
camera_pid_struct.voltage = camera_pwm_limit;
else if(camera_pid_struct.voltage < -camera_pwm_limit)
camera_pid_struct.voltage = -camera_pwm_limit;
return camera_pid_struct.voltage;
}
void turn_realize(void)//放在2ms一次的中断内
{
ADC_process();//获取电磁信息
Gyro_Steer = icm_gyro_z - GYROZ_ZERO;
cnt_2ms++;
if(cnt_2ms>=2)
{
dir_error = calc_position_image();
dif_speed = camerapid_realize(0,dir_error);//转向外环
cnt_2ms=0;
}
duty_Steer = -Hv_pid_realize((int32)dif_speed,Gyro_Steer);//转向内环输出差速
}（注：以上代码的陀螺仪角速度内环写的是位置式pi，理论上改成增量式pi可能会更好，但是当时用位置式调起来感觉也没啥问题就没管，有时间的小伙伴可以试试改成增量式的~）
控制部分的总流程图：
各环之间逻辑关系代码实现
//*************************************************************************
/*功能说明：直立，直行速度计算
* 参数说明：angle 融合后最终角度
* angle_dot 陀螺仪角速度
* 函数返回：无符号结果值
* @author 2021山威海韵二队
*/
//*************************************************************************
void Speed_Calculate(float angle,float angle_dot)//放在2ms一次的中断里
{
cnt_Av ++;
cnt_speed ++;
connect_balance();
/****************速度环，直立环，角速度环依次嵌套运行**************/
Get_encoder_pulse();
SpeedControlOutput(); //速度环的平滑输出
//直立时所要的速度
if(cnt_speed == 100){
g_fSpeedControlOutOld = g_fSpeedControlOutNew;
g_fSpeedControlOutNew = -speedpid_realize(-speed_expect_run,Avg_Speed_actual); //速度环
cnt_speed = 0;
}
if(cnt_Av == 10){
duty_up = uppid_realize((int32)duty_Speed,(int32)angle); //速度环 + 直立环
cnt_Av = 0;
}
speed_Start = Av_pid_realize(duty_up,(int32)angle_dot); //速度环 + 直立环 + 角速度环
//P_ANGLE P_GYRO 宏定义 直立所需要的 PD 参数
Straight_L = speed_Start; //左轮总速度
Straight_R = speed_Start; //右轮总速度
Speed_L_Result = Straight_L + duty_Steer; //加入转向环得最终速度
Speed_R_Result = Straight_R - duty_Steer;
}3.图像处理
3.1 获取图像数据
我们的摄像头使用的是MT9V034总钻风灰度摄像头，先通过摄像头采集获得原始的灰度图像，获得到二维的灰度图像数组，数组里内各元素的数值代表了每个相应位置上像素点的灰度值，范围是0-255，数值越大，代表这个点越白，越小代表越黑。
3.2 大津法与二值化
获取原始图像数据后，需要将白色的赛道部分和非白的赛道外的区域区分开来，进而得到赛道的边界信息，去先使用大津法求取划分赛道和非赛道图像之间的阈值，再根据该阈值进行二值化。
- 大津法阈值采用最大类间方差的原理，目标部分和平均灰度值的方差，加上背景部分和平均灰度值的方差，这个值最大的时候，获取当前阈值为最佳阈值。适合于图像灰度分布整体呈现“双峰”的情况，正好我们的赛道是铺在深色地板上的白色赛道，赛道边界处区分很明显。于是运用大津法就可以自动找出一个阈值，使得分割后的两部分类间方差最大。输入进去一个二位数组图像，然后会返回给一个阈值，然后就可以用这个阈值进行二值化（即大于阈值为白（黑），小于阈值为黑（白））。就可以得到一个二值化图像。
常规开源的大津法存在需要执行上万次的嵌套循环，速度比较缓慢。故对原有代码做以下算法结构上的优化：
- 去掉嵌套循环，只算前景，背景用总的减去前景即可
- 相间取点，减少运算
- 以0-255的灰度值为横坐标，相应的类间方差为纵坐标，最佳阈值附近的灰度值接近二次函数，当找到了最大类间方差，即类间方差有减小的趋势，便不用再继续遍历下去。
uint8 my_adapt_threshold(uint8 *image, uint16 col, uint16 row) //注意计算阈值的一定要是原图像
{
#define GrayScale 256
uint16 width = col;
uint16 height = row;
int pixelCount[GrayScale];
float pixelPro[GrayScale];
int i, j, pixelSum = width * height/4;
uint8 threshold = 0;
uint8* data = image; //指向像素数据的指针
for (i = 0; i < GrayScale; i++)
{
pixelCount[i] = 0;
pixelPro[i] = 0;
}
uint32 gray_sum=0;
//统计灰度级中每个像素在整幅图像中的个数
for (i = 0; i < height; i+=2)
{
for (j = 0; j < width; j+=2)
{
pixelCount[(int)data[i * width + j]]++; //将当前的点的像素值作为计数数组的下标
gray_sum+=(int)data[i * width + j]; //灰度值总和
}
}
//计算每个像素值的点在整幅图像中的比例
for (i = 0; i < GrayScale; i++)
{
pixelPro[i] = (float)pixelCount[i] / pixelSum;
}
//遍历灰度级[0,255]
float w0, w1, u0tmp, u1tmp, u0, u1, u, deltaTmp, deltaMax = 0;
w0 = w1 = u0tmp = u1tmp = u0 = u1 = u = deltaTmp = 0;
for (j = 0; j < GrayScale; j++)
{
w0 += pixelPro[j]; //背景部分每个灰度值的像素点所占比例之和 即背景部分的比例
u0tmp += j * pixelPro[j]; //背景部分 每个灰度值的点的比例 *灰度值
w1=1-w0;
u1tmp=gray_sum/pixelSum-u0tmp;
u0 = u0tmp / w0; //背景平均灰度
u1 = u1tmp / w1; //前景平均灰度
u = u0tmp + u1tmp; //全局平均灰度
deltaTmp = w0 * pow((u0 - u), 2) + w1 * pow((u1 - u), 2);
if (deltaTmp > deltaMax)
{
deltaMax = deltaTmp;
threshold = j;
}
if (deltaTmp < deltaMax)
{
break;
}
}
return threshold;
}此部分的原文链接：
3.3 元素处理
根据处理后的黑白二值化图像，使用最小二乘法拟合曲线，求出斜率、拐点等特征，进行补线等处理。
4.补充部分
1、图像部分
1.1 大津法优化
对大津法算法结构上进行优化，可将算法的执行效率提升了十倍以上。
大津法原理是寻找一个阈值，将图像分为前景和背景两部分。传统开源的大津法需要多次执行时间复杂度很高的循环，来寻找最佳阈值。
- 优化的方法就是利用前景、背景与全局灰度之间的关系，只算前景（平均灰度值、比例），背景的话用全局的减去前景，这样就省去了因为算背景导致的大量重复遍历。
- 然后以灰度值为横坐标，类间方差为纵坐标，使用了MATLAB观察分析了最佳阈值附近的灰度值所对应的类间方差，发现最佳阈值附近都接近二次函数。于是寻找到最大类间方差时，如果有一定减小的趋势，便不用在继续遍历下去。进一步提高了效率。
- 此外，发现当图像像素较高时，使用了相间取点的方法，大大减小了计算量。
1.2 图像处理
元素识别上，我们二值化后的图像根据获取了拐点位置、有效行等，并且用拟合函数求取边界斜率、以及使用横纵搜索结合的方式得到了更加全方位的赛道特征，同时使用了红外传感器，电感，陀螺仪等传感器加以辅助，使得我们的元素识别非常稳定。
2、控制算法
2.1 姿态与转向的双串级PID
由于直立平衡车系统中存在多个扰动信号，需要的控制精度较高，于是在姿态控制上，我们引入了从外到内的速度环、直立环、角速度环三个控制闭环组成的串级PID的方法来实现直立行进的控制，串级控制系统比单回路控制系统多了一个副回路，因而对进入副回路的比如 角速度信号 这样变化比较明显的二次波动有很强的抑制能力。使得振荡周期减小，调节更加迅速。
2.2 自适应P参数
转向控制的PID中P是一个很关键的参数，在调试过程中我们发现
- ①P参数较大的时候小车在由弯道进入直道之后会产生大幅度的的震荡
- ②P参数较小的时候直道很稳定，但弯道处响应太慢。
于是我们对P参数引入误差的指数型函数来动态调整：
这样改进后的P参数能够使得舵机在不同情况下的赛道的适应性得到很大的提高，而且比传统的分段PID控制更加平滑。
3、UI菜单设计
基于IPS液晶屏编程进行多级菜单的UI设计，还融合了Flash参数存储、旋转编码器与按键调参、拨码开关模式切换等功能，使得系统调试效率得到了极大的提高。
二、十七届多车编队——协同编队方案
通讯方面，使用沁恒公司提供的蓝牙组网模块，组网之后，头车通过串口发送数据，后两车均能收到，据此实现多车之间各种标志位的发送，完成多车发车、元素标志位通信、多车停车等功能。
控距方面，使用分体式超声波测距，其中头车装有超声波发送端，尾车和中间车则装有接收端，两个接收端可分别获取与头车发送端的距离，并将数据以串口通讯的方式发送给单片机，于是就可以使用该距离数据进行速度期望的控制，进而控制距离。
头车： 两尾车机械结构基本一致：
代码片段如下
/***********************通信（头车部分）**************************/
if(quan==0)
{
if(Fork_flag==2)
{
uart_write_byte(UART_elec, '2');//尾车拉远
}
if(Fork_flag==3||Fork_flag==4)
{
uart_write_byte(UART_elec, 250);//提示中间车发车
fork_integral = 0;
quan=1;
}
}
if(zebra_flag==1&&quan==1)
{
uart_write_byte(UART_elec, '6');//尾车第一次冲斑马线
}
if(quan==1&&Fork_flag==2)
{
uart_write_byte(UART_elec, '3');//电磁车左进三岔 尾车跟紧
ranging_over = 1;
quan=2;
}
if(Avg_Speed>speed_expect-50&&ruku_flag)
uart_write_byte(UART_elec, '4');//尾车预备斑马线停车
if(zebra_flag&&quan==2)
{
ruku_flag = 1;//第二圈入库
}
/***紧急情况处理↓***/
if(com_dat_camera == 251||com_dat_elec == 251)
{
stop_flag_out_of_track = 1;//三车全停
uart_write_byte(UART_elec, 251);
}
if(Avg_Speed<speed_expect_adjust/2&&encoder_integral>3000) stop_flag_out_of_track = 1;
if(stop_flag_out_of_track&&Avg_Speed>speed_expect/3)
{
if(!ruku_flag&&!rush_flag)
{
uart_write_byte(UART_elec, 251);//后两车紧急停车
}
}
/***紧急情况处理↑***///测距（基于STC芯片）
if(ranging_flage==0)
{
dat[num] = S2BUF;
if(dat[0] != 0xA5) num = 0;//检查第一个数据是否为0xa5
else num++;
if(num == 3) //接收完成 开始处理数据
{
num = 0;
ranging_counter = dat[1]<<8 | dat[2];
//得到超声波模块测出的距离（单位：毫米）
if((ranging_counter/10)<300&&(ranging_counter/10)>0)
{
datt[numm]=ranging_counter/10;
datt[15]+=datt[numm];
numm++;
}
if(numm==3)
{
if(datt[15]/10<300&&datt[15]/10>0)
ranging_meter=datt[15]/3;
numm=0;datt[15]=0;
ranging_flage = 0x01; //注：后面在其他地方使用完ranging_meter之后，ranging_flage 及时清零
}
}
}//后两车控距
if(ranging_meter-60>=0)
speed_goal=limit_max((speed_set+speed_set/10*(ranging_meter-60)*dis_P/20),speed_set+20);//防止超速
else
speed_goal=limit_min((speed_set+speed_set/10*(ranging_meter-60)*dis_P/15),0);
}
