本發明屬于仿生機器人控制領域，具體提供一種用于雙目頭眼協調運動的控制系統及其定位方法。

背景技術：

眼睛是人類獲取外界信息最重要的感官，擔負著人類80％左右的信息獲取任務。經過千百萬年的進化，人類的視覺系統已高度發達和完善，形成了極其精確和完備的機制，具備了適應內外環境變化的能力。因此，給機器人和機器視覺設備配備智能雙眼，使其像人類一樣感知和獲取環境信息、快速準確地識別和跟蹤目標，是很多國內外科研工作者目前正在致力于的研究方向之一。

然而，由于人類視覺系統和神經回路的復雜性，尚沒有較好的模型可以重現視覺系統的運動機理，只能在一定程度上模擬眼或者頭部的運動；此外，從神經通路推導出來的模型大多數是生理學實驗或者仿真獲得的，工程應用還有一定的困難。

相應地，本領域需要一種新的頭眼協調運動的控制系統及定位方法來解決上述問題。

技術實現要素：

為了解決現有技術中的上述問題，即為了解決現有頭眼協調運動控制系統在工程實際應用中比較困難的問題，本發明提供了一種用于雙目頭眼協調運動的控制系統及其定位方法，該控制系統包括轉動構件，所述轉動構件上設置有：第一相機以及驅動第一相機相對于所述轉動構件做俯仰和偏轉動作的第一驅動裝置；第二相機以及驅動第二相機相對于所述轉動板做俯仰和偏轉動作的第二驅動裝置。

在上述控制系統的優選技術方案中，所述第一驅動裝置包括用于驅動所述第一相機實現俯仰動作的第一電機和用于驅動所述第一相機實現偏轉動作的第二電機，并且所述第一電機上設置有第一旋轉編碼器，所述第二電機上設置有第二旋轉編碼器。

在上述控制系統的優選技術方案中，所述第二驅動裝置包括用于驅動所述第二相機實現俯仰動作的第三電機和用于驅動所述第二相機實現偏轉動作的第四電機，并且所述第三電機上設置有第三旋轉編碼器，所述第四電機上設置有第四旋轉編碼器。

在上述控制系統的優選技術方案中，所述第一、第二、第三和第四電機均是步進電機；并且/或者所述第一、第二、第三和第四電機上均設置有限位開關；并且/或者所述第一、第二、第三和第四旋轉編碼器均是光電式旋轉編碼器。

在上述控制系統的優選技術方案中，所述控制系統還包括旋轉架，所述轉動構件可旋轉地安裝到所述旋轉架上，并且所述旋轉架的側面設置有驅動所述轉動構件轉動的第三驅動裝置。

在上述控制系統的優選技術方案中，所述第三驅動裝置包括第五電機，所述第五電機用于驅動所述轉動構件轉動；并且所述第五電機上設置有第五旋轉編碼器。

在上述控制系統的優選技術方案中，所述第三驅動裝置還包括蝸輪蝸桿副，所述第五電機通過蝸輪蝸桿副驅動所述轉動構件轉動。

在上述控制系統的優選技術方案中，所述旋轉架的底部設置有驅動所述旋轉架旋轉的第四驅動裝置；并且所述第四驅動裝置包括第六電機；并且所述第六電機上設置有第六旋轉編碼器。

在上述控制系統的優選技術方案中，所述第五和第六電機均是步進電機；并且/或者所述第五和第六電機上均設置有限位開關；并且/或者所述第五和第六旋轉編碼器均是光電式旋轉編碼器。

在上述控制系統的優選技術方案中，所述轉動構件上在所述第一相機和所述第二相機之間設置有視覺定位板，所述第一相機和所述第二相機通過定位板進行初始定位。

在另一方面，本發明提供了一種用于雙目頭眼協調運動的定位方法，所述定位方法包括以下步驟：將兩個相機分別進行初始定位；通過離線標定的相機內參數和手眼參數實時獲得兩個相機之間的外參數矩陣。

在上述定位方法的優選技術方案中，所述將兩個相機分別進行初始定位的步驟進一步包括：將控制兩個相機的電機通電，使電機順時針或逆時針旋轉，當電機觸動限位開關時，使電機停止運動，從而使兩個相機被初步粗定位；采用基于橢圓中心擬合的視覺推算定位方法對兩個相機進行精確初定位。

