构建基于线性CCD的直立行走嵌入式系统

曹华杰,吕传龙,刘浩东

(1.西南交通大学 信息科学与技术学院,四川 成都　611756;2.西南交通大学 电气工程学院,四川 成都　611756)



构建基于线性CCD的直立行走嵌入式系统

曹华杰1,吕传龙2,刘浩东2

(1.西南交通大学 信息科学与技术学院,四川 成都611756;2.西南交通大学 电气工程学院,四川 成都611756)

摘要为设计一种能在狭小和复杂电磁环境下运动的双轮直立系统,以飞思卡尔公司xs128嵌入式单片机作为控制和处理核心,通过线性CCD进行路况识别,并将MMA7260加速度计和ENC03陀螺仪采集的角度信号通过卡尔曼算法进行滤波,从硬件设计到软件以及算法调试。通过实验测试表明,本系统运行稳定、可靠,可适用于该种特定场景。

关键词双轮直立;线性CCD;陀螺仪;加速度计;卡尔曼算法

随着机器人技术的发展和成熟,其应用范围不断扩大。然而在一些较为复杂的环境中,如复杂电磁干扰环境中或狭小的环境中,传统的多轮机器人受到冲击而翻倒后无法继续作业。因此,设计了一种能够动态直立的双轮行走系统[1]。本系统是一个轮式驱动的智能车系统,其中涉及到传感器技术、智能控制、驱动设计以及卡尔曼滤波算法等。这是一种适合复杂电磁环境下的双轮直立系统。其对进一步研究智能机器人具有较高的价值和意义[2]。

1直立系统

直立自平衡系统电机是小车的驱动装置,在未上电的时并不会旋转产生转矩。在小车偏离平衡位置,电机不进行控制时,小车是一个自然不稳定系统,会朝着向系统不稳定的方向运动。当通过一定的控制策略控制电机转矩时,可使小车反向于不稳定方向运动,从而实现直立任务。如图1所示,直立系统的3种状态。

图1　直立系统的3种状态

对于3种状态,电机采取不同的控制策略。平衡状态:车体处于平衡位置,重心落在平衡点处,电机静止。前倾状态:车体向前偏离平衡位置,电机驱动双轮向前滚动。后倾状态:车体向后偏离平衡位置,电机驱动双轮向后转动。

2系统方案

整个系统的设计分为两部分,硬件架构和软件设计。整个系统在车模基础上进行测试,车模电机采用R380电机。线性CCD采用Sony公司的TSL1401模块,硬件设计包括电源管理模块、MMA7260加速度计模块、ENC03陀螺仪模块、电机驱动模块、单片机电路,在实际调试过程中还使用了无线模块。软件设计主要将CCD采集的信息进行处理,同时还能根据陀螺仪和加速度计采集的信号准确驱动电机运动,实现动态平衡。整个系统架构如图2所示。

图2　系统架构方案

3系统硬件设计

硬件部分由信息采集模块、控制模块、电机驱动模块、电源模块和通信模块5部分构成。其中,控制模块是以MC9S12XS128嵌入式控制器为核心,各传感器采集信息数据,传给控制器来发出相应指令。电源模块采用LM2940稳压芯片输出5 V直流电压供电,相关设计可参考数据手册。

3.1信息采集模块

信息采集模块以陀螺仪加速度计和编码器为主,采用的是ENC03、MMA7260和欧姆龙编码器,其功能

是采集到车体运动时实际的重力加速度分量、旋转角速度的模拟电压量及车体运动速度。直立自平衡小车在稳运行状态下,频率在0.3～300 Hz之间,需要设定特定电路截取此区间的频率。陀螺仪是系统角速度的测量器件,针对ENC03芯片设计了带通滤波器。ENC03及带通滤波器电路如图3所示。

图3　陀螺仪滤波电路

3.2电机驱动模块

电机驱动模块是以驱动电路为主,PWM是主要控制方式。电机驱动电路有两种方案,可使用英飞凌公司的集成芯片BTS7971,两片BTS7971可构成一个全H桥电路,通过单片机产生PWM控制电机的正向和反向转动。也可使用分立的MOS管进行设计全H桥电路。如图4所示,设计使用分立的MOS管搭建电机驱动电路,这样控制的灵敏度更高。

图4　MOS管电机驱动电路

无线模块采用蓝牙模块实现电脑与自平衡系统的数据传输,主要在调试中使用。

4系统软件及算法设计

4.1程序执行流程

