CN116839591B - 一种轨迹跟踪定位滤波***及救援无人机的融合导航方法 - Google Patents

一种轨迹跟踪定位滤波***及救援无人机的融合导航方法 Download PDF

Info

Publication number
CN116839591B
CN116839591B CN202310853754.0A CN202310853754A CN116839591B CN 116839591 B CN116839591 B CN 116839591B CN 202310853754 A CN202310853754 A CN 202310853754A CN 116839591 B CN116839591 B CN 116839591B
Authority
CN
China
Prior art keywords
navigation
unmanned aerial
vector
measurement
aerial vehicle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202310853754.0A
Other languages
English (en)
Other versions
CN116839591A (zh
Inventor
张清江
赵龙
陶成钢
曾凡玲
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Tianshu Technology Co ltd
Original Assignee
Harbin Tianshu Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Tianshu Technology Co ltd filed Critical Harbin Tianshu Technology Co ltd
Priority to CN202310853754.0A priority Critical patent/CN116839591B/zh
Publication of CN116839591A publication Critical patent/CN116839591A/zh
Application granted granted Critical
Publication of CN116839591B publication Critical patent/CN116839591B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)

Abstract

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

Description

一种轨迹跟踪定位滤波***及救援无人机的融合导航方法
技术领域
本发明属于救援无人机导航领域，具体涉及一种轨迹跟踪定位滤波***及救援无人机的融合导航方法。
背景技术
危险事故常常出乎意料地发生，在不熟悉地形和未知地形危险因素的情况下进行搜救往往会浪费大量的时间。现有救援平台存在各种问题，如履带式救援平台，是在传统轮式机器人的基础上发展起来的，来满足军事侦察、拆除危险物体等需要。它的移动速度很快，但是当面对恶劣的环境时，由于它的尺寸比较大，进入狭窄的区域十分不方便，会导致救援进程中断。在空间方面，大部分救援平台的设计研究都是基于卫星定位进行轨迹模拟，在真实的环境中，卫星导航具有一定的局限性。例如，在建筑物、隧道、洞穴等封闭环境中，信号衰减非常严重，所以在大多数情况下，无法提供准确的定位信息。惯性导航***目前用于自主导航定位，其精度较差，因此提出利用惯性导航***结合北斗导航***构建通信降级条件下的救援无人机的融合导航方法。
发明内容
本发明的目的是为解决背景技术中存在的上述问题，提供一种轨迹跟踪定位滤波***及救援无人机的融合导航方法。
实现上述目的，本发明采取的技术方案如下：
一种轨迹跟踪定位滤波***，所述轨迹跟踪定位滤波***由轨迹跟踪模块和自动跟踪返回模块组成，轨迹跟踪模块和自动跟踪返回模块均固定安装在救援无人机导航平台上，轨迹跟踪模块与自动返回模块信号连接，自动返回模块与救援无人机导航平台信号连接；
轨迹跟踪模块，用于利用互补算法计算惯性导航***(INS)和北斗组合***的***状态向量；利用北斗接收机的测量，构建融合测量位置向量，并能够测量出对应北斗接收机的速度误差和位置误差；基于无迹卡尔曼滤波的滤波方法，使用无迹变换技术来近似非线性***状态的概率分布，预测均值和协方差；
自动跟踪返回模块，用于存储轨迹跟踪模块传输的轨迹数据，当信号中断，外部人员无法控制时，所述导航平台开始读取轨迹数据，实现导航平台自动沿原路径轨迹返回。
一种救援无人机的融合导航方法，所述方法包括以下步骤：
步骤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：速度和位置描述；惯性导航***(INS)的速度和位置方程为：
其中，为分别以纬度L、经度λ和高度h表示的位置；
分别为x、y、z轴方向上的速度分量，/>分别为x、y、z轴方向上比力的度量，ωie为地球自转角速度，g为重力加速度，RM和RN分别为子午线和主垂线方向的曲率半径，为加速度计产生的恒偏置误差在x、y、z轴方向的分量，/>为由b坐标系到g坐标系的姿态矩阵，w为高斯白噪声，陀螺仪的常值漂移ε和加速度计的恒偏置误差/>置零；
步骤4：定义惯性导航***(INS)和北斗组合***的***状态向量为：
根据惯性导航***(INS)和北斗组合***的状态，得到上述两个***的的组合状态向量：
其中，f(·)为一个非线性函数，G为过程白噪声矢量的分布矩阵，u为附加10％建模不确定性的集合；利用欧拉离散化公式将上式离散化，得到上述两个***的组合状态的离散向量；
xk＝f(xk-1)+Gu；
其中，xk为x(t)函数在k时刻的状态，xk-1为在x(t)函数上一时刻的状态；
步骤5：利用北斗接收机的测量，构建融合测量位置向量；采用北斗接收机在速度和位置方面的高精度输出，得到要测量的位置向量zk；
zk＝[vx,vy,vz,L,λ,h]T；
基于已确定的***状态向量和测量向量，构建惯性导航***(INS)和北斗组合***的融合测量向量zk；
其中，vv和vp是测量噪声，分别对应北斗接收机的速度误差和位置误差；Hk为矩阵，Hk＝[06×3,I6×6,06×6]；xk为x(t)函数在k时刻的状态；
由于所提出的INS/北斗是一个闭环***，滤波器估计的导航参数将用于补偿惯性导航***的惯性测量单元(IMU)的漂移，从而能够得到更精确的惯性导航***(INS)和北斗组合***的组合导航解；
步骤6：基于无迹卡尔曼滤波的滤波方法，使用无迹变换技术来近似非线性***状态的概率分布；首先，从状态的先验均值和协方差中确定地选择一组极小的sigma点；然后，将无迹变换技术依次应用于这些点以产生一个变换样本，并利用变换样本的加权均值和协方差计算预测均值和协方差。
进一步的是，步骤6的具体步骤如下：
步骤6.1：准备；假设给出了状态估计值和误差协方差矩阵/>
步骤6.2：预测；对每个sigma点通过导航平台状态向量进行转换，以产生一组新样本，并更新预测的均值和协方差；
ξi,k/k-1＝f(ξi,k-1),i＝0,1,…,2n
其中，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时刻的角速度；为协方差；Qk为白噪声的协方差；a为调谐参数，是常数；
步骤6.3：更新；由于测量向量是线性的，因此采用与传统卡尔曼滤波相同的方程进行测量更新；
步骤6.4：传输与存储；将测量向量作为状态向量传输到自动跟踪返回模块中。
与现有技术相比，本发明的有益效果是：惯性导航***(INS)是目前导航领域的一种重要定位方法。它具有自主性高、全参数导航、短期导航精度高等优点。基于此，本发明设计了一种基于惯性导航的轨迹跟踪定位滤波***，用于获取无人机的运动路径。轨迹跟踪定位滤波***安装在无人机导航平台上，跟踪和记录无人机在搜救中的路径，为工作人员提供简单的危险环境和状况定位，为进一步的搜救工作做好预期的准备。本发明的方法结合了惯性导航***的自主性好和卫星导航***的精度高等优点，具有较高的短期精度和稳定性。
附图说明
图1是航位推算算法示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是发明的一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
具体实施方式一：本实施方式披露了一种轨迹跟踪定位滤波***，所述轨迹跟踪定位滤波***由轨迹跟踪模块和自动跟踪返回模块组成，轨迹跟踪模块和自动跟踪返回模块均固定安装在救援无人机导航平台上，轨迹跟踪模块与自动返回模块信号连接，自动返回模块与救援无人机导航平台信号连接；
轨迹跟踪模块，用于利用互补算法计算惯性导航***(INS)和北斗组合***的***状态向量(在方法中的步骤4体现)；利用北斗接收机的测量，构建融合测量位置向量，并能够测量出对应北斗接收机的速度误差和位置误差(在方法的步骤5体现)；基于无迹卡尔曼滤波的滤波方法，使用无迹变换技术来近似非线性***状态的概率分布，预测均值和协方差(在方法的步骤6体现)；
自动跟踪返回模块，用于存储轨迹跟踪模块传输的轨迹数据，当信号中断，外部人员无法控制时，所述导航平台开始读取轨迹数据(姿态角、坐标值)，实现导航平台自动沿原路径轨迹返回。
具体实施方式二：本实施方式披露了一种基于具体实施方式一所述的轨迹跟踪定位滤波***实现救援无人机的融合导航方法，所述方法包括以下步骤：
步骤1：计算陀螺仪的旋转角度σ；根据陀螺仪的科里奥利力原理，输出陀螺仪的角速度ω(t)，进而得到陀螺仪的旋转角度σ，如式(2)所示；
σ＝∫ω(t)dt (2)
步骤2：计算导航平台的旋转角度；采集一定时间间隔(通常为1秒)的导航平台的旋转角度，得到平面加速度方向，并结合磁强计，实时补偿修正方向，导航平台的姿态角的初始值通过求解四元数旋转矩阵得到：
其中，ψ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：速度和位置描述；惯性导航***(INS)的速度和位置方程为：
其中，为分别以纬度L、经度λ和高度h表示的位置；
分别为x、y、z轴方向上的速度分量，/>分别为x、y、z轴方向上比力的度量，ωie为地球自转角速度，g为重力加速度，RM和RN分别为子午线和主垂线方向的曲率半径，为加速度计产生的恒偏置误差在x、y、z轴方向的分量，/>为由b坐标系到g坐标系的姿态矩阵，w为高斯白噪声，陀螺仪的常值漂移ε和加速度计的恒偏置误差/>置零；
步骤4：定义惯性导航***(INS)和北斗组合***的***状态向量为：
根据惯性导航***(INS)和北斗组合***的状态，得到上述两个***的的组合状态向量：
其中，f(·)为一个非线性函数，G为过程白噪声矢量的分布矩阵，u为附加10％建模不确定性的集合；利用欧拉离散化公式将上式离散化，得到上述两个***的组合状态的离散向量；
xk＝f(xk-1)+Gu；
其中，xk为x(t)函数在k时刻的状态(也就是定位的对象在当前的位置)，xk-1为在x(t)函数上一时刻的状态(也是位置)；
步骤5：利用北斗接收机的测量，构建融合测量位置向量；采用北斗接收机在速度和位置方面的高精度输出，得到要测量的位置向量zk；
zk＝[vx,vy,vz,L,λ,h]T；
基于已确定的***状态向量和测量向量，构建惯性导航***(INS)和北斗组合***的融合测量向量zk；
其中，vv和vp是测量噪声，分别对应北斗接收机的速度误差和位置误差；Hk为矩阵，Hk＝[06×3,I6×6,06×6]；xk为x(t)函数在k时刻的状态；
由于所提出的INS/北斗是一个闭环***，滤波器估计的导航参数将用于补偿惯性导航***的惯性测量单元(IMU)的漂移，从而能够得到更精确的惯性导航***(INS)和北斗组合***的组合导航解；
步骤6：基于无迹卡尔曼滤波的滤波方法，使用无迹变换技术来近似非线性***状态的概率分布；首先，从状态(位置)的先验均值和协方差中确定地选择一组极小的sigma点(状态点)；然后，将无迹变换技术依次应用于这些点以产生一个变换样本，并利用变换样本的加权均值和协方差计算预测均值和协方差。
具体实施方式三：本实施方式是对具体实施方式二作出的进一步说明，步骤6的具体步骤如下：
步骤6.1：准备；假设给出了状态估计值和误差协方差矩阵/>
步骤6.2：预测；对每个sigma点通过导航平台状态向量进行转换，以产生一组新样本，并更新预测的均值和协方差；
ξi,kk-1＝f(ξi,k-1),i＝0,1,…,2n
其中，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时刻的角速度；为协方差；Qk为白噪声的协方差；a为调谐参数，是常数；
步骤6.3：更新；由于测量向量是线性的，因此采用与传统卡尔曼滤波相同的方程进行测量更新；
步骤6.4：传输与存储；将测量向量作为状态向量传输到自动跟踪返回模块中。
惯性导航本质上是一种航位推算***，如图1所示为航位推算算法示意图。救援无人机导航平台采用被动惯性导航，在惯性参考***中采集遥控平台的惯性元件原始数据(偏航角、俯仰角和滚转角的初始值)，通过数据融合和姿态计算，计算出导航坐标系中的无人机速度、姿态角和偏航角信息。根据惯性导航***的工作原理，建立各姿态角和位置的数学模型。
图1中，x0和y0是节点的初始位置，△01和△02是推测位置，S0、S1和S2为推测航迹。
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的装体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同条件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。
此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (1)

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为重力加速度，RM和RN分别为子午线和主垂线方向的曲率半径，/>为加速度计产生的恒偏置误差在x、y、z轴方向的分量，/>为由b坐标系到g坐标系的姿态矩阵，w为高斯白噪声，陀螺仪的持续漂移ε和加速度计的恒偏置误差▽置零；
步骤4：定义惯性导航***和北斗组合***的***状态向量为：
根据惯性导航***和北斗组合***的状态，得到上述两个***的组合状态向量：
其中，f(·)为一个非线性函数，G为过程白噪声矢量的分布矩阵，u为附加10％建模不确定性的集合；利用欧拉离散化公式将上式离散化，得到上述两个***的组合状态的离散向量；
xk＝f(xk-1)+Gu；
其中，xk为x(t)函数在k时刻的状态，xk-1为在x(t)函数上一时刻的状态；
步骤5：利用北斗接收机的测量，构建融合测量位置向量；采用北斗接收机在速度和位置方面的高精度输出，得到要测量的位置向量zk；
zk＝[vx,vy,vz,L,λ,h]T；
基于已确定的***状态向量和测量向量，构建惯性导航***和北斗组合***的融合测量向量zk；
其中，vv和vp是测量噪声，分别对应北斗接收机的速度误差和位置误差；Hk为矩阵，Hk＝[06×3,I6×6,06×6]；xk为x(t)函数在k时刻的状态；
步骤6：基于无迹卡尔曼滤波的滤波方法，使用无迹变换技术来近似非线性***状态的概率分布；首先，从状态的先验均值和协方差中确定地选择一组极小的sigma点；然后，将无迹变换技术依次应用于这些点以产生一个变换样本，并利用变换样本的加权均值和协方差计算预测均值和协方差；
步骤6的具体步骤如下：
步骤6.1：准备；假设给出了状态估计值和误差协方差矩阵/>
步骤6.2：预测；对每个sigma点通过导航平台状态向量进行转换，以产生一组新样本，并更新预测的均值和协方差；
ξi,k/k-1＝g(ξi,k-1),i＝0,1,L,2n
其中，n为采样的样本数；
其中，
ξi,k-1为通过x(t)的k-1个采样值估计到的任意时刻的x(t)值；
ξi,k/k-1＝g(ξi,k-1)，ξi,k/k-1为基于前k-1个样本产生的新的样本值；
为均值；
g(·)为某一非线性函数；
ωi为在i时刻的角速度；为协方差；Qk为白噪声的协方差；a为调谐参数，是常数；
步骤6.3：更新；由于测量向量是线性的，因此采用与传统卡尔曼滤波相同的方程进行测量更新；
步骤6.4：传输与存储；将测量向量作为状态向量传输到自动跟踪返回模块中。
CN202310853754.0A 2023-07-12 2023-07-12 一种轨迹跟踪定位滤波***及救援无人机的融合导航方法 Active CN116839591B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310853754.0A CN116839591B (zh) 2023-07-12 2023-07-12 一种轨迹跟踪定位滤波***及救援无人机的融合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310853754.0A CN116839591B (zh) 2023-07-12 2023-07-12 一种轨迹跟踪定位滤波***及救援无人机的融合导航方法

