相信大家经过这段时间的调车发现,单纯在搜线获得的偏差上面乘以一个系数是没法获得很好的循迹效果的。因为这个系数如果太小的话,在弯道就会出现过不去的情况;如果这个系数给的很大的话,小车在直到上面也会出现抖动。这种无效的抖动很可能会影响小车后面的循迹,具体表现为小车越抖越厉害,然后就冲出赛道了。
这里附上一个反面例子:
例子
当时我也给出了一个初步解决这个问题的方法,就是用偏差的二次方来指导循迹。
原来驱动舵机的函数:
Set_Steer(2.5*offset); //控制舵机01
修改过后驱动舵机的函数:
Set_Steer(0.5*offset*fabs(offset)); //控制舵机
//fabs()是C语言中定义的求取绝对值的函数
//包含math.h的头文件就可以直接用
这个方法的思路是:
我们先假定知道的偏差是2,弯道是4;
弯道能过的情况是控制舵机的值为20,直道不抖的条件是控制舵机的值小于5。
用第一种循迹方法,直道输给舵机的值是2.5*2等于5,弯道输给舵机的值是2.5*4等于10。
用第一种循迹方法,直道输给舵机的值是2.5*2*|2|等于10,弯道输给舵机的值是2.5*4*|4|等于40。
第一种情况,我们为了让小车能通过弯道,系数等于20/4 (5); 这时在直道时控制舵机的值为5*2等于10。可见在这种情况下两者难以兼顾。
第二种情况,我们要保证小车能通过弯道,系数等于20/4/4 (1.25); 这时在直道时控制舵机的值为1.25*2(5);
通过数学就算可见,改方法可以在一定程度上提高车辆的上限。但要说能起到化腐朽为神奇的魔力的话还是夸张了。
大家可能也想到了另一个思路,就是赋给不同的偏差一个不同的系数。这个方法的理论上限应该是更高的。大概十年前的智能车就是用这种方法循迹的。
这里附上这个思路的伪代码:
if(offset <3)
{
Set_Steer(系数1*offset); //控制舵机
}
else if(offset >= 3 && offset < 6)
{
Set_Steer(系数2*offset); //控制舵机
}
else if(offset >= 6 && offset < 9)
{
Set_Steer(系数3*offset); //控制舵机
}
else if(offset >= 9 && offset < 12)
{
Set_Steer(系数4*offset); //控制舵机
}
这种方案虽然上限要高很多,但要调的系数也跟多了很多。大家有时间也可以自己尝试一下。
这里说明一下,因为我们做的是裸机开发,可能会遇到各种各样的问题。所以遇到问题大家要冷静分析。这里浅浅交代大家三点。
调车的时候要保持赛道和轮胎干净,避免有灰尘影响小车的循迹。
调车不要开屏幕显示,很费算力的,而且要在代码里面就注释掉。
如果你改了参数以后,发现小车运行的效果和你预想的不一样,应该先重复发几次车,如果问题还得不到解决,应该检查自己代码的逻辑问题。
既然前面提到的都是十几年前的技术了,那这十年肯定发展了。那就是用pid取代了if else if的简单分段控制。将经典控制理论中的pid调节运用的智能车的控制上,并在此基础上延伸出了模糊控制和分段pid控制。模糊控制后面会专门培训一次。
昨天叫大家看了逐飞科技发的文章,我截取中间最重要的一部分讲一下:
用pid控制舵机是为了让舵机打角度更灵敏,用pid来控制电机是为了让电机的速度调节更灵敏。
下面用我写的代码来进行分析。
首先我们使用中断来实现对舵机和电机实现定时控制,以前那种把所有要运行的代码全部写在while循环里面的叫做轮询控制。使用中断的控制方法叫做前后台模式。
这里简单介绍一下中断的概念:
中断即打断,实至CPU再执行当前程序时,由于系统出现了某种需要处理的紧急情况,CPU暂停正在执行的程序,转而去执行另一段特殊程序来处理的出现的紧急事务,处理结束后CPU自动返回到原先暂停的程序中去继续执行,这种执行过程由于外界的的原因被中间打断的情况成为中断。
中断的作用:
中断使得计算机系统具备应对对处理突发事件的能力,提高了CPU的工作效率,如果没有中断系统,CPU就只能按照原来的程序编写的先后顺序,对各个外设进行查询和处理,即轮询工作方式,轮询方法貌似公平,但实际工作效率却很低,却不能及时响应紧急事件。此外中断还可以设置中断优先级,优先级高的中断可以抢占优先级低的中断。
开了中断以后就要开始确定控制电机和舵机的周期了,C车模上面使用的舵机是S3010,额定频率是50hz,它的控制周期就是1000ms/50hz == 20ms;电机因为要经常加减速,所以它的控制周期也不宜过长,一般取10ms,或者20ms,控制周期短的话,加减速就会更加灵敏,代价就是容易发热。
这里附上我控制舵机和电机的中断服务函数:
/************************************************
函数名:State_Adjust
功 能:状态调节
参 数:void
返回值:void
************************************************/
void State_Adjust(void)
{
//调节标志位
if(flag.t_ms == 5)
{
flag.speed += 5;
flag.direction += 5;
flag.t_ms = 0;
}
//方向调节:10ms
if(flag.direction == 20)
{
Direction_pid(&dpid);
Set_Steer(dpid.out);
flag.direction = 0;
}
//速度调节:20ms
if(flag.speed == 20) //增量式
{
Speed_Measure(); //速度值测量v
L_speed_pid(&l_spid); //左轮pid调节
R_speed_pid(&r_spid); //右轮pid调节
Set_Motor(l_spid.out, r_spid.out);
flag.speed = 0;
}
}
这里面就调用了控制舵机和电机的pid函数:
控制舵机的二次动态pid:
/************************************************
函数名:Direction_pid
功 能:运用二次动态位置式控制小车的方向
参 数:struct PID *sptr
返回值:void
************************************************/
void Direction_pid(struct PID *sptr)
{
sptr->error = offset; //摄像头采集计算的偏差
real_kp = 1.0*(sptr->error*sptr->error)/ki + kp;
sptr->out = (int16)(real_kp*sptr->error + kd*(sptr->error - sptr->last_error));
sptr->last_error = sptr->error;
sptr->last_out = sptr->out;
}
我是运用结构体写的,为了方便大家理解,我附上普通的写法
/************************************************
函数名:Direction_pid
功 能:运用二次动态位置式控制小车的方向
参 数:struct PID *sptr
返回值:void
************************************************/
void Direction_pid(struct PID *sptr)
{
error = offset; //摄像头采集计算的偏差
real_kp = 1.0*(error*error)/ki + kp;
out = (int16)(real_kp*error + kd*(error - last_error));
last_error = error;
last_out = out;
}
//注意这段代码里面的ki不是pid里面的i,它只是一个普通的变量,为了不重新定义一个变量,所以直接用了ki
控制电机的增量式pid:
/************************************************
函数名:L_speed_pid
功 能:运用PID的增量式控制小车左轮的速度
参 数:struct PID *sptr
返回值:void
************************************************/
void L_speed_pid(struct PID *sptr)
{
Speed_Goal = speed_error(); //左轮速度偏差计算 0708
sptr->error = Speed_Goal_l - l_speed_now; //左轮速度偏差计算
sptr->out += sptr->kp*(sptr->error - sptr->last_error) + sptr->ki*sptr->error;
sptr->last_error = sptr->error;
}
同样改写为普通变量形式:
/************************************************
函数名:L_speed_pid
功 能:运用PID的增量式控制小车左轮的速度
参 数:struct PID *sptr
返回值:void
************************************************/
void L_speed_pid(struct PID *sptr)
{
Speed_Goal = speed_error(); //速度偏差计算
error = Speed_Goal_l - l_speed_now;
out += kp*(error - last_error) + ki*error;
last_error = error;
}
注意,要对电机进行闭环控制的话,就需要实时采集小车后轮的速度。这里使用编码器的脉冲值换算成车速。
/************************************************
函数名:Speed_Measure
功 能:带方向编码器返回的脉冲数为小车速度(虚拟速度)
参 数:void
返回值:void
************************************************/
void Speed_Measure(void)
{
//获取左编码器脉冲
l_encoder_pulse = gpt12_get(GPT12_T2);//正转脉冲为正
gpt12_clear(GPT12_T2);
//获取右编码器脉冲
r_encoder_pulse = -gpt12_get(GPT12_T5);
gpt12_clear(GPT12_T5);
l_speed_now = l_encoder_pulse;
r_speed_now = r_encoder_pulse;
break_speed = (l_speed_now + r_speed_now) /2;
}
至此,舵机和电机的pid控制代码都讲完了。
为了方便大家调参,这里我给大家附上一份eeprom按键调参代码:
首先我介绍一下,什么是eeprom:
EEPROM (Electrically Erasable Programmable read only memory)是指带电可擦可编程只读存储器。是一种掉电后数据不丢失的存储芯片。 EEPROM 可以在电脑上或专用设备上擦除已有信息,重新编程。一般用在即插即用。
第一步:从eeprom中读取数据
void Parameter_read_eeprom_canshu(void)
{
dpid.kp = flash_read(EXAMPLE_EEPROM_SECTOR, 0, float);
dpid.ki = flash_read(EXAMPLE_EEPROM_SECTOR, 1, float);
dpid.kd = flash_read(EXAMPLE_EEPROM_SECTOR, 2, float);
l_spid.kp = flash_read(EXAMPLE_EEPROM_SECTOR, 3, float);
l_spid.ki = flash_read(EXAMPLE_EEPROM_SECTOR, 4, float);
r_spid.kp = flash_read(EXAMPLE_EEPROM_SECTOR, 5, float);
r_spid.ki = flash_read(EXAMPLE_EEPROM_SECTOR, 6, float);
k_diff = flash_read(EXAMPLE_EEPROM_SECTOR, 7, float);
}
第二步:按键调参
void Key_Scan_canshu(void)
{
uint8 key1 = 1, key2 = 1, key3 = 1, key4 = 1;
uint8 key1_last = 1, key2_last = 1, key3_last = 1, key4_last = 1;
uint8 key_num = 0;
//显示当前参数
ips200_showstr(10, 0, "para");
ips200_showuint8(65, 0, parameter);
//方向环pid参数
ips200_showstr(10, 1, "d_kp");
ips200_showfloat(65, 1, dpid.kp, 2, 2);
ips200_showstr(10, 2, "d_ki");
ips200_showfloat(65, 2, dpid.ki, 3, 2);
ips200_showstr(10, 3, "d_kd");
ips200_showfloat(65, 3, dpid.kd, 2, 2);
//速度环pid参数
ips200_showstr(10, 4, "l_s_kp");
ips200_showfloat(65, 4, l_spid.kp, 2, 2);
ips200_showstr(10, 5, "l_s_ki");
ips200_showfloat(65, 5, l_spid.ki, 2, 2);
ips200_showstr(10, 6, "r_s_kp");
ips200_showfloat(65, 6, r_spid.kp, 2, 2);
ips200_showstr(10, 7, "r_s_ki");
ips200_showfloat(65, 7, r_spid.ki, 2, 2);
//差速
ips200_showstr(10, 8, "k_diff");
ips200_showfloat(65, 8, k_diff, 2, 2);
while(TRUE)
{
//保存按键状态
key1_last = key1;
key2_last = key2;
key3_last = key3;
key4_last = key4;
//检测当前按键状态并给出对应键值
key1 = !gpio_get(P22_0);
key2 = !gpio_get(P22_1);
key3 = !gpio_get(P22_2);
key4 = !gpio_get(P22_3);
if(key1 && !key1_last)
key_num = 1;
if((key2 && !key2_last) || (key2 == 1 && key2_last == 1))
key_num = 2;
if((key3 && !key3_last) || (key3 == 1 && key3_last == 1))
key_num = 3;
if( (key4 && !key4_last) || (key4 == 1 && key4_last == 1))
key_num = 4;
//根据键值选择对应功能
switch(key_num)
{
case 0:
break;
case 1:
adjust_ok_1 = 1;
break;
case 2:
parameter++;
parameter = (parameter <= PARAMETER_N) ? parameter : 1;
break;
case 3:
switch(parameter)
{
case 1:dpid.kp += 0.1; break;
case 2:dpid.ki += 1; break;
case 3:dpid.kd += 0.1; break;
case 4:l_spid.kp += 0.05; break;
case 5:l_spid.ki += 0.05; break;
case 6:r_spid.kp += 0.05; break;
case 7:r_spid.ki += 0.05; break;
case 8:k_diff += 0.05; break;
}
break;
case 4:
switch(parameter)
{
case 1:dpid.kp -= 0.1; break;
case 2:dpid.ki -= 1; break;
case 3:dpid.kd -= 0.1; break;
case 4:l_spid.kp -= 0.05; break;
case 5:l_spid.ki -= 0.05; break;
case 6:r_spid.kp -= 0.05; break;
case 7:r_spid.ki -= 0.05; break;
case 8:k_diff -= 0.05; break;
}
break;
}
//显示当前参数
if(key_num)
{
//显示游标
ips200_clear(0xFFFF);
ips200_showstr(0, parameter, "_");
//显示调节后的参数
ips200_showstr(10, 0, "para");
ips200_showuint8(65, 0, parameter);
//方向环pid参数
ips200_showstr(10, 1, "d_kp");
ips200_showfloat(65, 1, dpid.kp, 2, 2);
ips200_showstr(10, 2, "d_ki");
ips200_showfloat(65, 2, dpid.ki, 3, 2);
ips200_showstr(10, 3, "d_kd");
ips200_showfloat(65, 3, dpid.kd, 2, 2);
//速度环pid参数
ips200_showstr(10, 4, "l_s_kp");
ips200_showfloat(65, 4, l_spid.kp, 2, 2);
ips200_showstr(10, 5, "l_s_ki");
ips200_showfloat(65, 5, l_spid.ki, 2, 2);
ips200_showstr(10, 6, "r_s_kp");
ips200_showfloat(65, 6, r_spid.kp, 2, 2);
ips200_showstr(10, 7, "r_s_ki");
ips200_showfloat(65, 7, r_spid.ki, 2, 2);
//差速
ips200_showstr(10, 8, "k_diff");
ips200_showfloat(65, 8, k_diff, 2, 2);
//键值清零
key_num = 0;
}
//调参完成跳出循环
if(adjust_ok_1 == 1)
{
parameter = 1;
break;
}
}
//显示速度策略
ips200_clear(WHITE);//清屏
ips200_showstr(0, 0, "PID");
ips200_display_chinese(25, 0, 16, zifu[0], 6, RED); //调参完毕
systick_delay_ms(STM1, 200); //延时
ips200_clear(WHITE); //清屏
}
第三步:把数据写入eeprom
void Parameter_write_eeprom_canshu(void)
{
eeprom_erase_sector(EXAMPLE_EEPROM_SECTOR);
//数据类型转化----第0页
write_buf = float_conversion_uint32(dpid.kp);
eeprom_page_program(EXAMPLE_EEPROM_SECTOR, 0, &write_buf);
write_buf = float_conversion_uint32(dpid.ki);
eeprom_page_program(EXAMPLE_EEPROM_SECTOR, 1, &write_buf);
write_buf =float_conversion_uint32(dpid.kd);
eeprom_page_program(EXAMPLE_EEPROM_SECTOR, 2, &write_buf);
write_buf = float_conversion_uint32(l_spid.kp);
eeprom_page_program(EXAMPLE_EEPROM_SECTOR, 3, &write_buf);
write_buf = float_conversion_uint32(l_spid.ki);
eeprom_page_program(EXAMPLE_EEPROM_SECTOR, 4, &write_buf);
write_buf =float_conversion_uint32(r_spid.kp);
eeprom_page_program(EXAMPLE_EEPROM_SECTOR, 5, &write_buf);
write_buf = float_conversion_uint32(r_spid.ki);
eeprom_page_program(EXAMPLE_EEPROM_SECTOR, 6, &write_buf);
write_buf = float_conversion_uint32(k_diff);
eeprom_page_program(EXAMPLE_EEPROM_SECTOR, 7, &write_buf);
}
最后给大家最介绍一下pwm:
PWM的全称是脉冲宽度调制(Pulse-width modulation),是通过将有效的电信号分散成离散形式从而来降低电信号所传递的平均功率的一种方式;
所以根据面积等效法则,可以通过对改变脉冲的时间宽度,来等效的获得所需要合成的相应幅值和频率的波形;
想详细了解可以看这篇博客: