本发明描述了一种基于ros的机器人使用高精度bds/gps和雷达多传感器实时协同定位与导航技术，属于机器人自主导航定位技术领域。
背景技术：
基于机器人操作系统(ros)的室外机器人具有实时导航与定位能力，在3d环境重建、巡逻与救援等领域应用广泛。机器人载体通过自身携带的传感器在完全陌生的环境下通过感知到的信息估计自身的运动并实时的构建周围环境地图，为进一步的避障和路径规划等任务奠定基础。一方面，现有基于ros的机器人都缺乏高精度gnss定位功能的直接支持，本发明将高精度gnss定位模块整合到基于ros的机器人中。另一方面，由于室外的建筑物阻挡或反射等情形会导致gnss存在多径误差影响和非视线情景，而雷达可以通过实时的点云扫面匹配解算出载体位姿，其定位不受信号的影响，一般方法把两者定位互相作为备份，以提供持续可靠的定位，本发明以里程计运动模型作为系统状态方程，以两个定位结果作为量测方程，通过粒子滤波算法得到鲁棒性更强的定位结果。
技术实现要素：
本发明的目的在于提供一种基于gnss和激光雷达的实时高精度协同定位与导航方法，首先，该方法整合高精度gnss定位模块到ros中，能在ros中提供实时的高精度gnss定位结果，为户外机器人提供全局定位；其次，利用激光雷达在ros中发布的定位信息，与gnss发布的信息进行融合，以提升定位精度并增强适用性。
本发明所采用的技术方案提供一种基于高精度gnss实时协同定位的ros机器人导航方法，所述ros是机器人操作系统，将高精度gnss定位模块整合到ros中；将gnss定位结果转化到utm坐标系中，并将utm坐标框架加入到ros的坐标转换树中，实现全局坐标和局部坐标之间的连接；通过高精度gnss定位结果对机器人自身里程计进行校正，实现机器人全局定位。
而且，结合gnss与雷达进行室外机器人的协同定位与导航。
而且，在gnss信号受多径误差影响和非视线情景时，采用雷达定位代替gnss定位；在gnss信号良好时，gnss定位结果作为雷达点云解算的全局初始位姿，使雷达在室外具有全局定位能力，同时gnss定位结果对雷达定位结果与待选粒子点云进行最小距离判断计算，滤除受噪声干扰的粒子点云，使得定位精度提高。
而且，所述将高精度gnss定位模块整合到ros中，实现方式为，在gnss定位模块中建立广域改正数据接收节点、串口数据读取节点和广域高精度定位算法解算rtk节点，
在广域改正数据接收节点中使用tcp客户端连接广域播发数据服务器，接收精密卫星轨道、精密卫星钟差以及电离层改正数据，之后经过解析，并按照ros中的数据格式进行封装并发布给广域高精度定位算法；
gnss定位芯片接收到的卫星数据会通过串口传出，通过串口数据读取节点读取数据，进行解析，并进行ros消息格式封装后也发布给广域高精度定位算法；
广域高精度定位算法开始执行，实时计算出当前的高精度定位结果。
采用本发明所述方法，与现有技术相比，本发明首次将高精度gnss定位模块整合到ros中，为室外机器人提供了良好的定位服务。其次，通过对gnss与激光雷达定位结果进行融合，得到定位稳定性大大优于单一传感器导航，实现机器人在室外大规模复杂环境连续性无缝定位，增强了系统的适用性和鲁棒性。
附图说明
图1为本发明实施例的协同定位系统感知框架图。
图2为本发明实施例的机器人操作系统ros中的高精度gnss定位模块相关节点示意图。
图3为本发明实施例的gnss和激光雷达协同定位算法流程图。
具体实施方式
为了便于本领域普通技术人员理解和实施本发明，下面结合附图及实施例对本发明作进一步的详细描述，应当理解，此处所描述的实施示例仅用于说明和解释本发明，并不用于限定本发明。
本发明提出，一种基于ros的bds/gps和激光雷达进行高精度实时协同定位与导航技术，首先基于ros实现高精度gnss定位模块，并实现ros中高精度gnss定位结果的发布、gnss与激光雷达定位结果融合的协同定位。
所述核心模块是基于ros开发的高精度gnss定位模块，它包含了现有的广域高精度定位算法，能提供高精度定位，支持快速rtk。
为了在ros中发布高精度gnss定位结果数据的消息，可行的流程是：将现有的高精度gnss定位模块整合到ros中；将gnss定位结果转化到utm坐标系中，并将utm坐标框架加入到ros的坐标转换树中，实现全局坐标和局部坐标之间的连接；通过高精度gnss定位结果对机器人自身里程计进行校正、实现机器人全局定位。
进一步地，本发明实现gnss与雷达进行室外机器人的协同定位与导航。在gnss信号受多径误差影响和非视线情景时，采用雷达定位代替gnss定位；在gnss信号良好时，gnss定位结果作为雷达点云解算的全局初始位姿，使雷达在室外具有很好的全局定位能力。同时gnss定位结果对雷达定位结果与待选粒子点云进行最小距离判断计算，滤除受噪声干扰的粒子点云，使得定位精度提高。
本发明实施例提出一种gnss与雷达进行多传感器数据融合的高精度实时定位导航方法，主要包括以下几步：
第一步：现有高精度gnss定位模块整合到ros中。首先，在gnss定位模块中建立广域改正数据接收节点、串口数据读取节点和广域高精度定位算法解算rtk节点。一方面，在广域改正数据接收节点中使用tcp客户端连接广域播发数据服务器，接收精密卫星轨道、精密卫星钟差以及电离层改正等数据，之后将这些数据经过数据解析模块解析，并按照ros中的数据格式进行封装并发布给广域高精度定位算法解算rtk节点。另一方面，gnss定位芯片接收到的卫星数据会通过串口传出，通过串口数据读取节点读取数据，进行数据解析模块解析并进行ros消息格式封装后也发布给广域高精度定位算法解算rtk节点。然后广域高精度定位算法开始执行，实时计算出当前的高精度gnss定位结果。
第二步：将gnss坐标框架加入到ros坐标体系，通过高精度gnss定位结果对机器人自身里程计进行校正、实现各坐标框架之间的变换。首先建立gnss坐标转换节点，通过订阅广域高精度定位算法解算rtk节点发布的高精度gnss定位结果消息获取经纬度信息，然后将经纬度坐标转化为墨卡托平面坐标utm发布出来，并将该坐标框架加入到ros的坐标变换树中，完成全局坐标和局部坐标的完整转换。
第三步：在ros中将gnss和雷达发布的定位结果经过粒子滤波方法融合，得到鲁棒性更好，精度更高的定位结果。首先，给定机器人的初始位姿，采用高斯分布进行随机采样以得到粒子群；然后根据里程计得到里程计信息，使用里程计运动模型模拟粒子群的运动。接着以gnss和雷达的定位结果作为观测方程，观测方程的核心在于如何计算粒子权重，在这里雷达数据量测的权重使用概率栅格地图匹配的方式得到，gnss数据量测的权重通过以下公式计算得到；
其中zk为当前时刻的测量值，
具体实施时，可采用软件技术实现自动运行。
实施例在将高精度定位程序整合到ros后，实际进行定位导航时，采用以下步骤：
步骤1：从ros中的tcp客户端接收来自广域播发数据服务器播发的gnss相关数据，从串口接收底层gnss接收机接收到的卫星数据，使用激光雷达扫描得到环境点云数据。
步骤2：将广域播发数据服务器播发的gnss相关数据经过数据解析模块解析并封装得到满足ros要求的数据格式在主题上发布；将底层gnss接收机接收到的数据经串口传出，经过数据解析模块解析并进行ros消息格式封装，也在主题上发布。当接收到以上两个主题后，开始执行现成的广域高精度定位算法，实时计算出当前的高精度定位结果。
步骤3：定义gnss坐标系标架，将步骤2得到的坐标位置定义坐标系/utm，并与ros中的地图坐标系进行转换并修正，通过tf树实现/utm与里程计坐标系/odom和机器人自身坐标系/baselink的转换。通过gnss坐标能发布机器人航迹信息，为室外机器人提供全局定位结果。
步骤4：使用gnss定位结果作为激光雷达点云迭代解算位姿变化的初始值，也可解算得到机器人的位姿。
步骤5：以里程计信息作为机器人的运动模型来描述系统的状态方程，根据gnss和雷达定位结果作为系统的量测方程，使用粒子滤波融合算法来得到定位精度更高，鲁棒性更强的定位结果。
参见图1，图1为ros中实现多传感器信息融合定位的机器人框架，主要分为多源传感器层、协同定位感知层和底层控制层。多源传感器层主要获取外部高精度地图和基于激光雷达获取的点云信息，分别通过地图服务模块和激光雷达模块实现，这两个都传递给协同定位感知层，底层控制层的里程计信息也作为外部感知信息传输到协同定位感知层。协同定位感知层一方面通过对点云信息进行处理获取定位结果，另一方面通过高精度gnss定位模块能获得高精度gnss定位结果，二者结合里程计信息，通过简易粒子滤波算法实现鲁棒性更好的定位结果。接着将定位结果传递给自主规划模块，自主规划模块基于地图信息、实时采集到的点云信息转转化后的障碍物信息和高精度gnss定位结果进行全局规划和局部规划，并将控制信息传给底层控制层。底层控制层接收来自自主规划模块的控制命令后，基础控制器模块就驱动机器人运动，并将运动里程信息反馈给协同定位模块。
参见图2，图2为基于ros的高精度gnss定位模块的实现框图。此部分主要依托ros中的话题订阅与发布机制，主要使用了三个节点。节点recvclient4ros处理来自广域数据服务器的数据，通过tcp客户端连接广域数据服务器，然后将接收到的数据解析，最后将数据封装成ros要求的数据格式后在话题/correctnum上发布消息。节点gnss_meas处理来自底层gnss接收机的数据，先将串口采集到的数据进行解析，然后将数据封装成ros要求的数据格式后在话题/gnssmeas中发布消息。节点rtk_server通过订阅上面两个话题中的数据并结合来自ros参数服务器中的参数进行高精度gnss定位结果的解算，首先由ros参数服务器读取默认参数，根据ros中的提供的服务启动rtklib_fetchparam服务和rtklib_reset服务，前者提供rtk数据发送服务，后者接着提供rtk重置服务。将订阅到的话题/correctnum和话题/gnssmesa中的数据输入到广域高精度定位算法模块中进行解算，得到的高精度实时定位结果分别在话题/beseline和话题/latlon中发布。
参见图3，图3为基于gnss定位和激光雷达定位的数据融合算法流程图。其中/utm为gnss定位结果所在坐标系，/odom为里程计坐标系，/baselink为机器人坐标系，/map为全局坐标系，并且坐标系之间的转换具有可逆性。初始化过程主要实现雷达定位和gnss定位配准，将两者定位统一在一个坐标框架下，并根据给定的初始定位误差方差矩阵初始化粒子群；其次以里程计运动模型模拟粒子的运动，进行系统状态更新，以gnss和雷达两者分别定位，将两个定位结果作为粒子滤波的量测更新，分别计算粒子的权重，归一化重要性权值；接着，进行重采样，根据预设的阈值舍弃权重小的粒子，将权重大的粒子复制到新的粒子，用这些新粒子代替舍弃的粒子，为下一次系统的滤波迭代过程做准备，实现根据概率重采样，重新生成粒子，并重新分配粒子权重，然后根据重新生成的粒子加权得到位置的估计值，即对于机器人在全局坐标系/map中的最优估计，并更新定位误差方差矩阵，依据以上两项进行下一轮的粒子采样；最后在ros中实现坐标系之间的转换，包括发布基于全局坐标系/map的定位结果，计算机器人坐标系/baselink下全局坐标系/map的原点的位置，通过机器人坐标系/baselink到里程计坐标系/odom之间的坐标变换得到里程计坐标系/odom下全局坐标系/map原点的位置，发布里程计坐标系/odom到全局坐标系/map的坐标变换结果。
应当理解的是，上述针对较佳实施例的描述较为详细，并不能因此而认为是对本发明专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要求所保护的情况下，还可以做出替换或变形，均落入本发明的保护范围之内，本发明的请求保护范围应以所附权利要求为准。