Publications (2)

Publication Number Publication Date
CN116839591A CN116839591A (zh) 2023-10-03
CN116839591B true CN116839591B (zh) 2024-05-28

Family

ID=88164975

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310853754.0A Active CN116839591B (zh) 2023-07-12 2023-07-12 一种轨迹跟踪定位滤波***及救援无人机的融合导航方法

Country Status (1)

Country Link
CN (1) CN116839591B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117346794B (zh) * 2023-12-05 2024-02-23 山东省科学院海洋仪器仪表研究所 一种用于浒苔跟踪的无人船组合导航***及导航方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106291645A (zh) * 2016-07-19 2017-01-04 东南大学 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
CN107610157A (zh) * 2016-07-12 2018-01-19 深圳雷柏科技股份有限公司 一种无人机目标追踪方法及***
CN107831776A (zh) * 2017-09-14 2018-03-23 湖南优象科技有限公司 基于九轴惯性传感器的无人机自主返航方法
FR3070773A1 (fr) * 2017-09-05 2019-03-08 Thales Systeme de suivi d'un drone, procede et programme d'ordinateur associes
CN111722257A (zh) * 2020-06-29 2020-09-29 河南工业大学 一种基于gps和ins组合制导的远程救援机器人***
CN112630813A (zh) * 2020-11-24 2021-04-09 中国人民解放军国防科技大学 基于捷联惯导和北斗卫星导航***的无人机姿态测量方法
WO2021212462A1 (zh) * 2020-04-24 2021-10-28 深圳市大疆创新科技有限公司 移动控制方法、移动装置及移动平台
CN115540860A (zh) * 2022-09-26 2022-12-30 福建（泉州）哈工大工程技术研究院 一种多传感器融合位姿估计算法
CN116203600A (zh) * 2023-02-22 2023-06-02 中急管(北京)网络科技有限公司 一种无人机通信信号丢失后带动力运动轨迹循迹的方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11500056B2 (en) * 2015-07-17 2022-11-15 Origin Wireless, Inc. Method, apparatus, and system for wireless tracking with graph-based particle filtering
CN110487301B (zh) * 2019-09-18 2021-07-06 哈尔滨工程大学 一种雷达辅助机载捷联惯性导航***初始对准方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107610157A (zh) * 2016-07-12 2018-01-19 深圳雷柏科技股份有限公司 一种无人机目标追踪方法及***
CN106291645A (zh) * 2016-07-19 2017-01-04 东南大学 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
FR3070773A1 (fr) * 2017-09-05 2019-03-08 Thales Systeme de suivi d'un drone, procede et programme d'ordinateur associes
CN107831776A (zh) * 2017-09-14 2018-03-23 湖南优象科技有限公司 基于九轴惯性传感器的无人机自主返航方法
WO2021212462A1 (zh) * 2020-04-24 2021-10-28 深圳市大疆创新科技有限公司 移动控制方法、移动装置及移动平台
CN111722257A (zh) * 2020-06-29 2020-09-29 河南工业大学 一种基于gps和ins组合制导的远程救援机器人***
CN112630813A (zh) * 2020-11-24 2021-04-09 中国人民解放军国防科技大学 基于捷联惯导和北斗卫星导航***的无人机姿态测量方法
CN115540860A (zh) * 2022-09-26 2022-12-30 福建（泉州）哈工大工程技术研究院 一种多传感器融合位姿估计算法
CN116203600A (zh) * 2023-02-22 2023-06-02 中急管(北京)网络科技有限公司 一种无人机通信信号丢失后带动力运动轨迹循迹的方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
Liu F,Sun X,Xiong Y,et al..Combination of iterated cubature Kalman filter and neural networks for GPS/INS during GPS outages.Rev Sci Instrum.2019,全文. *
Yinghong Ma ; Xunan Li ; Yi Jiao ; Lin Guo ; Suping Ren ; Qi Zhang.A Fast Multi-UAV Cooperative Reconnaissance Method Exploiting Payload Diversity. 2022 IEEE 96th Vehicular Technology Conference .2022,全文. *
四旋翼无人机轨迹稳定跟踪控制;李俊芳;李峰;吉月辉;高强;;控制与决策;20200229(第02期);全文 *
基于无迹卡尔曼滤波的农用无人机定位研究;李斌飞;崔世钢;施国英;祖林禄;;中国农机化学报;20200930(第09期);全文 *
基于机动目标模型的无人机视场跟踪仿真研究;李文超;袁冬莉;;计算机测量与控制;20110228(第02期);全文 *

