基于光電掃描的室內(nèi)移動(dòng)機(jī)器人位姿測(cè)量系統(tǒng)及測(cè)量方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種室內(nèi)移動(dòng)機(jī)器人位姿測(cè)量方法。特別是涉及一種基于光電掃描的 室內(nèi)移動(dòng)機(jī)器人位姿測(cè)量系統(tǒng)及測(cè)量方法。
【背景技術(shù)】
[0002] 室內(nèi)移動(dòng)機(jī)器人（AGV、移動(dòng)加工平臺(tái)等）現(xiàn)已被廣泛應(yīng)用于工業(yè)現(xiàn)場(chǎng)大部件裝配 領(lǐng)域。在工作過(guò)程中，人們需要對(duì)其位置及姿態(tài)進(jìn)行實(shí)時(shí)測(cè)量。目前，移動(dòng)機(jī)器人的三維坐 標(biāo)及姿態(tài)測(cè)量方法主要分為兩類：航跡推測(cè)法和絕對(duì)定位法。航跡推測(cè)主要依靠里程計(jì)、 陀螺儀等內(nèi)部本體感受傳感器，通過(guò)給定初始狀態(tài)，測(cè)量機(jī)器人相對(duì)于初始位姿的距離和 方向來(lái)確定機(jī)器人的位姿；絕對(duì)定位主要采用引導(dǎo)信標(biāo)、主動(dòng)或被動(dòng)標(biāo)識(shí)、地圖匹配或全球 定位系統(tǒng)進(jìn)行位姿測(cè)量。相比二者，航跡推測(cè)法具有能夠不依賴外部設(shè)備自主提供測(cè)量數(shù) 據(jù)的優(yōu)勢(shì)，使機(jī)器人本體與測(cè)量系統(tǒng)構(gòu)成統(tǒng)一整體，從而連續(xù)穩(wěn)定地輸出機(jī)器人自身的位 置及姿態(tài)信息，對(duì)本系統(tǒng)外的環(huán)境不造成任何的干擾。但由于航跡推測(cè)算法多基于積分運(yùn) 算，其測(cè)量誤差會(huì)隨時(shí)間的積累被逐步放大，并存在數(shù)據(jù)漂移現(xiàn)象；絕對(duì)定位法通常會(huì)在空 間中布設(shè)一個(gè)或多個(gè)發(fā)射基站和接收裝置，并以接收裝置作為探測(cè)目標(biāo)。通過(guò)發(fā)射基站向 測(cè)量空間發(fā)送不同形式的信號(hào)（光，聲，電磁等），在接收到這些信號(hào)后，接收裝置基于不同 的物理原理將它們進(jìn)行處理整合并轉(zhuǎn)換為自身的位姿信息，或直接反射回發(fā)射基站由發(fā)射 端處理解算。絕對(duì)定位法的測(cè)量精度與時(shí)間不相關(guān)聯(lián)，沒(méi)有誤差累積及數(shù)據(jù)漂移現(xiàn)象，因此 通常具有較高的精度及穩(wěn)定性。但是鑒于移動(dòng)機(jī)器人工作區(qū)域大，執(zhí)行任務(wù)復(fù)雜，為了確保 其高效流暢運(yùn)行，需要在工作空間布設(shè)多個(gè)發(fā)射基站以保證測(cè)量范圍，對(duì)現(xiàn)場(chǎng)環(huán)境要求較 高。另外，在構(gòu)建測(cè)量場(chǎng)的過(guò)程中，坐標(biāo)系的建立及不同發(fā)射基站之間的全局定向步驟相對(duì) 繁瑣。

【發(fā)明內(nèi)容】

