一种惯导协作定位方法及定位设备与流程

技术特征：

1.一种惯导协作定位方法，其特征在于，包括：

步骤1：获取第一定位参数、第二定位参数以及第一设备到第二设备的距离向量，所述第一定位参数包括所述第一设备的惯导位置以及惯导误差，所述第二定位参数包括所述第二设备的惯导位置以及惯导误差，所述距离向量包括所述第一设备到所述第二设备的距离以及测距信号到达角；

步骤2：根据当前第一定位参数、当前第二定位参数以及所述距离向量，按照第一预设算法计算出第一目标定位参数，所述第一目标定位参数包括所述第一设备的目标惯导位置以及目标惯导误差；

若所述第一设备的目标惯导误差不低于预设误差门限时，执行步骤3；

若所述第一设备的目标惯导误差低于预设误差门限时，执行步骤4；

步骤3：根据所述第一目标定位参数、当前第二定位参数以及所述距离向量，按照第二预设算法计算出第二目标定位参数，其中，所述第二目标定位参数包括所述第二设备的目标惯导位置以及目标惯导误差；

将所述第一目标定位参数作为新的第一定位参数，并将所述第二目标定位参数作为新的第二定位参数，再次执行所述步骤2；

步骤4：输出所述第一设备的目标惯导位置。

2.根据权利要求1所述的方法，其特征在于，所述根据当前第一定位参数、当前第二定位参数以及所述距离向量，按照第一预设算法计算出第一目标定位参数包括：

根据当前第二定位参数以及所述距离向量，计算得到第三定位参数，所述第三定位参数包括第一设备的惯导协作位置以及惯导协作误差，计算公式如下：

