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

1.本发明属于救援无人机导航领域，具体涉及一种轨迹跟踪定位滤波系统及救援无人机的融合导航方法。


背景技术：

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


技术实现要素：

3.本发明的目的是为解决背景技术中存在的上述问题，提供一种轨迹跟踪定位滤波系统及救援无人机的融合导航方法。
4.实现上述目的，本发明采取的技术方案如下：
5.一种轨迹跟踪定位滤波系统，所述轨迹跟踪定位滤波系统由轨迹跟踪模块和自动跟踪返回模块组成，轨迹跟踪模块和自动跟踪返回模块均固定安装在救援无人机导航平台上，轨迹跟踪模块与自动返回模块信号连接，自动返回模块与救援无人机导航平台信号连接；
6.轨迹跟踪模块，用于利用互补算法计算惯性导航系统(ins)和北斗组合系统的系统状态向量；利用北斗接收机的测量，构建融合测量位置向量，并能够测量出对应北斗接收机的速度误差和位置误差；基于无迹卡尔曼滤波的滤波方法，使用无迹变换技术来近似非线性系统状态的概率分布，预测均值和协方差；
7.自动跟踪返回模块，用于存储轨迹跟踪模块传输的轨迹数据，当信号中断，外部人员无法控制时，所述导航平台开始读取轨迹数据，实现导航平台自动沿原路径轨迹返回。
8.一种救援无人机的融合导航方法，所述方法包括以下步骤：
9.步骤1：计算陀螺仪的旋转角度σ；根据陀螺仪的科里奥利力原理，输出陀螺仪的角速度ω(t)，进而得到陀螺仪的旋转角度σ，如式(2)所示；
10.σ＝∫ω(t)dt
ꢀꢀꢀꢀꢀꢀꢀ
(2)
11.步骤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为重力加速度，rm和rn分别为子午线和主垂线方向的曲率半径，为加速度计产生的恒偏置误差在x、y、z轴方向的分量，为由b坐标系到g坐标系的姿态矩阵，w为高斯白噪声，陀螺仪的常值漂移ε和加速度计的恒偏置误差置零；
[0026]
步骤4：定义惯性导航系统(ins)和北斗组合系统的系统状态向量为：
[0027][0028]
根据惯性导航系统(ins)和北斗组合系统的状态，得到上述两个系统的的组合状态向量：
[0029][0030]
其中，f(
·
)为一个非线性函数，g为过程白噪声矢量的分布矩阵，u为附加10％建模不确定性的集合；利用欧拉离散化公式将上式离散化，得到上述两个系统的组合状态的离散向量；
[0031]
xk＝f(x
k-1
)+gu；
[0032]
其中，xk为x(t)函数在k时刻的状态，x
k-1
为在x(t)函数上一时刻的状态；
[0033]
步骤5：利用北斗接收机的测量，构建融合测量位置向量；采用北斗接收机在速度和位置方面的高精度输出，得到要测量的位置向量zk；
[0034]
zk＝[v
x
,vy,vz,l,λ,h]
t
；
[0035]
基于已确定的系统状态向量和测量向量，构建惯性导航系统(ins)和北斗组合系统的融合测量向量zk；
[0036][0037]
其中，vv和v
p
是测量噪声，分别对应北斗接收机的速度误差和位置误差；hk为矩阵，hk＝[06×3,i6×6,06×6]；xk为x(t)函数在k时刻的状态；
[0038]
由于所提出的ins/北斗是一个闭环系统，滤波器估计的导航参数将用于补偿惯性导航系统的惯性测量单元(imu)的漂移，从而能够得到更精确的惯性导航系统(ins)和北斗组合系统的组合导航解；
[0039]
步骤6：基于无迹卡尔曼滤波的滤波方法，使用无迹变换技术来近似非线性系统状态的概率分布；首先，从状态的先验均值和协方差中确定地选择一组极小的sigma点；然后，将无迹变换技术依次应用于这些点以产生一个变换样本，并利用变换样本的加权均值和协方差计算预测均值和协方差。
[0040]
进一步的是，步骤6的具体步骤如下：
[0041]
步骤6.1：准备；假设给出了状态估计值和误差协方差矩阵
[0042]
步骤6.2：预测；对每个sigma点通过导航平台状态向量进行转换，以产生一组新样本，并更新预测的均值和协方差；
[0043]
ξ
i,k/k-1
＝f(ξ
i,k-1
),i＝0,1,
…
,2n
[0044]
其中，n为采样的样本数；
[0045][0046][0047]
其中，
[0048]
ξ
i,k-1
为通过x(t)的k-1个采样值估计到的任意时刻的x(t)值；
[0049]
ξ
i,k/k-1
＝f(ξ
i,k-1
)，ξ
i,k/k-1
为基于前k-1个样本产生的新的样本值；
[0050][0050]
为均值；
[0051]
f(
·
)为某一非线性函数；
[0052]
ωi为在i时刻的角速度；为协方差；qk为白噪声的协方差；a为调谐参数，是常数；
[0053]
步骤6.3：更新；由于测量向量是线性的，因此采用与传统卡尔曼滤波相同的方程进行测量更新；
[0054]
步骤6.4：传输与存储；将测量向量作为状态向量传输到自动跟踪返回模块中。
[0055]
与现有技术相比，本发明的有益效果是：惯性导航系统(ins)是目前导航领域的一种重要定位方法。它具有自主性高、全参数导航、短期导航精度高等优点。基于此，本发明设计了一种基于惯性导航的轨迹跟踪定位滤波系统，用于获取无人机的运动路径。轨迹跟踪定位滤波系统安装在无人机导航平台上，跟踪和记录无人机在搜救中的路径，为工作人员提供简单的危险环境和状况定位，为进一步的搜救工作做好预期的准备。本发明的方法结合了惯性导航系统的自主性好和卫星导航系统的精度高等优点，具有较高的短期精度和稳定性。
附图说明
[0056]
图1是航位推算算法示意图。
具体实施方式
[0057]
下面将结合本发明实施例中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是发明的一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0058]
具体实施方式一：本实施方式披露了一种轨迹跟踪定位滤波系统，所述轨迹跟踪定位滤波系统由轨迹跟踪模块和自动跟踪返回模块组成，轨迹跟踪模块和自动跟踪返回模块均固定安装在救援无人机导航平台上，轨迹跟踪模块与自动返回模块信号连接，自动返回模块与救援无人机导航平台信号连接；
[0059]
轨迹跟踪模块，用于利用互补算法计算惯性导航系统(ins)和北斗组合系统的系统状态向量(在方法中的步骤4体现)；利用北斗接收机的测量，构建融合测量位置向量，并能够测量出对应北斗接收机的速度误差和位置误差(在方法的步骤5体现)；基于无迹卡尔曼滤波的滤波方法，使用无迹变换技术来近似非线性系统状态的概率分布，预测均值和协方差(在方法的步骤6体现)；
[0060]
自动跟踪返回模块，用于存储轨迹跟踪模块传输的轨迹数据，当信号中断，外部人员无法控制时，所述导航平台开始读取轨迹数据(姿态角、坐标值)，实现导航平台自动沿原路径轨迹返回。
[0061]
具体实施方式二：本实施方式披露了一种基于具体实施方式一所述的轨迹跟踪定位滤波系统实现救援无人机的融合导航方法，所述方法包括以下步骤：
[0062]
步骤1：计算陀螺仪的旋转角度σ；根据陀螺仪的科里奥利力原理，输出陀螺仪的角速度ω(t)，进而得到陀螺仪的旋转角度σ，如式(2)所示；
[0063]
σ＝∫ω(t)dt
ꢀꢀꢀꢀꢀꢀꢀ
(2)
[0064]
步骤2：计算导航平台的旋转角度；采集一定时间间隔(通常为1秒)的导航平台的旋转角度，得到平面加速度方向，并结合磁强计，实时补偿修正方向，导航平台的姿态角的初始值通过求解四元数旋转矩阵得到：
[0065][0066]
其中，ψ0、θ0、γ0分别为导航平台的偏航角、俯仰角和滚转角的初始值；
[0067]
由导航平台的偏航角ψ0、俯仰角θ0和滚转角γ0的初始值构成导航平台的姿态角的初始值；
[0068][0069]
其中，εb＝(ε
x
,εy,εz)
t
为b坐标系中陀螺仪在x、y、z坐标方向上的持续漂移；
[0070]
为陀螺仪与持续漂移对应的白噪声在x、y、z坐标方向上的分量；
[0071]
ψ、θ、γ分别为导航平台的偏航角、俯仰角和滚转角，这些角组合构成导航平台的旋转角度；
[0072]
ω
gb
表示由g坐标系(地理坐标系)到b坐标系(载体坐标系)的角速度转换，ω
gb
在b坐标系中的投影为：
[0073]
其中，分别为b坐标系中在x、y、z坐标方向上的值；
[0074]
ε
x
、εy、εz分别为b坐标系中陀螺仪在x、y、z坐标方向上的常值漂移；
[0075]
步骤3：速度和位置描述；惯性导航系统(ins)的速度和位置方程为：
[0076][0077]
其中，为分别以纬度l、经度λ和高度h表示的位置；
[0078]
分别为x、y、z轴方向上的速度分量，分别为x、y、z轴方向上比力的度量，ω
ie
为地球自转角速度，g为重力加速度，rm和rn分别为子午线和主垂线方向的曲率半径，
为加速度计产生的恒偏置误差在x、y、z轴方向的分量，为由b坐标系到g坐标系的姿态矩阵，w为高斯白噪声，陀螺仪的常值漂移ε和加速度计的恒偏置误差置零；
[0079]
步骤4：定义惯性导航系统(ins)和北斗组合系统的系统状态向量为：
[0080][0081]
根据惯性导航系统(ins)和北斗组合系统的状态，得到上述两个系统的的组合状态向量：
[0082][0083]
其中，f(
·
)为一个非线性函数，g为过程白噪声矢量的分布矩阵，u为附加10％建模不确定性的集合；利用欧拉离散化公式将上式离散化，得到上述两个系统的组合状态的离散向量；
[0084]
xk＝f(x
k-1
)+gu；
[0085]
其中，xk为x(t)函数在k时刻的状态(也就是定位的对象在当前的位置)，x
k-1为在x(t)函数上一时刻的状态(也是位置)；
[0086]
步骤5：利用北斗接收机的测量，构建融合测量位置向量；采用北斗接收机在速度和位置方面的高精度输出，得到要测量的位置向量zk；
[0087]
zk＝[v
x
,vy,vz,l,λ,h]
t
；
[0088]
基于已确定的系统状态向量和测量向量，构建惯性导航系统(ins)和北斗组合系统的融合测量向量zk；
[0089][0090]
其中，vv和v
p
是测量噪声，分别对应北斗接收机的速度误差和位置误差；hk为矩阵，hk＝[06×3,i6×6,06×6]；xk为x(t)函数在k时刻的状态；
[0091]
由于所提出的ins/北斗是一个闭环系统，滤波器估计的导航参数将用于补偿惯性导航系统的惯性测量单元(imu)的漂移，从而能够得到更精确的惯性导航系统(ins)和北斗组合系统的组合导航解；
[0092]
步骤6：基于无迹卡尔曼滤波的滤波方法，使用无迹变换技术来近似非线性系统状态的概率分布；首先，从状态(位置)的先验均值和协方差中确定地选择一组极小的sigma点(状态点)；然后，将无迹变换技术依次应用于这些点以产生一个变换样本，并利用变换样本的加权均值和协方差计算预测均值和协方差。
[0093]
具体实施方式三：本实施方式是对具体实施方式二作出的进一步说明，步骤6的具体步骤如下：
[0094]
步骤6.1：准备；假设给出了状态估计值和误差协方差矩阵
[0095]
步骤6.2：预测；对每个sigma点通过导航平台状态向量进行转换，以产生一组新样本，并更新预测的均值和协方差；
[0096]
ξ
i,kk-1
＝f(ξ
i,k-1
),i＝0,1,
…
,2n
[0097]
其中，n为采样的样本数；
[0098][0099][0100]
其中，
[0101]
ξ
i,k-1
为通过x(t)的k-1个采样值估计到的任意时刻的x(t)值；
[0102]
ξ
i,k/k-1
＝f(ξ
i,k-1
)，ξ
i,k/k-1
为基于前k-1个样本产生的新的样本值；
[0103][0103]
为均值；
[0104]
f(
·
)为某一非线性函数；
[0105]
ωi为在i时刻的角速度；为协方差；qk为白噪声的协方差；a为调谐参数，是常数；
[0106]
步骤6.3：更新；由于测量向量是线性的，因此采用与传统卡尔曼滤波相同的方程进行测量更新；
[0107]
步骤6.4：传输与存储；将测量向量作为状态向量传输到自动跟踪返回模块中。
[0108]
惯性导航本质上是一种航位推算系统，如图1所示为航位推算算法示意图。救援无人机导航平台采用被动惯性导航，在惯性参考系统中采集遥控平台的惯性元件原始数据(偏航角、俯仰角和滚转角的初始值)，通过数据融合和姿态计算，计算出导航坐标系中的无人机速度、姿态角和偏航角信息。根据惯性导航系统的工作原理，建立各姿态角和位置的数学模型。
[0109]
图1中，x0和y0是节点的初始位置，
△
01和
△
02是推测位置，s0、s1和s2为推测航迹。
[0110]
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的装体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同条件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。
[0111]
此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

