CN116559926A - 定位定姿的方法、装置以及设备 - Google Patents

Abstract

本申请提供了一种定位定姿的方法，定位定姿的方法应用于定位定姿领域。定位定姿的方法包括以下步骤：设备获取物体的第一状态。第一状态包括第一位置和第一姿态。设备以第一状态为起点，通过惯性导航获取物体的第二状态。第二状态包括第二位置和第二姿态。设备通过初始对准获取物体的第三姿态。设备以第三姿态和第二位置为起点，为物体提供惯性导航。在本申请中，通过初始对准获取的第三姿态替换第二姿态，可以提高定位定姿的准确性。

Description

定位定姿的方法、装置以及设备

技术领域

本申请涉及定位定姿领域，尤其涉及定位定姿的方法、装置以及设备。

背景技术

全球定位系统(global positioning system，GPS)是一种以人造地球卫星为基础的高精度无线电导航的定位定姿系统。GPS可以为汽车、手机等提供准确的位置和姿态信息。

但是，在某些场景中，GPS可能无法为其提供位置和姿态信息。例如，矿井、隧道或室内等。此时，可以通过惯性导航技术为物体定位定姿。具体地，惯性测量单元(inertialmeasurement unit，IMU)可以获取物体的角速度和加速度。加速度可以计算物体相对于初始位置的位移信息。角速度可以计算物体相对于初始姿态的姿态改变信息。通过姿态改变信息和初始姿态，可以得到物体的最新的姿态。通过初始位置、位移信息、初始姿态和姿态改变信息，可以得到物体的最新的位置。

但是，IMU具有零偏和随机游走等误差。误差会随时间积累。时间积累的误差会降低定位定姿的准确性。

发明内容

本申请提供了一种定位定姿的方法，通过初始对准获取的第三姿态替换第二姿态，可以提高定位定姿的准确性。

本申请第一方面提供了一种定位定姿的方法。定位定姿的方法可以应用于定位定姿的设备。定位定姿的设备简称为设备。定位定姿的方法包括以下步骤：设备获取物体的第一状态。第一状态包括第一位置和第一姿态。第一位置包括3种平移自由度中的一种或多种平移自由度的信息。在其中一种描述中，3种平移自由度包括X轴平移、Y轴平移和Z轴平移。第一姿态包括3种旋转自由度中的一种或多种旋转自由度的信息。在其中一种描述中，3种旋转自由度包括X轴旋转、Y轴旋转和Z轴旋转。设备以第一状态为起点，通过惯性导航获取物体的第二状态。第二状态包括第二位置和第二姿态。设备通过初始对准获取物体的第三姿态。设备以第三姿态和第二位置为起点，为物体提供惯性导航。具体地，设备以第三姿态和第二位置为起点，通过惯性导航获取物体的第三状态。第三状态包括第四位置和第四姿态。

在本申请中，第二姿态携带有时间积累的误差。通过初始对准校准第二姿态，可以提高定位定姿的准确性。

在第一方面的一种可选方式中，在物体以第一速度运动时，设备以第一状态为起点，通过惯性导航获取物体的第二状态。在物体以第二速度运动时，设备通过初始对准获取物体的第三姿态。第二速度小于第一速度。其中，在物体的运动速度较低时，可以通过初始对准中的粗对准获取第三姿态。此时，设备不需要依赖于外部的定位设备。定位设备包括卫星等。因此，本申请可以降低定位定姿的成本。

在第一方面的一种可选方式中，在物体静止时，设备通过初始对准获取物体的第三姿态。在物体运动时，设备以第一状态为起点，通过惯性导航获取物体的第二状态。其中，物体的运动速度越低，通过粗对准获取的第三姿态越准确。因此，在物体静止时完成初始对准，可以提高第三姿态的准确性，进而提高定位定姿的准确性。

在第一方面的一种可选方式中，在物体静止、非直线运动、或非匀速运动时，设备通过初始对准获取物体的第三姿态。

在第一方面的一种可选方式中，定位定姿的方法还包括以下步骤：设备通过第三姿态和第一姿态修正第二位置，得到第三位置。或，设备通过第三姿态修正第二位置，得到第三位置。设备以第三姿态和第三位置为起点，为物体提供惯性导航。其中，第二位置也携带有时间积累的误差。通过第三姿态和第一姿态、或第三姿态修正第二位置，可以提高定位定姿的准确性。

在第一方面的一种可选方式中，定位定姿的方法还包括以下步骤：设备获取物体从第一位置到第二位置之间的位移信息。设备通过第三姿态、第一姿态和位移信息修正第二位置，得到第三位置。其中，通过第三姿态、第一姿态和位移信息修正第二位置，可以提高定位定姿的准确性。

在第一方面的一种可选方式中，定位定姿的方法还包括以下步骤：设备获取物体从第一位置到第二位置之间的轨迹。设备根据位移信息修正轨迹。其中，从第一位置到第二位置的期间，物体的运动轨迹也会携带时间积累的误差。通过位移信息修正轨迹，可以提高轨迹的准确性，从而提高定位定姿的准确性。

在第一方面的一种可选方式中，定位定姿的方法还包括以下步骤：设备获取物体从第一位置到第二位置之间的轨迹。设备根据第三姿态和第一姿态修正轨迹。或，设备根据第三姿态修正轨迹。其中，从第一位置到第二位置的期间，物体的运动轨迹也会携带时间积累的误差。通过第三姿态和第一姿态、或第三姿态修正轨迹，可以提高轨迹的准确性，从而提高定位定姿的准确性。

