CN109188352B - 一种组合导航相对定位方法 - Google Patents

一种组合导航相对定位方法 Download PDF

Info

Publication number
CN109188352B
CN109188352B CN201811042886.0A CN201811042886A CN109188352B CN 109188352 B CN109188352 B CN 109188352B CN 201811042886 A CN201811042886 A CN 201811042886A CN 109188352 B CN109188352 B CN 109188352B
Authority
CN
China
Prior art keywords
error
positioning
matrix
time
person
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
CN201811042886.0A
Other languages
English (en)
Other versions
CN109188352A (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.)
Southeast University
Original Assignee
Southeast University
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 Southeast University filed Critical Southeast University
Priority to CN201811042886.0A priority Critical patent/CN109188352B/zh
Publication of CN109188352A publication Critical patent/CN109188352A/zh
Application granted granted Critical
Publication of CN109188352B publication Critical patent/CN109188352B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
    • 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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

本发明公开了一种组合导航相对定位方法，该方法包括以下步骤：(1)结合TOA测距技术，建立导航相对定位估计模型；(2)考虑测距模型的误差，将坐标估计模型转化为有约束非线性规划问题，利用外部罚函数法能够从非可行解出发逐步逼近可行域的特点，并结合Powell方法能够逼近局部最优解的特点作为最优化求解方法，用于求解目标函数最优解；(3)建立无线通讯和惯导组合导航数学模型，利用卡尔曼滤波融合得出精确相对坐标。

Description

