以多光源实现移动设备空间测位的方法与装置与流程

本发明属于移动设备的空间定位与姿态测量技术领域，尤其涉及一种以多光源实现移动设备空间测位的方法与装置。

背景技术：

目前，空间位置与姿态测量的方法，按其测量原理分类，主要有以下几种：一、陀螺仪；二、视觉测量；三、结构光测量。

其中，陀螺仪可以同时测定承载它的移动设备的姿态(朝向)和加速度，但是移动设备的空间(相对)坐标只能通过对加速度的二次积分获得，精度较低。视觉测量方法，需要使用复杂的算法来识别对应点，运算量较大；同时，面阵图像传感器的采集速率有限(一般不超过200帧/秒)，因而不易高频率地获取位置与姿态信息。结构光测量方法，一般需要机械装置辅助，以使线激光在空间中往复扫描，实现方案较为复杂。综上所述，方案一不能获得高精度的空间位置坐标，二、三则有成本较高的缺点。

技术实现要素：

为了解决现有技术中存在的上述问题，本发明提供了一种可对移动设备进行快速、高精度的空间位置与姿态测定，实现全角度无死角的空间位置与姿态测定，同时可使得求出来的参照点三维坐标的误差在各方向均匀地分布的以多光源实现移动设备空间测位的方法，以及可实现该方法、且结构简单，生产加工容易成本低，有利于批量生产、普及推广使用的以多光源实现移动设备空间测位的装置。

本发明解决现有技术问题所采用的技术方案为：

一种以多光源实现移动设备空间测位的方法，包括：首先在室内或室外安装三个以上位置固定且不在一条直线上的时间编码不同的点光源或空间编码不同的点光源簇作为空间坐标的参照点，然后利用安装在移动设备上的三个以上一维的定向模组获取移动设备相对于参照点的相对坐标数据，实现移动设备空间坐标和取向的测定。

进一步地，所述“利用安装在移动设备上的三个以上一维的定向模组获取移动设备相对于参照点的相对坐标数据，实现移动设备空间坐标和取向的测定”具体以下步骤：

A1.所述三个以上定向模组同步接收各个参照点发出的光，并根据各个参照点的时间编码或空间编码识别出各个参照点的像和一维坐标；

A2.根据上述一维坐标计算移动设备和各参照点之间的距离以及相对取向；

A3.根据移动设备到各个参照点的距离和取向，确定移动设备的位置坐标和取向。

进一步地，所述“所述三个以上定向模组同步接收各个参照点发出的光，并根据各个参照点的时间编码或空间编码识别出各个参照点的像和一维坐标”具体为：三个以上定向模组同步对各个参照点进行扫描，并通过其光电传感器接收经过柱透镜的各个参照点发出的光，且根据各个参照点的时间编码或空间编码区分各个参照点的像和计算各个参照点的像在各个定向模组的光电传感器上的坐标。

进一步地，所述“计算各个参照点的像在各个定向模组的光电传感器上的坐标”具体为：首先以各个定向模组的光电传感器所在直线的交点为原点，以其中一直线为X轴，建议直角坐标系，并以各个定向模组的光电传感器靠近原点一侧的像素点为像素起始坐标；然后各个参照点采用以下方法求出：

B1.根据参照点的像与三个以上定向模组的光电传感器的交点的位置建立三个以上平面的平面方程；

B2.依次选取其中不同的两个平面求解交线，和其余各个平面求解交点，得到三组以上该参照点的坐标；

B3.将该三组以上坐标的平均值作为该参照点的坐标。

一种以多光源实现移动设备空间测位的装置，其特征在于，包括有三个以上时间编码不同的点光源或空间编码不同的点光源簇、三个以上定向模组和底板，所述三个以上定向模组互不平行地设于底板的上表面，所述三个以上的点光源或点光源簇设于三个以上定向模组和底板的上方，且不在同一直线上。

进一步地，该装置包括有三个点光源或点光源簇和三个定向模组，且三个定向模组呈均匀分布或非均匀分布。

进一步地，所述定向模组包括有柱透镜和光电传感器，所述光电传感器安装在底板的上表面，所述柱透镜通过支撑杆悬空地设置在底板上、并位于光电传感器的上方。

进一步地，所述光电传感器是线阵光电传感器。

进一步地，所述点光源是红外光照明光源，所述点光源簇是采用逆向反光结构的反光点。

本发明的有益效果如下：

