CN113137966B - 一种利用惯组和激光测距的组合导航自主定位方法 - Google Patents

Abstract

本发明涉及一种利用惯组和激光测距的组合导航自主定位方法，首先根据飞行剖面任务，结合星历信息制定分时观测方案，获取飞行器到观测卫星的几何距离；然后通过分时观测得到的测距信息，得到激光测距导航系统给出的飞行器位置在导航系下的估计值X_Star＝[x_Star,y_Star,z_Star]^T；以

为状态量，根据飞行器运动学模型建立kalman滤波方程，将X_Star引入观测方程中，得到状态量的估计值及方差；用估计值对基于惯组的导航结果进行修正，作为组合导航的输出值，实现飞行器高精度自主定位。本发明测量精度高、定向性好，具备自主抗干扰能力。

Description

一种利用惯组和激光测距的组合导航自主定位方法

技术领域

本发明属于导航制导技术领域，涉及一种利用惯组和激光测距的组合导航自主定位方法。

背景技术

飞行器系统一般采用INS+GPS组合导航技术，全球卫星导航系统利用卫星发射无线电码，接收机通过无线电码的时延进行测距定位。与飞行器上惯导系统进行信息融合，解决纯惯性导航系统误差随时间积累甚至发散的问题。然而无线电信号容易受到外来电磁信号的干扰，功率为1W的卫星导航干扰机就可以使距离85km范围内的卫星接收机无法工作，给使用这种复合制导方法的飞行器武器带来隐患。

发明内容

本发明解决的技术问题是：克服现有技术的不足，提出一种利用惯组和激光测距的组合导航自主定位方法，测量精度高、定向性好、能够自主抗干扰。

本发明解决技术的方案是：

一种利用惯组和激光测距的组合导航自主定位方法，该方法的步骤包括：

步骤一，根据飞行剖面任务，结合星历信息确定飞行剖面覆盖下的可观测卫星，制定分时观测方案，通过飞行器上激光发射和接收装置，获取飞行器到观测卫星的几何距离；

步骤二，通过分时观测得到的测距信息，确定激光测距导航系统给出的飞行器位置在导航系下的估计值X_Star＝[x_Star,y_Star,z_Star]^T；

步骤三，以

为状态量，根据飞行器运动学模型建立kalman滤波方程，将X_Star引入观测方程中，得到状态量的估计值及方差；

步骤四，用步骤三的估计值对基于惯组的导航结果进行修正，作为组合导航的输出值，实现飞行器高精度自主定位。

所述步骤二的实现方式如下：

t_n时刻，激光测距得到的飞行器与观测卫星之间的理论几何距离

表示为：

其中

为t_n时刻n号观测卫星在导航系下的位置分量，通过星历信息获取，为已知量；(x_tn,y_tn,z_tn)^T为t_n时刻飞行器在导航系下的位置分量，为待求解未知量；

仅进行三次观测时，记录各观测时刻t₁,t₂,t₃飞行器与1,2,3号观测卫星之间的实际几何距离

有：

其中

分别为t₁,t₂,t₃时刻1,2,3号观测卫星在导航系下的位置分量，通过星历信息获取，为已知量；(x_t1,y_t1,z_t1)^T、(x_t2,y_t2,z_t2)^T、(x_t3,y_t3,z_t3)^T分别为t₁,t₂,t₃时刻飞行器在导航系下的位置分量；

为求得观测时刻飞行器在导航系下的位置信息，将各次观测间隔内基于惯组给出的飞行器位置分量变化量(Δx_{INS_3,1},Δy_{INS_3,1},Δz_{INS_3,1})^T、(Δx_{INS_3,2},Δy_{INS_3,2},Δz_{INS_3,2})^T作为已知输入信息，建立多次观测之间飞行器位置变化的关系，即：

将上述方程代入几何距离方程组，有：

其中

为已知量，通过上式求得基于激光测距获取的t₃时刻飞行器位置信息(x_t3,y_t3,z_t3)^T，作为滤波方程的观测量X_Star＝(x_Star,y_Star,z_Star)^T，用于误差模型的滤波估计。

