以多光源實現移動設備空間測位的方法與裝置與流程

本發明屬于移動設備的空間定位與姿態測量技術領域，尤其涉及一種以多光源實現移動設備空間測位的方法與裝置。

背景技術：

目前，空間位置與姿態測量的方法，按其測量原理分類，主要有以下幾種：一、陀螺儀；二、視覺測量；三、結構光測量。

其中，陀螺儀可以同時測定承載它的移動設備的姿態(朝向)和加速度，但是移動設備的空間(相對)坐標只能通過對加速度的二次積分獲得，精度較低。視覺測量方法，需要使用復雜的算法來識別對應點，運算量較大；同時，面陣圖像傳感器的采集速率有限(一般不超過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)，在空間中可以確定一個平面，該平面的法線方向，配合光源坐標，可以實現空間姿態的測定。這樣，本發明所述以多光源實現移動設備空間測位的方法還可根據不同時刻的坐標、時間，可知對應時刻的速度、加速度等，利用連續的三個時刻的數據，可獲得移動設備空間位置與姿態的預測，且預測精度極高。

以上內容是結合具體的優選技術方案對本發明所作的進一步詳細說明，不能認定本發明的具體實施只局限于這些說明。對于本發明所屬技術領域的專業技術人員來說，在不脫離本發明構思的前提下，還可以做出若干簡單推演或替換，都應當視為屬于本發明的保護范圍。