绕机检查装置及其定位方法、存储介质
阅读说明：本技术 绕机检查装置及其定位方法、存储介质 (Winding inspection device, positioning method thereof and storage medium ) 是由 吕品 季博文 赖际舟 卢坤 陆建 徐扬 于 2020-07-16 设计创作，主要内容包括：本申请公开了一种绕机检查装置的定位方法,该方法包括：获取绕机检查装置坐标系下的机轮的激光雷达点云,根据所述激光雷达点云确定所述绕机检查装置在飞机坐标系下的第一位姿；获取绕机检查装置通过同步定位与建图的方法构建的位姿,根据所述绕机检查装置通过同步定位与建图的方法构建的位姿确定所述绕机检查装置在飞机坐标系下的第二位姿；将所述第一位姿、所述第二位姿和绕机检查装置的行驶数据进行数据融合,以得到绕机检查装置在飞机坐标系下的目标位姿。本申请还公开了一种绕机检查装置、系统和可读存储介质。本申请旨在提高在执行绕机检查任务的过程中,所述绕机检查装置的位姿的准确性。(The application discloses a positioning method of a winding machine inspection device, which comprises the following steps: the method comprises the steps of obtaining laser radar point cloud of a wheel under a coordinate system of a winding inspection device, and determining a first pose of the winding inspection device under the coordinate system of an airplane according to the laser radar point cloud; acquiring a pose constructed by the winding inspection device through a synchronous positioning and mapping method, and determining a second pose of the winding inspection device under an airplane coordinate system according to the pose constructed by the winding inspection device through the synchronous positioning and mapping method; and carrying out data fusion on the first pose, the second pose and the driving data of the winding inspection device to obtain the target pose of the winding inspection device in an airplane coordinate system. The application also discloses a winding inspection device, a winding inspection system and a readable storage medium. The present application aims to improve the accuracy of the pose of the winding inspection apparatus in the process of performing a winding inspection task.)
技术领域
本申请涉及激光雷达定位技术领域，尤其涉及一种绕机检查装置及其定位方法、存储介质。
背景技术
飞机因具有速度快、安全性高等特点成为了人们出行不可或缺的交通工具。为了保障飞机的航行安全，飞行前的安全检查是必不可少的一个环节。
同步定位与构图(SLAM，Simultaneous Localization and Mapping)技术是机器人自主导航技术领域中的一大研究热点，也是机器人实际应用中的关键技术。目前通常使用激光雷达扫描到的点数据进行匹配来求解位姿，通常是控制激光雷达围绕飞机运行得到飞机的激光雷达点，但是激光雷达在移动的过程中探测物体时，其得到的扫描点会直接影响匹配的结果，导致较大的误差，造成绕机检查装置的位姿信息不准确。
发明内容
本申请实施例通过提供一种绕机检查装置及其定位方法、存储介质，旨在解决目前采用同步定位与构图(SLAM)技术在激光扫描采集激光雷达点的点数据进行匹配的过程中，由于扫描点直接影响匹配结果从而导致获取的位姿不准确的问题。
为实现上述目的，本申请一方面提供一种绕机检查装置的定位方法，所述绕机检查装置的定位方法包括以下步骤：
获取绕机检查装置坐标系下的机轮的激光雷达点云，根据所述激光雷达点云确定所述绕机检查装置在飞机坐标系下的第一位姿；
获取绕机检查装置通过同步定位与建图的方法构建的位姿，根据所述同步定位与建图的方法构建的位姿确定所述绕机检查装置在飞机坐标系下的第二位姿；
将所述第一位姿、所述第二位姿和绕机检查装置的行驶数据进行数据融合，以得到绕机检查装置在飞机坐标系下的目标位姿。
可选地，所述根据所述激光雷达点云确定所述绕机检查装置在飞机坐标系下的第一位姿的步骤包括：
获取不同时刻的机轮的激光雷达点云建立飞机坐标系下机轮的激光雷达点云地图；
将在绕机检查装置坐标系下的机轮的激光雷达点云与所述激光雷达点云地图进行点云匹配，得到所述机轮的激光雷达点云在所述绕机检查装置坐标系转换至所述飞机坐标系的转换矩阵；
根据所述转换矩阵确定所述绕机检查装置在飞机坐标系下的第一位姿。
可选地，所述根据所述转换矩阵确定所述绕机检查装置在飞机坐标系下的位姿的步骤，包括：
若绕机检查装置处于初始检查时刻，则根据所述初始检查时刻的转换矩阵确定在所述初始检查时刻所述绕机检查装置在飞机坐标系下的第一位姿；
若绕机检查装置处于非初始检查时刻，则根据当前时刻的转换矩阵确定在所述绕机检查装置在飞机坐标系下的第一位姿。
可选地，所述根据所述同步定位与建图的方法构建的位姿确定所述绕机检查装置在飞机坐标系下的第二位姿的步骤，包括：
当所述绕机检查装置处于非初始检查时刻，获取当前时刻的上一时刻所述绕机检查装置在飞机坐标系下的位姿，根据所述当前时刻的上一时刻所述绕机检查装置在飞机坐标系下的位姿，将所述通过同步定位与建图的方法构建的位姿转换成飞机坐标系下的第二位姿。
可选地，所述将所述第一位姿、所述第二位姿和绕机检查装置的行驶数据进行数据融合，以得到绕机检查装置在飞机坐标系下的目标位姿的步骤之前，包括：
根据绕机检查装置的行驶数据进行数据判断所述所述绕机检查装置的第一位姿以及所述第二位姿是否准确；
若准确，则执行将所述第一位姿、所述第二位姿和绕机检查装置的行驶数据进行数据融合，以得到绕机检查装置在飞机坐标系下的目标位姿的步骤；
若不准确，则获取绕机检查装置的行驶数据计算得到的所述绕机检查装置的位姿作为所述绕机检查装置在飞机坐标系下的目标位姿。
可选地，所述判断所述所述绕机检查装置的第一位姿以及所述第二位姿是否准确的步骤，包括：
获取绕机检查装置预测的位姿；
将所述绕机检查装置的第一位姿以及所述第二位姿分别与预测的位姿进行比较，若计算的差值在预设范围内则认为所述当前时刻的第一位姿以及所述第二位姿准确。
可选地，所述获取不同时刻的机轮的激光雷达点云的步骤，包括：
控制绕机检查装置围绕飞机运行，采集激光雷达点；
对所述激光雷达点进行聚类处理，根据飞机机轮的相对距离筛选聚类处理后的激光雷达点，得到机轮的激光雷达点云；
获取不同时刻的激光雷达点并执行所述聚类处理，根据飞机机轮的相对距离筛选聚类处理后的激光雷达点，以得到不同时刻机轮的激光雷达点云。
此外，为实现上述目的，本申请另一方面还提供一种绕机检查装置，所述绕机检查装置包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的绕机检查装置的定位程序；所述绕机检查装置的定位程序被所述处理器执行时实现如上任一项所述的绕机检查装置的定位方法的步骤。
可选地，所述绕机检查装置装置包括：
第一获取模块，用于获取绕机检查装置坐标系下的机轮的激光雷达点云，根据所述激光雷达点云确定所述绕机检查装置在飞机坐标系下的第一位姿；
第二获取模块，用于获取绕机检查装置通过同步定位与建图的方法构建的位姿，根据所述同步定位与建图的方法构建的位姿确定所述绕机检查装置在飞机坐标系下的第二位姿；
融合模块，用于将所述第一位姿、所述第二位姿和绕机检查装置的行驶数据进行数据融合，以得到绕机检查装置在飞机坐标系下的目标位姿。
此外，为实现上述目的，本申请另一方面还提供一种绕机检查装置，所述绕机检查装置包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的绕机检查装置的定位程序；所述绕机检查装置的定位程序被所述处理器执行时实现如上任一项所述的绕机检查装置的定位方法的步骤。
此外，为实现上述目的，本申请另一方面还提供一种计算机可读存储介质，其上存储有绕机检查装置的定位程序，所述绕机检查装置的定位程序被处理器执行时实现如上任一项所述的方法的步骤。
本申请提出了一种绕机检查装置的定位方法，该方法首先获取绕机检查装置坐标系下的机轮的激光雷达点云，根据所述激光雷达点云确定所述绕机检查装置在飞机坐标系下的第一位姿，然后获取绕机检查装置通过同步定位与建图的方法构建的位姿，根据所述同步定位与建图的方法构建的位姿确定所述绕机检查装置在飞机坐标系下的第二位姿，进而将获取得到的所述第一位姿、所述第二位姿和绕机检查装置的行驶数据进行数据融合，以得到绕机检查装置在飞机坐标系下的目标位姿。通过上述方式，使得在确定绕机检查装置在绕机检查过程中的位姿时，能够将通过获取的机轮的激光雷达点云确定的绕机检查装置在飞机坐标的第一位姿以及通过同步定位与建图方法确定绕机检查装置在飞机坐标系下的第二位姿，结合绕机检查装置的行驶数据进行数据融合，提高了绕机检查装置在飞机坐标系下的定位的精确度的效果。
附图说明
图1为本申请实施例方案涉及的绕机检查装置的结构示意图；
图2为本申请绕机检查装置的定位方法一实施例的流程示意图；
图3为本申请绕机检查装置的定位方法又一实施例流程示意图；
图4为本申请绕机检查装置的定位方法另一实施例流程示意图；
图5为本申请绕机检查装置的定位方法又一实施例流程示意图；
图6为本申请绕机检查装置的定位方法模块示意图。
本申请目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
本申请实施例的主要解决方案是：获取绕机检查装置坐标系下的机轮的激光雷达点云，根据所述激光雷达点云确定所述绕机检查装置在飞机坐标系下的第一位姿；获取绕机检查装置通过同步定位与建图的方法构建的位姿，根据所述同步定位与建图的方法构建的位姿确定所述绕机检查装置在飞机坐标系下的第二位姿；将所述第一位姿、所述第二位姿和绕机检查装置的行驶数据进行数据融合，以得到绕机检查装置在飞机坐标系下的目标位姿。
由于现有技术在对在对绕机检查装置的定位过程中通过同步定位与建图的方法(SLAM)确定绕机检查装置的位姿时，由于在移动过程中获取的激光雷达点的点云数据不准确，从而影响绕机检查装置定位的可靠性。
本申请提供上述的解决方案，旨在提高绕机检查装置定位的准确性。
本申请实施例提出了一种绕机检查装置，该绕机检查装置包括激光雷达传感器、支撑部以及控制电路。所述激光雷达传感器用于扫描飞机机轮，以形成飞机机轮的激光雷达点云，支撑部用于在绕机检查装置围绕飞机执行检查任务时，能够支撑绕机检查装置内置的设备，所述控制电路用于控制绕机检查装置进行绕机检查等控制操作。
在本申请实施例中，如图1所示，图1是本申请实施例方案涉及的装置的硬件运行环境的终端结构示意图。
如图1所示，该终端可以包括：处理器1001，例如CPU，网络接口1004，用户接口1003，存储器1005，通信总线1002。其中，通信总线1002用于实现这些组件之间的连接通信。用户接口1003可以包括显示屏(Display)、输入单元比如键盘(Keyboard)，可选用户接口1003还可以包括标准的有线接口、无线接口。网络接口1004可选的可以包括标准的有线接口、无线接口(如WI-FI接口)。存储器1005可以是高速RAM存储器，也可以是稳定的存储器(non-volatile memory)，例如磁盘存储器。存储器1005可选的还可以是独立于前述处理器1001的存储装置。
本领域技术人员可以理解，图1中示出的终端结构并不构成对终端设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。
如图1所示，作为一种计算机可读存储介质的存储器1005中可以包括操作系统、网络通信模块、用户接口模块以及绕机检查设备的定位程序。
本申请还提供一种绕机检查装置的定位方法。
步骤S10，获取绕机检查装置坐标系下的机轮的激光雷达点云，根据所述激光雷达点云确定所述绕机检查装置在飞机坐标系下的第一位姿；
所述第一位姿为绕机检查装置在执行绕机检查任务的过程中通过激光雷达扫描设备获取飞机机轮的激光雷达点云进而确定的绕机检查装置在飞机坐标系下的位姿。所述机轮的激光雷达点云为通过筛选激光扫描设备扫描飞机所得到的激光雷达点的的集合。本申请中，所述绕机检查装置可为无人驾驶车辆。
在本实施例中，通过将绕机检查装置在执行绕机检查任务的过程获取的机轮的激光雷达点云与激光雷达点云地图进行匹配，其中，所述激光雷达点云地图基于飞机坐标系而建立，所述绕机检查装置在执行任务的过程中获取的机轮的激光雷达点云为基于绕机检查装置坐标系下的激光雷达点云，通过将绕机检查装置坐标系下的机轮的激光雷达点云与飞机坐标系下的激光雷达点云地图进行点云匹配进而确定在绕机检查装置在执行绕机检查任务过程中在飞机坐标系下的位姿，所述位姿为第一位姿。
步骤S20，获取绕机检查装置通过同步定位与建图的方法构建的位姿，根据所述同步定位与建图的方法构建的位姿确定所述绕机检查装置在飞机坐标系下的第二位姿；
所述第二位姿为绕机检查装置通过同步定位与建图的方法确定的位姿，所述通过不同定位与构图方法确定的位姿称为绕机检查装置SLAM坐标系下的位姿。
步骤S30，将所述第一位姿、所述第二位姿和绕机检查装置的行驶数据进行数据融合，以得到绕机检查装置在飞机坐标系下的目标位姿。
所述绕机检查装置的行驶数据包括绕机检查装置的惯性传感器、车载里程计的数据信息。通过将绕机检查装置的行驶数据信息与获取的第一位姿以及第二位姿进行比较，判断所述第一位姿、以及第二位姿是否正确，若所述第一位姿以及第二位姿正确，则将所述第一位姿、所述第二位姿和绕机检查装置的行驶数据进行数据融合，以得到绕机检查装置在飞机坐标系下的目标位姿。
本实施例提出了一种绕机检查装置的定位方法，该方法首先获取绕机检查装置坐标系下的机轮的激光雷达点云，根据所述激光雷达点云确定所述绕机检查装置在飞机坐标系下的第一位姿，然后获取绕机检查装置通过同步定位与建图的方法构建的位姿，根据所述同步定位与建图的方法构建的位姿确定所述绕机检查装置在飞机坐标系下的第二位姿，进而将获取得到的所述第一位姿、所述第二位姿和绕机检查装置的行驶数据进行数据融合，以得到绕机检查装置在飞机坐标系下的目标位姿。通过上述方式，使得在确定绕机检查装置在绕机检查过程中的位姿时，能够将通过获取的机轮的激光雷达点云确定的绕机检查装置在飞机坐标的第一位姿以及通过同步定位与建图方法确定绕机检查装置在飞机坐标系下的第二位姿，结合绕机检查装置的行驶数据进行数据融合，提高了绕机检查装置在飞机坐标系下的定位的精确度的效果。
参照图3，图3为本申请又一实施例流程示意图，所述根据所述激光雷达点云确定所述绕机检查装置在飞机坐标系下的第一位姿的步骤包括：
步骤S11，获取不同时刻的机轮的激光雷达点云建立飞机坐标系下机轮的激光雷达点云地图；
步骤S12，将在绕机检查装置坐标系下的机轮的激光雷达点云与所述激光雷达点云地图进行点云匹配，得到所述机轮的激光雷达点云在所述绕机检查装置坐标系转换至所述飞机坐标系的转换矩阵；
步骤S13，根据所述转换矩阵确定所述绕机检查装置在飞机坐标系下的第一位姿。
在绕机检查前，控制绕机检查装置围绕飞机运行一周，其中所述绕机检装置在本申请中可具体的为配有激光雷达的绕机检查装置。绕机检查装置在绕机检查前，获取不同时刻的机轮的激光雷达点云建立飞机坐标系下的激光雷达点云地图的方法可以为通过现场采集的方式构图，也可以通过仿真环境的方式构图(在仿真环境中搭建飞机的三维模型，控制绕机检查装置围绕三维模型的飞机绕行)。
其中，所述飞机坐标系具体可以飞机前轮中心为原点，y轴沿飞机纵轴指向机尾，x轴垂直向右构建。
在本申请中，首先要构建机轮的激光雷达点云地图，机轮的激光雷达点云地图的构成是在绕机检查前，绕机检查装置围绕飞机绕行获取不同时刻的机轮的激光雷达点云，其可以理解为是在绕机检查装置在执行绕机检查任务的准备工作。
所述获取不同时刻的机轮的激光雷达点云的步骤，包括：
步骤S1，控制绕机检查装置围绕飞机运行，采集激光雷达点；
步骤S2，对所述扫描点进行聚类处理，根据飞机机轮的相对距离筛选聚类处理后的激光雷达点，得到机轮的激光雷达点云；
步骤S3，获取不同时刻的激光雷达点并执行所述进行聚类处理，根据飞机机轮的相对距离筛选聚类处理后的激光雷达点，得到机轮的激光雷达点云的操作，以得到不同时刻机轮的激光雷达点云。
在绕机检查装置围绕飞机进行获取机轮的激光雷达点的过程中(包括构建机轮的激光雷达点云地图以及执行绕机检查任务的过程中获取机轮的激光雷达点云)，由激光扫描设备得到的激光雷达点并非全都是机轮的激光雷达点(扫描到机翼、机身或者尾翼等部位的激光雷达点云)，所以需要对其进行筛选以得到机轮的激光雷达点云，所述激光雷达点云为由众多激光雷达点组成的集合。其中，用最近邻聚类方法对激光雷达点云数据进行聚类。计算S(k)中相邻激光点之间的距离D：
如果D(ρ(i),ρ(i+1))大于Dth，将ρ(i)、ρ(i+1)标记为断点，将聚类完成后的每一类点云标记为Sd(k)，1≤d≤ND，ND为聚类后激光雷达点云的数量；继而将点云Sd(k)内的点求取平均值，得到每一类点云的中心点，再利用k-1时刻地面绕机检查装置位姿信息，对点云分割完成的类进行筛选，去除不符合要求的点，计算公式为，其中，dLW(k-1)是根据k-1时刻地面绕机检查装置在飞机坐标系中位置和机轮在飞机坐标系下的坐标得到的两者之间的欧式距离，ψLW(k-1)是根据k-1时刻地面绕机检查装置在飞机坐标系中位姿和机轮在飞机坐标系下的坐标得到的两者之间的角度信息。dLC(k)是k时刻根据机体系下各类的中心点到机体系原点的欧式距离，ψLC(k)是k时刻各类中心点位于机体系下的方位，r和γ是设立的距离阈值和角度阈值。dLW(k)、ψLW(k)、 dLC(k)、ψLC(k)的计算公式如下：
(1)
(2)
(3)
(4)
其中，
计算对应不同机轮的激光雷达点云的中心点之间的相对距离，与机轮之间的相对距离关系进行比较：
其中
筛选出满足机轮之间所有相对位置关系的一组激光雷达点云集合将其合并为L(k)，L(k)即为机轮的激光雷达点云。
在绕机检查过程中，通过绕机检查装置获取的激光雷达点经过筛选处理后得到飞机机轮的激光雷达点云，此时获取的激光雷达点云是基于绕机检查装置为坐标系获取的，将所述飞机机轮的激光雷达点云与绕机检查前获取的激光雷达点云地图进行点云匹配，得到所述机轮的激光雷达点云在所述绕机检查装置坐标系转换至所述飞机坐标系的转换矩阵，达到将以绕机检查装置为坐标系的激光雷达点云转换到飞机坐标系下的激光雷达点云。其中，所述绕机检查装置坐标系可为以绕机检查装置的质心为原点，y轴沿绕机检查装置纵轴向前，x 轴沿绕机检查装置纵轴向右而构建坐标系。
所述根据所述转换矩阵确定所述绕机检查装置在飞机坐标系下的位姿的步骤，包括：
步骤S131，若绕机检查装置处于初始检查时刻，则根据所述初始检查时刻的转换矩阵确定在所述初始检查时刻所述绕机检查装置在飞机坐标系下的第一位姿；
在本实施例中，提出了确认绕机检查装置初始检查时刻的位姿确定方法。即，在当绕机检查装置在执行初始检查任务时，将在初始检查时刻获取机轮的激光雷达点云与点云地图进行匹配，具体可理解为将初始检查时刻获取机轮的激光雷达点云设置为源点云，机轮的激光雷达点云地图设置为目标点云，通过旋转以及平移使原始点云与目标点云基本重合，其中得到从原始点云到目标点云的转换矩阵R、T。假设求出转换矩阵R、T为则初始检查时刻绕机检查装置在飞机坐标系的位置、绕机检查装置的航向信息
步骤S132，若绕机检查装置处于非初始检查时刻，则根据当前时刻的转换矩阵确定在所述绕机检查装置在飞机坐标系下的第一位姿。
在绕机检查装置(绕机检查装置)处于非初始检查时刻时，则获取当前时刻的上一时刻的位姿，k-1时刻绕机检查装置的位姿(xn(k-1),yn(k-1),ψn(k-1))，将步骤2处理后的机轮的激光雷达点云L(k)进行坐标转换，得到新的激光雷达点云L′(k)，将L′(k)设置为原始点云；
记pi为L(k)的第i个激光点(i＝1，2，…，N0)，N0为L(k)中激光点的数量，记p′i为L′(k)的第i个激光点(i＝1，2，…，N0)，N0为L′(k)中激光点的数量，其转换关系为：
其中，和
将绕机检查装置在执行绕机检查任务是获取的机轮的激光雷达点云地图设置为目标点云。设置参数，包括最大迭代次数Ks，均方误差和的最大值 RMSEmax，两次变换矩阵之间的差值ΔT，对应点之间的最大距离dmax。
通过点云配准的方法(如ICP算法)，得到从原始点云到目标点云的转换矩阵R、T，假设k时刻绕机检查装置在飞机坐标系的位姿为
从而根据R、T求解得到绕机检查装置在非初始检查时刻(k时刻)时在飞机坐标系下的第一位姿
在本实施例中，在绕机检查装置围绕飞机进行检查时首先围绕需要进行检查的飞机运行一周，通过绕机检查装置的激光扫描设备获取不同时刻的机轮的激光雷达点云，并建立飞机坐标系下机轮的激光雷达点云地图，在检查过程中，将获取的在绕机检查装置坐标系下的机轮的激光雷达点云与所述激光雷达点云地图进行点云匹配，得到所述机轮的激光雷达点云在所述绕机检查装置坐标系转换至所述飞机坐标系的转换矩阵，根据所述转换矩阵确定所述绕机检查装置在飞机坐标系下的第一位姿。
参照图4，图4为本申请另一实施例流程示意图，所述根据所述同步定位与建图的方法构建的位姿确定所述绕机检查装置在飞机坐标系下的第二位姿的步骤，包括：
步骤S21，当所述绕机检查装置处于非初始检查时刻，获取当前时刻的上一时刻所述绕机检查装置在飞机坐标系下的位姿，根据所述当前时刻的上一时刻所述绕机检查装置在飞机坐标系下的位姿，将所述通过同步定位与建图的方法构建的位姿转换成飞机坐标系下的第二位姿。
获取采集得到的激光雷达原始点云数据(不需要经过筛选的激光雷达点云)，通过同步定位与建图(SLAM)的方法,解算得到k时刻SLAM解算得到绕机检查装置在SLAM坐标系下位姿
若当前k时刻为初始检查时刻，读取经过实施例一中点云配准后得到的初始检查时刻绕机检查装置在飞机坐标系下的第二位姿得到SLAM 坐标系到飞机坐标系的转换关系为：
其中
若当前时刻为非初始检查时刻，如当前时刻为k时刻，则读取k-1时刻绕机检查装置在飞机坐标系下的位姿信息(xn(k-1),yn(k-1),ψn(k-1))，k-1时刻 SLAM解算得到绕机检查装置在SLAM坐标系下位姿
通过上述位姿之间的差值修正SLAM坐标系到飞机坐标系的旋转矩阵，根据如下方程：求解得到；
从而得到k时刻时，SLAM解算得到的绕机检查装置在飞机坐标系的第二位姿为：
本实施例中提供了一种通过建立SLAM坐标系，进而得到绕机检查装置在飞机坐标系下的位姿。其建立的SLAM坐标系中所获取的激光雷达点云数据为原始点云数据，通过分别与绕机检查装置在初始检查时刻的第一位姿以及非初始检查时刻的第一位姿进行转换，得到将SLAM坐标系下绕机检查装置的位姿转换成飞机坐标系下绕机检查装置初始检查时刻的第二位姿以及非初始检查时刻(当前时刻)的第二位姿。
参照图5，图5为本申请又一实施例流程示意图。所述将所述第一位姿、所述第二位姿和绕机检查装置的行驶数据进行数据融合，以得到绕机检查装置在飞机坐标系下的目标位姿的步骤之前，包括：
步骤S31，根据绕机检查装置的行驶数据进行数据判断所述所述绕机检查装置的第一位姿以及所述第二位姿是否准确；
步骤S32，若准确，则执行将所述第一位姿、所述第二位姿和绕机检查装置的行驶数据进行数据融合，以得到绕机检查装置在飞机坐标系下的目标位姿的步骤；
步骤S33，若不准确，则获取绕机检查装置的行驶数据计算得到的所述绕机检查装置的位姿作为所述绕机检查装置在飞机坐标系下的目标位姿。
所述绕机检查装置的行驶数据包括绕机检查装置的惯性传感器、车载里程计的数据信息，根据绕机检查装置的惯性传感器、车载里程计的数据信息，判断所述所述绕机检查装置当前时刻的第一位姿以及所述第二位姿是否准确，根据判断结果，将准确的位姿信息的位姿进行数据融合，得到绕机检查装置在飞机坐标系的位置、姿态信息。
一步预测均方误差Pk|k-1的计算公式如下：
其中，Φ是状态转移矩阵，Φk,k-1是k-1到k时刻的状态转移矩阵，Γk-1是k-1时刻滤波器的噪声矩阵，Wk-1是k-1时刻系统的噪声矩阵；
上其中M4×4，U4×3，N3×4均为中间变量，计算方法如下式所示:
其中，是k-1时刻机体系到飞机坐标系的坐标转换矩阵，
滤波器的噪声矩阵Γ的计算方法如下式所示：
系统的噪声矩阵
其中，εwx，εwy，εwz分别是
根据量测量进行状态更新；
系统的滤波增益方程的计算方法如下：
其中，Kk是k时刻系统的滤波增益，Rk是量测噪声矩阵，Hk是量测矩阵；
根据绕机检查装置当前状态以及量测信息可靠性不同，量测噪声矩阵、量测矩阵不同，具体包括如下步骤：
a)根据控制机构提供的反馈判断绕机检查装置当前是否处于零速状态，若绕机检查装置处于零速状态，使用k-1时刻时绕机检查装置的位置信息以及航向信息(xn(k-1),yn(k-1),ψn(k-1))作为量测，量测矩阵H(k)和量测噪声矩阵 R(k)分别为：
其中，diag表示矩阵对角化，其中分别为xn(k-1)、 yn(k-1)、ψn(k-1)的噪声；xn(k-1)、yn(k-1)、ψn(k-1)分别为k-1时刻绕机检查装置在飞机坐标系x、y方向坐标以及偏航角，ψn(k-1)与姿态四元数的关系如下：
系统的状态估值方程计算方法如下：
其中，为k时刻状态量的估计值，
Zk＝[xn(k-1) yn(k-1) ψn(k-1)]；
系统的估计均方误差方程为：
Pk|k＝(I-KkHk)Pk|k-1
其中，Pk|k为k时刻估计均方误差，I为单位矩阵。
b)若绕机检查装置当前不处于零速状态，第一位姿、第二位姿得到的信息可靠，则利用第一位姿、第二位姿以及里程计得到的信息作为量测，量测矩阵和量测噪声矩阵分别为：
其中，diag表示矩阵对角化，其中
系统的状态估值方程计算方法如下：
其中，为k时刻状态量的估计值，为k-1时刻到k时刻的状态变量一步预测值；Zk为待绕机检查装置在k时刻的量测值，其计算如下所示：；
系统的估计均方误差方程为：
Pk|k＝(I-KkHk)Pk|k-1
其中，Pk|k为k时刻估计均方误差，I为单位矩阵。
c)若绕机检查装置当前不处于零速状态，第一位姿得到的信息可靠，第二位姿得到的信息不可靠，则利用第一位姿以及里程计得到的信息作为量测，量测矩阵和量测噪声矩阵分别为：
其中，diag表示矩阵对角化，其中
系统的状态估值方程计算方法如下：
其中，为k时刻状态量的估计值，
系统的估计均方误差方程为：
Pk|k＝(I-KkHk)Pk|k-1
其中，Pk|k为k时刻估计均方误差，I为单位矩阵。
d)若绕机检查装置当前不处于零速状态，第一位姿得到的信息不可靠，第二位姿得到的信息可靠，则利用第二位姿以及里程计得到的信息作为量测，量测矩阵和量测噪声矩阵分别为：
其中，diag表示矩阵对角化，分别为
系统的状态估值方程计算方法如下：
其中，
系统的估计均方误差方程为：
Pk|k＝(I-KkHk)Pk|k-1
其中，Pk|k为k时刻估计均方误差，I为单位矩阵。
e)若绕机检查装置当前不处于零速状态，第一位姿以及第二位姿的信息不可靠，则利用里程计得到的信息作为量测，量测矩阵和量测噪声矩阵分别为：
H(k)＝[02×6 I2×2 02×6]
其中，和分别为
系统的状态估值方程计算方法如下：
其中，为k时刻状态量的估计值，为k-1时刻到k时刻的状态变量一步预测值；Zk为待绕机检查装置在k时刻的量测值；
系统的估计均方误差方程为：
Pk|k＝(I-KkHk)Pk|k-1
其中，Pk|k为k时刻估计均方误差，I为单位矩阵。
判断所述所述绕机检查装置当前时刻的第一位姿以及所述第二位姿是否准确的步骤，包括：
在本实施例中，将所述绕机检查装置的第一位姿以及所述第二位姿分别与预测的位姿进行比较，若计算的差值在预设范围内则认为所述当前时刻的第一位姿以及所述第二位姿准确。
所述判断所述所述绕机检查装置当前时刻的第一位姿以及所述第二位姿是否准确的步骤，包括：
步骤S311，获取绕机检查装置预测的位姿；
步骤S312，将所述绕机检查装置的第一位姿以及所述第二位姿分别与预测的位姿进行比较，若计算的差值在预设范围内则认为所述当前时刻的第一位姿以及所述第二位姿准确。
(1)根据所述惯性传感器信息以及扩展卡尔曼滤波算法，预测当前时刻待绕机检查装置的姿态、速度和位置。首先，选取的14维状态量为q0、q1、q2、q3表示绕机检查装置的姿态四元数，xn、yn分别表示绕机检查装置在飞机坐标系x、y方向上的位置，表示绕机检查装置在飞机坐标系x、y方向上的速度， 分别表示陀螺仪在x、y、z方向上的零偏，分别表示加速度计在x、y、z方向上的零偏。预测当前时刻待绕机检查装置的姿态、速度和位置采用如下公式：
姿态四元数预测公式为：
其中，k时刻为所述当前时刻，Q(k)＝[q0(k),q1(k),q2(k),q3(k)]T为k时刻绕机检查装置的姿态四元数；Q(k-1)＝[q0(k-1),q1(k-1),q2(k-1),q3(k-1)]Tk-1为时刻绕机检查装置的姿态四元数；上标T表示矩阵的转置；Δt是离散采样周期；Ω(k)为中间变量，通过以下公式计算：
其中
(2)位置预测公式为：
其中，xn(k)、yn(k)是k时刻绕机检查装置在飞机坐标系的位置；xn(k-1)、 yn(k-1)是k-1时刻绕机检查装置在飞机坐标系的位置。是k-1 时刻绕机检查装置机体系相对于飞机坐标系的线速度在飞机坐标系的投影在 X轴和Y轴方向的分量；
(3)通过以下公式计算：
其中是k时刻绕机检查装置机体系相对于飞机坐标系的加速度在飞机坐标系的投影，
(4)速度的预测公式为：
读取非初始检查时刻(k时刻)中解算得到的位姿与预测得到位姿(xn(k),yn(k),ψn(k))进行比较，计算两者之间的差值：
其中，Δx、Δy、Δψ为设置的阈值，若满足上述条件，则认为步骤4中得到的位姿可靠；
继而，获取当前时刻绕机检查装置在SLAM坐标系下的位姿
其中，Δx、Δy、Δψ为设置的阈值，若满足上述条件，则认为经过SLAM 得到的位姿可靠。
在本实施例中，通过获取绕机检查装置的里程计信息将当前时刻的第一位姿以及第二位姿进行判断，得到更准确的绕机检查装置在飞机坐标系下的位姿。
一种绕机检查装置，所述绕机检查装置包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的绕机检查装置的定位程序；所述绕机检查装置的定位程序被所述处理器执行时实现如权利要求1至7中任一项所述的绕机检查装置的定位方法的步骤。
如上所述的绕机检查装置，所述装置包括：
第一获取模块，用于获取绕机检查装置坐标系下的机轮的激光雷达点云，根据所述激光雷达点云确定所述绕机检查装置在飞机坐标系下的第一位姿；
第二获取模块，用于获取绕机检查装置通过同步定位与建图的方法构建的位姿，根据所述同步定位与建图的方法构建的位姿确定所述绕机检查装置在飞机坐标系下的第二位姿；
融合模块，用于将所述第一位姿、所述第二位姿和绕机检查装置的行驶数据进行数据融合，以得到绕机检查装置在飞机坐标系下的目标位姿。
此外，本申请还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有绕机检查装置的定位程序，所述绕机检查装置的定位程序被处理器执行时实现上述所述的绕机检查装置的定位方法的步骤。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、 CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和 /或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/ 或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
应当注意的是，在权利要求中，不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的部件或步骤。位于部件之前的单词“一”或“一个”不排除存在多个这样的部件。本申请可以借助于包括有若干不同部件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中，这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。
尽管已描述了本申请的可选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括可选实施例以及落入本申请范围的所有变更和修改。
显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：绕机检查装置及其定位方法、存储介质