一种轨迹跟踪定位滤波系统及救援无人机的融合导航方法技术方案

一种轨迹跟踪定位滤波系统及救援无人机的融合导航方法，属于救援无人机导航领域。所述轨迹跟踪定位滤波系统的轨迹跟踪模块和自动跟踪返回模块均固定安装在救援无人机导航平台上，轨迹跟踪模块与自动返回模块信号连接，自动返回模块与救援无人机导航平台信号连接。所述方法是：计算陀螺仪的旋转角度；计算导航平台的旋转角度；速度和位置描述；定义惯性导航系统和北斗组合系统的状态向量；利用北斗接收机的测量，构建融合测量位置向量；基于无迹卡尔曼滤波的滤波方法，使用无迹变换技术来近似非线性系统状态的概率分布。本发明专利技术能够跟踪和记录无人机在搜救中的路径，为工作人员提供简单的危险环境和状况定位，为进一步的搜救工作做好预期的准备。工作做好预期的准备。工作做好预期的准备。

【技术实现步骤摘要】
一种轨迹跟踪定位滤波系统及救援无人机的融合导航方法


[0001]本专利技术属于救援无人机导航领域，具体涉及一种轨迹跟踪定位滤波系统及救援无人机的融合导航方法。

技术介绍

[0002]危险事故常常出乎意料地发生，在不熟悉地形和未知地形危险因素的情况下进行搜救往往会浪费大量的时间。现有救援平台存在各种问题，如履带式救援平台，是在传统轮式机器人的基础上发展起来的，来满足军事侦察、拆除危险物体等需要。它的移动速度很快，但是当面对恶劣的环境时，由于它的尺寸比较大，进入狭窄的区域十分不方便，会导致救援进程中断。在空间方面，大部分救援平台的设计研究都是基于卫星定位进行轨迹模拟，在真实的环境中，卫星导航具有一定的局限性。例如，在建筑物、隧道、洞穴等封闭环境中，信号衰减非常严重，所以在大多数情况下，无法提供准确的定位信息。惯性导航系统目前用于自主导航定位，其精度较差，因此提出利用惯性导航系统结合北斗导航系统构建通信降级条件下的救援无人机的融合导航方法。

技术实现思路