一种组合导航相对定位方法
技术领域
本发明属于导航技术领域，尤其涉及一种组合导航相对定位方法。
背景技术
在地震救援，反恐作战，野外搜救等突发事件的复杂环境下，行动人员常常需要确定自己的当前位置，并把定位信息及时传达给同伴及上级指挥者，以利于行动人员掌握自身位置状态，保障安全，也有利于指挥者全面了解所有人员的姿态，统筹下一步的行动计划。
传统的通过地图、指南针，加以观察地图环境的定位方法不那么准确，在山区、丛林、楼宇、洞穴等复杂作战环境中，搜寻定位GPS模块往往也会因为没有信号无法发挥定位作用。解决方案从单纯的捷联惯导系统导航发展到捷联惯导系统加地磁信息系统，替代GPS组合的人员自主定位，但其定位不准确，所以目前需要提出一种定位方法，满足在山区、丛林、楼宇、洞穴等复杂作战环境中搜寻定位需要，为新型搜寻定位技术提供储备。
发明内容
发明目的：针对以上现有技术存在的问题，本发明提出一种组合导航相对定位方法，该方法可以在GNSS拒止条件下为单兵作战人员提供连续自主定位导航信息，满足在山区、丛林、楼宇、洞穴等复杂作战环境中搜寻定位需要，为新型搜寻定位技术提供储备。
技术方案：为实现本发明的目的，本发明所采用的技术方案是：一种组合导航相对定位方法，该方法包括以下步骤：
(1)结合TOA测距技术，建立N个人员的测距矩阵；
(2)根据测距矩阵建立N个人员导航相对定位估计模型；
(3)根据定位估计模型构造增广目标函数，对约束进行非线性规划，求解增广目标函数最优解以获得N个人员无线通讯定位坐标；
(4)建立无线通讯和惯导定位组合导航模型，利用卡尔曼滤波融合得出N个人员定位坐标。
其中，步骤(1)中，结合TOA测距技术，建立N个人员的测距矩阵，方法如下：
设N个人员分布在二维坐标系下某个平面区域内，
Figure BDA0001792543070000021
表示第i个定位人员待估计坐标：
计算两两人员i，j之间距离，T1时刻，i向j发送询问指令，T2时刻j收到询问指令，T3为j向i发送应答指令时刻，T4为i收到应答指令的时刻，利用异步TOA原理可得距离为dij：
Figure BDA0001792543070000022
其中，C为光速；
建立测距矩阵：
Figure BDA0001792543070000023
其中，步骤(2)中，根据测距矩阵建立N个人员导航相对定位估计模型方法如下：
Figure BDA0001792543070000024
Figure BDA0001792543070000025
其中，
Figure BDA0001792543070000026
为N个定位人员待估计坐标，Δ为系统调整因子，εij为允许定位精度误差。
其中，步骤(3)中，根据定位估计模型构造增广目标函数，方法如下：
Figure BDA0001792543070000027
其中，γij为罚因子，Gij为最大值函数，Gij＝max(0,gij)，gij为式(3)中约束条件。
进一步，步骤(3)中，求解目标函数最优解以获得N个人员无线通讯定位坐标，方法如下：调用随机函数产生N个人员的初始坐标记做P(0)，初始坐标值产生区间为(0,1)，将每组初始坐标点代入式(3)约束条件，判断是否满足约束条件，即gij是否成立，如果成立表明该初始值满足，则直接保留，待定位人员该时刻坐标为
Figure BDA0001792543070000031
如果不成立，则使用Powell方法进行求解。所述使用Powell方法进行求解，方法如下：
(3.1)定义初始非相关搜索方向分别为ξ1，ξ2，…ξ2N，其中，ξr为单位矩阵的第r行行向量；
(3.2)采用调用随机函数方式产生了N个人员初始坐标，记为P(0)，当r＝1时增加调整因子λr开始调整N个人员的第r个坐标值：
P(r)＝P(r-1)+λrξr
求出λr使该増广函数为最小，即：
F(P(r-1)+λrξr)＝minF(P(r-1)+λrξr) (5)
此过程即为调整第一个坐标值，之后递增r，继续调整下一个坐标值，直至r＝2N结束，其中，P(r)为第r次搜索所得到的N个坐标值；
计算整数q，且q<N；
使得Ω＝max(F(P(q-1))-F(P(q))) (6)
(3.3)计算并判断下式
Figure BDA0001792543070000032
如果式(7)成立，则将初始值更新P(0)＝P(2N)，重复步骤(3.2)继续调整坐标；若不成立，则令P(*)＝P(2N)，判断是否满足迭代结束条件|F(P(*))-F(P(0))|＜η，η为收敛精度，满足条件则将P(*)代入増广函数，即式(4)中计算出F(P(*))，判断是否满足约束条件gij，若不满足继续更新初始坐标P(0)＝P(2N)，重复步骤(3.2)；若满足则
Figure BDA0001792543070000033
从而得出利用无线通讯方式测距定位解算出的N个人员位置
Figure BDA0001792543070000034
在tk～tk-1时间内人员做匀速运动，则速度为
Figure BDA0001792543070000035
其中
Figure BDA0001792543070000036
为tk时刻无线通讯方式测距定位解算每个人员的速度，
Figure BDA0001792543070000037
为tk时刻无线通讯方式测距定位解算每个人员的坐标，
Figure BDA0001792543070000038
为tk-1时刻无线通讯方式测距定位解算每个人员的坐标。
其中，步骤(4)中，建立无线通讯和惯导定位组合导航模型，利用卡尔曼滤波融合得出精确相对坐标，方法如下：
(4.1)选取人员惯导系统导航误差作为卡尔曼滤波状态变量，即状态向量为：
Figure BDA0001792543070000041
其中，
Figure BDA0001792543070000042
为人员方向角误差，δvn为人员速度误差，δpn为人员位置误差，高斯白噪声
Figure BDA0001792543070000043
为陀螺仪误差，
Figure BDA0001792543070000044
为加速度计误差；
(4.2)建误差系统
Figure BDA0001792543070000045
其中，
Figure BDA0001792543070000046
为是人员失准角误差变化率，
Figure BDA0001792543070000047
为人员速度误差变化率，δvn为人员速度误差，
Figure BDA0001792543070000048
为人员位置误差变化率，
Figure BDA0001792543070000049
为人员的姿态矩阵，
Figure BDA00017925430700000410
为陀螺误差，fb为加速度计输出，δfb为加速度计误差；
(4.3)建立误差系统的连续时间状态方程：
Figure BDA00017925430700000411
其中，
Figure BDA00017925430700000412
表示状态向量的一阶微分，v(t)为高斯白噪声，f(t)为状态转移矩阵，g(t)为噪声输入矩阵；
Figure BDA00017925430700000413
(4.4)tk时刻惯导系统输出的速度为：
Figure BDA00017925430700000414
(4.5)tk时刻无线通讯方式测距定位解算的速度为：
Figure BDA0001792543070000051
其中，
Figure BDA0001792543070000052
表示tk时刻人员携带惯性系统输出的速度，
Figure BDA0001792543070000053
表示tk时刻人员实际速度，δVi n表示tk时刻惯导系统速度误差，
Figure BDA0001792543070000054
表示tk时刻无线通讯方式测距定位解算速度，
Figure BDA0001792543070000055
表示tk时刻无线通讯方式测距定位解算速度误差；
(4.6)建立速度观测方程：
ΔZv＝HΔX(t)+w(t) (10)
系统参数矩阵H＝[03×3 I3×3 03×3]，ΔZv为测量矩阵，w(t)，v(t)均为无关的高斯白噪声；
记待解算数据时长为T，将时间段0～T以采样周期Δt为间隔划分为多个时刻点t0,t1,t2...tm，k＝1,2,...,m，将式(9)、(10)离散化处理得：
Figure BDA0001792543070000056
其中，ΔXk为tk时刻人员的状态向量
Figure BDA0001792543070000057
Fk|k-1为tk-1到tk时刻转移矩阵，ΔXk-1为tk-1时刻人员状态向量，Gk|k-1为离散化噪声输入矩阵，ΔZk为tk时刻观测值
Figure BDA0001792543070000058
为tk时刻人员无线通讯方式测距定位解算速度，
Figure BDA0001792543070000059
为tk时刻携带惯性系统输出的速度，Wk-1和Vk均视为彼此不相关的零均值高斯白噪声序列，状态转移矩阵和离散化噪声输入矩阵：
Figure BDA00017925430700000510
Figure BDA00017925430700000511
对于(11)描述的线性离散线性系统，利用观测值ΔZk通过卡尔曼滤波融合方法对误差状态ΔXk进行估计，记初始状态向量为ΔX0＝(01×3，01×3，01×3)，P0＝I9×9为9阶单位矩阵
Figure BDA0001792543070000061
为陀螺仪误差，
Figure BDA0001792543070000062
为加速度计误差。
所述卡尔曼滤波融合方法如下：将初始状态向量代入式(12)，(13)进行计算获得tk时刻每个人员状态量估计值ΔXk：
时间更新
Figure BDA0001792543070000063
量测更新
Figure BDA0001792543070000064
其中，ΔXk|k-1为ΔXk的一步估计，I为单位矩阵，Hk为tk时刻H阵，Pk|k-1为估计值ΔXk|k-1的均方误差阵，Pk为ΔXk均方误差阵，Kk为卡尔曼滤波增益矩阵，系统噪声和观测噪声方差阵分别为
Figure BDA0001792543070000065
其中
Figure BDA0001792543070000066
为陀螺仪误差，
Figure BDA0001792543070000067
为加速度计误差，
Figure BDA0001792543070000068
为噪声误差。
进一步，利用卡尔曼滤波融合得出精确N个人员定位坐标，其方法如下：获得tk时刻每个人员状态量估计值ΔXk，取每个人员状态向量ΔXk中的位置误差部分
Figure BDA0001792543070000069
将N个人员的
Figure BDA00017925430700000610
合并为tk时刻全体人员的位置误差
Figure BDA00017925430700000611
作为状态反馈校正，结合tk时刻无线通讯方式测距定位解算获得的坐标位置
Figure BDA00017925430700000612
则N个人员tk时刻定位坐标:
Figure BDA00017925430700000613
有益效果：与现有技术相比，本发明的技术方案具有以下有益技术效果：
(1)利用无线通讯技术同惯导技术融合使得系统更加稳定；
(2)无线通讯中采用TOA技术测距具有较好的异步性和实用性；
(3)利用Powell函数、罚函数结合的方法使得非线性规划能够顺利解算；
(4)采用卡尔曼滤波融合方法提高了定位精度。
附图说明
图1为本发明使用的TOA测距示意图；
图2为本发明使用的方法流程图；
图3为本发明测距定位估算结果图；
图4为本发明融合定位结果图。
具体实施方式
下面结合附图和实施例对本发明的技术方案作进一步的说明。
针对TOA具有精度高、测距耗时少、传输距离远等特点，本发明采用TOA测距技术，建立导航相对坐标估计模型具体包括如下步骤：
有N个作战人员分布在二维坐标系下某个平面区域内，
Figure BDA0001792543070000071
表示第i个待定位人员在相对坐标系下的待估计位置。
如图1所示，为TOA测距示意图，两两人员i，j之间采用TOA技术相互测距，步骤如下，T1时刻，i向j发送询问指令，T2时刻j收到询问指令，T3为j向i发送应答指令时刻，T4为i收到应答指令的时刻，利用异步TOA原理可得：
距离为
Figure BDA0001792543070000072
Figure BDA0001792543070000073
其中，C为光速。
建立测距矩阵
Figure BDA0001792543070000074
确定待定位人员坐标估计模型：
Figure BDA0001792543070000075
Figure BDA0001792543070000076
其中，
Figure BDA0001792543070000077
为N个人员待估计坐标，Δ为系统调整因子，εij为允许定位精度误差，既在误差范围内，调整坐标值使得该目标函数即式(3)最小则为最优解。
考虑测距模型的误差，将坐标估计模型转化为有约束非线性规划问题，利用外部罚函数法“能够从非可行解出发逐步逼近可行域”的特点，并结合Powell方法能够“逼近局部最优解”的特点作为最优化求解方法，用于求解目标函数最优解，具体方法流程如图2所示。
由于当测距结果存在误差时，节点定位问题属于NP难问题，通常将基于测距的相对定位问题转化为非线性规划规划问题，调整坐标值使得该函数最小则为最优解。
利用问题的目标函数和约束函数构造出带参数的所谓增广目标函数，把约束非线形规划，由可行域外部逐渐逼近原约束优化问题的最优解，即原目标函数的最优解。
具体步骤如下：
1.设立増广函数
Figure BDA0001792543070000081
其中，γij为罚因子，Gij为最大值函数，Gij＝max(0,gij)，其中，gij为式(3)中约束条件。
2.调用随机函数设置初始坐标点
调用随机函数产生N个人员的初始坐标记做P(0)，初始坐标值产生区间为(0,1)，将每组初始坐标点，代入式(3)约束条件，判断是否满足约束条件，即gij是否成立，如果成立表明该初始值满足，则不需要处理直接保留，则待定位人员坐标
Figure BDA0001792543070000082
如果不成立，代入下面流程即Powell方法进行求解。
3.利用Powell方法进行非线性规划问题求解，得到最优解。
定义该方法初始非相关搜索方向分别为ξ1，ξ2，…ξ2N，其中，ξr为单位矩阵的第r行行向量，N为坐标数。
方法具体步骤如下：
步骤(1)上述采用调用随机函数方式产生了N个人员初始坐标，记为P(0)，当r＝1时增加调整因子λr开始调整N个人员的第r个坐标值：
即P(r)＝P(r-1)+λrξr
求出λr使该増广函数为最小，即：
F(P(r-1)+λrξr)＝minF(P(r-1)+λrξr) (5)
此过程即为调整第一个坐标值，之后递增r，继续调整下一个坐标值，直至r＝2N结束，每次只调整横坐标或纵坐标，其中，P(r)为第r次搜索所得到的N个坐标值。
计算整数q，且q<N；
使得Ω＝max(F(P(q-1))-F(P(q))) (6)
步骤(2)计算并判断
Figure BDA0001792543070000091
如果式(7)成立，则将初始值更新P(0)＝P(2N)重复步骤1继续调整坐标，若不成立，则令P(*)＝P(2N)，判断是否满足迭代结束条件|F(P(*))-F(P(0))|＜η，P(*)为迭代结果，η＝e-4为收敛精度。满足条件则将P(*)代入増广函数，即式(4)中计算出F(P(*))，判断是否满足约束条件gij，若不满足继续更新初始坐标P(0)＝P(2N)，重复步骤(1)，若满足则
Figure BDA0001792543070000092
从而得出人员的利用无线通讯方式测距定位解算出的N个人员位置
Figure BDA0001792543070000093
在tk～tk-1时间内可以看做匀速运动，则速度为
Figure BDA0001792543070000094
其中
Figure BDA0001792543070000095
为tk时刻无线通讯方式测距定位解算每个人员的速度，
Figure BDA0001792543070000096
为tk时间无线通讯方式测距定位解算每个人员的坐标，
Figure BDA0001792543070000097
为tk-1时间无线通讯方式测距定位解算每个人员的坐标，如图3所示为测距定位估算结果图。
建立无线通讯/惯导定位(CS/INS)组合导航数学模型，利用卡尔曼滤波融合得出精确相对坐标，具体方法包括：针对行人定位导航的特殊性，选取人员惯导系统导航误差作为卡尔曼滤波状态变量，即状态向量为
Figure BDA0001792543070000098
其中，
Figure BDA0001792543070000099
为人员方向角误差，δvn为人员速度误差，δpn为人员位置误差，系统噪声
Figure BDA00017925430700000910
其中，
Figure BDA00017925430700000911
为陀螺仪误差，
Figure BDA00017925430700000912
为加速度计误差。
相对于飞机、导弹、舰船等载体，行人速度低，行走范围有限，且MEMS惯性传感器的采样频率较高，可构建误差系统。
Figure BDA0001792543070000101
其中，
Figure BDA0001792543070000102
为是人员失准角误差变化率，
Figure BDA0001792543070000103
为人员速度误差变化率，δvn为人员速度误差，
Figure BDA0001792543070000104
为人员位置误差变化率，
Figure BDA0001792543070000105
为人员的姿态矩阵，
Figure BDA0001792543070000106
为陀螺误差，fb为加速度计输出，δfb为加速度计误差。
于是，误差系统的连续时间状态方程为：
Figure BDA0001792543070000107
其中，
Figure BDA0001792543070000108
表示状态向量的一阶微分，状态量
Figure BDA0001792543070000109
具体含义如上，v(t)为高斯白噪声，f(t)为状态转移矩阵，g(t)为噪声输入矩阵
Figure BDA00017925430700001010
由惯导系统输出的速度为：
Vi n＝Vn+δVi n
由无线通讯方式测距定位解算的速度为：
Figure BDA00017925430700001011
式中，Vi n分别表示人员携带惯性系统输出的速度，δVi n表示惯导系统速度误差，
Figure BDA00017925430700001012
表示无线通讯方式测距定位解算速度，δVw n表示无线通讯方式测距定位解算速度误差。
建立速度观测方程：
ΔZv＝HΔX(t)+w(t) (10)
参数矩阵H＝[03×3 I3×3 03×3]，ΔZv为测量矩阵，w(t)，v(t)均为无关的白噪声；
记待解算数据时长为T，将时间段0～T以采样周期Δt为间隔划分为多个时刻点t0,t1,t2...tm，k＝1,2,...,m。将式(9)、(10)离散化处理，得：
Figure BDA0001792543070000111
其中，ΔXk为tk时刻人员的状态向量
Figure BDA0001792543070000112
Fk|k-1为tk-1到tk时刻转移矩阵，ΔXk-1为tk-1时刻人员状态向量，Gk|k-1为离散化噪声输入矩阵，ΔZk为tk时刻观测值
Figure BDA0001792543070000113
为tk时刻无线通讯方式测距定位解算速度，
Figure BDA0001792543070000114
为tk时刻人员携带惯性系统输出的速度，H为观测矩阵，Wk-1和Vk均视为彼此不相关的零均值高斯白噪声序列，状态转移矩阵：
Figure BDA0001792543070000115
Figure BDA0001792543070000116
对于(11)描述的线性离散线性系统，可以利用观测值ΔZk通过卡尔曼滤波融合方法对误差状态ΔXk进行估计，记初始状态向量为ΔX0＝(01×3，01×3，01×3)，P0＝I9×9为9阶单位矩阵
Figure BDA0001792543070000117
为陀螺仪误差，
Figure BDA0001792543070000118
为加速度计误差，代入式(12)，(13)进行迭代：
时间更新
Figure BDA0001792543070000119
量测更新
Figure BDA00017925430700001110
其中，ΔXk|k-1为ΔXk的一步估计，I为单位矩阵，Hk为tk时刻H阵，Pk|k-1为估计值ΔXk|k-1的均方误差阵，Pk为ΔXk均方误差阵，Kk为卡尔曼滤波增益矩阵，系统噪声和观测噪声方差阵分别为
Figure BDA0001792543070000121
其中
Figure BDA0001792543070000122
为陀螺仪误差，
Figure BDA0001792543070000123
为加速度计误差，
Figure BDA0001792543070000124
为噪声误差。
因此获得tk时刻每个人员状态量估计值ΔXk，取人员状态向量ΔXk中的位置误差部分
Figure BDA0001792543070000125
将N个人员的
Figure BDA0001792543070000126
合并为tk时刻全体人员的位置误差
Figure BDA0001792543070000127
作为状态反馈校正，结合tk时刻无线通讯方式测距定位解算获得的坐标位置
Figure BDA0001792543070000128
则N个人员tk时刻准确的坐标值:
Figure BDA0001792543070000129
如图4所示为融合定位结果图。
如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。

