CN110857860A - 一种定位转换方法及其系统 - Google Patents

Abstract

本发明公开了一种定位转换及其系统。该方法包括如下步骤：根据IMU的加速度以及IMU和定位件的相对位置关系，得到定位件的加速度；通过定位件的加速度对定位件做惯导处理，得到定位件的位置信息。该方法可以有效地解决由于定位件与IMU位置关系造成的测量误差，提高了定位的准确性。

Description

一种定位转换方法及其系统

技术领域

本发明涉及一种定位转换方法，同时涉及实现该定位转换方法的定位转换系统，属于空间定位技术领域。

背景技术

虚拟现实(简称VR)和增强现实(简称AR)技术就是通过计算机技术以及各类传感器技术在特定范围内产生出在视觉、听觉、触觉的虚拟环境或在其真实环境上进行效果叠加。

在VR系统中，很多应用场景涉及空间定位，利用空间定位技术可以实现真实世界与虚拟世界的更好互动。目前，主要是通过IMU(惯性测量单元)对定位点进行空间定位。

IMU是测量物体三轴姿态角(或角速度)以及加速度的装置。一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于惯性系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。

IMU通常安装在刚体内部的电路板上，刚体例如可以为无人机、VR(虚拟现实)头显和手柄等，在刚体进行空间定位时，使用的定位点和IMU的安装位置不一致时，利用IMU的位置对应的姿态数据对定位点的坐标进行数据处理会引起偏差。

发明内容

针对现有技术的不足，本发明所要解决的首要技术问题在于提供一种定位转换方法。

本发明所要解决的另一技术问题在于提供一种实现上述定位转换方法的定位转换系统。

为实现上述发明目的，本发明采用下述的技术方案：

根据本发明实施例的第一方面，提供一种定位转换方法，包括如下步骤：

根据IMU的加速度以及IMU和定位件的相对位置关系，得到定位件的加速度；

通过定位件的加速度对定位件的坐标做惯导处理，得到定位件的位置信息。

其中较优地，所述根据IMU的加速度以及IMU和定位件的相对位置关系，得到定位件的加速度，包括如下步骤：

IMU和定位件绕一原点做定轴旋转运动；

根据IMU的旋转角速度得到IMU的加速度；

根据IMU的加速度、IMU的旋转角速度得到转换的定位件的加速度。

其中较优地，所述定位件的加速度为：

其中，r2-r1为刚体上固定的杆臂；a1为IMU的加速度；ω为IMU的旋转角速度；

为旋转角速度求导。

其中较优地，所述通过定位件的加速度对定位件的坐标做惯导处理，得到定位件的位置信息，包括如下步骤：

获取定位件任意时刻的姿态变化四元数；

根据任意时刻的姿态变化四元数、定位件的加速度，通过速度更新方程得到定位件的惯导速度；

根据惯导速度计算定位件的位置信息。

其中较优地，所述通过速度更新方程得到定位件的惯导速度采用如下公式：

其中，

为t_m时刻的惯导速度，

为与姿态变换四元数

对应的姿态阵；Δv_m是加速度计在时间段[t_m-1，t_m]内输出的比力增量Δv_m＝a2*T_s，a2为定位件的加速度，T_s为采样间隔。

其中较优地，所述根据惯导速度计算定位件的位置信息，采用如下公式：

其中，

为t_m时刻的惯导速度；

为t_m-1时刻的惯导速度；T_s为采样间隔。

其中较优地，所述定位转换方法，还包括如下步骤：

通过定位件的实际坐标对惯导处理得到的定位件的位置信息进行修正计算。

其中较优地，所述通过定位件的实际坐标对惯导处理得到的定位件的位置信息进行修正计算，包括如下步骤：

构建低精度惯导系统的误差方程；

构建低精度惯导系统的空间模型；

将所述误差方程与所述空间模型结合，并将所述定位件的实际坐标作为输入，得到对姿态和位置的修正量，通过所述修正量对原有的结果进行修正。

根据本发明实施例的第二方面，提供一种定位转换系统，包括处理器和存储器；所述存储器上存储有可用在所述处理器上运行的计算机程序，当所述计算机程序被所述处理器执行时实现如下步骤：

根据IMU的加速度以及IMU和定位件的相对位置关系，得到定位件的加速度；

通过定位件的加速度对定位件的坐标做惯导处理，得到定位件的位置信息。