[0003] 本發(fā)明所要解決的技術(shù)問(wèn)題是，提供一種用于在無(wú)法進(jìn)行多基站交會(huì)測(cè)量的工業(yè) 環(huán)境中的移動(dòng)機(jī)器人的基于光電掃描的室內(nèi)移動(dòng)機(jī)器人位姿測(cè)量系統(tǒng)及測(cè)量方法。
[0004] 本發(fā)明所采用的技術(shù)方案是：一種基于光電掃描的室內(nèi)移動(dòng)機(jī)器人位姿測(cè)量系 統(tǒng)，包括有移動(dòng)機(jī)器人，所述的移動(dòng)機(jī)器人上設(shè)置有激光發(fā)射站，所述的激光發(fā)射站的周邊 設(shè)置有至少3個(gè)用于接收激光發(fā)射站所發(fā)出的光信號(hào)的接收器，還設(shè)置有至少1個(gè)與所述 的接收器相連用于對(duì)所述的接收器所接收的信號(hào)進(jìn)行處理確定接收器在測(cè)量空間內(nèi)激光 發(fā)射站坐標(biāo)系下的精確坐標(biāo)的信號(hào)處理器，以及與所述的信號(hào)處理器無(wú)線連接通過(guò)確定激 光發(fā)射站到各個(gè)接收器之間的距離來(lái)確定移動(dòng)機(jī)器人的姿態(tài)角度和移動(dòng)機(jī)器人的位置的 終端計(jì)算機(jī)。
[0005] 每一個(gè)信號(hào)處理器連接有3~8個(gè)接收器。
[0006] -種基于光電掃描的室內(nèi)移動(dòng)機(jī)器人位姿測(cè)量系統(tǒng)的測(cè)量方法，包括如下步驟：
[0007] 1)建立全局導(dǎo)航坐標(biāo)系，并用激光跟蹤儀或室內(nèi)GPS精確測(cè)量出每個(gè)接收器在全 局導(dǎo)航坐標(biāo)系下的精確三維坐標(biāo)；
[0008] 2)每個(gè)接收器通過(guò)接收激光發(fā)射站紅外激光器發(fā)出的掃描激光信號(hào)與同步脈沖 激光器發(fā)出的同步脈沖激光信號(hào)，并發(fā)送給相連接的信號(hào)處理器，信號(hào)處理器分別計(jì)算相 連接的每個(gè)接收器在測(cè)量空間內(nèi)激光發(fā)射站坐標(biāo)系下的精確坐標(biāo)，包括兩個(gè)掃描角Θ u和 θι2及接收器相對(duì)于激光發(fā)射站坐標(biāo)系下的水平角a i和垂直角β i，并將計(jì)算結(jié)果存入終 端計(jì)算機(jī)；
[0009] 3)終端計(jì)算機(jī)根據(jù)不同接收器的精確坐標(biāo)，分別計(jì)算出激光發(fā)射站到各個(gè)接收器 之間的距離；
[0010] 4)在求定激光發(fā)射站到每個(gè)接收器的距離真值山后，每個(gè)接收器在激光發(fā)射站坐 標(biāo)系下的近似坐標(biāo)值表示為P i= Kcos Pi cos CXi, COS Pi sin CXi, SinPi)，以近似坐標(biāo)值 作為迭代初始值，進(jìn)而優(yōu)化解算得出每個(gè)接收器在激光發(fā)射站坐標(biāo)系下的精確坐標(biāo)值；
[0011] 5)通過(guò)步驟4)求得的接收器在激光發(fā)射站坐標(biāo)系下的精確坐標(biāo)和步驟1)建立的 每個(gè)接收器在全局導(dǎo)航坐標(biāo)系下的三維坐標(biāo)，經(jīng)過(guò)坐標(biāo)系轉(zhuǎn)換，得到激光發(fā)射站坐標(biāo)系相 對(duì)于全局導(dǎo)航坐標(biāo)系的姿態(tài)矩陣及平移矩陣；
[0012] 6)通過(guò)姿態(tài)矩陣獲得移動(dòng)機(jī)器人的姿態(tài)角度，通過(guò)平移矩陣獲得移動(dòng)機(jī)器人的位 置。
[0013] 步驟2)所述的兩個(gè)掃描角Θ u和θ 12及水平角α滿垂直角β馮采用如下公式 獲得：
[0014]
[0015]
[0016]
[0017]
[0018]
[0019] 其中，ω代表激光發(fā)射站轉(zhuǎn)子的旋轉(zhuǎn)角速度；tu和112分別表示激光發(fā)射站的第1 束和第2束光平面分別掃過(guò)第i個(gè)接收器所用的時(shí)間；Ii ll和η 12分別表示激光發(fā)射站的第 一束和第二束光平面分別掃過(guò)第i個(gè)接收器時(shí)光平面的法矢量，它們的叉乘運(yùn)算結(jié)果込由 (r ix riy riz)表示。
[0020] 步驟3)所述的計(jì)算激光發(fā)射站到各個(gè)接收器之間的距離是采用如下公式獲得： 通過(guò)任意兩個(gè)接收器可以列出如下方程：
[0021]
[0022] 若多于三個(gè)接收器收到發(fā)射站的激光信號(hào)，即可列出三個(gè)形如上式的方程，未知 數(shù)和方程個(gè)數(shù)相同，進(jìn)而可以求解出發(fā)射站分別到三個(gè)接收器的距離山，i = 1，2, 3。其中， Θ ^表示激光發(fā)射站與兩個(gè)接收器（i和j)連線之間的夾角，可通過(guò)如下公式計(jì)算：
[0023] 〇'
[0024] 步驟5)中所述的姿態(tài)矩陣和平移矩陣可通過(guò)如下公式獲得：
[0025]
[0026] 其中，G代表姿態(tài)矩陣，IT代表平移矩陣，假設(shè)測(cè)量空間中共有i個(gè)接收器收到了 激光發(fā)射站的光信號(hào)，G與S分別為接收器在導(dǎo)航坐標(biāo)系與激光發(fā)射站坐標(biāo)系下的三維坐 標(biāo)點(diǎn)矩陣，表示為：
[0027]
[0028]
[0029] 求解如下最小二乘問(wèn)題，即可的到er和?；》的最終解：
[0030] 'a'
[0031] 步驟6)所述的移動(dòng)機(jī)器人的姿態(tài)角度是根據(jù)步驟5)所求出的姿態(tài)矩陣得出：
[0032]
[0033]
[0034]
[0035] 其中，Φ表示航向角度，Θ表示俯仰角度，γ表示橫滾角度。
[0036] 步驟6)所述的移動(dòng)機(jī)器人的位置是直接通過(guò)平移矩陣獲得，將平移矩陣?yán)瓕?xiě)成：
[0037]
[0038] 則平移矩陣的三個(gè)分量即為移動(dòng)機(jī)器人在導(dǎo)航坐標(biāo)系下的三維坐標(biāo)。
[0039] 本發(fā)明的基于光電掃描的室內(nèi)移動(dòng)機(jī)器人位姿測(cè)量系統(tǒng)及測(cè)量方法，是一種高精 度、高效率且便捷的位姿測(cè)量系統(tǒng)和方法。操作者在測(cè)量時(shí)無(wú)需在空間中布設(shè)多個(gè)發(fā)射基 站，亦無(wú)需經(jīng)過(guò)繁瑣的全局定向，依靠由光電接收器構(gòu)成的多個(gè)引導(dǎo)信標(biāo)及固定在移動(dòng)機(jī) 器人機(jī)身上的高速激光掃描轉(zhuǎn)臺(tái)，實(shí)現(xiàn)對(duì)室內(nèi)移動(dòng)機(jī)器人進(jìn)行三維空間位置及姿態(tài)的實(shí)時(shí) 測(cè)量。本發(fā)明具有如下特點(diǎn)：
[0040] 1、系統(tǒng)結(jié)構(gòu)簡(jiǎn)單：本發(fā)明所提供的系統(tǒng)是基于單一移動(dòng)測(cè)量基站輔以引導(dǎo)信標(biāo)的 工作原理，比起其它需要多個(gè)測(cè)站的位姿測(cè)量系統(tǒng)，節(jié)約了成本，簡(jiǎn)化了工作步驟；
[0041] 2、高精度：本系統(tǒng)的坐標(biāo)測(cè)量誤差小于3_，姿態(tài)角度測(cè)量誤差小于0. Γ。精度 遠(yuǎn)高于激光雷達(dá)等其它基于引導(dǎo)信標(biāo)的測(cè)量系統(tǒng)；
[0042] 3、受現(xiàn)場(chǎng)環(huán)境影響小：由于作為引導(dǎo)信標(biāo)的接收器的體積很小，其可以根據(jù)測(cè)量 任務(wù)被放置在任何通視條件較好的位置。另外，固定在機(jī)器人上并跟隨其移動(dòng)的發(fā)射站采 用全空間旋轉(zhuǎn)掃描的工作方式，可以探測(cè)360°視場(chǎng)范圍，進(jìn)一步放寬了接收器布設(shè)位置的 要求。
【附圖說(shuō)明】
[0043] 圖1是本發(fā)明的基于光電掃描的室內(nèi)移動(dòng)機(jī)器人位姿測(cè)量系統(tǒng)的結(jié)構(gòu)示意圖；
[0044] 圖2是基于光電掃描的室內(nèi)移動(dòng)機(jī)器人位姿測(cè)量系統(tǒng)中激光發(fā)射站的結(jié)構(gòu)示意 圖；
[0045] 圖3是基于光電掃描的室內(nèi)移動(dòng)機(jī)器人位姿測(cè)量系統(tǒng)中接收器的結(jié)構(gòu)示意圖；
[0046] 圖4是全局導(dǎo)航坐標(biāo)系；
[0047] 圖5是激光發(fā)射站坐標(biāo)系；
[0048] 圖6是待求解坐標(biāo)系。
[0049] 圖中
[0050] 1 :移動(dòng)機(jī)器人 2 :激光發(fā)射站
[0051] 3:接收器 4:信號(hào)處理器
[0052] 5 ;終端計(jì)算機(jī) 21 :光平面
[0053] 22 :旋轉(zhuǎn)軸 23 :轉(zhuǎn)子
[0054] 24:同步脈沖激光器 25:基座
[0055] 26 :紅外激光器 27 :接收器外殼
[0056] 28 :接收器光敏中心
【具體實(shí)施方式】
[0057] 下面結(jié)合實(shí)施例和附圖對(duì)本發(fā)明的基于光電掃描的室內(nèi)移動(dòng)機(jī)器人位姿測(cè)量系 統(tǒng)及測(cè)量方法做出詳細(xì)說(shuō)明。
[0058] 使用一臺(tái)小型高速旋轉(zhuǎn)平臺(tái)（發(fā)射站）輔以一定數(shù)量的引導(dǎo)信標(biāo)，以已知每個(gè)引 導(dǎo)信標(biāo)在全局坐標(biāo)系下精確的三維坐標(biāo)為前提，通過(guò)測(cè)量發(fā)射站相對(duì)于每個(gè)引導(dǎo)信標(biāo)的空 間掃描角度，結(jié)合發(fā)射站的精確內(nèi)部參數(shù)完成自身的位姿測(cè)量。
[0059] 本發(fā)明，將一個(gè)可以向全空間發(fā)射激光信號(hào)的小型高速旋轉(zhuǎn)平臺(tái)（發(fā)射站）固定 在移動(dòng)機(jī)器人機(jī)體上，在跟隨機(jī)器人移動(dòng)的同時(shí)，其以掃描的方式持續(xù)將激光光信號(hào)發(fā)射 到工作空間。與此同時(shí)，多個(gè)引導(dǎo)信標(biāo)（接收器）被分散放置在發(fā)射站能夠探測(cè)到的工作 區(qū)域。當(dāng)發(fā)射站發(fā)出的激光信號(hào)掃過(guò)引導(dǎo)信標(biāo)時(shí)，信標(biāo)內(nèi)部的光電傳感器，將激光信號(hào)轉(zhuǎn)變 成電信號(hào)，經(jīng)后續(xù)處理轉(zhuǎn)變成掃描角度信息。以掃描角度作為基本觀測(cè)量，并根據(jù)發(fā)射站的 內(nèi)部參數(shù)及引導(dǎo)信標(biāo)在全局坐標(biāo)系下的精確坐標(biāo)（導(dǎo)航任務(wù)開(kāi)始前事先標(biāo)定好），最終優(yōu) 化求解發(fā)射站在全局坐標(biāo)系下的三維坐標(biāo)及姿態(tài)角度。
[0060] 如圖1、圖2所示，本發(fā)明的基于光電掃描的室內(nèi)移動(dòng)機(jī)器人位姿測(cè)量系統(tǒng)，包括 有移動(dòng)機(jī)器人1，所述的移動(dòng)機(jī)器人1上設(shè)置有激光發(fā)射站2,所述的激光發(fā)射站2的周邊 設(shè)置有至少3個(gè)用于接收激光發(fā)射站2所發(fā)出的光信號(hào)的接收器3,還設(shè)置有至少1個(gè)與所 述的接收器3相連用于對(duì)所述的接收器3所接收的信號(hào)進(jìn)行處理確定接收器3在測(cè)量空間 內(nèi)激光發(fā)射站坐標(biāo)系下的精確坐標(biāo)的信號(hào)處理器4,以及與所述的信號(hào)處理器4無(wú)線連接 通過(guò)確定激光發(fā)射站到各個(gè)接收器之間的距離來(lái)確定移動(dòng)機(jī)器人的姿態(tài)角度和移動(dòng)機(jī)器 人的位置的終端計(jì)算機(jī)5。每一個(gè)信號(hào)處理器4可以連接有3~8個(gè)接收器3。
[0061] 本發(fā)明所述的激光發(fā)射站2和接收器3是采用申請(qǐng)?zhí)枮?01210126759. 5中所公 開(kāi)的結(jié)構(gòu)。其中：
[0062] 如圖2所示，激光發(fā)射站主要由基座25和轉(zhuǎn)子23兩部分組成：轉(zhuǎn)子23位于激光 發(fā)射站頂部，可以高速旋轉(zhuǎn)，轉(zhuǎn)子上安裝有兩個(gè)線性紅外激光器26,這樣即可形成兩束對(duì)測(cè) 量空間高速掃描的扇形紅外激光平面；基站上還安裝有一圈同步脈沖激光器24,該激光器 可在轉(zhuǎn)子轉(zhuǎn)過(guò)某一特定位置時(shí)發(fā)出脈沖激光，形成一個(gè)計(jì)時(shí)基準(zhǔn)。這樣，接收器通過(guò)接收同 步脈沖激光與扇形掃