一種基于關(guān)鍵幀策略的移動機器人視覺伺服控制方法
【技術(shù)領域】
[0001] 本發(fā)明涉及一種機器人領域的視覺導航方法，特別是涉及一種基于關(guān)鍵幀策略的 移動機器人視覺伺服控制方法。
【背景技術(shù)】
[0002] 移動機器人視覺導航技術(shù)是以視覺系統(tǒng)作為反饋信息控制移動機器人運動的方 法，是移動機器人研宄的重要部分。視覺導航是目前移動機器人研宄中的熱點和難點，在機 器人執(zhí)行大環(huán)境或復雜環(huán)境下任務時起到重要的作用。
[0003] 移動機器人視覺導航技術(shù)可以分為基于位置的方法和基于圖像的方法。其中，基 于位置的方法將視覺系統(tǒng)作為位置傳感器使用，使用路標或視覺里程計等方式對機器人進 行定位，并依此控制機器人運動，這種方法控制器設計簡單，適用于大范圍的運動，但是導 航精度差，具有誤差累積效應。基于圖像的方法使用視覺系統(tǒng)反饋的圖像作為反饋信息，控 制機器人運動到拍攝到與目標圖像一致的場景處，這種方法導航精度高，但是需要對系統(tǒng) 在給定軌跡上預先進行訓練，而且要求目標圖像在整個運動過程中都可見。
【發(fā)明內(nèi)容】
[0004] 本發(fā)明為了克服現(xiàn)有技術(shù)的不足，提供了一種基于關(guān)鍵幀策略的移動機器人視 覺伺服控制方法，該方法在保證導航精度的同時，不要求預先對機器人在給定軌跡上進行 訓練，同時也不要求在運動過程中目標持續(xù)可見，只需要目標位置和參考軌跡，實現(xiàn)成本較 低。
[0005] 本發(fā)明的目的是通過以下技術(shù)方案來實現(xiàn)的：一種基于關(guān)鍵幀策略的移動機器人 視覺伺服控制方法，用于移動機器人在局部已知環(huán)境下的導航，所述的移動機器人為履帶 式機器人，自帶單目攝像頭可實時采集圖像信息。該方法將導航任務轉(zhuǎn)化到基于圖像單應 性定義的圖像特征空間中，設計預測控制器在圖像特征空間中進行軌跡跟蹤控制，包括以 下步驟：
[0006] (1)將機器人放置在初始位置上，給定期望位置坐標，利用已知的機器人起始、終 止位置結(jié)合機器人自身的非完整約束規(guī)劃出機器人在二維平面下的移動路徑；
[0007] (2)根據(jù)步驟（1)得到的機器人移動路徑，通過利用單應性關(guān)系定義圖像特征，并 從單應性矩陣中選取合適的元素作為系統(tǒng)狀態(tài)量，進而將該路徑轉(zhuǎn)化到圖像特征空間里， 生成一組圖像空間特征即系統(tǒng)狀態(tài)；
[0008] (3)機器人開始移動后，由攝像頭獲取圖像信息，首先把第一幀作為關(guān)鍵幀，之后 基于當前圖像幀與關(guān)鍵幀之間的單應性關(guān)系定義圖像特征，以圖像特征隱式表達機器人 的運動情況。其中關(guān)鍵幀在機器人移動過程中需要進行切換，而切換需滿足一定的條件： a.目標物體對于當前幀不可見，但是對于下一幀可見；b.切換的關(guān)鍵幀與上一可見幀之間 應存在一定的時間間隔；c.當系統(tǒng)狀態(tài)有接近奇異的趨勢；d.關(guān)鍵幀切換的時間間隔不應 太長。且在每一次關(guān)鍵幀切換之后，需要重新對路徑進行規(guī)劃，然后通過步驟（2)轉(zhuǎn)化為圖 像空間軌跡；
[0009] (4)將步驟⑵得到的圖像空間軌跡的系統(tǒng)狀態(tài)作為設定值，步驟（3)中實時得到 的當前系統(tǒng)狀態(tài)作為測量值，共同作為控制器的輸入信號，再由控制器輸出信號驅(qū)動機器 人運動，控制器采用模型預測控制，其中相互關(guān)系模型通過如下方式建立：基于機器人運動 學模型和相機模型，經(jīng)過系統(tǒng)狀態(tài)對時間的微分最終得出系統(tǒng)狀態(tài)關(guān)于速度的圖像雅可比 矩陣即作為相互關(guān)系模型；
[0010] (5)機器人接收驅(qū)動信號進行移動，同時利用攝像頭獲取實時圖像，進而得到系統(tǒng) 狀態(tài)，結(jié)合規(guī)劃的圖像空間特征，判斷是否達到目標位置，若沒有則繼續(xù)上述步驟（4)，需要 切換關(guān)鍵幀則進行步驟（3);若達到目標，則方法結(jié)束。
[0011] 步驟（1)中所述在二維平面下機器人移動軌跡滿足的約束根據(jù)公式（1)和（2)進 行，公式⑴和⑵分別為：
[0012] P (y, z) = 0
[0013] sincpdz - coscpdy = 0
[0014] 式中（y，Z)表示機器人在二維坐標系（坐標軸為y軸和Z軸）下的坐標，P表示 移動軌跡，φ表示機器人的偏轉(zhuǎn)角度。
[0015] 步驟（1)中在控制過程中的機器人幾何中心的起始、終止坐標分別表示為公式 (5)，公式（5)為：
[0016]
【主權(quán)項】
1. 一種基于關(guān)鍵幀策略的移動機器人視覺伺服控制方法，用于移動機器人在局部已知 環(huán)境下的導航，所述的移動機器人為履帶式機器人，自帶單目攝像頭可實時采集圖像信息。 其特征在于，該方法將導航任務轉(zhuǎn)化到基于圖像單應性定義的圖像特征空間中，設計預測 控制器在圖像特征空間中進行軌跡跟蹤控制，包括以下步驟： (1)將機器人放置在初始位置上，給定期望位置坐標，利用已知的機器人起始、終止位 置結(jié)合機器人自身的非完整約束規(guī)劃出機器人在二維平面下的移動路徑； ⑵根據(jù)步驟⑴得到的機器人移動路徑，通過利用單應性關(guān)系定義圖像特征，并從單 應性矩陣中選取合適的元素作為系統(tǒng)狀態(tài)量，進而將該路徑轉(zhuǎn)化到圖像特征空間里，生成 一組圖像空間特征即系統(tǒng)狀態(tài)； (3) 機器人開始移動后，由攝像頭獲取圖像信息，首先把第一幀作為關(guān)鍵幀，之后基于 當前圖像幀與關(guān)鍵幀之間的單應性關(guān)系定義圖像特征，以圖像特征隱式表達機器人的運動 情況。 其中關(guān)鍵幀在機器人移動過程中需要進行切換，而切換需滿足一定的條件：a.目標物 體對于當前幀不可見，但是對于下一幀可見；b.切換的關(guān)鍵幀與上一可見幀之間應存在一 定的時間間隔；c.當系統(tǒng)狀態(tài)有接近奇異的趨勢；d.關(guān)鍵幀切換的時間間隔不應太長。且 在每一次關(guān)鍵幀切換之后，需要重新對路徑進行規(guī)劃，然后通過步驟（2)轉(zhuǎn)化為圖像空間 軌跡； (4) 將步驟（2)得到的圖像空間軌跡的系統(tǒng)狀態(tài)作為設定值，步驟（3)中實時得到的當 前系統(tǒng)狀態(tài)作為測量值，共同作為控制器的輸入信號，再由控制器輸出信號驅(qū)動機器人運 動， 控制器采用模型預測控制，其中相互關(guān)系模型通過如下方式建立：基于機器人運動學 模型和相機模型，經(jīng)過系統(tǒng)狀態(tài)對時間的微分最終得出系統(tǒng)狀態(tài)關(guān)于速度的圖像雅可比矩 陣即作為相互關(guān)系模型； (5) 機器人接收驅(qū)動信號進行移動，同時利用攝像頭獲取實時圖像，進而得到系統(tǒng)狀 態(tài)，結(jié)合規(guī)劃的圖像空間特征，判斷是否達到目標位置，若沒有則繼續(xù)上述步驟（4)，需要切 換關(guān)鍵幀則進行步驟（3);若達到目標，則方法結(jié)束。
2. 根據(jù)權(quán)利要求1所述的方法，其特征在于：步驟（1)中所述在二維平面下機器人移 動軌跡滿足的約束根據(jù)公式（1)和（2)進行，公式（1)和（2)分別為： P(y,z) = 0
式中（y，Z)表示機器人在二維坐標系（坐標軸為y軸和z軸）下的坐標，P表示移動 軌跡，9表示機器人的偏轉(zhuǎn)角度。
3. 根據(jù)權(quán)利要求1所述的方法，其特征在于：步驟（1)中在控制過程中的機器人幾何 中心的起始、終止坐標分別表示為公式（5)，公式（5)為：
其中（yD,zu,tpu)表示機器人幾何中心的初始坐標，（yf,zf,(pf)表示機器人幾何中心的終 止位置坐標，（y，z，cp)表示相機中心的終止位置坐標，為待辨識參數(shù)，具體定義見權(quán)利要求 8〇
4. 根據(jù)權(quán)利要求1所述的方法，其特征在于：步驟（2)中所述的單應性圖像特征由公 式（6)定義，公式（6)為：
其中，H即為單應性矩陣，（R，t)表示了機器人的運動狀態(tài)，n為相機坐標系下地面的法 向量，d為相機坐標系原點與地面之間的距離，K為相機內(nèi)部參數(shù)矩陣。
5. 根據(jù)權(quán)利要求1所述的方法，其特征在于：步驟⑵中單應性圖像特征的獲取以及 系統(tǒng)狀態(tài)變量的定義分別由公式（10)和（11)決定，公式（10)和（11)分別為：
其中fu、fv分別表示焦距在像素坐標軸上對應的像素點數(shù)目，0是相機光軸與地面的 夾角，（P是機器人期望位姿下的偏轉(zhuǎn)角度。
6. 根據(jù)權(quán)利要求1所述的方法，其特征在于：步驟⑷中所述的機器人運動學模型根 據(jù)公式（12)確定，公式（12)為：
其中Vv、Vz、W分別表示相機的側(cè)向、前向線速度和角速度，Vl、Vl^分別為左右兩條履帶 的速度，yicVyicVZra分別表示三個旋轉(zhuǎn)瞬心的坐標。
7. 根據(jù)權(quán)利要求1所述的方法，其特征在于：步驟（4)中新定義了一種位姿參數(shù)，根據(jù) 公式（13)進行，公式（13)為：
其中s為系統(tǒng)狀態(tài)變量，（>V, 9)為相機中心的期望位置坐標，其余參數(shù)定義參照上 文。
8. 根據(jù)權(quán)利要求1所述的方法，其特征在于：步驟（4)中所述的相互關(guān)系模型由公式 (14)和（15)確定，公式（14)和（15)分別為： V _
7 - 」 其H
s為被控變量
為待辨識參 數(shù)。
9. 根據(jù)權(quán)利要求1所述的方法，其特征在于：步驟⑷中所述的根據(jù)線性化相互關(guān)系 模型得出的代價函數(shù)由公式（17)定義，公式（17)為：
其中Q，R分別是加權(quán)矩陣，se=s-s1?為當前系統(tǒng)狀態(tài)變量與過去生成的參考軌跡狀態(tài) 變量的偏差，N是預測時域。
【專利摘要】本發(fā)明公開了一種基于關(guān)鍵幀策略的移動機器人視覺伺服控制方法，包含以下步驟：1）根據(jù)給定條件規(guī)劃機器人移動路徑；2）將視覺導航任務中的物理軌跡跟蹤和目標可見下的姿態(tài)調(diào)整轉(zhuǎn)化到圖像特征空間中的軌跡；3）基于當前圖像幀與關(guān)鍵幀之間的單應性關(guān)系定義圖像特征，以圖像特征隱式表達了機器人的運動情況；4）基于機器人運動學模型和相機模型建立視覺控制系統(tǒng)的相互關(guān)系模型，設計預測控制器跟蹤軌跡。本發(fā)明不要求預先對機器人在給定軌跡上進行訓練，也不要求在運動過程中目標持續(xù)可見，同時在一定程度上保證了機器人的導航精度。
【IPC分類】G05B19-414
【公開號】CN104808590
【申請?zhí)枴緾N201510081317
【發(fā)明人】劉山, 曹雨, 賈丙西
【申請人】浙江大學
【公開日】2015年7月29日
【申請日】2015年2月14日