本申请涉及激光雷达定位技术领域,尤其涉及一种绕机检查装置及其定位方法、存储介质。
背景技术:
飞机因具有速度快、安全性高等特点成为了人们出行不可或缺的交通工具。为了保障飞机的航行安全,飞行前的安全检查是必不可少的一个环节。但因为飞行前的安全检查时间短、任务重,人工检测难免存在疏漏,受环境影响人工检查的效率和可靠行也会下降。相对于人工检查,使用地面无人车或者无人机进行检查能够提升巡检的效率和可靠性,并且巡检时记录的图像方便事后复现。
目前在室外环境中,地面无人车的定位主要采用卫星/惯性传感器融合的方法,但在客机巡检过程中有卫星拒止的环境存在,导致使用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-volatilememory),例如磁盘存储器。存储器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时刻地面绕机检查装置位姿信息,对点云分割完成的类进行筛选,去除不符合要求的点,计算公式为,
(1)
(2)
(3)
(4)
其中,
计算对应不同机轮的激光雷达点云的中心点之间的相对距离,与机轮之间的相对距离关系进行比较:
其中
筛选出满足机轮之间所有相对位置关系的一组激光雷达点云集合
步骤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坐标系下位姿
若当前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的计算公式如下:
上其中m4×4,u4×3,n3×4均为中间变量,计算方法如下式所示:
其中,
滤波器的噪声矩阵γ的计算方法如下式所示:
系统的噪声矩阵
其中,εwx,εwy,εwz分别是
根据量测量进行状态更新;
系统的滤波增益方程的计算方法如下:
其中,kk是k时刻系统的滤波增益,rk是量测噪声矩阵,hk是量测矩阵;
根据绕机检查装置当前状态以及量测信息可靠性不同,量测噪声矩阵、量测矩阵不同,具体包括如下步骤:
a)根据控制机构提供的反馈判断绕机检查装置当前是否处于零速状态,若绕机检查装置处于零速状态,使用k-1时刻时绕机检查装置的位置信息以及航向信息(xn(k-1),yn(k-1),ψn(k-1))作为量测,量测矩阵h(k)和量测噪声矩阵r(k)分别为:
其中,diag表示矩阵对角化,其中
系统的状态估值方程计算方法如下:
其中,
系统的估计均方误差方程为:
pk|k=(i-kkhk)pk|k-1
其中,pk|k为k时刻估计均方误差,i为单位矩阵。
b)若绕机检查装置当前不处于零速状态,第一位姿、第二位姿得到的信息可靠,则利用第一位姿、第二位姿以及里程计得到的信息作为量测,量测矩阵和量测噪声矩阵分别为:
其中,diag表示矩阵对角化,其中
系统的状态估值方程计算方法如下:
其中,
系统的估计均方误差方程为:
pk|k=(i-kkhk)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×6i2×202×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)为中间变量,通过以下公式计算:
其中
(2)位置预测公式为:
其中,xn(k)、yn(k)是k时刻绕机检查装置在飞机坐标系的位置;xn(k-1)、yn(k-1)是k-1时刻绕机检查装置在飞机坐标系的位置。
(3)
其中
(4)速度的预测公式为:
读取非初始检查时刻(k时刻)中解算得到的位姿
其中,δx、δy、δψ为设置的阈值,若满足上述条件,则认为步骤4中得到的位姿可靠;
继而,获取当前时刻绕机检查装置在slam坐标系下的位姿
其中,δx、δy、δψ为设置的阈值,若满足上述条件,则认为经过slam得到的位姿可靠。
在本实施例中,通过获取绕机检查装置的里程计信息将初始检查时刻的第一位姿以及第二位姿、非初始检查时刻的第一位姿以及第二位姿进行判断,得到更准确的绕机检查装置在飞机坐标系下的位姿。
此外,本申请还提供了一种计算机可读存储介质,所述计算机可读存储介质存储有绕机检查装置的定位程序,所述绕机检查装置的定位程序被处理器执行时实现上述所述的绕机检查装置的定位方法的步骤。
本领域内的技术人员应明白,本申请的实施例可提供为方法、系统、或计算机程序产品。因此,本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
应当注意的是,在权利要求中,不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的部件或步骤。位于部件之前的单词“一”或“一个”不排除存在多个这样的部件。本申请可以借助于包括有若干不同部件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中,这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。
尽管已描述了本申请的可选实施例,但本领域内的技术人员一旦得知了基本创造性概念,则可对这些实施例作出另外的变更和修改。所以,所附权利要求意欲解释为包括可选实施例以及落入本申请范围的所有变更和修改。
显然,本领域的技术人员可以对本申请进行各种改动和变型而不脱离本发
明的精神和范围。这样,倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内,则本申请也意图包含这些改动和变型在内。