其中较优地，当所述计算机程序被所述处理器执行时实现如下步骤：

通过定位件的实际坐标对惯导处理得到的定位件的位置信息进行修正计算。

其中较优地，当根据IMU的加速度以及IMU和定位件的相对位置关系，得到定位件的加速度时，所述计算机程序被所述处理器执行时实现如下步骤：

IMU和定位件绕一原点做定轴旋转运动；

根据IMU的旋转角速度得到IMU的加速度；

根据IMU的加速度、IMU的旋转角速度得到转换的定位件的加速度。

本发明所提供的定位转换方法，通过IMU的加速度以及IMU和定位件的相对位置关系，计算得到定位件的加速度；再通过定位件的加速度对定位件做惯导处理，得到定位件的位置信息。该方法可以有效地解决由于定位件与IMU位置关系造成的测量误差，提高了定位的准确性。而且该方法通过后期修正，可以有效地解决由于定位件和IMU刷新率不一致而产生的测量误差。

附图说明

图1为定位转换方法的流程图；

图2为本发明所提供的一个实施例中，构建的旋转模型的示意图；

图3为本发明所提供的定位转换系统的结构示意图。

具体实施方式

下面结合附图和具体实施例对本发明的技术内容进行详细具体的说明。

IMU通常安装在刚体内部的电路板上，刚体例如可以为无人机、VR(虚拟现实)头显和手柄等，在本发明所提供的实施例中，以VR领域的手柄作为刚体为例进行说明。

由于VR手柄在使用过程中可能发生遮挡等情况，出现定位不准的情况，为了定位的准确性，最优的方式是通过惯性导航的方式，对手柄的定位数据进行惯导处理。在手柄中，接收定位信号的定位件和获得手柄姿势等信息的IMU通常位于手柄的不同位置，手柄定位时使用的是定位件的坐标点坐标，惯导使用的是IMU的位置进行处理，这样就会有偏差，需要经过杆臂转换把坐标点坐标转换到IMU的位置才能做惯导，但是需要跟姿态相关，如果姿态不准确会导致杆臂转换不准确，之后会导致一系列误差。具体地：定位件的初始姿态是相对于大地坐标系，与信号发射装置(基站)的坐标系并没有对准，此时根据这个定位来修正的姿态是错误的结果，如果根据这个错误的姿态来进行杆臂转换，得到的还是一个错误的位置，这样误差就会传递下去导致系统完全错误。

如图1所示，本发明所提供的定位转换方法，包括如下步骤：首先，根据IMU的加速度以及IMU和定位件的相对位置关系，得到定位件的加速度；然后，通过定位件的加速度对定位件的坐标做惯导处理，得到定位件的位置信息。在本发明所提供的实施例中，还会通过定位件的实际坐标对惯导处理得到的定位件的位置信息进行修正计算。下面对这一过程做详细具体的说明。

S11，根据IMU的加速度以及IMU和定位件的相对位置关系，得到定位件的加速度。

在本发明中，刚体可以为无人机、VR(虚拟现实)头显和手柄等，图2以刚体为手柄为例进行说明。其手柄上部为接收定位信号的定位件，其位置用点B表示，IMU设置在手柄下的握手部分，其位置用点A表示。根据IMU的加速度以及IMU和定位件的相对位置关系，得到定位件的加速度，具体包括如下步骤：

S111，IMU和定位件绕一原点O做旋转运动。

手柄在操作过程中，刚体在任意时刻都可以寻找到一个原点O，使刚体绕原点O做定轴旋转运动。旋转角速度为ω，

为角速度求导，IMU和定位件位于刚体上，则同样绕原点O做旋转运动，A和B处的旋转角速度相同。

S112，根据IMU的旋转角速度得到IMU的加速度，即图2中点A的加速度。

A处的加速度为

其中，a1为点A的加速度；ω为IMU的旋转角速度；

为旋转角速度求导；r₁为IMU到原点O的距离。

S113，根据IMU的加速度、IMU的旋转角速度得到转换的定位件的加速度。

即B处的加速度为：

其中，r2为定位件到原点0的距离；r2-r1为刚体上固定的杆臂，为已知值；a1为点A的加速度。

通过以上转换关系，即可利用IMU的加速度、IMU的旋转角速度、IMU和定位件的相对位置关系得到B点处的加速度。

S12，通过定位件的加速度对定位件的坐标做惯导处理，得到定位件的位置信息。

定位件B点坐标的刷新率是60hz，惯导的最终结果要求是240hz位置刷新率，所以在没有实际坐标的时候，采用IMU得到的加速度和角速度来计算得到位置，主要运用的是牛顿定律。

通过定位件的加速度对定位件B的坐标做惯导处理，得到定位件的位置信息。其中定位转换方法定位点B的坐标可通过激光+超声定位方法、lighthouse的激光定位方法、三个激光面的空间交会原理等得到。在本发明所提供的实施例中，通过定位件的加速度对定位件B的坐标做惯导处理，得到定位件的位置信息，具体包括如下步骤：

S121，获取定位件任意时刻的姿态变化四元数；

在本发明所提供的实施例中，定位件任意时刻的姿态变化四元数采用如下公式获取：

其中，

为t_m时刻的姿态变换四元数，n为惯性坐标系，b(m)为载体坐标系；为从t_m-1时刻到t_m时刻的姿态四元数的变化(记采样T_s＝t_m-t_m-1)，Δθ_m为陀螺在时间段[t_m-1，t_m]内输出的角增量且Δθ_m＝|Δθ_m|。在本发明所提供的实施例中采用低精度陀螺，低精度陀螺一般采用角速率输出采样方式，只需简单地将其乘以采样间隔T_s即可近似变换为角增量。

S122，根据任意时刻的姿态变化四元数、定位件的加速度，通过速度更新方程得到定位件的惯导速度。

通过公式：加速度×时间＝速度，就可以求得需要的惯导速度。在本发明所提供的实施例中，对于中低速形式的运载体，比如低速v＜100m/s，可以忽略地球自转以及地球曲率的影响。根据任意时刻的姿态变化四元数、定位件的加速度，通过速度更新方程可以得到定位件的惯导速度。

其中，速度更新方程用下式表示：

其中，

为t_m时刻的惯导速度，

为与姿态变换四元数

对应的姿态阵；Δv_m是加速度计在时间段[t_m-1，t_m]内输出的比力增量Δv_m＝a2*T_s，实际也可采用比力输出乘以采样间隔进行计算。

S123，根据惯导速度得到定位件的位置信息，采用如下公式：

其中，为t_m时刻的惯导速度；

为t_m-1时刻的惯导速度；T_s为采样间隔。

S13，通过定位件的实际坐标对惯导处理得到的定位件的位置信息进行修正计算。

定位件B点坐标的刷新率是60hz，惯导的最终结果要求是240hz位置刷新率，所以在没有实际坐标的时候，采用IMU得到的加速度和角速度来计算得到位置。

但是由于IMU存在零偏等问题，长时间积分会导致数据发散，所以需要外部的定位信息，用来纠正IMU的积分误差。通过数据融合的方式，将高频率的IMU数据和低频率的定位信息做融合，最终得到高频率的定位信息。

当定位件B点的实际坐标到来时，通过定位件B的实际坐标来修正上面纯惯导的计算得到的坐标，称为组合导航，下面对通过定位件的实际坐标对惯导处理得到的定位件的位置信息进行修正计算进行详细描述，具体包括如下步骤：

S131，构建低精度惯导系统的误差方程。

误差方程由以下公式表示：

其中，w_s和

分别为陀螺角速率白噪声和加计比力白噪声，

和分别为陀螺和加速度计一阶马尔科夫过程随机误差；其中：

i＝x，y，z；

其中，τ_Gi和τ_Ai是相应的时间常数；w_rGi和w_rAi是一阶马尔科夫过程激励白噪声。

S132，构建低精度惯导系统的空间模型。

在低精度惯导系统中，选择惯导系统的姿态失准角

速度误差δvⁿ、定位误差δPⁿ、陀螺相关漂移

加速度相关偏值

安装偏差角γ，以及磁倾角δη_x和磁偏角δη_z作为状态，如下：

构建的系统状态空间模型为：

其中，

H_G＝[0_6×3 I_6×6 0_6×11]；

β_x＝diag(1/τ_xx 1/τ_xy 1/τ_xz)(s＝G，A)；

下标表示矩阵的第1列和第3列，V为测量噪声。

S133，将构建的低精度惯导系统的误差方程与空间模型结合，并将获得的定位件的实际坐标作为输入进行计算，得到对姿态和位置的修正量，通过修正量对原有的结果进行修正。

