CN110388916B - 面向三维空间的组合定位方法及其系统 - Google Patents

Abstract

本发明公开了一种面向三维空间的组合定位方法，同时也公开了采用该方法的组合定位系统。在该组合定位方法中，采用至少两种不同的定位技术手段，通过信息融合的方式进行相互校正，可以实现组合定位导航。利用本发明，一方面可以消除惯性测量单元的累计积分误差，实现三维空间精确定位；另一方面可以进一步提高三维空间定位设备的刷新速率，改善用户的使用体验。

Description

面向三维空间的组合定位方法及其系统

技术领域

本发明涉及一种面向三维空间的组合定位方法，同时也涉及采用该方法的组合定位系统，属于无线定位技术领域。

背景技术

无线定位技术根据定位方式的不同，可以分为基于测距和无需测距两种方法。基于测距的方法主要是通过测量节点之间的距离或者角度，使用三边测量法、三角测量法或最大似然法等定位算法来估算未知节点的位置；无需测距的方法主要利用空间几何关系或者网络多跳路由来完成定位，例如质心算法、凸规划算法等。

图1所示为现有技术中的一种三维空间定位设备。该模型包括定位基站和待定位设备两部分。其中，定位基站的内部除了常规的运算模块之外，至少还包括两个相互垂直、不断旋转的电机，激光发射源和至少一个超声波测距模块。待定位设备优选为手柄或者头盔，其内部包括感光模块(例如光电管)、通信模块和超声波接收器等。待定位设备通过通信模块不断与定位基站交换信息，并解算感光模块和超声波接收器上各个传感器传输的数据，提供实际应用中所需的精确定位服务。

在图1所示的三维空间定位设备中，横向电机和纵向电机分别绕着转轴O₁O₂和O₃O₄做角匀速旋转运动。两个激光发射源发射的激光束分别照射一字镜表面，形成两个垂直的激光平面；电机转动带动一字镜一起做匀速旋转运动，在三维空间内形成不断旋转的激光平面；超声波测距模块和超声波接收器为定位提供距离信息。定位基站与待定位设备在运行过程中，不断通过通信模块进行时间同步，使得整个三维空间定位设备拥有同一个时钟基准。当待定位设备中的光电管与不断旋转的激光平面恰好共面时，待定位设备感知到激光束的光信号并标定当前时刻(称之为时间戳)，因此根据事件触发的时间戳，定位基站/待定位设备可以解算出当前激光平面旋转的角度，即确定待定位设备的方位信息。

但是，上述三维空间定位设备针对位置的刷新速率较低。对于目标运动速度较大的VR(虚拟现实)应用而言，采用上述三维空间定位设备存在或多或少的时间延迟，导致可供显示的VR画面迟滞，影响用户体验。

发明内容

针对现有技术中的不足，本发明所要解决的首要技术问题在于提供一种面向三维空间的组合定位方法。

本发明所要解决的另一技术问题在于提供一种采用上述方法的组合定位系统。

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

根据本发明实施例的第一方面，提供一种面向三维空间的组合定位方法，其中在惯性测量单元进行位置积分的过程中，

当定位基站获取定位信息后，执行基于卡尔曼滤波算法的预测环节和更新环节；

通过所述更新环节提供的概率最优模型，对所述惯性测量单元的当前位置进行矫正。

其中较优地，所述更新环节提供的概率最优模型为：

K＝PH^T(HPH^T+V)^-1

P←(I-KH)P；

其中，K为状态误差相关矩阵，P为协方差，H为测量系统的参数。

其中较优地，所述预测环节为：

其中，P为协方差，F为状态转移矩阵，Q为量测噪声相关矩阵。

其中较优地，在使用卡尔曼滤波算法的过程中，对状态量进行一阶近似。

其中较优地，对所述惯性测量单元相对虚拟坐标系的姿态进行参数标定，实现外参估计。

其中较优地，通过如下步骤实现外参估计：

在定位基站被放置并保持静止的情况下，在虚拟坐标系上三个轴上分别安装加速度计，用于感知重力在三个轴方向的分解力大小；

根据所述重力的分解情况，获得静止状态下虚拟坐标系相对大地坐标系之间的相对姿态。

其中较优地，所述定位基站从多个视角对平面标定板进行投影成像，获取所述定位基站相对于所述平面标定板的姿态矩阵；

解算包括姿态参数的方程组，得到内参模型和外参模型的所有参数值。