按照如下方法确定激光测距获取的飞行器位置精度：

将方程组

去掉观测时刻对应的角标简写为如下形式：

其中(x⁽ⁿ⁾,y⁽ⁿ⁾,z⁽ⁿ⁾)^T,n＝1,2,3为当前时刻n号观测卫星在导航系下的位置分量，(x,y,z)^T为当前时刻飞行器在导航系下的位置分量，

为当前时刻飞行器与n号观测卫星之间的实际几何距离；进行线性化处理，非线性方程组在(x_k,y_k,z_k)^T处线性化后，得到定位方程的矩阵形式：

其中位置分量线性化后小偏差量

几何距离线性化偏差

其中

为(x_k,y_k,z_k)^T对应的几何距离，矩阵

激光测距误差用

表示，对1,2,3号观测卫星的测距误差分别为

激光测距得到的飞行器与观测卫星之间的实际几何距离表示为

其中r⁽ⁿ⁾,n＝1,2,3为当前时刻飞行器与n号观测卫星之间的理论几何距离，上述定位方程变为：

代表由激光测距误差

引起的导航系下位置分量对应的定位误差；

定义矩阵

为导航系下权系数阵；

定位误差协方差矩阵对角线上的元素分别为各个定位误差σ_x,σ_y,σ_z的方差，即：

即三维空间定位误差的标准差σ_p：

激光测距获取的飞行器位置精度因子

其中

为上述H矩阵对角线元素h₁₁,h₂₂,h₃₃的估计值。

测距误差

满足：

a)各个观测卫星的测距误差

均呈正态分布，均值为零，方差为σ²，即：

b)不同卫星之间的测距误差互不相关，即测距误差

的协方差矩阵

为对角阵：

其中I为3*3阶单位矩阵。

所述步骤三中，建立的kalman滤波方程如下：

状态方程：

观测方程：Z＝||(X_Star-X_INS)+δX||

其中：

为状态量；

δX为惯性导航系统的位置误差；

δV为惯性导航系统的速度误差；

φ_xyz＝[φ_x,φ_y,φ_z]^T为惯组x、y、z三方向的失准角；

ε＝[ε_x,ε_y,ε_z]^T为陀螺x、y、z三方向的随机零漂；

ε_b＝[ε_bx,ε_by,ε_bz]^T为陀螺x、y、z三方向的一阶马尔柯夫过程；

为加速度计x、y、z三方向的零偏；

为加速度x、y、z三方向的一阶马尔柯夫过程；

A(t)为状态矩阵，W(t)为系统噪声矩阵，根据飞行器模型给出；

X_INS为惯性导航系统计算的飞行器位置分量X_INS＝[x_INS,y_INS,z_INS]^T；

采用kalman滤波方法，针对上述kalman滤波方程进行多次观测和估计，得到状态量X的估计结果。

δX＝[δx,δy,δz]^T＝[x_INS-x,y_INS-y,z_INS-z]^T。

δV＝(δv_x,δv_y,δv_z)^T＝(v_{x_INS}-v_x,v_{y_INS}-v_y,v_{z_INS}-v_z)^T，其中V_INS＝(v_{x_INS},v_{y_INS},v_{z_INS})^T

为基于惯组数据导航计算得到的飞行器速度，V＝(v_x,v_y,v_z)^T为飞行器在导航系下的速度。

所述步骤四中，

其中

和

为利用惯组和激光测距进行组合导航后得出的飞行器位置和速度。

飞行器上仅安装一套激光发射设备和一套接收设备。

本发明与现有技术相比的有益效果是：

本发明克服了传统惯组+卫星导航系统的缺点，不引入卫星时钟的系统误差，可通过分时观测实现，只需在飞行器上安装一套激光发射接收装置，便于导航设备轻小型化实现。在大幅度提高飞行器导航精度前提下，具有抗干扰能力强、自主可控、安全性好、系统简单、使用方便、可拓展性强等特点，可广泛应用于包括临近空间在内的大气中云层以上的滑翔导弹、巡航导弹、各类飞机以及其他需要高精度导航的空中飞行器，彻底消除了无线电导航带来的易受干扰、易被诱骗等隐患，可在现有惯性器件精度水平下，修正惯性自主导航系统的速度和位置误差，大幅度提高我国战略及战术打击武器及其它飞行器、运载器的制导精度。

