一种组合导航相对定位方法与流程

本发明属于导航技术领域，尤其涉及一种组合导航相对定位方法。

背景技术：

在地震救援，反恐作战，野外搜救等突发事件的复杂环境下，行动人员常常需要确定自己的当前位置，并把定位信息及时传达给同伴及上级指挥者，以利于行动人员掌握自身位置状态，保障安全，也有利于指挥者全面了解所有人员的姿态，统筹下一步的行动计划。

传统的通过地图、指南针，加以观察地图环境的定位方法不那么准确，在山区、丛林、楼宇、洞穴等复杂作战环境中，搜寻定位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,1)，将每组初始坐标点代入式(3)约束条件，判断是否满足约束条件，即gij是否成立，如果成立表明该初始值满足，则直接保留，待定位人员该时刻坐标为如果不成立，则使用powell方法进行求解。所述使用powell方法进行求解，方法如下：

(3.1)定义初始非相关搜索方向分别为ξ1，ξ2，…ξ2n，其中，ξr为单位矩阵的第r行行向量；

(3.2)采用调用随机函数方式产生了n个人员初始坐标，记为p⁽⁰⁾，当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⁽⁰⁾＝p⁽²ⁿ⁾，重复步骤(3.2)继续调整坐标；若不成立，则令p^(*)＝p⁽²ⁿ⁾，判断是否满足迭代结束条件|f(p^(*))-f(p⁽⁰⁾)|＜η，η为收敛精度，满足条件则将p^(*)代入増广函数，即式(4)中计算出f(p^(*))，判断是否满足约束条件gij，若不满足继续更新初始坐标p⁽⁰⁾＝p⁽²ⁿ⁾，重复步骤(3.2)；若满足则从而得出利用无线通讯方式测距定位解算出的n个人员位置在tk～tk-1时间内人员做匀速运动，则速度为其中为tk时刻无线通讯方式测距定位解算每个人员的速度，为tk时刻无线通讯方式测距定位解算每个人员的坐标，为tk-1时刻无线通讯方式测距定位解算每个人员的坐标。

其中，步骤(4)中，建立无线通讯和惯导定位组合导航模型，利用卡尔曼滤波融合得出精确相对坐标，方法如下：

(4.1)选取人员惯导系统导航误差作为卡尔曼滤波状态变量，即状态向量为：其中，为人员方向角误差，δvⁿ为人员速度误差，δpⁿ为人员位置误差，高斯白噪声为陀螺仪误差，为加速度计误差；

(4.2)建误差系统

其中，为是人员失准角误差变化率，为人员速度误差变化率，δvⁿ为人员速度误差，为人员位置误差变化率，为人员的姿态矩阵，为陀螺误差，f^b为加速度计输出，δf^b为加速度计误差；

(4.3)建立误差系统的连续时间状态方程：

其中，表示状态向量的一阶微分，v(t)为高斯白噪声，f(t)为状态转移矩阵，g(t)为噪声输入矩阵；

(4.4)tk时刻惯导系统输出的速度为：

(4.5)tk时刻无线通讯方式测距定位解算的速度为：

其中，表示tk时刻人员携带惯性系统输出的速度，表示tk时刻人员实际速度，δviⁿ表示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时刻全体人员的位置误差作为状态反馈校正，结合tk时刻无线通讯方式测距定位解算获得的坐标位置则n个人员tk时刻定位坐标:

有益效果：与现有技术相比，本发明的技术方案具有以下有益技术效果：

(1)利用无线通讯技术同惯导技术融合使得系统更加稳定；

(2)无线通讯中采用toa技术测距具有较好的异步性和实用性；

(3)利用powell函数、罚函数结合的方法使得非线性规划能够顺利解算；

(4)采用卡尔曼滤波融合方法提高了定位精度。

附图说明

图1为本发明使用的toa测距示意图；

图2为本发明使用的方法流程图；

图3为本发明测距定位估算结果图；

图4为本发明融合定位结果图。

具体实施方式

下面结合附图和实施例对本发明的技术方案作进一步的说明。

针对toa具有精度高、测距耗时少、传输距离远等特点，本发明采用toa测距技术，建立导航相对坐标估计模型具体包括如下步骤：

有n个作战人员分布在二维坐标系下某个平面区域内，表示第i个待定位人员在相对坐标系下的待估计位置。

如图1所示，为toa测距示意图，两两人员i，j之间采用toa技术相互测距，步骤如下，t1时刻，i向j发送询问指令，t2时刻j收到询问指令，t3为j向i发送应答指令时刻，t4为i收到应答指令的时刻，利用异步toa原理可得：

距离为

其中，c为光速。

建立测距矩阵

确定待定位人员坐标估计模型：

其中，为n个人员待估计坐标，δ为系统调整因子，εij为允许定位精度误差，既在误差范围内，调整坐标值使得该目标函数即式(3)最小则为最优解。

考虑测距模型的误差，将坐标估计模型转化为有约束非线性规划问题，利用外部罚函数法“能够从非可行解出发逐步逼近可行域”的特点，并结合powell方法能够“逼近局部最优解”的特点作为最优化求解方法，用于求解目标函数最优解，具体方法流程如图2所示。