在第一方面的一种可选方式中，设备通过IMU进行初始对准，以获取物体的第三姿态。IMU安装于运动单元。IMU相对运动单元静止。运动单元安装于物体。当物体静止时，运动单元相对物体运动。其中，当物体和IMU都静止时，通过粗对准获取的第三姿态的准确性较低。因此，通过增加运动单元，可以提高初始对准的准确性，进而提高定位定姿的准确性。

在第一方面的一种可选方式中，当物体运动时，运动单元相对物体静止。因此，本申请可以降低能耗。并且，当运动单元和物体同时运动时，定位定姿的算法更为复杂。因此，本申请可以节约处理资源。

在第一方面的一种可选方式中，定位定姿的方法还包括以下步骤：设备通过IMU获取第一信息。第一信息用于表征物体处于运动或静止状态。在另一种方式中，第一信息可以用于设备确定进行惯性导航还是初始对准。设备也可以从传感器获取第一信息。传感器安装于物体的运动机构。相比于传感器的方案，本申请可以降低定位定姿的成本。

在第一方面的一种可选方式中，定位定姿的方法还包括以下步骤：当第一信息表征物体处于运动状态时，则设备控制运动单元静止。当第一信息表征物体处于静止状态时，则设备控制运动单元运动。在另一种方式中，设备可以从运动传感器获取物体的状态。运动传感器安装于物体的运动机构。相比于传感器方案，本申请可以降低定位定姿的成本。

在第一方面的一种可选方式中，设备通过陀螺寻北中的多位置法和连续旋转法获取物体的第三姿态。相比于单位置法，多位置法和连续旋转法可以提高初始对准的准确性，进而提高定位定姿的准确性。

本申请第二方面提供了一种定位定姿的装置。定位定姿的装置包括第一获取模块、第二获取模块、第三获取模块和惯性导航模块。第一获取模块用于获取物体的第一状态。第一状态包括第一位置和第一姿态。第二获取模块用于以第一状态为起点，通过惯性导航获取物体的第二状态。第二状态包括第二位置和第二姿态。第三获取模块用于通过初始对准获取物体的第三姿态。惯性导航模块用于以第三姿态和第二位置为起点，为物体提供惯性导航。

在第二方面的一种可选方式中，第二获取模块用于在物体以第一速度运动时，以第一状态为起点，通过惯性导航获取物体的第二状态。第三获取模块用于在物体以第二速度运动时，通过初始对准获取物体的第三姿态，第二速度小于第一速度。

在第二方面的一种可选方式中，第三获取模块用于在物体静止时，通过初始对准获取物体的第三姿态。第二获取模块用于在物体运动时，以第一状态为起点，通过惯性导航获取物体的第二状态。

在第二方面的一种可选方式中，定位定姿的装置还包括第一修正模块。第一修正模块用于通过第三姿态和第一姿态、或第三姿态修正第二位置，得到第三位置。惯性导航模块用于以第三姿态和第三位置为起点，为物体提供惯性导航。

在第二方面的一种可选方式中，第一获取模块还用于获取物体从第一位置到第二位置之间的位移信息。修正模块用于通过第三姿态、第一姿态和位移信息修正第二位置，得到第三位置。

在第二方面的一种可选方式中，第一获取模块还用于获取物体从第一位置到第二位置之间的轨迹。定位定姿的装置还包括第一修正模块。第一修正模块用于根据位移信息修正轨迹。

在第二方面的一种可选方式中，第一获取模块还用于获取物体从第一位置到第二位置之间的轨迹。定位定姿的装置还包括第二修正模块。第二修正模块用于根据第三姿态和第一姿态、或第三姿态修正轨迹。

在第二方面的一种可选方式中，第三获取模块用于通过IMU进行初始对准，以获取物体的第三姿态。其中，IMU安装于运动单元。IMU相对运动单元静止。运动单元安装于物体。当物体静止时，运动单元相对物体运动。

在第二方面的一种可选方式中，当物体运动时，运动单元相对物体静止。

在第二方面的一种可选方式中，第一获取模块还用于通过IMU获取第一信息，第一信息用于表征物体处于运动或静止状态。

在第二方面的一种可选方式中，定位定姿的装置还包括控制模块。控制模块用于当第一信息表征物体处于运动状态时，则控制运动单元静止。控制模块还用于当第一信息表征物体处于静止状态时，则控制运动单元运动。

在第二方面的一种可选方式中，第三获取模块用于通过陀螺寻北中的多位置法和连续旋转法获取物体的第三姿态。

本申请第三方面提供了一种定位定姿的设备。设备包括处理器和IMU。处理器用于获取物体的第一状态。第一状态包括第一位置和第一姿态。IMU用于以第一状态为起点，为物体提供惯性导航，得到惯性导航数据。处理器还用于根据惯性导航数据和第一状态得到物体的第二状态。第二状态包括第二位置和第二姿态。IMU还用于通过初始对准获取物体的第三姿态。IMU还用于以第三姿态和第二位置为起点，为物体提供惯性导航。

在第三方面的一种可选方式中，IMU用于在物体以第一速度运动时，以第一状态为起点，为物体提供惯性导航，得到惯性导航数据。IMU用于在物体以第二速度运动时，通过初始对准获取物体的第三姿态。第二速度小于第一速度。

在第三方面的一种可选方式中，IMU用于在物体静止时，通过初始对准获取物体的第三姿态。IMU用于在物体以第一速度运动时，以第一状态为起点，为物体提供惯性导航，得到惯性导航数据。