具体实施方式

下面对本发明作进一步阐述。

本发明提出了一种基于惯组+激光测距组合导航自主定位技术，飞行器发射激光测量与已知惯性空间位置卫星的相对距离，并与惯性导航计算的位置求差，通过组合导航滤波算法修正惯导系统误差，提高飞行器导航制导精度。该技术具有自主抗干扰性，可通过分时观测飞行器与已知卫星的距离，不引入卫星的时钟误差，实现高精度定位。

惯性+激光测距组合导航技术与INS+GPS紧耦合组合模式类似，重点需要解决在卫星数目不满足三维定位情况下的组合导航问题。惯性+激光测距组合导航系统误差模型为线性时变模型，针对线性时变系统，引入基于时间区间的参数冻结处理思想，即分段线性定常系统PWCS(piece-wise constant system)可观测性分析方法。利用PWCS可观测分析理论开展惯性+激光测距组合导航系统的可观测度分析，并基于弱观测度系统建立kalman滤波模型，获取导航结果。进一步的，根据不同观测模式下的组合导航精度，提出飞行过程中对观测卫星的选择原则，为激光测距仪的任务规划服务。

飞行器在飞行过程中，根据卫星地面运维系统提供的激光导航卫星高精度轨道参数，飞行器伺服平台引导激光发射和接收系统指向、跟踪选定的卫星，向卫星发射激光并接收反射光，通过测量发射和接收的时间差，计算飞行器与在轨卫星之间的距离。理论上同时向三颗卫星发射激光并接收反射光，测得与三颗卫星的距离，即可完成飞行器在三维空间中的定位，进行与惯组导航结果的融合修正计算。为了实现飞行器上设备轻小型化，简化系统配置，只能在飞行器安装一套定位系统，故考虑单星测距、分时测量方案，每次测量飞行器与一颗导航卫星之间的距离，多次测量与不同卫星的距离，记录各次测量间隔时间以及飞行器运动变化量。利用惯性导航短时间内漂移小精度高的特点，将多次观测的激光测距结果进行时标对齐修正，然后采用kalman滤波方法与惯性导航结果进行信息融合，从概率统计最优的角度估算出系统误差并予以补偿，输出修正后的导航信息，供飞行器精确制导使用。

本发明飞行器激光自主定位方法采用惯组+激光测距的组合导航方式，利用星历信息和激光测距获取飞行器位置信息，融合惯性导航结果，实现飞行器高精度自主定位。

利用惯组和激光测距的组合导航自主定位方法主要步骤为：

1、根据飞行剖面任务，结合星历信息制定分时观测方案。

根据飞行剖面任务，结合星历信息确定飞行剖面覆盖下的可观测卫星，制定分时观测方案。通过飞行器上激光发射和接收装置，获取飞行器到观测卫星的几何距离，各观测时刻t₁,t₂,t₃,…t_n飞行器与1,2,3,…,n号观测卫星的几何距离记为

2、通过分时观测得到的测距信息，得到激光测距导航系统给出的飞行器位置在导航系下的估计值X_Star＝[x_Star,y_Star,z_Star]^T。

激光测距自主定位方案由于激光发射装置和反射接收装置都安装在飞行器上，发射和接收时钟信息同源，只需在导航开始时刻进行UTC授时，不引入观测卫星时钟及其误差，因此与GPS系统最少需要观测四个卫星进行定位不同，理论上同时向三颗卫星发射激光并接收反射光，观测到飞行器到三颗卫星的距离值即可求取飞行器在导航系下的位置信息。为求解飞行器的三维坐标分量，需要建立三个方程，即通过激光测距对飞行器进行定位的本质为求解三元非线性方程组。