采用Netroworks公司Codewarrior IDE进行程序编译以及实现在线调试等功能。程序先初始化各模块参数,然后可在人机交互界面进行参数设置。调试程序时,在液晶上显示相关数据,并可通过按键进行调节。对于加速度计和陀螺仪采集的信号通过卡尔曼算法进行融合,对于电机控制使用位置式PID算法进行调节,软件系统的执行结构如图5所示。

图5　软件执行结构

4.2卡尔曼算法

小车的倾角信息处理是双轮直立平衡系统的重要部分。陀螺仪和加速度计分别为不同的角度传感器,在系统中采用双传感器进行角度测量。然而,加速度计易受到自身器件的噪声、电机噪声以及陀螺仪混叠的影响,而且在高速运动体系下具有严重的滞后特性;陀螺仪噪声小,但存在着温度漂移现象,静态性能不足以达到要求。为保留加速度计的静态性能,陀螺仪的动态性能,使两者优势可相互结合,采用离散卡尔曼算法对陀螺仪和加速度计数据进行融合[3-4]。

卡尔曼滤波算法利用反馈控制来实现对状态的估计,其根据前一时刻的系统状态,估计出当前的系统状态,然后根据当前时刻的实际观测值作为反馈,来修正估计状态。因此,卡尔曼滤波算法可分为状态预测和测量修正两个步骤[5]。状态预测方程由前一刻系统状态和噪声方差预测出当前的系统状态;而测量修正方程则负责修正,在状态预测方程中得到的预测估计状态中加入当前实际传感器的信号,最终得到修正后的系统状态。

卡尔曼算法由状态预测方程和测量修正方程组成,其中状态预测方程由式(1)和式(2)组成。其中式(1)为向前推算状态变量,式(2)为向前推算误差协方差

X(k)=AX(k-1)+BU(k)

(1)

P(k)=AP(k-1)AT+Q

(2)

测量修正方程由式(3)～式(5)组成。其中,式(3)为计算卡尔曼增益;式(4)是根据测量值进行修正估计;式(5)为更新协方差计算

K(k)=P(k)HT(HP(k)HT+R)-1

(3)

X(k)=X(k)+K(k)(Z(k)-HX(k))

(4)

P(k)=(1-K(k)H)P(k)

(5)

其中,X(k-1)和P(k-1)为初始估计值。

卡尔曼滤波算法,即利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。状态预测方程如式(6)所示,修正方程如式(7)所示

(6)

(7)

当状态预测方程和测量修正方程循环运算时,就能使输出信号越来越接近真值。使用卡尔曼滤波算法能有效过滤信号中的噪声。

根据卡尔曼滤波原理。设计滤波算法如下:以小车的车身倾角φ,陀螺仪的漂移温差b,作为状态向量,相应的状态方程如下

(8)

式中,ωgro为陀螺仪的实际输出角速度;φocce为加速度计的实际输出;ω1为加速度计过程噪声;ω2为陀螺仪过程噪声;v为加速度观测噪声。

卡尔曼滤波过程的协方差,测量协方差如式(9)所示

(9)

qacce和qgro速度计和陀螺仪测量的协方差,协方差取值的物理意义是对传感器信号品质的一种评估。例如,若认为加速度计测量的数据更可靠,便可将qacce值设置的较小,从而在滤波过程中体现这种可靠性。R矩阵代表测量误差的协方差。同理,R取值的大小,就代表测量噪声的大小。Q和R矩阵的取值,通过大量实验获取比较合理的参数值[6]。

卡尔曼算法容易通过单片机编程实现,而且不需要转换到频域进行高通或低通滤波,且只需保存上一状态估计值,可对现场数值实时进行更新,滤波效果能达到控制要求。经过卡尔曼滤波器得到的角度波形示意图,如图6所示,加速度计具有较大的噪声,单独使用加速度计无法使车保持直立状态,通过卡尔曼滤波器之后输入的角度平滑,且跟随性良好。

图6　卡尔曼滤波结果

4.3PID算法

PID 控制器是最实用的一种控制器,其是一种滞后-超前校正装置,PID控制器同时兼具PI和PD两种调节器的作用,前者用于提高系统的稳态精度。后者用于改善系统的动态性能。两者相辅相成,使校正后的系统具有优良的性能[7-8]。PID算法根据实际使用有两种,分别为位置式PID算法和增量式PID算法。本系统中使用位置式PID算法。

对于传统的PID控制结构,将比例、积分、微分项中的积分项,对导数项用后向差分逼近[9],得到数字PID控制器的基本位置算式如式(10)所示

