CN109751992A - 面向室内三维空间的定位校正方法、定位方法及其设备 - Google Patents

Abstract

本发明公开了一种面向室内三维空间的定位校正方法，同时也公开了采用该定位校正方法的三维空间定位方法及三维空间定位设备。在该定位校正方法中，首先创建一个在室内三维空间中独立使用的三维正交坐标系；然后将三维正交坐标系与三维空间定位设备已经建立的真实坐标系进行比较，得到三维空间定位设备的真实坐标系与三维正交坐标系之间的偏差；最后基于该偏差，对三维空间定位设备的定位计算进行校正。利用本发明，可以使三维空间定位设备实现精确定位。

Description

面向室内三维空间的定位校正方法、定位方法及其设备

技术领域

本发明涉及一种面向室内三维空间的定位校正方法，同时也涉及采用该定位校正方法的三维空间定位方法及三维空间定位设备，属于无线定位技术领域。

背景技术

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

针对室内环境中进行三维空间定位的需求，现有技术提供了一种利用两个激光平面及超声波信号进行三维空间定位的方法。如图1所示，该方法将定位基站内带动两个激光平面旋转的电机所在的旋转轴定义为两个坐标轴(X、Y)，两个坐标轴的交点位置为原点，同时也是超声波发射器所在的位置。超声波发射器以垂直于两个坐标轴所在平面发射超声波信号，假设超声波发射的方向为Z轴，则可建立一个三维空间坐标系。在三维空间坐标系中的待定位设备接收激光信号和超声信号，即可以在理论上得到待定位设备的精确位置。

但是，上述方法的缺陷在于：由于三维空间坐标系是根据电机的旋转轴建立的。如果电机安装偏差，不是相互完全垂直，则由电机旋转轴建立的坐标轴(X、Y)不是完全正交，利用这样的坐标系得到的待定位设备的坐标值就会存在误差。

吴军等人在论文《采用红外扫描激光与超声技术的室内空间定位》(刊载于《光学精密工程》2016年第24卷第10期)中，也提出了一种基于激光测距原理的室内空间定位方法。该方法通过单台测量定位基站向被测空间内发射旋转扫描红外激光信号以及超声脉冲信号，采用旋转扫描红外激光形成多平面约束，结合采用高精度超声测距形成距离约束。然后，将多平面约束与距离约束相耦合，得到测量靶标的非线性约束方程组，最后利用非线性最优化算法解算得到测量靶标的精确空间坐标。但是，该室内空间定位方法也没有考虑消除电机安装偏差的不利影响。

发明内容

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

本发明所要解决的另一技术问题在于提供一种采用上述定位校正方法的三维空间定位方法。

本发明所要解决的又一技术问题在于提供一种采用上述定位校正方法的三维空间定位设备。

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

根据本发明实施例的第一方面，提供一种面向室内三维空间的定位校正方法，包括如下步骤：

创建一个在室内三维空间中独立使用的三维正交坐标系；

将所述三维正交坐标系与三维空间定位设备已经建立的真实坐标系进行比较，得到三维空间定位设备的真实坐标系与所述三维正交坐标系之间的偏差；

基于所述偏差，对所述三维空间定位设备的真实坐标系进行校正。

其中较优地，所述三维正交坐标系以放置为竖直状态的平面标定板为基准创建而成，所述竖直状态由水平仪或者惯性测量单元予以确定。

其中较优地，所述平面标定板上包括多个数据点，每个数据点分别包括一个光电管和一个超声波接收器；

所述三维空间定位设备中包括定位基站，所述定位基站中的两个激光发射源发射激光束分别照射到光学透镜上，形成两个垂直的激光平面；电机转动带动所述激光平面进行匀速旋转运动，在三维空间内形成不断旋转的激光平面；

所述定位基站还包括超声波测距模块。

其中较优地，所述定位基站从多个视角对所述平面标定板进行投影成像，获取所述定位基站相对于所述平面标定板的姿态矩阵；解算包括姿态参数的方程组，得到所述真实坐标系与所述三维正交坐标系之间的偏差。

其中较优地，所述姿态参数包括内参和外参；所述内参由安装误差导致，包括两个激光面之间的夹角、两个激光面与参考零度平面所对应的绝对转角和超声波测距模块的安装误差；所述外参为真实坐标系相对大地坐标系之间的位姿。

根据本发明实施例的第二方面，提供一种三维空间定位方法。该方法在定位过程中，采用上述的定位校正方法对三维空间定位设备的定位计算进行校正。

根据本发明实施例的第三方面，提供一种三维空间定位设备，包括定位基站和待定位设备，所述定位基站的内部包括两个相互垂直的电机、两个激光发射源和至少一个超声波测距模块，所述待定位设备包括感光模块和超声波接收器，其中：

所述定位基站采用上述的定位校正方法得到经过校正的真实坐标系，所述待定位设备根据经过校正的真实坐标系进行定位计算。

根据本发明实施例的第四方面，提供一种三维空间定位矫正设备，包括定位基站，所述定位基站的内部包括两个相互垂直的电机、两个激光发射源和至少一个超声波测距模块，其中：

