基于RRT改进算法的伪卫星导航轨迹纠偏策略研究.pdf

2023 年 8 月第 4 期 现代导航 235 基于 RRT 改进算法的伪卫星导航轨迹纠偏策略研究 何 鑫，陈鲤文（福建工程学院泛在感知与多传感器智能融合研究所，福州 350118）摘 要：针对现有基于伪卫星的室内小车导航实际轨迹偏离的问题，提出了一种基于快速搜索随机树法（RRT）改进算法的导航轨迹纠偏方案。在轨迹纠偏方案中，利用软件无线电（SDR）软件对小车原有的遥控信号进行采集并进行调制，得到可以操控小车的信号；并在纠偏程序中添加改进的 RRT 算法在小车轨迹偏离时进行轨迹规划，从而达到小车轨迹纠偏。结果表明，此方案在进行伪卫星室导航时减小了小车的轨迹偏差，纠偏效果明显。关键词：伪卫星；室内导航；软件无线电；快速搜索随机树算法 中图分类号：TN953 文献标志码：A 文章编号：1674-7976-(2023)-04-235-07 Research on Trajectory Correction Strategy for Pseudo Satellite Navigation Based on Improved RRT Algorithm HE Xin,CHEN Liwen Abstract:A navigation trajectory correction scheme based on improved Rapidly-exploring Random Tree(RRT)algorithm is proposed to address the issue of actual trajectory deviation in indoor car navigation based on pseudo satellites.In the trajectory correction scheme,Software Defined Radio(SDR)is used to collect and modulate the original remote control signals of the car,obtaining signals that can control the car;And an improved RRT algorithm is added to the correction program to plan the trajectory of the car when it deviates,thereby achieving the correction of the car trajectory.The results show that this scheme reduces the trajectory deviation of the car during pseudo satellite navigation,and the correction effect is significant.Key words:Pseudo Satellite;Indoor Navigation;Software Defined Radio;Rapidly-exploring Random Tree Algorithm 0 引言 全球导航定位系统（Global Navigation Satellite System，GNSS）主要由美国 GPS、俄罗斯 GLONASS、中国北斗和欧盟伽利略等组成1。GNSS 在各行各业的运用愈加广泛，但是复杂的室内环境或者室外有大范围建筑物遮挡的情况使得用户无法正常接 收稿日期：2023-04-28。何鑫（1999.02），福建福州人，硕士研究生，主要研究方向为导航定位技术与无线通信技术。收到卫星信号。面对此问题，提出了超声波、红外线、地磁、可见光和无线电等技术方案，实现了为待检测目标提供主动的导航定位或者是被动的目标位置检测的服务。其中无线电方案中，有 WiFi、超宽带、射频识别（Radio Frequency Identification，RFID）和伪卫星技术等。其中伪卫星技术是一种能发射出类似于GNSS卫星信号的无线电导航信号的手段，伪卫星设备通常安装于露天矿场、地下空间、城市峡谷、大型室内环境及高度较低的开阔地带等，因GNSS卫星信号的遮蔽现象而需测试验证和增强GNSS 卫星信号的定位性能，从而让用户享受更便 236 现代导航 2023 年 捷、更高效的定位服务2。伪卫星技术在 GPS 卫星发射之前就得到了验证研究。伪卫星技术相较于无线电方案中的其他方案，优点在于：1）其传输距离可以达到数百米甚至是数百千米远，在一次投入建设后，可以在很大的范围内实现室内定位，相对而言建设成本会相应减少；2）伪卫星信号类似于真实的卫星信号，其信号组织结构与真实的卫星信号高度相似，可以根据实际需求更改其定位方式，以确保实现其室内外的无缝定位可能性。在伪卫星技术中，伪卫星室内导航技术是一个重要的使用场景，此技术通过模拟轨迹和发射 GPS生成式信号3，可以为在室内接收不到 GPS 信号从而无法行驶的设备提供帮助。但由于室内环境复杂，会产生多径效应等问题4，发射端发射的路径与小车接收到的路径信号产生较大的偏差，导致小车最终的导航路径不理想5。为了解决此问题，提出了一种室内导航轨迹纠偏的策略，在进行伪卫星导航的同时，通过小车传回的实际轨迹数据与模拟轨迹作对比，在轨迹偏差达到一定范围的情况下，在纠偏程序中利用快速搜索随机树法（Rapidly-exploring Random Tree，RRT）改进算法规划偏离点到指定点的路径，再采用软件无线电（Software Defined Radio，SDR）和全向天线发送遥控信号遥控小车，最终实现对室内小车的轨迹纠偏。此套策略首先使用 SDR采集小车的遥控信号和并编写含有使用 RRT 改进算法的纠偏程序，再搭建硬件平台进行测试。1 系统模型 在此章节中，介绍了以下四个部分。首先是此纠偏系统的系统总体流程，其次是 SDR，最后是RRT 改进算法的基本原理和纠偏方案。1.1 系统总体流程图 系统总体流程如图 1 所示。此系统总体模型由三个部分组成：1）伪卫星导航系统；2）采集信号系统；3）轨迹纠偏系统。在采集信号方面，通过连接 SDR 以及开发板HackRF 对小车的原遥控信号进行采样。图 1 系统总体流程 40.82 MHz1.575 42 GHzRRT改进算法规划轨迹偏离误差3 m 开始 室内导航轨迹纠偏系统伪卫星导航 实时星历 制定的模拟轨迹 数字卫星 信号 软件无线电SDR 模拟信号 功放1 天线1 采集信号小车原遥控信号 软件无线电SDR 功放2 天线2 小车 轨迹纠偏 小车实际轨迹数据 实际和模拟轨迹对比 导航继续 进行 结束YN 第 4 期 何鑫等：基于 RRT 改进算法的伪卫星导航轨迹纠偏策略研究 237 系统总体流程如下：1）在伪卫星导航系统方面，首先通过 Socket编程将计算机与树莓派连接实现相互通信，树莓派生成数字信号。再连接 SDR 开发板 BladeRF 实现信号的数字信号转换为模拟信号（Digital-to-Analog，D/A）转换，通过外部功率放大器实现输出模拟信号，此信号被放大。最后将 1.575 GHz 的伪卫星轨迹生成式信号通过全向天线发送给小车进行基础的导航。小车在接收到伪卫星信号后会通过内部解析伪卫星轨迹信号程序生成小车下一步的轨迹，并通过小车底层 stm32 模块控制连接车轮的电机转动实现小车的导航移动。2）通过里程计计算出实际轨迹数据，并将数据传回上位机。上位机将接收到的实际轨迹与原先制定的模拟轨迹进行误差对比。判断是否需要进行轨迹纠偏。3）若需要轨迹纠偏，轨迹纠偏程序会选取当前小车的位置为起点，通过 RRT 改进算法规划出当前点到下一个导航点的路径，将它们转化为相应的控制信号。此后再将这些控制信号传给开发板HackRF，HackRF 把这些信号通过 40.82 MHz 的频率发射给小车，从而达到轨迹纠偏的目的。1.2 SDR 及硬件平台 SDR，就是用现代化软件来操纵、控制传统的“纯硬件电路”的无线通信。SDR 技术的重要价值在于：传统的硬件无线电通信设备只是作为无线通信的基本平台，而许多的通信功能则是由软件来实现，打破了有史以来设备通信功能的实现仅仅依赖于硬件发展的格局。SDR 技术的出现是通信领域继固定通信到移动通信，模拟通信到数字通信之后第三次革命6。在 20 世纪 90 年代，美国的 Joe Mitola 首次提出了 SDR 的概念7，随后 SDR 技术便引发了一场通信领域的技术革命。其中美军首先对其进行了技术开发，研制出了为军方服务的一种多频段多模式的无线电系统（Multi-Band Multi-Mode Radio，MBMMR）8，目的是方便不同的军种之间实现共同作战。之后，美国在此基础上进行了多次研究，并且在 1997 年通过了联合战术无线电系统计划，此无线电作战系统使用 SDR 技术实现的硬件架构可以真正做到统一海陆空等各个军种之间所使用的不同频段不同制式的无线电平台，并且还具有保密以及抗干扰的功能。SDR 接收信号流程图如图 2 所示。在图 2 左侧，samp_rate 是采样率，代表接收电磁波频率的宽度，也将是生成波形图的宽度。center_freq 是中心频率，此系统中定义中心频率为 40 MHz，channel_freq 是引导频率。在图 2 右侧，总共有四个模块：1）osmocom Source（开源移动通信协议）模块，是最基本的获取信号输入的模块；2）Signal Source 模块是人工加一个输入信号，由 CPU 生成需要的信号，和 osmocom 的原始信号相乘，起到转移中心频率的效果；3）Multiply（乘法）模块；4）QT GUI Sink 模块用来显示收到的波形。图 2 SDR 接收信号流程图 outoutin0in1outin 238 现代导航 2023 年 硬件平台如图 3 所示，此硬件平台由四个部分组成：1）搭载 Ubuntu 系统的发射伪卫星信号和进行纠偏算法计算的电脑；2）将发射信号功率放大的功率放大电路；3）发射信号的全向天线；4）接收导航和纠偏信号行驶的小车。图 3 硬件平台 1.3 算法基本原理 RRT 算法由 Steven M.LaValle 和 James J.Kuffner Jr 开发，为一种递增式的构造方法，在构造过程中，算法不断在搜索空间中随机生成状态点，如果此点位于无碰撞位置，则寻找搜索树中离此节点最近的节点为基准结点，由基准节点出发以一定步长朝着此随机节点进行延伸，延伸线的终点所在的位置被当做有效节点加入搜索树中。这个搜索树的生长过程一直持续，直到目标节点与搜索树的距离在一定范围以内时终止9。虽然 RRT 算法在路径规划方面相对效率较高，且可以较好地处理非完整约束的路径规划问题，但是 RRT 算法的不足之处在于往往最后给出的可行路径不是相对优化的10。因此，对 RRT 算法进行改进，得到 RRT 改进算法。此 RRT 改进算法是 RRT算法的优化算法，其在 RRT 算法的基础上加入了两个模块，两个模块如下：1）加入一个 review 模块，可重新为新节点newx择父节点；2）加入一个重布线模块。RRT 改进算法流程如图 4 所示。图 4 RRT 改进算法总体流程 RRT 算法的示意图如图 5 所示，在图 5 中，先 设起始点为startx，目标点为goalx。利用 Sample 函 数来随机采样，在工作空间中得到一个随机采样点randx。此后再在工作空间中找出一个节点nearx，nearx节点是所构建的树中距离随机采样点randx最近的一个节点。当算法刚开始时，所构建的树中只有一个节点，节点为起始点startx。将节点nearx指向随机采样点randx的方向设置为新节点扩展的方向，连接节点nearx和随机采样点randx。设置步长为s，在连接线上从节点nearx生长一个步长生成新节点newx，节点nearx被称为新节点newx的父节点 parentx。开始 确定起、终点及障碍物 生成随机采样点所构建树中寻找离采样点最近的节点 采样点连接节点，在树生长的一步长方向产生一新节点是否有比父节点更近的节点？更近的节点作为新节点的新父节点 是否穿过障碍物？邻近节点的父节点改为新节点，整个路径步长是否最小？新节点到目标点的距离是否小于一个步长？结束YYYYNNNN重新布线 第 4 期 何鑫等：基于 RRT 改进算法的伪卫星导航轨迹纠偏策略研究 239 图 5 RRT 算法规划示意图 新节点newx的位置可表示为节点nearx加上在节点nearx到随机采样点randx方向上的步长s，表达式如式（1）所示 nearrandnearrandnearnewxxxxsxx （1）判断连接randx和nearx的连接线是否穿过了障碍物，若穿过了障碍物就判定放弃产生新节点newx，否则产生新节点newx。介绍 RRT 改进算法在 RRT 算法基础上添加的模块如图 6 所示。首先定义一个半径范围r，当开始第二次产生新节点newx时，新节点newx会在半径 范围内寻找父节点parentx的备选节点。依次计算每 个备选节点到新节点newx的步长，最后选择步长最 短的节点作为新的父节点parentx。这就是 review 模 块。图 6 RRT 改进算法规划示意图 在为新节点newx重新选择父节点之后，为了达到更加优化的路径效果，需要为随机树进行重新布线，即若邻近节点的父节点改为新节点newx可以优化路径效果，则进行更改。最后，判断新节点newx到目标点goalx的距离是 否小于一个步长s。若小于一个步长s，则此次路径规划结束，反之继续进行上述步骤。在同样场景下 RRT 算法与改进 RRT 算法的路径规划结果如图 7 所示。从图 7 中可以看出，改进RRT 算法的路径距离更短。图 7 RRT 与 RRT 改进算法规划路径结果 1.4 小车纠偏 在制定好模拟轨迹后，先将模拟轨迹的经纬度路径转化成以 X 为横轴、Y 为纵轴二维坐标轨迹，并以轨迹的第一个点为起始点(0,0)。小车在接收到伪卫星轨迹生成式信号并开始基础的导航后，小车自带的里程计会通过小车的移动来记录小车的移动轨迹，里程计的起始点的坐标也是为(0,0)。设定每隔10 s的时候小车将里程计记录的轨迹数据发回上位机电脑。将此轨迹数据与转化为 XY 坐标的相同时段的模拟轨迹数据进行对比，以 3 m 为最大可允许偏离值maxd，判断小车是否需要进行轨迹纠偏。若偏离值d大于maxd，则进行轨迹纠偏。规划路径分段示意图如图 8 所示，图 8 中黑色方形为障碍物，左右两端分别是起始点startx和目标 点goalx，连接两点的是通过 RRT 改进算法规划出 当前点到下一个导航点的路径，将纠偏路径信息分段成多个路段为“路径分段 1”，“路径分段 2”直至“路径分段 n（n=1,2,3）”。并以每个路段相对于上一个路段的方向来判断小车在此路段的行驶方向，将它们转化为相应的控制信号。最后将这些控制信号传给开发板 HackRF 发射给小车。xgoalxrand xnew xnear/xparent s xnew xstart xparent xparentxnew xnew xstart xstart 2 2 4 2 8 32 2 4 2 2 4 2 2 2 8 34 1 1 2 2 4 2 8 2 2 31 RRT 算法 改进 RRT 障碍物 xgoalxstart 240 现代导航 2023 年 图 8 规划路径分段示意图 2 实验结果 2.1 遥控信号采集实验结果 通过 SDR 来分别采集小车原本的前、后、左、右四个方向遥控信号。采集的小车原遥控信号如 图 9 所示，在 40.845 MHz 左右的频率上，可以在软件窗口上方观察到四信号突出达到-50 dB 左右。在软件窗口下方蓝色背景中可以观察到黄色的电波，此电波就是接收机连接的天线接收到的小车遥控信号。（a）向前信号 （b）向后信号 （c）向左信号 （d）向右信号 图 9 采集的小车原遥控信号 2.2 轨迹纠偏实验结果 轨迹纠偏程序中，为了使得程序的数据更好地计算和观察，通过经纬度转换成XY坐标程序算法，将模拟轨迹与实际轨迹的经纬度分别转换成 XY 坐标轴中的 X 和 Y，单位为 m。未使用纠偏方法的基于伪卫星室内导航小车的模拟轨迹和实际轨迹的对比如图 10 所示。可从图 10 中观察到，因为电波在室内环境较为复杂，小车前期出现偏差致使整体轨迹达不到预期的效果，使用纠偏方法之前，在坐标(5,25)左右，小车的实际路径轨迹相比定制的轨迹提早开始向右前行驶。在坐标(40,3)左右，小车又提早开始向左前行。小车出现此现象的原因可能有：1）小车可能对于接收到的信号指令没有立即执行，并与此后接收到的信号指令重叠，导致小车出现轨迹偏差；2）小车只接收到了部分伪卫星发射的信号指令，无法完整地通过伪卫星信号的指令行驶，使得实际轨迹与模拟出现偏差。图 10 纠偏前的轨迹 使用纠偏方法的基于伪卫星室内导航小车的模拟轨迹和实际轨迹的对比如图 11 所示。可从 图 11 中观察到，在坐标(5,25)之后定制轨迹已开始向右前方前进时，小车还未做出及时的向右前方前xgoal障碍物 xstart 路径分段1路径分 段2 路径分段 n（n=1,2,3）0 10 20 30 40 50 60 70 80 90 100 110 120 横向距离/m 模拟轨迹实际轨迹9080706050403020100纵向距离/m 第 4 期 何鑫等：基于 RRT 改进算法的伪卫星导航轨迹纠偏策略研究 241 进的动作，致使小车偏离。当小车到达(8,32)左右时，系统判断小车已经偏离了轨道，对小车进行轨迹纠偏。此后在坐标(38,2.5)和坐标(70,65)左右小车也产生了偏离，系统也对小车进行轨迹纠偏。图 11 可以看出整体达到了预期效果。对比图 10 和图 11，可以看出在使用了基于RRT 改进算法的纠偏算法的室内导航系统，被导航的小车的行驶轨迹得到了较为明显的提升，改善了因为电波在室内环境较为复杂，小车出现偏差致使整体轨迹达不到预期的效果。图 11 纠偏后的轨迹 3 结语 针对现有基于伪卫星的室内小车导航轨迹不精准的缺点，提出了一种基于 RRT 改进算法的导航轨迹纠偏方案。在轨迹纠偏方案中，利用 SDR软件对小车原有的遥控信号进行采集并进行调制，得到可以操控小车的发射信号；对原有的 RRT 算法进行改进，优化了小车从偏离点到定制轨迹上的路径，为伪卫星室内导航轨迹偏离提供了一种有效方案。实验结果表明：此方案在进行伪卫星室导航时减小了小车的轨迹偏差。参考文献：1 刘健，曹冲.全球卫星导航系统发展现状与趋势J.导航定位学报，2020，8（1）：1-8.2 作蒋锐，虞跃，徐友云，等.基于 CHAN 的改进卡尔曼滤波室内定位算法J.通信学报，2023，44（2）：136-147.3 张会锁，高关根，寇磊，等.利用轨迹诱导的欺骗式GPS 干扰技术研究J.弹箭与制导学报，2013，33（3）：149-152.4 谢钢.GPS 原理与接收机设计M.北京：电子工业出版社，2009.5 陆伟.基于伪卫星的小车室内导航技术研究D.福州：福建工程学院，2022.6 余亮亮，杨智明，俞洋，等.基于软件无线电的通信原理实验平台设计J.江苏通信，2021，37（1）：46-48+53.7 袁良晨.基于 AD9361 软件无线电收发机设计与实现D.西安：西安科技大学，2018.8 C.Andrews.A Passive Mixer-First Receiver With Digitally Controlled and Widely Tunable RF InterfaceJ.IEEE JSSC，2010：2696-2708.9 林梓健，刘凯，林群煦.路径规划算法的研究综述J.现代信息科技，2023，7（4）：75-80.10 李金良，舒翰儒，刘德建，等.基于改进 RRT 路径规划算法J.组合机床与自动化加工技术，2021（2）：22-24+29.0 10 20 30 40 50 60 70 80 90 100 110 120 横向距离/m 90 80 70 60 50 40 30 20 10 0 纵向距离/m 模拟轨迹实际轨迹