在上述定位方法的優選技術方案中，所述采用基于橢圓中心擬合的視覺推算定位方法對兩個相機進行精確初定位的步驟進一步包括：在初始位置在視覺定位板上打印標準圓；采用一維Prewitt算子分別沿橫向和縱向對圖像進行掃描獲取邊緣點；對邊緣點用最小二乘法進行橢圓擬合，獲得橢圓中心；控制相機運動，使橢圓中心與相機像平面中心重合，從而完成視覺推算定位。

在上述定位方法的優選技術方案中，所述通過離線標定的相機內參數和手眼參數實時獲得兩臺相機之間的外參數矩陣的步驟進一步包括：采用平面標定法分別獲得兩個相機的內參數和畸變參數；通過立體標定獲得初始位置時第一相機相對于第二相機的外參數矩陣；通過手眼標定獲得兩個相機相對于各自運動模塊末端的外參數。

在上述定位方法的優選技術方案中，所述通過離線標定的相機內參數和手眼參數實時獲得兩臺相機之間的外參數矩陣的步驟進一步包括：根據初始位置時第一相機相對于第二相機的外參數矩陣和兩個相機相對于各自運動模塊末端的外參數，實時計算兩個相機之間的外參數矩陣。

本領域技術人員能夠理解的是，在本發明的優選技術方案中，通過步進電機分別驅動第一相機和第二相機相對于轉動板做俯仰和偏轉動作，模擬人的雙眼運動；通過步進電機驅動相互嚙合的蝸輪蝸桿帶動轉動板轉動，模擬人頭部的俯仰(抬頭、低頭)動作；通過步進電機驅動旋轉架旋轉，模擬人頭部的轉動(轉頭)動作。進一步，在每個步進電機上都設置有光電式旋轉編碼器和限位開關；當控制系統通電后，第一相機和第二相機由步進電機驅動旋轉，觸動限位開關時完成初步粗定位，然后根據橢圓中心擬合的視覺推算定位方法對兩個相機進行精確初定位，最后通過離線標定的相機內參數和手眼參數實時獲得兩臺相機之間的外參數矩陣。因此，本發明的控制系統只需在初始位置立體標定一次，就可以在相機運動過程中實時獲得兩臺相機之間的外參數，能夠被容易地應用于工程實踐中。

附圖說明

圖1是本發明的用于雙目頭眼協調運動的控制系統的機械結構示意圖；

圖2是本發明的用于雙目頭眼協調運動的定位方法的步驟流程圖；

圖3是本發明的用于雙目頭眼協調運動的定位方法的初始定位的詳細流程圖；

圖4是本發明的用于雙目頭眼協調運動的定位方法的兩相機之間實時定位的詳細流程圖；

圖5是采用本發明的雙目頭眼協調運動的定位方法和采用用立體視覺標定方法獲得的兩個相機之間的實時外參數實驗結果的對比表格。

具體實施方式

下面參照附圖來描述本發明的優選實施方式。本領域技術人員應當理解的是，這些實施方式僅僅用于解釋本發明的技術原理，并非旨在限制本發明的保護范圍。例如，雖然附圖中的各部件之間是按一定比例關系繪制的，但是這種比例關系并非一成不變，本領域技術人員可以根據需要對其作出調整，以便適應具體的應用場合。

需要說明的是，在本發明的描述中，術語“中心”、“上”、“下”、“左”、“右”、“豎直”、“水平”、“內”、“外”等指示的方向或位置關系的術語是基于附圖所示的方向或位置關系，這僅僅是為了便于描述，而不是指示或暗示所述裝置或元件必須具有特定的方位、以特定的方位構造和操作，因此不能理解為對本發明的限制。此外，術語“第一”、“第二”、“第三”僅用于描述目的，而不能理解為指示或暗示相對重要性。

此外，還需要說明的是，在本發明的描述中，除非另有明確的規定和限定，術語“安裝”、“相連”、“連接”應做廣義理解，例如，可以是固定連接，也可以是可拆卸連接，或一體地連接；可以是機械連接，也可以是電連接；可以是直接相連，也可以通過中間媒介間接相連，可以是兩個元件內部的連通。對于本領域技術人員而言，可根據具體情況理解上述術語在本發明中的具體含義。

如圖1所示，本發明的用于雙目頭眼協調運動的控制系統在機械結構上主要包括：轉動板1、旋轉架2和底座3。轉動板1的兩端通過旋轉裝置11可旋轉地固定到旋轉架2上，優選地，旋轉裝置11是軸承轉軸組，或者本領域技術人員也可以采用其他形式的轉動件將轉動板1和旋轉架2可旋轉地固定到一起，例如銷釘。旋轉架2的底端(圖1中旋轉架2的底端)可旋轉地固定到底座3上。

