CN117308938B - 一种基于多次激光测距的惯导寻北收敛误差快速补偿方法 - Google Patents

Abstract

一种基于多次激光测距的惯导寻北收敛误差快速补偿方法。属于目标定位误差修正领域，具体涉及基于多次激光测距的惯导寻北收敛误差快速补偿方法。所述方法包括如下步骤：S1、实时数据采集；S2、筛选激光测距值；S3、FAST匹配算法得出跟踪偏移量；S4、解算目标单帧定位结果；S5、快速寻北误差补偿。解决了以往方法中需要无人机进行绕飞模式的问题，在无人机进行短时间内的直线飞行时仍能完成寻北误差的补偿，得到很高的目标定位精度。本发明可以应用在无人机目标定位领域、无人机目标追踪领域以及大中小型无人机寻北误差补偿领域。

Description

一种基于多次激光测距的惯导寻北收敛误差快速补偿方法

技术领域

本发明属于目标定位误差修正领域，具体涉及基于多次激光测距的惯导寻北收敛误差快速补偿方法。

背景技术

在无人机进行对地目标定位时，常常在光电吊舱内部安装小型惯导，以避免减震器导致的误差。然而，小型惯导的寻北精度一直是个问题。在短程飞行中，其近似系统误差的形式，即偏差角度短时间内几乎不变；而在长程飞行中，其呈现出随机误差的形式，即偏差角度长时间呈随机波动状态。这就给其校正带来了很大的麻烦。

在专利“无人机高精度目标定位方法”（CN113074715A）中，利用激光测距值。并基于交叉测量的原理，通过围绕目标点飞行获取数据，再解算方程组得到最优结果。这种方法的优点在于可以避免惯导姿态角误差的影响，但局限性在于只能应用于小型无人机。大中型无人机的飞行轨迹需要长时间保持一条直线，无法满足任意的绕飞的需求，且这种方法耗时较长，限制条件较多，很难做到快速动态的补偿。

发明内容

为了解决以往方法中需要无人机进行绕飞模式而由于大中型无人机绝大多数时间都直线飞行，以往的绕飞修正方法失效的问题，本发明提供一种惯导寻北收敛误差快速补偿方法，所述方法包括如下步骤：

S1、实时数据采集：实时采集内部惯导的航向角和俯仰角数据、飞机GPS的经度、纬度和高度数据以及飞机与跟踪目标的激光测距值数据；

S2、筛选激光测距值：通过先后设定阈值的方法依次对激光测距值进行初步筛选和最终筛选；

S3、FAST匹配算法得出跟踪偏移量：通过FAST特征匹配的方法得出当前图像与10秒后图像的精确跟踪偏移量，其中帧频为20帧/秒，得到一个测距周期内的偏移量序列，由像素值表示：，其中，k表示测距采样数；

S4、解算目标单帧定位结果：根据内部惯导的航向角和俯仰角数据以及步骤S2中筛选的激光测距值，通过北东地坐标系→地固地心坐标系→地理坐标系的坐标系解算转换，得出目标单帧的经度、纬度和高度定位结果；

S5、快速寻北误差补偿：通过最优化函数对步骤S4中解算的定位结果进行迭代获得寻北误差，将所述寻北误差用于对后续解算到的目标单帧经度、纬度和高度的误差补偿。

所述方法的有益效果为：解决了以往方法中需要无人机进行绕飞模式的问题，在无人机进行短时间内的直线飞行时仍能完成寻北误差的补偿，得到很高的目标定位精度。

本发明可以应用在无人机目标定位领域、无人机目标追踪领域以及大中小型无人机寻北误差补偿领域。

附图说明

图1为本发明所述方法流程图。

具体实施方式

下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明的保护范围。

实施例1、

本实施例提供一种惯导寻北收敛误差快速补偿方法，如图1所示，所述方法包括如下步骤：

S1、实时数据采集：实时采集内部惯导的航向角和俯仰角数据、飞机GPS的经度、纬度和高度数据以及飞机与跟踪目标的激光测距值数据；

S2、筛选激光测距值：通过先后设定阈值的方法依次对激光测距值进行初步筛选和最终筛选；

S3、FAST匹配算法得出跟踪偏移量：通过FAST特征匹配的方法得出当前图像与10秒后图像的精确跟踪偏移量，其中帧频为20帧/秒，得到一个测距周期内的偏移量序列，由像素值表示：，其中，k表示测距采样数；

S4、解算目标单帧定位结果：根据内部惯导的航向角和俯仰角数据以及步骤S2中筛选的激光测距值，通过北东地坐标系→地固地心坐标系→地理坐标系的坐标系解算转换，得出目标单帧的经度、纬度和高度定位结果；

S5、快速寻北误差补偿：通过最优化函数对步骤S4中解算的定位结果进行迭代获得寻北误差，将所述寻北误差用于对后续解算到的目标单帧经度、纬度和高度的误差补偿。

实施例2、