为了实现飞行器上设备轻小型化尽可能简化系统配置，考虑在飞行器上仅安装一套激光发射和接收设备的约束下，设计单星测距、分时测量方案，每次测量飞行器与一颗导航卫星之间的距离，多次测量与不同卫星的距离，记录各次测量间隔时间以及飞行器位置变化量。利用惯组导航短时间内漂移小精度高的特点，将多次观测的激光测距结果进行时标对齐修正后估计出飞行器位置作为的惯组+激光测距组合导航模型的观测量。

t_n时刻，激光测距得到的飞行器与观测卫星之间的理论几何距离

表示为：

其中

为t_n时刻卫星n在导航系下的位置分量，可通过星历信息获取，为已知量；

为t_n时刻观测获取的飞行器与观测卫星n之间的理论几何距离，为测量量；(x_tn,y_tn,z_tn)^T为t_n时刻飞行器在导航系下的位置分量，为待求解未知量。仅进行三次观测时，记录各观测时刻t₁,t₂,t₃飞行器对1,2,3号观测卫星的实际测距信息(实际几何距离)

此时有：

利用惯性导航短时间内误差积累少精度高的特点，将各次观测间隔内惯组给出的飞行器位置变化量(Δx_{INS_3,1},Δy_{INS_3,1},Δz_{INS_3,1})^T、(Δx_{INS_3,2},Δy_{INS_3,2},Δz_{INS_3,2})^T作为已知输入信息，建立多次观测之间飞行器位置变化的关系，即：

将上述方程代入几何距离方程组，有：

其中

为已知量，通过上式求得基于激光测距获取的t₃时刻飞行器位置信息(x_t3,y_t3,z_t3)^T，作为滤波方程的观测量X_Star＝(x_Star,y_Star,z_Star)^T，用于误差模型的滤波估计。

将方程组

去掉观测时刻对应的角标简写为如下形式：

其中(x⁽ⁿ⁾,y⁽ⁿ⁾,z⁽ⁿ⁾)^T,n＝1,2,3为当前时刻n号观测卫星在导航系下的位置分量，(x,y,z)^T为当前时刻飞行器在导航系下的位置分量，

为当前时刻飞行器与n号观测卫星之间的实际几何距离；并进行线性化处理，非线性方程组在(x_k,y_k,z_k)^T处线性化后，得到定位方程的矩阵形式：

其中位置分量线性化后小偏差量

几何距离线性化偏差

其中

为(x_k,y_k,z_k)^T对应的几何距离，矩阵

激光测距误差用

表示，对1,2,3号观测卫星的测距误差分别为

激光测距得到的飞行器与观测卫星之间的实际几何距离表示为

其中r⁽ⁿ⁾,n＝1,2,3为当前时刻飞行器与n号观测卫星之间的理论几何距离，上述定位方程变为：

代表由激光测距误差

引起的定位误差：

为导出测距误差对定位误差的影响关系，假设测距误差的模型

满足：

a)各个卫星的测距误差

均呈正态分布，均值为零，方差为σ²：

b)不同卫星之间的测距误差互不相关，即测距误差向量

的协方差矩阵

为对角阵：

定义矩阵

为导航系下权系数阵。上式表明测距误差的方差σ²被权系数矩阵H放大后转化为定位误差的方差。即定位精度取决于三方面：1)激光直接测量误差；2)观测卫星的轨道误差；3)观测卫星的几何分布。权系数矩阵中的元素值越小，则测量误差被放大成定位误差的程度就越低。用精度因子DOP的概念来表示测量误差的放大系数。

定位误差协方差矩阵对角线上的元素分别为各个定位误差σ_x,σ_y,σ_z的方差，即：

即三维空间定位误差的标准差

激光导航卫星三维位置精度因子

当飞行器分时捕获三颗激光导航卫星时，如果卫星轨道精度为3m，激光测距精度优于1m，当三维位置精度因子PDOP<4时，则此工况下导航定位精度优于16m。实际应用中通过对不同空间位置的卫星进行多次观测，降低精度因子，可进一步提高定位精度。

3、以

为状态量，根据飞行器运动学模型建立kalman滤波方程，将X_Star引入观测方程中，得到状态量的估计值及方差。