繼續參閱圖1，轉動板1上設置有第一相機41以及驅動第一相機41相對于轉動板1做俯仰和偏轉動作的第一驅動裝置(圖中未標示)、第二相機51以及驅動第二相機51相對于轉動板1做俯仰和偏轉動作的第二驅動裝置(圖中未標示)和視覺定位板6。在本發明的優選實施方案中，第一相機41和第二相機51用于模仿人的雙眼。

具體地，第一驅動裝置包括用于驅動第一相機41實現俯仰動作的第一電機42和用于驅動第一相機41實現偏轉動作的第二電機44；第一電機42的伸出軸(圖中未示出)垂直于第一相機41的鏡頭軸線方向并與第一相機41固定連接，使得第一電機42能夠驅動第一相機41繞其伸出軸旋轉，進而實現第一相機41的俯仰動作；第二電機44例如通過法蘭與轉動板1固定連接，其伸出軸(圖中未示出)垂直于第一電機42的伸出軸并與第一電機42固定連接，使得第二電機44能夠驅動第一電機42和第一相機41繞其伸出軸旋轉，進而實現第一相機41的偏轉動作。

類似地，第二驅動裝置包括用于驅動第二相機51實現俯仰動作的第三電機52和用于驅動第二相機51實現偏轉動作的第四電機54。第三電機52的伸出軸(圖中未示出)垂直于第二相機51的鏡頭軸線方向并與第二相機51固定連接，使得第三電機52能夠驅動第二相機51繞其伸出軸旋轉，進而實現第二相機51的俯仰動作；第四電機54例如通過法蘭與轉動板1固定連接，其伸出軸(圖中未示出)垂直于第三電機52的伸出軸并與第三電機52固定連接，使得第四電機54能夠驅動第三電機52和第二相機51繞其伸出軸旋轉，進而實現第二相機51的偏轉動作。

進一步，電機42、44、52和54的轉軸上與輸出軸相對的另一端上分別設置有第一光電式旋轉編碼器43、第二光電式旋轉編碼器45、第三光電式旋轉編碼器53和第四光電式旋轉編碼器55，以便通過光電式旋轉編碼器43、45、53和55來實時監測電機42、44、52和54旋轉的角度，進而精確控制第一相機41和第二相機51的俯仰、偏轉角度。

如圖1所示，視覺定位板6固定在轉動板1上，并且位于第一相機41和第二相機51之間，優選地，視覺定位板6位于第二電機44和第四電機54伸出軸在轉動板1上連線的中間位置。進一步，視覺定位板6上設置有圓孔61，優選地，當第一相機41的鏡頭和第二相機51的鏡頭同時面對視覺定位板6時，其軸線能夠同時與該圓孔61的軸線平行。

繼續參閱圖1，在旋轉架2的一側(圖1中旋轉架2的左側)設置有用于驅動轉動板1轉動的第三驅動裝置(圖中未標示)，第三驅動裝置用于驅動轉動板1來模擬人頭部的俯仰(抬頭、低頭)動作。具體地，第三驅動裝置包括第五電機71和與第五電機71的一端固定連接的第五光電式旋轉編碼器72。進一步，第三驅動裝置和轉動板1之間還設置有蝸輪蝸桿副8。由于蝸輪與蝸桿之間具有自鎖功能，使得當轉動板1旋轉至一定角度時，能夠通過蝸輪蝸桿副8進行鎖定。作為示例，蝸桿與第五電機71的伸出軸(圖中未示出)固定連接，蝸輪與旋轉裝置11固定連接，使得當第五電機71驅動蝸桿旋轉時，蝸輪會通過旋轉裝置11驅動轉動板1旋轉。本領域技術人員也可以根據需要將蝸輪與第五電機71的伸出軸固定連接，將蝸桿與旋轉裝置11固定連接。

進一步參閱圖1，旋轉架2的底端設置有驅動其旋轉的第四驅動裝置(圖中未標示)，該第四驅動裝置包括第六電機91和與第六電機91的一端固定連接的第六光電式旋轉編碼器92。第六電機91通過螺栓與底座3固定連接，第六電機91的伸出軸(圖中未示出)與旋轉架2固定連接。第六電機91驅動旋轉架2旋轉，用以模擬人頭部的旋轉(左右扭頭)動作。

優選地，電機42、44、52、54、71和91均是步進電機，或者本領域技術人員也可以根據需要選用伺服電機等其他電機。

