[發(fā)明專(zhuān)利]一種模塊化舞臺(tái)激光定位方法

在審

• 申請(qǐng)?zhí)枺?/td>: 201710631289.0
• 申請(qǐng)日: 2017-07-28
• 公開(kāi)（公告）號(hào): CN107817500A
• 公開(kāi)（公告）日: 2018-03-20
• 發(fā)明（設(shè)計(jì)）人: 楊佳琦;張文安;倪洪杰;黃光普
• 申請(qǐng)（專(zhuān)利權(quán)）人: 浙江工業(yè)大學(xué)
• 主分類(lèi)號(hào): G01S17/42
• 分類(lèi)號(hào): G01S17/42
• 代理公司: 杭州浙科專(zhuān)利事務(wù)所(普通合伙)33213
• 代理人: 杜立

1.一種模塊化舞臺(tái)激光定位方法，所述方法包括以下步驟：

1)：建立地圖

在舞臺(tái)周邊放置M根反光柱；反光柱個(gè)數(shù)大于等于3，小于20；以舞臺(tái)中心作為坐標(biāo)原點(diǎn)建立全局的直角坐標(biāo)系，各反光柱在全局直角坐標(biāo)系中的坐標(biāo)用集合GF＝{F_Gi(X_Gi,Y_Gi)|i＝1,2,…,M}表示，各反光柱之間的距離用集合GD＝{D_Gij|i,j＝1,2,…,M}表示；集合GF和集合GD中的數(shù)據(jù)即為地圖數(shù)據(jù)；

2)：使用靜態(tài)定位方式確定模塊化舞臺(tái)位姿

激光傳感器的靜態(tài)定位方式不依賴(lài)于上一時(shí)刻的位姿，在任意時(shí)刻開(kāi)始定位，用于確定模塊化舞臺(tái)初始位姿；模塊化舞臺(tái)上的激光傳感器掃描周?chē)恢埽阅K化舞臺(tái)中心為坐標(biāo)原點(diǎn)，激光傳感器0度方向?yàn)閅軸正方向建立局部坐標(biāo)系；利用激光原始數(shù)據(jù)計(jì)算出各個(gè)反光柱在局部坐標(biāo)系中的坐標(biāo)，用集合LF＝{F_Li(X_Li,Y_Li)|i＝1，2，…,n；n≤M}表示；計(jì)算局部坐標(biāo)系中的各個(gè)反光柱之間距離，用集合LD＝{D_Lij|i,j＝1,2,…,n；n≤M}表示；設(shè)模塊化舞臺(tái)在全局坐標(biāo)系中的位姿為P_k(X_k,Y_k,θ_k)；將集合GD的距離值與集合LD中的距離值作比較，獲得局部坐標(biāo)系中的反光柱坐標(biāo)與全局坐標(biāo)系中的反光柱坐標(biāo)的對(duì)應(yīng)關(guān)系用集合GLF＝{C_ij|i,j＝1,2,…,n；n≤M}；利用基于最小二乘法的位姿計(jì)算公式得P_k；計(jì)算公式參見(jiàn)式(1)：

其中，矩陣A中數(shù)據(jù)為局部坐標(biāo)系中反光柱坐標(biāo)，矩陣b中數(shù)據(jù)為與局部坐標(biāo)系中的反光柱相對(duì)應(yīng)的全局坐標(biāo)系中的坐標(biāo)；

利用最小二乘法求解公式獲得矩陣H的值，求解公式參見(jiàn)式(2)：

H＝[cosθ_k sinθ_k X_k Y_k]^T＝(A^T·A)^-1·A^T·b (2)

其中，矩陣H中含有模塊化舞臺(tái)的位姿數(shù)據(jù)，根據(jù)矩陣H求得舞臺(tái)位姿P_k(X_k,Y_k,θ_k)；

基于最小二乘法的位姿計(jì)算公式相對(duì)于三點(diǎn)定位法定位更加精確，該公式利用多個(gè)反光柱坐標(biāo)進(jìn)行位姿計(jì)算，實(shí)現(xiàn)誤差的平方和最小化；

3)采用動(dòng)態(tài)定位方式確定模塊化舞臺(tái)位姿

a.估計(jì)下一時(shí)刻舞臺(tái)位姿及反光柱在局部坐標(biāo)系中的位置