用激光测距获取的飞行器位置信息X_Star＝(x_Star,y_Star,z_Star)^T作为观测量，建立惯性+激光测距组合导航定位的kalman滤波方程，如下：

状态方程：

观测方程：Z＝||(X_Star-X_INS)+δX||          (11)

其中：

为状态量；

δX＝[δx,δy,δz]^T＝[x_INS-x,y_INS-y,z_INS-z]^T为惯性导航系统的位置误差；

δV＝[δV_x,δV_y,δV_z]^T＝[V_xINS-V_x,V_yINS-V_y,V_zINS-V_z]^T为惯性导航系统的速度误差；

φ_xyz＝[φ_x,φ_y,φ_z]^T为惯性平台三方向的失准角；

ε＝[ε_x,ε_y,ε_z]^T为陀螺随机零漂；

ε_b＝[ε_bx,ε_by,ε_bz]^T为陀螺一阶马尔柯夫过程；

为加速度计零偏；

为加速度计一阶马尔柯夫过程；

A(t)为状态矩阵，W(t)为系统噪声矩阵，根据飞行器模型给出。

X_Star为卫星激光测距导航获取的飞行器位置分量X_Star＝[x_Star,y_Star,z_Star]^T；

X_INS为基于惯组的导航结果计算的飞行器位置分量X_INS＝[x_INS,y_INS,z_INS]^T；

采用kalman滤波方法，针对上述模型进行多次观测和估计，得到状态量X的估计结果。

4、用误差模型估计值对惯导结果进行修正，作为组合导航系统输出值：

实现飞行器高精度自主定位。其中

和

为利用惯组和激光测距进行组合导航后得出的飞行器位置和速度。

本发明中，利用飞行器发射激光测量与已知惯性空间位置卫星的相对距离，并与惯性导航计算的位置求差，通过组合导航滤波算法修正惯导系统误差，提高飞行器导航制导精度。具有具自主抗干扰性，可通过分时观测飞行器与已知卫星的距离，不引入卫星的时钟误差，实现高精度定位。

本发明克服了传统惯组+卫星导航系统的缺点，不引入卫星时钟的系统误差，可通过分时观测实现，只需在飞行器上安装一套激光发射接收装置，便于导航设备轻小型话实现。在大幅度提高飞行器导航精度前提下，具有抗干扰能力强、自主可控、安全性好、系统简单、使用方便、可拓展性强等特点，可广泛应用于包括临近空间在内的大气中云层以上的滑翔导弹、巡航导弹、各类飞机以及其他需要高精度导航的空中飞行器，彻底消除了无线电导航带来的易受干扰、易被诱骗等隐患，可在现有惯性器件精度水平下，修正惯性自主导航系统的速度和位置误差，大幅度提高我国战略及战术打击武器及其它飞行器、运载器的制导精度。

本发明未详细说明部分属本领域技术人员公知常识。

Claims (6)

1.一种利用惯组和激光测距的组合导航自主定位方法，其特征在于该方法的步骤包括：

步骤一，根据飞行剖面任务，结合星历信息确定飞行剖面覆盖下的可观测卫星，制定分时观测方案，通过飞行器上激光发射和接收装置，获取飞行器到观测卫星的几何距离；

步骤二，通过分时观测得到的测距信息，确定激光测距导航系统给出的飞行器位置在导航系下的估计值X_Star＝[x_Star,y_Star,z_Star]^T；

实现方式如下：

t_n时刻，激光测距得到的飞行器与观测卫星之间的理论几何距离

表示为：

其中

为t_n时刻n号观测卫星在导航系下的位置分量，通过星历信息获取，为已知量；(x_tn,y_tn,z_tn)^T为t_n时刻飞行器在导航系下的位置分量，为待求解未知量；

仅进行三次观测时，记录各观测时刻t₁,t₂,t₃飞行器与1,2,3号观测卫星之间的实际几何距离

有：

其中