另外，電機42、44、52、54、71和91上還分別設置有限位開關(圖中未示出)，當電機旋轉并觸動限位開關時，控制系統會控制電機停止旋轉。在本發明的優選實施方案中，限位開關用于當第一相機41和第二相機51旋轉至視覺定位板6上的圓孔處時，使所有電機停止轉動，進而對第一相機41和第二相機51實現初步粗定位。

本領域技術人員能夠理解的是，在保證第一相機41與第二相機51俯仰、偏轉精度的前提下和轉動板1轉動精度以及旋轉架2旋轉精度的前提下，上述旋轉編碼器43、45、53、55、72和92還可以采用其他任何適當的形式，例如絕對值編碼器等。

綜上所述，當本發明的用于雙目頭眼協調運動的控制系統動作時，第一相機41和第二相機51，先通過視覺定位板6進行初步粗定位，然后控制系統控制電機42、44、52、54驅動第一相機41和第二相機51俯仰或偏轉，模擬人的雙眼動作，控制電機71驅動轉動板1轉動，模擬人頭部的俯仰動作，控制電機91驅動旋轉架2旋轉，模擬人頭部的扭動旋轉動作。需要說明的是，上述動作既可以是單獨進行也可以同時進行。

如圖2所示，本發明的用于雙目頭眼協調運動的控制系統中的雙目頭眼協調運動的定位方法主要包括以下步驟：步驟S100，將兩個相機分別進行初始定位；步驟S200，通過離線標定的相機內參數和手眼參數實時獲得兩臺相機之間的外參數矩陣。

如圖3所示，步驟S100又具體包括：步驟S110，將控制兩個相機的電機通電，使電機順時針或逆時針旋轉，當電機觸動限位開關時，電機停止運動，兩個相機被初步粗定位；步驟S120，采用基于橢圓中心擬合的視覺推算定位方法對兩個相機進行精確初定位。

在步驟S110中，由于電機42、44、52和54在斷電后不具有保持功能，使得第一相機41和第二相機51在自身重力和其他外力的作用下，會迫使電機42、44、52和54產生相應的角位移，所以當控制系統通電之后需要對電機42、44、52和54的角度進行重新調整和對第一相機41和第二相機51進行重新定位。具體地，先將控制系統通電，控制系統會控制電機42、44、52和54沿某一方向(順時針或逆時針)轉動，當電機42、44、52和54分別觸動限位開關時停止轉動。此時，第一相機41和第二相機51的鏡頭分別面向視覺定位板6，并與其平行，第一相機41和第二相機51完成初步粗定位。

本領域技術人員能夠理解的是，電機42、44、52和54的初始轉動方向可事先在控制程序中設定好，也可以根據控制系統檢測到的光電式旋轉編碼器43、45、53和55的角度信息，粗略確定電機42、44、52和54的角度位置，從而通過控制系統驅動電機42、44、52和54向與各自的限位開關角度距離較小的方向旋轉。

在步驟S120中，由于安裝誤差、連接結構形變和畸變，當第一相機41和第二相機51分別面向視覺定位板6時，視覺定位板6的平面與相機成像平面很難平行，導致兩個相機41、51各自鏡頭采集到的圓孔61并非是一個圓形，而是一個橢圓形。基于此，采用橢圓中心擬合的視覺推算定位方法對第一相機41和第二相機51分別進行精確初定位。電機42、44、52和54轉過固定角度后(如步驟S110所述)，第一相機41和第二相機51能夠分別捕捉到視覺定位板6上的圓孔61，進而，第一相機41和第二相機51分別采集圓孔61的圖像并對其進行處理，提取出圖形的中心。更具體地，采用一維Prewitt算子分別沿橫向和縱向對圖像進行掃描獲取邊緣，經過閾值處理獲取進行橢圓擬合的邊緣點。當采集到的圖形中心不在相機41、51的圖像中心時，控制系統控制電機42、44、52和54轉動，使采集到的圖形中心與相機41、51的圖像中心重合并保持穩定。

需要說明的是，Prewitt算子是一種一階微分算子的邊緣檢測方法，其利用像素點上下、左右鄰點的灰度差，在邊緣處達到極值檢測邊緣，去掉部分偽邊緣，對噪聲具有平滑作用。其原理是在圖像空間利用兩個方向模板與圖像進行鄰域卷積來完成的，這兩個方向模板一個檢測水平邊緣，一個檢測垂直邊緣。

還需要說明的是，最小二乘法是最早的橢圓擬合方法，它是數據擬合中的基本方法，它的基本思想就是考慮數據受隨機噪聲的影響進而追求整體誤差的最小化。對橢圓擬合而言，就是先假設橢圓參數，得到每個待擬合點到該橢圓的距離之和，也就是點到假設橢圓的誤差，求出使這個和最小的參數。

