一种卫星拒止环境高精度定位方法、系统及电子设备

技术领域

本发明涉及导航技术领域，尤其是涉及一种卫星拒止环境高精度定位方法、系统及电子设备。

背景技术

目前大多数定位算法需要借助卫星导航系统，可用于拒止环境中的定位算法较为数量较为有限。现有的拒止环境定位算法均为在局部坐标系下或里程计坐标系下运行，例如LIOM、VINS、ORB_SLAM、平面标记的映射与定位(mapping and localization fromplanar marker)、UWB定位系统等等。

其中，LIOM、VINS等里程计算法运行时所绘制的地图为里程计坐标系或局部坐标系下，在拒止环境中无法与全局坐标系建立转换关系。上述算法虽有回环检测等修正方式，但这均为基于局部坐标系的修正，并未真正与全局坐标系产生联系，这就导致当算法长时间运行于体量较大的拒止环境中时，由于误差累计，里程计无法根据所处的真实位置对结果进行有效、真实的修正。

平面标记的映射与定位中导航算法仅可应用于小体量环境中，要求传感器单次必须看到多个标记点，并将标记作为一种便于识别的标记。但是将标记密度布置的极高不仅影响其他任务的作业，也不现实；且该算法同样并不能通过标记反映出里程计在大地坐标系下的位置。

UWB定位系统类似于拒止环境中的卫星导航系统，通过预布置于环境中的电磁波为里程计提供位置信息，但造价高昂，对电磁环境要求极为严格，作用距离短，易受干扰，在大尺度拒止环境中，无法形成使用通过少数基站低成本完成定位。

因此，有必要研究一种能够解决上述技术问题的拒止环境高精度定位方法。

发明内容

针对上述现有技术中存在的问题，本发明提供一种卫星拒止环境高精度定位方法、系统及电子设备。

为了实现上述目的，第一方面，本发明提供一种卫星拒止环境高精度定位方法，其包括：

获取激光雷达在大地坐标系的位姿；

对激光雷达在大地坐标系的位姿和IMU采集的信息进行融合；以及

使用融合后的激光雷达在大地坐标系的位姿通过因子图优化与激光雷达及IMU进行信息融合与全局修正。

优选地，获取激光雷达在大地坐标系的位姿的过程，包括：

获取至少一个aruco标记四个顶点在像素坐标系的坐标值；

根据至少一个aruco标记四个顶点在像素坐标系的坐标值和对应aruco标记的边长，获取至少一个aruco标记相对于相机坐标系的旋转矩阵及平移矩阵；

根据至少一个aruco标记相对于相机坐标系的旋转矩阵及平移矩阵、云台的姿态角、激光雷达与云台之间的相对位置和至少一个aruco标记在大地坐标系的位姿，解算激光雷达在大地坐标系的位姿。

第二方面，本发明提供一种卫星拒止环境高精度定位系统，其包括：

获取激光雷达在大地坐标系的位姿的模块；

对激光雷达在大地坐标系的位姿和IMU采集的信息进行融合的模块；以及

使用融合后的激光雷达在大地坐标系的位姿通过因子图优化与激光雷达及IMU进行信息融合与全局修正的模块。

第三方面，本发明提供一种电子设备，其包括：存储器，处理器；

存储器用于存储处理器可执行指令；

处理器用于根据存储器存储的可执行指令，实现第一方面的卫星拒止环境高精度定位方法。

本发明的卫星拒止环境高精度定位方法、系统及电子设备所具有的有益效果包括：

(1)本发明与现有技术适用于卫星导航环境或二维导航相比，更适用于卫星拒止环境的三维导航；

(2)本发明基于aruco标记在拒止环境中能够得到激光雷达相对于真实环境的位姿，从而实现高精度的定位，而现有技术只能得到激光雷达在激光雷达里程计坐标系的位姿，无法得到该坐标系与真实的大地坐标系转换关系；

(3)本发明在拒止环境中能够根据真实环境对激光雷达误差进行修正，解决了现有技术累计误差大且无法修正的问题；

(4)本发明操作简单，只需在环境中布置少量的aruco标记即可实现，无需其他大量的传感器等设备，节约成本。

附图说明

图1是本发明一种优选实施方式的卫星拒止环境高精度定位方法的流程示意图；

图2是本发明一种优选实施方式的卫星拒止环境高精度定位系统的结构示意图；