由于当测距结果存在误差时，节点定位问题属于np难问题，通常将基于测距的相对定位问题转化为非线性规划规划问题，调整坐标值使得该函数最小则为最优解。

利用问题的目标函数和约束函数构造出带参数的所谓增广目标函数，把约束非线形规划，由可行域外部逐渐逼近原约束优化问题的最优解，即原目标函数的最优解。

具体步骤如下：

1.设立増广函数

其中，γij为罚因子，gij为最大值函数，gij＝max(0,gij)，其中，gij为式(3)中约束条件。

2.调用随机函数设置初始坐标点

调用随机函数产生n个人员的初始坐标记做p⁽⁰⁾，初始坐标值产生区间为(0,1)，将每组初始坐标点，代入式(3)约束条件，判断是否满足约束条件，即gij是否成立，如果成立表明该初始值满足，则不需要处理直接保留，则待定位人员坐标如果不成立，代入下面流程即powell方法进行求解。

3.利用powell方法进行非线性规划问题求解，得到最优解。

定义该方法初始非相关搜索方向分别为ξ1，ξ2，…ξ2n，其中，ξr为单位矩阵的第r行行向量，n为坐标数。

方法具体步骤如下：

步骤(1)上述采用调用随机函数方式产生了n个人员初始坐标，记为p⁽⁰⁾，当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)计算并判断

如果式(7)成立，则将初始值更新p⁽⁰⁾＝p⁽²ⁿ⁾重复步骤1继续调整坐标，若不成立，则令p^(*)＝p⁽²ⁿ⁾，判断是否满足迭代结束条件|f(p^(*))-f(p⁽⁰⁾)|＜η，p^(*)为迭代结果，η＝e^-4为收敛精度。满足条件则将p^(*)代入増广函数，即式(4)中计算出f(p^(*))，判断是否满足约束条件gij，若不满足继续更新初始坐标p⁽⁰⁾＝p⁽²ⁿ⁾，重复步骤(1)，若满足则从而得出人员的利用无线通讯方式测距定位解算出的n个人员位置在tk～tk-1时间内可以看做匀速运动，则速度为其中为tk时刻无线通讯方式测距定位解算每个人员的速度，为tk时间无线通讯方式测距定位解算每个人员的坐标，为tk-1时间无线通讯方式测距定位解算每个人员的坐标，如图3所示为测距定位估算结果图。

建立无线通讯/惯导定位(cs/ins)组合导航数学模型，利用卡尔曼滤波融合得出精确相对坐标，具体方法包括：针对行人定位导航的特殊性，选取人员惯导系统导航误差作为卡尔曼滤波状态变量，即状态向量为其中，为人员方向角误差，δvⁿ为人员速度误差，δpⁿ为人员位置误差，系统噪声其中，为陀螺仪误差，为加速度计误差。

相对于飞机、导弹、舰船等载体，行人速度低，行走范围有限，且mems惯性传感器的采样频率较高，可构建误差系统。

其中，为是人员失准角误差变化率，为人员速度误差变化率，δvⁿ为人员速度误差，为人员位置误差变化率，为人员的姿态矩阵，为陀螺误差，f^b为加速度计输出，δf^b为加速度计误差。

于是，误差系统的连续时间状态方程为：

其中，表示状态向量的一阶微分，状态量具体含义如上，v(t)为高斯白噪声，f(t)为状态转移矩阵，g(t)为噪声输入矩阵

由惯导系统输出的速度为：

viⁿ＝vⁿ+δviⁿ

由无线通讯方式测距定位解算的速度为：

式中，viⁿ分别表示人员携带惯性系统输出的速度，δviⁿ表示惯导系统速度误差，表示无线通讯方式测距定位解算速度，δvwⁿ表示无线通讯方式测距定位解算速度误差。

建立速度观测方程：

δ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时刻人员携带惯性系统输出的速度，h为观测矩阵，wk-1和vk均视为彼此不相关的零均值高斯白噪声序列，状态转移矩阵：

对于(11)描述的线性离散线性系统，可以利用观测值δzk通过卡尔曼滤波融合方法对误差状态δxk进行估计，记初始状态向量为δx0＝(01×3，01×3，01×3)，p0＝i9×9为9阶单位矩阵为陀螺仪误差，为加速度计误差，代入式(12)，(13)进行迭代：

时间更新

量测更新

其中，δxk|k-1为δxk的一步估计，i为单位矩阵，hk为tk时刻h阵，pk|k-1为估计值δxk|k-1的均方误差阵，pk为δxk均方误差阵，kk为卡尔曼滤波增益矩阵，系统噪声和观测噪声方差阵分别为其中为陀螺仪误差，为加速度计误差，为噪声误差。

因此获得tk时刻每个人员状态量估计值δxk，取人员状态向量δxk中的位置误差部分将n个人员的合并为tk时刻全体人员的位置误差作为状态反馈校正，结合tk时刻无线通讯方式测距定位解算获得的坐标位置则n个人员tk时刻准确的坐标值:如图4所示为融合定位结果图。

如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。