[发明专利]一种组合导航相对定位方法

权利要求书

1.一种组合导航相对定位方法，其特征在于，该方法包括以下步骤：

(1)结合TOA测距技术，建立N个人员的测距矩阵；

(2)根据测距矩阵建立N个人员导航相对定位估计模型；

(3)根据定位估计模型构造增广目标函数，对约束进行非线性规划，求解增广目标函数最优解以获得N个人员无线通讯定位坐标；

(4)建立无线通讯和惯导定位组合导航模型，利用卡尔曼滤波融合得出N个人员定位坐标；

步骤(1)中，结合TOA测距技术，建立N个人员的测距矩阵，方法如下：

设N个人员分布在二维坐标系下平面区域内，表示第i个定位人员待估计坐标：

计算两两人员i，j之间距离，T₁时刻，i向j发送询问指令，T2时刻j收到询问指令，T3为j向i发送应答指令时刻，T4为i收到应答指令的时刻，利用异步TOA原理可得距离为d_ij：

其中，C为光速；

建立测距矩阵：

步骤(2)中，根据测距矩阵建立N个人员导航相对定位估计模型方法如下：

其中，为N个定位人员待估计坐标，Δ为系统调整因子，ε_ij为允许定位精度误差；

步骤(3)中，根据定位估计模型构造增广目标函数，方法如下：

其中，γ_ij为罚因子，G_ij为最大值函数，G_ij＝max(0,g_ij)，g_ij为式(3)中约束条件；

步骤(3)中，求解目标函数最优解以获得N个人员无线通讯定位坐标，方法如下：调用随机函数产生N个人员的初始坐标记做P⁽⁰⁾，初始坐标值产生区间为(0,1)，将每组初始坐标点代入式(3)约束条件，判断是否满足约束条件，即g_ij是否成立，如果成立表明该初始值满足，则直接保留，待定位人员该时刻坐标为如果不成立，则使用Powell方法进行求解。

2.根据权利要求1所述的一种组合导航相对定位方法，其特征在于，所述使用Powell方法进行求解，方法如下：

(3.1)定义初始非相关搜索方向分别为ξ₁，ξ₂，…ξ_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，且qN；

使得Ω＝max(F(P^(q-1))-F(P^(q))) (6)

(3.3)计算并判断下式

如果式(7)成立，则将初始值更新P⁽⁰⁾＝P^(2N)，重复步骤(3.2)继续调整坐标；若不成立，则令P^(*)＝P^(2N)，判断是否满足迭代结束条件|F(P^(*))-F(P⁽⁰⁾)|＜η，η为收敛精度，满足条件则将P^(*)代入增广函数，即式(4)中计算出F(P^(*))，判断是否满足约束条件g_ij，若不满足继续更新初始坐标P⁽⁰⁾＝P^(2N)，重复步骤(3.2)；若满足则从而得出利用无线通讯方式测距定位解算出的N个人员位置在t_k～t_k-1时间内人员做匀速运动，则速度为其中为t_k时刻无线通讯方式测距定位解算每个人员的速度，为t_k时刻无线通讯方式测距定位解算每个人员的坐标，为t_k-1时刻无线通讯方式测距定位解算每个人员的坐标。