所述三维空间定位矫正设备中还包括平面标定板和能够测量所述平面标定板本身姿态的装置；

所述平面标定板放置为竖直状态，作为基准创建三维正交坐标系；

将所述三维正交坐标系与所述三维空间定位设备已经建立的真实坐标系进行比较，得到三维空间定位设备的真实坐标系与所述三维正交坐标系之间的偏差；

基于所述偏差，对所述定位基站的真实坐标系进行校正。

其中较优地，所述平面标定板上包括多个数据点，每个数据点分别包括一个光电管和一个超声接收器；当所述平面标定板全部位于所述定位基站的信号区域内时，所述数据点感知定位基站的两束旋转激光面的光信号和超声信号，并标记当前的位置。

其中较优地，所述能够测量所述平面标定板本身姿态的装置为水平仪或者惯性测量单元。

与现有技术相比较，本发明创造性地利用水平仪和平面标定板创建一个在室内三维空间中能够独立使用的三维正交坐标系，然后将平面标定板在各个方位放置，依次收集多个数据点的在真实坐标系内的位置坐标，通过每一次位置坐标的收集过程获取定位基站相对平面标定板的姿态矩阵，得到定位基站的真实坐标系和理想的正交坐标系之间的偏差，由此矫正定位基站中电机的安装偏差，使三维空间定位设备实现精确定位。

附图说明

图1为现有技术中，利用两个激光平面及超声波信号进行三维空间定位的示意图；

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

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

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

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

图6为本发明所提供的三维空间定位矫正设备的结构示意图；

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

具体实施方式

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

在本发明的一个实施例中，所提供的三维空间定位设备包括定位基站和待定位设备两部分，主要用作VR/AR(虚拟现实/增强现实)或者无人机组件。其中，定位基站的内部除了常规的运算模块之外，至少还包括两个相互垂直、不断旋转的电机、激光发射源(优选为面发射型)和至少一个超声波测距模块。待定位设备优选为手柄或者头盔，其内部包括感光模块(例如光电管)、通信模块和超声波接收器等。待定位设备通过通信模块不断与定位基站交换信息，并解算感光模块(例如光电管)和超声波接收器上各个传感器传输的数据，提供实际应用中所需的精确定位服务。

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

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

参见图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[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]的关系。

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

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

其次，通过专门制作的测试装置测量计算得到内参模型的各个参数值。

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

如图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次定位测量，则外参估计的代价函数为：

相应的最优解为矩阵特征值中最大值对应的特征向量。其中，

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

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

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

下面对上述计算过程展开详细具体的说明。

为了量化计算各个数据点在平面标定板中的几何位置，设平面标定板创建的三维正交坐标系为Ref坐标系，则Ref坐标系下第k行p列(k、p均为正整数)的数据点(也称为光电节点)的坐标为：

设平面标定板中第k行p列的数据点位于对应的真实坐标系v下的坐标为^v[x_kp y_kpz_kp]，定位基站的激光旋转平面转角处，根据三维空间定位模型的测量原理可知：

设定位基站相对平面标定板的位置和姿态(简称为位姿)为T_v-＞Ref＝[R t； 0 1]，Ref坐标系与真实坐标系系v之间的坐标变换为：

PnP算法(即透视n点定位算法，是指通过多对3D与2D匹配点，最小化重投影误差来求解内参或者外参的方法)的主要内容描述为：当已知定位基站相对平面标定板的位姿T_v-＞Ref以及定位基站的内参θ_c时，根据投影关系可以唯一确定可视区域内所有数据点的成像，用如下公式表示：

为了描述投影关系的正确性，斑点成像的重投影误差用内参θ_c和位姿T_v-＞Ref表示为

st.^vx_kp＝R_v-＞Ref·^Refx_kp+t_v-＞Ref

其中，代表第k行p列数据点在定位基站内的重投影误差向量，简写为h_kp，ref表示数据点在Ref坐标系下的坐标矩阵且ref_kp＝^Refx_kp，表示定位基站检测到数据点出现时激光旋转平面的转角且若对平面标定板中所有数据点成像，则所有数据点投影变换后重投影误差为

其中，表示3D点到2D点之间的重投影误差矩阵，简写为h。

为了对内参进行更准确的估计，定位基站应该从多个视角对平面标定板进行投影成像并获取多方位下的感知测量。若第j次视角对平面标定板测量时，已知内参θ_c和外参T_j下的重投影误差为h_j。对平面标定板在不同视角下测量N次(N为正整数)，则所有位姿下，数据点的重投影误差的平方和为：

其中，h_1,.,N代表所有位姿态下全部斑点的重投影误差向量，即：

h_1,.,N＝[h₁ ... h_j ... h_N]^T

用符号Π表示一组待估计量[θ_c R₁/t₁ ... R_M/t_M]^T，则内参变量的估计等价于为对非线性代价函数F(Π)进行最小化，即：

上述最优化实质上是一个非线性最小二乘问题，使用常规的高斯-牛顿方法或者列文伯格-马夸尔特方法可以求解，其优化迭代过程中变量更新的每次步长由如下关系确定：