在第三方面的一种可选方式中，处理器还用于通过第三姿态和第一姿态、或第三姿态修正第二位置，得到第三位置。IMU用于以第三姿态和第三位置为起点，为物体提供惯性导航。

在第三方面的一种可选方式中，处理器还用于获取物体从第一位置到第二位置之间的位移信息。处理器用于通过第三姿态、第一姿态和位移信息修正第二位置，得到第三位置。

在第三方面的一种可选方式中，处理器还用于获取物体从第一位置到第二位置之间的轨迹。处理器用于根据位移信息修正轨迹。

在第三方面的一种可选方式中，处理器还用于获取物体从第一位置到第二位置之间的轨迹。处理器还用于根据第三姿态和第一姿态、或第三姿态修正轨迹。

在第三方面的一种可选方式中，设备还包括运动单元。IMU安装于运动单元。IMU相对运动单元静止。运动单元安装于物体。当物体静止时，运动单元相对物体运动。

在第三方面的一种可选方式中，当物体运动时，运动单元相对物体静止。

在第三方面的一种可选方式中，IMU还用于获取第一信息，第一信息用于表征物体处于运动或静止状态。

在第三方面的一种可选方式中，处理器还用于当第一信息表征物体处于运动状态时，则控制运动单元静止。处理器还用于当第一信息表征物体处于静止状态时，则控制运动单元运动。

在第三方面的一种可选方式中，IMU用于通过陀螺寻北中的多位置法和连续旋转法获取物体的第三姿态。

本申请第四方面提供了一种计算机存储介质，其特征在于，所述计算机存储介质中存储有指令，所述指令在计算机上执行时，使得所述计算机执行如第一方面或第一方面任意一种实施方式所述的方法。

本申请第五方面提供了一种计算机程序产品，其特征在于，所述计算机程序产品在计算机上执行时，使得所述计算机执行如第一方面或第一方面任意一种实施方式所述的方法。

附图说明

图1为本申请实施例中提供的定位定姿的方法的第一个流程示意图；

图2为本申请实施例中提供的轨迹修正的结构示意图；

图3为本申请实施例中提供的3个坐标系的结构示意图；

图4为本申请实施例中提供的矿井的结构示意图；

图5为本申请实施例中提供的液压支架的结构示意图；

图6为本申请实施例中提供的定位定姿的方法的第二个流程示意图；

图7为本申请实施例中提供的定位定姿的设备的第一结构示意图；

图8为本申请实施例中提供的定位定姿的设备的第二结构示意图；

图9为本申请实施例中提供的定位定姿的设备的第三结构示意图；

图10为本申请实施例中提供的定位定姿的装置的结构示意图。

具体实施方式

本申请提供了一种定位定姿的方法、装置以及设备，通过初始对准获取的第三姿态替换第二姿态，可以提高定位定姿的准确性。应理解，本申请中使用的“第一”、“第二”、等仅用于区分描述的目的，而不能理解为指示或暗示相对重要性，也不能理解为指示或暗示顺序。另外，为了简明和清楚，本申请多个附图中重复参考编号和/或字母。重复并不表明各种实施例和/或配置之间存在严格的限定关系。在未特别说明时，本申请中的运动或静止是指相对于地球运动或静止。

本申请中提供的定位定姿的方法应用于定位定姿领域。在定位定姿领域中，当GPS无法提供位置信息和姿态信息时，定位定姿的设备可以通过IMU为物体提供惯性导航。但是，IMU具有零偏和随机游走等误差。误差会随时间积累。时间积累的误差会降低定位定姿的准确定。

为此，本申请提供了一种定位定姿的方法。定位定姿的方法可以应用于定位定姿的设备。定位定姿的设备简称为设备。图1为本申请实施例中提供的定位定姿的方法的第一个流程示意图。如图1所示，定位定姿的方法包括以下步骤。

在步骤101中，设备获取物体的第一状态，第一状态包括第一位置和第一姿态。第一位置包括3种平移自由度中的一种或多种平移自由度的信息。第一姿态包括3种旋转自由度中的一种或多种旋转自由度的信息。在后续的示例中，将以第一状态包括3种平移自由度的信息和3种旋转自由度的信息为例进行描述。设备可以通过初始对准或人工输入等方式获取第一状态。按照物体是否运动，初始对准包括粗对准和精对准。在物体相对地球运动时，通过精对准获取位置和姿态信息。精对准可以依赖外部定位设备，例如GPS卫星。在物体相对地球静止或缓慢运动时，通过粗对准获取姿态信息。粗对准依赖地球的重力矢量和地球自转角速率矢量。

在步骤102中，设备以第一状态为起点，通过惯性导航获取物体的第二状态。第二状态包括第二位置和第二姿态。惯性导航是依据牛顿惯性原理，利用IMU来测量物体的加速度和角速度。加速度可以计算物体相对于初始位置的位移信息。角速度可以计算物体相对于初始姿态的姿态改变信息。通过姿态改变信息、位移信息、第一姿态和第一位置，可以得到物体的最新的第二位置。通过姿态改变信息和第一姿态，可以得到物体的最新的第二姿态。