技术特征：
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：定义惯性导航系统和北斗组合系统的系统状态向量为：根据惯性导航系统和北斗组合系统的状态，得到上述两个系统的的组合状态向量：其中，f(
·
)为一个非线性函数，g为过程白噪声矢量的分布矩阵，u为附加10％建模不
确定性的集合；利用欧拉离散化公式将上式离散化，得到上述两个系统的组合状态的离散向量；x
k
＝f(x
k-1
)+gu；其中，x
k
为x(t)函数在k时刻的状态，x
k-1
为在x(t)函数上一时刻的状态；步骤5：利用北斗接收机的测量，构建融合测量位置向量；采用北斗接收机在速度和位置方面的高精度输出，得到要测量的位置向量z
k
；z
k
＝[v
x
,v
y
,v
z
,l,λ,h]
t
；基于已确定的系统状态向量和测量向量，构建惯性导航系统和北斗组合系统的融合测量向量z
k
；其中，v
v
和v
p
是测量噪声，分别对应北斗接收机的速度误差和位置误差；h
k
为矩阵，h
k
＝[06×3,i6×6,06×6]；x
k
为x(t)函数在k时刻的状态；步骤6：基于无迹卡尔曼滤波的滤波方法，使用无迹变换技术来近似非线性系统状态的概率分布；首先，从状态的先验均值和协方差中确定地选择一组极小的sigma点；然后，将无迹变换技术依次应用于这些点以产生一个变换样本，并利用变换样本的加权均值和协方差计算预测均值和协方差。3.根据权利要求2所述的救援无人机的融合导航方法，其特征在于：步骤6的具体步骤如下：步骤6.1：准备；假设给出了状态估计值和误差协方差矩阵步骤6.2：预测；对每个sigma点通过导航平台状态向量进行转换，以产生一组新样本，并更新预测的均值和协方差；ξ
i,k/k-1
＝f(ξ
i,k-1
),i＝0,1,
…
,2n其中，n为采样的样本数；其中，n为采样的样本数；其中，ξ
i,k-1
为通过x(t)的k-1个采样值估计到的任意时刻的x(t)值；ξ
i,k/k-1
＝f(ξ
i,k-1
)，ξ
i,k/k-1
为基于前k-1个样本产生的新的样本值；为均值；f(
·
)为某一非线性函数；ω
i
为在i时刻的角速度；为协方差；q
k
为白噪声的协方差；a为调谐参数，是常数；
步骤6.3：更新；由于测量向量是线性的，因此采用与传统卡尔曼滤波相同的方程进行测量更新；步骤6.4：传输与存储；将测量向量作为状态向量传输到自动跟踪返回模块中。

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


技术研发人员：张清江 赵龙 陶成钢 曾凡玲
受保护的技术使用者：哈尔滨天枢问道技术有限公司
技术研发日：2023.07.12
技术公布日：2023/10/8