绕机检查装置及其定位方法、存储介质
阅读说明：本技术 绕机检查装置及其定位方法、存储介质 (Winding inspection device, positioning method thereof and storage medium ) 是由 吕品 季博文 赖际舟 卢坤 陆建 徐扬 于 2020-07-16 设计创作，主要内容包括：本申请公开了一种绕机检查装置及其定位方法、可读存储介质。控制绕机检查装置围绕飞机运行,根据获取不同时刻的机轮的激光雷达点云建立飞机坐标系下机轮的激光雷达点云地图；在检查过程中,将在绕机检查装置坐标系下的机轮的激光雷达点云与所述激光雷达点云地图进行点云匹配,得到所述机轮的激光雷达点云在所述绕机检查装置坐标系转换至所述飞机坐标系的转换矩阵；根据所述转换矩阵确定所述绕机检查装置在飞机坐标系下的位姿。解决了现有技术中通过GPS定位方法确定绕机检查装置位姿信息存在不准确的问题。(The application discloses a winding machine inspection device, a positioning method thereof and a readable storage medium. Controlling the winding inspection device to run around the airplane, and establishing a laser radar point cloud map of the airplane wheels under an airplane coordinate system according to the obtained laser radar point clouds of the airplane wheels at different moments; in the checking process, carrying out point cloud matching on the laser radar point cloud of the airplane wheel under the coordinate system of the winding checking device and the laser radar point cloud map to obtain a conversion matrix for converting the laser radar point cloud of the airplane wheel to the airplane coordinate system in the coordinate system of the winding checking device; and determining the pose of the winding inspection device in an airplane coordinate system according to the transformation matrix. The problem of inaccurate in determining the position and orientation information of the winding inspection device through a GPS positioning method in the prior art is solved.)
技术领域
本申请涉及激光雷达定位技术领域，尤其涉及一种绕机检查装置及其定位 方法、存储介质。
背景技术
飞机因具有速度快、安全性高等特点成为了人们出行不可或缺的交通工 具。为了保障飞机的航行安全，飞行前的安全检查是必不可少的一个环节。但 因为飞行前的安全检查时间短、任务重，人工检测难免存在疏漏，受环境影响 人工检查的效率和可靠行也会下降。相对于人工检查，使用地面无人车或者无 人机进行检查能够提升巡检的效率和可靠性，并且巡检时记录的图像方便事后 复现。
目前在室外环境中，地面无人车的定位主要采用卫星/惯性传感器融合的方 法，但在客机巡检过程中有卫星拒止的环境存在，导致使用GPS定位的可靠 性低。在工厂环境中，无人车一般采用预先铺设导轨或磁条的方式进行导航， 但该方法比较繁琐，更改轨迹较为困难，且不适用于机场环境。
发明内容
本申请实施例通过提供一种绕机检查装置及其定位方法、存储介质，旨在 解决目前采用卫星/惯性传感器融合的方式对绕机检查装置进行定位时，由于在 客机巡检过程中有卫星拒止的环境存在，导致绕机检查装置的定位不准确的问 题。
为实现上述目的，本申请一方面提供一种绕机检查装置的定位方法，所述 绕机检查装置的定位方法包括以下步骤：
控制绕机检查装置围绕飞机运行，根据获取不同时刻的机轮的激光雷达点 云建立飞机坐标系下机轮的激光雷达点云地图；
在检查过程中，将在绕机检查装置坐标系下的机轮的激光雷达点云与所述 激光雷达点云地图进行点云匹配，得到所述机轮的激光雷达点云在所述绕机检 查装置坐标系转换至所述飞机坐标系的转换矩阵；
根据所述转换矩阵确定所述绕机检查装置在飞机坐标系下的位姿。
可选地，所述将获取的在绕机检查装置坐标系下的机轮的激光雷达点云与 所述激光雷达点云地图进行点云匹配，得到所述机轮的激光雷达点云在所述绕 机检查装置坐标系转换至所述飞机坐标系的转换矩阵的步骤，包括：
当所述绕机检查装置处于初始检查时刻时，获取所述初始检查时刻在绕机 检查装置坐标系下的机轮的激光雷达点云；
将所述机轮的激光雷达点云与所述激光雷达点云地图进行点云匹配，得到 所述初始检查时刻所述机轮的激光雷达点云在所述绕机检查装置坐标系转换 至所述飞机坐标系的转换矩阵。
可选地，所述将在绕机检查装置坐标系下的机轮的激光雷达点云与所述激 光雷达点云地图进行点云匹配，得到所述机轮的激光雷达点云在所述绕机检查 装置坐标系转换至所述飞机坐标系的转换矩阵的步骤，还包括：
当所述绕机检查装置处于非初始检查时刻时，获取当前时刻在绕机检查装 置坐标系下的机轮的激光雷达点云；
根据上一时刻所述绕机检查在飞机坐标系下的位姿对所述激光雷达点云 进行转换，得到原始激光雷达点云；
将所述原始激光雷达点云与所述激光雷达点云地图进行点云匹配，得到当 前时刻所述机轮的激光雷达点云在所述绕机检查装置坐标系转换至所述飞机 坐标系下所述的转换矩阵。
可选地，所述根据所述转换矩阵确定所述绕机检查装置在飞机坐标系下的 位姿的步骤，包括：
若绕机检查装置处于初始检查时刻，则根据所述初始检查时刻的转换矩阵 确定在所述初始检查时刻所述绕机检查装置在飞机坐标系下的第一位姿；
若绕机检查装置处于非初始检查时刻，则根据所述当前时刻的转换矩阵确 定在所述当前时刻所述绕机检查装置在飞机坐标系下的第一位姿。
可选地，所述根据所述转换矩阵确定所述绕机检查装置在飞机坐标系下的 第一位姿的步骤之后，包括：
获取绕机检查装置在SLAM坐标系下的位姿；
若所述绕机检查装置处于初始检查时刻，则获取所述初始检查时刻的第一 位姿，将所述初始时刻所述绕机检查装置在SLAM坐标系下的位姿转换成飞机 坐标系下的位姿；
若所述绕机检查装置处于非初始检查时刻，则获取当前时刻的上一时刻所 述绕机检查装置在飞机坐标系下的位姿，将所述当前时刻所述SLAM坐标系下 的位姿转换成飞机坐标系下的第二位姿。
可选地，所述根据所述转换矩阵确定所述绕机检查装置在飞机坐标系下的 第一位姿的步骤之后，还包括：
根据绕机检查装置的惯性传感器、车载里程计的数据信息，判断所述所述 绕机检查装置当前时刻的第一位姿以及所述第二位姿是否准确；
根据绕机检查装置的惯性传感器、车载里程计的数据信息，判断所述所述 绕机检查装置当前时刻的第一位姿以及所述第二位姿是否准确；
若准确，则利用扩展卡尔曼滤波将当前时刻的第一位姿、第二位姿和惯性 传感器数据以及车载里程计数据进行数据融合，以得到更准确的绕机检查装置 在飞机坐标系下的位姿；
若不准确，则获取惯性传感器和车载里程计递推得到的所述绕机检查装置 的位姿作为当前时刻绕机检查装置在飞机坐标系下的位姿。
可选地，判断所述所述绕机检查装置当前时刻的第一位姿以及所述第二位 姿是否准确的步骤，包括：
将所述绕机检查装置当前时刻的第一位姿以及所述第二位姿分别与预测 的当前时刻的位姿进行比较，若计算的差值在预设范围内则认为所述当前时刻 的第一位姿以及所述第二位姿准确。
可选地，所述获取不同时刻的机轮的激光雷达点云的步骤，包括：
控制绕机检查装置围绕飞机运行，采集激光雷达点；
对所述扫描点进行聚类处理，根据飞机机轮的相对距离筛选聚类处理后的 激光雷达点，得到机轮的激光雷达点云；
获取不同时刻的激光雷达点并执行所述进行聚类处理，根据飞机机轮的相 对距离筛选聚类处理后的激光雷达点，得到机轮的激光雷达点云的操作，以得 到不同时刻机轮的激光雷达点云。
此外，为实现上述目的，本申请另一方面还提供一种绕机检查装置，所述 装置包括：
获取模块，获取绕机检查装置的扫描设备扫描飞机机轮在飞机坐标系下的 全局点云坐标；
转换模块，将所述全局点云坐标与绕机检查过程中扫描飞机机轮基于装置 坐标系的点云坐标进行转换；
确定模块，根据所述转换结果确定所述绕机检查装置在飞机坐标系下的位 姿。
此外，为实现上述目的，本申请另一方面还提供一种计算机可读存储介质， 其特征在于，其上存储有绕机检查装置的定位程序，所述绕机检查装置的定位 程序被处理器执行时实现如上任一项所述的方法的步骤。
本申请提出了一种绕机检查装置的定位方法，该方法在绕机检查装置围绕 飞机进行检查时首先围绕需要进行检查的飞机运行，通过绕机检查装置的激光 扫描设备获取不同时刻的机轮的激光雷达点云，并建立飞机坐标系下机轮的激 光雷达点云地图，在检查过程中，将获取的在绕机检查装置坐标系下的机轮的 激光雷达点云与所述激光雷达点云地图进行点云匹配，得到所述机轮的激光雷 达点云在所述绕机检查装置坐标系转换至所述飞机坐标系的转换矩阵，根据所 述转换矩阵确定所述绕机检查装置在飞机坐标系下的位姿。通过上述方式，使 得在确定绕机检查装置在绕机检查过程中的位姿时，不需要使用GPS等定位 系统，从而避免了使用GPS定位系统确定绕机检查装置的位姿时，由于存在 卫星拒止的环境时，导致绕机检查装置的定位不准确的问题，达到了提高绕机 检查装置在执行检查任务的过程中定位的精确度的效果。
附图说明
图1为本申请实施例方案涉及的装置的结构示意图；
图2为本申请绕机检查装置的定位方法一实施例的流程示意图；
图3为本申请绕机检查装置的定位方法步骤S20的详细流程示意图；
图4为本申请绕机检查装置的定位方法步骤S20的详细流程示意图；
图5为本申请绕机检查装置的定位方法另一实施例的流程示意图；
图6为本申请绕机检查装置的定位方法另一实施例的流程示意图。
本申请目的的实现、功能特点及优点将结合实施例，参照附图做进一步说 明。
具体实施方式
本申请实施例的主要解决方案是：控制绕机检查装置围绕飞机运行，根据 获取不同时刻的机轮的激光雷达点云建立飞机坐标系下机轮的激光雷达点云 地图；在检查过程中，将在绕机检查装置坐标系下的机轮的激光雷达点云与所 述激光雷达点云地图进行点云匹配，得到所述机轮的激光雷达点云在所述绕机 检查装置坐标系转换至所述飞机坐标系的转换矩阵；根据所述转换矩阵确定所 述绕机检查装置在飞机坐标系下的位姿。
由于现有技术在对在对绕机检查装置的定位主要采用卫星/惯性传感器融 合的方法，但在客机巡检过程中有卫星拒止的环境存在时，则会降低绕机检查 装置定位的可靠性。
本发明提供上述的解决方案，旨在提高绕机检查装置定位的准确性。
本申请实施例提出了一种绕机检查装置，该绕机检查装置包括激光雷达传 感器、支撑部以及控制电路。所述激光雷达传感器用于扫描飞机机轮，以形成 飞机机轮的激光雷达点云，支撑部用于在绕机检查装置围绕飞机执行检查任务 时，能够支撑绕机检查装置内置的设备，所述控制电路用于控制绕机检查装置 进行绕机检查等控制操作。
在本申请实施例中，如图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中可以包括操 作系统、网络通信模块、用户接口模块以及绕机检查设备的定位程序。
本申请还提供一种绕机检查装置的定位方法。
参照图2，提出本申请绕机检查装置的定位方法的一实施例。在本实施中， 所述绕机检查装置的定位方法包括：
步骤S10，控制绕机检查装置围绕飞机运行，根据获取不同时刻的机轮的 激光雷达点云建立飞机坐标系下机轮的激光雷达点云地图；
在绕机检查前，控制绕机检查装置围绕飞机运行一周，其中所述绕机检装 置在本申请中可具体的为配有激光雷达的绕机检查装置。绕机检查装置在绕机 检查前，获取不同时刻的机轮的激光雷达点云建立飞机坐标系下的激光雷达点 云地图的方法可以为通过现场采集的方式构图，也可以通过仿真环境的方式构 图(在仿真环境中搭建飞机的三维模型，控制绕机检查装置围绕三维模型的飞 机绕行)。
其中，所述飞机坐标系具体可以飞机前轮中心为原点，y轴沿飞机纵轴指 向机尾，x轴垂直向右构建。
在本申请中，首先要构建机轮的激光雷达点云地图，机轮的激光雷达点云 地图的构成是在绕机检查前，绕机检查装置围绕飞机绕行获取不同时刻的机轮 的激光雷达点云，其可以理解为是在绕机检查装置在执行绕机检查任务的准备 工作。
所述获取不同时刻的机轮的激光雷达点云的步骤，包括：
步骤S11，控制绕机检查装置围绕飞机运行，采集激光雷达点；
步骤S12，对所述扫描点进行聚类处理，根据飞机机轮的相对距离筛选聚 类处理后的激光雷达点，得到机轮的激光雷达点云；
步骤S13，获取不同时刻的激光雷达点并执行所述进行聚类处理，根据飞 机机轮的相对距离筛选聚类处理后的激光雷达点，得到机轮的激光雷达点云的 操作，以得到不同时刻机轮的激光雷达点云。
在绕机检查装置围绕飞机进行获取机轮的激光雷达点的过程中(包括构建 机轮的激光雷达点云地图以及执行绕机检查任务的过程中获取机轮的激光雷 达点云)，由激光扫描设备得到的激光雷达点并非全都是机轮的激光雷达点(扫 描到机翼、机身或者尾翼等部位的激光雷达点云)，所以需要对其进行筛选以 得到机轮的激光雷达点云，所述激光雷达点云为由众多激光雷达点组成的集 合。其中，用最近邻聚类方法对激光雷达点云数据进行聚类。计算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)
其中，是k时刻该类的中心点在机体系下的坐标；
计算对应不同机轮的激光雷达点云的中心点之间的相对距离，与机轮之间 的相对距离关系进行比较：
其中表示对应机轮n1中的某组激光雷达点云的中心点与属于对应 n2中的某组激光雷达点云的中心点之间的欧式距离，
筛选出满足机轮之间所有相对位置关系的一组激光雷达点云集合将其合并为L(k)，L(k)即为机轮的激光雷达点云。
步骤S20，在检查过程中，将在绕机检查装置坐标系下的机轮的激光雷达 点云与所述激光雷达点云地图进行点云匹配，得到所述机轮的激光雷达点云在 所述绕机检查装置坐标系转换至所述飞机坐标系的转换矩阵；
在绕机检查过程中，通过绕机检查装置获取的激光雷达点经过筛选处理后 得到飞机机轮的激光雷达点云，此时获取的激光雷达点云是基于绕机检查装置 为坐标系获取的，将所述飞机机轮的激光雷达点云与绕机检查前获取的激光雷 达点云地图进行点云匹配，达到将以绕机检查装置为坐标系的激光雷达点云转 换到飞机坐标系下的激光雷达点云。其中，所述绕机检查装置坐标系可为以绕 机检查装置的质心为原点，y轴沿绕机检查装置纵轴向前，x轴沿绕机检查装 置纵轴向右而构建坐标系。
参照图3，所述将获取的在绕机检查装置坐标系下的机轮的激光雷达点云 与所述激光雷达点云地图进行点云匹配，得到所述机轮的激光雷达点云在所述 绕机检查装置坐标系转换至所述飞机坐标系的转换矩阵的步骤，包括：
步骤S21，当所述绕机检查装置处于初始检查时刻时，获取所述初始检查 时刻在绕机检查装置坐标系下的机轮的激光雷达点云；
步骤S22，将所述机轮的激光雷达点云与所述激光雷达点云地图进行点云 匹配，得到所述初始检查时刻所述机轮的激光雷达点云在所述绕机检查装置坐 标系转换至所述飞机坐标系的转换矩阵。
其中，所述根据所述转换矩阵确定所述绕机检查装置在飞机坐标系下的位 姿的步骤，包括：
步骤S23，若绕机检查装置处于初始检查时刻，则根据所述初始检查时刻 的转换矩阵确定在所述初始检查时刻所述绕机检查装置在飞机坐标系下的第 一位姿。
在本实施例中，提出了确认绕机检查装置初始检查时刻的位姿确定方法。 即，在当绕机检查装置在执行初始检查任务时，将在初始检查时刻获取机轮的 激光雷达点云与点云地图进行匹配，具体可理解为将初始检查时刻获取机轮的 激光雷达点云设置为源点云，机轮的激光雷达点云地图设置为目标点云，通过 旋转以及平移使原始点云与目标点云基本重合，其中得到从原始点云到目标点 云的转换矩阵R、T。假设求出转换矩阵R、T为则初始检查时刻绕机检查装置在飞机坐标系的位置、绕机检查装置的航向信息
参照图4，所述将在绕机检查装置坐标系下的机轮的激光雷达点云与所述 激光雷达点云地图进行点云匹配，得到所述机轮的激光雷达点云在所述绕机检 查装置坐标系转换至所述飞机坐标系的转换矩阵的步骤，还包括：
步骤S24，当所述绕机检查装置处于非初始检查时刻时，获取当前时刻在 绕机检查装置坐标系下的机轮的激光雷达点云；
步骤S25，根据上一时刻所述绕机检查在飞机坐标系下的位姿对所述激光 雷达点云进行转换，得到原始激光雷达点云；
步骤S26，将所述原始激光雷达点云与所述激光雷达点云地图进行点云匹 配，得到当前时刻所述机轮的激光雷达点云在所述绕机检查装置坐标系转换至 所述飞机坐标系下所述的转换矩阵。
所述根据所述转换矩阵确定所述绕机检查装置在飞机坐标系下的位姿的 步骤，包括：
步骤S27，若绕机检查装置处于非初始检查时刻，则根据所述当前时刻的 转换矩阵确定在所述当前时刻所述绕机检查装置在飞机坐标系下的第一位姿。
在绕机检查装置(绕机检查装置)处于非初始检查时刻时，则获取当前时 刻的上一时刻的位姿，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)中激光点的数 量，记pi′为L′(k)的第i个激光点(i＝1，2，…，N0)，N0为L′(k)中激光点的数 量，其转换关系为：
其中，和
将绕机检查装置在执行绕机检查任务是获取的机轮的激光雷达点云地图 设置为目标点云。设置参数，包括最大迭代次数Ks，均方误差和的最大值RMSEmax，两次变换矩阵之间的差值ΔT，对应点之间的最大距离dmax。
通过点云配准的方法(如ICP算法)，得到从原始点云到目标点云的转换 矩阵R、T，假设k时刻绕机检查装置在飞机坐标系的位姿为则：
从而根据R、T求解得到绕机检查装置在非初始检查时刻(k时刻)时在 飞机坐标系下的位姿
步骤S30，根据所述转换矩阵确定所述绕机检查装置在飞机坐标系下的位 姿。
通过将以绕机检查装置为坐标系获取的激光雷达点云通过与点云地图进 行匹配，将其转换成以飞机为坐标体系的点云，从而求出绕机检查装置在不同 时刻的位姿变化，所述绕机检查装置的位姿包括绕机检查装置在飞机坐标系下 x轴、y轴的位置信息及其行驶方向。
在本实施例中，在绕机检查装置围绕飞机进行检查时首先围绕需要进行检 查的飞机运行一周，通过绕机检查装置的激光扫描设备获取不同时刻的机轮的 激光雷达点云，并建立飞机坐标系下机轮的激光雷达点云地图，在检查过程中， 将获取的在绕机检查装置坐标系下的机轮的激光雷达点云与所述激光雷达点 云地图进行点云匹配，得到所述机轮的激光雷达点云在所述绕机检查装置坐标 系转换至所述飞机坐标系的转换矩阵，根据所述转换矩阵确定所述绕机检查装 置在飞机坐标系下的位姿。通过上述方式，使得在确定绕机检查装置在绕机检 查过程中的位姿时，不需要使用GPS等定位系统，从而避免了使用GPS定位 系统确定绕机检查装置的位姿时，由于存在卫星拒止的环境时，导致绕机检查 装置的定位不准确的问题，达到了提高绕机检查装置在执行检查任务的过程中 定位的精确度的效果。
参照图5，图5为本申请的另一实施例流程示意图，所述根据所述转换矩 阵确定所述绕机检查装置在飞机坐标系下的第一位姿的步骤之后，包括：
步骤S31，获取绕机检查装置在SLAM坐标系下的位姿；
步骤S32，判断所述绕车检查装置是否处于初始时刻；
步骤S33，若所述绕机检查装置处于初始检查时刻，则获取所述初始检查 时刻的第一位姿，将所述初始时刻所述绕机检查装置在SLAM坐标系下的位姿 转换成飞机坐标系下的位姿；
步骤S34，若所述绕机检查装置处于非初始检查时刻，则获取当前时刻的 上一时刻所述绕机检查装置在飞机坐标系下的位姿，将所述当前时刻所述 SLAM坐标系下的位姿转换成飞机坐标系下的第二位姿。
获取采集得到的激光雷达原始点云数据(不需要经过筛选的激光雷达点 云)，通过同时定位与构图(SLAM)的方法,解算得到k时刻SLAM解算得到 绕机检查装置在SLAM坐标系下位姿通过SLAM坐标系的 方式确定绕机检查装置在飞机坐标系下的位姿分为初始检查时刻以及非初始 检查时刻计算方法：
若当前k时刻为初始检查时刻，读取经过实施例一中点云配准后得到的初 始检查时刻绕机检查装置在飞机坐标系下的第一位姿
其中
若当前时刻为非初始检查时刻，如当前时刻为k时刻，则读取k-1时刻绕 机检查装置在飞机坐标系下的位姿信息(xn(k-1),yn(k-1),ψn(k-1))，k-1时刻 SLAM解算得到绕机检查装置在SLAM坐标系下位姿
通过上述位姿之间的差值修正SLAM坐标系到飞机坐标系的旋转矩阵，根 据如下方程：求解得到；
从而得到k时刻时，SLAM解算得到的绕机检查装置在飞机坐标系的位姿
本实施例中提供了一种通过建立SLAM坐标系，进而得到绕机检查装置在 飞机坐标系下的位姿。其建立的SLAM坐标系中所获取的激光雷达点云数据为 原始点云数据，通过分别与绕机检查装置在初始检查时刻的第一位姿以及非初 始检查时刻的第一位姿进行转换，得到将SLAM坐标系下绕机检查装置的位姿 转换成飞机坐标系下绕机检查装置初始检查时刻的第二位姿以及非初始检查 时刻的第二位姿。
参照图6，图6为本申请的另一实施例，所述根据所述转换矩阵确定所述 绕机检查装置在飞机坐标系下的第一位姿的步骤之后，还包括：
步骤S34，根据绕机检查装置的惯性传感器、车载里程计的数据信息，判 断所述所述绕机检查装置当前时刻的第一位姿以及所述第二位姿是否准确；
步骤S35，若准确，则利用扩展卡尔曼滤波将当前时刻的第一位姿、第二 位姿和惯性传感器数据以及车载里程计数据进行数据融合，以得到更准确的绕 机检查装置在飞机坐标系下的位姿；
步骤S36，若不准确，则获取惯性传感器和车载里程计递推得到的所述绕 机检查装置的位姿作为当前时刻绕机检查装置在飞机坐标系下的位姿。
根据绕机检查装置的惯性传感器、车载里程计的数据信息，判断所述所述 绕机检查装置当前时刻的第一位姿以及所述第二位姿是否准确，根据判断结 果，将准确的位姿信息的位姿进行数据融合，得到绕机检查装置在飞机坐标系 的位置、姿态信息。
一步预测均方误差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时刻机体系到飞机坐标系的坐标转换矩阵， 分别是的第一、二、三行；Δt是系统的离散时间；I 是单位矩阵；
滤波器的噪声矩阵Γ的计算方法如下式所示：
系统的噪声矩阵
其中，ε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)与姿态四元数的关系如 下：
系统的状态估值方程计算方法如下：
其中，
系统的估计均方误差方程为：
Pk|k＝(I-KkHk)Pk|k-1
其中，Pk|k为k时刻估计均方误差，I为单位矩阵。
b)若绕机检查装置当前不处于零速状态，第一位姿、第二位姿得到的信 息可靠，则利用第一位姿、第二位姿以及里程计得到的信息作为量测，量测矩 阵和量测噪声矩阵分别为：
其中，diag表示矩阵对角化，其中分别为
系统的状态估值方程计算方法如下：
其中，
系统的估计均方误差方程为：
Pk|k＝(I-K kH k)Pk|k-1
其中，Pk|k为k时刻估计均方误差，I为单位矩阵。
c)若绕机检查装置当前不处于零速状态，第一位姿得到的信息可靠，第 二位姿得到的信息不可靠，则利用第一位姿以及里程计得到的信息作为量测， 量测矩阵和量测噪声矩阵分别为：
其中，diag表示矩阵对角化，其中分别为
系统的状态估值方程计算方法如下：
其中，
系统的估计均方误差方程为：
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]
其中，
系统的状态估值方程计算方法如下：
其中，
系统的估计均方误差方程为：
Pk|k＝(I-KkHk)Pk|k-1
其中，Pk|k为k时刻估计均方误差，I为单位矩阵。
判断所述所述绕机检查装置当前时刻的第一位姿以及所述第二位姿是否 准确的步骤，包括：
在本实施例中，将所述绕机检查装置当前时刻的第一位姿以及所述第二位 姿分别与预测的当前时刻的位姿进行比较，若计算的差值在预设范围内则认为 所述当前时刻的第一位姿以及所述第二位姿准确。
根据惯性传感器、车载里程计的数据，判断由实施例一以及实施例二获取 得到的绕机检查装置初始检查时刻在飞机坐标系下得到的第一位姿以及第二 位姿、在非初始检查时刻的第一位姿以及第二位姿是否准确，步骤如下：
(1)根据所述惯性传感器信息以及扩展卡尔曼滤波算法，预测当前时刻 待绕机检查装置的姿态、速度和位置。首先，选取的14维状态量为
姿态四元数预测公式为：
其中，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)为中间变 量，通过以下公式计算：
的计算方法如下：
其中是ωb(k)在x,y,z方向上的分量，ωb(k)是k时刻绕机检查装置机体系相对飞机坐标系的角速度在机体系下的表示；
(2)位置预测公式为：
其中，xn(k)、yn(k)是k时刻绕机检查装置在飞机坐标系的位置；xn(k-1)、 yn(k-1)是k-1时刻绕机检查装置在飞机坐标系的位置。
(3)
其中是k时刻绕机检查装置机体系相对于飞机 坐标系的加速度在飞机坐标系的投影，
(4)速度的预测公式为：
读取非初始检查时刻(k时刻)中解算得到的位姿与预 测得到位姿(xn(k),yn(k),ψn(k))进行比较，计算两者之间的差值：
其中，Δx、Δy、Δψ为设置的阈值，若满足上述条件，则认为步骤4中得到的 位姿可靠；
继而，获取当前时刻绕机检查装置在SLAM坐标系下的位姿 与预测得到位姿(xn(k),yn(k),ψn(k))进行比较，计算两者之间 的差值：
其中，Δx、Δy、Δψ为设置的阈值，若满足上述条件，则认为经过SLAM 得到的位姿可靠。
在本实施例中，通过获取绕机检查装置的里程计信息将初始检查时刻的第 一位姿以及第二位姿、非初始检查时刻的第一位姿以及第二位姿进行判断，得 到更准确的绕机检查装置在飞机坐标系下的位姿。
此外，本申请还提供了一种计算机可读存储介质，所述计算机可读存储介 质存储有绕机检查装置的定位程序，所述绕机检查装置的定位程序被处理器执 行时实现上述所述的绕机检查装置的定位方法的步骤。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计 算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结 合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包 含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、 CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产 品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和 /或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/ 或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入 式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算 机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一 个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设 备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中 的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个 流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使 得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处 理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个 流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
应当注意的是，在权利要求中，不应将位于括号之间的任何参考符号构造 成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的部件或步 骤。位于部件之前的单词“一”或“一个”不排除存在多个这样的部件。本申 请可以借助于包括有若干不同部件的硬件以及借助于适当编程的计算机来实 现。在列举了若干装置的单元权利要求中，这些装置中的若干个可以是通过同 一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。 可将这些单词解释为名称。
尽管已描述了本申请的可选实施例，但本领域内的技术人员一旦得知了基 本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要 求意欲解释为包括可选实施例以及落入本申请范围的所有变更和修改。
显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本发
明的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要 求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。