在步骤103中，设备通过初始对准获取物体的第三姿态。第三姿态包括航向角、横滚角和俯仰角。航向角、横滚角和俯仰角是3种旋转自由度的一种表达方式。初始对准包括粗对准和精对准。粗对准包括陀螺寻北、加速度计测量地球重力分量等方法。下面以陀螺寻北中的单位置法为例，对设备获取航向角的过程进行描述。IMU包括三个陀螺仪和三个加速度计。三个陀螺仪中的水平轴陀螺仪测量地球旋转速度的两个投影值。一个投影值包括地球自转角速度投影到地球表面任一点指向北极方向(即北向)的角速度值。另一个投影值包括北向的角速度值再进一步投影到水平轴陀螺仪敏感轴的数值。第一次投影通过纬度角，第二次投影通过北向角(即水平轴陀螺仪敏感轴与北向的夹角)。通过两个投影值可以求解得到纬度角和北向角。物体的航向角可以通过北向角计算得到。在地理坐标系中，北向角等于航向角。

设备可以通过三个加速度计中的水平轴加速度计获取横滚角和俯仰角。具体地，设备通过水平轴加速度计测量地球的重力加速度分量。加速度分量指的是当地重力加速度通过横滚角、俯仰角投影到水平轴加速度计敏感轴的加速度值。横滚角、俯仰角可以通过解算该加速度值得到。当物体平行于水平面时，横滚角和俯仰角为0。

应理解，前述只是对设备获取第三姿态的一种示例性描述。在实际应用中，设备还可以通过其它方式获取第三姿态。例如，设备可以通过多位置法或连续旋转法获取航向角。相比于单位置法，多位置法和连续旋转法可以降低陀螺仪的漂移误差，提高获取的第三姿态的准确性。

根据前面的描述可知，本申请中的定位定姿的方法可以应用于没有GPS的场景。因此，本申请实施例可以通过自主式对准获取第三姿态。其中，初始对准包括自主式对准和非自主式对准。自主式对准不需要依赖外部定位设备。非自主式对准依赖外部定位设备。

在步骤104中，设备以第三姿态和第二位置为起点，为物体提供惯性导航。在步骤102中，设备通过惯性导航获取了物体的第二姿态。第二姿态携带有惯性导航的误差。在步骤103中，设备通过初始对准获取了物体的第三姿态。设备以第三姿态和第二位置为起点，为物体提供惯性导航。具体地，设备以第三姿态和第二位置为起点，通过惯性导航获取物体的第三状态。第三状态包括第四位置和第四姿态。关于惯性导航的描述，可以参考前述步骤102中的描述。在获取第三状态后，设备可以回到步骤103。设备通过初始对准获取物体的第五姿态。设备重复执行步骤104，在步骤104中，设备以第五姿态和第四位置为起点，为物体提供惯性导航。因此，在实际应用中，设备可以不断重复执行步骤103和步骤104。

在本申请中，设备可以通过陀螺寻北和加速度计测倾角得到第三姿态。陀螺寻北和加速度计测倾角都是测量地球物体特征的运动学感知，无需积分解算过程。因此，第三姿态没有时间累积误差。因此，通过初始对准校准第二姿态，可以提高定位定姿的准确性。

根据前述步骤101的描述可知，在物体相对于地球静止缓慢运动时，设备可以通过粗对准获取第三姿态。此时，设备可以不需要依赖于外部定位设备。为此，设备可以在物体的运动速度较大时，进行惯性导航。设备可以在物体的运动速度较小时，进行初始对准。具体地，在步骤102中，设备可以在物体以第一速度运动时，通过惯性导航获取物体的第二状态。在步骤103时，设备可以在物体以第二速度运动时，通过初始对准获取物体的第三姿态。第二速度小于第一速度。应理解，第一速度或第二速度可以是指一个速度范围。

根据前述步骤101的描述可知，在物体静止时，通过粗对准获取的第三姿态的准确性更高。因此，设备可以在物体的运动时，进行惯性导航。设备可以在物体的静止时，进行初始对准。具体地，在步骤102中，在物体静止时，设备通过初始对准获取物体的第三姿态。在步骤101中，在物体运动时，设备通过惯性导航获取物体的第二状态。

应理解，在物体运动时，设备通过惯性导航获取物体的第二状态并不一定是指设备在物体运动时得到第二状态。例如，物体在第一时间段处于运动状态。设备在第一时间段通过IMU为物体提供惯性导航。IMU在第一时间段的终止时刻输出惯性导航数据。物体在第二时间段静止。第二时间段和第一时间段衔接。第二时间段在第一时间段之后。在第二时间段，设备通过第一状态和惯性导航数据得到第二状态。此时，物体的状态为静止。

根据前面的描述可知，设备可以在物体运动时进行惯性导航，在物体静止时进行初始对准。此时，设备需要获取物体是处于静止状态或运动状态的信息。在实际应用中，设备可以通过以下几种方式获取前述信息。在一种方式中，物体存在运动部件，例如气缸、马达、轮子等。在运动部件上安装有传感器。设备根据传感器的信息确定物体处于静止状态或运动状态。在另一种方式中，物体存在控制运动部件运动的控制器。运动部件根据控制器的控制信号运动或静止。此时，设备可以从控制器接收控制信号。设备根据控制信号确定物体处于静止状态或运动状态。在另一种方式中，IMU输出第一信息。设备根据第一信息确定物体处于静止状态或运动状态。

在前述步骤102中，设备得到了第二姿态和第二位置。第二位置也可能携带有时间积累的误差。因此，设备可以修正第二位置。例如，设备可以根据第三姿态和第一姿态、或第三姿态修正第二位置，得到第三位置。在一种方式中，修正算法可以是前向－后向平滑算法。前向－后向平滑算法包括前向滤波器和后向滤波器。前向滤波器是标准的卡尔曼滤波。后向滤波器是一个在时间上从后往前工作的卡尔曼滤波。平滑估计的结果是两个滤波结果的加权组合。

在得到第三位置后，在步骤104中，设备以第三姿态和第三位置为起点，为物体提供惯性导航。应理解，前述修正算法只是一种或多种示例。例如，设备上安装有神经网络模型。在输入第三姿态、第一姿态和第一位置后，神经网络模型输出第三位置。

在实际应用中，为了提高第三位置的准确性，设备可以获取物体从第一位置到第二位置之间的位移信息。例如，物体的运动部件上安装有传感器。传感器上传了物体的位移信息。设备通过第三姿态、第一姿态和位移信息修正第二位置，得到第三位置。

根据前述的描述可知，设备可以通过第三姿态修正第二位置。但是，在某些场景中，用户还需要关注物体的运动轨迹。由于第二位置可能携带误差，物体从第一位置到第二位置之间的轨迹也可能携带误差。设备可以修正物体的运动轨迹。例如，图2为本申请实施例中提供的轨迹修正的结构示意图。如图2所示，A点为物体的第一位置。B点为物体的第二位置。B′为第三位置。物体原来的运动轨迹为轨迹201。在修正轨迹201后，设备得到了轨迹202。设备可以根据第三姿态、第一姿态、或位移信息中的一项或多项内容修正轨迹。本申请实施例不限定设备修正轨迹的具体算法。

根据前面的描述可知，为了提高获取的第三姿态的准确性，设备可以在物体静止时获取第三姿态。为了进一步提高获取的第三姿态的准确性，IMU可以相对于物体运动。此时，IMU安装于运动单元。IMU相对于运动单元静止。运动单元安装于物体。在物体静止时，运动单元相对于物体运动。例如，运动单元是一个转盘。在物体静止，运单单元在物体上做旋转运动。

在物体运动时，设备需要为物体进行惯性导航。此时，设备可以无需通过初始对准获取物体的姿态信息。因此，物体运动时，运动单元可以相对于物体静止，从而降低能耗。并且，当运动单元和物体同时运动时，设备进行惯性导航的算法更为复杂。通过让运动单元相对于物体静止，可以节约处理资源。

在前面的描述可知，设备可以通过多种方式确定物体处于静止状态或运动状态。例如，设备可以根据IMU输出的第一信息确定物体是处于静止状态或运动状态。当第一信息表征物体处于运动状态时，则设备可以控制运动单元静止。当第一信息表征物体处于静止状态时，则设备可以控制运动单元运动。

在前述步骤103中，设备通过初始对准获取了物体的第三姿态。第三姿态包括东北天坐标系里的航向角、横滚角和俯仰角。东北天坐标系以物体的北方向、物体的东方向和物体指向天空的方向作为坐标系的三轴。在实际应用中，第三姿态可以是位于任意自定义坐标系中姿态信息。因此，设备可以将东北天坐标系中第三姿态转换至任意自定义坐标系。图3为本申请实施例中提供的3个坐标系的结构示意图。如图3所示，自定义坐标系的三轴为xs、ys和zs。物体的坐标系的三轴为x、y和z。东北天坐标系的三轴为东、北和天。在东北天坐标系中，物体绕天轴旋转为航向角。物体绕东旋转为俯仰角。物体绕北轴旋转为航向角。在前述步骤103中，设备确定了物体在东北天坐标系中的航向角、横滚角和俯仰角。设备获取自定义坐标系与东北天坐标系的夹角。应用姿态转换公式和夹角，设备物体可以将东北天坐标系下的位置和姿态变换自定义坐标系中。

在前述步骤102中，设备可以获取物体的y轴在东北平面内的投影与北轴的第一夹角。东北平面是指东轴和北轴组成的平面。设备可以获取物体的y轴在东天平面内的投影与东轴的第二夹角。设备可以获取物体的y轴在北天平面内的投影与天轴的第三夹角。第一夹角对应物体在东北天坐标系下的航向角。第二夹角对应物体在东北天坐标系下的俯仰角。第三夹角对应物体在东北天坐标系下的横滚角。类似地，设备也可以根据物体的x轴或z轴的投影得到物体在东北天坐标系下的航向角、俯仰角和横滚角。

本申请中的定位定姿的方法可以应用于矿井、隧道或室内等。下面矿井为例，对本申请中提供的定位定姿的方法进行示例性描述。图4为本申请实施例中提供的矿井的结构示意图。如图4所示，矿井下包括刮板运输机401、采煤机402和液压支架403～406。刮板运输机401也称为轨道。液压支架403～406用于推动刮板运输机401，使得刮板运输机401的位置发送改变。

图5为本申请实施例中提供的液压支架的结构示意图。如图5所示，液压支架包括第一运动部件503、第二运动部件507、顶板508和传感器504。第一运动部件503包括第一推杆502和第一支架501。第一推杆502可以相对于第一支架501伸缩。第二运动部件507包括第二推杆505和第二支架506。第二推杆505可以相对于第二支架506伸缩。顶板508安装于第二推杆505之上。第二支架506安装于第一支架501之上。第一推杆502和刮板运输机401连接。定位定姿的设备511安装于第一支架501上。定位定姿的设备511包括IMU 509、运动单元510和处理器(图中未示出)。传感器504安装于第一推杆502或第一支架501上。

液压支架可以推动或拉动刮板运输机401，使得刮板运输机401的位置发送改变。下面以液压支架推动刮板运输机401，使得刮板运输机401向南方向运动为例进行描述。液压支架包括以下五个动作。五个动作包括支护、支护中推溜、降架、移架和升架。