如圖4所示，步驟S200又具體包括：步驟S210，采用平面標定法分別獲得第一相機41和第二相機51的內參數和畸變參數；步驟S220，通過立體標定獲得初始位置時第一相機41相對于第二相機51的外參數矩陣；步驟S230，通過手眼標定獲得兩個相機相對于各自運動模塊末端的外參數；步驟S240；根據初始位置時第一相機41相對于第二相機51的外參數矩陣和兩個相機相對于各自運動模塊末端的外參數，實時計算兩個相機之間的外參數矩陣。

具體地，在步驟S210中，采用平面標定法獲得第一相機41和第二相機51的內參數M_inL和M_inR以及畸變參數K_dL和K_dR，以便第一相機41和第二相機51能夠通過調節其焦距來使其獲取到的圖像盡可能地相同。需要說明的是，相機內參數的意義在于：對于從相機坐標系到像平面坐標系的變換，像平面坐標系是用像素單位來表示的，而相機坐標系則是以毫米為單位來表示，因此，要完成改變換過程就需要先得到像平面的像素單位與毫米單位之間的線性關系；相機畸變是由相機鏡頭設計造成的。

具體地，在步驟S220中，通過立體標定獲得初始位置時第一相機41相對于第二相機51的變換旋轉矩陣R和平移向量p。為了確保R和p的準確性，在本發明的優選實施方案中通過重復實驗獲得R和p的一組數據，并將與這組數據中的其他數據都比較靠近的R和p作為真實結果的初始近似值。接著運行穩健的L-M迭代算法查找出棋盤角點在兩個相機41、51視圖上的最小投影誤差，進而得到R和p的結果。

需要說明的是，L-M迭代算法是一種非線性規劃方法，其主要用于無約束的多維非線性規劃問題，其是一階牛頓法的一種改進，改進的目的是為了更快的收斂。

在步驟S230中，通過手眼標定獲得兩個相機41和51相對于其各自運動模塊(驅動兩個相機41和51轉動的電機42、44、52和54)末端的外參數T_mL和T_mR。手眼標定是本領域技術人員熟知的操作，可以采用本領域已知的任意合適的方法來實現，此處不再贅述。

在步驟S240中，根據初始位置時第一相機41相對于第二相機51的外參數矩陣和兩個相機41、51相對于各自運動模塊末端的外參數，實時計算兩個相機之間的外參數矩陣，其計算公式如下：

其中，Ti11是第一相機41相對于第二相機51的旋轉矩陣：

Ti22是兩個相機41、51相對于各自運動模塊末端的平移向量：

R_ml是第一相機41相對于第一運動模塊的旋轉矩陣，R_mr是第二相機51相對于第二運動模塊的旋轉矩陣。R_il是第一運動模塊末端轉過一定角度后，其相對于第一運動模塊基坐標系的旋轉矩陣；R_ir是第二運動模塊末端轉過一定角度后，其相對于第二運動模塊基坐標系的旋轉矩陣。R是初始位置時，第一相機41坐標系相對于第二相機51坐標系的旋轉矩陣。

如圖5所示，該圖示出了實時計算兩個相機41和51之間外參數的實驗結果。具體地，兩個運動模塊分別控制兩個相機41和51運動到160個不同的位置，分別用立體視覺標定的方法和利用本發明提供的方法計算得到兩個相機坐標系之間的變換關系，共160組數據。為了顯示方便，每隔一定距離截取一組數據，共計20組數據。計算兩個方法所獲得數據之間的角度偏差和平移偏差，其中，角度偏差是通過對兩個相機41、51的旋轉變換矩陣求通用旋轉變換的轉角得到的，平移偏差是立體標定的平移向量和計算得到的平移向量的歐氏距離。從圖4中不難看出，本發明的優選實施方案和立體標定得到的值比較接近，然而，本發明的方法避免了相機41、51在工作過程中邊工作邊標定，只需在相機41、51初始位置立體標定一次，就可以在相機運動過程中實時得到兩個相機41、51之間的外參數。

至此，已經結合附圖所示的優選實施方式描述了本發明的技術方案，但是，本領域技術人員容易理解的是，本發明的保護范圍顯然不局限于這些具體實施方式。在不偏離本發明的原理的前提下，本領域技術人員可以對相關技術特征作出等同的更改或替換，這些更改或替換之后的技術方案都將落入本發明的保護范圍之內。