一种组合导航相对定位方法技术

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

A Relative Positioning Method for Integrated Navigation

The invention discloses a relative positioning method for integrated navigation, which includes the following steps: (1) establishing a relative positioning estimation model for navigation based on TOA ranging technology; (2) transforming the coordinate estimation model into a constrained non-linear programming problem considering the error of the ranging model, using the external penalty function method, the feasible region can be approached step by step from the infeasible solution, and combining Powell's characteristics. The method can approximate the local optimal solution as an optimization method, which can be used to solve the optimal solution of the objective function. (3) The mathematical model of wireless communication and INS integrated navigation is established, and the exact relative coordinates are obtained by Kalman filter fusion.

【技术实现步骤摘要】
一种组合导航相对定位方法
本专利技术属于导航
，尤其涉及一种组合导航相对定位方法。
技术介绍
在地震救援，反恐作战，野外搜救等突发事件的复杂环境下，行动人员常常需要确定自己的当前位置，并把定位信息及时传达给同伴及上级指挥者，以利于行动人员掌握自身位置状态，保障安全，也有利于指挥者全面了解所有人员的姿态，统筹下一步的行动计划。传统的通过地图、指南针，加以观察地图环境的定位方法不那么准确，在山区、丛林、楼宇、洞穴等复杂作战环境中，搜寻定位GPS模块往往也会因为没有信号无法发挥定位作用。解决方案从单纯的捷联惯导系统导航发展到捷联惯导系统加地磁信息系统，替代GPS组合的人员自主定位，但其定位不准确，所以目前需要提出一种定位方法，满足在山区、丛林、楼宇、洞穴等复杂作战环境中搜寻定位需要，为新型搜寻定位技术提供储备。
技术实现思路
专利技术目的：针对以上现有技术存在的问题，本专利技术提出一种组合导航相对定位方法，该方法可以在GNSS拒止条件下为单兵作战人员提供连续自主定位导航信息，满足在山区、丛林、楼宇、洞穴等复杂作战环境中搜寻定位需要，为新型搜寻定位技术提供储备。技术方案：为实现本专利技术的目的，本专利技术所采用的技术方案是：一种组合导航相对定位方法，该方法包括以下步骤：(1)结合TOA测距技术，建立N个人员的测距矩阵；(2)根据测距矩阵建立N个人员导航相对定位估计模型；(3)根据定位估计模型构造增广目标函数，对约束进行非线性规划，求解增广目标函数最优解以获得N个人员无线通讯定位坐标；(4)建立无线通讯和惯导定位组合导航模型，利用卡尔曼滤波融合得出N个人员定位坐标。其中，步骤(1)中，结合TOA测距技术，建立N个人员的测距矩阵，方法如下：设N个人员分布在二维坐标系下某个平面区域内，表示第i个定位人员待估计坐标：计算两两人员i，j之间距离，T1时刻，i向j发送询问指令，T2时刻j收到询问指令，T3为j向i发送应答指令时刻，T4为i收到应答指令的时刻，利用异步TOA原理可得距离为dij：其中，C为光速；建立测距矩阵：其中，步骤(2)中，根据测距矩阵建立N个人员导航相对定位估计模型方法如下：其中，为N个定位人员待估计坐标，Δ为系统调整因子，εij为允许定位精度误差。其中，步骤(3)中，根据定位估计模型构造增广目标函数，方法如下：其中，γij为罚因子，Gij为最大值函数，Gij＝max(0,gij)，gij为式(3)中约束条件。进一步，步骤(3)中，求解目标函数最优解以获得N个人员无线通讯定位坐标，方法如下：调用随机函数产生N个人员的初始坐标记做P(0)，初始坐标值产生区间为(0,1)，将每组初始坐标点代入式(3)约束条件，判断是否满足约束条件，即gij是否成立，如果成立表明该初始值满足，则直接保留，待定位人员该时刻坐标为如果不成立，则使用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)计算并判断下式如果式(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)；若满足则从而得出利用无线通讯方式测距定位解算出的N个人员位置在tk～tk-1时间内人员做匀速运动，则速度为其中为tk时刻无线通讯方式测距定位解算每个人员的速度，为tk时刻无线通讯方式测距定位解算每个人员的坐标，为tk-1时刻无线通讯方式测距定位解算每个人员的坐标。其中，步骤(4)中，建立无线通讯和惯导定位组合导航模型，利用卡尔曼滤波融合得出精确相对坐标，方法如下：(4.1)选取人员惯导系统导航误差作为卡尔曼滤波状态变量，即状态向量为：其中，为人员方向角误差，δvn为人员速度误差，δpn为人员位置误差，高斯白噪声为陀螺仪误差，为加速度计误差；(4.2)建误差系统其中，为是人员失准角误差变化率，为人员速度误差变化率，δvn为人员速度误差，为人员位置误差变化率，为人员的姿态矩阵，为陀螺误差，fb为加速度计输出，δfb为加速度计误差；(4.3)建立误差系统的连续时间状态方程：其中，表示状态向量的一阶微分，v(t)为高斯白噪声，f(t)为状态转移矩阵，g(t)为噪声输入矩阵；(4.4)tk时刻惯导系统输出的速度为：(4.5)tk时刻无线通讯方式测距定位解算的速度为：其中，表示tk时刻人员携带惯性系统输出的速度，表示tk时刻人员实际速度，δVin表示tk时刻惯导系统速度误差，表示tk时刻无线通讯方式测距定位解算速度，表示tk时刻无线通讯方式测距定位解算速度误差；(4.6)建立速度观测方程：ΔZv＝HΔX(t)+w(t)(10)系统参数矩阵H＝[03×3I3×303×3]，ΔZv为测量矩阵，w(t)，v(t)均为无关的高斯白噪声；记待解算数据时长为T，将时间段0～T以采样周期Δt为间隔划分为多个时刻点t0,t1,t2...tm，k＝1,2,...,m，将式(9)、(10)离散化处理得：其中，ΔXk为tk时刻人员的状态向量Fk|k-1为tk-1到tk时刻转移矩阵，ΔXk-1为tk-1时刻人员状态向量，Gk|k-1为离散化噪声输入矩阵，ΔZk为tk时刻观测值为tk时刻人员无线通讯方式测距定位解算速度，为tk时刻携带惯性系统输出的速度，Wk-1和Vk均视为彼此不相关的零均值高斯白噪声序列，状态转移矩阵和离散化噪声输入矩阵：对于(11)描述的线性离散线性系统，利用观测值ΔZk通过卡尔曼滤波融合方法对误差状态ΔXk进行估计，记初始状态向量为ΔX0＝(01×3，01×3，01×3)，P0＝I9×9为9阶单位矩阵为陀螺仪误差，为加速度计误差。所述卡尔曼滤波融合方法如下：将初始状态向量代入式(12)，(13)进行计算获得tk时刻每个人员状态量估计值ΔXk：时间更新量测更新其中，ΔXk|k-1为ΔXk的一步估计，I为单位矩阵，Hk为tk时刻H阵，Pk|k-1为估计值ΔXk|k-1的均方误差阵，Pk为ΔXk均方误差阵，Kk为卡尔曼滤波增益矩阵，系统噪声和观测噪声方差阵分别为其中为陀螺仪误差，为加速度计误差，为噪声误差。进一步，利用卡尔曼滤波融合得出精确N个人员定位坐标，其方法如下：获得tk时刻每个人员状态量估计值ΔXk，取每个人员状态向量ΔXk中的位置误差部分将N个人员的合并为tk时刻全体人员的位置误差作为状本文档来自技高网...

