参数化运动模型和PSO的自主车运动规划方法.pdf

Computer Engineering and Applications计算机工程与应用2012，48（29）作者简介：王珂珂（1982），博士研究生，主要研究方向：自主车运动规划及控制；赵汗青（1961），博士，副教授，主要研究方向：自主车环境感知、任务规划；吕强（1962），博士生导师，主要研究方向：非线性系统及控制。E-mail：收稿日期：2011-04-02修回日期：2011-06-17CNKI出版日期：2011-08-04DOI：10.3778/j.issn.1002-8331.2012.29.044http:/ Keke1，2,ZHAO Hanqing1,LV Qiang11.装甲兵工程学院 控制工程系，北京 1000722.中国人民解放军75160部队1.Department of Control Engineering,Academy ofArmored Force Engineering,Beijing 100072,China2.Unit 75160 of PLA,ChinaWANG Keke,ZHAO Hanqing,LV Qiang.Motion planning method of ALV based on parameterized kinematicmodel and PSO algorithm.Computer Engineering andApplications,2012,48（29）：214-219.Abstract：The motion planning of anAutonomous Land Vehicle（ALV）in an obstacle environment is a nonholonomicconstraint global optimization.For this optimization problem,this paper proposes an motion planning method basedon parameterized kinematic model and an improved PSO algorithm.The kinematic model of ALV is divided into aparameterized arclength-curvature model and a velocity model.The PSO algorithm is improved by chaotic mappingmethod.The improved PSO algorithm is used in the parameter optimization of arclength-curvature model.The simu-lation result shows the effectiveness of the proposed method in solving the problem ofALV motion planning.Key words：autonomous land vehicle;motion planning;parameterized model;particle swarm optimization摘要：自主地面车辆在障碍物环境下的运动规划问题是一个包含非完整约束条件的全局优化问题。针对该优化问题，提出了一种基于参数化运动模型和改进粒子群优化算法的运动规划方法。该方法将车辆运动模型解耦为参数化弧长-曲率模型和速度模型，并采用混沌映射方法对粒子群优化算法进行了改进，将改进的粒子群优化算法应用于弧长-曲率模型中的参数优化问题。仿真结果证明了该方法的有效性，是自主地面车辆运动规划的一种较好方法。关键词：自主地面车辆；运动规划；参数化模型；粒子群优化文章编号：1002-8331（2012）29-0214-06文献标识码：A中图分类号：TP241引言自主地面车辆（ALV）是一种能够通过多种传感器感知局部环境信息和自身状态，实现在有障碍物的环境中向目标点自主运动的智能系统。在复杂环境下的运动规划是目前挑战性最大的问题。传统运动规划算法，如基于几何构造的规划方法、图搜索算法和基于随机采样的运动规划方法等1，仅仅考虑了位形空间的几何约束，忽略了车辆的运动学和动力学特性，因而规划结果不一定是可行的，尤其阿克曼结构的轮式车辆存在最小转弯半径，且运动规划受到光滑约束条件的限制。因此，运动规划除了要在障碍空间找到一条无碰撞、代价优化的路径之外，重点强调规划的路径能够满足车辆的运动约束和动力约束，同时需要保证规划算法的实时性。Thomas M.Howard2-3等为保证运动规划结果满足运动约束，设计了模型预测轨迹生成器，使用3参数2阶样条描述弧长-曲率模型，并采用梯度投影法进行参数优化。该方法有以下缺点：（1）由于3参数2阶样条函数对平面坐标系下轨迹的构造能力有限，该方法在多数情况下需采用分段规划、轨迹拼接的2142012，48（29）方法完成运动规划；（2）基于梯度投影的参数优化方法需要离线生成大量的参数参考值作为数据表，优化计算根据目标点信息通过查表方式获取适当的待优化参数的初始值，以提高优化速度和精度。吴宪祥、郭宝龙等采用三次样条曲线对移动机器人进行路径规划，保证了规划路径的平滑性。但借助三次样条连接描述的路径，不一定满足车辆的运动学和动力学约束4。本文从轮式地面车辆的运动学建模入手，利用弧长-曲率的曲线微分关系对车辆的运动约束和动力约束进行了分析，给出了完整的参数化运动模型，包括参数化弧长-曲率模型和参数化速度模型。在此基础上进一步对其参数优化方法进行了研究，给出了一种基于改进PSO的运动规划方法。实验结果表明，该方法通过约束分解的策略很大程度地降低了问题空间的复杂度，使得运动规划能够在较短的计算时间内获得满足运动和动力约束的避障路径。2问题描述对ALV进行运动规划时，除了要考虑障碍物空间带来的几何约束之外，还要考虑车辆运动特性和动力特性对轨迹和速度的约束。因此，ALV运动规划是一个包含非完整约束条件的全局优化问题。为满足实际工程应用需求，在研究运动规划算法时，需要生成一组参数化的控制量，并满足一组状态约束方程。该问题可以使用一组微分方程表示：(p)=f(u(p)表示车辆的状态，=xyv，(xy)为车辆位姿状态，v为车速，为前轮摆角；p为需要求解的一组参数；u(p)是参数化的控制输入量；是u(p)与的一个函数，因为ALV对于某一特定控制输入的响应与当前车辆状态有关系。T=xTyTTvT表示期望的目标位姿状态。I=xIyIIvII表示车辆初始状态，包括初始位姿状态、初始车速和初始前轮摆角。F(p)=I+0sf(p)dsF(p)表示从初始状态I开始，在控制参数p的作用下的车辆状态。C(p)定义为目标位姿状态约束条件，其表达形式为C(p)=T-F(p)。为允许最大目标位姿误差。C(p)表示规划结果达到目标状态。Cobs是一些不可通行的非法状态组成的集合，即位姿空间中的障碍集合。用w=f(p)表示目标函数，作为评价优化效果的指标。对于一组控制参数p，产生一组行为控制序列u1u2uk，并 由 此 序 列 导 出 一 个 状 态 序 列：IF。如 果 满 足 约 束 条 件C(p)并 且IFCobs=，那么称参数p为问题的一个解决方案。可以根据参数p导出的状态序列IF计算目标函数w=f(p)的值。因此，运动规划问题可以表述如下：minw=minf(p)s.tC(p)，IFCobs=3参数化运动模型3.1运动学模型状态空间描述地面轮式车辆的位姿空间为C=R2S1，由三元组q=(xy)表示，其中xy表示车辆后轮中心所在的位置，表示车体的方向角。L表示车辆前后轮轴距。如图1所示。输入空间为车速v和前轮摆角，地面车辆运动的状态空间模型为：x=vcosy=vsin=vtan/L（1）地面轮式车辆的输入空间可以解耦为转向角度控制和纵向速度控制。其运动学约束条件描述为：由 于 车 辆 转 向 机 构 的 限 制，前 轮 摆 角满 足|max。由于车辆系统特性的限制，其动力学约束的产生可以从两个方面来思考：一方面考虑控制输出量的有界性和控制输出量一阶导数的有界性；另一方面考虑地面车辆工作的惯性系中，所有能量的变化都是连续的，即所有控制输出量是连续的。所以，转向角度输出函数=f(t)和速度控制输出函数v=fv(t)均应为有界且一阶连续可微。运动规划的结果只有满足上述有界、一阶连续可微的条件，即运动轨迹连续光滑，才能满足实际行驶的需求。vXLxyYO图1轮式车辆的运动关系描述王珂珂，赵汗青，吕强：参数化运动模型和PSO的自主车运动规划方法215Computer Engineering and Applications计算机工程与应用2012，48（29）3.2运动学模型弧长-曲率空间描述根据弗雷内曲线微分关系，光滑曲线L:y=f(x)在M0点的曲率定义为：k=limDs0DDs=dds曲率表现了曲线上包含点M0的弧段偏离直线的程度，也表现了曲线方向变化的快慢。因此，可以用弧长-曲率特征来描述车辆每一时刻的位置变化和角度变化。对于式（1），可进行如下变形：Dx=vDtcos=DscosDy=vDtsin=DssinD=vDttan/L=Dstan/L上式对弧长s进行微分可得：dxds=cosdyds=sindds=tanL且k=dds=tanL（2）由此可知，在车辆行驶轨迹的曲率由当前车辆前轮摆角决定，与车速无关。在弧长-曲率空间下，地面轮式车辆的运动学约束为：轨迹曲线有最大曲率限制|k kmax。以车辆后轮轴中心点作为车体坐标系的原点，当方向盘转至极 限 位 置 时，经 测 量 该 中 心 点 的 转 弯 半 径Rmin=6.4 m，车辆前后轮距L=2.6 m，通过式（2）可以计算max=22.11度，因此，在车辆转向控制过程中，曲率k与转向控制量（前轮摆角）是等价的；当车速v已知时，由s=vt可知，时间t与弧长s是等价的。因此可以用弧长-曲率关系k=fk(s)表征转向控制量=f(t)。因此其动力学约束可以描述为：k=fk(s)在s0sf上一阶连续可微，且|kkmax；由于车辆速度v和转速的极限值产生的对轨迹曲线曲率的限制，即当v已知，有kk|max/v。3.3参数化弧长-曲率模型本文提出的参数化弧长-曲率模型是将转向过程中的k=fk(s)曲线用一个3次样条函数描述，通过m个控制样条形状的特征点(k1k2km)来控制k=fk(s)曲线的形式，|kikmax，i=12m。根据定义可知，3次样条函数所构造的曲线比较光滑，即在给定的区域上（包括节点处）具有一阶导数和二阶导数存在且连续的基本特性，因此可以保证k和k连续。这意味着能够满足在进行车辆转向控制时，前轮摆角和前轮摆速的连续性，生成的轨迹符合实际的车辆动力系统变化特征，满足车辆平稳行驶的要求。考虑到基于3次样条函数的弧长-曲率模型对路径曲线的构造能力，应根据环境特点、起始点与目标点的距离等因素对m进行适当取值。在参数化弧长-曲率模型中，当车速v0时，k0由车辆当前状态下的方向盘转角决定，为定值，无需优化；当车速v=0时，k0为待优化参数。如图2所示，当v0，m=4时，运动规划需要优化的参数包括k1k2k3sf，3次样条函数特征点坐标分别为(k00)(k113sf)(k223sf)(k3sf)。3.4速度规划模型本文采用线性模型对车辆速度进行规划。规划时，主要考虑路径规划中最大曲率点对车速的限制值vkmax、任务规划层的期望速度vtask和当前车速v0。为此，本文设计了3种不同的线性模型，即匀速模型（v0=vtaskvkmax时）、加速模型（v0vtask或v0vkmax时）和减速模型（vkmaxv0或vtaskDsafe(1-dminDsafe)2,0dminDsafe1,dmin=0dmin=minoCobsmint01(x(s)-ox)2+(y(s)-oy)2式中，x(s)y(s)由弧长-曲率模型确定，Cobs为位姿空间中所有障碍物的集合。（2）规划的路径应尽量短，即弧长-曲率模型中的弧长越小越好。路径长度适应值函数与路径的弧长成反比。定义如下路径长度适应度函数：wlen=(1-smins)2，ssmin式中，smin为起始点到目标点的欧式距离，s为轨迹的弧长。4改进PSO算法对于障碍物环境下的运动规划问题，位姿空间中障碍物约束条件很难用函数解析的形式进行表达，优化问题的自变量必须满足地面车辆系统的状态微分方程，且目标函数在待优化区域内具有多个极值点。由于上述三个因素的存在，大大增加了优化问题的困难程度，传统的优化方法很难凑效。粒子群优化5（Particle Swarm Optimization，PSO），由Kennedy和Eberhart基于一种社会心理学模型中的社会影响和社会学习6而提出的一种新的全局优化进化算法，其基本概念源于对鸟类捕食行为的模拟。与其他的进化算法相比，PSO算法简单通用、调整参数少、容易实现，具有较强的全局收敛能力和鲁棒性，且不需要借助问题的特征信息，非常适于对复杂环境中的优化问题的求解。作为一种高效的优化方法，该算法近年来已引起了优化及演化计算等领域的学者们的广泛关注6。4.1基本PSO算法PSO算法首先生成初始种群，并由目标函数为每个粒子确定一个适应值。粒子将在解空间中运动，其位置的调整依赖于自身的经验以及周围邻居的经验，并由一个速度量决定其下一步前进的方向和距离。设搜索空间为m维，即决策变量的个数；粒子种 群 中 粒 子 个 数 为n，第i个 粒 子 的 位 置 为xi=xi1xi2ximT，其速度为vi=vi1vi2vimT。令xi(t)代表第i个粒子在时刻t在搜索空间的位置，t代表离散时间点。粒子的位置变化由加入的速度项vi(t)引起，即xi(t+1)=xi(t)+vi(t+1)（3）其中xi(t)U(xminxmax)。第i个粒子的个体最优值为Pi=Pi1Pi2PimT，种 群 的 全 局 最 优 值 为Pg=Pg1Pg2PgmT。速度向量是驱动整个优化进程的动力。按照追随当前最优粒子的原理，第i个粒子在第j维上的速度vij(t+1)将按照下式计算：vij(t+1)=wvij(t)+c1r1Pij(t)-xij(t)+c2r2Pgj(t)-xij(t)（4）式中，j=12m；i=12n；vij(t)为惯性项，可以防止粒子大幅度的改变搜索方向，只是对现行飞行方向有所偏向。w为惯性权值；r1和r2为服从均匀分布于01之间的随机数，这些随机数将不确定性因素引入到算法中；c1和c2是正数的加速度常量，分别用来度量认知成分和社会成分对于速度更新的贡献。4.2改进粒子群优化算法虽然基本粒子群优化算法存在着许多优点，取得了一些成功，但分析表明该算法存在着容易陷入局部最优，进化后期收敛速度慢等缺点。为此，采用混沌映射方法对粒子群优化算法进行了改进7。混沌运动是确定性系统在非线性的相互作用下，有可能产生的对初始值极为敏感的、貌似无规则的复杂运动，是局部不稳定与整体稳定的矛盾统一体。混沌运动具有遍历性、轨道不稳定性及分岔、长期不可预测性、分形结构和普适性等特征7。常见的离散混沌系统有：（1）帐篷映射zn+1=zn/a 0zna(1-zn)/(1-a)aznN。将zi的各个分量映射到对应待优化参数的取值区间。计算所有粒子的适应值，从N*个粒子中选择性能较好的N个作为初始种群，并保存全局最优位置Pg和个体最优位置Pi。步骤3 按照公式（2）（3），产生新一代的粒子位置，并计算其适应值，找到所有导致路径与障碍物发生碰撞的粒子，并用混沌序列随机产生数量相同的新粒子将其替代，然后更新Pg和Pi。步骤4 判断算法是否停滞。若停滞则执行步骤5，否则执行步骤6。步骤5 按照混沌映射公式，生成Np个m维混沌向量，线性映射到规定的解空间范围内，随机取代原来的Np个种群粒子；返回步骤3。步骤6 判断是否满足算法终止条件，若满足则终止算法，执行步骤7；否则，执行步骤3。步骤7 输出全局最优路径。5实验结果为了验证算法的效果，对提出的基于参数化弧长-曲率模型和改进PSO的运动规划方法进行了大量的仿真实验。各参数设置如下：待优化参数的个数m=6，种群规模N=100，学习因子c1=c2=1.494 45，惯性权值w随着迭代次数的增加从0.9线性减小到0.6，最大迭代次数Gen=20，替代概率p=0.4，初始种群的速度均为0。本文以两个仿真环境为例，分别采用基本PSO和改进PSO进行了对比实验，其实验结果如图3、图4所示。环境1 环境地图尺寸为30 m30 m，起点坐标为（8，3），车体方向角为0，车辆前轮摆角为0。终点坐标为（1.5，27），车体方向角为-/2。环境2 环境地图尺寸为60 m60 m，起点坐标为（24，3），车体方向角为0，车辆前轮摆角为0。终点坐标为（42，55），车体方向角为/6。0.220.200.180.160.140.120.10最优适应值10121416182002468迭代次数改进PSO基本PSO目标点终止点改进PSO基本PSO环境地图图3环境1的仿真结果目标点终止点改进PSO基本PSO环境地图0.550.500.450.400.350.300.250.200.150.100.05最优适应度10121416182002468迭代次数改进PSO基本PSO图4环境2的仿真结果2182012，48（29）实验数据证明，该算法对于一般障碍物环境下的轮式车辆路径规划，能以很大的概率收敛到全局最优，并且收敛速度很快，满足自主地面车辆系统的实时性要求。6结论本文针对障碍物环境下自主地面车辆的运动规划问题，将车辆运动模型解耦为参数化弧长-曲率模型和速度模型，并提出了一种基于参数化模型的改进PSO算法。采用混沌映射方法对PSO算法进行了改进，有效解决了基本PSO算法容易陷入局部最优，进化后期收敛速度慢等缺点，得到了问题最优解，仿真结果表明了方法的有效性。本文只对静态障碍物环境下的运动规划进行了研究，将该运动规划方法应用于动态障碍物环境是需要进一步研究的课题。参考文献：1 刘华军，杨静宇，陆建峰，等.移动机器人运动规划研究综述J.中国工程科学，2006（1）：85-94.2 Howard T M，Kelly A.Optimal rough terrain trajectorygeneration for wheeled mobileJ.The International Jour-nal of Robotics Research，2007，26（2）：141-166.3 Ferguson D，Howad T M，Likhachev M.Motion planningin urban environmentsJ.Journal of Field Robotics，2008，25：939-960.4 吴宪祥，郭宝龙，王娟.基于粒子群三次样条优化的移动机器人路径规划算法J.机器人，2009，31（6）：556-560.5 Kennedy J，Eberhart R C.Particle swarm optimizationC/International Conference on Neural Networks，1995.6 Kennedy J，Mendes R.Neighborhood topologies in fully-informed and best-of-neighborhood particle swarmsC/International Workshop on Soft Computing in IndustrialApplications，2003：45-50.7 刘玲，钟伟民，钱锋.改进的混沌粒子群优化算法J.华东理工大学学报：自然科学版，2010，36（2）：267-272.8 Parsopoulos K E，Vrahatis M N.Recent approach to globaloptimization problem through particle swarm optimiza-tionJ.Natural Computing，2002，1（2）：235-256.9 唐贤伦.混沌粒子群优化算法理论及应用D.重庆：重庆大学，2007.环境1环境2基本PSO30.1856.73改进PSO28.0655.51基本PSO192.91263.84改进PSO177.03229.34平均运行时间/ms平均最短路径/m表1算法统计结果10 翟云，杨炳儒，曲武.不平衡类数据挖掘研究综述J.计算机科学，2010，37（10）：27-32.11 Debole F，Sebastiani F.An analysis of the relative hard-ness of Reuters-21578 subsetsJ.Journal of the Ameri-can Society for Information Science and Technology，2004，56（6）：584-596.12 李蓉，叶世伟.SVM-KNN分类器一种提高SVM精度的新方法J.电子学报，2002，30（5）：745-748.13 李程雄，丁月华，文贵华.SVM-KNN组合改进算法在专利文本分类中的应用J.计算机工程与应用，2006，42（20）：193-212.14 李蓉，孙媛.SVM-KNN分类器在网页分类中的应用J.科学技术与工程，2009，9（16）：4653-4656.15 Blake C，Keogh E，Merz C.UCI repository of machinelearning databasesEB/OL.http:/www.ics.uci.edu/mlearn/MLRepository.html.16 Yan R，Liu Y，Jin R，et al.On predicting rare classeswith SVM ensembles in scene classificationC/IEEEInternational Conference on Acoustics,Speech and Sig-nal Processing（ICASSP），Hong Kong，2003：21-24.17 Chang Chih-Chung，Lin Chih-Jen.LIBSVM：a library forsupport vector machinesEB/OL.http：/www.csie.ntu.edu.tw/cjlin/libsvm.（上接123页）5 Sheh A，Ellis D.Chord segmentation and recognition usingEM-trainedhiddenMarkovmodelsC/ProcIntConfMusicInfRetrieval（ISMIR），Baltimore，MD，2003：185-191.6 Wright J，Yang A，Ganesh A，et al.Robust face recognitionvia sparse representationJ.IEEE Trans on Pattern Anal-ysis and Machine Intelligence，2009，31（2）：210-227.7 Ellis D.Beat tracking by dynamic programmingJ.NewMusic Research，Special Issue on Beat and Tempo Ex-traction，2007，36（1）：51-60.8 Brown J.Calculation of a constant Q spectral transformJ.J Acoust Soc Amer，1991，89（1）：425-434.9 Donoho D.For most large underdetermined systems oflinear equations the minimall1-norm solution is also thesparsest solutionJ.Comm on Pure and Applied Math，2006，59（6）：797-829.10 Harte C，Sandler M，Abdallah S，et al.Symbolic repre-sentation of musical chords：a proposed syntax for textannotationsC/ProcIntConfMusicInfRetrieval（ISMIR），London UK，2005：66-71.11 Bello J，Pickens J.A robust mid-level representation forharmonic content in music signalsC/Proc Int ConfMusic Inf Retrieval（ISMIR），London，UK，2005：304-311.（上接136页）王珂珂，赵汗青，吕强：参数化运动模型和PSO的自主车运动规划方法219