本实施例是对实施例1的进一步限定，对步骤S1进行进一步说明。

将光电吊舱安装到无人机的固定位置，在无人机遵循指定航线飞行时，如发现所要定位的地理目标，锁定并开启跟踪状态。在吊舱对目标处于跟踪状态时，持续对目标进行激光照射，并通过主控板实时采集内部惯导的航向角和俯仰角数据、飞机GPS的经纬高数据以及飞机与跟踪目标的激光测距值数据。在一个测距周期中，从接收到第一组测距值开始，收集20—30秒的数据。在采集数据的过程中，保证每组数据的时刻都是经过对准的，即每组中的各项数据都来自同一时刻。

实施例3、

本实施例是对实施例1的进一步限定，对步骤S2进行进一步说明。

由于在用激光照射目标的过程中有时会遇到一些遮挡（如云雾等），测距值中可能出现一些异常值，这就需要对测距值的有效性进行辨别。首先获得飞行区域的地面平均海拔，同时已知飞机平稳飞行时的海拔为/>，内部惯导的实时俯仰角为/>，k表示测距采样数，为从开始测距算起的第k次测距采样，可计算得到目标的大致距离为

设定一个阈值（100到200之间即可），实时采集到的测距值为/>，如果，t代表测距采样的时刻，则认定此时的测距值是正常的，反之则剔除此时的测距值。在初步筛选后，用保留的测距值，通过最小二乘法拟合出一条随时间变化的直线/>，其中，/>表示设定的第1次测距的理论值，/>表示相邻两帧的时间间隔，/>表示单位时间内激光测距值的变化量，拟合方法如下：

首先假设初步筛选后，数据的变化遵循线性关系，即能用函数的形式来表示，那么总误差的平方为：

；

其中，代表的是初步筛选后第k次测距采样的值，/>表示第k次测距采样的理论值，a，b为常数。

解出以下方程组：

最终得到线性函数的参数，并将公式转化为的形式。

另设定一个阈值（20到50之间即可），如果

；

则保留此时刻的测距值，反之则剔除。

实施例4、

本实施例是对实施例1的进一步限定，对步骤S3进行进一步说明。

在吊舱对目标处于持续跟踪状态时，有时会由于飞机的安装平台的震动或目标的视角变化较大而导致跟踪出现偏差，这时需要通过FAST特征匹配的方法得出当前图像与十秒后的图像（帧频为20帧/秒）的精确的跟踪偏移量。之所以用FAST特征匹配方法而不是其它更有效的匹配方法，是因为在跟踪状态下的图像之间的相似度非常高，用运行速度快的FAST算法就是最优解。最终得到一个测距周期内的偏移量序列，由像素值表示：

。

实施例5、

本实施例是对实施例1的进一步限定，对步骤S4进行进一步说明。

根据收集到的一个周期内的各组数据，计算每一次测距采样时刻的目标经度、纬度和高度。计算过程如下：

通过内部惯导的航向角和俯仰角、测距仪的测距值计算得到目标在以内部惯导中心为北东地坐标系原点的坐标值，采用：

；

将目标在北东地坐标系的坐标转换到地固地心坐标系上，采用

；

获得，其中，表示余弦函数cos，/>表示正弦函数sin，/>表示目标单帧在地固地心坐标系中经度，/>表示目标单帧在地固地心坐标系中纬度，/>表示目标单帧在地固地心坐标系中高度，/>，为椭球曲率半径/>，为椭球长半轴，/>，为椭球短半轴，/>为椭球第一偏心率。

最终我们将得出的值代入以下公式中，获得目标单帧在地理坐标系的经度、纬度/>和高度/>：

；

获得，其中，，/>为椭球第二偏心率。

由此得到最终的目标经纬高，最终我们收集到一组解算出的目标经纬高：

。

实施例6、

本实施例是对实施例1的进一步限定，对步骤S5进行进一步说明。

将用FAST匹配算法得到的像素偏移量换算成实际距离，通过

；

获得，其中，、/>分别表示用吊舱中光学系统拍摄到的图像的横轴和纵轴的像元尺寸，f表示吊舱中光学系统镜头的焦距，/>和中的第/>个数对。

设置寻北误差补偿值的分辨率为0.01°，将步骤S4中解算得出目标单帧的经度、纬度和高度定位结果的过程视作将输入值代入特定函数最终得到输出值的过程，记作：

；

为便于后续的计算，拆分这一公式，得到：

；

；

；

由于在同一时刻，无需进行更改，可将上式改为

；

这样就将问题简化为了只补偿航向角误差（即惯导寻北误差）的情况。我们给出最优化函数：

；

其中表示地球周长，/>表示寻北误差。

通过迭代最优化函数，当达到/>时，即可计算出/>。通过分析发现，当内部惯导的航向角不同时，需要在上面的公式内加上权重，以更好地凸显不同航向中经纬度补偿地侧重点，这里设权重因子的函数为：/>；