模塊化舞臺(tái)的動(dòng)態(tài)定位方式需要獲取前一時(shí)刻的位姿以及舞臺(tái)的運(yùn)動(dòng)狀態(tài)；根據(jù)前兩次的位置估算出下一時(shí)刻的位姿，位姿估計(jì)公式參見(jiàn)式(3)：

根據(jù)估算的位姿以及平面直角坐標(biāo)系坐標(biāo)轉(zhuǎn)換矩陣，獲得全局坐標(biāo)系中的反光柱在下一時(shí)刻局部坐標(biāo)系中的坐標(biāo)，坐標(biāo)轉(zhuǎn)換公式參見(jiàn)式(4):

其中，為全局坐標(biāo)系中的第i個(gè)反光柱在局部坐標(biāo)系中的估計(jì)坐標(biāo)，矩陣R為直角坐標(biāo)系的旋轉(zhuǎn)平移矩陣，[X_Gi Y_Gi]^T為在全局坐標(biāo)系中第i個(gè)反光柱的坐標(biāo)，為模塊化舞臺(tái)在全局坐標(biāo)系中的估計(jì)坐標(biāo)，為模塊化舞臺(tái)在全局坐標(biāo)系中的估計(jì)角度；

b.使用反光柱估計(jì)的位置與掃描到的位置進(jìn)行匹配

反光柱在局部坐標(biāo)系中的估計(jì)位置用集合表示；使用激光傳感器進(jìn)行一次掃描，獲得反光柱在局部坐標(biāo)系中的實(shí)際位置，用集合LF＝{F_Li(X_Li,Y_Li)|i＝1,2,…,n；n≤M}表示，匹配集合LE和集合LF中的反光柱位置，當(dāng)兩個(gè)集合中的反光柱位置差距小于閾值，則代表兩反光柱匹配成功；匹配判斷公式參見(jiàn)式(5)

其中，F(xiàn)_Li為采集到的反光柱在局部坐標(biāo)系中的實(shí)際位置，為估計(jì)的反光柱在局部坐標(biāo)系中的位置，ε為閾值；

采用動(dòng)態(tài)匹配將干擾反射源剔除，提高定位穩(wěn)定性和精度；因?yàn)樾枰獙⒉杉降臄?shù)據(jù)與估計(jì)值匹配，而干擾反射源與估計(jì)值差距較大無(wú)法匹配成功，所以可以剔除干擾反射源；

c.更新模塊化舞臺(tái)位姿及反光柱坐標(biāo)

經(jīng)過(guò)匹配，篩選出有效的反光柱位置，使用基于最小二乘法的位姿計(jì)算公式計(jì)算獲得模塊化舞臺(tái)的位姿。

2.根據(jù)權(quán)利要求1所述的一種模塊化舞臺(tái)激光定位方法，其特征在于，模塊化舞臺(tái)的定位過(guò)程，包括以下步驟：

1)：建立地圖，在舞臺(tái)周邊布置反光柱，以舞臺(tái)中心為坐標(biāo)原點(diǎn)建立直角坐標(biāo)系；使用全站儀獲取反光柱在全局坐標(biāo)系中的坐標(biāo)，將反光柱位置數(shù)據(jù)輸入底層控制計(jì)算機(jī)并計(jì)算各反光柱之間的距離，保存反光柱位置數(shù)據(jù)和距離數(shù)據(jù)，建圖完畢；

2)：初始化位姿，在上位機(jī)控制軟件中點(diǎn)擊“初始化位姿”按鈕，模塊化舞臺(tái)中的控制計(jì)算機(jī)啟動(dòng)激光傳感器，獲取反光柱位置數(shù)據(jù)，并計(jì)算反光柱之間的距離；將獲取的反光柱位置數(shù)據(jù)和計(jì)算得到的距離數(shù)據(jù)與之前保存的地圖數(shù)據(jù)匹配；使用基于最小二乘法的位姿計(jì)算公式計(jì)算匹配的數(shù)據(jù)，獲得模塊化舞臺(tái)的位姿；

3)：實(shí)時(shí)定位，當(dāng)確定模塊化舞臺(tái)位姿后，點(diǎn)擊上位機(jī)中“實(shí)時(shí)定位”按鈕；模塊化舞臺(tái)底層控制計(jì)算機(jī)使用動(dòng)態(tài)定位算法實(shí)時(shí)處理激光傳感器的數(shù)據(jù)，計(jì)算舞臺(tái)位姿并將位姿上傳到上位機(jī)軟件中顯示。