第16届智能车竞赛双车接力组—直立车经验语录

前言

  这是我第一次参加智能车竞赛，也是我第一次接触直立车，开始的时候真的是个啥也不懂的小白，随着时间的推移，对直立车的理解和认知也慢慢地由浅入深，逐渐学到了很多关于PID的理论知识和直立车的特性。这段备赛和参赛的经历非常宝贵，所以我想总结一下这段时光收获的点点滴滴，同时也希望能帮助到大家。这篇博客是我的处女作，有些地方的排版和表达可能不是很合理，也可能存在表述错误的问题，请大家见谅。接下来让我们直入主题吧！

直立环核心控制算法—串级PID

  串级PID算法是我这次比赛最大的收获，在写出这个算法之前我尝试过各种各样的直立环控制算法：最简单的PD控制，清华方案等等。在第一次接触到串级PID时，很不能理解它的控制思路，而且还看不懂流程框图，没法将算法用代码实现，但随着时间的推移和阅历的增加，我在一两个月之后再次看到它时，居然看懂了，并且很快地用代码实现了，但对于这个算法为什么这样进行计算还是不解，在之后调车的过程中才慢慢地心领神会，逐渐理解了这个控制算法的每一步。
  串级PID的核心思想是让环跟环之间形成闭环，也就是级间反馈。这套算法的反馈量和输出量很关键，因为各环的反馈输入量不仅包括传感器的反馈量，还包括各环的输出。下面用流程框图进行详细讲解：

图1.串级PID流程框图

  串级直立PID的最外环是速度环，外环角度，内环角速度，而角速度环是这套算法的核心。作为最外环的速度环不同于并行PID（最简单的三环PID直接相加）的速度环，并行PID的速度环对与直立环属于干扰因素，两个环的输出直接耦合作用于电机会严重影响系统的稳定性，同时也会使PID的参数变得更难整定。而串行PID的速度环属于直立环的一部分，即速度环输出作为角度环输入，角度环输出作为角速度环的输入，最后由角速度环的输出作为直立环的总输出作用于电机，而且由于角速度环的原因，直立环的直立效果极好，调整好的直立环会给你一种车咬住电机的感觉，车的直立环会非常的硬。凡事有利有弊，虽然串级直立环的直立效果极好，但随之而来的问题就是作用于电机的电流很大，但电机运转的幅度很小，也就是电机堵转，导致电机发热现象严重，长时间调试甚至会烧毁电机，所以调车时注意时刻关注电机温度，适当地给电机降降温。串级PID中各环的嵌套其实是各个量之间的量纲转换，速度环的输出去作为角度环的输入参量，其实是把速度变化转换为角度变化，而角度环的输出去作为角速度环的输入，是把角度变化转换为角速度的变化，最后由角速度环将所有的变化进行统一计算，然后将输出作用于电机，这样就不存在速度环与直立环的输出互相干扰的问题。
注：在这里对上图中的参量做一些说明，以使大家能顺畅地写出这个控制算法。俯仰角度是由两个轴的加速度数据通过反正切函数计算而得，角速度就是陀螺仪的数据。

转向环控制算法

  转向环的控制我没有找到非常好的控制算法，因为转向环的控制算法历来都是各大强校代码的核心部分，所以我就用了最简单的PD控制，但在比赛前的一段时间听说过串级的转向环，串级的直立环非常稳定，可想而知，串级转向环的效果肯定也会让人大吃一惊，可惜临近比赛，我没有多余的时间去尝试这个方案，在这里说一下大概的思路（不一定全对，我也是听别人随口一说，自己想了想感觉还是有道理的）：车模绕车轴的旋转运动其实是在垂直于车轴的平面内（这里称为垂直面）的运动，在这个平面内车模进行旋转运动时会产生角速度变化和加速度变化，同理，车模跑动时的转向也可以近似地看作绕一根轴线的旋转运动，只不过此时的旋转运动是在平行于地面的平面（这里称为水平面）上发生的。既然车模在垂直面上做旋转运动时会产生加速度和角速度变化，那么在水平面上做旋转运动时也会产生加速度和角速度的变化。我的想法是内环和外环还是像串级直立环那样嵌套，最外环为普通的偏差转向环。还有一种就是动态的转向环，将转向环的P和D跟速度环进行关联，让P和D随速度的变化而变化，可以去尝试一下。

图2.车模三维空间坐标系简图

