基于單應性矩陣的最短路徑視覺伺服控制方法
【技術領域】
[0001] 本發(fā)明設及一種視覺伺服控制方法，尤其設及一種基于單應性矩陣的最短路徑視 覺伺服控制方法。
【背景技術】
[0002] 自動尋物機器人已經(jīng)逐步被投用于工業(yè)生產(chǎn)中，現(xiàn)有的自動尋物機器人通常采用 已知地圖的高精確定位尋物，靈活性差并且實現(xiàn)成本高。一些機器人采用基于視覺信息的 伺服控制方法控制機器人運動W提高靈活性和穩(wěn)定性?；趫D像的視覺伺服控制系統(tǒng)反饋 信息定義在圖像平面，其控制目標是使當前獲取的圖像狀態(tài)S和給定的目標圖片狀態(tài)S*的 誤差最小。利用機器視覺的相關知識，可W通過對當前圖像和目標圖像匹配得到對應的特 征點，用圖像特征點的像素坐標結(jié)合相機參數(shù)構(gòu)造圖片狀態(tài)S，設計控制器使S和S*間的 誤差減小W實現(xiàn)控制目標。但是利用單目相機獲取圖像信息進行控制的方案中一個十分重 要的問題就是無法獲取深度信息。近年來有許多學者提出了不同的辦法W試圖解決該一問 題，如利用線性化系統(tǒng)模型并結(jié)合擴展卡爾曼濾波巧K巧算法W補償深度信息，使用單目 全景攝像機系統(tǒng)的反饋完成沿墻壁運動、跟蹤標記或位置校準等等。但是，使用EKF算法估 計深度信息存在需要對模型進行線性化處理的缺點。除EKF算法外，有學者嘗試利用從連 續(xù)圖像的光流信息中得到的表觀速度去估計深度信息，但是光流技術需要大量的圖像處理 工作，會導致機器人控制方面的實時性變差。也有學者在工作空間中增加一個不需標定的 固定相機W開發(fā)自適應跟蹤控制器，用W彌補相機參數(shù)和機器人動力學建模的誤差，但該 種方法需要相機到機器人運動平面的距離維持不變。
[0003] 機器人的非完整約束是指機器人在移動過程中同時受到空間位置和運動速度的 約束，并且運動速度不能積分W轉(zhuǎn)換為空間位置。由于存在非完整約束，機器人的控制變量 的維數(shù)小于機器人狀態(tài)空間的維數(shù)，使對機器人的運動控制變得困難。非完整約束在機器 人系統(tǒng)中占據(jù)著十分重要的位置，許多移動機器人、角動量守恒空間機器人W及冗余度機 器人等系統(tǒng)結(jié)構(gòu)中都包含著非完整約束的應用。對非完整約束早期的研究主要是運用微分 方程理論和變分原理等對系統(tǒng)進行建模分析等等，隨后有學者將微分幾何、群論等數(shù)學工 具應用于非線性系統(tǒng)分析，使得對非完整系統(tǒng)的研究有了較快發(fā)展，但是分析的過程仍然 較為復雜。
[0004] 現(xiàn)有的自動尋物機器人通常采用已知地圖的高精確定位尋物，靈活性差并且實現(xiàn) 成本高，有些通過視覺伺服控制的機器人需要雙目相機來計算深度信息，使得運算復雜并 且實現(xiàn)成本較高。

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