Also Published As

Publication number Publication date
CN116839591A (zh) 2023-10-03

Similar Documents

Publication Publication Date Title
Bian et al. Inertial navigation
CN109813311B (zh) 一种无人机编队协同导航方法
Huang et al. Kalman-filtering-based in-motion coarse alignment for odometer-aided SINS
CN1322311C (zh) 车载快速定位定向***
Savage Strapdown inertial navigation integration algorithm design part 2: Velocity and position algorithms
CN104655131B (zh) 基于istssrckf的惯性导航初始对准方法
CN102519470B (zh) 多级嵌入式组合导航***及导航方法
CN105509739A (zh) 采用固定区间crts平滑的ins/uwb紧组合导航***及方法
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN105509769B (zh) 一种运载火箭捷联惯导全自主对准方法
CN116839591B (zh) 一种轨迹跟踪定位滤波***及救援无人机的融合导航方法
CN107576327A (zh) 基于可观测度分析的可变结构综合导航***设计方法
CN106979781A (zh) 基于分布式惯性网络的高精度传递对准方法
Liu et al. Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter
CN111964675A (zh) 一种黑障区的飞行器智能导航方法
CN111207773B (zh) 一种用于仿生偏振光导航的姿态无约束优化求解方法
Iqbal et al. Experimental results on an integrated GPS and multisensor system for land vehicle positioning
CN113503892A (zh) 一种基于里程计和回溯导航的惯导***动基座初始对准方法
Yang et al. Multilayer low-cost sensor local-global filtering fusion integrated navigation of small UAV
Al-Jlailaty et al. Efficient attitude estimators: A tutorial and survey
CN113375664B (zh) 基于点云地图动态加载的自主移动装置定位方法
Bose et al. Modern inertial sensors and systems
CN113008229A (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
RU2502049C1 (ru) Малогабаритная бесплатформенная инерциальная навигационная система средней точности, корректируемая от системы воздушных сигналов
CN115479605A (zh) 基于空间目标定向观测的高空长航时无人机自主导航方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant