[发明专利]一种多机协同无源定位方法、装置及系统

权利要求书

1.一种多机协同无源定位方法，其特征在于，包括：

分别确定各辅机在主机的机体坐标系下的状态坐标；

获取所述主机对待定位目标的视线角，及各所述辅机对所述待定位目标的视线角；

根据各所述辅机在所述主机的机体坐标系下的状态坐标，各所述辅机对所述待定位目标的视线角，以及所述主机对所述待定位目标的视线角，分别确定各所述辅机对应的所述待定位目标的状态初始值；

将所有所述辅机对应的所述待定位目标的状态初始值进行融合，得到所述待定位目标的定位结果；

所述根据各辅机在所述主机的机体坐标系下的状态坐标，各所述辅机对所述待定位目标的视线角，以及所述主机对所述待定位目标的视线角，分别确定各所述辅机对应的所述待定位目标的状态初始值包括：

建立基于所述主机的机体坐标系的所述待定位目标的状态方程：

以各所述辅机对所述待定位目标的视线角以及所述主机对所述待定位目标的视线角为量测信息，建立量测方程：

其中，X表示所述待定位目标的状态，W、V分别为过程噪声和测量噪声，Z表示量测值，以A表示主机，B表示各辅机，M表示所述待定位目标，为主机A对所述待定位目标M的视线角，为各辅机B对所述待定位目标M的视线角，为所述待定位目标M在主机A机体坐标系下的位置坐标，为各辅机B在主机A机体坐标系下的位置坐标；

对所述状态方程离散化处理，得到：

X_k＝FX_k-1+GW，

其中，X_k表示所述待定位目标在k时刻的状态，X_k-1表示所述待定位目标在k-1时刻的状态，F为k-1时刻至k时刻的状态转移矩阵，G为系统噪声驱动矩阵，T为滤波周期，I_3×3表示3阶单位矩阵；

对所述量测方程线性化处理，得到：

其中，Z_k表示k时刻的量测值，H_k为k时刻的量测矩阵，为k时刻所述待定位目标M在主机A机体坐标系下的位置坐标，为k时刻各辅机B在主机A机体坐标系下的位置坐标，sign()为符号函数。

2.如权利要求1所述的多机协同无源定位方法，其特征在于，所述将所有所述辅机对应的所述待定位目标的状态初始值进行融合，得到所述待定位目标的定位结果包括：

计算所有所述辅机对应的所述待定位目标的状态初始值的加权平均，将所述加权平均作为所述待定位目标的定位结果。

3.如权利要求1所述的多机协同无源定位方法，其特征在于，所述对所述量测方程线性化处理之后，还包括：

基于离散化处理后的所述状态方程以及线性化处理后的所述量测方程，按照预设流程进行扩展卡尔曼滤波，所述预设流程包括：

根据k-1时刻状态的最优估计值预测k时刻的状态：其中，为k-1时刻状态的最优估计值，为根据k-1时刻状态的最优估计值预测得到的k时刻状态的先验估计值；

获取量测值：Z(k)表示k时刻的量测值；

计算对应的均方误差：P(k|k-1)＝FP(k-1)F^T+GQG^T，其中，P(k|k-1)指对应的均方误差，P(k-1)指对应的均方误差，F^T表示状态转移矩阵的转置矩阵，G^T表示系统噪声驱动矩阵的转置矩阵；

计算滤波增益：K(k)＝P(k|k-1)H(k)^T[H(k)P(k|k-1)H(k)^T+R]，其中，K(k)表示滤波增益，H(k)为k时刻的量测矩阵，H(k)^T为k时刻的量测矩阵的转置矩阵，R表示量测噪声方差矩阵；

进行状态估计：

其中，表示k时刻状态的最优估计值；

更新对应的均方误差：P(k)＝[1-K(k)H(k)]P(k|k-1)，其中，P(k)表示对应的均方误差。