在支护时，第一推杆502处于收缩状态。第二推杆505处于伸长状态。此时，顶板508接触上壁(图中未示出)，第一支架501接触下壁(图中未示出)。液压支架处于受力状态，无法在北轴和东轴方向正常移动。在支护中推溜时，第一推杆502从收缩状态转变为伸长状态。第一推杆502推动刮板运输机401往南移动。此时，第二推杆505仍处于伸长状态。在降架时，第二推杆505从伸长状态转变为收缩状态。在移架时，第一推杆502从伸长状态转变为收缩状态。此时，由于第二推杆505处于收缩状态，液压支架可以在北轴和东轴方向移动。第一推杆502拉动液压支架往南移动。在升架时，第二推杆505收缩状态转变为伸长状态。此时，第一推杆502处于收缩状态。按照顺序重复执行上述五个动作，则液压支架可以一直推动拉动刮板运输机401往南移动。

在前述5个动作中，在移架时，第一支架501处于移动状态。在支护、支护中推溜、降架或升架时，第一支架501处于静止状态。第一支架501可以作为前述需要定位定姿的方法中的物体。图6为本申请实施例中提供的定位定姿的方法的第二个流程示意图。如图6所示，定位定姿的方法包括以下步骤。

在步骤601中，在物体运动时，设备以第一状态为起点，通过惯性导航获取物体的第二状态。第二状态包括第二位置和第二姿态。第一状态包括第一位置和第一姿态。根据前面的描述可知，设备可以通过多种方式确定物体处于静止状态或运动状态。例如，设备可以根据IMU的第一信息确定物体处于静止状态或运动状态。第一信息可以包括IMU的加速度测量数据和/或角速度测量数据。当第一信息满足一定条件时，设备确定物体处于静止状态或运动状态。例如，当加速度测量数据的均方根小于阈值时，设备确定物体处于静止状态。当加速度测量数据的均方根大于或等于阈值时，设备确定物体处于运动状态。阈值的数值可以由IMU中加速度计或陀螺仪的性能参数确定。第一支架501处于运动状态时，运动单元510相对第一支架501静止。每次执行步骤601时，IMU 509回到相同的初始位置。关于设备获取第一状态的描述，可以参考前述图1中步骤101的描述。关于惯性导航的描述，可以参考前述图1中步骤102的描述。

在步骤602中，在物体静止时，设备通过初始对准获取物体的第三姿态。设备将第三姿态作为第一姿态，将第二位置作为第一位置。根据前述的描述可知，设备可以通过多位置法来获取第三姿态。下面以四位置法为例进行描述。运动单元510可以带动IMU 509相对第一支架501运动。因此，在IMU 509处于4个相互正交的位置时，设备分别获取IMU509输出的加速度测量数据和角速度测量数据。设备根据加速度测量数据和角速度测量数据得到第三姿态。第三姿态包括航向角、横滚角和俯仰角。设备也可以通过连续旋转法获取第三姿态。例如当运动单元510为转盘时，转盘匀速转动。在转盘匀速转动的区间，IMU509持续输出加速度测量数据和角速度测量数据。设备根据加速度测量数据和角速度测量数据得到第三姿态。关于设备通过初始对准获取第三姿态的描述，可以参考前述图1中步骤103的描述。设备将第三姿态作为第一姿态，将第二位置作为第一位置。回到步骤601。

应理解，在使用多位置法时，运动单元510可以间隙性的相对第一支架501运动。例如，在第一时间段，IMU 509处于第一位置。在第二时间段，运动单元510将IMU 509从第一位置旋转到第二位置。在第三时间段，IMU 509处于第二位置。在第四时间段，运动单元510将IMU 509从第二位置旋转到第三位置。在第五时间段，IMU 509处于第三位置。在第六时间段，运动单元510将IMU 509从第三位置旋转到第四位置。在第七时间段，IMU509处于第四位置。在第八时间段，运动单元510将IMU 509从第四位置旋转到第一位置。在不同的四个位置上，IMU 509分别输出加速度测量数据和角速度测量数据。四个位置包括第一位置、第二位置、第三位置和第四位置。

根据前述的描述可知，设备可以获取第一位置到第二位置之间的位移信息。例如，在图5中，传感器504安装于第一推杆502或第一支架501。因此，传感器504可以获取第一支架501在运动时的位移信息。设备根据位移信息修正第二位置或轨迹。

应理解，在图5中，定位定姿的设备511也可以安装于顶板508。此时，顶板508可以作为需要定位定姿的物体。在前述5个动作中，在降架、移架或升架时，顶板508处于运动状态。在支护或支护中推溜时，顶板508处于静止状态。

前面对本申请中的定位定姿的方法进行描述，下面对本申请中的定位定姿的设备进行描述。图7为本申请实施例中提供的定位定姿的设备的第一结构示意图。如图7所示，定位定姿的设备700包括IMU 701和处理器702。

处理器702可以是中央处理器(central processing unit，CPU)，网络处理器(network processor，NP)或者CPU和NP的组合。处理器702还可以进一步包括硬件芯片或其他通用处理器。上述硬件芯片可以是专用集成电路(application specific integratedcircuit，ASIC)，可编程逻辑器件(programmable logic device，PLD)或其组合。

