第五届部分大学舵机控制策略.docx

北京科技大学:（多行平均）

将每场图像的黑线中心加权平均值与舵机PID参考角度值构成一次线性关系。

(1) 将积分项系数置零，我们发现相比稳定性和精确性，舵机在这种随动系统中对动态响应性能的要求更高。更重要的是，在KI置零的情况下，我们通过合理调节Kp，发现车能够在直线高速行驶时仍能保持车身非常稳定，没有震荡，基本没有必要使用KI参数； (2) 微分项系数KD使用定值，原因是舵机在一般赛道中都需要较好的动态响应能力； (3) 对Kp，我们使用了二次函数曲线，Kp随黑线位置与中心值的偏差呈二次函数关系增大，在程序中具体代码如下： loca_Kp = (loca_error * loca_error)/2 + 1000 其中，local_error是黑线位置与中心值的偏差。

华中科技大学（单行）
从图像中，经过对于跑道图像的处理，得到小车行走的期望路线，与小车前
轮所处的中心线，即图像中心比较得到一个偏差，根据偏差直接线性控制舵机
转角。
在这个过程中，所选取参考位置的远近影响到小车的转弯性能，因而，我们
也采取了根据速度的不同，动态地选取参考位置，充分考虑到了小车提前拐弯
的性能。
4.3.3

广东工业（同北科）

多点(间隔较远)平均,