根据本发明实施例的第二方面，提供一种面向三维空间的组合定位系统，包括惯性测量单元和三维空间定位设备；所述三维空间定位设备中包括定位基站，所述定位基站中的两个激光发射源发射激光束分别照射到一字镜上，形成两个垂直的激光平面；电机转动带动所述一字镜进行匀速旋转运动，在三维空间内形成不断旋转的激光平面；其中：

在惯性测量单元进行位置积分的过程中，当收到定位基站的数据时，将所述惯性测量单元输出的位置信息和所述定位基站提供的位置信息进行融合，利用概率最优模型对所述惯性测量单元的当前位置进行矫正。

其中较优地，所述概率最优模型是基于卡尔曼滤波算法的预测环节和更新环节得到的。

与现有技术相比较，本发明采用至少两种不同的定位技术手段，通过信息融合的方式进行相互校正，可以实现组合定位导航。利用本发明，一方面可以消除惯性测量单元的累计积分误差，实现三维空间精确定位；另一方面可以进一步提高三维空间定位设备的刷新速率，改善用户的使用体验。

附图说明

图1为现有技术中，一种三维空间定位设备的示意图；

图2为定位基站的真实三维坐标系示意图；

图3为经校正后的非正交坐标系示意图；

图4为外参模型的示意图；

图5为平面标定板的结构示例图；

图6为实施本发明的三维空间定位设备的结构示意图；

图7为惯性测量单元相对于定位基站的姿态示意图。

具体实施方式

下面结合附图和具体实施例对本发明的技术方案展开详细具体的说明。

本发明的核心思想在于采用至少两种不同的定位技术手段，通过信息融合的方式进行相互校正，以便实现组合定位导航。在进行组合定位导航之前，首先说明本发明所涉及的一些坐标系，包括大地坐标系n、虚拟坐标系v和手柄坐标系b。其中，将大地坐标系n定义为以重力方向为轴，平面位于水平面的坐标系；将虚拟坐标系v定义为经内参校正和手眼标定后的正交坐标系；将手柄坐标系b定义为以手柄的惯性测量单元(IMU)的坐标轴为基准的坐标系。组合定位导航输出的坐标系以虚拟坐标系v为参照。

下面，首先对如何实现虚拟坐标系v的具体过程展开详细说明。

参见图1所示，定义平面O₁O₂Z₂Z₁和平面O₃O₄Z₄Z₃分别为电机转角的参考零度平面(Zero-Angle Reference Plane)，即当待定位设备恰好处于参考零度平面内时，规定定位基站对目标测量的转角为零度。当然，最优的参考零度平面位置应尽可能处于激光扫描区域的正中间。

当确定三维坐标系后，为了统一电机的旋转方向，一般选取右手坐标系，握住当前电机旋转轴，则另一个电机转轴正半轴指向为旋转方向。因此，根据参考零度平面O₁O₂Z₂Z₁和O₃O₄Z₄Z₃以及右手螺旋法则，确立以{xyd}为轴的坐标系，d处于两个参考零度平面的交线处，x、y处于电机旋转中心轴，将坐标系{xyd}称之为虚拟坐标系v，它为VR(虚拟现实)应用提供了一个正比例缩放的空间参考。

理想情况下，如果不考虑测量过程存在的时间对齐因素，假设一次测量的传感器值均在同一时刻下观测。设超声波测距模块的测距中心位于虚拟坐标系v下原点，即超声波的坐标为^v[0 0 0]^T，在t时刻横向和纵向电机的激光平面恰好与待定位设备的感光模块(例如光电管)重合，其相对参考零值平面的转角为

此时超声波测距模块的测距为R，即当前时刻提供了一组感知数据

若假设目标在虚拟坐标系v下的坐标为^v[x,y,z]，则根据几何关系容易得到：

但是，由于三维坐标系是根据电机的旋转轴建立的。如果电机存在安装偏差，不是相互完全垂直，则由电机旋转轴建立的坐标轴(X、Y)不是完全正交，利用这样的坐标系得到的待定位设备的坐标值就会有明显误差。在当前制作工艺的局限下，电机的安装位置总是存在或大或小的偏差，两个激光发射源难以做到完全垂直。因此，采用上述三维空间定位设备的定位系统在实际使用时会不可避免地存在误差。

为了从根本上消除上述误差，本发明首先建立面向三维空间定位的内参模型。

