[發(fā)明專利]一種基于圖像測量的移動機器人多階段平穩(wěn)自主對接方法

有效

• 申請?zhí)枺?/td>: 202110236423.3
• 申請日: 2021-03-03
• 公開（公告）號: CN113103225B
• 公開（公告）日: 2022-06-10
• 發(fā)明（設計）人: 江濤;謝澤宇;鄭志;蘇曉杰;肖洋;竇明星
• 申請（專利權(quán)）人: 重慶大學
• 主分類號: B25J9/16
• 分類號: B25J9/16;B25J19/02
• 代理公司: 廣州市華學知識產(chǎn)權(quán)代理有限公司 44245
• 代理人: 張晨

1.一種基于圖像測量的移動機器人多階段平穩(wěn)自主對接方法，其特征在于，包括如下步驟：

A1、在移動機器人上放置AprilTag標簽和攝像頭，其朝向分別與捕獲端和主動端相同，通過攝像頭獲得其它移動機器人AprilTag標簽相對于自身主動端的位姿信息；

A2、通過參數(shù)設計獲得改進跟蹤微分器，采用改進的跟蹤微分器實現(xiàn)對原始位姿信息的跟蹤和濾波；

A3、通過軌跡規(guī)劃，使兩個移動機器人的對接機構(gòu)趨于一條直線上，為精確對接做準備;

A4、設計對接控制律，輸入攝像頭的實時反饋數(shù)據(jù)，上位機控制兩個移動機器人的相對距離和姿態(tài)偏差角變量，最終實現(xiàn)精確對接；

改進跟蹤微分器設計如下：

選取k個數(shù)據(jù)為一隊列，隊列內(nèi)數(shù)據(jù)的幅值和頻率可以描述數(shù)據(jù)的穩(wěn)定性；

用標準差來定量描述幅值的穩(wěn)定性，數(shù)據(jù)的頻率穩(wěn)定性則用隊內(nèi)離群點數(shù)據(jù)的統(tǒng)計值來描述；

計算隊列內(nèi)的離群點閾值，，為閾值系數(shù)，取大于3；

統(tǒng)計隊列內(nèi)幅值大于離群點閾值s的數(shù)據(jù)個數(shù)n；

假設離群點按最大頻率分布，可以得到隊列數(shù)據(jù)最大頻率值為，其中m為隊列長度；

利用以上兩個幅值和頻率的定量描述值，可以得到隊列速度因子r的表達式：

；

步驟A4具體實現(xiàn)如下：

A401、設計對接控制律，輸入攝像頭的實時反饋數(shù)據(jù)，上位機控制兩個移動機器人的相對距離和姿態(tài)偏差角等變量，使這些變量收斂為0；

A402、使對接主動方移動機器人的攝像頭中心和對接被動方移動機器人的AprilTag標簽中心的直線距離為，對接主動方移動機器人朝向與兩中心連線的夾角為，姿態(tài)偏差角為，兩個角度之和為， 可得：，， 設為對接主動方移動機器人的速度；

A403、采用一個正定二次型李雅普諾夫函數(shù)：

其中，和分別代表了距離誤差和角度誤差，對該函數(shù)求導，可得：

式子可化為：

為了使和為非正，從而李雅普諾夫函數(shù)收斂，則線速度取，角速度取，得：

可以得出末端對接過程的控制律為，，其中，，都是攝像頭能實時獲得到的位姿信息中所包含的數(shù)據(jù)；

A404、上位機根據(jù)攝像頭的反饋信息和該控制律來下發(fā)線速度和角速度給對接主動方機器人，完成自主對接。

2.根據(jù)權(quán)利要求1所述的一種基于圖像測量的移動機器人多階段平穩(wěn)自主對接方法，其特征在于，步驟A1具體實現(xiàn)如下：

A101、輸入攝像頭拍攝的含有AprilTag標簽的彩色圖像；

A102、輪廓查找，自適應閾值分割，求解連通域，尋找可能組成AprilTag標簽的輪廓；

A103、擬合四邊形，對輪廓查找到的輪廓，進行直線擬合；

A104、位姿輸出，解碼并通過編碼庫判斷四邊形是否是AprilTag標簽，若是，用攝像頭內(nèi)參和標簽的物理邊長構(gòu)建位姿數(shù)據(jù)的PnP方程，求解得到標簽在相機坐標系里的位置坐標以及在三個坐標軸上的旋轉(zhuǎn)角度（yaw、pitch、roll）。

3.根據(jù)權(quán)利要求1所述的一種基于圖像測量的移動機器人多階段平穩(wěn)自主對接方法，其特征在于，所述跟蹤微分器二階離散形式為：

其中，；；；；；為輸入信號；為跟蹤數(shù)據(jù)；為的導數(shù)；為速度因子，h為濾波因子。

4.根據(jù)權(quán)利要求1所述的一種基于圖像測量的移動機器人多階段平穩(wěn)自主對接方法，其特征在于，采用改進的跟蹤微分器來對原始位姿信息的跟蹤和濾波，具體步驟如下：

A201、確定隊列內(nèi)數(shù)據(jù)個數(shù)，將隊列更新周期設定為AprilTag算法反饋位姿信息的周期；

A202、計算隊內(nèi)標準差，設定離群點閾值，統(tǒng)計隊內(nèi)離群點數(shù)量，最后計算速度因子；

A203、更新隊內(nèi)數(shù)據(jù)，剔除隊首數(shù)據(jù)，隊尾加入新的位姿數(shù)據(jù)，用速度因子不斷更新的跟蹤微分器對位姿信息進行濾波。

5.根據(jù)權(quán)利要求1所述的一種基于圖像測量的移動機器人多階段平穩(wěn)自主對接方法，其特征在于，步驟A3的具體實現(xiàn)如下：

A 301、對接主動方移動機器人根據(jù)位姿信息中的姿態(tài)偏差角來進行軌跡規(guī)劃，通過設定起點角度與終點角度以及時間，進行多項式插補，上位機下發(fā)角速度控制對接主動方移動機器人旋轉(zhuǎn)至與對接被動方移動機器人呈垂直狀態(tài)；

A302、根據(jù)相對位置坐標再進行軌跡規(guī)劃得到的線速度及角速度來使對接主動方移動機器人直線前進一段相應的距離，再旋轉(zhuǎn)90°，使該移動機器人的主動端朝向?qū)颖粍臃揭苿訖C器人的捕獲端。