舵机控制算法 #include "includes.h"
static sint Error[4]; //偏差数组
static uint turnleft_count; //转向标记用uint可以记得数到达65535足够大
static uint turnright_count;
sint si_SteerEx = 0; //期望的舵机控制量
uchar SteerP = 8;
uchar SteerD = 6;
sint VE = 0; //图像偏移
uchar zhi_flag = 0;
static uint Last_Direction = SteerCenter;
sint D_sum; //远端转向趋势
//-----
//求出中心线的最大最少值，用来判断小S和直线
//-----------------------------------------
uchar JudgeSt(void)
{
uchar max, min;
uchar CenterMaxR, CenterMinR;
uchar i;
schar temp;
sint Ve_temp = 0;
sint Dsum_temp = 0;
uchar cacular_mid;
zhi_flag = 0;
cacular_mid = BlackCenter[10];
max = min = BlackCenter[0];
for (i = 2; i < CaculaCenter - 2; i = i + 2)
{
Ve_temp = Ve_temp + *(BlackCenter + i);
Dsum_temp = Dsum_temp + *(BlackCenter+i)-*(BlackCenter+i-1);
if (max <= BlackCenter[i])
{

东秦

我
们选择第35 行作为主要的控制行，把这一行黑线的位置与图像中心位置的差作
为偏差进行PID 控制。另外用第50 行和第37 行的差即空间上的偏差变化量作
为辅助控制量，目的是充分利用摄像头在前瞻方面的优势，提前判断赛道走势。
PD 控制式如公式4：
PWMDTY 45 = KP *error + KD1 *Ec1 + KD2 *Ec2 公式 4
其中error 为控制行黑线位置偏离中心位置的值， 1 Ec 为时间上的偏差变化
量，即error 的变化量， 2 Ec 为空间上的偏差变化量。
变参数主要体现在当识别出赛道为直道或小 S 时，选用较小的PD 参数，让
小车舵机输出小转角，基本实现沿直线前进。而当为其它赛道类型是则选用较
大的PD 参数，实现及时转弯和内切另外B 型车舵机左右不对称的客观因素在
调车过程中也是不容忽视的，为此需先算出PWM 输出，判断小车转向左或是右，
然后调用不同的PD 参数。
参数加权则是两个控制量之间的权重分配。要配合着PD 参数在调车过程中
一起调。
/****************************************************************
/
/*** 函数名　rudder_control
***/
/*** 功能：根据控制点偏离中心位置实施PID 控制 ***/
/*** 日期：20100627 ***/
/***********************
/
void rudder_control(void){
static int cha=0,cha2=0,cha3=0;
int last_cha=0;
unsigned char r_i=0;
unsigned int pwm_cal=0;
byte s_high=0;
byte s_low=0;
last_cha=cha;
cha=location[control_point]-centre_location;
if(lie_end_flag){
第五届全国大学生智能汽车邀请赛技术报告
XXIV
lie_end_flag=0;
if(lie_end_hang<20){
if(last_cha>=0)
cha=-50;
else
cha=50;
}
}else if(invalid_c_first){
invalid_c_first=0;
if(last_cha>=0)
cha=-50;
else
cha=50;
}
if(cha>=0){
//右拐
angle.kp=65;
angle.kd=7;
angle.kd1=2;
}
else {
//左拐
angle.kp=60 ;
angle.kd=9;
angle.kd14;
}
if(location[lie_end_hang]>80) {
if(lie_end_hang>30) {
angle.kp+=55;
}else if(lie_end_hang>=20) {
angle.kp+=65;
}
}else if(location[lie_end_hang]<20){
附录
XXV
if(lie_end_hang>30) {
angle.kp+=55;
}else if(lie_end_hang>=20) {
angle.kp+=60;
}
}
if(s_flag==1){
if(road_type==2) angle.kp-=10;
}
cha=-cha;
cha2=cha-last_cha;
cha3=location[37]-location[50];
pwm_cal=angle.kp*cha/20+angle.kd*cha2+angle.kd1*cha3+centre_pwm;
if(pwm_cal>2300) pwm_cal=2300;
else if(pwm_cal<500) pwm_cal=500;
PWMDTY45=pwm_cal;

哈尔滨极品飞车
byte YDstCorrect[231] = {155, 148, 142, 136, 130, 125, 121, 116, 112, 109, 105, 102, 99, 96,
93, 90, 88, 86, 83, 81, 79, 78, 76, 74, 72, 71, 69, 68, 66 ,65, 64, 63, 61,
60, 59, 58, 57, 56, 55, 54, 53, 53, 52, 51, 50, 49, 49, 48, 47, 47, 46, 45,
45, 44, 43, 43, 42, 42, 41, 41, 40, 40, 39, 39, 38, 38, 37, 37, 37, 36, 36,
35, 35, 35, 34, 34, 34, 33 33, 33, 32, 32, 32, 31, 31, 31, 30, 30, 30, 30,
29, 29, 29, 29, 28, 28, 28, 28, 27, 27, 27, 27, 26, 26, 26, 26, 26, 25, 25,
25, 25, 25, 24, 24, 24, 24, 24, 24, 23, 23, 23, 23, 23, 23, 22, 22, 22, 22,
22, 22, 22, 21, 21, 21, 21, 21, 21, 21, 20, 20, 20, 20, 20, 20, 20, 20, 19,
19, 19, 19, 19, 19 19, 19, 19, 19, 18, 18, 18, 18, 18, 18, 18, 18, 18, 18,
17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 17, 16, 16, 16, 16, 16, 16, 16, 16,
16, 16, 16, 16, 16, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15,
14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 13,
13, 13, 13, 13 13, 13, 13, 13, 13}; //梯形校正数组
byte XDstCorrect[227] = {0, 0, 0, 1, 1, 1, 1, 1, 1, 2 , 2 , 2 , 2 , 2 ,
2 , 2 , 3, 3 , 3, 3 , 3 , 3, 3, 4, 4, 4, 4, 4, 4, 5 , 5 , 5 , 5 ,
5 , 5 , 5 , 6 , 6, 6 , 6, 6 , 6 , 6, 7, 7, 7, 7, 7, 8, 8, 8, 8,
8, 9, 9 , 9 , 9 , 9 , 10 , 10 , 10 , 10 , 10, 11, 11, 11, 11, 11, 12, 12, 12,
12, 12, 13, 13, 13 , 13 , 13 , 14 , 14 , 14 , 14 , 14 , 15, 15, 15, 15 , 15 , 16, 16,
16, 16, 16, 17, 17, 17, 17, 18, 18, 18, 18, 19, 19, 19, 20 , 20 , 20 , 21 , 21 ,
21 , 21, 22, 22 , 22, 23, 23 , 23 , 24, 24, 24, 24, 25, 25, 25, 26, 26 , 26 , 27 ,
27 , 27, 28, 28, 29, 29, 30, 30, 30, 31 , 31 , 32 , 32 , 33, 33, 33, 34, 34 ,
35, 35, 36 , 36 , 36, 37, 37, 38, 39, 39, 40, 40, 41, 42, 42 , 43 , 43 , 44 , 45 ,
45 , 46 , 46 , 47, 48, 49, 49, 50, 51, 52, 53, 54 , 54 , 55 , 56 , 57 , 58, 59, 60,
61, 62, 63, 64, 65, 66 , 67 , 68, 69, 71, 72, 73, 74, 76, 77, 79, 80,
Generated by Foxit PDF Creator © Foxit Software
For evaluation only.
附录
III
82, 84, 85, 87, 89, 91, 93, 95, 97, 99, 101, 103, 105,
107, 109, 112, 114, 117, 119, 122, 124, 127, 130, 134,
138, 142};

DstStartY = (Middle - DstLine[1])*13;
DstStopX = XDstCorrect[PrccesslineSto[LineStop] - 50];
Generated by Foxit PDF Creator © Foxit Software
For evaluation only.
附录
V
DstStopY = (Middle - DstLine[LineStop])*YDstCorrect[280 -
PrccesslineSto[LineStop]];
DstK = (DstStopY - DstStartY)/DstStopX;

if(LineStop > 30)
{
Generated by Foxit PDF Creator © Foxit Software
For evaluation only.
第五届全国大学生智能汽车邀请赛技术报告
VI
e = Middle - DstLine[30];
setDty(80);
}
else
{
setDty(90);
}
Ipe = 8*e;
if(Ipe > 1000)
Ipe = 1300;
if(Ipe < -1000)
Ipe = -1300;
PWMDTY45 = Center + Ipe;

河北理工:（参数多，复杂）
上、下半幅图像的斜率为：
slop1=black_mid[line_able-1]-black_mid[(line_able-1)/2];
slop2=black_mid[(line_able-1)/2]-black_mid[0];
上、下半幅图像斜率之间的差距为：
slop_dif=abs_stub(slop1,slop2);
之后，即可得出拟合直线坐标：
x1=black_mid[0];
y1=0;
x2=black_mid[line_able-1];
y2=line_able-1;
err_s_e=x2-x1;//首末行偏差
slop_line=(x2-x1)/y2;//拟合直线斜率

steer=Kp*err_0+Ki*(err_0+err_1+err_2)+Kd*(err_0-err_1); 公式1
其中控制量是黑线与CCD摄像头镜头中线的平均偏移量。在这样的控制策略下，赛车过于沿黑线行驶，尤其是在走大S形赛道时表现不好，寻迹太规矩，这样不仅浪费时间，而且赛车很容易冲出赛道。
于是，我们尝试在传统PD控制的基础上加补偿量Ks,Ks的控制量为图像最远处一行黑线位置与最近处一行黑线位置的偏移量err_s_e。
steer=Kp*err_0+Ki*(err_0+err_1+err_2)+Kd*(err_0-err_1)+Ks*(err_s_e);
公式2
加上这一补偿量后，我们发现赛车的提前转向令人满意，尤其是在大S赛道，小车实现了沿近路行驶。
具体而言，我们针对区分好的不同道路类型采取不同的舵机转角控制，例如若判断为直道，则KP，KS较小；若判断为小C，则KP相应增大，KS也较大，同时KD也变大，以增强智能车预判断能力。

西南民族:
了矫正梯形失真所带来的远端图像较窄近端图像较宽的问题，我们采取在以第 1 行中心点为坐标原点的相对坐标中分段求斜率的方法，选择第 1、9、17、25、33、41、48 行作为划分各段的界线，把原有图像分为 6 段。接下来求出各段图像中黑线的斜率，然后各段斜率再乘以各自的系数相加后求平均值即得到总斜率。
这种算法不仅解决了图像失真的问题还可以通过调整各段斜率的权重来改变小车的入弯出弯姿态，可谓一举两得。（部分程序代码如下图所示，因为数组调用上使用指针，所以行数均进行了减一运算）
„„„„ 图 5.2.3 斜率计算及路径识别程序部分代码 我们根据实际测试情况中计算出总斜率的数值，将路径识别为 5 种情况：直线、大左弯、大右弯、小左弯和小右弯。然后根据不同的赛道情况修改转向参数，从而减小由于图像变形和舵机左右不对称带来的系

西安交通（透视变换）:复杂！
根据智能车的运动模型，在车不出边界线的情况下，保证每一个时刻，使舵机转向角θ最小，且小车行驶路径最短。优化路径的示意图如下图4.12所示。第五届全国大学生智能车竞赛技术报告 36
图4.12 优化路径示意图

对于S型弯道，需要找到两个圆弧的连接点，称为转折点。利用最小二乘法可以对每一个赛道上的点球出一个斜率，转折点的特点就是其斜率曲线为极值点。原始图像如图4.13所示。计算出来的斜率如图4.14所示。

图4.13 S型道路
图4.14 计算出来的斜率曲线

这样就可以求出A,B两点。从而确定智能车行驶的路线。在赛道识别中，有时会由于某场图像不清晰造成漏判，或由于小车所走路径变化造成误判。这些偶然情况是不可能完全消除的。经过大量的实验与修改，最终我们的赛道识别算法是比较稳定可靠的。

舵机控制:

西北工业大学多点平均偏差判断赛道S,直，弯
均值计算：
X_PRESENT=X_PRESENT+(black_wire[lcount-1]-black_wire[0])
*line_near_n/line_near_d;
最近与最远端差
line_near是视场近处的行数，line_near_d是一个比例常数；这两个场数可以

a) 微分项系数Kd 则使用定值，原因是舵机在一般赛道中都需要好的动态响应能力；
b) 对Kp，我们使用了二次函数的曲线，Kp 随黑线的偏离位置二次函数关系增大。我们选择最后测试了一些PID 参数，得到了较为理想的转向控制效果。有一定抗干扰和抗反光能力的黑线提取算法经过不断的试验之后，最后调出了适合是本系统的PD系数。函数结构如下：
KP=(X_PRESENT-VIDEO_CENTER)*
(X_PRESENT-VIDEO_CENTER)/28+40;
steer=steer_center+(VIDEO_CENTER-X_PRESENT)*KP
+KD*EE_PRESENT;
其中KD是常数，steer是舵机输出量，VIDEO_CENTER是视场中心，第五届全国大学生智能汽车邀请赛技术报告 42 / 70
EE_PRESENT是前后两次均值之差。