通过上面模型进行计算，可以得到对姿态和位置的修正量，通过修正量对原有的结果进行修正，从而降低误差。

本发明还提供了一种定位转换系统，用以实现上述的定位转换方法。如图3所示，该系统包括处理器32以及存储有处理器32可执行指令的存储器31；

其中，处理器32可以是通用处理器，例如中央处理器(CPU)，还可以是数字信号处理器(DSP)、专用集成电路(ASIC)，或者是被配置成实施本发明实施例的一个或多个集成电路。

其中，存储器31，用于存储程序代码，并将该程序代码传输给CPU。存储器31可以包括易失性存储器，例如随机存取存储器(RAM)；存储器31也可以包括非易失性存储器，例如只读存储器、快闪存储器、硬盘或固态硬盘；存储器31还可以包括上述种类的存储器的组合。

具体地，本发明实施例所提供的第三方动态库接口拦截系统，包括处理器32和存储器31；存储器31上存储有可用在处理器32上运行的计算机程序，当计算机程序被处理器32执行时实现如下步骤：

S21，根据IMU的加速度以及IMU和定位件的相对位置关系，得到定位件的加速度。

S22，通过定位件的加速度对定位件B的坐标做惯导处理，得到定位件的位置信息。

其中，当根据IMU的加速度以及IMU和定位件的相对位置关系，得到定位件的加速度时，计算机程序被处理器32执行时实现如下步骤；

S211，IMU和定位件绕一原点做定轴旋转运动。

S212，根据IMU的旋转角速度得到IMU的加速度。

S213，根据IMU的加速度、IMU的旋转角速度得到转换的定位件的加速度。

其中，当计算机程序被处理器32执行时实现如下步骤；

定位件B处的加速度为：

其中，r2-r1为刚体上固定的杆臂；a1为IMU的加速度；ω为IMU的旋转角速度；

为旋转角速度求导。

其中，当通过定位件的加速度对定位件B的坐标做惯导处理，得到定位件的位置信息时，计算机程序被处理器32执行时实现如下步骤；

S221，获取定位件任意时刻的姿态变化四元数；

S222，根据任意时刻的姿态变化四元数、定位件的加速度，通过速度更新方程得到定位件的惯导速度。

S223，根据惯导速度计算定位件的的位置信息，采用如下公式：

其中，

为t_m时刻的惯导速度；

为t_m-1时刻的惯导速度；T_s为采样间隔。

其中，当计算机程序被处理器32执行时实现如下步骤；

根据任意时刻的姿态变化四元数、定位件的加速度，通过速度更新方程得到定位件的惯导速度时，速度更新方程用下式表示：

其中，

为t_m时刻的惯导速度，

为与姿态变换四元数

对应的姿态阵；Δv_m是加速度计在时间段[t_m-1，t_m]内输出的比力增量Δv_m＝a2*T_s。

其中，当计算机程序被处理器32执行时还实现如下步骤；

S23，通过定位件的实际坐标对惯导处理得到的定位件的位置信息进行修正计算。

其中，当通过定位件的实际坐标对惯导处理得到的定位件的位置信息进行修正计算时，计算机程序被处理器32执行时还实现如下步骤；

S231，构建低精度惯导系统的误差方程。

误差方程由以下公式表示：

其中，w_s和

分别为陀螺角速率白噪声和加计比力白噪声，

和分别为陀螺和加速度计一阶马尔科夫过程随机误差；其中：

i＝x，y，z；

其中，τ_Gi和τ_Ai是相应的时间常数；w_rGi和w_rAi是一阶马尔科夫过程激励白噪声。

S232，构建低精度惯导系统的空间模型。

在低精度惯导系统中，选择惯导系统的姿态失准角

速度误差δvⁿ、定位误差δPⁿ、陀螺相关漂移

加速度相关偏值

安装偏差角γ，以及磁倾角δη_x和磁偏角δη_z作为状态，如下：

构建的系统状态空间模型为：

其中，

H_G＝[0_6×3 I_6×6 0_6×11]；

β_x＝diag(1/τ_xx 1/τ_xy 1/τ_xz)(s＝G，A)；

下标表示矩阵的第1列和第3列，V为测量噪声。

S133，将构建的低精度惯导系统的误差方程与空间模型结合，并将获得的定位件的实际坐标作为输入进行计算，即可得到对姿态和位置的修正量，通过修正量对原有的结果进行修正。

