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

Malc

大赛结束了，由于时间紧迫，做车的时间很短，所以留下了一些遗憾，趁这段时间有空，整理一下几个月来的资料，既是对自己几个月来的总结，也算上和大家的一些交流吧，这篇文章就谈谈我的平衡车所用到的一些方案和算法吧

首先我用的方案是卓晴老师（在常大看到卓老师啦~）的方案，这次方案是做了一些修改和拓展，比如滤波我采用的互补滤波，速度控制用的是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/gravityVibrationGate;//动态权重

    }

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，这个方法非常好用，在此表示感谢，原帖地址：http://www.znczz.com/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“路过”~

奋斗小青年、

我们的起跑线也很给力   而且不需要更改曝光时间啊

wen123

奋斗小青年、 发表于 2013-7-30 07:21
我们的起跑线也很给力   而且不需要更改曝光时间啊

分享出来不是更好么

wen123

好帖！！lz分享的精神令我佩服！！，你用了这么多的低通滤波会不会有滞后了，，，，我也用了低通滤波没有用你的这么多，我的比例是1：1，不知道你用多大的比例，还有trackWidth动态的和入弯姿势有关也就是你所说的小车姿态角度，这个有点误差的，不知道你怎么处理的，，，，，，建立数学模型算出精确的误差，还是直接算出这个trackwidth再通过trackwidth算出误差，，这样是有误差的，而且会随着丢线的次数而累积误差！！！！！希望交流！

A楼飘房

要向你學習啦。

w717342899

前排支持。。。。。。。。。。。

vip101vip

顶

理工小别

w717342899 发表于 2013-7-30 08:55
前排支持。。。。。。。。。。。

有些东西你还是没考虑到吧 多学学吧 数学很重要哦

wenshuai536

感觉好深奥的样子，刚刚开始接触这个，现在也没办法理解，等我学会了就能理解了吧

暗里着迷

学习了！谢过LZ！