处理器702用于获取物体的第一状态。第一状态包括第一位置和第一姿态。IMU701用于以第一状态为起点，为物体提供惯性导航，得到惯性导航数据。处理器702还用于根据惯性导航数据和第一状态得到物体的第二状态。第二状态包括第二位置和第二姿态。IMU701还用于通过初始对准获取物体的第三姿态。IMU 701还用于以第三姿态和第二位置为起点，为物体提供惯性导航。

在其它实施例中，定位定姿的设备700还可以包括存储器。存储器用于存储第一姿态。存储器可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(read-only memory，ROM)、可编程只读存储器(programmable ROM，PROM)、可擦除可编程只读存储器(erasable PROM，EPROM)、或闪存等。易失性存储器可以是随机存取存储器(random access memory，RAM)。

在其它实施例中，定位定姿的设备700还可以包括收发器。收发器可以是无线射频模块或光收发模块。收发器可以用于向其它设备传输定位定姿的结果。定位定姿的结果包括第二姿态或第三姿态等。收发器可以用于从其它设备接收第一姿态。

应理解，关于定位定姿的设备700的描述，可以参考前述定位定姿的方法的描述。例如，处理器702获取物体的第一状态的描述可以参考前述步骤101中的描述。例如，图8为本申请实施例中提供的定位定姿的设备的第二结构示意图。如图8所示，在图7的基础上，定位定姿的设备还包括运动单元801。IMU 701安装于运动单元801。IMU 701相对运动单元801静止。在物体静止，运动单元801相对物体运动。在物体运动时，运动单元801相对物体静止。

应理解，在实际应用中，IMU 701可以包括多个子IMU。例如，图9为本申请实施例中提供的定位定姿的设备的第三结构示意图。如图9所示，在图7的基础上，IMU 701包括第一子IMU 901和第二子IMU 902。第一子IMU 901用于实现对物体的惯性导航。第二子IMU 902用于通过初始对准获取第三姿态。例如，在图1所示的定位定姿的方法中，第二子IMU 901可以用于执行步骤103。第二子IMU 902用于执行104。

应理解，根据前述步骤103的描述可知，只使用IMU中的部分加速度计或陀螺仪便可以通过初始对准获取物体的第三姿态。因此，第二IMU 902可以替换为其它可以实现初始对准的传感器件，从而降低定位定姿的设备的成本。

前面对本申请中的定位定姿的设备进行描述，下面对本申请中的定位定姿的装置进行描述。图10为本申请实施例中提供的定位定姿的装置的结构示意图。如图10所示，定位定姿的装置1000包括第一获取模块1001、第二获取模块1002、第三获取模块1003和惯性导航模块1004。第一获取模块1001用于获取物体的第一状态。第一状态包括第一位置和第一姿态。第二获取模块1002用于以第一状态为起点，通过惯性导航获取物体的第二状态。第二状态包括第二位置和第二姿态。第三获取模块1003用于通过初始对准获取物体的第三姿态。惯性导航模块1004用于以第三姿态和第二位置为起点，为物体提供惯性导航。

以上，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。

Claims (25)

1.一种定位定姿的方法，其特征在于，包括：

获取物体的第一状态，所述第一状态包括第一位置和第一姿态；

以所述第一状态为起点，通过惯性导航获取所述物体的第二状态，所述第二状态包括第二位置和第二姿态；

通过初始对准获取所述物体的第三姿态；

以所述第三姿态和所述第二位置为起点，为所述物体提供惯性导航。

2.根据权利要求1所述的方法，其特征在于，

所述以所述第一状态为起点，通过惯性导航获取所述物体的第二状态包括：在所述物体以第一速度运动时，以所述第一状态为起点，通过惯性导航获取所述物体的所述第二状态；

所述通过初始对准获取所述物体的第三姿态包括：在所述物体以第二速度运动时，通过初始对准获取所述物体的所述第三姿态，所述第二速度小于所述第一速度。

3.根据权利要求1所述的方法，其特征在于，

所述通过初始对准获取所述物体的第三姿态包括：在所述物体静止时，通过初始对准获取所述物体的所述第三姿态；

所述以所述第一状态为起点，通过惯性导航获取所述物体的第二状态包括：在所述物体运动时，以所述第一状态为起点，通过惯性导航获取所述物体的所述第二状态。

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

通过所述第三姿态和所述第一姿态、或所述第三姿态修正所述第二位置，得到第三位置；

所述以所述第三姿态和所述第二位置为起点，为所述物体提供惯性导航包括：以所述第三姿态和所述第三位置为起点，为所述物体提供惯性导航。

5.根据权利要求4所述的方法，其特征在于，所述方法还包括：

获取所述物体从所述第一位置到所述第二位置之间的位移信息；

所述通过所述第三姿态和所述第一姿态、或所述第三姿态修正所述第二位置，得到第三位置包括：通过所述第三姿态、所述第一姿态和所述位移信息修正所述第二位置，得到所述第三位置。

6.根据权利要求5所述的方法，其特征在于，所述方法还包括：

获取所述物体从所述第一位置到所述第二位置之间的轨迹；

根据所述位移信息修正所述轨迹。

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

获取所述物体从所述第一位置到所述第二位置之间的轨迹；

根据所述第三姿态和所述第一姿态、或所述第三姿态修正所述轨迹。

8.根据权利要求1至7中任意一项所述的方法，其特征在于，所述通过初始对准获取所述物体的第三姿态包括：通过惯性测量单元IMU进行初始对准，以获取所述物体的所述第三姿态；

其中，所述IMU安装于运动单元，所述运动单元安装于所述物体；

当所述物体静止时，所述运动单元相对所述物体运动。