图3为本发明因子图结构示意图；

图4为本发明实施例、对比例1～2的轨迹结果仿真模拟结果图。

具体实施方式

下面结合附图对本发明的较佳实施例进行详细阐述，以使本发明的优点和特征能更易于被本领域技术人员理解，从而对本发明的保护范围做出更为清楚明确的界定。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括……”限定的要素，并不排除在包括要素的过程、方法、物品或者设备中还存在另外的相同要素。

目前大多数定位算法需要借助卫星导航系统，可用于拒止环境中的定位算法较为数量较为有限。现有的拒止环境定位算法如LIOM、VINS、ORB_SLAM等均为在局部坐标系下或里程计坐标系下运行，其生成的全局地图与真实的大地坐标系转换关系未知；或者平面标记的映射与定位、UWB定位系统需要提前布置大量标记点或传感器，且对传感器环境要求比较苛刻。

经研究发现，获取激光雷达在真实环境中的位姿，并利用真实位置对其误差进行校正，能够在大场景拒止环境中大幅度提高导航精度。

为了实现上述过程，本发明通过在因子图中增加aruco因子，避免了卫星导航系统仅利用IMU与激光雷达数据进行无人机位姿计算在长时间中产生无法修正的误差。Aruco因子可以将IMU与激光雷达解算时使用的全局坐标系与大地坐标系固连并对正，并通过aruco位置信息对IMU与激光雷达解算所产生的无人机位姿进行误差修正，同时，作为激光雷达匹配退化时维持卫星导航系统正常运行的信息源。

示例性的，对本发明卫星拒止环境高精度定位方法的应用环境进行说明，搭建包含各传感器的无人机系统，具体过程为选定一架无人机(可采用常规型号)，在无人机上分别搭载各传感器，包括：视觉惯性里程计(VIO)和激光雷达等等；其中，视觉惯性里程计包含相机(可含云台)和惯性测量单元(IMU)，相机镜头方向与无人机前进方向一致。视觉惯性里程计和激光雷达均可以集成在无人机的飞控板上。视觉惯性里程计和激光雷达均可采用常规型号部件，视觉惯性里程计和激光雷达的重量总和不超过无人机的额定载荷。当无人机系统工作时，激光雷达采集点云信息、IMU采集加速度、姿态角(角度)、角速率，相机采集图像信息，云台采集相机(或云台)的姿态角，同时各传感器将采集的信息传输给能够进行数据处理的终端或服务器。

第一方面，本发明提供一种卫星拒止环境高精度定位方法，如图1所示。该方法主要包括以下步骤：

步骤S101、获取激光雷达在大地坐标系的位姿。

其中，拒止环境是指屏蔽卫星信号的环境，在该环境下利用卫星进行定位的导航系统都不能正常接收到卫星信号，不能正常工作。

优选地，在步骤S101之前，可以对各传感器进行标定，分别获得每个传感器对应的参数。例如可以包括：相机的内外参矩阵，IMU的内参数和测量噪声，激光雷达安装参数，相机坐标系和IMU坐标系之间的相对位姿关系、IMU与激光雷达里程计位姿，相机与激光雷达位姿等。其中，相机、IMU、激光雷达等已有成熟的标定方法，比如相机的张正友标定法、IMU的误差分析标定法、激光雷达的交互信息最大化外部自动标定法等。

优选地，在步骤S101之前需要在所选区域(真实环境)内布置aruco标记，并记录各个aruco标记中任一点在大地坐标系下的位姿，为了方便计算优选为周边任一点或中心点，更优选为aruco标记中心点在大地坐标系下的位姿

本发明对aruco标记的布置和尺寸不作具体的限制，本领域技术人员根据实际情况而确定。其中aruco标记的尺寸可以任意的更改，为了成功检测可根据所选区域大小和场景选择合适的尺寸。在实际使用中，如果标记的尺寸太小，可能无法检测到它，这时可以选择更换较大尺寸的标记，或者将相机离标记更近一些。

示例性的，当所选区域为工厂车间时，可以在工厂车间的地面的四个角落布置aruco标记，为了增加检测的精准性，还可以在地面的四个边每一定间隔处设置一个aruco标记，例如间隔5米。

在本发明的一个优选实施方式中，通过获取激光雷达在大地坐标系的位姿的过程，可以包括以下步骤：