Claims (5)

1.一种组合导航相对定位方法，其特征在于，该方法包括以下步骤：
(1)结合TOA测距技术，建立N个人员的测距矩阵；
(2)根据测距矩阵建立N个人员导航相对定位估计模型；
(3)根据定位估计模型构造增广目标函数，对约束进行非线性规划，求解增广目标函数最优解以获得N个人员无线通讯定位坐标；
(4)建立无线通讯和惯导定位组合导航模型，利用卡尔曼滤波融合得出N个人员定位坐标；
步骤(1)中，结合TOA测距技术，建立N个人员的测距矩阵，方法如下：
设N个人员分布在二维坐标系下平面区域内，
Figure FDA0003757598530000011
表示第i个定位人员待估计坐标：
计算两两人员i，j之间距离，T1时刻，i向j发送询问指令，T2时刻j收到询问指令，T3为j向i发送应答指令时刻，T4为i收到应答指令的时刻，利用异步TOA原理可得距离为dij：
Figure FDA0003757598530000012
其中，C为光速；
建立测距矩阵：
Figure FDA0003757598530000013
步骤(2)中，根据测距矩阵建立N个人员导航相对定位估计模型方法如下：
Figure FDA0003757598530000014
Figure FDA0003757598530000015
其中，
Figure FDA0003757598530000016
为N个定位人员待估计坐标，Δ为系统调整因子，εij为允许定位精度误差；
步骤(3)中，根据定位估计模型构造增广目标函数，方法如下：
Figure FDA0003757598530000021
其中，γij为罚因子，Gij为最大值函数，Gij＝max(0,gij)，gij为式(3)中约束条件；
步骤(3)中，求解目标函数最优解以获得N个人员无线通讯定位坐标，方法如下：调用随机函数产生N个人员的初始坐标记做P(0)，初始坐标值产生区间为(0,1)，将每组初始坐标点代入式(3)约束条件，判断是否满足约束条件，即gij是否成立，如果成立表明该初始值满足，则直接保留，待定位人员该时刻坐标为
Figure FDA0003757598530000022
如果不成立，则使用Powell方法进行求解。
2.根据权利要求1所述的一种组合导航相对定位方法，其特征在于，所述使用Powell方法进行求解，方法如下：
(3.1)定义初始非相关搜索方向分别为ξ1，ξ2，…ξ2N，其中，ξr为单位矩阵的第r行行向量；
(3.2)采用调用随机函数方式产生了N个人员初始坐标，记为P(0)，当r＝1时增加调整因子λr开始调整N个人员的第r个坐标值：
P(r)＝P(r-1)+λrξr
求出λr使该增广目标 函数为最小，即：
F(P(r-1)+λrξr)＝minF(P(r-1)+λrξr) (5)
此过程即为调整第一个坐标值，之后递增r，继续调整下一个坐标值，直至r＝2N结束，其中，P(r)为第r次搜索所得到的N个坐标值；
计算整数q，且q<N；
使得Ω＝max(F(P(q-1))-F(P(q))) (6)
(3.3)计算并判断下式
Figure FDA0003757598530000031
如果式(7)成立，则将初始值更新P(0)＝P(2N)，重复步骤(3.2)继续调整坐标；若不成立，则令P(*)＝P(2N)，判断是否满足迭代结束条件|F(P(*))-F(P(0))|＜η，η为收敛精度，满足条件则将P(*)代入增广函数，即式(4)中计算出F(P(*))，判断是否满足约束条件gij，若不满足继续更新初始坐标P(0)＝P(2N)，重复步骤(3.2)；若满足则
Figure FDA0003757598530000032
从而得出利用无线通讯方式测距定位解算出的N个人员位置
Figure FDA0003757598530000033
在tk～tk-1时间内人员做匀速运动，则速度为
Figure FDA0003757598530000034
其中
Figure FDA0003757598530000035
为tk时刻无线通讯方式测距定位解算每个人员的速度，
Figure FDA0003757598530000036
为tk时刻无线通讯方式测距定位解算每个人员的坐标，
Figure FDA0003757598530000037
为tk-1时刻无线通讯方式测距定位解算每个人员的坐标。
3.根据权利要求2所述的一种组合导航相对定位方法，其特征在于，步骤(4)中，建立无线通讯和惯导定位组合导航模型，利用卡尔曼滤波融合得出精确相对坐标，方法如下：
(4.1)选取人员惯导系统导航误差作为卡尔曼滤波状态变量，即状态向量为：
Figure FDA0003757598530000038
其中，
Figure FDA0003757598530000039
为人员方向角误差，δvn为人员速度误差，δpn为人员位置误差，高斯白噪声
Figure FDA00037575985300000310
Figure FDA00037575985300000311
为陀螺仪误差，
Figure FDA00037575985300000312
为加速度计误差；
(4.2)建误差系统
Figure FDA00037575985300000313
其中，
Figure FDA00037575985300000314
为是人员失准角误差变化率，
Figure FDA00037575985300000315
为人员速度误差变化率，δvn为人员速度误差，
Figure FDA00037575985300000316
为人员位置误差变化率，
Figure FDA00037575985300000317
为人员的姿态矩阵，
Figure FDA00037575985300000318
为陀螺误差，fb为加速度计输出，δfb为加速度计误差；
(4.3)建立误差系统的连续时间状态方程：
Figure FDA0003757598530000041
其中，
Figure FDA0003757598530000042
表示状态向量的一阶微分，v(t)为高斯白噪声，f(t)为状态转移矩阵，g(t)为噪声输入矩阵；
Figure FDA0003757598530000043
(4.4)tk时刻惯导系统输出的速度为：
Figure FDA0003757598530000044
(4.5)tk时刻无线通讯方式测距定位解算的速度为：
Figure FDA0003757598530000045
其中，
Figure FDA0003757598530000046
表示tk时刻人员携带惯性系统输出的速度，
Figure FDA0003757598530000047
表示tk时刻人员实际速度，δVi n表示tk时刻惯导系统速度误差，
Figure FDA0003757598530000048
表示tk时刻无线通讯方式测距定位解算速度，
Figure FDA0003757598530000049
表示tk时刻无线通讯方式测距定位解算速度误差；
(4.6)建立速度观测方程：
ΔZv＝HΔX(t)+w(t) (10)
系统参数矩阵H＝[03×3 I3×3 03×3]，ΔZv为测量矩阵，w(t)，v(t)均为无关的高斯白噪声；
记待解算数据时长为T，将时间段0～T以采样周期Δt为间隔划分为多个时刻点t0,t1,t2...tm，k＝1,2,...,m，将式(9)、(10)离散化处理得：
Figure FDA00037575985300000410
其中，ΔXk为tk时刻人员的状态向量
Figure FDA00037575985300000411
Fk|k-1为tk-1到tk时刻转移矩阵，ΔXk-1为tk-1时刻人员状态向量，Gk|k-1为离散化噪声输入矩阵，ΔZk为tk时刻观测值
Figure FDA0003757598530000051
Figure FDA0003757598530000052
为tk时刻人员无线通讯方式测距定位解算速度，
Figure FDA0003757598530000053
为tk时刻携带惯性系统输出的速度，Wk-1和Vk均视为彼此不相关的零均值高斯白噪声序列，状态转移矩阵和离散化噪声输入矩阵：
Figure FDA0003757598530000054
Figure FDA0003757598530000055
对于(11)描述的线性离散线性系统，利用观测值ΔZk通过卡尔曼滤波融合方法对误差状态ΔXk进行估计，记初始状态向量为ΔX0＝(01×3，01×3，01×3)，P0＝I9×9为9阶单位矩阵
Figure FDA0003757598530000056
Figure FDA0003757598530000057
为陀螺仪误差，
Figure FDA0003757598530000058
为加速度计误差。
4.根据权利要求3所述的一种组合导航相对定位方法，其特征在于，卡尔曼滤波融合方法如下：将初始状态向量代入式(12)，(13)进行计算获得tk时刻每个人员状态量估计值ΔXk：
时间更新
Figure FDA0003757598530000059
量测更新
Figure FDA00037575985300000510
其中，ΔXk|k-1为ΔXk的一步估计，I为单位矩阵，Hk为tk时刻H阵，Pk|k-1为估计值ΔXk|k-1的均方误差阵，Pk为ΔXk均方误差阵，Kk为卡尔曼滤波增益矩阵，系统噪声和观测噪声方差阵分别为
Figure FDA00037575985300000511
其中
Figure FDA00037575985300000512
为陀螺仪误差，
Figure FDA0003757598530000061
为加速度计误差，
Figure FDA0003757598530000062
为噪声误差。
5.根据权利要求4所述的一种组合导航相对定位方法，其特征在于，利用卡尔曼滤波融合得出精确N个人员定位坐标，其方法如下：获得tk时刻每个人员状态量估计值ΔXk，取每个人员状态向量ΔXk中的位置误差部分
Figure FDA0003757598530000063
将N个人员的
Figure FDA0003757598530000064
合并为tk时刻全体人员的位置误差
Figure FDA0003757598530000065
作为状态反馈校正，结合tk时刻无线通讯方式测距定位解算获得的坐标位置
Figure FDA0003757598530000066
则N个人员tk时刻定位坐标:
Figure FDA0003757598530000067
CN201811042886.0A 2018-09-07 2018-09-07 一种组合导航相对定位方法 Active CN109188352B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811042886.0A CN109188352B (zh) 2018-09-07 2018-09-07 一种组合导航相对定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811042886.0A CN109188352B (zh) 2018-09-07 2018-09-07 一种组合导航相对定位方法