算法框架

  主要的计算控制都放在一个2ms的定时器中断函数里，也可以采用5ms的定时中断，中断触发时间取决于代码的计算量和芯片的处理速度。在进行算法计算前需要采集俯仰角、角速度、车轮转速（编码器值）这三个参数，采集函数都放在同一个定时器中断里。各环的PID计算可以封装成函数，以外调函数的方法来进行各环输出的计算，各环的计算顺序如下：
  首先进行速度环的计算，10ms计算一次（官方给的思路是100ms计算一次，个人感觉控制间隔太长了）。输入参量为左右轮两编码器的值，将两编码器的值取绝对值再相加，然后再取平均，即为车模当前的运行速度。速度环采用P控制就足够了，理论上应该采用PI控制，但积分项I需要限幅和定时清零操作，用起来比较困难，而且如果处理不好，车模在运行过程中会出现不可预测的情况。
  其次计算角度环，10ms计算一次。输入参量为车模当前的俯仰角和速度环的输出量。角度环采用PD控制，P影响直立控制的响应速度，D可以弥补P的超调现象。
  最后计算角速度环，作为输出直接作用于电机的最内环，角速度环需要高频控制，2ms计算一次。输入参量为角度环的输出量和角速度。角速度环采用PI控制，I可以消除静差，并影响电机的控制力度，即输出给电机的电流，P可以消除I产生的低频大幅抖动。

  以上是直立环各环的计算顺序。另外，速度环的输出需要限幅后再传给角度环，角度环的输出也需要限幅后再传给角速度环。

  方向环的计算放在直立环计算前或者计算后都可以，因为方向环和直立环是独立开的，两环的输出都会在最后共同作用于电机。方向环采用PD控制，P影响车模转向的灵敏度，D可以弥补P的超调现象，有效改善车模连续转向时的晃动情况。

/*速度环&角度环计算&方向控制-10ms一次*/
	if(TIM_COUNT_10ms==5){
		TIM_COUNT_10ms=0;
		
		/*速度环*/
		Speed_PWM = Speed(Encoder_Left,Encoder_Right);
		/*角度环*/
		Angle_PWM = Angle(angle_out,Speed_PWM);
		/*方向环*/			
		ImgDirec_PWM = ImgDirec(XK.error);
	}
	/*角速度环计算-2ms一次*/
	Angspeed_PWM = Angspeed(icm_gyro_y*0.005,Angle_PWM);

	MotorOutput(Angspeed_PWM,ImgDirec_PWM);	//电机控制

这里推荐给大家一篇具有启蒙意义的技术报告：中南民族大学技术报告

搭车方法

  对于直立车而言，好的控制算法固然重要，但机械结构才是重中之重，算法和机械至少“三七分”，七分机械，三分算法，这一点是我的亲身经历，备赛过程中就是在车的结构上走了不少弯路。直立车的机械结构应该遵循“质量集中、重心低、质量轻”这十字箴言。质量集中是为了减少转动惯量，电池作为主要重物应尽量离车轴近；重心低是为了防止过弯侧滑，能够使过弯更平滑顺畅；质量轻是为了便于控制，减少整车惯性。搭车时应尽量使重心靠近车轴，最好压在车轴上，建议采用V字形车体结构，前后的臂展要适中，质量分布尽量均匀，就是无论车在哪个角度位置都不会出现明显的加速倾倒。车体的自然直立姿态最好“抬头压尾”，就是车倾倒时会自然地往后倒，车体的前端留出较大的空余角度空间，因为前端留出的角度空间越大提速空间越多，上坡时越不容易卡坡。最后也是最重要的一点，车体结构一定要有较好的机械零点，车要能在机械零点位置立住一段时间，立住的时间越久越好，当然，如果能够一直立住那再好不过了。

波形拟合

  在整定各环参数之前需要做一个准备工作—滤波拟合。陀螺仪传回来的原始数据抖动很大，计算出的俯仰角数据抖动自然也大，波形上的体现就是有许多小幅度的尖峰跳变。滤波算法有很多选择，最经典的应属互补滤波和卡尔曼滤波了，我用的是卡尔曼滤波，将初步计算的俯仰角和Y轴陀螺仪数据（具体哪个轴取决于你陀螺仪怎么安放）进行拟合从而得出滤波后的俯仰角。滤波拟合出的俯仰角波形要圆滑，并且波形的跟随性（与原始数据波形的重合度）要好。

图3.车模规律摆动下的正确拟合波形

图4.车模突然大幅摆动的正确拟合波形

  可以用各种不同的摆动方式摆动车模以观测波形是否正确。
  如果波形不够圆滑，那么俯仰角数据就会存在突变现象，如果跟随性不好就会导致俯仰角数据反馈滞后，车模控制就会有迟滞性。

图5.跟随性不足的拟合波形

图6.圆滑度欠缺的拟合波形

具体怎么拟合我就不细说了，卡尔曼滤波还是挺复杂的，直接看下面吧：