代表某一时刻的俯仰值，因为俯仰短时间内变动不大，所以随意选取一个值代入即可。

由此上面的最优化函数的完整形式可重写为：

；

通过此函数迭代至最小值，即可完成的计算。最终将/>代入到定位公式中，对目标单帧经度、纬度和高度的误差补偿，得到准确的经纬高数值：

；

；

。

Claims (7)

1.一种惯导寻北收敛误差快速补偿方法，其特征在于，所述方法包括如下步骤：

S1、实时数据采集：实时采集内部惯导的航向角和俯仰角数据、飞机GPS的经度、纬度和高度数据以及飞机与跟踪目标的激光测距值数据；

S2、筛选激光测距值：通过先后设定阈值的方法依次对激光测距值进行初步筛选和最终筛选；

S3、FAST匹配算法得出跟踪偏移量：通过FAST特征匹配的方法得出当前图像与10秒后图像的精确跟踪偏移量，其中帧频为20帧/秒，得到一个测距周期内的偏移量序列，由像素值表示：，其中，k表示测距采样数；

S4、解算目标单帧定位结果：根据内部惯导的航向角和俯仰角数据以及步骤S2中筛选的激光测距值，通过北东地坐标系→地固地心坐标系→地理坐标系的坐标系解算转换，得出目标单帧的经度、纬度和高度定位结果；

S5、快速寻北误差补偿：通过最优化函数对步骤S4中解算的定位结果进行迭代获得寻北误差，将所述寻北误差用于对后续解算到的目标单帧经度、纬度和高度的误差补偿；

所述通过最优化函数对步骤S4中解算的定位结果进行迭代获得寻北误差为通过迭代优化函数：

；

直至最小值，此时对应的/>即为寻北误差；

其中，，其中，i表示第i次测距采样，每次测距采样都会解算出一次单帧定位结果，/>随意选取一个值代入即可；

、/>分别表示用吊舱中光学系统拍摄到的图像的横轴和纵轴的像元尺寸；

，k表示测距采样数，/>即表示第k次测距采样的单帧经度定位结果，代表将步骤S4中解算得到目标单帧经度定位结果的过程视作将输入值代入特定函数/>最终得到输出值的过程，在此将/>与/>等同；

=/>，/>即表示第k次测距采样的单帧纬度定位结果，/> =/>代表将步骤S4中解算得到目标单帧纬度定位结果的过程视作将输入值代入特定函数/>最终得到输出值的过程；

=/> ，/>即表示第k次测距采样的单帧高度定位结果，/> =/>代表将步骤S4中解算得到目标单帧高度定位结果的过程视作将输入值代入特定函数/>最终得到输出值的过程；

表示地球周长。

2.根据权利要求1所述的惯导寻北收敛误差快速补偿方法，其特征在于，步骤S1中，在一个测距周期中，从接收到第一组测距值开始，收集20—30秒的数据，在采集数据的过程中，保证每组中的各项数据都来自同一时刻。

3.根据权利要求1所述的惯导寻北收敛误差快速补偿方法，其特征在于，步骤S2中，所述初步筛选通过：

；

进行，其中，表示通过海拔及俯仰角估算得出的目标距离，其中，/>表示激光测距值，t代表测距采样的时刻，k表示测距采样数，为从开始测距算起的第k次测距采样，/>表示初步筛选阈值，abs表示求取绝对值的函数。

4.根据权利要求3所述的惯导寻北收敛误差快速补偿方法，其特征在于，步骤S2中，所述最终筛选具体为：将初步筛选后得到的激光测距值通过最小二乘法拟合为一条直线，自变量为测距采样相邻两帧的时间间隔，因变量为激光测距值；另设定最终筛选阈值对上述直线表示的激光测距值进行最终筛选，通过：

；

进行，表示通过拟合的直线获得的测距值，/>表示最终筛选阈值。

5.根据权利要求3所述的惯导寻北收敛误差快速补偿方法，其特征在于，步骤S4中，通过航向角、俯仰角和激光测距值计算得到目标单帧在以内部惯导中心为北东地坐标系原点的坐标值通过：

；

获得，其中，表示俯仰角，/>表示航向角。

6.根据权利要求5所述的惯导寻北收敛误差快速补偿方法，其特征在于，步骤S4中，将目标单帧在北东地坐标系的坐标转换到地固地心坐标系上，通过：

；

获得，其中，表示余弦函数cos，/>表示正弦函数sin，/>表示目标单帧在地固地心坐标系中经度，/>表示目标单帧在地固地心坐标系中纬度，/>表示目标单帧在地固地心坐标系中高度，/>，为椭球曲率半径/>，为椭球长半轴，，为椭球短半轴，/>为椭球第一偏心率。

7.根据权利要求6所述的惯导寻北收敛误差快速补偿方法，其特征在于，步骤S4中，目标单帧在地理坐标系的经度、纬度/>和高度/>通过：

；

获得，其中，，/>为椭球第二偏心率。