[发明专利]用于无人机蜂群协同导航的动态互观测在线建模方法

权利要求书

1.用于无人机蜂群协同导航的动态互观测在线建模方法，其特征在于，包括如下步骤：

步骤1，对无人机蜂群中的每个成员进行编号并表示为1,2,…,n，按照当前时刻各成员机载卫星导航接收机接收到可用星数量，对成员进行第一级筛选，确定各成员在协同导航中的角色：设接收到可用星数量小于4的成员为对象成员，将对象成员编号集合记为A；设接收到可用星数量不小于4的成员为备选参考成员，将备选参考成员编号集合记为B；且

步骤2，获取对象成员i机载导航系统指示位置，并以该指示位置为原点，建立该对象成员当地东北天地理坐标系，i表示成员编号且i∈A；

步骤3，获取备选参考成员j机载导航系统指示位置及其定位误差协方差，并将备选参考成员j机载导航系统指示位置及其定位误差协方差均转换到步骤2建立的对象成员i当地东北天地理坐标系中，j表示成员编号且j∈B；

步骤4，按照每个对象成员与每个备选参考成员之间是否可以相互测距，对备选参考成员进行第二级筛选，确定各备选参考成员在协同导航中的角色：设与对象成员i可以相互测距的备选参考成员为对象成员i的可用参考成员，将对象成员i的可用参考成员编号集合记为C_i，且

步骤5，计算对象成员与其可用参考成员的互观测矢量，并根据互观测矢量计算对象成员与其可用参考成员的矢量投影矩阵；

步骤6，计算对象成员与其可用参考成员的对象位置投影矩阵以及可用参考位置投影矩阵；

步骤7，利用步骤5获得的矢量投影矩阵和步骤6获得的对象位置投影矩阵，计算对象成员与其可用参考成员之间状态互观测矩阵；

步骤8，利用步骤5获得的矢量投影矩阵和步骤6获得的可用参考位置投影矩阵，计算对象成员与其可用参考成员之间噪声互观测矩阵；利用噪声互观测矩阵，计算对象成员与其可用参考成员之间互观测噪声协方差；

步骤9，利用步骤7获得的状态互观测矩阵，建立对象成员对其全部可用参考成员的互观测集合矩阵；

步骤10，利用步骤8获得的互观测噪声协方差，建立对象成员对其全部可用参考成员的互观测集合协方差；

步骤11，利用步骤5获得的互观测矢量，建立对象成员对其全部可用参考成员的互观测集合观测量；

步骤12，根据步骤9获得的互观测集合矩阵、步骤10获得的互观测集合协方差以及步骤11获得的互观测集合观测量，建立无人机蜂群协同导航的动态互观测模型，根据动态互观测模型进行对象成员加权最小二乘定位，得到对象成员位置的经度修正量、纬度修正量、高度修正量，并计算修正的经度、纬度、高度；

步骤13，利用步骤7获得的状态互观测矩阵和步骤8获得的互观测噪声协方差，计算对象成员位置估计协方差；

步骤14，利用步骤6获得的对象位置投影矩阵和步骤12得到的对象成员位置的经度修正量、纬度修正量、高度修正量，计算在线建模误差量；当在线建模误差量小于事先设置的动态互观测在线建模误差控制标准时，判定在线建模迭代收敛，即在线建模结束并转入步骤15，否则返回步骤5对互观测模型进行迭代修正；

步骤15，判断是否导航结束，如是则结束；否则返回步骤1进行下一时刻建模。

2.根据权利要求1所述用于无人机蜂群协同导航的动态互观测在线建模方法，其特征在于，步骤5所述互观测矢量，表达式为：

其中，为对象成员i与其可用参考成员k的互观测矢量，分别为在对象成员i当地东北天地理坐标系东、北、天向的分量，Δλ_ik、ΔL_ik、Δh_ik分别为对象成员i与其可用参考成员k机载导航系统输出经度、纬度、高度之差，R_N为地球参考椭球卯酉圈曲率半径，f为地球参考椭球扁率，L_i、h_i分别为对象成员i机载导航系统输出的纬度、高度。

3.根据权利要求1所述用于无人机蜂群协同导航的动态互观测在线建模方法，其特征在于，步骤5所述矢量投影矩阵，表达式为：

其中，为对象成员i与其可用参考成员k的矢量投影矩阵，分别为在对象成员i当地东北天地理坐标系东、北、天向的分量，为对象成员i与其可用参考成员k的互观测矢量，d_ik为对象成员i与其可用参考成员k之间的距离计算值，