【技术保护点】
1.一种组合导航相对定位方法，其特征在于，该方法包括以下步骤：(1)结合TOA测距技术，建立N个人员的测距矩阵；(2)根据测距矩阵建立N个人员导航相对定位估计模型；(3)根据定位估计模型构造增广目标函数，对约束进行非线性规划，求解增广目标函数最优解以获得N个人员无线通讯定位坐标；(4)建立无线通讯和惯导定位组合导航模型，利用卡尔曼滤波融合得出N个人员定位坐标。

【技术特征摘要】
1.一种组合导航相对定位方法，其特征在于，该方法包括以下步骤：(1)结合TOA测距技术，建立N个人员的测距矩阵；(2)根据测距矩阵建立N个人员导航相对定位估计模型；(3)根据定位估计模型构造增广目标函数，对约束进行非线性规划，求解增广目标函数最优解以获得N个人员无线通讯定位坐标；(4)建立无线通讯和惯导定位组合导航模型，利用卡尔曼滤波融合得出N个人员定位坐标。2.根据权利要求1所述的一种组合导航相对定位方法，其特征在于，步骤(1)中，结合TOA测距技术，建立N个人员的测距矩阵，方法如下：设N个人员分布在二维坐标系下平面区域内，表示第i个定位人员待估计坐标：计算两两人员i，j之间距离，T1时刻，i向j发送询问指令，T2时刻j收到询问指令，T3为j向i发送应答指令时刻，T4为i收到应答指令的时刻，利用异步TOA原理可得距离为dij：其中，C为光速；建立测距矩阵：3.根据权利要求1所述的一种组合导航相对定位方法，其特征在于，步骤(2)中，根据测距矩阵建立N个人员导航相对定位估计模型方法如下：其中，为N个定位人员待估计坐标，Δ为系统调整因子，εij为允许定位精度误差。4.根据权利要求3所述的一种组合导航相对定位方法，其特征在于，步骤(3)中，根据定位估计模型构造增广目标函数，方法如下：其中，γij为罚因子，Gij为最大值函数，Gij＝max(0,gij)，gij为式(3)中约束条件。5.根据权利要求4所述的一种组合导航相对定位方法，其特征在于，步骤(3)中，求解目标函数最优解以获得N个人员无线通讯定位坐标，方法如下：调用随机函数产生N个人员的初始坐标记做P(0)，初始坐标值产生区间为(0,1)，将每组初始坐标点代入式(3)约束条件，判断是否满足约束条件，即gij是否成立，如果成立表明该初始值满足，则直接保留，待定位人员该时刻坐标为如果不成立，则使用Powell方法进行求解。6.根据权利要求5所述的一种组合导航相对定位方法，其特征在于，所述使用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)计算并判断下式如果式(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)；若满足则从而得出利用无线通讯方式测距定位解算出的N个人员位置在tk～tk-1时间内人员做匀速运动，则速度为其中...

【专利技术属性】
技术研发人员：刘锡祥，许广富，杨文强，马晓爽，刘贤俊，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏,32