本发明通过上述技术方案，即可对移动设备进行快速、高精度的空间位置与姿态测定，实现全角度无死角的空间位置与姿态测定，同时可使得求出来的参照点三维坐标的误差在各方向均匀地分布，避免误差分布在某一方向过大的情形出现，而且装置结构十分简单，生产加工容易成本低，有利于批量生产、普及推广使用。

【附图说明】

图1是本发明所述一种以多光源实现移动设备空间测位的装置实施例的结构示意图；

图2是本发明所述一种以多光源实现移动设备空间测位的装置实施例的测定原理示意图。

【具体实施方式】

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

本发明实施例所述的一种以多光源实现移动设备空间测位的方法，包括：首先在室内或室外安装三个以上位置固定且不在一条直线上的时间编码不同的点光源或空间编码不同的点光源簇作为空间坐标的参照点，然后利用安装在移动设备上的三个以上一维的定向模组获取移动设备相对于参照点的相对坐标数据，实现移动设备空间坐标和取向的测定。

其中，所述“利用安装在移动设备上的三个以上一维的定向模组获取移动设备相对于参照点的相对坐标数据，实现移动设备空间坐标和取向的测定”具体以下步骤：

A1.所述三个以上定向模组同步接收各个参照点发出的光，并根据各个参照点的时间编码或空间编码识别出各个参照点的像和一维坐标；

A2.根据上述一维坐标计算移动设备和各参照点之间的距离以及相对取向；

A3.根据移动设备到各个参照点的距离和取向，确定移动设备的位置坐标和取向。

所述“所述三个以上定向模组同步接收各个参照点发出的光，并根据各个参照点的时间编码或空间编码识别出各个参照点的像和一维坐标”具体为：三个以上定向模组同步对各个参照点进行扫描，并通过其光电传感器接收经过柱透镜的各个参照点发出的光，且根据各个参照点的时间编码或空间编码区分各个参照点的像和计算各个参照点的像在各个定向模组的光电传感器上的坐标。所述“计算各个参照点的像在各个定向模组的光电传感器上的坐标”具体为：首先以各个定向模组的光电传感器所在直线的交点为原点，以其中一直线为X轴，建议直角坐标系，并以各个定向模组的光电传感器靠近原点一侧的像素点为像素起始坐标；然后各个参照点采用以下方法求出：

B1.根据参照点的像与三个以上定向模组的光电传感器的交点的位置建立三个以上平面的平面方程；

B2.依次选取其中不同的两个平面求解交线，和其余各个平面求解交点，得到三组以上该参照点的坐标；

B3.将该三组以上坐标的平均值作为该参照点的坐标。

本发明还提供了一种以多光源实现移动设备空间测位的装置，其特征在于，包括有三个以上时间编码不同(按不同规律闪烁)的点光源或空间编码不同(由多个反光点组成一个光点簇，簇与簇之间光点的空间分布不同)的点光源簇、三个以上定向模组和底板，所述三个以上定向模组互不平行地设于底板的上表面，所述三个以上的点光源或点光源簇设于三个以上定向模组和底板的上方，且不在同一直线上。

例如图1，本发明实施例所述以多光源实现移动设备空间测位的装置包括有三个点光源或点光源簇101、102、103和三个定向模组，且相邻两个定向模组之间的夹角为120°、呈均匀分布(当然也可以是其它角度，即三个定向模组可以呈非均匀分布)。其中，所述定向模组包括有柱透镜201、202、203和光电传感器301、302、303，而且光电传感器301、302、303安装在底板4的上表面，所述柱透镜201、202、203通过支撑杆204悬空地设置在底板4上、并位于光电传感器201、202、203的上方(最好位于光电传感器201、202、203的正上方、且其光学中心对准光电传感器201、202、203的感光面正中心。)。所述光电传感器201、202、203可以是线阵光电传感器(最好是千赫兹量级线阵光电传感器)，所述点光源可以是红外光照明光源，所述点光源簇可以是采用逆向反光结构的反光点。通过本实施例所述装置进行移动设备空间测位原理为：假设柱透镜201、202、203的焦距均为f毫米，其光学中心分别对准光电传感器301、302、303的感光面正中心，光电传感器301、302、303的感光像元长、宽均为a毫米；点光源或点光源簇101的像在光电传感器301、302、303上的坐标为u1、v1、w1；点光源或点光源簇102的像在光电传感器301、302、303上的坐标为u2、v2、w2；点光源或点光源簇103的像在光电传感器301、302、303上的坐标为u3、v3、w3；光电传感器301、302、303的中心像素坐标为c；所有坐标的单位均为像素。测定时，各测量单元同时接收各参照点(即点光源或点光源簇101、102、103)发出的光，根据各参照点的发光规律(闪烁规律或光点的空间分布规律)区分各参照点的像计算各个参照点的像在各个定向模组的光电传感器(光电传感器301、302、303)上的坐标。

