一种惯导协作定位方法及定位设备与流程
本发明涉及定位领域，尤其涉及一种惯导协作定位方法及定位设备。
背景技术：
惯性导航技术(Inertial Navigation System，简称INS)是指利用惯性元件(加速度计和陀螺仪)测量承载惯性元件的设备本身的加速度和航行姿态角度，经过积分和运算得到设备的速度和位置，进行导航定位的技术。惯性导航系统广泛应用于制导武器、火箭、飞机、船舶以及航海等诸多导航领域。惯性导航系统应用在定位领域，可以实现无源自主定位，不必依赖惯性部件之外的定位信号或系统，但是，惯性元件在长时间运行中会造成误差累积，从而影响定位精度。
为了提高定位精度，本领域技术人员提出惯导协作定位方法(专利申请号201410243722.X)，设备A可以与周围一定距离内惯导误差较小的设备B进行惯导协作定位，获取协作惯导位置作为其惯导位置，以降低惯导误差。假设设备A与设备B移动轨迹如图1所示，设备A从a0点出发，沿a1，a2，a3，a4，a1，a2，最后到达m0点，此时设备B从b0点出发直行到达m1点(m0附近)，基于人行航迹推算法，由于设备A绕行一圈到达m0点的惯导位置会由于绕行的多余步伐导致极大的误差，而设备B沿直线移动，比设备A的惯导误差小。如果设备A的惯导误差远大于比设备B的惯导误差，那么通过上述方法优化后，可以将设备A的惯导误差降低到约等于设备B的惯导误差，以达到提高定位精度的目的。
但是，当某个业务对定位精度要求很高，设备B的惯导误差不能满足该业务要求时，定位精度不足，此时计算得到的惯导位置不符合该业务要求，因此上述方法存在不足。
技术实现要素：
本申请提供了一种惯导协作定位方法以及定位设备，能够有效降低惯导 设备的惯导误差，提高定位精度。
第一方面提供了一种惯导协作定位方法，包括：
步骤1：获取第一定位参数、第二定位参数以及第一设备到第二设备的距离向量，第一定位参数包括第一设备的惯导位置以及惯导误差，第二定位参数包括第二设备的惯导位置以及惯导误差，距离向量包括第一设备到第二设备的距离以及测距信号到达角；
步骤2：根据当前第一定位参数、当前第二定位参数以及距离向量，按照第一预设算法计算出第一目标定位参数，第一目标定位参数包括第一设备的目标惯导位置以及目标惯导误差；
若第一设备的目标惯导误差不低于预设误差门限时，执行步骤3；
若第一设备的目标惯导误差低于预设误差门限时，执行步骤4；
步骤3：根据第一目标定位参数、当前第二定位参数以及距离向量，按照第二预设算法计算出第二目标定位参数，其中，第二目标定位参数包括第二设备的目标惯导位置以及目标惯导误差；
将第一目标定位参数作为新的第一定位参数，并将第二目标定位参数作为新的第二定位参数，再次执行步骤2；
步骤4：输出第一设备的目标惯导位置。
这样，当第一设备通过第二设备进行惯导协作定位得到的目标惯导位置不满足实际要求时，通过分别获取第一设备的第一目标定位参数，和第二设备的第二目标定位参数，对第一设备的目标惯导位置进行持续优化，就可以满足实际要求。
结合第一方面，在第一方面的第一种实现方式中，根据当前第一定位参数、当前第二定位参数以及距离向量，按照第一预设算法计算出第一目标定位参数包括：
根据当前第二定位参数以及距离向量，计算得到第三定位参数，第三定位参数包括第一设备的惯导协作位置以及惯导协作误差，计算公式如下：
其中，EA1(m0)为第一设备的惯导协作位置的东向分量，NA1(m0)为第一设 备的惯导协作位置的北向分量，EB(m1)为第二设备的惯导位置的东向分量，NB(m1)为第二设备的惯导位置的北向分量，DAB为第一设备到第二设备的距离，φAB为测距信号到达角，VarA1(m0)为第一设备的惯导协作误差，VarB(m1)为第二设备的惯导误差；
根据当前第一定位参数、第三定位参数以及距离向量，计算得到第一设备的目标惯导位置以及目标惯导误差，计算公式如下：
其中，为第一设备的目标惯导位置的东向分量，为第二设备的目标惯导位置的北向分量，为第一设备的目标惯导误差，EA(m0)为第一设备的惯导位置的东向分量，NA(m0)为第一设备的惯导位置的北向分量，VarA(m0)为第一设备的惯导误差。
结合第一方面的第一种实现方式，在第一方面的第二种实现方式中，根据第一目标定位参数、第二定位参数以及距离向量，按照第二预设算法计算第二目标定位参数包括：
根据第一目标定位参数以及距离向量，计算第四定位参数，第四定位参数包括第二设备的惯导协作位置以及惯导协作误差，计算公式如下：
其中，EB1(m1)为第二设备的惯导协作位置的东向分量，NB1(m1)为第二设备的惯导协作位置的北向分量，VarB1(m1)为第二设备的惯导协作误差；
根据当前第二定位参数、第四定位参数以及距离向量，计算得到第二目标定位参数，计算公式如下：
其中，为第二设备的目标惯导位置的东向分量，为第二设备的目标惯导位置的北向分量，为第二设备的目标惯导误差。
这样，本申请进一步提供了计算第一目标定位参数和第二目标定位参数的具体方法，保证了方案实施的可行性。
结合第一方面，或以上第一方面的任意一种实现方式，在第一方面的第三种实现方式中，在步骤1之前，上述方法还包括：
确定到第一设备距离最近的相邻设备为第二设备，相邻设备为到第一设备的距离小于第一预设距离的惯导设备。
这样，通过优选第二设备，以降低距离误差，进一步提高了定位精度。
结合第一方面，或以上第一方面的任意一种实现方式，在第一方面的第四种实现方式中，在步骤1之前，上述方法还包括：
确定惯导误差最小的相邻设备为第二设备，相邻设备为到第一设备的距离小于第二预设距离的惯导设备。
这样，通过优选第二设备，以降低惯导误差，优化了定位过程，提高了定位速度。
第二方面提供一种定位设备，具有实现上述惯导协作定位方法的功能。功能可以通过硬件实现，也可以通过硬件执行相应的软件实现。硬件或软件包括一个或多个与上述功能相对应的模块。
一种可能的实现方式中，定位设备包括：获取模块、计算模块、判断模块和输出模块；其中，
获取模块，用于获取第一定位参数、第二定位参数以及第一设备到第二设备的距离向量，第一定位参数包括第一设备的惯导位置以及惯导误差，第二定位参数包括第二设备的惯导位置以及惯导误差，距离向量包括第一设备 到第二设备的距离以及测距信号到达角；
计算模块，用于根据当前第一定位参数、当前第二定位参数以及距离向量，按照第一预设算法计算出第一目标定位参数，第一目标定位参数包括第一设备的目标惯导位置以及目标惯导误差；
判断模块，用于判断所述第一设备的目标惯导误差是否不低于预设误差门限，若是，则触发所述计算模块根据所述第一目标定位参数、当前第二定位参数以及所述距离向量，按照第二预设算法计算出第二目标定位参数；将所述第一目标定位参数作为新的第一定位参数，并将所述第二目标定位参数作为新的第二定位参数，若否，则触发输出模块输出所述第一设备的目标惯导位置。
第三方面提供一种定位设备，包括：
输入装置、输出装置、处理器、存储器和总线；输入装置、输出装置、处理器和存储器之间通过总线相互连接通信，其中，通过调用存储器存储的操作指令，处理器用于执行如下方法：
控制输入装置获取第一定位参数、第二定位参数以及第一设备到第二设备的距离向量，第一定位参数包括第一设备的惯导位置以及惯导误差，第二定位参数包括第二设备的惯导位置以及惯导误差，距离向量包括第一设备到第二设备的距离以及测距信号到达角；
根据当前第一定位参数、当前第二定位参数以及距离向量，按照第一预设算法计算出第一设备的目标惯导位置以及目标惯导误差；
判断所述第一设备的目标惯导误差是否不低于预设误差门限，若是，则根据所述第一目标定位参数、当前第二定位参数以及所述距离向量，按照第二预设算法计算出第二目标定位参数；将所述第一目标定位参数作为新的第一定位参数，并将所述第二目标定位参数作为新的第二定位参数，若否，则控制输出装置输出所述第一设备的目标惯导位置。
一种可能的实现方式中，
处理器具体用于根据当前第二定位参数以及距离向量，计算第三定位参数，第三定位参数包括第一设备的惯导协作位置以及惯导协作误差，计算公式如下：
其中，EA1(m0)为第一设备的惯导协作位置的东向分量，NA1(m0)为第一设备的惯导协作位置的北向分量，EB(m1)为第二设备的惯导位置的东向分量，NB(m1)为第二设备的惯导位置的北向分量，DAB为第一设备到第二设备的距离，φAB为测距信号到达角，VarA1(m0)为第一设备的惯导协作误差，VarB(m1)为第二设备的惯导误差；
根据当前第一定位参数、第三定位参数以及距离向量，计算得到第一设备的目标惯导位置以及目标惯导误差，计算公式如下：
其中，为第一设备的目标惯导位置的东向分量，为第二设备的目标惯导位置的北向分量，为第一设备的目标惯导误差，EA(m0)为第一设备的惯导位置的东向分量，NA(m0)为第一设备的惯导位置的北向分量，VarA(m0)为第一设备的惯导误差。
另一种可能的实现方式中，
处理器具体用于根据第一目标定位参数以及距离向量，计算得到第四定位参数，第四定位参数包括第二设备的惯导协作位置以及惯导协作误差，计算公式如下：
其中，EB1(m1)为第二设备的惯导协作位置的东向分量，NB1(m1)为第二设备的惯导协作位置的北向分量，VarB1(m1)为第二设备的惯导协作误差；
根据当前第二定位参数、第四定位参数以及距离向量，计算得到第二目标定位参数，计算公式如下：
其中，为第二设备的目标惯导位置的东向分量，为第二设备的目标惯导位置的北向分量，为第二设备的目标惯导误差。
另一种可能的实现方式中，处理器，还用于确定到第一设备距离最近的相邻设备为第二设备，相邻设备为到第一设备的距离小于第一预设距离的惯导设备。
另一种可能的实现方式中，处理器，还用于确定惯导误差最小的相邻设备为第二设备，相邻设备为到第一设备的距离小于第二预设距离的惯导设备。
本申请提供的技术方案中，第一设备可以根据第一定位参数、第二定位参数以及距离向量，对第一设备的惯导位置以及惯导误差进行优化，若优化后的惯导误差不低于预设误差门限，第一设备可以对第二设备的惯导位置以及惯导误差进行优化，再根据第二设备优化后的惯导位置以及惯导误差、第一设备优化后的惯导位置以及惯导误差、以及距离向量来对第一设备优化后的惯导位置进行再次优化，直至优化的惯导误差低于预设误差门限时，确定最终满足精度条件的惯导位置为第一设备的惯导位置，从而保证定位精度能够满足实际的业务要求。
附图说明
图1为本发明实施例中应用场景的一个示意图；
图2为现有技术中基本惯导定位法的一个示意图；
图3为本发明实施例中惯导协作定位方法的一个流程示意图；
图4为本发明实施例中仿真实验的一个误差累积分布曲线示意图；
图5为本发明实施例中定位设备的一个结构示意图；
图6为本发明实施例中定位设备的另一个结构示意图；
图7为本发明实施例中定位设备的另一个结构示意图。
具体实施方式
为便于理解，下面先对本申请中惯导设备以及惯导定位的基本计算方法进行介绍：
惯导设备是指具有惯导元件，可以实现惯导定位的移动设备。
惯导定位的基本计算方法有人行航迹推算法，请参阅图2：
以N为正北方向，E为正东方向，θ为人行航迹方向角(通过陀螺仪可获取)，假设移动设备的初始位置为(E(t0),N(t0))，人行跨步的步长距离为Dis(通过加速度计积分得到步行速度与步行时间获取)，则可得到时刻i的惯导位置为(E(ti),N(ti))，计算公式如下所示：
由于惯导元件运行时测量所得信息存在误差，时间越长，累积误差越大。
为了保证定位精度能够满足实际要求，本申请提供了一种惯导协作定位的方法，请参阅图3，方法包括：
S301、获取第一定位参数、第二定位参数以及第一设备到第二设备的距离向量；
本实施例中，第一定位参数包括第一设备的惯导位置以及惯导误差，第二定位参数包括第二设备的惯导位置以及惯导误差，距离向量包括第一设备到第二设备的距离以及测距信号到达角，测距信号到达角是指第一设备和第二设备之间的直线，与正北或正南方向之间的夹角。惯导误差可以是第一设备的惯导模块估计的方差，还可以是通过其他惯导误差模型得到的表示惯导误差的参数，此处不作限定。
第一设备、第二设备均为具有惯导定位能力的惯导设备，可以通过惯导元件获取相应的惯导位置以及惯导误差。第一设备或第二设备可以通过声学测距仪，或电磁测距仪测量第一设备到第二设备的距离。第一设备或第二设备可以通过惯导元件、麦克风阵列或天线阵列测量测距信号到达角，具体实 现过程可参阅现有技术。
当定位设备为第一设备时，定位设备可以从自身获取第一定位参数以及距离向量，并接收第二设备发送的第二定位参数；
当定位设备为第二设备时，与定位设备为第一设备相似，此处不再赘述。
当定位设备为第一设备、第二设备之外的其他设备时，例如定位服务器，定位设备可以从第一设备和第二设备获取第一定位参数、第二定位参数以及第一设备到第二设备的距离向量。需要说明的是，定位设备获取第一定位参数和/或第二定位参数的触发时机可以是第一设备或第二设备发现无环境定位信号，也可以是惯导误差大于预设误差门限时，此处不作限定。
具体的，惯导位置可以以东向、北向坐标二维位置表示，例如，第一设备为设备A，第二设备为设备B，第一设备的位置为m0，采用(EA(m0)，NA(m0))表示，第一设备的惯导误差采用VarA(m0)表示。第二设备的位置为m1，采用(EA(m1)，NA(m1))表示，第二设备的惯导误差采用VarB(m0)表示。
S302、根据当前第一定位参数、当前第二定位参数以及距离向量，计算计算出第一目标定位参数，第一目标定位参数包括第一设备的目标惯导位置以及目标惯导误差；
其中，在第一次计算第一目标定位参数时，当前第一定位参数和当前第二定位参数为S301中第一定位参数和第二定位参数，在第二次以后计算第一目标定位参数时，当前第一定位参数和当前第二定位参数是优化处理后的第一定位参数和第二定位参数。
具体的，计算过程可以包括以下步骤：
步骤A、根据当前第二定位参数以及距离向量，计算得到第三定位参数，第三定位参数包括第一设备的惯导协作位置以及惯导协作误差，计算公式如下；
其中，EA1(m0)为第一设备的惯导协作位置的东向分量，NA1(m0)为第一设备的惯导协作位置的北向分量，EB(m1)为第二设备的惯导位置的东向分量，NB(m1)为第二设备的惯导位置的北向分量，DAB为第一设备到第二设备的距 离，φAB为测距信号到达角，VarA1(m0)为第一设备的惯导协作误差，VarB(m1)为第二设备的惯导误差。
这样，第一设备的惯导协作误差等于第二设备的惯导误差，因此小于第一设备的惯导误差，因此相对于第一设备的惯导位置，计算得到的惯导协作位置也更接近m0。
步骤B、根据当前第一定位参数、第三定位参数以及距离向量，计算得到第一设备的目标惯导位置以及目标惯导误差，计算公式如下：
其中，为第一设备的目标惯导位置的东向分量，为第二设备的目标惯导位置的北向分量，为第一设备的目标惯导误差，EA(m0)为第一设备的惯导位置的东向分量，NA(m0)为第一设备的惯导位置的北向分量，VarA(m0)为第一设备的惯导误差。其他参数可参阅步骤A。
因为VarB(m1)/VarA(m0)>0，所以因此相对于惯导协作位置，计算得到的目标惯导位置也更接近m0。
S303、判断第一设备的目标惯导误差是否不低于预设误差门限，若是，则执行步骤S304，若否，则执行步骤S306；
其中，预设误差门限根据实际业务要求的精度设定。当第一设备的目标惯导误差不低于预设误差门限时，表明惯导误差太大，计算得到的目标惯导位置不符合实际要求精度，需要对目标惯导位置再次进行优化，执行步骤S304；当第一设备的目标惯导误差低于预设误差门限时，表明惯导误差合理，计算得到的目标惯导位置符合实际要求精度，不需要对目标惯导位置再次进行优化，执行步骤S306。
S304、根据第一目标定位参数、第二定位参数以及距离向量，计算第二目标定位参数；
其中，第一目标定位参数包括第一设备的目标惯导位置以及目标惯导误 差，第二目标定位参数包括第二设备的目标惯导位置以及目标惯导误差。
具体的，计算过程可以包括以下步骤：
步骤C、根据第一目标定位参数以及距离向量，计算第四定位参数，第四定位参数包括第二设备的惯导协作位置以及惯导协作误差，计算公式如下：
其中，EB1(m1)为第二设备的惯导协作位置的东向分量，NB1(m1)为第二设备的惯导协作位置的北向分量，VarB1(m1)为为第二设备的惯导协作误差，其他参数可参阅S303；
步骤D、根据当前第二定位参数、第四定位参数以及距离向量，计算第二目标定位参数，计算公式如下：
其中，为第二设备的目标惯导位置的东向分量，为第二设备的目标惯导位置的北向分量，为第二设备的目标惯导误差，其他参数可参阅步骤S303以及步骤C。
S305、将第一目标定位参数作为新的第一定位参数，并将第二目标定位参数作为新的第二定位参数，再次执行步骤S302；
具体的，将第一设备的目标惯导位置作为第一定位参数中的惯导位置，将第一设备的目标惯导误差作为第一定位参数中的惯导误差，第二定位参数与第一定位参数相似，此处不再赘述。举例来说，第一设备将第一目标定位参数作为第一定位参数，将第二目标定位参数作为第二定位参数的具体赋值过程如下所示：
赋值完成后，重复执行步骤S302至步骤S305，直至第一设备的目标惯导误差低于预设误差门限为止。
S306、输出第一设备的目标惯导位置。
当第一设备的目标惯导误差低于预设误差门限时，表明计算所得目标惯导位置满足实际要求的精度，定位设备输出第一设备的目标惯导位置，解决了现有技术中无法保证定位精度满足实际要求的问题。
假设第一设备到第二设备的实际距离为DAB，测距传感器测量得到的距离误差为ΔS，则根据第二定位参数以及距离向量计算第一设备的惯导协作位置，计算公式如下：
由该计算公式可知，距离误差会影响第一设备的惯导协作位置的精度，进而影响到目标惯导位置的精度。为了提高定位精度，本发明提供了一种根据距离筛选相邻设备的方法，具体如下：
可选的，本发明提供的惯导协作定位方法还包括：确定到第一设备距离最近的相邻设备为第二设备。
本实施例中，相邻设备是到第一设备的距离小于第一预设距离的惯导设备，第一预设距离可以根据测距传感器的有效量程设定，或是根据实际测试设定，此处不作限定。
第一设备可以通过测距传感器对相邻设备进行测量，得到第一设备到相邻设备之间的距离。第一设备或其他定位设备可以通过上述距离，确定距离最近的相邻设备为第二设备，在实际应用中，惯导设备之间的距离越远，测距传感器测量得到的距离误差ΔS越大，反之，距离误差ΔS越小，因此将相邻设备作为第二设备能够得到最为精确的惯导协作位置，进而得到最为精确的 目标惯导位置。
另外，第二设备的惯导误差为VarB(m1)，假设第三设备与第一设备相邻，其惯导误差为VarC(m1)，由第二设备的惯导误差和第三设备的惯导误差计算得到的第一目标惯导误差分别如下：
当VarC(m1)<VarB(m1)时，
由上可知，相邻设备的惯导误差越小，计算所得第一设备的目标惯导误差越小，为了尽快地减小惯导误差，提高定位速度，本发明还提供了一种根据惯导误差筛选相邻设备的方法，具体如下：
可选的，本发明提供的惯导协作定位方法还包括：确定惯导误差最小的相邻设备为第二设备。
本实施例中，相邻设备是到第一设备的距离小于第二预设距离的惯导设备，第二预设距离可以根据测距传感器的有效量程设定，或是根据实际测试设定，第二预设距离与上述第一预设距离可以相同，也可以不同，此处不做限定。
第二定位参数中的惯导误差越小，第一设备的目标惯性误差进入预设误差门限的速度越快，定位设备确定惯导误差最小的相邻设备为第二设备，能以最快速度计算得到满足实际要求的目标惯导误差，并确定目标惯导位置。
本发明还可以先确定距离第一设备小于预设距离的相邻设备为候选设备，再选取惯导误差最小的候选设备作为第二设备；本发明还可以确定距离并非最小或惯导误差并非最小的相邻设备为第二设备，此处不作限定。
可以理解的是，本发明提供的惯导协作定位方法同样适用于其他平面坐标系和三维坐标系。
为便于理解，下面对本发明提出的惯导协作定位方法进行仿真比较：
仿真对象：基本惯导定位法、现有技术中的惯导协作定位法、本发明中的惯导协作定位法；
仿真场景：设备A分别采用三种方法，从同样的起始位置开始，基于同样的惯导设备误差概率(正态分布平均值为0.5)，步长假设为0.8m，每前进50步校正一次惯导误差，周围存在一惯导误差概率正态分布平均值为0.1的设备B作为惯导协作设备，仿真步数为1000；
仿真结果：
基本惯导定位法作为基线参考，主要比较现有技术协作惯导定位法与本发明的协作惯导定位法(计算二次优化惯导位置)：
1、设备A的惯导位置与实际位置的方差均值：本发明方法得到的方差均值比现有技术降低72.8％；
2、两种方法的误差累积分布曲线如图4所示(误差＝每个定位点惯导位置与实际位置的方差开根号)，可见，本发明方法得到的惯导误差曲线性能最优，惯导误差最小。
可选的，设备A可以通过选取距离最近作为惯导协作设备，能进一步提高定位精度。
可选的，设备A可以选取惯导误差最小的相邻设备作为惯导协作设备，能加快降低惯导误差的速度。
基于上述惯导协作定位方法，本申请提供一种定位设备500，具有实现上述惯导协作定位方法的功能，请参阅图5，定位设备500包括：
获取模块501，用于获取第一定位参数、第二定位参数以及第一设备到第二设备的距离向量，第一定位参数包括第一设备的惯导位置以及惯导误差，第二定位参数包括第二设备的惯导位置以及惯导误差，距离向量包括第一设备到第二设备的距离以及测距信号到达角；
计算模块502，用于根据第一定位参数、第二定位参数以及距离向量，计算出第一目标定位参数，第一目标定位参数包括第一设备的目标惯导位置以及目标惯导误差；
判断模块503，用于判断第一设备的目标惯导误差是否不低于预设误差门限，若是，则触发计算模块502根据第一目标定位参数、当前第二定位参数 以及距离向量，按照第二预设算法计算出第二目标定位参数；将第一目标定位参数作为新的第一定位参数，并将第二目标定位参数作为新的第二定位参数，输出给计算模块502，若否，则触发输出模块504用于输出第一设备的目标惯导位置；
计算模块502还用于根据第一目标定位参数、当前第二定位参数以及距离向量，按照第二预设算法计算出第二目标定位参数，其中，第二目标定位参数包括第二设备的目标惯导位置以及目标惯导误差；将第一目标定位参数作为新的第一定位参数，并将第二目标定位参数作为新的第二定位参数，输出给计算模块502；
输出模块504用于输出第一设备的目标惯导位置。
本实施例中，当定位设备为第一设备或第二设备时，获取模块501可以包括测距传感器以及以下设备中的至少一个：陀螺仪，麦克风阵列或者天线阵列。获取模块501可以通过测距传感器实现测量第一设备到第二设备的距离向量的功能，并通过陀螺仪、麦克风阵列和/或天线阵列实现测量测距信号到达角的功能。当定位设备为第一设备、第二设备之外的其他设备时，获取模块501可以是接收器，通过接收第一设备和第二设备发送的定位参数以及距离向量来实现获取上述参数的功能。
可选的，计算模块502具体用于根据当前第二定位参数以及距离向量，计算第三定位参数，第三定位参数包括第一设备的惯导协作位置以及惯导协作误差，计算公式如下：
其中，EA1(m0)为第一设备的惯导协作位置的东向分量，NA1(m0)为第一设备的惯导协作位置的北向分量，EB(m1)为第二设备的惯导位置的东向分量，NB(m1)为第二设备的惯导位置的北向分量，DAB为第一设备到第二设备的距离，φAB为测距信号到达角，VarA1(m0)为第一设备的惯导协作误差，VarB(m1)为第二设备的惯导误差；
根据当前第一定位参数、第三定位参数以及距离向量，计算得到第一设备的目标惯导位置以及目标惯导误差，计算公式如下：
其中，为第一设备的目标惯导位置的东向分量，为第二设备的目标惯导位置的北向分量，为第一设备的目标惯导误差，EA(m0)为第一设备的惯导位置的东向分量，NA(m0)为第一设备的惯导位置的北向分量，VarA(m0)为第一设备的惯导误差。
再进一步的，计算模块502具体用于根据第一目标定位参数以及距离向量，计算得到第四定位参数，第四定位参数包括第二设备的惯导协作位置以及惯导协作误差，计算公式如下：
其中，EB1(m1)为第二设备的惯导协作位置的东向分量，NB1(m1)为第二设备的惯导协作位置的北向分量，VarB1(m1)为第二设备的惯导协作误差；
根据当前第二定位参数、第四定位参数以及距离向量，计算得到第二目标定位参数，计算公式如下：
其中，为第二设备的目标惯导位置的东向分量，为第二设备的目标惯导位置的北向分量，为第二设备的目标惯导误差。
可选的，定位设备500还包括：
第一确定模块601，用于确定到第一设备距离最近的相邻设备为第二设备，相邻设备为到第一设备的距离小于第一预设距离的惯导设备。
可选的，定位设备500还包括：
第二确定模块602，用于确定惯导误差最小的相邻设备为第二设备，相邻设备为到第一设备的距离小于第二预设距离的惯导设备。
基于上述惯导协作定位方法，本申请提供一种定位设备700，具有实现上述惯导协作定位方法的功能，定位设备700包括输入装置701、输出装置702、处理器703、存储器704和总线705；输入装置701、输出装置702、处理器703和存储器704之间通过总线705相互连接通信，其中，通过调用存储器704存储的操作指令，处理器703用于执行如下方法：
控制输入装置701获取第一定位参数、第二定位参数以及第一设备到第二设备的距离向量，第一定位参数包括第一设备的惯导位置以及惯导误差，第二定位参数包括第二设备的惯导位置以及惯导误差，距离向量包括第一设备到第二设备的距离以及测距信号到达角；
根据当前第一定位参数、当前第二定位参数以及距离向量，按照第一预设算法计算出第一设备的目标惯导位置以及目标惯导误差；
判断第一设备的目标惯导误差是否不低于预设误差门限，若是，则根据第一目标定位参数、当前第二定位参数以及距离向量，按照第二预设算法计算出第二目标定位参数；将第一目标定位参数作为新的第一定位参数，并将第二目标定位参数作为新的第二定位参数，若否，则控制输出装置702输出第一设备的目标惯导位置。
处理器703可以是通用处理器，包括中央处理器(Central Processing Unit，简称CPU)、网络处理器(Network Processor，简称NP)等；还可以是数字信号处理器(Digital Signal Processing，简称DSP)、专用集成电路(Application Specific Integrated Circuit，简称ASIC)、现场可编程门阵列(Field－Programmable Gate Array，简称FPGA)或者其他可编程逻辑器件等。
具体地，程序可以包括程序代码，程序代码包括计算机操作指令。存储器704可能包含随机存取存储器(Random Access Memory，简称RAM)，也可能还包括非易失性存储器(non-volatile memory)，例如至少一个磁盘存储器。
可选的，处理器703具体用于根据当前第二定位参数以及距离向量，计 算第三定位参数，第三定位参数包括第一设备的惯导协作位置以及惯导协作误差，计算公式如下：
其中，EA1(m0)为第一设备的惯导协作位置的东向分量，NA1(m0)为第一设备的惯导协作位置的北向分量，EB(m1)为第二设备的惯导位置的东向分量，NB(m1)为第二设备的惯导位置的北向分量，DAB为第一设备到第二设备的距离，φAB为测距信号到达角，VarA1(m0)为第一设备的惯导协作误差，VarB(m1)为第二设备的惯导误差；
处理器703根据当前第一定位参数、第三定位参数以及距离向量，计算得到第一设备的目标惯导位置以及目标惯导误差，计算公式如下：
其中，为第一设备的目标惯导位置的东向分量，为第二设备的目标惯导位置的北向分量，为第一设备的目标惯导误差，EA(m0)为第一设备的惯导位置的东向分量，NA(m0)为第一设备的惯导位置的北向分量，VarA(m0)为第一设备的惯导误差。
进一步的，处理器703具体用于根据第一目标定位参数以及距离向量，计算得到第四定位参数，第四定位参数包括第二设备的惯导协作位置以及惯导协作误差，计算公式如下：
其中，EB1(m1)为第二设备的惯导协作位置的东向分量，NB1(m1)为第二设备的惯导协作位置的北向分量，VarB1(m1)为第二设备的惯导协作误差；
处理器703根据当前第二定位参数、第四定位参数以及距离向量，计算得到第二目标定位参数，计算公式如下：
其中，为第二设备的目标惯导位置的东向分量，为第二设备的目标惯导位置的北向分量，为第二设备的目标惯导误差。
可选的，处理器703，还用于确定到第一设备距离最近的相邻设备为第二设备，相邻设备为到第一设备的距离小于第一预设距离的惯导设备。
可选的，处理器703，还用于确定惯导误差最小的相邻设备为第二设备，相邻设备为到第一设备的距离小于第二预设距离的惯导设备。
在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本 发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。
- 还没有人留言评论。精彩留言会获得点赞！