定位件的实际坐标例如以60Hz的频率获得，惯导数据以240Hz的频率获得，则每次获得定位件的实际坐标时，即可获得该坐标对惯导得到的坐标的修正量，从而通过该修正量对原有的结果进行修正。

本发明实施例还提供了一种计算机可读存储介质。这里的计算机可读存储介质存储有一个或者多个程序。其中，计算机可读存储介质可以包括易失性存储器，例如随机存取存储器；存储器也可以包括非易失性存储器，例如只读存储器、快闪存储器、硬盘或固态硬盘、MCU等；存储器还可以包括上述种类的存储器的组合。当计算机可读存储介质中所述一个或者多个程序可被一个或者多个处理器执行，以实现上述的用于实现上述方法实施例中定位转换方法的部分步骤或者全部步骤。

上面对本发明所提供的定位转换方法及其系统进行了详细的说明。对本领域的一般技术人员而言，在不背离本发明实质精神的前提下对它所做的任何显而易见的改动，都将构成对本发明专利权的侵犯，将承担相应的法律责任。

Claims (10)

1.一种定位转换方法，其特征在于包括如下步骤：

根据IMU的加速度以及IMU和定位件的相对位置关系，得到定位件的加速度；

通过定位件的加速度对定位件的坐标做惯导处理，得到定位件的位置信息。

2.如权利要求1所述的定位转换方法，其特征在于所述根据IMU的加速度以及IMU和定位件的相对位置关系，得到定位件的加速度，包括如下步骤：

IMU和定位件绕一原点做定轴旋转运动；

根据IMU的旋转角速度得到IMU的加速度；

根据IMU的加速度、IMU的旋转角速度得到转换的定位件的加速度。

3.如权利要求2所述的定位转换方法，其特征在于：

所述定位件的加速度为：

其中，r2-r1为刚体上固定的杆臂；a1为IMU的加速度；ω为IMU的旋转角速度；

为旋转角速度求导。

4.如权利要求1所述的定位转换方法，其特征在于所述通过定位件的加速度对定位件的坐标做惯导处理，得到定位件的位置信息，包括如下步骤：

获取定位件任意时刻的姿态变化四元数；

根据任意时刻的姿态变化四元数、定位件的加速度，通过速度更新方程得到定位件的惯导速度；

根据惯导速度计算定位件的位置信息。

5.如权利要求4所述的定位转换方法，其特征在于所述通过速度更新方程得到定位件的惯导速度采用如下公式：

其中，

为t_m时刻的惯导速度，为与姿态变换四元数

对应的姿态阵；Δv_m是加速度计在时间段[t_m-1,t_m]内输出的比力增量Δv_m＝a2*T_s，a2为定位件的加速度，T_s为采样间隔。

6.如权利要求1所述的定位转换方法，其特征在于还包括如下步骤：

通过定位件的实际坐标对惯导处理得到的定位件的位置信息进行修正计算。

7.如权利要求6所述的定位转换方法，其特征在于，所述通过定位件的实际坐标对惯导处理得到的定位件的位置信息进行修正计算，包括如下步骤：

构建低精度惯导系统的误差方程；

构建低精度惯导系统的空间模型；

将所述误差方程与所述空间模型结合，并将所述定位件的实际坐标作为输入，得到对姿态和位置的修正量，通过所述修正量对原有的结果进行修正。

8.一种定位转换系统，其特征在于包括处理器和存储器；所述存储器上存储有可用在所述处理器上运行的计算机程序，当所述计算机程序被所述处理器执行时实现如下步骤：

根据IMU的加速度以及IMU和定位件的相对位置关系，得到定位件的加速度；

通过定位件的加速度对定位件的坐标做惯导处理，得到定位件的位置信息。

9.如权利要求8所述的定位转换系统，其特征在于当所述计算机程序被所述处理器执行时实现如下步骤：

通过定位件的实际坐标对惯导处理得到的定位件的位置信息进行修正计算。

10.如权利要求8所述的定位转换系统，其特征在于所述根据IMU的加速度以及IMU和定位件的相对位置关系，得到定位件的加速度，包括如下步骤：

IMU和定位件绕一原点做定轴旋转运动；

根据IMU的旋转角速度得到IMU的加速度；

根据IMU的加速度、IMU的旋转角速度得到转换的定位件的加速度。

Images