基于集员卡尔曼滤波的智能车定位研究

２０１４年第３期　闽南师范大学学报（自然科学版）　Ｊｏｕｒｎａｌ　ｏｆ　Ｍｉｎｎａｎ　Ｎｏｒｍａｌ　Ｕｎｉｖｅｒｓｉｔｙ（Ｎａｔ．Ｓｅｉ．）　Ｎｏ．３．２０１４年　Ｇｅｎｅｒａｌ　Ｎｏ．８５　（总第８５期）　基于集员卡尔曼滤波的智能车定位研究　许艳萍　，王武　ｆ１．闽南师范大学物理与信息工程学院，福建漳州３６３０００；　２．福州大学电气工程与自动化学院，福建福州３５０１０８）　摘要：本文针对道路约束的实际问题，应用集员卡尔曼滤波算法对具有状态约束的智能车进行研究．以具有行　驶路线约束的Ｆｒｅｅｓｃａｌｅ　１６住单片机ＭＣ９Ｓ１２ＤＧ１２８Ｂ控制的智能模型车为控制对象进行集员卡尔曼滤波算法　设计和应用。实验结果表明该算法能有效地减小估计误差，也能够很好的满足实际应用的要求．　关键词：状态约束；集员卡尔曼滤波；ＭＡＴＬＡＢ　ＧＵＩ；智能车　中图分类号：ＴＰ１３　文献标志码：Ａ　文章编号：２０９５—７１２２（２０１４）０３－００７４－０７　Ｓｔｕｄｙ　ｏｆ　Ｉｎｔｅｌｌｉｇｅｎｔ　Ｖｅｈｉｃｌｅ　Ｌｏｃａｔｉｏｎ　Ｂａｓｅｄ　ｏｎ　Ｓｅｔ－ｍｅｍｂｅｒｓｈｉｐ　Ｋａｌｍａｎ　Ｆｉｌｔｅｒｉｎｇ　ｘｕ　Ｙａｎ—ｐｉｎｇ　．ＷＡＮＧ　ＷＵ２　（１．Ｓｃｈｏｏｌ　ｏｆ　Ｐｈｙｓｉｃｓ　ａｎｄ　Ｉｎｆｏｒｍａｔｉｏｎ　Ｅｎｇｉｎｅｅｒｉｎｇ，Ｍｉｎｎａｎ　Ｎｏｒｍａｌ　Ｕｎｉｖｅｒｓｉｔｙ，Ｚｈａｎｇｚｈｏｕ，Ｆｕｊｉａｎ　３６３０００，Ｃｈｉｎａ；　２．Ｓｃｈｏｏｌ　ｏｆ　Ｅｌｅｃｔｉｒｃａｌ　Ｅｎｇｉｎｅｅｒｉｎｇ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ，Ｆｕｚｈｏｕ　Ｕｎｉｖｅｒｓｉｔｙ，Ｆｕｚｈｏｕ，Ｆｕｊｉａｎ　３５０１０８，Ｃｈｉｎａ）　Ａｂｓｔｒａｃｔ：Ａｓ　ｔｏ　ｔｈｅ　ｐｒｏｂｌｅｍ　ｏｆ　ｒｏａｄ　ｃｏｎｓｔｒａｉｎｔｓ，ｔｈｅ　ｓｅｔ－ｍｅｍｂｅｒｓｈｉｐ　Ｋａｌｍａｎ　ｆｉｌｔｅｒｉｎｇ　ｉｓ　ｐｒｏｐｏｓｅｄ　ｆｏｒ　ｓｙｓｔｅｍｓ　ｗｉｔｈ　ｓｔａｔｅ　ｃｏｎｓｔｒａｉｎｔｓ　ａｎｄ　ｉｔ　ｉｓ　ａｐｐ￣ｅｄ　ｔｏ　ｔｈｅ　Ｆｒｅｅｓｃａｌｅ　１６一ｂｉｔ　ＭＣ９Ｓ１２ＤＧ１２８Ｂ　ｉｎｔｅｌｌｉｇｅｎｔ　ｖｅｈｉｃｌｅ．Ｔｈｅ　ｒｅｓｕｌｔｓ　ｓｈｏｗｓ　ｔｈａｔ　ｔｈｅ　ｅｓｔｉｍａｔｉｏｎ　ｅｒｒｏｒ　ｃａｎ　ｂｅ　ｇｒｅａｔｌｙ　ｒｅｄｕｃｅｄ　ｗｉｔＩ１　ｔＩｌｅ　ａｌｇｏｒｉｔｈｍ．ａｎｄ　ｉｔ　ｍｅｅｔｓ　ｒｅｑｕｉｒｅｍｅｎｔｓ　ｗｅｌ１．　Ｋｅｙ　ｗｏｒｄｓ：ｓｔａｔｅ　ｃｏｎｓｔｒａｉｎｔｓ；ｓｅｔ—ｍｅｍｂｅｒｓｈｉｐ　Ｋａｌｍａｎ　ｆｉｌｔｅｒ；ＭＡＴＬＡＢ　ＧＵＩ；ｉｎｔｅｌｌｉｇｅｎｔ　ｖｅｈｉｃｌｅ　随着交通和汽车工业的发展，特别是计算机、电子、图像处理等技术的飞速发展，智能交通和智能车　辆逐渐发展起来．智能车辆又称为轮式机器人（Ｗｈｅｅｌ　Ｍｏｂｉｌｅ　Ｒｏｂｏｔ），是一个集环境感知、规划决策和多等　级辅助驾驶等功能于一体的综合系统，它集中运用了自动控制理论、人工智能、模式识别、传感器技术、电　气、计算机、汽车电子及机械等多学科知识，是典型的高新技术综合体［１］．智能车辆能够利用各种传感技术　获取车体自身和车外环境的状态信息，经过智能算法对其进行分析、融合处理，最后做出最佳的决策．因　此．智能车辆在改善道路安全状况，提高道路利用率方面做出了很大的贡献．　另一方面，车辆不可避免的要行驶在道路上。可以把这一情况看做是具有行驶路线约束［２１的情况．如　何使车辆能够安全的行驶在道路上，避免交通事故已经越来越被大家所重视．智能车辆因其高新技术性，　一方面可应用其导航及高精度ＧＰＳ系统来辅助驾驶【３Ｊ，另一方面也可以通过各种智能算法，如滤波算法，　控制决策算法【４］等来提高行驶的安全性．本文将具有状态约束的线性离散系统集员卡尔曼滤波算法四应用　在具有行驶路线约束的Ｆｒｅｅｓｃａｌｅ　１６位单片机ＭＣ９Ｓ１２ＤＧ１２８Ｂ智能模型车上，力求达到模型车较好的自　动行驶性能．　１智能车建模　本文使用的智能车是全国大学生“飞思卡尔”杯智能汽车竞赛所采用的Ｂ型四轮驱动车模．其主要参　数为：　收稿日期：２０１４　ｌ４—２５　基金项目：福建省自然科学基金资助项目（２００９Ｊ０１２７９）　作者简介：许艳萍（１９８５＿］，女，福建省龙岩市人，助教．　・７４・　许艳萍．王武：基于集员卡尔曼滤波的智能车定位研究　第３期　模型车：四轮驱动，长×宽×高为２７０ｘ１７０￣９６（单位：ｍｍ），有调零装置，套装中包含组装好的车底（含车　轮）、一个伺服器、一个马达、一块电池、一个充电器．　马达：ＤＣ　７．２Ｖ　ＮＤ３８０马达，５０００转，分，最大功率可达２６．５Ｗ．　伺服器：ＩＰＳ０１７伺服器，扭力６．０Ｋｇ．　轮胎：轮宽２７ｒａｍ，直径６５ｍｍ，材料ＡＢＳ．　电池：７．２Ｖ　２０００ｍＡｈ。飞思卡尔大赛指定，电动工具专用型．　充电器：７．２Ｖ　６００ｍＡ，采用先进的单片机技术，一ＡＶ检测方式，充满电自动停止．空载桔黄灯（或绿灯），　正常充电红灯，充满电绿灯．　由汽车运动学和动力学知识可知，当车的车轮只滚动而不滑动的时候，每个车轮围绕某个中心而转　动，而这个中心是落在后轴中心线的延长线上，并且左右前轮也必须以这个中心点为圆心而转动问，如图１　所示，其中智能车轮距Ｂ＝１５０ｒａｍ，轴距Ｌ＝１７５ｍｍ．　｝＿－一…上………一　一－▲一　Ｂ　Ｉ　．一夏一　一　／　／　・　，　，　一　＿／，　一　　．图１智能车几何模型　图２智能车运动模型　由图１的几何关系可得：　尺＝　ｔａｎ／Ｊ　（　）　（２）　ｃｔｇ　：ｃ　＋　其中Ｒ为转向内轮的转弯半径，　为左轮转角，　为右轮转角．智能车运动模型如图２所示，其中ＯＺｋ为智　能车左轮转角．车体在转角不变的情况下以固定圆心和不变的转向半径做圆弧运动同．　图２中智能车运动模型各参数满足的关系为．　ＲＣ　＝　（　）＝　其中Ｒ　为智能车做圆弧运动时的半径，　为左轮转角，左转Ｏｋ＞Ｏ，右转Ｏｋ＜Ｏ．　１．１智能车速度和位置建模　下面对智能车的位置和速度进行建模．　当智能车做直线运动时，其运动方向不变，位移仅由时间和速度决定，位置可由下式表示：　＋１＝　＋Ｓｃｏｓ　０　（３）　“１＝　・７＇　＋ＳｓｉｎＯ　（４）　Ｓ＝ｌ　ｖ（ｔ）ｄｔ　・７５・　２０１４．皋　闽南师范大学学报（自然科学版）　其中，　“１，ｙｋ＋１为运动时间Ｔ后的位置，　，Ｙｋ为小车初始位置，０为速度角度方向，Ｓ为时ｆｎ－Ｉ　Ｔ内的　运动位移，　（ｔ）为小车的速度．　当智能车做弧线运动时，其位置和速度建模有些复杂．根据智能车的运动模型有：　２。２５州面Ｌ＋　Ｂ　ｚ　‘５　．当Ｔ很小时，我们可以认为小车在Ｔ时间内所做的运动为以圆心不变的圆弧运动．在图２中，假设小　车车体中心从一点Ｃ作圆弧运动到另一点Ａ，圆心坐标为（　，‰）．　根据几何关系可得：　Ｙ　Ｙｋ：＋　ＲＣ　ｓｍ（ａ￣　二＋　ｚ　Ｊ　㈤　（７）　其中，　，　为起始点Ｃ坐标，　为车体中心Ｃ与运动圆心Ｏ连线与ｘ轴的夹角．　Ｔ时间后，车体中心从Ｃ点运动到Ａ点，车方向改变角度为／ＣＯＡ：△Ｏｔｋ，　在本模型中假设顺时针为负，逆时针为正　ｖ为转向时智能车的速度．经过Ｔ时间，车体中心位置Ａ　的坐标为（　＋】，　＋１）．　＋ＲＣ　ＣＯＳ（６ｇ　＋Ａｃｚ　）　＋ｌ：　＋ＲＣ　ｓｉｎ（　＋△　）　＋１＝）Ｃｏ　（８）　１．２智能车仿真运动模型　当智能车做圆周运动时，对其实时位置进行建模．图３速度和转角的关系图，可得到如下式：　１ｆ　△＾ＳｌＩｒＶｋ　Ｒ　Ａ　Ｏ，Ｖｋ∈　，且｝ｆＶ川　１　『Ｊ　且ＩＩ　ｓ　ＩＩ＝ｌＩｓ　ｌＩ＝Ｒ　Ｉ＋Ｔ　＝　＝　，，其中△　表示转向角的最小增量，即最小转角，Ｒ表示小车圆形轨迹　的半径，ＩＩ　ｌＩ表示满足式（９）的数值大小的速度．由于式中Ａ０和Ｒ均为　常数，即速度的模为某一定常数，△Ｓ表示从ｋ时刻到ｋ＋ｌ的位移增量，　Ｉ　ＩｌＩ表示位移矢量的模长．　为了确保所获得的轨迹为一定圆，半径为Ｒ，中心点为原点Ｏ有下式：　ｊＪ　０　＝Ｔ冰．ＪＩ　Ｖ　　０　ｄｋ＝０Ｖ　一△　七　ｋ　（１ｏｕ／）　，、　图３速度和转角的关系图　其中ｋｆ＝２／ＴｒＡ０，表示完成一个圆周所需的时间．　由图４和图５可得速度建模公式：　：十Ｉ　（１　１）　ｖＮ＝ＶＮ—ＡＶＮ　七　ｌ　ｋ　血　（１２）　其中△　冰ｃｏｓ（０￣）一　１乖ｃｏｓ（０￣＋１）为从　时刻到　＋１时刻速度的北向增量，　△　水ｓｉｎ（Ｏｋ）一　・水ｓｉｎ（Ｏ￣＋１）为从　时刻到　＋１时刻速度的东向增量．式（１１）和（１２）为从　时刻到　７６・　许艳萍，王武：基于集员卡尔曼滤波的智能车定位研究　第３期　＋ｌ时刻的速度状态转移方程　Ｎ　图４速度状态转移矢量尉　图５速度增重矢量Ｉ奎Ｉ　由图４和图５可得位置建模公式：　１＝ｓ／＋△　１＝　＋　（１３）　（１４）　其中，△‘５，　＝ＡＳ＊ｓｉｎ（０ｋ）＝ＡＳ＇＝ｆＨ　＝　乖　为从　时刻到　＋１时刻的位移的北向增　量，Ａｓ［＝ＡＳ＊ｃｏｓ（０，）＝ＡＳ＝ｌ＝，＋　＝　水　为从　时刻到ｋ＋１时刻的位移的东向增量．由　式（１３）和（１４）为由　时刻到ｋ＋１时刻的位移状态转移方程．　把式（１１），（１２），（１３），（１４），写成矩阵的形式为：　１　０　Ｔ　０　Ｓ　ｓ　ｎ　０　１　０　Ｔ　ｓ　０　０　１　０　　ｖ　０　０　０　ｌ＋　木ｕ　（１５）　其中，　＝［０　０△　△　】　为整个状态空间的控制输入．　在（９）式中，ｌＩｖ，ＩＩ跟ＡＯ的精度即与小车的舵机的精度息息相关，Ｎ＿￣．ＮＮ／Ｊ、车电机端电压也无法完　全的稳压和钳制在一个理想的常数，此外系统的测速环节和控制环节都不可避免的引入过程噪声和测量　噪声，致使系统的测速精度和控制精度都不可能相当的精确．　由此假设在以上噪声的的干扰下。速度的模值总是在区间　ｎｉｎ，　Ｊ之间波动，如图６所示．　同理，在（９）式中，ｌＩｓ　ｌｌ跟ｌ１　ＩＩ的精度和采样周期精度息　息相关。而且由以上分析小车实际车速不可避免的波动，此外　系统的测距环节和控制环节也都存在一定的过程噪声和测量　噪声．由此我们假设在以上噪声的的干扰下，位移的模值总是　在区间　，　～Ｊ之间扰动．　０　图６速度的噪声界图示　・７７・　２０１４血　闽南师范大学学报（自然科学版）　由此包含噪声的系统模型为：　１　Ｏ　Ｔ　０　ｓ　Ｓ　０　１　０　Ｔ　Ｓ　０　０　１　Ｏ　吣　＋　ｕ　＋　（１６）　０　Ｏ　Ｏ　１　Ｖ　其中，Ｕ　＝［Ｏ　０△　△　ｒ，　＝［ＣＦ　ｅ１．ｅｖ　ｅｖ］　．　图７智能车整体结构框图　２软硬件设计　２．１硬件设计　本文使用的是以飞思卡尔公司提供的Ｓ１２系列的１６位ＭＣ９Ｓ１２ＤＧ１２８Ｂ单片机为主控芯片的模型　车．智能车的硬件电路主要由电源管理模块、电机模块和传感器模块组成，通过ＭＣ９Ｓ１２ＤＧ１２８Ｂ控制器完　成转角控制和速度控制．转向控制主要由舵机完成，舵机的转动会转化为车模转向拉杆的横向移动，从而　带动车模前轮的转动，以控制智能车的行驶方向．数据通过ＲＳ２３２连接智能车和上位机进行传输．智能车　整体结构框图如图７所示．　２．２软件设计　将具有状态约束的线性离散系统集员卡尔曼滤波算法［５１应用于智能车硬件上．由前面可知，智能车建　模所得系统满足系统方程描述，采用模块化的程序结构来设计，如图８所示．用ＭＡＴＬＡＢ串口通信ＧＵＩ　界面来实时显示智能车运动轨迹．　一　接收到初始化数据　初始化智能车单片机系统　启动智能车Ｊ　Ｉ发送实时数据发送标志住｝　ｆ　Ｉ　『实时数据发送至上位机ｌ　ｆ发送实时数据传输结束标识符　ＤＣＥ：智能车单片机　Ｄ１１￡：上位机　图８智能车单片机与上位机通信程序流程图　・７８・