0 引言
无人弹药补给车是无人防空武器系统配套装备之一,是提升系统装备无人化、信息化、智能化水平,提高战场保障能力,缩短弹药补给时间的重要装备。随着当前战场环境的改变以及无人技术的不断发展,无人弹药补给车因其无人化、便捷化等特点,正逐渐成为现代战场中不可或缺的一环。精确定位与路径规划是无人弹药补给车运行的重要基础,无人弹药补给车能否到达既定目标点的关键是车辆的定位导航是否精确,精确的定位能有效减少无人弹药补给车到达路径点误差。路径规划是使无人弹药补给车能够高效地完成任务的又一重要前提,其目的是寻找一条从起始节点到目标节点的无碰撞路径[1-2]。高效合理、安全可靠的路径可以提高整个系统的执行效率,降低执行代价[3]。因此精确定位与路径规划是无人弹药补给车运行的基础。
针对无人车路径规划,在此领域有着大量的研究包括基于采样的快速搜索随机数方法、Voronoi图方法等[4-5],基于节点的Dijkstra算法、A*算法、D*算法等[6],基于模型的人工势场法、动态窗口法[7]等,以及基于生物启发式的神经网络算法、粒子群算法[8]和蚁群算法[9]等。
A*算法是一种启发式搜索算法,广泛应用于机器人和无人车自主路径规划。A*算法结构简单、搜索效率高,在静态全局环境下能得到最优解[10],但由于路径规划时需要一直查询Openlist与Closelist的节点,存在冗余节点较多、距离障碍物较近、搜索效率低等问题。为此,相关学者针对A*算法的问题提出了很多改进方法。JIA等[11]采用双向搜索的方法,加快了搜索效率,但扩展节点较多,消耗大量内存;张红梅等[12]加入安全距离代价值,使规划路径与障碍物保持安全距离;辛煜等[13]引入无线邻域的概念,极大减少拐点数量,但运算量较大;YU等[14]提出了跳点搜索策略,减少大量扩展节点,提高运算效率,但由于8向固定搜索机制,仍存在大量冗余节点。
综上所述,现有对A*算法的改良主要集中于对算法进行优化设计,较少与无人车自主定位联系起来。为此,本文中提出了一种改进的A*算法,通过引入加权系数的形式对搜索方向进行设计,减少冗余节点缩短路径规划时间。随后在此优化算法上引入无人车导航坐标系赋予栅格坐标以实际坐标值,使得规划出的路径具有实际坐标意义,为无人弹药补给车到达目标点提供了精确路线。
1 无人弹药补给车定位整体方案
1.1 无人弹药补给车硬件组合
由于无人弹药补给车运行环境主要为野外环境,因此车辆定位系统中可不需激光雷达和摄像头等进行辅助定位。结合无人弹药补给车性能要求,首先通过组合导航的方式获得无人弹药补给车的精确定位,再利用高斯-克吕格投影以及四参数法获得无人车平面坐标系,以便为后续的路径规划提供位置信息。其次利用改进A*算法进行全局路径寻优,寻求一条最优路径。为无人弹药补给车路径规划提供理论支撑。无人弹药补给车整体方案如图1所示。
图1 无人弹药补给车整体方案
Fig.1 Overall plan for unmanned ammunition supply vehicle
车辆定位系统由GPS-惯性组合导航与无线终端设备(DTU)模块组成。GPS-惯性组合导航由高精度测绘级卫星导航采集/数据处理单元、三轴MEMS陀螺仪、三轴MEMS加速度计组成,其可在星况良好的环境下提供厘米级定位精度,并在卫星信号遮挡、多路径等环境下保持位置、速度、姿态精度。但在实际操作中由于经常性信号遮挡等环境原因,该组合导航仅仅能达到分米级定位经度,因此需要引入实时动态测量(RTK)技术使得该定位系统达到厘米级定位经度以满足无人弹药补给车的实际需求。DTU模块则是通过 NTRIP Server 协议与差分数据中心通信,结合 GNSS 接收机,将 GPS 差分数据提交给差分数据中心,差分数据中心把差分数据传送给 NTRIP Client,从而实现更高精度的差分定位。定位系统安装如图2所示。
图2 组合导航系统安装
Fig.2 Integrated navigation system installation
1.2 无人弹药补给车坐标转换
此时无人弹药补给车定位系统获取到经纬度坐标为WGS-84坐标,并不能直接作为无人车导航坐标系使用,需通过高斯投影转换公式进行转换。高斯投影正算公式[15]为
(1)
y=
(2)
其中:
为投影带中央子午线经度, ρ=206 265,卯覃圈曲率
椭球第一偏心率e=2α-α2,辅助变量t=tanB, β=e′cosB椭球第二偏心率
分别为参考椭球的长、短半径,扁率α=(a-b)/a,X为赤道至纬度为B的平行圈的子午线弧长,其计算公式为
(3)
中国西起东经73°至东经135°,按照6°带划分总计11带(13～23带)。我国处于北纬地区,投影纵坐标全为正值,为了防止横坐标为负值,将纵坐标轴向西移500 km,分带之后的精度能够达到厘米级。
y=y+500 000.00
(4)
获得高斯平面坐标后再进行旋转变化,使得X轴成为横轴,Y轴成为纵轴。
(5)
由于WGS-84经过高斯投影转化之后获取的平面坐标过于复杂且无人弹药补给车活动范围较小,所以还需利用四参数转换[16]将坐标框定在一定的小范围内,四参数公式如下:
(6)
与代表原平面坐标系下的坐标,与代表目标平面下的坐标,Δx为x轴平移参数,Δy为y轴平移参数,α为坐标系旋转参数,m为尺度因子。
经过高斯-克吕格投影以及四参数转换,即可将WGS-84转换为无人车导航坐标系坐标,该坐标可将任意位置设置为原点,设置后可得到小范围内无人车导航精确坐标,以便于后续开发。
2 基于改进A*算法的全局路径规划
2.1 构建环境地图
在无人弹药补给车的路径规划中,构建地图是路径规划的前提。地图的种类有很多,具有真实物理尺寸的尺度地图,如特征地图、栅格地图等;不具备真实物理尺寸,只表示不同地点的连通关系和距离的拓扑地图,如铁路网等;还有每个地点和道路用标签集合表示的语义地图。
其中栅格地图即为栅格分解法[17],该方法采用栅格为单位表示环境信息,即利用栅格按照一定方式分解环境地图。分解环境地图的方式主要有均匀分解和递阶分解,均匀分解是将环境地图划分为大小一致的栅格;递阶分解首先将环境地图分为可行区域与不可行区域,利用二者大小确定栅格的大小。无人弹药补给车运行环境主要是野外环境,障碍物主要有一些块状障碍物为主,可以简化为栅格地图中的矩形障碍物。因此根据作战环境的特点使用栅格地图法进行环境建模,得到栅格地图后,在栅格该地图的基础上进行路径规划算法研究得到最优路径[18-19]。
2.2 传统A*算法
A*算法的基本原理是利用启发式搜索的思想,在保证找到最优解的前提下尽可能减少搜索的时间和空间开销[20]。它结合了广度优先搜索的盲目性和启发式搜索的启发性,能够更智能地选择搜索方向。 A*算法使用一个估价函数来评估节点的优先级,并根据优先级选择节点进行搜索。该估价函数由2个部分组成:实际代价(g值)和预估代价(h值)。实际代价(g值)表示从起始节点到当前节点的实际代价,它是通过遍历路径上节点的代价之和计算得出的。实际代价反映了从起始节点到当前节点的路径的真实开销。这些代价可以是两节点之间的距离、时间、费用等,根据具体问题而定。预估代价(h值)表示从当前节点到目标节点的预估代价,它是通过启发式函数计算得出的。启发式函数通常基于某种距离度量,如欧氏距离或曼哈顿距离。预估代价是一种启发式的估计,用于评估当前节点到目标节点的预期开销。该估价函数的表达式为
f(n)=g(n)+h(n)
(7)
式中: f(n)为节点 n 的估计总代价;g(n)为从起始节点到节点 n 的实际代价,也称为路径代价或已经走过的距离;h(n)为从节点 n 到目标节点的启发式估价代价,也称为启发式函数或估价函数。
2.3 基于加权函数的改进A*算法
A*算法能否更直接、更快速的寻找到最优路径,其关键点就在于代价函数的选取。在估价函数前加入权重因子,可以使得算法更倾向于目标点前进,选取合适的权重因子,可以保证在减少搜索点的同时,更快速的获取最快、最优路径。因此根据代价函数的特性,采用加权函数的形式将代价函数修改为
f(n)=g(n)+v(n)·h(n)
(8)
式中: v(n)为h(n)的权重。
通过调整权重因子v(n),可在搜索过程中灵活地权衡实际代价和启发式估计代价。较小的v(n)值会更加重视实际代价,使算法更趋向于选择已经走过的路径。较大的v(n)值会更加重视启发式估计代价,使算法更趋向于朝着目标节点前进。
如图3所示,基于加权代价函数的A*算法路径规划具体实现步骤如下:① 初始化。② 根据环境信息建立栅格地图。③ 将无人弹药补给车起始位置加openlist中,④ 迭代循环:a.遍历openlist,查找F值最小的节点,把它作为当前要处理的节点。b.把这个节点移到closelist。c.对于相邻点:如果该节点不在openlist中,将其加入openlist中。如果他已经在closelist中,则不处理,继续下一个节点。如果该节点存在于openlist或closelist中,判断当前节点的代价值是否更小,若成立,则用当前更小代价替代之前的代价,选取距离起始点和目标点的代价和最小的待选点进行拓展,以此节点作为下一次拓展的父节点,将其放到closelist中,并从openlist中删除。每次拓展,把得到的符合要求的点放到openlist中作为待选点,不断循环迭代,直找到目标节点或者没有点可拓展。⑤ 若找到了目标节点,通过回溯父节点的信息,从目标节点一直追溯到起始节点,即可得到最优路径。
图3 基于加权代价函数A*算法流程
Fig.3 Flow chart of A* algorithm based on weighted cost function
3 算法仿真与数据分析
3.1 关键参数对A*算法的影响
为说明改进A*算法的关键参数v(n)对A*算法的影响,采用Matlab对加权代价A*算法在加权系数v(n)不同时进行仿真试验。仿真过程如下:首先创建一个尺寸为60×60的栅格环境地图,将障碍物占比设定为0.4。基于此栅格地图设置起始点与目标点,分别取v(n)的值为0.5、1、3、5。图中黑色表示障碍物,红色圆圈表示起始位置,绿色方块表示目标位置。仿真结果如图4所示。
图4 关键参数v(n)对算法的影响
Fig.4 Influence of key parameters on the algorithm
由表1可知:加权代价系数v(n)取值的不同,算法搜索点的数量也不同。当v(n)<1时改进A*算法会优先考虑距离起点最短的路径,更加注重实际代价g(n)的影响,因此算法所搜索的搜索点会显著增加,这可能导致寻找的路径会消耗更多的计算资源。当v(n)=1时为传统A*算法。当 v(n)>1时改进A*算法会更加注重路径的启发式估价代价h(n),从而会优先选择更接近终点的节点,这可能会忽略某些重要的节点以至于不能准确地找到最短路径。例如图4中当v(n)=3时相较于传统A*算法在障碍物占比为0.4时,改进后的A*算法能保证在减少搜索点的同时寻找到最优路径。而当v(n)=5时搜索点进一步减少,此时改进后的A*算法不再能寻找到最优路径。因此v(n)的取值决定了算法性能的优劣。
表1 v(n)不同时的搜索点个数以及路径长度
Table 1 Number of search points and path length at different v(n)
搜索点个数路径长度v(n)=0.51 25868v(n)=157668v(n)=318668v(n)=516274
3.2 不同环境下的地图仿真试验
基于改进A*算法思想,采用Matlab对加权代价A*算法的全局路径规划进行仿真试验。仿真过程如下:首先创建栅格地图尺寸为60×60,并对2种不同环境下的环境地图进行仿真,分别是简单环境地图5(障碍物占比为0.2)和复杂环境地图6(障碍物占比为0.4)。基于此栅格地图随机设置起始点与目标点,并在算法搜索过程中,对子节点进行着色处理。图中黑色表示障碍物,黑色实线表示生成的全局路径。红色圆圈表示起始位置,绿色方块表示目标位置。由于权重v(n)取值的不同,路径规划的结果也不同,下图为v(n)=1和v(n)=3时在不同环境下路径的规划结果如图5、图6所示。规划所用的时间如表2所示。
表2 不同环境下所需时间
Table 2 Time required in different environments
简单环境用时/s复杂环境用时/sv(n)=17.5468.145v(n)=33.2713.572
图5 简单环境下路径规划
Fig.5 Path planning in a simple environment
图6 复杂环境下路径规划
Fig.6 Path planning in complex environments
v(n)=1时为传统A*算法,v(n)=3时为改进A*算法。由仿真结果可知,在简单环境下障碍物数量偏少,利用传统A*算法搜索路径时由图5及表2可知,不必要的搜索点过多导致搜索成本上升,使用改进A*算法搜索可有效地减少搜索成本。当在复杂环境时障碍物数量偏多,传统A*算法虽然在不必要搜索点的数量上有所减少,但与改进A*算法相比其搜索成本仍然偏高。但在复杂环境中改进A*算法存在过于重视启发式估计代价,使得算法更趋向于朝着目标节点前进以至于不能准确找到最短路径,因此在实际使用中应充分考虑v(n)得选取。综上所述改进后的A*算法可以减少计算成本,只需选取合适的v(n)便可明显优于传统A*算法,因此在无人弹药补给车路径规划中使用加权代价函数A*算法。
3.3 无人弹药补给车实车试验
基于Qt Creator对无人弹药补给车自主定位与路径规划进行相结合验证。调试界面如图7所示。
图7 无人弹药补给车调试界面
Fig.7 Unmanned ammunition supply vehicle debugging interface
该调试界面在右侧模块是上位机通过RS422串口接收定位数据,并通过高斯投影正算以及四参数法获取无人车导航坐标系坐标。界面左侧为基于加权A*算法的路径规划区域,根据实地环境建立栅格坐标。并且设置每个栅格的刻度,使得栅格坐标与无人车导航坐标可一一对应起来。基于无人弹药补给车调试界面流程如图8所示。
图8 无人弹药补给车调试界面流程
Fig.8 Unmanned ammunition supply vehicle debugging interface flow chart
首先,通过定位系统采集到4个已知点经纬度坐标并计算出与点1实际距离,再以转换后的以点1坐标为原点计算其他3点与该点距离,最后与实际距离进行比较并得出结论。4个采集点经纬度分别为点1(38.017 914 0,112.436 825 4),点2(38.018 004 1,112.436 825 4),点3(38.017 914 0,112.436 915 5),点4(38.018 004 1,112.436 915 5)。设置点1为原点,表3为各点到点1的距离。
表3 各点到点1的距离
Table 3 Distance from each point to point 1
转换后坐标转换后距离/m实际距离/m点1(0,0)00点2(0,10.001)10.00110.018点3(7.912,0)7.9127.892点4(7.912,10.001)12.75212.754
由表3可知,实际距离与转换后的距离误差均在厘米级经度,满足无人弹药补给车所要求的定位精度。
根据校园广场建立180×120栅格地图,权重系数v(n)=3,界面左侧以左上角栅格为原点、以0.5 m为单位的栅格坐标,建立了一块90 m×60 m具有实际意义的栅格环境坐标系,并在此栅格坐标系下分别基于传统A*算法以及加权A*算法对栅格地图进行路径规划,规划路径如图9,算法优化前后路径长度以及所用时间如表4。
表4 算法优化前后路径长度和运行时间
Table 4 The algorithm optimizes the path length and running time before and after
路径长度运行时间/s传统A*算法517.352加权A*算法513.157
图9 分别基于传统A*算法以及加权A*算法的路径规划
Fig.9 Path planning based on traditional A* algorithm and weighted A* algorithm respectively
经过无人弹药补给车实车试验可知,加权A*函数在保持路径长度不变的同时,通过减少搜索点的方式有效的加快了路径规划的时间。初步满足了无人弹药补给车路径规划的要求。
4 结论
针对无人弹药补给车自主定位与路径规划结合的问题,本文中利用在GPS-惯性组合导航中加入RTK技术以获得高精度的车辆定位,并通过高斯投影计算获得无人弹药补给车可以使用的无人车导航坐标。在路径规划部分提出了加权代价函数A*算法解决了路径规划中搜索时间长的问题,仿真及试验验证均表明改进后A*算法能更快找到最优路径。最后将栅格坐标系与无人导航坐标系融合的方式,将无人弹药补给车自主定位与路径规划结合。
[1]王雷,李明,蔡劲草,等.改进遗传算法在移动机器人路径规划中的应用研究[J].机械科学与技术,2017,36(5):711-716.WANG Lei,LI Ming,CAI Jincao et al.Application of improved genetic algorithm in path planning of mobile robot[J].Mechanical Science and Technology for Aerospace Engineering,2017,36(5):711-716.
[2]沈克宇,游志宇,刘永鑫,等.基于改进A*算法的移动机器人路径规划[J].计算机应用研究,2023,40(1):75-79.SHEN Keyu,YOU Zhiyu,LIU Yongxin,et al.Path planning of mobile robot based on improved A* algorithm[J].Application Research of Computers,2023,40(1):75-79.
[3]陈继伟,包长春,赵子恒.基于改进A*算法的物流无人机航迹规划研究[J].软件导刊,2023,22(11):123-128.CHEN Jiwei,BAO Changchun,ZHAO Ziheng.Research on logistics UAV trajectory planning based on improved A* algorithm[J].Software Guide,2023,22(11):123-128.
[4]KOTHARI M,POSTLETHWAITE I.A probabilistically robust path planning algorithm for UAVs using rapidly exploring random tree[J].Journal of Intelligent andRobotic Systems,2013,71(2):231-253.
[5]BHATTACHARYA P,GAVRILOVA M L.Roadmap-basedpath planning-using the voronoi diagram for a clearance based shortest path[J].IEEERobotics &Automation Magazine,2008,15(2):58-66.
[6]张彪,曹其新,王雯珊.使用三维栅格地图的移动机器人路径规划[J].西安交通大学学报,2013,47(10):57-61.ZHANG Biao,CAO Qixin,WANG Wenshan.Path planning of mobile robot using 3D raster map[J].Journal of Xi’an Jiaotong University,2013,47(10):57-61.
[7]FOX D,BURGARD W,THRUN S.The dynamic window approach to collision avoidance[J].IEEE Robotics & Automation Magazine,1997,4(1):23-33.
[8]ABAAS T F,SHABEEB A H.Autonomous mobile robot navigation based on PSO algorithm with inertia weight variants foroptimal path planning[J」.IOP Conference Series:MaterialsScience and Engineering,2020,5(1):13-23.
[9]LUO Q,WANG H B,ZHENG Y,et al.Research on path planning of mobile robot based on improved ant colony algorithm[J」.Neural Computing and Applications,2020,32(1):15-23.
[10]王维,裴东,冯璋.改进A*算法的移动机器人最短路径规划[J].计算机应用,2018,38(5):1523-1526.WANG W,PEI D,FENG Z.Shortest path planning of Mobile robot withimprove A* algorithm[J].Journal of Computer Applications,2018,38(5):1523-1526.
[11]JIA J,PAN J S,XUHR,et al.An improved JPS algorithm in symmetric graph[C」//Third International Conference onRobot.IEEE,2016.
[12]张红梅,李明龙,杨乐.基于改进A*算法的移动机器人安全路径规划[J].计算机仿真,2018,35(4) ;319-324.ZHANG Hongmei,LI Minglong,YANG Le.Safe path planning of mobile robot based on improved A* algorithm[J].Computer Simulation,2018,35(4):319-324.
[13]辛煜,梁华为,杜明博,等.一种可搜索无限个邻域的改进A*算法[J].机器人,2014(5):627-633.XIN Yu,LIANG Huawei,DU Minggo,et al.An Improved A* algorithm for searching infinite neighborhoods[J].Robot,2014(5):627-633.
[14]YU X,LIANG H W,DUMB,et al.An improved A* algorithm for searching infinite neighborhoods[J」.Robot,2014,36(5):627-633.
[15]CHANG G,XU T,WANG Q.Error analysis of the 3D similarity coordinate transformation[J].GPS Solutions,2017,21:963-971.
[16]于中伟.四参法和七参法在GPS-RTK坐标转换中的应用[J].黑龙江科技信息,2017(9):129-130.YU Zhongwei.Application of Four-parameter method and seven-parameter method in GPS-RTK coordinate conversion[J].Heilongjiang Science and Technology Information,2017(9):129-130.
[17]JUNTAO L,TINGTING D,YUANYUAN L,et al.Study on robot path collision avoidance planningbased on the improved ant colony algorithm[C]//2016 8th International Conference onIntelligent Human-Machine Systems and Cybernetics(IHMSC).IEEE,2016,2:540-544.
[18]PARK W I,KIM D J,LEE H J.Terrain trafficability analysis for autonomous navigation:A GIS-based approach[J].International Journal of Control,Automation and Systems,2013,11(2):354-361.
[19]HEBECKER T,BUCHHOLZ R,ORTMEIER F.Model-based local path planning for UAVs[J].Journal of Intelligent &Robotic Systems,2015,78(1):127-142.
[20]LIN C J,SIE T Y,CHU W L,et al.Tracking control ofpneumatic artificial muscle-activated robot arm based onsliding-mode control[J].Actuators,2021,10(3):66-72.