运动控制系统
一、舵机控制
1、理论介绍
舵机和电机都是通过PWM来控制的，只不过舵机控制的PWM的频率是固定的（50-60HZ），通过改变占空比来实现打角角度。
舵机的使用多用于四轮车，当然当我们选择三轮车的时候，前面的轮子可以无需选择舵机，甚至例如我们现在使用的电磁三轮车，前面的轮子用小钢珠代替，可以直接省去舵机的使用。
再次说明 通过改变占空比来实现舵机的打角角度。
很多模友错误以为:“数码舵机的PWM驱动频率300Hz比模拟舵机的50Hz高6倍,则舵机电机转速快6倍,所以数码舵机的反应速度就比模拟舵机快6倍”。这里请大家注意占空比的概念，脉宽为每周期有效电平时间，占空比为脉宽/周期的百分比，所以大小与频率无关。占空比决定施加在电机上的电压，在负载转矩不变时，就决定电机转速，与PWM的频率无关。
（1）模拟舵机
是直流伺服电机控制器芯片一般只能接收50Hz频率(周期20ms)~300Hz左右的PWM外部控制信号，太高的频率就无法正常工作了。若PWM外部控制信号为50Hz，则直流伺服电机控制器芯片获得位置信息的分辨时间就是20ms，比较PWM控制信号正比的电压与反馈电位器电压得出差值,该差值经脉宽扩展(占空比改变,改变大小正比于差值)后驱动电机动作,也就是说由于受PWM外部控制信号频率限制最快20ms才能对舵机摇臂位置做新的调整。
（2）数码舵机
通过MCU可以接收比50Hz频率 (周期20ms) 快得多的PWM外部控制信号就可在更短的时间分辨出PWM外部控制信号的位置信息，计算出PWM信号占空比正比的电压与反馈电位器电压的差值，去驱动电机动作，做舵机摇臂位置最新调整。
结论:不管是模拟还是数码舵机，在负载转矩不变时，电机转速取决于驱动信号占空比大小而与频率无关。数码舵机可接收更高频率的PWM外部控制信号，可在更短的周期时间后获得位置信息.对舵机摇臂位置做最新调整。所以说数码舵机的反应速度比模拟舵机快，而不是驱动电机转速比模拟舵机快。
2、代码实现
//-------------------------------------------------------------------------------------------------------------------
// @brief 舵机初始化
// @param NULL
// @return void
// Sample usage:
//-------------------------------------------------------------------------------------------------------------------
void servo_motor_init()
{
pwm_init(PWMB_CH1_P74,200,3650);//pwm初始化频率为50hz 舵机居中 值越小，舵机初始位置时越向右偏转
}
//-------------------------------------------------------------------------------------------------------------------
// @brief 舵机控制
// @param duty 调整占空比使舵机旋转相应角度
// @return void
// Sample usage:
// Angle = (t-0.5)/(2.5-0.5) * 180° = （PWM*2*10^(-3) - 0.5）/(2.5-0.5) * 180°
// Angle:[0,180] PWM:[250,1250]
//-------------------------------------------------------------------------------------------------------------------
void servo_motor_ctrl(float duty) //调整占空比使舵机旋转相应角度
{
if(duty > 350) //22.5°
duty = 350;
if(duty < -350)
duty = -350;
pwm_duty(PWMB_CH1_P74,3650+duty);
}
二、电机控制
1、理论介绍
电机的频率没有指定范围，但速度也是通过调节占空比来控制的，通过调节电机pwm输出占空比完成对电机速度的调整，电磁组需注意电机对电磁信号会有干扰，建议电机控制的pwm频率设置为13-19khz，以尽可能的消除干扰。
而电机是控制智能车速度的重中之重，要使电磁车跑的非常快、非常稳定，那么电机的速度调参必须要非常的严谨！！！
首先，对智能车的运动控制有一个大概的初步认识结构：
通过以上而知，要想准确的实行对智能车的运动控制，我们必须采用编码器测速反馈和电机速度的执行，因此，在具体的代码实现中，我们主要是针对这两项便可以直接实现小车的运动。
而为了确保电机的正常运转，我们需要使用PID对其进行控制，而且这类设计PID运算的函数一定要放在定时器中断内保持控制周期的稳定
2、电机代码控制
//-------------------------------------------------------------------------------------------------------------------
// @brief 电机控制
// @param duty 配置电机正反转及转速
// @return void
// Sample usage: motor_ctrl(3000,3000);
//-------------------------------------------------------------------------------------------------------------------
void motor_ctrl(int16 l_motor_duty,int16 r_motor_duty)
{
if(l_motor_duty >= 0) //正转
{
P60 = 1;
pwm_duty(PWMA_CH2P_P62, l_motor_duty);
}
if(l_motor_duty < 0) //反转
{
P60 = 0;
pwm_duty(PWMA_CH2P_P62, -l_motor_duty);
}
if(r_motor_duty >= 0) //正转
{
P64 = 1;
pwm_duty(PWMA_CH4P_P66, r_motor_duty);
}
if(r_motor_duty < 0) //反转
{
P64 = 0;
pwm_duty(PWMA_CH4P_P66, -r_motor_duty);
}
}3、编码器速度反馈
//-------------------------------------------------------------------------------------------------------------------
// @brief 获取编码器脉冲数
// @param
// @return
// Sample usage:
//-------------------------------------------------------------------------------------------------------------------
int get_l_speed()
{
int16 templ_pluse = 0;
templ_pluse= ctimer_count_read(SPEEDL_PLUSE);
ctimer_count_clean(SPEEDL_PLUSE);
if (1 == SPEEDL_DIR)
{
templ_pluse = abs(templ_pluse);
}
else
{
templ_pluse = -abs(templ_pluse);
}
return templ_pluse;
}
int get_r_speed()
{
int16 tempr_pluse = 0;
tempr_pluse = ctimer_count_read(SPEEDR_PLUSE);
ctimer_count_clean(SPEEDR_PLUSE);
if (1 == SPEEDR_DIR)
{
tempr_pluse = -abs(tempr_pluse);
}
else
{
tempr_pluse = abs(tempr_pluse);
}
return tempr_pluse;
}三、PID控制算法（重点！！！自动化专业必修！！！）
1、PID理论控制算法
整个智能车基本上就在做两件事，一个是方向的控制，一个是速度的控制 （当然平衡车还多一个平衡的控制），只要能够将这两个量给控制的恰到好处 （恰到好处不光是两者单独控制的很好，而是两者一起工作时配合的很好） 控制器中仅引入"比例项KP"往往是不够的，比例项的作用仅是放大误差的幅值， 而目前需要增加的是“微分项Kd”，它能预测误差变化的趋势具有“比例 + 微分”的控制器， 就能够提前使抑制误差的控制作用等于零，甚至为负值，从而避免了被控量的严重超调
（1）舵机角度计算（电磁）
1 、采集方向传感器的信息，比如对于摄像头就是进行图像的采集；
2 、方向偏差信息的求取，对于摄像头来说就是图像中线的求取，进而得到得到车体当前位置与理想位置的偏差；
3 、方向控制模块根据这个偏差来计算应该输给舵机的 PWM 占空比，进而来调整车体的前进方向。
（ Proportion 比例 +Differential 微分 +Integral 积分）
输出量 =P* 误差 +D* 误差的变化 +I* 误差的积分
PD （比例微分）控制， Ki 为 0 考虑偏差及偏差的变化趋势，当误差为error 时，输出的占空比是 DUTY+Kp*error+Kd*(current_error-last_error) 。可以看出，对于同样的一个偏差：
1、若偏差正在减小的过程， PD 控制的量要比纯比例控制量小一些，这样能够避免过度控制
2、若偏差在逐渐增加的过程， PD 所要施加的量比纯比例要多一些，目的是为了抑制误差增大的趋势。所以 D 这一项有一些预测控制的味道 ，相比 P 而言要更智能一些。
(2)速度控制（增量式）
△u(k)=(Kp+Ki*T+Kd/T)e(k) - (Kp+2Kd/T)e(k-1) + Kd/T * e(k-2)
PID 就是一种快速而准确地将系统的某一状态调节到设定值并保持稳定的控制算法。
比如，我的小车现在是 0m/s 的速度，我想让它的速度稳定在 3m/s ，那我用 PID 算法就很容易做到。
当然这一切的前提就是安装了编码器，车速有反馈，只有加上编码器，有了反馈，才能组成一个闭环系统。
当然您也可以加上码盘，或者霍尔开关等一切可以返回车速的东西都可以。
2、PID代码（自动化专业的必修）
（1）舵机角度控制
void dir_ctrl(struct PID *dir,int16 err)
{
float Kd,Delta,KpOut;
KdOut = 0;
dir->err = 1.5*err;
KpOut = 1.0*dir->P2*dir->err*abs(dir->err)/dir->T2 + dir->p*dir->err;
Kd = dir->d + 1.0*abs(dir->err)*abs(dir->err);
Delta = dir->err - dir->last_err;
KdOut = Kd * Delta;
// if(KdOut>1000) KdOut=1000;
// if(KdOut<-1000) KdOut=-1000;
// if(abs(dir->err)<=3)
// {
// KdOut = 0;
// }
dir->output = (KpOut + KdOut)/dir->T1 ;
// if(abs(err)<5)
// {
// if(dir->output > 30)
// {
// dir->output = 30;
// }
// }
dir->last_err = dir->err;
servo_motor_ctrl(dir->output);
}(2)速度控制
void speed_ctrl(struct PID *speed,int16 cur_speed,int16 target)
{
speed->err = target - cur_speed;
speed->output += speed->p*(speed->err-speed->last_err)+ speed->i*speed->err+ speed->d*(-2*speed->last_err+speed->err+speed->llast_err); //TD微分时间常数为1
speed->llast_err = speed->last_err;
speed->last_err = speed->err;
if (speed->output>8000) //输出限幅
speed->output = 8000;
}四、分享总结
这三篇文章分别介绍了赛道信息获取及转向原理、 电感寻迹及元素判断、运动控制系统，这三部分就是电磁车的主要核心内容。其中从专业的角度出发，PID控制对于自动化学子是重中之重，建议每一位同学都能够认真学习并且领悟其中的内涵。
感谢各位同学们的支持！！！
2255
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