Publications (2)

Publication Number Publication Date
CN109188352A CN109188352A (zh) 2019-01-11
CN109188352B true CN109188352B (zh) 2022-09-30

Family

ID=64915380

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811042886.0A Active CN109188352B (zh) 2018-09-07 2018-09-07 一种组合导航相对定位方法

Country Status (1)

Country Link
CN (1) CN109188352B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110376627A (zh) * 2019-07-24 2019-10-25 东南大学 一种复杂环境中自适应的绝对定位方法及定位系统
CN115016466A (zh) * 2022-05-17 2022-09-06 上海船舶运输科学研究所有限公司 一种目标定位误差最小的水面自主航行器最优路径规划方法
CN116182873B (zh) * 2023-05-04 2023-07-11 长沙驰芯半导体科技有限公司 室内定位方法、系统及计算机可读介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103207383A (zh) * 2013-05-16 2013-07-17 沈阳化工大学 基于单个移动节点对一静止节点进行二维无线定位的方法
CN103471595A (zh) * 2013-09-26 2013-12-25 东南大学 一种面向ins/wsn室内移动机器人紧组合导航的迭代扩展rts均值滤波方法
CN104076382A (zh) * 2014-07-22 2014-10-01 中国石油大学(华东) 一种基于多源信息融合的车辆无缝定位方法
CN104406605A (zh) * 2014-10-13 2015-03-11 中国电子科技集团公司第十研究所 机载多导航源综合导航仿真系统
CN207182640U (zh) * 2017-04-28 2018-04-03 华电郑州机械设计研究院有限公司 火电厂巡检人员定位监控系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103207383A (zh) * 2013-05-16 2013-07-17 沈阳化工大学 基于单个移动节点对一静止节点进行二维无线定位的方法
CN103471595A (zh) * 2013-09-26 2013-12-25 东南大学 一种面向ins/wsn室内移动机器人紧组合导航的迭代扩展rts均值滤波方法
CN104076382A (zh) * 2014-07-22 2014-10-01 中国石油大学(华东) 一种基于多源信息融合的车辆无缝定位方法
CN104406605A (zh) * 2014-10-13 2015-03-11 中国电子科技集团公司第十研究所 机载多导航源综合导航仿真系统
CN207182640U (zh) * 2017-04-28 2018-04-03 华电郑州机械设计研究院有限公司 火电厂巡检人员定位监控系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
高精度系统时钟下的相对定位算法研究;柯栋等;《舰船科学技术》;20100228;全文 *