分别为t₁,t₂,t₃时刻1,2,3号观测卫星在导航系下的位置分量，通过星历信息获取，为已知量；(x_t1,y_t1,z_t1)^T、(x_t2,y_t2,z_t2)^T、(x_t3,y_t3,z_t3)^T分别为t₁,t₂,t₃时刻飞行器在导航系下的位置分量；

为求得观测时刻飞行器在导航系下的位置信息，将各次观测间隔内基于惯组给出的飞行器位置分量变化量(Δx_{INS_3,1},Δy_{INS_3,1},Δz_{INS_3,1})^T、(Δx_{INS_3,2},Δy_{INS_3,2},Δz_{INS_3,2})^T作为已知输入信息，建立多次观测之间飞行器位置变化的关系，即：

将上述方程代入几何距离方程组，有：

其中

为已知量，通过上式求得基于激光测距获取的t₃时刻飞行器位置信息(x_t3,y_t3,z_t3)^T，作为滤波方程的观测量X_Star＝(x_Star,y_Star,z_Star)^T，用于误差模型的滤波估计；

步骤三，以

为状态量，根据飞行器运动学模型建立kalman滤波方程，将X_Star引入观测方程中，得到状态量的估计值；其中，δX为惯性导航系统的位置误差；δV为惯性导航系统的速度误差；φ_xyz＝[φ_x,φ_y,φ_z]^T为惯组x、y、z三方向的失准角；ε＝[ε_x,ε_y,ε_z]^T为陀螺x、y、z三方向的随机零漂；ε_b＝[ε_bx,ε_by,ε_bz]^T为陀螺x、y、z三方向的一阶马尔柯夫过程；

为加速度计x、y、z三方向的零偏；

为加速度x、y、z三方向的一阶马尔柯夫过程；

步骤四，用步骤三的估计值对基于惯组的导航结果进行修正，作为组合导航的输出值，实现飞行器高精度自主定位。

2.根据权利要求1所述的一种利用惯组和激光测距的组合导航自主定位方法，其特征在于，所述步骤三中，建立的kalman滤波方程如下：

状态方程：

观测方程：Z＝||(X_Star-X_INS)+δX||

其中：

为状态量；

δX为惯性导航系统的位置误差；

δV为惯性导航系统的速度误差；

φ_xyz＝[φ_x,φ_y,φ_z]^T为惯组x、y、z三方向的失准角；

ε＝[ε_x,ε_y,ε_z]^T为陀螺x、y、z三方向的随机零漂；

ε_b＝[ε_bx,ε_by,ε_bz]^T为陀螺x、y、z三方向的一阶马尔柯夫过程；

为加速度计x、y、z三方向的零偏；

为加速度x、y、z三方向的一阶马尔柯夫过程；

A(t)为状态矩阵，W(t)为系统噪声矩阵，根据飞行器模型给出；

X_INS为惯性导航系统计算的飞行器位置分量X_INS＝[x_INS,y_INS,z_INS]^T；

采用kalman滤波方法，针对上述kalman滤波方程进行多次观测和估计，得到状态量X的估计结果。

3.根据权利要求2所述的一种利用惯组和激光测距的组合导航自主定位方法，其特征在于，δX＝[δx,δy,δz]^T＝[x_INS-x,y_INS-y,z_INS-z]^T。

4.根据权利要求3所述的一种利用惯组和激光测距的组合导航自主定位方法，其特征在于，δV＝(δv_x,δv_y,δv_z)^T＝(v_{x_INS}-v_x,v_{y_INS}-v_y,v_{z_INS}-v_z)^T，其中V_INS＝(v_{x_INS},v_{y_INS},v_{z_INS})^T为基于惯组数据导航计算得到的飞行器速度，V＝(v_x,v_y,v_z)^T为飞行器在导航系下的速度。

5.根据权利要求4所述的一种利用惯组和激光测距的组合导航自主定位方法，其特征在于，所述步骤四中，

其中

和

为利用惯组和激光测距进行组合导航后得出的飞行器位置和速度。

6.根据权利要求1所述的一种利用惯组和激光测距的组合导航自主定位方法，其特征在于，飞行器上仅安装一套激光发射设备和一套接收设备。