前已述及，在当前制作工艺的局限下，电机的安装位置总是存在或大或小的偏差，两个激光发射源难以做到完全垂直。在本发明中，我们将由安装误差导致的这一固有特性称之为内参。进一步地，用角度α表示两个线激光之间的夹角(理论上α等于90°)，用φ₀表示横向激光束转动中参考零度平面所对应的绝对转角，用

表示纵向激光束转动中参考零度平面所对应的绝对转角，用[x₀,y₀,z₀]表示超声波测量中心相对与坐标原点的安装误差。因此，内参模型可以表示为：

另一方面，以定位基站的两个电机的旋转轴建立坐标系(X、Y)，其中X轴和Y轴的夹角为α，则定位基站的真实三维坐标系如图2所示。由于参考零度平面O₁O₂Z₂Z₁和O₃O₄Z₄Z₃也需要精准的平面对齐。为了保证坐标系{xyd}的正交性，参考零度平面的对齐并不需要平面的绝对位置对齐，而是保证两个参考零度平面保持垂直的几何关系，即交线d必须垂直于xy平面。

在内参模型中，通常x轴与y轴之间并不为90度。为了校正这种非正交性，可以选择保留y轴并以垂直于y轴的x′轴为新度量基准，这样校正后的非正交坐标系为{x′yd}，如图3所示。

当选择坐标系{x′yd}为度量基准时，定位系统的测量原理重新表述为：

上式表示激光正交系和测量值[Ф φ R]的关系。

在使用定位基站的过程中，有时需要知道虚拟坐标系相对大地坐标系n的姿态，比如大多数VR应用必须明确虚拟空间与室内水平面之间的倾斜角，以便进行空间对齐。因此如图4所示，我们将虚拟坐标系相对大地坐标系之间的位姿称之为外参，使用[R t]表示，即：

其中，R表示虚拟坐标系相对大地坐标系的姿态，t表示虚拟坐标系相对大地坐标系的位移。

在本发明中，要求惯性测量单元(IMU)与虚拟坐标系的三个轴方向严格对齐，这就需要对惯性测量单元相对虚拟坐标系的姿态进行参数标定，实现外参估计。在本发明的一个实施例中，实现外参估计的方法为：在虚拟坐标系上三个轴上均安装加速度计，它能感知重力矢量在三个轴方向的分解力大小。因此根据重力矢量的分解，可以得出静止状态下虚拟坐标系相对大地坐标系之间的相对姿态。在大多数的VR应用中，定位基站常常被放置并保持静止，因此这一方法简单可行。

另外，也可以通过专门制作的测试装置计算得到外参模型的各个参数值。该测试装置包括一块平面标定板和一个能够测量平面标定板本身姿态的装置，例如可以是水平仪或者惯性测量单元(IMU)。如图5所示，该平面标定板上包括若干个数据点(也称为光电节点)，每个数据点分别包括一个感光模块(即光电管)和一个超声波接收器。例如，在一个固定大小的平面标定板上等距排列m行n列个数据点(即光电节点的个数M＝m*n，其中m、n均为正整数)，数据点的横向距离间隔为L，纵向距离间隔为W。当平面标定板全部位于定位基站的可视区域内时，光电节点能够感知定位基站的两束旋转激光面的光信号和超声波测距模块发射的超声信号，并标记当前的转角。

在本发明的一个实施例中，超声波测距模块采用TOF(time-of-fly)测距方式，测量超声波在定位基站和待定位设备之间直线距离上的飞行时间。当已知当前超声波传播速度时，定位基站与待定位设备之间的距离可以表示为飞行时间乘以超声波速度。

如图6所示，如果将平面标定板2放置为竖直状态(与大地水平面垂直)，则定位基站1相对于平面标定板2之间的姿态等于定位基站1相对于大地水平面的姿态。因此，在本发明的一个实施例中将平面标定板2垂直放置在平台3上，平台3的四周分别放置一个水平仪4以便与大地水平面重合，可以使用水平仪4将平面标定板2调节至竖直并予以固定，从而以该平面标定板2为基准创建一个在室内三维空间中能够独立使用的三维正交坐标系(即理想的正交坐标系)，然后将平面标定板保持在竖直状态，在各个方位放置并依次收集多个数据点的平面倾斜角，则每一次激光信号和超声信号的收集过程均能获取定位基站相对平面标定板的姿态矩阵，即

Output＝{Accel_j,R_j}

如图7所示，假设惯性测量单元(IMU)或平面标定板相对于定位基站的姿态为R_Nolo-＞IMU，其对应的四元数为q，则存在关联表达式如下：