S101-1、获取至少一个aruco标记四个顶点在像素坐标系的坐标值。

本发明中像素坐标系、相机坐标系、大地坐标系、云台坐标系、机体坐标系、世界坐标系都是本领域中已知的坐标系，且各个坐标系之间可以相互转换；其中，像素坐标系是表现相机成像平面的坐标系，相机坐标系是固连于相机的坐标系，云台坐标系为固连于云台基座(与机体相连部分)的坐标系，大地坐标系指与大地固连的坐标系，在本发明中指在已知场景中某个预设的坐标系。机体坐标系是指固定在无人机上的遵循右手法则的三维正交直角坐标系，其原点位于无人机的质心。

具体地，当无人机系统执行任务时，相机采集在所选区域采集图像信息，然后对图像信息依次进行灰度处理，二值化，比特位提取，识别至少一个aruco标记，最后获取至少一个aruco标记四个顶点在像素坐标系的坐标值。

其中，灰度处理为把RGB格式的图像三个原色信号处理为灰度图像的单一颜色信号。

二值化为根据处理后的灰度图像及指定阈值将灰度信号处理为0或255。

比特位提取为在二维码范围内识别每一aruco像素的灰度信号均值，并将均值按照预设阈值进行二值化，并作为该aruco像素的信号值，然后根据各个像素信号值的排列识别aruco标记编号。

通过上述步骤，能够获得aruco标记四个顶点在像素坐标系的准确的坐标值，避免了对aruco标记的误检测。

S101-2、根据至少一个aruco标记四个顶点在像素坐标系的坐标值和对应aruco标记的边长，获取至少一个aruco标记相对于相机坐标系的旋转矩阵及平移矩阵。

由于每个aruco标记在像素坐标系的坐标不同，所以对应的aruco标记在相机坐标系的位置也不同，因此需要计算每个aruco标记中心点相对于相机坐标系的旋转矩阵及平移矩阵。

具体地，根据相机参数、该aruco标记四个顶点的坐标值和该aruco标记边长使用PNP算法解算出对应aruco标记中心点相对于相机坐标系的旋转矩阵及平移矩阵，分别记为r

具体地，PNP算法为现有的算法，其计算过程可以参考https://blog.csdn.net/u014709760/article/details/88029841。

S101-3、根据至少一个aruco标记相对于相机坐标系的旋转矩阵及平移矩阵、云台的姿态角、激光雷达与云台之间的相对位置和至少一个aruco标记在大地坐标系的位姿，解算激光雷达在大地坐标系的位姿。

现有的拒止环境定位算法均为在局部坐标系下或里程计坐标系下运行，其生成的全局地图(整个地图)与真实的大地坐标系并不固连，其会导致当算法长时间运行于大体量的拒止环境中时，激光雷达无法根据所处的真实位置对累计误差及偏差进行有效、真实的修正。

因此，为了得到激光雷达在大地坐标系的位姿，以及保证激光雷达在大地坐标系的位姿的准确性，需要对各个传感器的结果进行融合。

具体为激光雷达在大地坐标系的位姿通过式一表示：

其中，

在本发明中，通过式一一系列的坐标系转换就能得到检测到第i个aruco标记时，激光雷达在大地坐标系的位姿。

由式一可以看出，

在本发明的一个优选实施方式中，

其中，x

由式一可以看出，

在本发明的一个优选实施方式中，

其中，

其中，φ

由式一可以看出，

在本发明的一个优选实施方式中，

其中，

r

其中，i为aruco标记的编号；r

由式一可以看出，

在本发明的一个优选实施方式中，

其中，x

在本发明中，通过设置少量的aruco标记就能够获得拒止环境中激光雷达相对于真实环境的位姿，从而适用于卫星拒止环境的三维导航。

步骤S102、对激光雷达在大地坐标系的位姿和IMU采集的信息进行融合。

优选地，对激光雷达在大地坐标系的位姿和IMU采集的信息进行卡尔曼滤波，获得激光雷达在大地坐标系下的位姿和协方差矩阵。

根据本发明，由于激光雷达、IMU和无人机系统固连，故激光雷达的位姿与无人机系统的姿态相同，也就是它们一起旋转平移，但是得到的姿态角等会根据初始位置的不同而不同。因此可通过激光雷达的位姿状态方程(或无人机系统姿态状态方程)，即可获得无人机姿态协方差矩阵。

