校园智能车定位导航系统设计与实现

　　摘要：为实现智能车平台在校园场景下自主定位和导航，提出一种校园智能车导航系统设计方法。采用惯性导航系统对智能车进行定位，激光雷达创建局部地图，利用混合A+算法在局部地图中规划出合理的路径。整个系统中，上位机获取传感器采集的数据，通过惯性导航系统解算出智能车当前位姿，规划模块获取当前智能车位姿和周围障碍物信息后进行路径规划，并通过串口通信模块将路径信息传递给下位机控制模块，实现智能车的自主移动。实验结果表明：在校园内，智能车能够从起点出发，自主规划路径并绕过障碍物到达目标地点，验证了该导航系统的有效性。

　　关键词：智能车;定位;路径规划;自主导航

　　代成; 程永杰; 蒋涛; 许林成都信息工程大学学报2021-12-13

　　0引言

　　随着自动驾驶技术的不断发展，智能车在日常生活中的应用更加广泛，如校园场景下的人员接驳、路面清洁、外卖快递派送等。自主导航是智能车实现其智能性和自主性的关键。自主导航主要包括环境感知、定位和路径规划。无人车的定位导航技术所使用的传感器类型主要有GPS、惯性导航系统、WiFi、UWB等，而采用单一传感器的定位方法大多都具有局限性。如GPS在室内、水下等被遮挡情况下定位精度低;惯性导航系统精度受限于传感器的成本，低成本惯导漂移误差过大无法单独使用，高成本惯导无法普及;而基于无线信号的定位方法需要预先铺设设备，使用场景受限。定位导航技术按照定位结果可以分为相对定位和绝对定位两种方式[1]。绝对定位方式可以直接获取到目标当前在全局坐标系中的绝对位置，不用通过初始位置进行航迹推算获取下一时刻的位置，当前时刻的位置和下一时刻的位置没有关联。但是，绝对定位方式容易受外界因素影响，如GPS就是一种绝对定位方式，信号遮挡会影响定位精度。相对定位方式是通过上一时刻的位置来推算下一时刻位置[2]，该定位方式精度受限于传感器本身测量精度，如惯导系统。因此与传统的使用单一传感器的定位方式相比，多传感器融合的方式有利于提高定位的精度。目前主要采用GPS与惯导系统通过卡尔曼滤波器融合的方式，实现高精度定位。GPS能够全天获取相对精确的位置，但是更新频率较低;惯导系统能够以高频率提供短时间内精确的位置信息，故二者信息融合能够优势互补，从而得到更精确的位置信息[3]。

　　路径规划的本质是在一定约束条件下，找到一条连接起点与终点的无碰撞路径[4]。常用的智能车路径规划算法包括基于采样的方法和基于图搜索的方法。基于采样的方法包含概率路线图(PRM)[5]与快速搜索随机树(RRT)[6]等。Karaman等[7]在RRT算法基础上引人重选父节点与重布随机树过程，在时间充沛的情况下jRT*的路径最优。Gammell等[8]在RRTs找到初始路径解后，引人椭圆采样的思想，加快RRT*收敛到最优解的速度。Yang等…将RRT与三阶贝塞尔曲线相结合，边采样边平滑，解决路径曲率不连续的问题。杨瑶等[1°]结合Reeds-Sheep曲线、贝塞尔曲线与Bi-RRT*算法，解决了Bi-RRT*转折次数多、路径不平滑、过于贴近障碍物和转角过大等问题。A?nn%与jpsM均是经典的图搜索算法，但是这些方法并不适用于智能车，因为其路径不连续。Dolgov等[14]直接在车辆控制空间内米样，再利用A的思想搜索路径，解决这一问题。齐尧等[15]考虑安全因素，提出适用于混合的惩罚函数，得到更为安全、合理的路径。文中针对的校园场景，行人较多，道路狭窄，车道线残缺不全，故智能车的规划路径不考虑车道问题。基于校园场景下的导航规划，主要考虑规划路径是否合理，智能车行驶途中能否安全避障等。因此，本文自主导航系统主要解决在车辆运动学约束下，定位和路径规划问题，目的是使智能车从当前位置安全平稳地到达目标位置。

