直立的控制
传感器
- 加速度计:MMA7361
- 比较精确,但是干扰比较大、波形比较震荡
- 陀螺仪:enc-03
- 稳定,但是有零漂
测量值
- 加速度计:acc
- AD输入
- 陀螺仪:gyro
- AD输入
角度计算方法
互补滤波
- 将角速度积分输出值和加速度输出值按照一定的比例加权相加,即可得到真实的输出值
互补滤波程序理解
测定小车转过的位移:计数器只计数,D触发器测定方向
角度计算函数:
二阶互补滤波
void AngleCalculate(void) 这个角度计算函数运用了以下方法
这些方法可以滤波、平滑等GravityAngle = (VOLTAGE_GRAVITY-GRAVITY_OFFSET) * GRAVITY_ANGLE_RATIO
将得到的所有的值与理论值的差值扩散到 0 —- 180 之间
GravityAngle = FIRZ(GravityAngle);
将差值对应的角度与历史的8次进行均值滤波,得到新的差值,并将数组里的最古老的值删除,依次类推
GyroscopeAngleSpeed = (VOLTAGE_GYRO-GYROSCOPE_OFFSET) * GYROSCOPE_ANGLE_RATIO;
将陀螺仪得到的值与理论值相减乘以一个系数得到待处理的差值
CarAngle = GyroscopeAngleIntegral;
将陀螺仪得到角度差值的积分赋值给车的角度控制变量
fDeltaValue = (GravityAngle - CarAngle) / GRAVITY_ADJUST_TIME_CONSTANT;
将重力加速度计得到的数值与车的角度控制变量相减,除以一个常值系数
GyroscopeAngleIntegral += (GyroscopeAngleSpeed + fDeltaValue) / GYROSCOPE_ANGLE_SIGMA_FREQUENCY
将上一步得到的互补滤波的值与陀螺仪的差值和上一步中陀螺仪计算的积分值相加,除以采集频率,得到下一步的积分值
卡尔曼滤波
角度计算流程
- 计算加速度计芯片在竖直时的的输出值
- 调节陀螺仪里零偏,即调节陀螺仪的输出值,使其在静止的时刻的芯片管脚电输出电压等于陀螺仪参考电压输出值(在这里对陀螺仪的输出电压进行去直流后放大,否则采集到的值太小)
- 将小车运行过程中采集到的acc、gyro进行输出,分别减去2个值的静态偏差值,用互补滤波得到输出的角度
- 将角度作为输出,一直给设定值,
e(k)=setangle-actualangle
,用PID控制得到实际电机输出(常用PD即可) - 在调节的过程中
- 应先调节P、再调节D
- 如果小车不能产生震荡,并且沿一个方向加速,则有以下两者可能
- P的值太小
- PID输出极性弄反了(可能性较大)
速度的控制
- 方案一:速度闭环采用增量式PID控制(常用PI)
- 将设定输出与速度输出的差值作为误差,利用增量式PID算法得到速度闭环的输出值,直接叠加到角度(直立)PID的输出中去
- 调节方法:
- PI:先调I、再调P(直立车参考视频)
- PD:先调P、再调D
- 注意:在调节的过程中需注意速度环极性,应保证轻推小车,小车会减速
- 方案二:双闭环控制
- 原理:直立车的每个倾角对应一个速度,因此,将角度作为对应速度的输入,经过角度PID控制。可以实现双闭环控制
- 将速度控制环的输出作为PID的设定角度输入值
- 在该处速度PID输出得到设定的角度的过程为增量式PID控制(常只用PD)
- 注意:调节过程中注意极性
速度环调车心得
小车直立环调节好之后,直接加速度环时,需要判断速度闭环极性,具体现象为:如果小车震荡,但最终会减速,则小车的速度闭环极性正确;如果小车向一个方向加速,则速度极性不正确,需要将速度的增量式输出变号之后再叠加到直立环上去(切不可相信网上的什么关闭直立环,单独观察速度环的极性判断方法!)
调试过程中,如果出现小车被推出去又回来、并最终停止的情况,是正常的现象;出现小车向后推,会减速至0,渐渐停止,但是向前推,会减速到0,再反向加速,再减速至0的过程,也属于正常现象。
转向控制
传感器:TSL1401
- 可以采集128个点的灰度值,通过AD转换传给单片机
- 曝光时间为10ms~100ms,曝光时间越大,采集得到的灰度值越大(灰度值是在曝光时间内通过曝光得来),因此可以在暗处增加曝光时间,在亮处减少曝光时间
转向计算流程与步骤
转向原理
- 忽略电机特性导致的差异时,左右轮电机输出PWM波占空比相同时,小车直立前进,输出不同时,小车转弯。
图像采集与处理
- 线性CCD采集到1X128的灰度数组
- 二值化处理,即找到数组的阈值
- 中线提取
- 中线提取算法有许多,如附近搜索、otsu等
* 学长寻线
转向方法:
- 需要获取的值
- 上次的阈值T_old
- 上次的左右线位置lst_old,rst_old
- 本次标志位(若在128个点内没有2个相邻值将阈值夹在中间,则标志位置1)
- 本次读取到的128个像素值
- 上次计算得到的转向参数值diff_old
- 需要获取的值
本次阈值获取方法
- 得到本次128个点的max,min
- 计算本次的阈值初值T = max * 0.6
- 得到本次最终的阈值Tf = T * 0.1 + T_old * 0.9
本次左右线起始和宽度搜寻方法
- 搜索的起始点:lst = lst_old+5,rst = lst - 5
- 限幅 : lst = (lst > 127)?lst:127,rst = (rst <0)?rst:0
- 开始搜索,左边向左搜寻,直到采集到的像素点小于阈值时停止,右边同理
计算左边位置和右边位置(用于寻找中线以及转向控制的直接参数)
- 若标志位为1,则对应得到左边线和右边线的计算位置,否则放在边缘
- PD控制
如果是希望小车严格按照中线走,则中线的目标值为64(灰度数组下标),对中线偏差进行PD控制,利用差速得到左右电机输出,叠加到速度双闭环的控制输出中,即可使小车在保持一定速度的前提下发生转向
转向PD参数的调整
- 弯道类型可以根据小车转向的P、D值大致判断
- 当连续转弯时,应使小车速度大一点或者小车转向的P、D值小一点;当过直角弯时,应使小车的速度小一点或者P、D值大一点。
- CCD的前瞻越大,P、D值应该调得越小。
模糊控制
PD模糊表的建立
转向时,为加快响应速度,同时防止小车冲出赛道,可以根据转向算出的PD项的大小(中线位置的变化得到的P、D的值),将小车运行状态分为不同情况,以下为大概的对应关系 P>0,D>0向左入弯 P>0,D<0向左出弯 P<0,D>0向右入弯 P<0,D<0向右出弯 1. 当入弯时可以增大kP、kD值切内弯,出弯时可以减小kP、kD值防止拐得太过,因此可以建立符合以下规则的模糊表 1. Kp |大 小 | |小 大 | 2. Kd |大 小| |小 大| 当检测到P>0,D>0,即入弯时,可以取矩阵右下角的值, 当检测到P>0,D<0,即出弯时,可以取矩阵右上角的值,
模糊表的建立与参数调整方法
- 建立一个所有参数都相同的kP矩阵,这是在直道上适应得比较好的值
- 检测在入弯处算出的P、D值,得到kP、kD采取的下标,调节对应位置的kP,使系统有较好的响应。
- 以此类推
周期设置
一个周期0.4ms
每1个周期设置1次目标速度,一开始调速度闭环时,可以将速度设置为0
每3个周期读一次加速度计和陀螺仪的AD值,得到偏移的角度
每4个周期读一次编码器的值
每240个周期改变一次速度控制量
每24个周期读一次CCD值
每24个周期计算一次位置,并控制方向(与上面时序相差1个周期)
电路部分(线宽)
- 信号线:10~14
电源线:7.2V:120(电机)、50(2596)
- 5V:30(主干)、20(枝干)
- GND:刚出来是50、(树状)枝干30,给每个元件:20
- 驱动板分开画的时候,GND一般大于100(学长118)
过孔: 内径—外径
10---15、15---20(信号)、20----25/30(电源)、40---45
- 圆弧(无必要)
铺铜:power signal driver(尽量分开铺铜)
后期测试方向思考
存储CCD得到的图像及其相应处理得到得到的值
- CCDData[128]
- 中线位置(下标值)
- 中线提取的P(中线变化量)
- 中线提取的D(中线变化量的变化量)
- 单片机算法得到的阈值
- 其他
利用采集得到的CCDData[128]和阈值得出二值化后的图像
- 在PC中利用图像和相应的数据,利用其他算法得到阈值与二值化的图像,与上位机得到的二值化图像进行比较,观察两者的区别与优劣(算法时间复杂度、准确度等)