在本发明中，根据检测第i个aruco标记时，其在相机坐标系的位姿以及在相机坐标系下的位置调整

根据上述动态的调整测量噪声协方差矩阵，能够保证卡尔曼滤波的准确性。

具体地，状态方程可以表示为：

观测方程可以表示为：

其中，k表示当前时刻；k-1表示上一时刻；x表示状态向量；x

在卡尔曼滤波时，将k时刻检测到的所有aruco标记所解算出的R

在本发明中

卡尔曼滤波是常用的一种滤波方法，可以表示为：

其中，

∑

将(p

在本发明中，当3秒内未检测到aruco标记则停止EKF过程并初始化，在重新检测到aruco标记时继续EKF的计算。

经研究发现，根据上述过程，能够精确的获得激光雷达(无人机系统)姿态的协方差矩阵P

步骤S103、对融合后的激光雷达在大地坐标系的位姿通过因子图优化与激光雷达及IMU进行信息融合与全局修正。

在无人机系统中，还包括IMU等传感器，为了保证无人机系统导航的精度和鲁棒性，可以将多种传感器的信息进行融合。

在本发明中，因子图中还包括预计分因子、激光雷达因子、回环因子等，如图3所示。其中预计分因子、激光雷达因子、回环因子的获得过程可以为现有方法。在k时刻，将aruco因子作为优化初值，并添加k-1与k时刻之间由IMU预计分解算出的预计分因子，k-1与k时刻之间由激光雷达运动估计解算出的激光雷达因子，并通过aruco标记判定回环，若对于j时刻产生回环则继续在因子图中加入j与k时刻的回环因子。

其中，因子图中的无人机k时刻的状态为：

X

其中，p

优选地，预计分因子的获得过程为:

对IMU所采集的加速度、姿态角、角速度进行预计分；

将预计分结果中的两帧之间位姿T

优选地，激光雷达因子的获得过程为：

(1)获取多帧图像：当采集到激光点云时，先对各个点进行运动补偿、对齐时间戳，将一个周期的点云投影到一帧点云图像上，记为第n帧；

(2)特征提取：对该帧点云图像进行特征提取，计算每一线上某一点b

当k

(3)确定关键帧：关键帧选取时，第一帧作为关键帧；其余，确定第n帧是否为关键帧时，对比第n帧特征点集合{F

(4)特征匹配：对于两关键帧，分别使用最新关键帧k

另外，若读取到激光雷达第n帧时同时检测到第i个aruco标记，即便共视关系未低于预设阈值，亦额外添加该帧为关键帧k

经研究发现，通过引入aruco因子，使得激光雷达因子更加准确，并进一步提高定位的准确性。

优选地，回环因子的获得过程为:

当导航系统第l次采集到对第i个aruco标记的连续检测时，判定为回环。

将l-1次第i个aruco标记检测到回环所对应的的关键帧k

在本发明中，通过融合多种传感器的信息，能够发挥各传感器的优点，从而提高了系统的鲁棒性。

第二方面，本发明提供一种卫星拒止环境高精度定位系统，如图2所示。该系统包括：

获取激光雷达在大地坐标系的位姿的模块201；

对激光雷达在大地坐标系的位姿和IMU采集的信息进行融合的模块202；以及

使用融合后的激光雷达在大地坐标系的位姿通过因子图优化与激光雷达及IMU进行信息融合与全局修正的模块203。

本发明提供的卫星拒止环境高精度定位系统，可用于执行上述任一实施例描述的一种卫星拒止环境高精度定位方法，其实现原理和技术效果类似，在此不再赘述。

优选地，本发明一种卫星拒止环境高精度定位系统中各模块可直接在硬件中、在由处理器执行的软件模块中或在两者的组合中。

软件模块可驻留在RAM存储器、快闪存储器、ROM存储器、EPROM存储器、EEPROM存储器、寄存器、硬盘、可装卸盘、CD-ROM或此项技术中已知的任何其它形式的存储介质中。示范性存储介质耦合到处理器，使得处理器可从存储介质读取信息和向存储介质写入信息。

处理器可以是中央处理单元(英文：CentralProcessingUnit，简称：CPU)，还可以是其他通用处理器、数字信号处理器(英文：DigitalSignalProcessor，简称：DSP)、专用集成电路(英文：ApplicationSpecificIntegratedCircuit，简称：ASIC)、现场可编程门阵列(英文：FieldProgrammableGateArray，简称：FPGA)或其它可编程逻辑装置、离散门或晶体管逻辑、离散硬件组件或其任何组合等。通用处理器可以是微处理器，但在替代方案中，处理器可以是任何常规处理器、控制器、微控制器或状态机。处理器还可实施为计算装置的组合，例如DSP与微处理器的组合、多个微处理器、结合DSP核心的一个或一个以上微处理器或任何其它此类配置。在替代方案中，存储介质可与处理器成一体式。处理器和存储介质可驻留在ASIC中。ASIC可驻留在用户终端中。在替代方案中，处理器和存储介质可作为离散组件驻留在用户终端中。

第三方面，本发明提供一种电子设备，其包括：存储器，处理器；

存储器用于存储处理器可执行指令；

处理器用于根据存储器存储的可执行指令，实现第一方面的卫星拒止环境高精度定位方法。

第四方面，本发明提供一种计算机可读存储介质，其特征在于，计算机可读存储介质中存储有计算机执行指令，计算机执行指令被处理器执行时用于实现如第一方面的卫星拒止环境高精度定位方法。

第五方面，一种程序产品，程序产品包括计算机程序，计算机程序存储在可读存储介质中，至少一个处理器可以从可读存储介质读取计算机程序，至少一个处理器执行计算机程序使执行实施例一描述的卫星拒止环境高精度定位方法。

在本发明所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

在40m*110m的江西省丰城市丰城热电厂一期干煤棚内，在地面除起飞点及附近两个aruco标记外，其余全部布置于距起飞点1.5m高的人行步道上，y轴坐标为-14.8m，间距10m，70m处因有固定设备未放置，一共13个aruco标记，具体布置见图4，range表示真实场景中干煤棚地基范围。无人机搭载相机(型号禅思X5s)、IMU(型号MTI-300)和激光雷达(型号Velodyne-16)。

当无人机执行任务时，相机采集棚内的图像信息，然后对图像信息依次进行灰度处理，二值化，比特位提取，识别每一个aruco标记。

根据相机参数和PNP解算出13个aruco标记中心点相对于相机坐标系的旋转矩阵及平移矩阵，分别记为r

分别根据13个aruco标记中心点相对于相机坐标系的旋转矩阵及平移矩阵、云台的姿态角、雷达与云台之间的相对位置和13个aruco标记中心点在大地坐标系的位姿，获取激光雷达在大地坐标系的位姿。

激光雷达在大地坐标系的位姿通过式一表示：

其中，

其中，x

其中，

φ

其中，

r

其中，i为aruco标记的编号；r

其中，x

对激光雷达在大地坐标系的位姿和IMU采集的信息进行卡尔曼滤波，获得激光雷达的误差协方差矩阵和协方差矩阵。

具体地，状态方程可以表示为：

观测方程可以表示为：

其中，k表示当前时刻；k-1表示上一时刻；x表示状态向量；x

在卡尔曼滤波时，将k时刻检测到的所有aruco标记所解算出的R

将(p

利用现有的loam算法对无人机进行定位，其中loam算法可以参照https://blog.csdn.net/shoufei403/article/details/103664877。具体仿真结果见图4。

利用现有的lio_sam算法对无人机进行定位，其中lio_sam算法可以参照https://blog.csdn.net/tiancailx/article/details/109483450。具体仿真结果见图4。rotate表示实施例的结果手动旋转至与loam、lio_sam两种算法近似的状态的结果。

从图4可以看出，loam、lio_sam算法解算得到的轨迹受到了作业时无人机摆放位姿偏差、激光雷达安装角度偏差等影响，且没有对轨迹旋转偏差进行修正的功能，所计算得到的轨迹均与实际飞行中的轨迹差距甚远，在应用中极大地提高了无人机碰撞的风险，且无法满足后续视觉测方等任务，不满足工况需求。而实施例通过aruco标记，完成了对导航系统的全局优化，优化不仅大幅修正了航线的方向，同时还修正了累计误差。

以上结合具体实施方式和范例性实例对本发明进行了详细说明，不过这些说明并不能理解为对本发明的限制。本领域技术人员理解，在不偏离本发明精神和范围的情况下，可以对本发明技术方案及其实施方式进行多种等价替换、修饰或改进，这些均落入本发明的范围内。