[智能车]平衡车/直立车的入门经验(代码讲解)

        做为第十六届智能车的FW,在半年的做车经历中把能踩的坑都踩了个遍。写这篇文章是为了留个纪念,也是为了帮新车友快速入门(可能完全0基础)。我自己的经验也不足,所以可能会存在一些漏洞,还请大佬指正。

车模的搭建

        在调试的时候,我们多次换过车模的结构,最后发现车模的重心越低越好,质量分布要尽可能的集中。不到不得已的时候不要加配重,把车沿轴的转动惯量降到最低。车模结构还是挺重要的,重心低点能明显感觉车更稳。这是对电路设计也要提出一定的要求,省赛的时候见到有一辆车把电路集成到了一块板子上,而我们是三块板子(还非常大),所以更难搭出好的结构。新手设计的话没必要整那么难的,但要提前计划好每块板子的位置,尽可能的精简。我不是写程序的吗,怎么在考虑电路(bushi

(下图不是最终版,但比较接近) 

[智能车]平衡车/直立车的入门经验(代码讲解)_第1张图片

 车的整体方案

        我自己是主要负责车的程序,所以就细说怎么让车动起来。我们的车是采用了最简单的并级PID,电磁引导的方案。(太菜了,摄像头不会,串级PID也不会QAQ)核心是用的灵动的MM32,Cortex-M0内核,后换到M3内核(笑死,3080都能买到,这个芯片买不到)。陀螺仪用了龙邱的MPU9250,后换到逐飞的ICM20602(问就是龙邱垃圾)。双车的通讯模块是Lc12s(问就是便宜)。

        来简单说一下程序的整体框架。智能车程序和学弟们最开始学的51还是有些区别,底层的驱动函数已经是由商家帮忙写好了,不需要再从头开始用寄存器。下面的程序基本适用于任何芯片,只要理解每个函数的具体含义,以及程序整体的框架,套用不同芯片的具体例程,就能快速上手。我在给变量命名的时候基本没用缩写,所以还是比较容易看懂的。

//其他器件初始化一下就不用动了。最重要的是这三个定时器
tim_interrupt_init_ms(TIM_8, 1, 0);			//直立环中断	
tim_interrupt_init_ms(TIM_6, 10, 1);		//速度环中断	
tim_interrupt_init_ms(TIM_7, 5, 1);			//方向环中断

         直立车的三个PID就都放在了这三个定时器中,固定时间判断直立,转向和方向的情况。而main函数里最后进到以下程序。不断循环这个就可以。(定时器是需要用到的最基本的中断,就是相当于定了3个闹钟,每个闹钟到点后放下手头活,去做指定的事)

while(1)
	{
        run();            //车的循迹程序
        Branch();         //三岔路程序
		fixedCircle();    //环岛程序
	}
//车模运行控制算法
void run()
{
    if (Vol_left1_actual + Vol_right1_actual > Threshold_circle && err_angle < 10 && err_angle > -10)
        dirControlOut *= 0.05;                                  //屏蔽环岛
    if (Flag_dirPID == open)
        dirOut = dirControlOut;                                 //方向环开关
    
    duty_Left = angleControlOut - speedControlOut + dirOut;     //dirOut为正是左转
    duty_Right = angleControlOut - speedControlOut - dirOut;

    if (duty_Left > 0)            //加电机死区
        duty_Left = duty_Left + duty_death_left_P;
    if (duty_Left < 0)
        duty_Left = duty_Left + duty_death_left_N;
    if (duty_Right > 0)
        duty_Right = duty_Right + duty_death_left_P;
    if (duty_Right < 0)
        duty_Right = duty_Right + duty_death_left_N;

    if (duty_Left > duty_Max_P)    //输出限幅
        duty_Left = duty_Max_P;
    if (duty_Right > duty_Max_P)
        duty_Right = duty_Max_P;
    if (duty_Left < duty_Max_N)
        duty_Left = duty_Max_N;
    if (duty_Right < duty_Max_N)
        duty_Right = duty_Max_N;

    MotorCtrl(-duty_Left, -duty_Right);                         //输出到电机上
}

        duty表示占空比,通过输出不同占空比的pwm波,到电机驱动上,就可以控制电机的转速。循迹函数中最主要的就是将三环的结果叠加到duty_Left和duty_Right,然后输出作用到电机上,其他都是辅助作用,先暂且不管。 接下来我就讲一下这三个环的具体内容。

直立环

        直立环是最重要的一个部分,也是最先开始调的,第一次让车立起来,还是很有成就感的。我主要说一下程序的内容就好了,原理还是得看《第七届全国大学生“飞思卡尔”杯智能汽车竞赛 电磁组直立行车参考设计方案》。上古资料,但是特别详细,B站有配套的高糊视频。简单来说,就是通过采集到陀螺仪的原始数据,解算出车模的实际角度,得出角度误差。kp乘上这个误差(比例控制,就是差的越多,输出越多,很好理解)再叠加kd乘角速度(消除震荡,起到一个阻尼的作用,就是不让它转那么快)。

        另一个比较重要的就是角度的解算,这个的响应速度影响了车控制的平稳。我采用的是一阶互补滤波算法,后换到二阶。下面附上完整代码。

/*************************************** 互补滤波函数 (包含直立环PD)*********************************************/
/*angle_Filtering = K1 * angle_m+ (1-K1) * (angle_Filtering + gyro_m * dt);
angle_Filtering 是融合后的角度值
angle_acc是加速度测量简单计算得到的角度
angle + angle_gyro * dt是陀螺仪积分得到的角度
dtt为采样周期,单位s,就是直立环中断的时间,这里是0.001
K1是滤波器系数,取0.005。
一阶互补滤波也可以看做是加权平均。*/
void ComplementaryFiltering()
{
    if (icm_acc_x >= 0)
        icm_acc_x = -1;                                 //tan90°
    acc_x = - icm_acc_x * 9.8 / 4096.0;                 //转化单位
    acc_z = icm_acc_z * 9.8 / 4096.0;                   //转化单位
    gyro_y = icm_gyro_y / 16.4;                         //转化单位

    anglespeed = - (gyro_y - 0.25);                     //零点偏差
    angle_acc = atan((acc_z) / acc_x) * 57.3;           //加速度得到的角度,可加零点偏差
    
    angle_gyro = angle_Filtering + anglespeed * dtt;    //角速度得到的角度
    angle_Filtering = K1 * angle_acc + (1 - K1) * angle_gyro;
    
    err_angle = angle_Filtering - angle_balance;
    angleControlOut = P_angle * err_angle + D_angle * anglespeed;
}

/*二阶互补滤波(原理是啥?学会了教教我!)*/
void ComplementaryFiltering2()
{
    if (icm_acc_x >= 0)
        icm_acc_x = -1;                             
    acc_x = - icm_acc_x * 9.8 / 4096.0;             //转化单位
    acc_z = icm_acc_z * 9.8 / 4096.0;               //转化单位
    gyro_y = icm_gyro_y / 16.4;                     //转化单位

    anglespeed = - (gyro_y - 0.25);                 //零点偏差
    angle_acc = atan((acc_z) / acc_x) * 57.3;       //加速度得到的角度,可加零点偏差
    
    x1 = (angle_acc - angle_Filtering) * (1 - K2) * (1 - K2);
    y1 = y1 + x1 * dtt;
    x2 = y1 + 2 * (1 - K2) * (angle_acc - angle_Filtering) + anglespeed;
    angle_Filtering = angle_Filtering + x2 * dtt;
    
    err_angle = angle_Filtering - angle_balance;
    angleControlOut = P_angle * err_angle + D_angle * anglespeed;
}

        代码中的一些正负号和陀螺仪的安放位置有关,需要自己看。关于一阶的原理,注释里已经说的很详细了,二阶的我是没看懂QAQ。这个滤波算法就是一些固定的东西,icm_xxx为采集到的原始数据,把它们换一下就可以用了,甚至没必要搞懂原理(做比赛是为了学习,还是多看看好)。

 注:一定要看“清华方案”把原理搞懂,再看代码会轻松一些。

        中断里面长这个样子

void TIM8_UP_IRQHandler (void)
{
	uint32 state = TIM8->SR;														// 读取中断状态
	TIM8->SR &= ~state;																// 清空中断状态

	get_icm20602_accdata_spi();    //读陀螺仪的原始数据
	get_icm20602_gyro_spi();
	//ComplementaryFiltering();
    ComplementaryFiltering2();     //比赛时用了这个,原理可以不会,代进去数就算好了。
}

关于调参

        emmm调参的话也应该看那个清华方案的配套视频,非常详细,大致流程就是先取kd为0,慢慢的加kp,差不多能立住在原地抖动时加kd。慢慢将参数调到车能立在原地且没有剧烈抖动。这时推一下车做测试,如果车要跑很长一段距离才能重新立住,说明参数小了,再慢慢加p和d。差不多稳一些就可以上另外两个环了,跑起来后参数还要略微修改。

速度环

        关于这个,我自己也有些迷惑。我用的就是kp乘上速度误差,叠加ki乘误差的积分,这样简单的实现一个闭环。然鹅大佬们说直立车的速度环应该是开环控制,这个我是没搞明白。直接放上代码吧。

//速度闭环控制算法(PI)
void SpeedControl()
{
    actualSpeed = (speed_Left + speed_Right) * 0.5;
    err_speed = setSpeed - actualSpeed;                         //速度偏差
    speedIntegral = speedIntegral + err_speed * I_speed;        //积分

    if(speedIntegral>3000)  speedIntegral=3000;                 //限幅
    if(speedIntegral<-2000)  speedIntegral=-2000;               //限幅

    speedControlOutOld = speedControlOutNew;
    speedControlOutNew = err_speed * P_speed + speedIntegral;
}

void OutputSpeedControl()                           //速度环平滑输出
{
    float fValue;
    fValue = speedControlOutNew - speedControlOutOld;
    speedControlOut = fValue * (speedControlPeriod + 1) / 10.0 + speedControlOutOld;

    if (speedControlOut > 4500)
        speedControlOut = 4500;                     //限幅
    if (speedControlOut < -4500)
        speedControlOut = -4500;                    //限幅
}
void TIM6_IRQHandler (void)
{
	uint32 state = TIM6->SR;														// 读取中断状态
	TIM6->SR &= ~state;																// 清空中断状态

	speedControlPeriod++;
    OutputSpeedControl();                           //速度环平滑输出	
        
	speed_Left = tim_encoder_get_count(TIM_3);              //左编码器 注意插槽位置!
	tim_counter_rst (TIM_3);
	speed_Right = -tim_encoder_get_count(TIM_4);            //右编码器 
	tim_counter_rst (TIM_4);
    Encoder_accumulate = Encoder_accumulate + speed_Left + speed_Right;
	Encoder_total = Encoder_total + speed_Left + speed_Right;

    if(speedControlPeriod > 10)                       //速度周期控制
    {
    	SpeedControl();
    	speedControlPeriod = 0;
    }

}

        编码器(encoder)是一个在轮子旋转时能产生脉冲的传感器,脉冲频率和车速成正比,这个都有例程的,自己康康就好。这里车实际速度我直接采用了编码器读到的值,这里没必要换算成实际速度。将读到的速度带到这个简单的PI即可。这里用了一个速度的平滑输出,即每10个速度环周期更新一次速度输出,这10个周期内将作用在电机上的占空比逐步递增/减过去。

方向环

        电磁引导是采用电感与赛道上20kHz的电磁线,产生互感,再将信号整流放大,得到能被单片机检测到的电压。一个车上安装3-5个电感,根据这几个电压强度来判断当前车的位置。电感与电磁线所成的角度,距离都会反映到这个电压值。所以合理安排电感就能判断赛道上不同的元素。

        这个采用了PD。。。反正我是没理解这个D参数怎么影响运行效果。而且我们的方向偏差就是生硬的采用了最外边两个电感的差值,速度快的时候效果不好。(第十六届只要完赛就是成功,还轮不到比速度QAQ)有一些比较好的算法,比如差比和什么的,自己去研究吧,我太菜(懒)了。

//方向闭环控制算法(PD)
void directionControl()
{
    dvar = BiasIndActual - BiasIndActual_last;
    dirControlOut = P_direction * BiasIndActual + D_direction * dvar;
    BiasIndActual_last = BiasIndActual;
}

最后

        这次比赛我是学到了不少东西,但是有因为经验不足,耽误了太长时间,导致没有好的成绩。当初我让车动起来就花了2个月的时间,从一脸懵逼到上海之旅,感触还是挺深的,也希望这个文章能快速的帮小白入门(理解了之后就没那么难,M3的芯片从上手到去比赛只调了20个小时,相当于重新做了个直立车)。当然,我发现就算有学长帮助,也会把能踩的坑都踩一遍2333,这才能真正学习到吧。

        完整代码我整理好之后放上来。

        

        没想到有好多小伙伴想看看完整代码,但是我一直懒得上传hhhh。放到GitHub了,gitee太拉跨,一次只能传20个文件。

https://github.com/fomalhaut251/Balance-car.giticon-default.png?t=M0H8https://github.com/fomalhaut251/Balance-car.git


/********************************************************************************
 * 致谢:真诚感谢李子昊,刘希文,曾繁钦,高集殊,季炜,雷超等学长学姐对我们提供的支持与帮助.
 * 		感谢一起交流的外校同学,尤其是中国矿业大学徐海学院的同学,在交流中学习到了很多.
 * 		感谢往届的车友前辈,许多时候还是靠祖传方案才能快乐逮虾户.
 * 
 * 说明:作者水平有限,采用了上古时期多个PID结果直接相加的方案.
 * 		程序的变量名都比较长,但也便于理解,注释较全,希望能帮到学弟们.
 * 		仅限初学者把直立车立起来走两步,元素处理也就图一乐,后续可能更新串级PID方案.
 * 		欢迎技术交流讨论.
 * 
 * 版权信息:
 *	中国矿业大学 机电工程学院 不显山车神队
 *  马梦雅,刘宇鑫,张书晨,陈振尧.
 *  (C)Copyright CUTM, 2021. 
 *  ALL RIGHTS RESERVED.
********************************************************************************/

你可能感兴趣的:(单片机,嵌入式硬件,mcu)