我的平衡车设计--软件篇

大赛结束了，由于时间紧迫，做车的时间很短，所以留下了一些遗憾，趁这段时间有空，整理一下几个月来的资料，既是对自己几个月来的总结，也算上和大家的一些交流吧，这篇文章就谈谈我的平衡车所用到的一些方案和算法吧
首先我用的方案是卓晴老师（在常大看到卓老师啦~）的方案，这次方案是做了一些修改和拓展，比如滤波我采用的互补滤波，速度控制用的是P控制而非PI，方向控制使用了转向陀螺仪。

控制周期5ms，除了蓝牙发送程序外，其他代码均放在中断中，这样的好处是控制周期准确，5ms周期中，CCD采样572us，ACC、Gyro采样+计算 784us，直立、方向、速度控制 208us，CCD计算500us左右，总共时间略大于2ms。

曝光时间固定5ms，这个时间在白天以及比赛现场是很合适的，但是到了晚上就不行了，到了晚上一般我会将曝光时间x2，并用运放对CCD信号进行2~5倍的放大。

下面详细说一下一些我认为比较重要的部分吧。

FIR滤波器
首先介绍的FIR滤波器，在我的代码中，我大量使用了FIR低通滤波器，主要是这么5个地方：加速度计角度、速度、直立控制中的Error、赛道中心线、速度控制中的Error。

关于FIR的介绍和设计估计可以重开一贴了，所以这里主要说一下FIR的效果，在直立控制中，我对Error=Setpoint-angle进行了一次低通滤波，带来的效果就是直立控制的P、D参数可以使用更高的值，也就是说小车立的更硬了，平常跑的时候基本1.6~1.8m/s过单障碍，FIR之前，kp=1250,kd=18，FIR之后kp=1800,kd=30。

速度值通过低通滤波之后杂波明显少了很多，曲线也更光滑了。

计算出来的赛道中心线之所以使用低通滤波进行滤波主要是为了防止赛道中心突变，因为整个赛道是连续的，即使是在十字弯。

下面附上一张加速度计经过FIR低通滤波之后的效果图：
滤波之前，黄色的是加速度计角度，红色是融合之后的角度：
滤波之后：
当然FIR滤波器也有他的缺点，比如频率选择特性没有IIR好，会有固定的相位延迟，延迟为拍数/2*采样间隔
互补滤波
平衡车角度的获取我并没有采用清华方案，而是采用了动态权值互补滤波，好处是可以得到小车的真实角度，计算量有所增加，但几百us还是可以接受的。

互补滤波关键代码：
tempX=angleAX*5/4096*1000/AccSense;//5v,12bit ADC,AccSense查datasheet 可得
tempZ=angleAZ*5/4096*1000/AccSense;
tempZ=angleAZ*5/4096*1000/AccSense;
angleA=-atan2(angleAX,angleAZ)*180/PI;//加速度计计算出的角度
angleA=FIR(angleA);
gravity=sqrt(tempX*tempX+tempY*tempY+tempZ*tempZ);
gravityError=fabs(gravity-gravityG);
if(gravityError>gravityVibrationGate)// avoid vibration
weight=1;
else{
weight=weight2+(weight1-weight2)*gravityError/gravityVibrat ionGate;//动态权重
}
time=micros()-gyroTimer;//micros()用于读取当前的时间，单位us gyroTimeTest=time;
angleDelta=(testGyroZ1-TLYLDZ1)*5/4096*1.5/5.1/GyroSense2*time/1000;/ /自制的陀螺仪，放大倍数=5.1/1.5
angleFinal=angleA*(1-weight)+(angleDelta+angleFinal)*weight;//这就是互补滤波了
在这里weight的变化范围是0.98~0.995
黑线提取
黑线提取我采用的是跳变沿检测+动态阀值，好处是可以减少共模干扰，建议采集的128个点经过一次中值滤波，3点足够，5点的时候边沿已经变得略平滑了，代码如下:
for(i=0,CCDAvr0=0;i<128;i++)
CCDAvr0=CCDa+CCDBuf2;
CCDAvr0=CCDAvr0/128;
FZ=CCDa*FZBL;//阀值=赛道亮度平均值*阀值比例（0.4~0.8）
for(i=LineCenter;i+DCCD<128;i++)//Rblack
{
if(CCDBuf2-CCDBuf2[i+DCCD]>FZ)
Rblack=i+DCCD;
}
值得注意的是边沿检测不一定是两两相邻的点进行比较，因为这样可能会因为边沿特征的不明显而识别失败，建议取3~8个点。

另外上面的搜索中，并不是从64点往两边搜索，而是从上次的centerline往两边搜索。

补线：
if(Lblack>50)
LineCenter=Lblack+trackWidth/2;
else if(Rblack<78)
LineCenter=Rblack-trackWidth/2;
else
LineCenter=(Lblack+Rblack)/2;
至于trackWidth，我采用的是动态值，因为它会随车体的前倾角度二变化，我的小车只用了一个CCD，几次尝试使用一前一后的双CCD检测，最后蛋疼的放弃了，就是因为前瞻和前倾的蛋疼关系。

起跑线的识别，使用的是“夏日的冰”的方法，检测跳变沿个数是否大于5，这个方法非常好用，在此表示感谢，原帖地址：
/thread-141798-1-1.html
时间的获取
在很多地方我使用了micros()、millis()这两个函数，这两个函数来自于Arduino，作用就是利用一个定时器，在单片机开机的时候进行计时，通过这两个函数可以获取当前的时间，unsigned long的格式，micros()70分钟后溢出，millis()50天后溢出，用在小车上足够。

有了这两个函数可以很方便的计算出一段代码的运行时间、gyro的积分时间等，加上SerialChart、XS在线调试，示波器被彻底取代了：）代码：
void PIT_Init(void) {
PITCFLMT_PITE=0; //定时中断通道0 关
PITCE_PCE0=1;//定时器通道0 使能
PITMTLD0=80-1;//8 位定时器初值设定。

80 分频，在80MHzBusClock 下，为1MHz。

即1us.
PITLD0=54321-1;//16 位定时器初值设定。

50ms溢出
PITINTE_PINTE0=1;//定时器中断通道0 中断使能
PITCFLMT_PITE=1;//定时器通道0 使能
PITLD1=5000-1;//16 位定时器初值设定。

2ms溢出
PITCE_PCE1=1;//定时器通道1 使能
PITINTE_PINTE1=0; //定时器中断通道1 中断
}
unsigned long micros() {
return time_us+(54321-PITCNT0);
}
unsigned long millis() {
unsigned long mm=micros();
if(mm>500)
return (54321-PITCNT0)/1000+time_ms+1;
return (54321-PITCNT0)/1000+time_ms;
}
#pragma CODE_SEG __NEAR_SEG NON_BANKED
void interrupt 66 PIT0(void)
{
PITTF_PTF0 = 1; //clear interrupts flag
time_ms_mod+=321;
if(time_ms_mod>=1000){
time_ms_mod-=1000;
time_ms+=1;
}
time_ms+=54;
time_us+=54321;
}
#pragma CODE_SEG DEFAULT
这段代码中，利用PIT0作为定时时钟，溢出周期54321us，而控制周期是5ms，为什么会用这么奇葩的数字，因为我发现如果溢出周期若为50ms,就会导致每隔10次就使得控制中断被忽略一次，没办法，只好增大他们的最小公倍数了。

关于软件，我想写的就这么多吧，时间仓促肯定有遗漏，以后再补充吧，欢迎大家谈谈自己的看法，而不是只留一个“顶”or“路过”~。