9.根据权利要求8所述的方法，其特征在于，当所述物体运动时，所述运动单元相对所述物体静止。

10.根据权利要求9所述的方法，其特征在于，所述方法还包括：

通过所述IMU获取第一信息，所述第一信息用于表征所述物体处于运动或静止状态。

11.根据权利要求10所述的方法，其特征在于，所述方法还包括：

当所述第一信息表征所述物体处于运动状态时，则控制所述运动单元静止；

当所述第一信息表征所述物体处于静止状态时，则控制所述运动单元运动。

12.根据权利要求1至11中任意一项所述的方法，其特征在于，

所述通过初始对准获取所述物体的第三姿态包括：通过陀螺寻北中的多位置法和连续旋转法获取所述物体的所述第三姿态。

13.一种定位定姿的装置，其特征在于，包括：

第一获取模块，用于获取物体的第一状态，所述第一状态包括第一位置和第一姿态；

第二获取模块，用于以所述第一状态为起点，通过惯性导航获取所述物体的第二状态，所述第二状态包括第二位置和第二姿态；

第三获取模块，用于通过初始对准获取所述物体的第三姿态；

惯性导航模块，用于以所述第三姿态和所述第二位置为起点，为所述物体提供惯性导航。

14.根据权利要求13所述的装置，其特征在于，

所述第二获取模块用于以所述第一状态为起点，通过惯性导航获取所述物体的第二状态包括：所述第二获取模块用于在所述物体以第一速度运动时，以所述第一状态为起点，通过惯性导航获取所述物体的所述第二状态；

所述第三获取模块用于通过初始对准获取所述物体的第三姿态包括：所述第三获取模块用于在所述物体以第二速度运动时，通过初始对准获取所述物体的所述第三姿态，所述第二速度小于所述第一速度。

15.根据权利要求13所述的装置，其特征在于，

所述第三获取模块用于通过初始对准获取所述物体的第三姿态包括：所述第三获取模块用于在所述物体静止时，通过初始对准获取所述物体的所述第三姿态；

所述第二获取模块用于以所述第一状态为起点，通过惯性导航获取所述物体的第二状态包括：所述第二获取模块用于在所述物体运动时，以所述第一状态为起点，通过惯性导航获取所述物体的所述第二状态。

16.根据权利要求13至15中任意一项所述的装置，其特征在于，所述装置还包括：

第一修正模块，用于通过所述第三姿态和所述第一姿态、或所述第三姿态修正所述第二位置，得到第三位置；

所述惯性导航模块用于以所述第三姿态和所述第二位置为起点，为所述物体提供惯性导航包括：所述惯性导航模块用于以所述第三姿态和所述第三位置为起点，为所述物体提供惯性导航。

17.根据权利要求16所述的装置，其特征在于，

所述第一获取模块还用于获取所述物体从所述第一位置到所述第二位置之间的位移信息；

所述第一修正模块用于通过所述第三姿态和所述第一姿态、或所述第三姿态修正所述第二位置，得到第三位置包括：所述修正模块用于通过所述第三姿态、所述第一姿态和所述位移信息修正所述第二位置，得到所述第三位置。

18.根据权利要求17所述的装置，其特征在于，

所述第一获取模块还用于获取所述物体从所述第一位置到所述第二位置之间的轨迹；

所述装置还包括：

第一修正模块，用于根据所述位移信息修正所述轨迹。

19.根据权利要求13至17中任意一项所述的装置，其特征在于，

所述第一获取模块还用于获取所述物体从所述第一位置到所述第二位置之间的轨迹；

所述装置还包括：

第二修正模块，用于根据所述第三姿态和所述第一姿态、或所述第三姿态修正所述轨迹。

20.根据权利要求13至19中任意一项所述的装置，其特征在于，所述第三获取模块用于通过初始对准获取所述物体的第三姿态包括：所述第三获取模块用于通过惯性测量单元IMU进行初始对准，以获取所述物体的所述第三姿态；

其中，所述IMU安装于运动单元，所述运动单元安装于所述物体；

当所述物体静止时，所述运动单元相对所述物体运动。

21.根据权利要求20所述的装置，其特征在于，当所述物体运动时，所述运动单元相对所述物体静止。

22.根据权利要求21所述的装置，其特征在于，

所述第一获取模块还用于通过所述IMU获取第一信息，所述第一信息用于表征所述物体处于运动或静止状态。

23.根据权利要求22所述的装置，其特征在于，所述装置还包括：

控制模块，用于当所述第一信息表征所述物体处于运动状态时，则控制所述运动单元静止，当所述第一信息表征所述物体处于静止状态时，则控制所述运动单元运动。

24.根据权利要求13至23中任意一项所述的装置，其特征在于，

所述第三获取模块用于通过初始对准获取所述物体的第三姿态包括：第三获取模块用于通过陀螺寻北中的多位置法和连续旋转法获取所述物体的所述第三姿态。

25.一种定位定姿的设备，其特征在于，包括处理器和惯性测量单元IMU，其中：

所述处理器用于获取物体的第一状态，所述第一状态包括第一位置和第一姿态，

所述IMU用于以所述第一状态为起点，为所述物体提供惯性导航，得到惯性导航数据；

所述处理器还用于根据所述惯性导航数据和所述第一状态得到所述物体的第二状态，所述第二状态包括第二位置和第二姿态；

所述IMU还用于通过初始对准获取所述物体的第三姿态，以所述第三姿态和所述第二位置为起点，为所述物体提供惯性导航。