下面以点光源或点光源簇101为例，如图2，以光电传感器301、302、303所在的三条直线的交点为原点，以301所在直线为X轴，建立直角坐标系；三个光电传感器301、302、303中靠近原点一侧的像素点为像素起始坐标；O1、O2、O3分别为光电传感器301、302、303的感光面中心点，距坐标原点距离均为r0；O1F1、O2F2、O3F3分别在光电传感器301、302、303的光轴上，长度为f毫米；S1、S2、S3是点光源或点光源簇101的像与传感器的交点；点光源或点光源簇101所在点为K点。

根据几何关系，K点是平面S1F1K、S2F2K、S3F3K的交点。K点坐标可按如下方法求出：根据S1、S2、S3的位置，建立上述三个平面的平面方程；选取其中的两个平面方程求解交线方程，再由该交线方程与剩余的一个平面方程求解交点；依次选取不同的两个平面求解交线，和第三个面求解交点，可得三组K点的坐标；最后以这三组坐标的平均值作为K点的坐标，也即点光源或点光源簇101在图2所示坐标系中的坐标(x1,y1,z1)。同时，根据同一K点的三组坐标的方差大小，还可判断K点坐标值的可靠程度(方差越小，该坐标值越可靠)。

以平面S1F1K和S2F2K求交线方程，再和平面S3F3K求交点为例，详细说明计算K点坐标的过程：

坐标系中各点坐标为：

求解平面S1F1K、S2F2K、S3F3K的法向量

由法向量和可求出S1F1K和S2F2K交线的方向向量

根据直线方程定义，在Z＝0平面内，601的直线方程l1由向量和点S1给出，602的直线方程l2由向量和点S2给出。解方二元一次程组{l1,l2}得到601、602交点坐标P：[px,py,pz]^T。

由向量和点P，可直接给出平面S1F1K和S2F2K交线的方程l3。

由法向量和点F3，可直接得到平面S3F3K的方程，将该方程与直线方程l3组成线性方程组，即可求解出K点坐标

之后，以平面S2F2K和S3F3K求交线方程，将该交线与平面S1F1K求交点，得到K点坐标以平面S3F3K和S1F1K求交线方程，将该交线与平面S2F2K求交点，得到K点坐标

最后，将三次求得的坐标取平均，得到K的准确坐标为：

根据上述计算方法，可同理计算出图1中光源102和103的三维坐标(x2,y2,z2)和(x3,y3,z3)。因此，上述原理可实现高速空间位置的测定。

由于光源在空间中不共线，光源坐标(x1,y1,z1)，(x2,y2,z2)，(x3,y3,z3)在空间中可以确定一个平面，该平面的法线方向，配合光源坐标，可以实现空间姿态的测定。

这样，通过本发明所述方法和装置即可对移动设备进行快速、高精度(可达毫秒级延时、毫米级精度测定)的空间位置与姿态测定，可在参照点足够的条件下实现全角度无死角的空间位置与姿态测定，同时环形阵列设置的三个以上定向模组测量得到的坐标等权重地参与参照点空间坐标的求解，从而使得求出来的参照点三维坐标的误差在各方向均匀地分布，避免误差分布在某一方向过大的情形出现，而且装置结构十分简单，生产加工容易成本低，有利于批量生产、普及推广使用。

另外，由于本发明所述以多光源实现移动设备空间测位的装置中光电传感器采样频率非常高，根据两次以上连续测得的空间坐标和姿态数据，可以较高精度预测下一时刻的位置与姿态。

比如：假定上一时刻点光源或点光源簇101、102、103的坐标分别为(x10,y10,z10)，(x20,y20,z20)，(x30,y30,z30)，可以预测下一时刻点光源或点光源簇101、102、103的坐标(x11,y11,z11)，(x21,y21,z21)，(x31,y31,z31)为：

然后根据预测的点光源或点光源簇101、102、103坐标(x11,y11,z11)，(x21,y21,z21)，(x31,y31,z31)，在空间中可以确定一个平面，该平面的法线方向，配合光源坐标，可以实现空间姿态的测定。这样，本发明所述以多光源实现移动设备空间测位的方法还可根据不同时刻的坐标、时间，可知对应时刻的速度、加速度等，利用连续的三个时刻的数据，可获得移动设备空间位置与姿态的预测，且预测精度极高。

以上内容是结合具体的优选技术方案对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的专业技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。