R_Nolo-＞IMUR_IMU-＞Earth＝R_{Nolo-＞Earth}

因此对于重力矢量g＝[0,0,g]的观测，有：

若进行了N(N为正整数，下同)次定位测量，则外参估计的代价函数为：

相应的最优解为矩阵

特征值中最大值对应的特征向量。其中，

则测量这若干个数据点的方程为：

其中，θ_Laser为内参模型，[R、t]为外参模型。在本发明的一个实施例中，θ_Laser存在6个自由度，R存在3个自由度，t存在3个自由度，因此至少需要12个参考点使得上式有解。实践中为了保证计算精度，应该建立12×3＝36个参考点。

通过解算上述方程组，即可得到内参模型和外参模型的所有参数值，从而得到定位基站的真实坐标系和理想的正交坐标系的偏差，将该偏差带入待定位设备的定位计算中，即可矫正待定位设备的位置数据。

上述内参标定方法将惯性测量单元(IMU)的度量坐标系与定位基站的度量坐标系建立了精确的转换关系，在工业上被叫作手眼(hand-eye)标定。它为组合定位导航系统提供了一种统一测量坐标的桥梁，是组合定位导航系统在使用之前或者说生产过程中需要提前进行的。

另一方面，现有的三维空间定位设备虽然可以提供三维空间内的精确位置服务，但对于高速运动的应用场景而言，在刷新速率上存在局限性。惯性测量单元(IMU)可以在短时间内提供精确的位置积分，刷新速率高，不需要依赖外部设施，但容易累计积分误差，较长时间段内的位置误差极大。因此，可以将这两方面的技术特性相互结合，实现组合定位导航，并且进一步提高组合定位系统的刷新速率。下面对此展开具体的说明。

在室内三维空间定位场景中，由于目标处于低速运动(小于100m/s)且定位区域为小范围比如数米，则忽略地球自转及地球曲率的影响，假定大地坐标系n在运动中并无变化。因此，室内定位场景下的组合定位导航可以得到简化。

在本发明的一个实施例中，采用15阶状态量建立刚体运动的系统方程以实现组合定位导航，即状态量选取目标在虚拟坐标系v下的位置、速度、姿态、加速度计漂移以及陀螺仪漂移，即状态x＝[^vp_k ^vv_k δ_f,k δ_ω,k q_k]^T。其惯性导航的运动状态方程为

其中，虚拟坐标系v下目标运动加速度

表示大地坐标系n相对虚拟坐标系v的姿态，通过手眼标定计算获取，

为手柄坐标系b到虚拟坐标系v下的姿态转换矩阵，旋转角速度经过误差补偿后有

Δt为采样周期，q{ω_kΔt}表示陀螺仪旋转角度ω_kΔt对应的四元数，过程噪声

为加速度计白噪声、一阶马尔卡夫噪声和陀螺仪白噪声、一阶马尔卡夫噪声。

将利用前述三维空间定位设备测量的激光平面转角和测距的噪声近似作为白噪声处理，因此建立量测方程为

其中量测噪声

分别为横向转角、纵向转角和超声波测距的噪声，

表示手柄与定位基站之间超声波测距值。

根据运动状态方程和量测方程，可以获得组合定位方法中目标运动的机理和模型特征。进一步地，采用卡尔曼(kalman)滤波算法对其进行滤波校正和融合。

在本发明的一个实施例中，鉴于其运动状态方程和量测方程的非线性，需要对状态量进行一阶近似，即使用可扩展的卡尔曼滤波算法。具体说明如下：

使用误差量作为滤波器的状态量，令误差量为位置误差、速度误差、姿态误差、加速度计零值漂移及陀螺仪零值漂移，表示为：

则状态转移方程为

将对误差方程

求导得：

预测环节：

在量测方程中，为了实现卡尔曼滤波算法的非线性处理，需要对量测方程进行一阶梯度近似，则量测误差量

关于状态误差量

的偏导数为

更新环节：

K＝PH^T(HPH^T+V)^-1

P←(I-KH)P

上式中的K、H、P、Q、F等均为卡尔曼滤波算法中使用的常规参量。其中，K为卡尔曼增益，V为量测噪声，P为量测噪声的协方差，H为测量系统的参数，Q为系统噪声的协方差，F为状态转移矩阵。不带前缀的量代表实际量，其下缀里面的“k”或“t”代表该量是第k或第t时刻的实际量；带前缀“^”的量代表预测量；带前缀“～”的量代表与预测量对应的偏差值。