　　1GPS/惯导组合导航

　　由于GPS和惯导梅工作特性，二者单独使用性能F.(RN+h)c〇sg>Rjn+h00(5)受限，因此将两种传感器信息通过卡尔曼滤波器进行V00U融合是目前常用的定位方式。通过组合导航的方式，式(5)是导航系下坐标误差。￣可以实时地提供载体较翕精度的速度fn、姿态和傖寶傖0L息g本系统采用的是惯性传感器为捷II愤导系统F?=-fu0fe(6)(strapdowiiinertialnayigatioiisystem，SlHS)〇SINS主￣fe0J要是由三轴陀螺仪，三轴加速度计和微塑计算机组成，式(6)是导航系下速度误差。其相较于平台式惯导拥有绪构简单<童量_、体积小、尺12/?丨3、成本低廉、可靠性高等特点。4=尺21尺22穴23(7)目前.，研究者们对于CPS/惯导系统的组合方式提、况31尺32尺33J出了多种方法，主要可以分为松耦合、紧耦合和超紫耦#3种方式s考虑实际实验中的輿时性和鲁#性，采用松耦合的方式爾个系统各自猶重工作将两个系统输出的导■航结果使用卡尔曼滤波器估计出最优导航参数，其系统框图如图1'所示。

　　GPS/SINS松耦合形式将惯导系统的误義值作为状态囊，将GPS误差作为量测矂禽，卡尔曼_波器的系统模型为8x=FSx+G〇)(1)式中，&为当前时刻先验状态估计，F为动态系数矩阵为误羞状态矢量，G为噪声吳量，W为预测过程中的噪声;对应状态矢鐵中每一个分量的_声：F(〇3xl〇J%3〇3x3FrF，〇3x：〇，.：Sx=[Srl3xlG=[〇%，丨式(2)中，Svi0Fv〇3x：〇3x：〇3x：del3x3〇3x〇3x3R[及6〇3x3〇3x〇Ff3x3I/，SAxl1y°"f，lx3]⑵(3)⑷

　　式(3)中，5/^是泣置误差失量;蠢导航系下的速度摄塗矢量;W基载体姿态误差矢量;是惯导陀螺仪的误_矢量;5/鳥惯导加速度计的误差矢量。为方便编程实现，将连续系统模型离散化后摒'到离散模型为Szk=HkSxk+rjt(11)屮SINS_沪鄉Szk：rSINS_rGPS^SINS—^GPSyA—A/lSINSGPSh-hILsim，lg?s\vu，sim￣(12)式(11)中，由于内包含惯导系统的谟差暈则：羹测矢量屯为GPS观测值与SINS预测值之差，如式(12)所示;表示量测噪声;叽表示零均值、方差为&的纛测嵊_。

　　2路径规划

　　混合A*募法最早应用?于斯坦福自主研发的智能车Juiiitir上頌过在车_鱗控制中肇样，梅车辆的擎完整性约束融入其中，：再通过A4的算法流程搜索，其路径能够直接用于踉私

　　2.1节点扩展方式

　　传统As算法节点通常不考虑车朝难完：整性约東，_W格屬围四个或者：八个方向扩據，如图2U)所示。由于智能车无法原：地旋转，因此这种扩展方式不符合智能车的运动方式。?昆合A*在智能车的3D状态空间中(*>：y，0)离散采样，生成向一t方位、不同方向扩展的连续曲线，如图2(b)所示，遍獅扩展方麟径能够满足动力学约束6

　　2，2估价函数

　　与传统A*算法一：样，混合A*算法建立估价函数判断节点的优先性，眞表达式为f{n)=g{：n)+h{n){B)其中.gU)为起始节点到当前节点《的代价和，通过递推累加可得g(n)=g(n^m)+cM(nlli)S!tl{，n){14)，n)=(1+lt.r?，rrs(j+?-8n｝?(15)萁伞表游，点n的父货点，0喊(的父节点到节点所盡的代价，%?为智能车后退行驶的惩罚值，为转角发生改变的惩罚值，s(Kls?ia，ra〗讓亦从到售貞n的实际行驶距离。惩罚向后行驶与方向改变的行为，能够使规划的路径更容易跟踪，尽量滅少对智能车的操作sA(?)表示节点7T到目标节点的预估代价值。Mn)由两种启发函数构成，取两者中的最大值作为节点77的预估代价值。如式(16)所汞h{n)=max(ht(n)，/i2(n))(16)笫一种唇发函数为忽略环境障碍物、考虑车辆的非S整性约東，即车辆的最小转弯半径，该值为当前节蛊(n0，，)到目标笮点最低的节点，作为即格拓展的节点(nwfcj，同时将n?det實入CI<>siedIjt|中。Step3：若nodey在终点一定范围内，则用Dubim曲线扩展分析，若成功，则返回路径。&ep4：若n〇dei超出局部地图，则返M瑪翦路径+Step5：扩展node，：遍历其后鐘的每一个'子节腐(nodea)。(i)计算!Krfe，t到终点的启发代价值：UU))与从起点到当前点的累计代价值(gU)>，两者求和得到总代价值即/(w)=n)+/i

　　3系统搭建3.1车载传感器类型

　　实验平台为绿通LT-A627电动汽车，如图3所示。定位糢块主要使用SpatialNAV982i|号惯性导航系统，该系统集成GPS接收模块，可以实现组合导航定位。霄达使用VelodyneVLP-16，可以实时扫描车辆周围的环境，从而获取车辆周围的轉标偉眞，实现对周围目标的识别、跟踪等，传感器参数如表U车载计算机侮用net邮ys，Nim)-7000，处理器角Int?srCorei7-8700，主频为3.20GHz，运行内存为16G，底层控制器使用tm32-F407b上位机与下位机俊用理232_：B通射。

　　3.2系统工作流程

　　R0S(ro丨x>top魄tihg夸st伽)[15]是由薇：坦稿大攀人工智能实验室发布协开源的机器人操作系统&该操作系貌可以实现各个硬件模块的抽象、梢息传递、底层的驱动协同管理，_时其分布式的特点，可以分模块迸行开发，被广泛运用子机器人行业，故苹系统采用基于11〇3/11丨)_：1116.04醜基;本框架1?在11〇3操作系统框架下，系统工作流程如图4所示。系统开始工作时，惯导模块进行初始化，通过惯导模块内GPS接收车辆当前位置，实现惯导瘼块的初始对准a*于惯性器件是逋过积分的方式工作，由上一时刻位餐推算：下一时刻的位寘■此只有初始位姿信息确定之后，惯导系统才能开始正常。同財，雷达初始化生成当前焉部代价地图。系统初始化时间在1分钟左右，简初始化完成以后，车辆得到当前的位姿信息与周围环境信息，在给定车辆要到达的目标位置后，规划模块开始规划路径，规划好的路径由跟踪算法解算出车辆将要行驶的速度和_角槪自、。获取拿植将要行驶的速度和转角信獻后，通过RS232串□下发绐底层控制器控制车辆移动。在行驶过程中.，规划模块会根磨霄达有描到的障碍物信息，实时重新规划新的路径，实现自主避障6

　　4实验结果

　　为骚证系统的有效性各楱抉调试完成后，在R0S框架下进行联合调试，并便用Ryk进行界菌可视化，实验场景为校园场景，全局地图采用成都信息工程大擧分梆傘为0.2983m/像素的栅格地图(4846X2816)，黑色方框标注为实验地点，如图5所示。为验怔智能车在校园场景下是否能够实现自主导航规划，实验分为三部分^智能车.对静态障碍物、动态障碍物避障和卖际道路实测。本卖验将在栅格地虜上设置目标位寳，使智能车自主导航规划到自标位置，通过转折数量{智能车规划路径

　　40。为转角)和与障碍物的距离来评价规划的路径好坏s

　　4.1静态障碍物避障实验

　　静态障碍物避障实验中，实验场景设置在空旷的场地上，障碍物在智能车前方，设置目标点使智能车启主舞过障碍物到达目标点&实验结果如M6所示。

　　图6中，连接车辆的红色细线条为局部规划路径，连接车辆的红色粗线条为行驶过的轨迹，红色箭头为目标位置以及目标位置车头朝向，白色点集为障碍物，车身，苘围輝圈为雷达激光扫描地面得到的结果^根据图6可知，车辆在静态障碍物的避障实验中，规划路线成功绕过车辆与師点之间的障碍物，购障碍物保持安全距氣

　　4.2动态障碍物避障实验

　　动态避障实验中，给智能车设定目标俊豈，在车辆对向车辆，。智能车—(a)识别目标障碍物规划路径(c)车辆成功避开障碍物向目标僮置行进过程中，另外一辆本向反方向驶来，由于平合主要基于校园低速环境，故动态障碍物车辆速慶在10ba/h左右，实验结巣如图7所示。如图7(a)所示，动态障碍物车辆在较远处时，智能车已规划出避障路径，车辆靠近时，局部路径规划出更大转角，使智能车快速绕开动态障碍物如图7(b)所示。图7(c)中可以明显现察到智能车驶过的轨迹，在对向车辆掌近时出现明显的大转角，箐能车快速避开动态障碍物。图7(d)为智能车完整行驶轨迹。

　　4.3校园道路实验

　　校0道路实测实验中，选敢学校内一段路况相对较为复杂的路径作为实验场攀，逋路宽度为7m左右，该段道路一旁■停有部分车辆及自行车，可行驶道路区域为411去右。途中常着：行人来住?學：簾目标位：置后，智能车需要i人起始佼置自主导航到目标位置。实验结果如图8所示。

　　图8为道路实验结果。图8(a)中道路一旁停有车辆，道路另一旁有行人走过，智能车规划出安全路径行驶。图8(b)为驶过弯道，弯道外侧和内侧均有障碍物，智能车规划路径安全通过。图8(c)中，车辆到达目标位置。图8(d)为智能车行驶轨迹，全程行驶在道路内侧且未贴近路沿。在车辆行驶过程中对路径质量进行评价，其参数如表2所示。

　　由表2可以看出，无论是静态避障、动态避障还是道路实测实验，智能车均能与障碍物保持较安全的距离。道路实测时，智能车全程未贴近路沿，安全行驶在道路内侧。实验中最大转角为39°，转折数量均为0，规划路径平滑符合车辆运动学约束，智能车跟踪路径平稳。

　　5结束语

　　采用惯导和GPS组合导航的方式实现定位，利用激光雷达创建局部地图，利用混合As算法在当前校园地图进行路径规划，纯跟踪算法对路径解算获取速度和转角，通过RS232串口实现与底层的通信，从而控制电机驱动车辆移动，实现了车辆的自主导航。在校园内进行实验，实验结果表明：(1)智能车在静、动态障碍物环境中，能够自主避障;(2)智能车在校园非结构化道路中，能够有效地自主规划出平稳安全的行驶路径，在遇到无法通行的障碍物前，车辆会采取制动操作等待障碍物消失或离开后重新规划路径。校园实验表明，系统能够实现在校园内进行自主导航，为针对特定应用场景的无人接驳车、无人清洁车及快递派送车等的功能型智能车提供了有效的自主导航解决方案。