/*Q值为过程噪声，越小系统越容易收敛，我们对模型预测的值信任度越高；
但是太小则容易发散，如果Q为零，那么我们只相信预测值；
Q值越大我们对于预测的信任度就越低，而对测量值的信任度就变高；
如果Q值无穷大，那么我们只信任测量值；

R值为测量噪声，太小太大都不一定合适。
R太大，卡尔曼滤波响应会变慢，因为它对新测量的值的信任度降低；
越小系统收敛越快，但过小则容易出现震荡；
测试时可以保持陀螺仪不动，记录一段时间内陀螺仪的输出数据，
这个数据近似正态分布，按3σ原则，取正态分布的(3σ)^2作为R的初始化值。

测试时可以先将Q从小往大调整，将R从大往小调整；先固定一个值去调整另外一个值，看收敛速度与波形输出。

系统中还有一个关键值P，它是误差协方差初始值，表示我们对当前预测状态的信任度，
它越小说明我们越相信当前预测状态；
它的值决定了初始收敛速度，一般开始设一个较小的值以便于获取较快的收敛速度。
随着卡尔曼滤波的迭代，P的值会不断的改变，当系统进入稳态之后P值会收敛成一个最小的估计方差矩阵，
这个时候的卡尔曼增益也是最优的，所以这个值只是影响初始收敛速度。*/

float LowPassFilter_kalman(float datan, int w){
	static int Xlast=0;
	static int P=5;
	float Q=0.3;	//过大噪声严重，越小越平滑但跟随性会下降
	int R=40;
	float S=0.045;	//角速度积分时间，影响跟随性
	float Kg, PP;
	float Xnow,x;
	x=datan*100;
	PP=P+Q;
	Kg=PP/(PP+R);
	Xnow=Xlast+Kg*(x-Xlast);
	P=(1-Kg)*PP;
	Xlast=Xnow+w*S;
	return Xnow;
}

另外推荐给大家两篇关于卡尔曼滤波的博客：
卡尔曼滤波算法原理、C语言实现及实际应用
卡尔曼滤波中关键参数的调整

  这里给大家提供一个方便的波形调试工具——山外上位机，这个上位机不仅仅可以观测波形还有很多其他辅助调车的工具。与上位机的通信代码如下：

/*本文件函数用于与山外上位机通信*/

//串口发送一个字节
//void UART_send_byte(uint8 byte) 
//{
//    while(!((USART1->ISR)&(1<<7)));
//    USART1->TDR=byte;    
//}

void uart4_putbuff(uint8 *data,uint32 size)
{
    uint8 i=0;
    for(;i<size;i++)
    {
        uart_putchar(UART_4,*(data+i));
    }
}

void vcan_sendware(uint8 *wareaddr, uint32 waresize)
{
    uint8_t cmdf[2] = {0x03, 0xfc};    //串口调试 数据包帧头协议
    uint8_t cmdr[2] = {0xfc, 0x03};    //串口调试 数据包帧尾协议

    uart4_putbuff(cmdf, sizeof(cmdf));    //先发送前命令
    uart4_putbuff(wareaddr, waresize);    //发送数据
    uart4_putbuff(cmdr, sizeof(cmdr));    //发送后命令
}

下载链接：山外上位机

调车方法

  直立车的调车思路都是一样的，直立环是基础，车要先能立住才能跑。但不同于并级PID的直立环，串级直立环包含了速度环，速度环是直立环的一部分，而并级PID的速度环和直立环是相互独立的。串级直立环应先整定角速度环，再整定角度环，最后整定速度环，即整定的顺序自内向外，未整定的各环参数全部置零。

角速度环整定方法

  角速度环采用PI控制，增量式和位置式的PID都可以，我用的是位置式。角速度环的整定方式有点特殊，需要先调I再调P。因为陀螺仪的角速度数据抖动非常大，十分容易出现静差，需要加上i来消除那部分静差，调整的现象是加上i车就能大幅的低频来回晃，能勉强立住，立住的效果越好i的参数越好，i过大也会导致高频抖动；在大幅度低频抖动的情况下逐渐增加P直至低频抖动消失，然后再缓慢增加P直至出现轻微的高频抖动，适当减小一点P或I至高频抖动消失，至此角速度环整定完毕。过大的P和I都会引起高频抖动，整定好的角速度环已经基本可以直立了，但还不具备直立姿态的自动调节能力，车体姿态在机械零点附近时车就能保持基本直立，而且还很硬。整定好的角速度环当双手抓住轮子时给车体一个倾角，车就会缓慢地向一侧倾倒，可以用这种抓住车轮让车体动的方法检验角速度环是否已整定好。