关于卡尔曼滤波算法的进一步说明，可以参阅如下网址：www.cs.unc.edu/～welch/kalman/index.html，在此就不具体说明了。

在本发明的一个实施例中，以较高速率对惯性测量单元(IMU)进行位置积分，实时刷新位置数据。在此期间，当收到定位基站的数据时，将惯性测量单元输出的位置信息和定位基站提供的位置信息进行融合，利用上述更新环节所提供的概率最优模型对惯性测量单元的当前位置进行矫正。上述计算过程一直循环进行。即，当每次定位基站获取定位信息后，组合定位系统就执行一次如下的组合定位方法：

初始化：

预测环节：

更新环节：

K＝PH^T(HPH^T+V)^-1

P←(I-KH)P

通过上述操作，可以消除惯性测量单元的累计积分误差。整个组合定位系统不断进行预测和更新，可以有效地提高三维空间定位设备的刷新速率。

与现有技术相比较，本发明采用至少两种不同的定位技术手段，通过信息融合的方式进行相互校正，可以实现组合定位导航。利用本发明，一方面可以消除惯性测量单元的累计积分误差，实现三维空间精确定位；另一方面可以进一步提高三维空间定位设备的刷新速率，改善用户的使用体验。

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

Claims (7)

1.一种面向三维空间的组合定位方法，其特征在于在惯性测量单元进行位置积分的过程中，

当定位基站获取定位信息后，执行基于卡尔曼滤波算法的预测环节和更新环节，具体步骤为：

使用误差量作为滤波器的状态量，令误差量为位置误差、速度误差、姿态误差、加速度计零值漂移及陀螺仪零值漂移，表示为：

则状态转移方程为

对误差方程

求导得：

所述预测环节为：

在量测方程中，为了实现卡尔曼滤波算法的非线性处理，需要对量测方程进行一阶梯度近似，则量测误差量

关于状态误差量

的偏导数为

所述更新环节为：

K＝PH^T(HPH^T+V)^-1

P←(I-KH)P

其中，K为卡尔曼增益，V为量测噪声，P为量测噪声的协方差，H为测量系统的参数，Q为系统噪声的协方差，F为状态转移矩阵，不带前缀的量代表实际量，其下缀里面的“k”或“t”代表该量是第k或第t时刻的实际量；带前缀“^”的量代表预测量；带前缀“～”的量代表与预测量对应的偏差值；

当收到所述定位基站的数据时，将所述惯性测量单元输出的位置信息和所述定位基站提供的位置信息进行融合，利用所述更新环节提供的概率最优模型对所述惯性测量单元的当前位置进行矫正。

2.如权利要求1所述的组合定位方法，其特征在于在使用卡尔曼滤波算法的过程中，对状态量进行一阶近似。

3.如权利要求1所述的组合定位方法，其特征在于：

对所述惯性测量单元相对虚拟坐标系的姿态进行参数标定，实现外参估计。

4.如权利要求3所述的组合定位方法，其特征在于通过如下步骤实现外参估计：

在定位基站被放置并保持静止的情况下，在虚拟坐标系上三个轴上分别安装加速度计，用于感知重力在三个轴方向的分解力大小；

根据所述重力的分解情况，获得静止状态下虚拟坐标系相对大地坐标系之间的相对姿态。

5.如权利要求1所述的组合定位方法，其特征在于：

所述定位基站从多个视角对平面标定板进行投影成像，获取所述定位基站相对于所述平面标定板的姿态矩阵；

解算包括姿态参数的方程组，得到内参模型和外参模型的所有参数值。

6.一种面向三维空间的组合定位系统，包括惯性测量单元和三维空间定位设备；所述三维空间定位设备中包括定位基站，所述定位基站中的两个激光发射源发射激光束分别照射到一字镜上，形成两个垂直的激光平面；电机转动带动所述一字镜进行匀速旋转运动，在三维空间内形成不断旋转的激光平面；其特征在于：

在惯性测量单元进行位置积分的过程中，当收到定位基站的数据时，将所述惯性测量单元输出的位置信息和所述定位基站提供的位置信息进行融合，利用权利要求1～5中任意一项所提供的概率最优模型对所述惯性测量单元的当前位置进行矫正。

7.如权利要求6所述的组合定位系统，其特征在于：

所述概率最优模型是基于卡尔曼滤波算法的预测环节和更新环节得到的。

Images