Also Published As

Publication number Publication date
CN109188352A (zh) 2019-01-11

Similar Documents

Publication Publication Date Title
CN109459040B (zh) 基于rbf神经网络辅助容积卡尔曼滤波的多auv协同定位方法
CN109682375B (zh) 一种基于容错决策树的uwb辅助惯性定位方法
CN109188352B (zh) 一种组合导航相对定位方法
CN111189441A (zh) 一种多源自适应容错联邦滤波组合导航系统及导航方法
Song et al. Neural-network-based AUV navigation for fast-changing environments
CN110837854A (zh) 一种基于因子图的auv多源信息融合方法和设备
CN110208740A (zh) Tdoa-imu数据自适应融合定位装置及方法
CN113411744B (zh) 一种高精度的室内定位追踪方法
CN110346821A (zh) 一种解决gps长时间失锁问题的sins/gps组合定姿定位方法及系统
Goppert et al. Invariant Kalman filter application to optical flow based visual odometry for UAVs
Zhang et al. Cooperative navigation based on cross entropy: Dual leaders
Belhajem et al. A robust low cost approach for real time car positioning in a smart city using Extended Kalman Filter and evolutionary machine learning
CN114111802A (zh) 一种行人航迹推算辅助uwb的定位方法
CN113029173A (zh) 车辆导航方法及装置
CN116399335A (zh) 基于高斯信念传播的飞行器集群分布式协同定位方法
CN116839591A (zh) 一种轨迹跟踪定位滤波系统及救援无人机的融合导航方法
Malleswaran et al. A hybrid approach for GPS/INS integration using Kalman filter and IDNN
Kaygısız et al. Enhancing positioning accuracy of GPS/INS system during GPS outages utilizing artificial neural network
CN115143954A (zh) 一种基于多源信息融合的无人车导航方法
Xin et al. Stable positioning for mobile targets using distributed fusion correction strategy of heterogeneous data
Chiang et al. Rapid and accurate INS alignment for land applications
Tian et al. Novel hybrid of strong tracking Kalman filter and improved radial basis function neural network for GPS/INS integrated navagation
Chen et al. On SINS/GPS integrated navigation filtering method aided by radial basis function neural network
Li et al. Research on Strong Tracking UKF Algorithm of Integrated Navigation Based on BP Neural Network
CN109269499B (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