(10)

其中,T是采样时间;kp、Ti、Td为3个待调参数,在用实际代码实现算法时,处理成式(11)形式

PreU=Kp×error+Ki×Integral+Kd×derror

(11)

经过大量的调试之后,3个闭环均选择了PD控制器。3个闭环按照线性叠加的关系进行运算。其中直立PID运算周期为6ms,速度PID运算周期为120ms,方向PID周期为6ms。系统PID的控制策略如下:(1)直立闭环采用负反馈控制,使用PD控制器,控制量为车身倾角;(2)速度闭环采用负反馈控制,使用PI控制器,控制量为车体速度;(3)方向控制为负反馈控制,使用PD控制器,控制量为转向偏差量。

4.4CCD图像处理

经过CCD采集得到的图像是一幅一维阵列,阵列元素成为灰度值或者强度值。图像在经过数字单片机采集之前是一系列连续强度函数的集合,场景信息包含在这些强度之中。图像通常被划分为256个不同的灰度值,在对某些应用中,也常有32、64、128或512个灰度级的情况。对于传感器来说,图像灰度级越高,图像质量越好,但所需的内存越大。为保证得到足够准确的图像信息,在本系统中将图像划分为4 096个灰度级,相应的每个像素点占用内存为2Byte,整幅图像占用内存为256Byte。

本课题中,在保证图像成像质量的基础上尽可能减少每幅图像的处理时间,改进后的图像处理周期为6ms,即可达到166幅/s。本设计选用阈值分割法,即设定静态阈值,大于阈值的像素点置1,反之置0。

5结束语

双轮直立系统是一个复杂的控制系统,其中涉及到传感器技术、智能控制、驱动设计以及卡尔曼滤波算法等。本文先给出了系统的设计框架,分别从硬件和软件上分别设计了该系统。在硬件设计部分主要介绍了陀螺仪滤波电路和电机驱动电路。软件设计部分,主要说明了卡尔曼算法以及控制的PID算法。使用液晶调节卡尔曼参数和PD参数,如图7所示。实物效果如图8所示,整个系统经过测试,能较好地满足动态直立的要求。

图7　液晶调试界面

图8　实物效果图

参考文献

[1]缪寿洪,曹其新,孔祥宣.自行直立双轮移动机器人设计与实现[J].上海交通大学学报,2006,40(11):1832-1835,1841.

[2]郭玉龙,于永会,何志琴.基于MC9S12XS128的直立轮式智能小车设计[J].现代机械,2013(5):31-34.

[3]崔景泰.卡尔曼滤波及实时应用[M].北京:清华大学出版社,2013.

[4]王仲生.智能检测与控制技术[M].西安:西北工业大学出版社,2010.

[5]马磊,史习智.多智能体系统中一致性卡尔曼滤波的研究进展[J].西南交通大学学报,2011,46(2):287-293.

[6]王振宇.两轮移动机器人平衡控制系统的研究[D].成都:西南交通大学,2010.

[7]李凡红.两轮自平衡小车系统[D].北京:北京交通大学,2010.

[8]张阳.MC9S12XS单片机原理及嵌入式开发[M].北京:电子工业出版社,2011.

[9]肖文健,李永科.基于增量式PID控制算法的智能车设计[J].信息技术,2012(10):125-127.

A Vertical Working Embedded System Based on Linear CCD

CAO Huajie1,LÜ Chuanlong2,LIU Haodong2

(1.School of Information Technology,Southwest Jioatong University,Chengdu 611756,China;

2.School of Electrical Engineering,Southwest Jioatong University,Chengdu 611756,China)

AbstractA two wheeled upright systems for narrow and complex electromagnetic environment is designed with a Freescale xs128 embedded micro-controller as the controlling and processing core.The linear CCD and MMA7260 accelerometer and ENC03 gyroscope are used for road condition detection and Kalman algorithm for filtering.Hardware and software design is performed and the algorithm debugged.The experimental tests show that the system is stable and reliable,and can be applied to this particular scene.

Keywordsvertical working;linear CCD;gyroscope;accelerometer;Kalman

中图分类号TP316.2

文献标识码A

文章编号1007-7820(2016)02-058-05

doi:10.16180/j.cnki.issn1007-7820.2016.02.015

作者简介:曹华杰(1989—),男,硕士研究生。研究方向:嵌入式系统应用等。

基金项目:全国大学生创新基金资助项目(201310292023Z);江苏省大学生创新基金资助项目(201310292039Y)

收稿日期:2015- 06- 16