(J^TJ+λI)ΔΠ_step＝-J^Th_Π

其中，J代表所有位姿下的数据点重投影误差向量h_1,.,N在当前估计Π的梯度函数，λ为阻尼系数，h_Π为h_1,.,N(θ_c,R_1..M,t_1..M)向量的简写。

利用以上的内参估计算法，不仅可以估计出上述三维空间定位模型的内参系数，而且也可以估计出定位基站相对于平面标定板之间的姿态，即得到外参系数。将该内参系数和外参系数带入待定位设备的定位计算中，即可得到待定位设备在室内环境下精准的三维空间位置数据。

以上对本发明所提供的定位校正方法进行了详细说明。在上述定位校正方法的基础上，本发明进一步提供了一种采用该定位校正方法的三维空间定位方法。该三维空间定位方法并不限于使用在图1所示的三维空间定位模型中，只要在确定待定位设备的方位过程中，利用一个理想的正交坐标系与三维空间定位设备已经建立的三维坐标系进行比较，获得定位基站的真实坐标系和理想的正交坐标系之间的偏差，然后利用该偏差对三维空间定位结果进行校正，均为本发明所限定的三维空间定位方法。

与现有技术相比较，本发明所提供的面向室内三维空间的定位校正方法创造性地利用水平仪和平面标定板创建一个在室内三维空间中能够独立使用的三维正交坐标系，然后将平面标定板在各个方位放置并依次收集多个数据点的平面倾斜角，通过每一次激光信号和超声信号收集过程获取定位基站相对平面标定板的姿态矩阵，得到定位基站的真实坐标系和理想的正交坐标系之间的偏差，由此矫正定位基站中电机的安装偏差，使三维空间定位设备实现精确定位。

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

Claims (10)

1.一种面向室内三维空间的定位校正方法，其特征在于包括如下步骤：

创建一个在室内三维空间中独立使用的三维正交坐标系；

将所述三维正交坐标系与三维空间定位设备已经建立的真实坐标系进行比较，得到三维空间定位设备的真实坐标系与所述三维正交坐标系之间的偏差；

基于所述偏差，对所述三维空间定位设备的真实坐标系进行校正。

2.如权利要求1所述的定位校正方法，其特征在于：

所述三维正交坐标系以放置为竖直状态的平面标定板为基准创建而成，所述竖直状态由水平仪或者惯性测量单元予以确定。

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

所述平面标定板上包括多个数据点，每个数据点分别包括一个光电管和一个超声波接收器；

所述三维空间定位设备中包括定位基站，所述定位基站中的两个激光发射源发射激光束分别照射到光学透镜上，形成两个垂直的激光平面；电机转动带动所述激光平面进行匀速旋转运动，在三维空间内形成不断旋转的激光平面；

所述定位基站还包括超声波测距模块。

4.如权利要求3所述的定位校正方法，其特征在于：

所述定位基站从多个视角对所述平面标定板进行投影成像，获取所述定位基站相对于所述平面标定板的姿态矩阵；解算包括姿态参数的方程组，得到所述真实坐标系与所述三维正交坐标系之间的偏差。

5.如权利要求4所述的定位矫正方法，其特征在于：

所述姿态参数包括内参和外参；所述内参由安装误差导致，包括两个激光面之间的夹角、两个激光面与参考零度平面所对应的绝对转角和超声波测距模块的安装误差；所述外参为所述真实坐标系相对大地坐标系之间的位姿。

6.一种三维空间定位方法，其特征在于在定位过程中，采用权利要求1～5中任意一项所述的定位校正方法，对三维空间定位设备的定位计算进行校正。

7.一种三维空间定位设备，包括定位基站和待定位设备，所述定位基站的内部包括两个相互垂直的电机、两个激光发射源和至少一个超声波测距模块，所述待定位设备包括感光模块和超声波接收器，其特征在于：

所述定位基站采用权利要求1～5中任意一项所述的定位校正方法得到经过校正的真实坐标系，所述待定位设备根据经过校正的真实坐标系进行定位计算。

8.一种三维空间定位矫正设备，包括定位基站，所述定位基站的内部包括两个相互垂直的电机、两个激光发射源和至少一个超声波测距模块，其特征在于：

所述三维空间定位矫正设备中还包括平面标定板和能够测量所述平面标定板本身姿态的装置；

所述平面标定板放置为竖直状态，作为基准创建三维正交坐标系；

将所述三维正交坐标系与所述三维空间定位设备已经建立的真实坐标系进行比较，得到三维空间定位设备的真实坐标系与所述三维正交坐标系之间的偏差；

基于所述偏差，对所述定位基站的真实坐标系进行校正。

9.如权利要求8所述的三维空间定位矫正设备，其特征在于：

所述平面标定板上包括多个数据点，每个数据点分别包括一个光电管和一个超声接收器；当所述平面标定板全部位于所述定位基站的信号区域内时，所述数据点感知定位基站的两束旋转激光面的光信号和超声信号，并标记当前的位置。

10.如权利要求8所述的三维空间定位矫正设备，其特征在于：

所述能够测量所述平面标定板本身姿态的装置为水平仪或者惯性测量单元。