[0005] 本發(fā)明的目的在于針對現(xiàn)有技術的不足，提供一種簡單穩(wěn)定、易于實現(xiàn)的基于單 應性矩陣的最短路徑視覺伺服控制方法。
[0006] 本發(fā)明的基于單應性矩陣的最短路徑視覺伺服控制方法，包括如下步驟：
[0007] 1) W機器人前進方向為Z軸，垂直地面向下為y軸，建立機器人的右手坐標系；
[000引 2)將機器人捕捉的實時畫面與給定的目標圖像進行匹配得到相應的實時的單應 性矩陣：
[0009]
[0010] 分=個階段控制機器人的運動，根據(jù)實時計算出的單應性矩陣得到每一階段實時 的控制律如下：
[0011]階段I
[001引當W =0時，階段I結(jié)束，機器人的運動進入階段II ;
[0013]階段II
[0014] 當V =0時，階段II結(jié)束，機器人的運動進入階段III ;
[00巧]階段III;v =0,W = -kjii3
[0016] 當w=0時，機器人到達目標位姿，運動結(jié)束；
[0017] 其中，V為機器人實時線速度，W為機器人實時角速度，1^、、1^，1、1^，2人3為比例因子， 且 kyG(0，l)，kwG (0,0.3)，kw2E(〇,〇.3)，kw3E(0,2)，a X為機器人的相機的焦距在 圖像平面像素水平排列方向上占據(jù)的單位像素個數(shù)。
[0018] 上述技術方案所依據(jù)的控制思路如下：
[0019] 將機器人的運動分解成=個階段，首先使機器人旋轉(zhuǎn)到其面向目標圖像所在位置 點，簡稱目標點，然后機器人平移至該目標點，最后機器人旋轉(zhuǎn)到目標位姿的角度，比較每 個階段單應性矩陣的各元素的差異得出速度的控制方程。
[0020] 對于一個平面上的機器人，W其前進方向為Z軸，垂直地面向下為y軸，建立機器 人的右手坐標系，如圖1所示；可W將它相對于目標位姿的旋轉(zhuǎn)矩陣和平移矩陣化簡如下：
[0021]
[0022] 其中，X、Z分別為機器人當前位置在目標位姿坐標系中的表示，4為機器人實時 位姿與目標位姿間的夾角，機器人的當前位姿可W由狀態(tài)向量（X，z，4)表示。
[0023] 相應的機器人的單應性矩陣可W寫成：
[0024]
[0025] 其中各元素分別由下式計算得出：
[0026]

[0032] 其中，a,為機器人的相機的焦距在圖像平面像素水平排列方向上占據(jù)的單位像 素個數(shù)，
[0033] ay為機器人的相機的焦距在圖像平面像素垂直排列方向上占據(jù)的單位像素個 數(shù)，
[0034] 枉，rvriz]T為給定的目標圖像的法向量在目標位姿坐標系中的表示，
[0035] d為目標位姿坐標系的原點到給定的目標圖像的距離，
[0036] 將機器人的運動分解為=個階段：第一階段使機器人旋轉(zhuǎn)到其相機指向目標位 置，即只做旋轉(zhuǎn)運動；第二階段使機器人平移到目標位置，即只做平移運動；第=階段使機 器人旋轉(zhuǎn)到正確的角度，即只做旋轉(zhuǎn)運動；=個階段的運動過程如圖2所示。
[0037] 下面將分析每一階段中單應性矩陣的變化情況。
[003引機器人剛開始運動時，設機器人當前位置與目標位置的連線與目標位姿坐標系的 Z軸所形成的夾角為（K，當?shù)谝浑A段結(jié)束時，4 = (K，將該式子代入單應性矩陣中可W得 出第一階段完成時的單應性矩陣的表達式：
[0039]
[0040] 第二階段中，機器人始終指向目標位置，此時一直有4 = (K，第二階段完成時，X =〇，z = 0, 4 = (l)t，將上式代入單應性矩陣中可W得出第二階段完成時的單應性矩陣的 表達式：
[0041]
[0042] 第=階段中，機器人在原地旋轉(zhuǎn)到正確的角度，最終達到目標位姿，此時有X= 0，z = 0, d) = 0,將上式代入單應性矩陣中可W得出第=階段完成時的單應性矩陣的表達 式：
[0043] H(x = 0,Z= 0,本=本t)= I
[0044] 假定相機的標定矩陣是已知的，則可W得到焦距長度a ,的值，分析比較該S階段 中單應性矩陣的變化情況，可W得出每一階段的控制律：
[0045] 第一階段的運動目標是達到4 =(K的狀態(tài)，也就是使機器人指向目標點，該個 過程中V始終為0,當d) = (K時，從相應的單應性矩陣中可W得出：
[0046]
[0047] 且機器人由4變化到的過程中，