$\left\{\begin{array}{c}{E}_{A1}\left(m0\right)=E_B\left(m1\right)-D_{AB}\×sin\left({\mathrm{\φ}}_{AB}\right)\\ N_{A1}\left(m0\right)=N_B\left(m1\right)-D_{AB}\×\cos \left({\mathrm{\φ}}_{AB}\right)\\ {\mathrm{Var}}_{A1}\left(m0\right)={\mathrm{Var}}_B\left(m1\right)\end{array}\right.$

其中，E_A1(m0)为第一设备的惯导协作位置的东向分量，N_A1(m0)为所述第一设备的惯导协作位置的北向分量，E_B(m1)为第二设备的惯导位置的东向分量，N_B(m1)为第二设备的惯导位置的北向分量，D_AB为第一设备到所述第二设备的距离，φ_AB为测距信号到达角，Var_A1(m0)为所述第一设备的惯导协作误差，Var_B(m1)为所述第二设备的惯导误差；

根据当前第一定位参数、所述第三定位参数以及所述距离向量，计算得到所述第一设备的目标惯导位置以及目标惯导误差，计算公式如下：

$\left\{\begin{array}{c}{E}_{A1}^{*}\left(m0\right)=(\frac{E_A\left(m0\right)}{{\mathrm{Var}}_A\left(m0\right)}+\frac{E_{A1}\left(m0\right)}{{\mathrm{Var}}_{A1}\left(m0\right)})/(\frac{1}{{\mathrm{Var}}_A\left(m0\right)}+\frac{1}{{\mathrm{Var}}_{A1}\left(m0\right)})\\ N_{A1}^{*}\left(m0\right)=(\frac{N_A\left(m0\right)}{{\mathrm{Var}}_A\left(m0\right)}+\frac{N_{A1}\left(m0\right)}{{\mathrm{Var}}_{A1}\left(mv\right)})/(\frac{1}{{\mathrm{Var}}_A\left(m0\right)}+\frac{1}{{\mathrm{Var}}_{A1}\left(m0\right)})\\ {\mathrm{Var}}_{A1}^{*}\left(m0\right)=\frac{{\mathrm{Var}}_B\left(m1\right)}{1+{\mathrm{Var}}_B\left(m1\right)/{\mathrm{Var}}_A\left(m0\right)}\end{array}\right.$

其中，为第一设备的目标惯导位置的东向分量，为第二设备的目标惯导位置的北向分量，为第一设备的目标惯导误差，E_A(m0)为第一设备的惯导位置的东向分量，N_A(m0)为第一设备的惯导位置的北向分量，Var_A(m0)为第一设备的惯导误差。

3.根据权利要求2所述的方法，其特征在于，所述根据所述第一目标定位参数、当前第二定位参数以及所述距离向量，按照第二预设算法计算出第二目标定位参数包括：

根据所述第一目标定位参数以及所述距离向量，计算得到第四定位参数，所述第四定位参数包括所述第二设备的惯导协作位置以及惯导协作误差，计算公式如下：

$\left\{\begin{array}{c}{E}_{B1}\left(m1\right)=E_{A1}^{*}\left(m0\right)+D_{AB}\×sin\left({\mathrm{\φ}}_{AB}\right)\\ N_{B1}\left(m1\right)=N_{A1}^{*}\left(m0\right)+D_{AB}\×\cos \left({\mathrm{\φ}}_{AB}\right)\\ {\mathrm{Var}}_{B1}\left(m1\right)={\mathrm{Var}}_{A1}^{*}\left(m0\right)\end{array}\right.$

其中，E_B1(m1)为第二设备的惯导协作位置的东向分量，N_B1(m1)为第二设备的惯导协作位置的北向分量，Var_B1(m1)为第二设备的惯导协作误差；

根据当前第二定位参数、所述第四定位参数以及所述距离向量，计算得到第二目标定位参数，计算公式如下：

$\left\{\begin{array}{c}{E}_{B1}^{*}\left(m1\right)=(\frac{E_B\left(m1\right)}{{\mathrm{Var}}_B\left(m1\right)}+\frac{E_{B1}\left(m1\right)}{{\mathrm{Var}}_{B1}\left(m1\right)})/(\frac{1}{{\mathrm{Var}}_B\left(m1\right)}+\frac{1}{{\mathrm{Var}}_{B1}\left(m1\right)})\\ N_{B1}^{*}\left(m1\right)=(\frac{N_B\left(m1\right)}{{\mathrm{Var}}_B\left(m1\right)}+\frac{N_{B1}\left(m1\right)}{{\mathrm{Var}}_{B1}\left(m1\right)})/(\frac{1}{{\mathrm{Var}}_B\left(m1\right)}+\frac{1}{{\mathrm{Var}}_{B1}\left(m1\right)})\\ {\mathrm{Var}}_{B1}^{*}\left(m1\right)=1/(\frac{1}{{\mathrm{Var}}_B\left(m1\right)}+\frac{1}{{\mathrm{Var}}_{B1}\left(m1\right)})=\frac{{\mathrm{Var}}_{B1}\left(m1\right)}{1+{\mathrm{Var}}_{B1}\left(m1\right)/{\mathrm{Var}}_B\left(m1\right)}\\ =\frac{{\mathrm{Var}}_{A1}^{*}\left(m0\right)}{1+{\mathrm{Var}}_{A1}^{*}\left(m0\right)/{\mathrm{Var}}_B\left(m1\right)}\end{array}\right.$

其中，为第二设备的目标惯导位置的东向分量，为第二设备的目标惯导位置的北向分量，为第二设备的目标惯导误差。

4.根据权利要求1至3中任一项所述的方法，其特征在于，在所述步骤1之前，所述方法还包括：

确定到所述第一设备距离最近的相邻设备为所述第二设备，所述相邻设备为到所述第一设备的距离小于第一预设距离的惯导设备。

5.根据权利要求1至3中任一项所述的方法，其特征在于，在所述步骤1之前，所述方法还包括：

确定惯导误差最小的相邻设备为所述第二设备，所述相邻设备为到所述第一设备的距离小于第二预设距离的惯导设备。

6.一种定位设备，其特征在于，包括：获取模块、计算模块、判断模块、输出模块；其中，

所述获取模块，用于获取第一定位参数、第二定位参数以及第一设备到第二设备的距离向量，所述第一定位参数包括所述第一设备的惯导位置以及惯导误差，所述第二定位参数包括所述第二设备的惯导位置以及惯导误差，所述距离向量包括所述第一设备到所述第二设备的距离以及测距信号到达角；

所述计算模块，用于根据当前第一定位参数、当前第二定位参数以及所述距离向量，按照第一预设算法计算出第一目标定位参数，所述第一目标定位参数包括所述第一设备的目标惯导位置以及目标惯导误差；

所述判断模块，用于判断所述第一设备的目标惯导误差是否不低于预设误差门限，若是，则触发所述计算模块根据所述第一目标定位参数、当前第二定位参数以及所述距离向量，按照第二预设算法计算出第二目标定位参数；将所述第一目标定位参数作为新的第一定位参数，并将所述第二目标定位参数作为新的第二定位参数，若否，则触发所述输出模块输出所述第一设备的目标惯导位置。

7.根据权利要求6所述的定位设备，其特征在于，

所述计算模块具体用于根据当前第二定位参数以及所述距离向量，计算第三定位参数，所述第三定位参数包括第一设备的惯导协作位置以及惯导协 作误差，计算公式如下：

$\left\{\begin{array}{c}{E}_{A1}\left(m0\right)=E_B\left(m1\right)-D_{AB}\×sin\left({\mathrm{\φ}}_{AB}\right)\\ N_{A1}\left(m0\right)=N_B\left(m1\right)-D_{AB}\×\cos \left({\mathrm{\φ}}_{AB}\right)\\ {\mathrm{Var}}_{A1}\left(m0\right)={\mathrm{Var}}_B\left(m1\right)\end{array}\right.$

其中，E_A1(m0)为第一设备的惯导协作位置的东向分量，N_A1(m0)为所述第一设备的惯导协作位置的北向分量，E_B(m1)为第二设备的惯导位置的东向分量，N_B(m1)为第二设备的惯导位置的北向分量，D_AB为第一设备到所述第二设备的距离，φ_AB为测距信号到达角，Var_A1(m0)为所述第一设备的惯导协作误差，Var_B(m1)为所述第二设备的惯导误差；

根据当前第一定位参数、所述第三定位参数以及所述距离向量，计算得到所述第一设备的目标惯导位置以及目标惯导误差，计算公式如下：

$\left\{\begin{array}{c}{E}_{A1}^{*}\left(m0\right)=(\frac{E_A\left(m0\right)}{{\mathrm{Var}}_A\left(m0\right)}+\frac{E_{A1}\left(m0\right)}{{\mathrm{Var}}_{A1}\left(m0\right)})/(\frac{1}{{\mathrm{Var}}_A\left(m0\right)}+\frac{1}{{\mathrm{Var}}_{A1}\left(m0\right)})\\ N_{A1}^{*}\left(m0\right)=(\frac{N_A\left(m0\right)}{{\mathrm{Var}}_A\left(m0\right)}+\frac{N_{A1}\left(m0\right)}{{\mathrm{Var}}_{A1}\left(mv\right)})/(\frac{1}{{\mathrm{Var}}_A\left(m0\right)}+\frac{1}{{\mathrm{Var}}_{A1}\left(m0\right)})\\ {\mathrm{Var}}_{A1}^{*}\left(m0\right)=\frac{{\mathrm{Var}}_B\left(m1\right)}{1+{\mathrm{Var}}_B\left(m1\right)/{\mathrm{Var}}_A\left(m0\right)}\end{array}\right.$

其中，为第一设备的目标惯导位置的东向分量，为第二设备的目标惯导位置的北向分量，为第一设备的目标惯导误差，E_A(m0)为第一设备的惯导位置的东向分量，N_A(m0)为第一设备的惯导位置的北向分量，Var_A(m0)为第一设备的惯导误差。

8.根据权利要求7所述的定位设备，其特征在于，

所述计算模块具体用于根据第一目标定位参数以及所述距离向量，计算得到第四定位参数，所述第四定位参数包括所述第二设备的惯导协作位置以及惯导协作误差，计算公式如下：

$\left\{\begin{array}{c}{E}_{B1}\left(m1\right)=E_{A1}^{*}\left(m0\right)+D_{AB}\×sin\left({\mathrm{\φ}}_{AB}\right)\\ N_{B1}\left(m1\right)=N_{A1}^{*}\left(m0\right)+D_{AB}\×\cos \left({\mathrm{\φ}}_{AB}\right)\\ {\mathrm{Var}}_{B1}\left(m1\right)={\mathrm{Var}}_{A1}^{*}\left(m0\right)\end{array}\right.$

其中，E_B1(m1)为第二设备的惯导协作位置的东向分量，N_B1(m1)为第二设备的惯导协作位置的北向分量，Var_B1(m1)为第二设备的惯导协作误差；

根据当前第二定位参数、所述第四定位参数以及所述距离向量，计算得到第二目标定位参数，计算公式如下：

$\left\{\begin{array}{c}{E}_{B1}^{*}\left(m1\right)=(\frac{E_B\left(m1\right)}{{\mathrm{Var}}_B\left(m1\right)}+\frac{E_{B1}\left(m1\right)}{{\mathrm{Var}}_{B1}\left(m1\right)})/(\frac{1}{{\mathrm{Var}}_B\left(m1\right)}+\frac{1}{{\mathrm{Var}}_{B1}\left(m1\right)})\\ N_{B1}^{*}\left(m1\right)=(\frac{N_B\left(m1\right)}{{\mathrm{Var}}_B\left(m1\right)}+\frac{N_{B1}\left(m1\right)}{{\mathrm{Var}}_{B1}\left(m1\right)})/(\frac{1}{{\mathrm{Var}}_B\left(m1\right)}+\frac{1}{{\mathrm{Var}}_{B1}\left(m1\right)})\\ {\mathrm{Var}}_{B1}^{*}\left(m1\right)=1/(\frac{1}{{\mathrm{Var}}_B\left(m1\right)}+\frac{1}{{\mathrm{Var}}_{B1}\left(m1\right)})=\frac{{\mathrm{Var}}_{B1}\left(m1\right)}{1+{\mathrm{Var}}_{B1}\left(m1\right)/{\mathrm{Var}}_B\left(m1\right)}\\ =\frac{{\mathrm{Var}}_{A1}^{*}\left(m0\right)}{1+{\mathrm{Var}}_{A1}^{*}\left(m0\right)/{\mathrm{Var}}_B\left(m1\right)}\end{array}\right.$

其中，为第二设备的目标惯导位置的东向分量，为第二设备的目标惯导位置的北向分量，为第二设备的目标惯导误差。

9.根据权利要求6至8中任一项所述的定位设备，其特征在于，所述定位设备还包括：

第一确定模块，用于确定到所述第一设备距离最近的相邻设备为所述第二设备，所述相邻设备为到所述第一设备的距离小于第一预设距离的惯导设备。

10.根据权利要求6至8中任一项所述的定位设备，其特征在于，所述定位设备还包括：

第二确定模块，用于确定惯导误差最小的相邻设备为所述第二设备，所述相邻设备为到所述第一设备的距离小于第二预设距离的惯导设备。

11.一种定位设备，其特征在于，包括输入装置、输出装置、处理器、总线和存储器；所述输入装置、所述输出装置、所述处理器和所述存储器之间通过所述总线相互连接，其中，通过调用所述存储器存储的操作指令，所述处理器用于执行如下方法：

控制输入装置获取第一定位参数、第二定位参数以及第一设备到第二设备的距离向量，所述第一定位参数包括所述第一设备的惯导位置以及惯导误差，所述第二定位参数包括所述第二设备的惯导位置以及惯导误差，所述距离向量包括所述第一设备到所述第二设备的距离以及测距信号到达角；

根据当前第一定位参数、当前第二定位参数以及所述距离向量，按照第一预设算法计算出所述第一设备的目标惯导位置以及目标惯导误差；

判断所述第一设备的目标惯导误差是否不低于预设误差门限，若是，则根据所述第一目标定位参数、当前第二定位参数以及所述距离向量，按照第 二预设算法计算出第二目标定位参数；将所述第一目标定位参数作为新的第一定位参数，并将所述第二目标定位参数作为新的第二定位参数；若否，则控制所述输出装置输出所述第一设备的目标惯导位置。