[0003]本专利技术的目的是为解决
技术介绍
中存在的上述问题，提供一种轨迹跟踪定位滤波系统及救援无人机的融合导航方法。
[0004]实现上述目的，本专利技术采取的技术方案如下：
[0005]一种轨迹跟踪定位滤波系统，所述轨迹跟踪定位滤波系统由轨迹跟踪模块和自动跟踪返回模块组成，轨迹跟踪模块和自动跟踪返回模块均固定安装在救援无人机导航平台上，轨迹跟踪模块与自动返回模块信号连接，自动返回模块与救援无人机导航平台信号连接；
[0006]轨迹跟踪模块，用于利用互补算法计算惯性导航系统(INS)和北斗组合系统的系统状态向量；利用北斗接收机的测量，构建融合测量位置向量，并能够测量出对应北斗接收机的速度误差和位置误差；基于无迹卡尔曼滤波的滤波方法，使用无迹变换技术来近似非线性系统状态的概率分布，预测均值和协方差；
[0007]自动跟踪返回模块，用于存储轨迹跟踪模块传输的轨迹数据，当信号中断，外部人员无法控制时，所述导航平台开始读取轨迹数据，实现导航平台自动沿原路径轨迹返回。
[0008]一种救援无人机的融合导航方法，所述方法包括以下步骤：
[0009]步骤1：计算陀螺仪的旋转角度σ；根据陀螺仪的科里奥利力原理，输出陀螺仪的角速度ω(t)，进而得到陀螺仪的旋转角度σ，如式(2)所示；
[0010]σ＝∫ω(t)dt
ꢀꢀꢀꢀꢀꢀꢀ
(2)
[0011]步骤2：计算导航平台的旋转角度；采集一定时间间隔的导航平台的旋转角度，得到平面加速度方向，并结合磁强计，实时补偿修正方向，导航平台的姿态角的初始值通过求解四元数旋转矩阵得到：
[0012][0013]其中，ψ0、θ0、γ0分别为导航平台的偏航角、俯仰角和滚转角的初始值；
[0014]由导航平台的偏航角ψ0、俯仰角θ0和滚转角γ0的初始值构成导航平台的姿态角的初始值；
[0015][0016]其中，ε
b
＝(ε
x
,ε
y
,ε
z
)
T
为b坐标系中陀螺仪在x、y、z坐标方向上的持续漂移；
[0017]为陀螺仪与持续漂移对应的白噪声在x、y、z坐标方向上的分量；
[0018]ψ、θ、γ分别为导航平台的偏航角、俯仰角和滚转角，这些角组合构成导航平台的旋转角度；
[0019]ω
gb
表示由g坐标系到b坐标系的角速度转换，ω
gb
在b坐标系中的投影为：
[0020]其中，分别为b坐标系中在x、y、z坐标方向上的值；
[0021]ε
x
、ε
y
、ε
z
分别为b坐标系中陀螺仪在x、y、z坐标方向上的常值漂移；
[0022]步骤3：速度和位置描述；惯性导航系统(INS)的速度和位置方程为：
[0023][0024]其中，为分别以纬度L、经度λ和高度h表示的位置；
[0025]分别为x、y、z轴方向上的速度分量，分别为x、y、z轴方向上比力的度量，ω
ie
为地球自转角速度，g为重力加速度，R
M
和R
N
分别为子午线和主垂线方向的曲率半径，为加速度计产生的恒偏置误差在x、y、z轴方向的分量，为由b坐标系到g坐标系的姿态矩阵，w为高斯白噪声，陀螺仪的常值漂移ε和加速度计的恒偏置误差置零；
[0026]步骤4：定义惯性导航系统(INS)和北斗组合系统的系统状态向量为：
[0027][0028]根据惯性导航系统(INS)和北斗组合系统的状态，得到上述两个系统的的组合状态向量：
[0029][0030]其中，f(
·
)为一个非线性函数，G为过程白噪声矢量的分布矩阵，u为附加10％建模不确定性的集合；利用欧拉离散化公式将上式离散化，得到上述两个系统的组合状态的离散向量；
[0031]x
k
＝f(x
k
‑1)+Gu；
[0032]其中，x
k
为x(t)函数在k时刻的状态，x
k
‑1为在x(t)函数上一时刻的状态；
[0033]步骤5：利用北斗接收机的测量，构建融合测量位置向量；采用北斗接收机在速度和位置方面的高精度输出，得到要测量的位置向量z
k
；
[0034]z
k
＝[v
x
,v
y
,v
z
,L,λ,h]T
；
[0035]基于已确定的系统状态向量和测量向量，构建惯性导航系统(INS)和北斗组合系统的融合测量向量z
k
；
[0036][0037]其中，v
v
和v
p
是测量噪声，分别对应北斗接收机的速度误差和位置误差；H
k
为矩阵，H
k
＝[06×3,I6×6,06×6]；x
k
为x(t)函数在k时刻的状态；
[0038]由于所提出的INS/北斗是一个闭环系统，滤波器估计的导航参数将用于补偿惯性导航系统的惯性测量单元(IMU)的漂移，从而能够得到更精确的惯性导航系统(INS)和北斗组合系统的组合导航解；
[0039]步骤6：基于无迹卡尔曼滤波的滤波方法，使用无迹变换技术来近似非线性系统状态的概率分布；首先，从状态的先验均值和协方差中确定地选择一组极小的sigma点；然后，将无迹变换技术依次应用于这些点以产生一个变换样本，并利用变换样本的加权均值和协方差计算预测均值和协方差。
[0040]进一步的是，步骤6的具体步骤如下：
[0041]步骤6.1：准备；假设给出了状态估计值和误差协方差矩阵
[0042]步骤6.2：预测；对每个sigma点通过导航平台状态向量进行转换，以产生一组新样本，并更新预测的均值和协方本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种轨迹跟踪定位滤波系统，其特征在于：所述轨迹跟踪定位滤波系统由轨迹跟踪模块和自动跟踪返回模块组成，轨迹跟踪模块和自动跟踪返回模块均固定安装在救援无人机导航平台上，轨迹跟踪模块与自动返回模块信号连接，自动返回模块与救援无人机导航平台信号连接；轨迹跟踪模块，用于利用互补算法计算惯性导航系统和北斗组合系统的系统状态向量；利用北斗接收机的测量，构建融合测量位置向量，并能够测量出对应北斗接收机的速度误差和位置误差；基于无迹卡尔曼滤波的滤波方法，使用无迹变换技术来近似非线性系统状态的概率分布，预测均值和协方差；自动跟踪返回模块，用于存储轨迹跟踪模块传输的轨迹数据，当信号中断，外部人员无法控制时，所述导航平台开始读取轨迹数据，实现导航平台自动沿原路径轨迹返回。2.一种基于权利要求1所述的轨迹跟踪定位滤波系统实现救援无人机的融合导航方法，所述方法包括以下步骤：步骤1：计算陀螺仪的旋转角度σ；根据陀螺仪的科里奥利力原理，输出陀螺仪的角速度ω(t)，进而得到陀螺仪的旋转角度σ，如式(2)所示；σ＝∫ω(t)dt
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)步骤2：计算导航平台的旋转角度；采集一定时间间隔的导航平台的旋转角度，得到平面加速度方向，并结合磁强计，实时补偿修正方向，导航平台的姿态角的初始值通过求解四元数旋转矩阵得到：其中，ψ0、θ0、γ0分别为导航平台的偏航角、俯仰角和滚转角的初始值；由导航平台的偏航角ψ0、俯仰角θ0和滚转角γ0的初始值构成导航平台的姿态角的初始值；其中，ε
b
＝(ε
x
,ε
y
,ε
z
)
T
为b坐标系中陀螺仪在x、y、z坐标方向上的持续漂移；为陀螺仪与持续漂移对应的白噪声在x、y、z坐标方向上的分量；ψ、θ、γ分别为导航平台的偏航角、俯仰角和滚转角，这些角组合构成导航平台的旋转角度；ω
gb
表示由g坐标系到b坐标系的角速度转换，ω
gb
在b坐标系中的投影为：
其中，分别为b坐标系中在x、y、z坐标方向上的值；ε
x
、ε
y
、ε
z
分别为b坐标系中陀螺仪在x、y、z坐标方向上的常值漂移；步骤3：速度和位置描述；惯性导航系统的速度和位置方程为：其中，为分别以纬度L、经度λ和高度h表示的位置；分别为x、y、z轴方向上的速度分量，分别为x、y、z轴方向上比力的度量，ω
ie
为地球自转角速度，g为重力加速度，R
M
和R
N
分别为子午线和主垂线方向的曲率半径，为加速度计产生的恒偏置误差在x、y、z轴方向的分量，为由b坐标系到g坐标系的姿态矩阵，w为高斯白噪声，陀螺仪的常值漂移ε和加速度计的恒偏置误差置零；步骤4：定义惯性导航系统和北斗组合系统的系统状态向量为：根据惯性导航系统和北斗组合系统的状态，得到上述两个...

【专利技术属性】
技术研发人员：张清江，赵龙，陶成钢，曾凡玲，
申请(专利权)人：哈尔滨天枢问道技术有限公司，
类型：发明
国别省市：