[發(fā)明專利]一種基于空間多點約束的AGV動態(tài)位姿測量方法

有效

• 申請?zhí)枺?/td>: 201810151783.1
• 申請日: 2018-02-13
• 公開（公告）號: CN108489382B
• 公開（公告）日: 2020-02-18
• 發(fā)明（設(shè)計）人: 任永杰;邾繼貴;林嘉睿;楊凌輝;趙顯
• 申請（專利權(quán)）人: 天津大學(xué)
• 主分類號: G01B11/00
• 分類號: G01B11/00
• 代理公司: 天津市北洋有限責(zé)任專利代理事務(wù)所 12201
• 代理人: 李麗萍

1.一種基于空間多點約束的AGV動態(tài)位姿測量方法，其特征在于，包括以下步驟：

步驟一、發(fā)射站和空間約束點的布置：

測量系統(tǒng)由一個發(fā)射站和4個以上的空間約束點組成，所述多個空間約束點按照包絡(luò)AGV運行軌跡且不在一個平面內(nèi)布局，所述空間約束點均采用球形接收器并通過球座固定，

步驟二、獲得各空間約束點在發(fā)射站坐標(biāo)系的坐標(biāo)：

所述發(fā)射站和多個空間約束點之間具有兩種約束，分別為光平面約束和距離約束；

所述光平面約束指的是接收器接收到的發(fā)射站發(fā)出的激光掃描角度信息和發(fā)射站內(nèi)部參數(shù)共同構(gòu)建的平面方程，其數(shù)學(xué)表達式為：

式(1)中， (A_ij B_ij C_ij D_ij)表示發(fā)射站激光平面的內(nèi)參數(shù)；表示的是發(fā)射站轉(zhuǎn)子繞轉(zhuǎn)軸旋轉(zhuǎn)的姿態(tài)變換矩陣；θ_ij指的是發(fā)射站掃描角度，θ_ij由接收器收到的激光信號間隔以及發(fā)射站轉(zhuǎn)子的轉(zhuǎn)速得到；R_i和T_i為發(fā)射站外參數(shù)，由全局定向得到；(X_i Y_i Z_i)^T為空間約束點在全局導(dǎo)航坐標(biāo)系下的坐標(biāo)，該(X_i Y_i Z_i)^T由激光跟蹤儀測得；

所述距離約束的數(shù)學(xué)表達式為：

式(7)中，多個空間約束點在發(fā)射站坐標(biāo)系下的坐標(biāo)R_n(X_n，Y_n，Z_n)由非線性最優(yōu)化求得：

從而，求得各空間約束點在發(fā)射站坐標(biāo)系的坐標(biāo)；

步驟三、結(jié)合空間約束點在全局導(dǎo)航坐標(biāo)系的坐標(biāo)(X_i Y_i Z_i)^T和步驟二獲得的各空間約束點在發(fā)射站坐標(biāo)系的坐標(biāo)解算出發(fā)射站坐標(biāo)系和全局導(dǎo)航坐標(biāo)系之間的轉(zhuǎn)換關(guān)系，

設(shè)第i個約束點在發(fā)射站坐標(biāo)系和全局導(dǎo)航坐標(biāo)系下的坐標(biāo)分別是P_i和Q_i，那么兩個坐標(biāo)系之間的坐標(biāo)轉(zhuǎn)換關(guān)系為：

式(8)中，R為旋轉(zhuǎn)矩陣，T為平移矩陣；

R為正交矩陣，式(9)中各元素與發(fā)射站坐標(biāo)系相對于全局導(dǎo)航坐標(biāo)系的轉(zhuǎn)動角度α，β，γ的關(guān)系如下：

T＝(x y z)^T，表示的是發(fā)射站坐標(biāo)系原點到全局導(dǎo)航坐標(biāo)系原點的平移量，(α,β,γ,x,y,z)為發(fā)射站坐標(biāo)系相對于全局導(dǎo)航坐標(biāo)系之間的位姿。

2.根據(jù)權(quán)利要求1所述基于空間多點約束的AGV動態(tài)位姿測量方法，其特征在于，

步驟二中，由激光跟蹤儀測得空間約束點在全局導(dǎo)航坐標(biāo)系下的坐標(biāo)(X_i Y_i Z_i)^T的具體內(nèi)容如下：

使用激光跟蹤儀在空間中設(shè)站測量各空間約束點的三維坐標(biāo)，將所述激光跟蹤儀依次移動到新的站位，再次測量各空間約束點的三維坐標(biāo)，激光跟蹤儀的站位擺放滿足每個站位能觀測到3個以上的空間約束點，且空間約束點的數(shù)目和激光跟蹤儀的站位數(shù)滿足測長誤差方程的建立條件：mn＞3(m+n)，其中，m是激光跟蹤儀的測量站位數(shù)量，n是空間約束點的數(shù)量；

將激光跟蹤儀某個站位下的測站坐標(biāo)系定義為全局導(dǎo)航坐標(biāo)系，其他站位下的測站坐標(biāo)系相對于全局導(dǎo)航坐標(biāo)系的關(guān)系為：

P_i＝RQ_i+T (2)

式(2)中，P_i為全局導(dǎo)航坐標(biāo)系下空間約束點的三維坐標(biāo)，Q_i為激光跟蹤儀在其他站位下的測站坐標(biāo)系下空間約束點的三維坐標(biāo)測量值；

結(jié)合配準(zhǔn)算法計算得到激光跟蹤儀所有站位間的定位關(guān)系；

然后，利用激光跟蹤儀高精度測距對全局約束點的三維坐標(biāo)值進行改正優(yōu)化，過程如下：

假設(shè)空間中存在n個全局約束點，三維坐標(biāo)為(x_i,y_i,z_i),i＝1,2,...,n；激光跟蹤儀在空間中通過m個站位對這n個約束點進行測量每個站位三維坐標(biāo)為(X_k,Y_k,Z_k),k＝1,2,...,m；

在測量每個站位三維坐標(biāo)過程中，對于每個站位和全局約束點，它們之間的距離r_ik是激光跟蹤儀高精度的干涉測距值，假設(shè)為真值，r_ik的實際測量值l_ik表示如下：

對式(3)進行線性化：

式(4)中，Δx_i和ΔX_k就是全局約束點和激光跟蹤儀站位坐標(biāo)的優(yōu)化改正值，初值和由之前激光跟蹤儀站位之間的初步定向來確定，利用式(4)建立測長誤差方程：

將式(5)轉(zhuǎn)化為線性方程組的形式，如下：

V＝AΔX-b (6)

式(6)中，矩陣A是由式(3)泰勒展開的一階求導(dǎo)項所組成的大型稀疏矩陣，

ΔX＝(ΔX₁,ΔY₁,ΔZ₁,ΔX₂,ΔY₂,ΔZ₂,...,ΔX_m,ΔY_m,ΔZ_m,Δx₁,Δy₁,Δz₁,Δx₂,Δy₂,Δz₂,...,Δx_n,Δy_n,Δz_n)^T

由于矩陣A是病態(tài)矩陣，條件數(shù)極大，因此，對矩陣A進行奇異值分解或者QR分解來求解方程組，結(jié)合相應(yīng)的迭代條件得到最終的三維坐標(biāo)優(yōu)化改正值。