角度环整定方法

  角度环采用PD控制。将角度环的设定角度（期望角度）设为车的机械零点角度，在角速度环调好的基础上加入角度环的P（数量级大概是角速度环P的一千分之一）后，车就具备了基本的自动调节能力，但会产生较大的振荡，需要加入D（数量级跟P相同）来进行稳定。调整好角度环时，角速度环的P和I参数值大概是原来的60%~70%，所以在调整角度环前可以把角速度环的参数值先调低，也可以在调角度环的时候随时伺机调低角速度环的参数。这么做是因为如果不调低角速度环的参数值，那么即使将角度环参数调节到高频抖动，此时虽然车的直立姿态很硬，但是自我调节的响应速度不够，也就是此时的角度环参数还偏小。可以先将角速度环的参数调低到原来的60%，然后调节角度环的P至车体出现轻微的高频抖动，然后加入D，逐渐增加D至抖动消失，此时可以再微调一下P，然后在车体不抖动的情况下再慢慢增加角速度环的I和P至车体抖动，再降低至车体抖动消失，此时角度环就差不多整定完了，整定参数时，P和D过大都会引起车体的高频抖动。整定时可以拍打车体的前端或者后端来观察车体姿态自我调节的响应速度和振荡幅度，响应速度越快，振荡幅度越小，角速度环的参数越好。整定好角度环后，只要车的机械结构合理，车是不会倒的，好的直立环即使车在运行过程中撞到或者碾过赛道路肩车也不会倒。

  角速度环和角度环都整定完毕，那么基本的直立环也就整定完毕了，可以用检验角速度环的方式检验当前的直立环：抓住车的两个轮子，将车体往前压或者往后压都会感觉到明显的阻力，阻力越大说明直立环越硬，松开手后车体就会缓慢回到设定的角度位置；手抓住两个轮子来回晃车体也不会倾倒，会回到设定的角度位置。

演示视频：串级直立环直立效果

速度环整定方法

  速度环按照理论来说应该采用PI控制，但I比较难用，需要限幅和定时清零操作，否则车的运行会出现不可预料的错误，所以I用不好不如不用，纯P控制也足够了。速度环的P参数跟设定的速度有关，成反比关系。合适的P可以让车在设定速度下稳定地运行。P过大时，车体姿态会先往前倾从而快速加速，然后车体姿态会往后倾倒从而进行减速，如此往复，也就是所谓的“点头车”；P过小时，车从静止开始运行提速会很慢甚至跑不起来，给车推一把有可能会导致车一直加速直至前倾倒地，这是P过小时的两个常见现象。另外，速度环跟转向环虽然没有关联，但是会有互相影响，如果想提速，需要增加设定速度，减小速度环的P，使加速平缓，而转向环就需要更加灵敏，同时增加P和D，同时角度环的P和D也需要调小，使转向更加稳定，否则车在转向时会很僵硬，转向迟缓，但过小会导致车在出弯的时候转向过冲，会撞向赛道的一边。

转向环整定方法

  转向环采用PD控制，先整定P参数，P过小时转向的转角不足，转向时会撞向弯道外圈，P过大时转向时会内切，当车能够稳稳当当地跑完一个环形弯，P参数基本整定完毕。在只有P的情况下出弯时车体会有明显的左右晃动，这是因为单纯的P控制振荡较大，加入D能使车的振荡幅度减小。D的值大概是P的15~20倍，D参数可以调的粗一些，P调好后直接猛加D，D过大不会引起左右晃动，但会导致曲线预判超前，车还没出弯可能就撞上了弯道外圈，给人一种出弯时转不过来的感觉。

其他问题

  如果车在跑的过程中有抖动，可能是直立环调得太硬了，需要适当调小角度环的P，调大D。

END

  直立车是个很有意思的控制模型，对于PID的运用非常灵活。研究直立车能加深对PID算法的理解和运用，也能间接锻炼耐力和心态，因为后期的调参过程非常无聊乏味，而且直立车是个非常敏感的控制系统，车体结构一旦变动就会导致重心偏移，机械零点的位置就会改变，车体机械零点对应的俯仰角也就随之改变了，这样的话各环参数都需要重新整定，一次两次还好，但是次数多了，心态崩了也是再所难免的（作为一个过来人，调参过程中的辛酸泪就不多说了T-T）。所以建议大家在准备调车前就敲定车模的机械结构，并且把机械结构搭得牢固可靠一点，不会轻易被撞散架的那种（有个不成熟的想法，可以试着去跟麦轮或者越野碰一碰，看看谁的头更硬^_）。实践出真知，越是敢于尝试越能收获意外惊喜。希望这篇文章能帮到看到这里的每一位朋友！