樊紹桐 婁小平
(北京信息科技大學(xué)光電測試技術(shù)北京市重點實驗室,北京,100192)
在仿人足球機器人比賽中,運動學(xué)標定的精確度是機器人在比賽場上表現(xiàn)好壞的關(guān)鍵因素之一。機器人在使用一段時間后,各個關(guān)節(jié)的零點會與初始值產(chǎn)生較大差異,而標定的目的是:減小基于系統(tǒng)數(shù)學(xué)模型得到的預(yù)測值與實際測量值之間的誤差,這些誤差通常來自于機器人本身不精確的裝配和日常操作過程中舵機發(fā)生的磨損、變形[1]等。
目前,很多研究者對仿人機器人運動學(xué)標定進行了廣泛研究。其中,文獻[2]和文獻[3]都提出了基于手眼標定[4]理論的運動學(xué)標定方法。該方法首先通過機器人頭部的攝像頭來觀察附著于機器人末端執(zhí)行器上的標記,而且每個標記的特征點在像素坐標系中的位置和讀取到的各個關(guān)節(jié)的角度值都被收集起來,然后用于最小化測量值和基于機器人的數(shù)學(xué)模型(即運動學(xué)正解和攝像頭的內(nèi)參、外參)所得到的估計值之間的誤差。但由于該方法只能對單個末端執(zhí)行器進行標定,而且不能自動完成標定過程,因此并不適用于仿人足球機器人的應(yīng)用環(huán)境。文獻[5]提出利用粒子群算法進行逆運動學(xué)解,取得了一定成果,但是由于標準粒子群算法容易陷入局部最優(yōu)解,無法跳出停滯狀態(tài),導(dǎo)致搜索停滯。
本文首先使用機器人頭部偏下方的攝像頭觀察附著在機器人腳底的棋盤格標定板,以獲取其中的角點坐標,并將該坐標與關(guān)節(jié)傳感器讀取到的角度值進行處理,然后再將標定過程轉(zhuǎn)化為求解一個非線性最小二乘問題,并對用D-H(Denavit-Hartenberg)法建立起的NAO機器人的運動鏈進行齊次坐標變換,且將攝像頭模型簡化為針孔攝像機模型。這樣,在攝像頭內(nèi)參已知的情況下,筆者采用LM(Levenberg-Marquardt)算法對NAO機器人的各參數(shù)進行最優(yōu)化計算,最終實現(xiàn)標定參數(shù)的補償。
本文針對如圖1所示的NAO機器人進行標定設(shè)計,需要根據(jù)機器人的腳部尺寸和傳感器分布位置自主設(shè)計標定板的樣式。在標定過程中,首先需要機器人利用自帶的攝像頭,在不同角度下對標定板進行拍攝,然后建立機器人運動鏈模型,最后根據(jù)投影模型完成解算。
圖1 機器人NAO
NAO機器人腳底的整個棋盤格標定板的圖案由35個邊長為2.5cm的方格構(gòu)成(5行7列),打印后貼在一塊板子上,如圖2(b)所示。每一塊板上都安裝有4個固定的針腳,這4個針腳位于NAO機器人腳底部壓力傳感器所對應(yīng)的凹槽處,并通過一個帶鉤和環(huán)扣帶進一步固定。如圖2(a)所示,由于壓力傳感器的位置是已知的,其與踝關(guān)節(jié)在腳底的投影有一定的關(guān)系,因此棋盤格角點的位置可以很容易計算出來。棋盤格角點是指黑白色塊相交部分的那些十字點,刨除邊緣那些不符合定義的點之后,一共是24個(4行6列)。而且,從圖2(b)可以看出棋盤的中心位置與上文提到的關(guān)節(jié)投影之間的距離是17cm,基于這些位置即可推算出每個角點在圖像坐標系上的位置。
本文從世界坐標系到圖像坐標系的轉(zhuǎn)換采用的是針孔攝像機模型P[6]:
其中,光心Oc和焦距fl是已知的攝像頭內(nèi)參,可以通過張正友標定法進行標定。
在各種運動學(xué)模型中,Denavit和Hartenberg提出的D-H模型由于具有明確清晰的物理意義、易于編程而被廣泛采用。該模型使用一個4×4的齊次變換矩陣來描述相鄰關(guān)節(jié)坐標系的空間關(guān)系,進而推導(dǎo)出末端執(zhí)行器相對于參考坐標系的等價齊次變換矩陣。
基于文獻[1]中,從機器人腳部到頭部攝像頭的運動學(xué)鏈圖和機器人生產(chǎn)廠家提供的文檔信息,建立如圖3所示的運動學(xué)樹狀鏈。
圖3 機器人NAO的運動學(xué)樹狀鏈
攝像頭相對于世界坐標系的三維坐標可以通過測量到的傳感器角度值q、從NAO機器人左腳踝關(guān)節(jié)到左腳腳底的運動學(xué)正解 及棋盤格標定板角點相對于NAO機器人左腳腳底的位置pLSα,這3個量進行計算得到。
每一個關(guān)節(jié)的角度值qi都是由實際測量值θi和偏移量αi計算得出:
其中, ,每個元素i都代表了如圖2所示的一個末端執(zhí)行器(舵機)。根據(jù)機器人生產(chǎn)廠家提供的數(shù)據(jù),這些舵機能夠以0.1°的精度來測量其當(dāng)前角度值(同樣的精度也適用于舵機對于自身角度的控制)。圖2中標注了每個舵機的位置,它們的名稱也與機器人生產(chǎn)廠家所提供的數(shù)據(jù)保持一致。
NAO機器人頭底部攝像頭相對于機器人的位姿(即從機器人原點到攝像頭的齊次坐標變換)[7]:
其中,Rotx、Roty、Rotz表示攝像頭繞對應(yīng)坐標軸的旋轉(zhuǎn)矩陣。同理,Transx、Transy、Transz表示攝像頭沿對應(yīng)坐標軸的平移矩陣,由于在標定過程中攝像頭并沒有產(chǎn)生沿Z軸方向上的位移,所以在解算過程中沒有涉及到Transz。從公式(3)可以看出, 受αCamPitch、αCamRoll、αCamYaw這3個方向的偏移量影響。
根據(jù)第1.1節(jié)給出的NAO機器人腳底棋盤格標定板的信息,可得出所有角點相對于機器人腳底部的坐標 :
其中,因裝配標定板的過程產(chǎn)生誤差,所以對于每塊標定板還有另外3個參數(shù)需要考慮,即:機器人左腳的標定板相對于Z軸有一個 的旋轉(zhuǎn)偏移量,X和Y方向上有平移偏移量 和 。
最終的角點投影模型Proj為:
式(5)中,角點相對于腳底的坐標最終被投射到NAO機器人的攝像頭平面上。本文在參考當(dāng)前的關(guān)節(jié)傳感器數(shù)據(jù)和標定參數(shù)后,得到該點在圖像上的預(yù)測坐標值。
以上內(nèi)容全部是針對NAO機器人左腿的運動學(xué)解算,本文對其右腿也進行了類似的計算。此外,截至到目前,仿人足球機器人比賽中并不涉及手部操作,因此本文只對機器人腿部進行運動學(xué)解算。
表1列出了機器人腿部運動學(xué)解算過程中所涉及到的21個參數(shù),其中最關(guān)鍵的參數(shù)是3個攝像頭旋轉(zhuǎn)偏移量和12個腿部關(guān)節(jié)偏移量。
表1 NAO機器人腿部運動學(xué)解算涉及到的所有參數(shù)
試驗分為兩個階段:通過SimRobot軟件[8]在NAO機器人上實際完成的自動測量階段,以及估算測量圖像得到的坐標值與根據(jù)模型預(yù)測得到的預(yù)測值之間誤差的數(shù)據(jù)分析階段。
測量的目的是要得到棋盤格標定板角點的二維圖像坐標。角點定位是通過文獻[9]的角點檢測算法,文獻[10]、文獻[11]、文獻[12]、文獻[13]的棋盤格提取算法和文獻[2]的亞像素細分算法來實現(xiàn)的。
在沒有人為干預(yù)的情況下,為了讓NAO機器人在標定過程中自動穩(wěn)定地運行,筆者首先把NAO機器人放倒在平面上,每一次標定過程都會從3個角度對棋盤格標定板各采集一幅圖像。在選擇這3個角度時,需要保證棋盤格標定板在攝像頭前進行旋轉(zhuǎn)和移動時一直是完全可見的,以保證標定過程的連續(xù)性。采集圖像則是在機器人達到預(yù)定姿態(tài)靜止以后,這是因為攝像頭圖像采集和傳感器數(shù)據(jù)讀取之間存在時間差。圖4展示了NAO機器人完整的自動測量流程。
筆者對于圖4中觀察到的每個角點,采集一個新的測量值m并將其添加到一個集合M中,則有∶
圖4 標定流程圖
測量值由3個部分構(gòu)成,即:角點在圖像上的位置Pimg,角點相對于腳底部坐標系的三維坐標Pcb,測量到的關(guān)節(jié)角度值θ,由這3個值可得出:
筆者首先建立一個殘差集合R,表示測量圖像的坐標值m[Pimg]與預(yù)測值之間的誤差,其用到了基于公式(5)中的投影模型,測量的關(guān)節(jié)角度值m[θ],角點相對于腳底的坐標m[Pcb],還有表1的偏移量 ,則有:
然后,通過對殘差求平方和,標定可以被定義為非線性最小二乘的問題:
迭代過程筆者采用了LM算法來最小化參數(shù)集合 的殘差平方和,并將初始的參數(shù)值設(shè)為0。其中,有關(guān)攝像頭的3個旋轉(zhuǎn)偏移量計算是在求解NAO機器人的攝像頭矩陣的時候完成的。
筆者在NAO機器人上連續(xù)3次運行自動標定程序,在此之前已經(jīng)通過標定得到了攝像頭的內(nèi)參,焦距是562.5個像素,光心是(324,189)T,最終的參數(shù)值以及細節(jié)信息,如標定過程的持續(xù)時間等,如表2所示。
表2 NAO機器人連續(xù)3次標定后的參數(shù)值
從表2中可以看出,一個標定周期平均耗時為590s,LM平均迭代41次后收斂。均方根誤差在x方向上大約3個像素,y方向上2.5個像素。值得注意的是,對于優(yōu)化后的參數(shù),NAO機器人左腿胯部傾斜角的偏移量很大,最大值達到0.5421°。這說明,不同的機器人擺放姿勢有助于最小化殘差平方和。相比之下,NAO機器人右腿各個參數(shù)的最大偏移量為0.2413°。在經(jīng)過3次標定后,絕大多數(shù)的參數(shù)值都是相似的,特別是3個相機的旋轉(zhuǎn)偏移量,而棋盤格標定板的裝配補償參數(shù)非常小,如圖5所示。
圖5 標定板投影效果
很明顯,NAO機器人在經(jīng)過連續(xù)3次自動標定后,得到的參數(shù)對于最小化問題來說不是一個完美的解決方案,因為有許多殘差大于3倍的標準差(見圖6b)。而并不是所有情況下都能像圖5那樣,NAO機器人在經(jīng)過標定后,棋盤格標定板的投影能夠與圖像完全匹配。在許多姿態(tài)下,棋盤格標定板的投影與圖像并不是完全匹配的,最可能的原因是:相比站立的機器人,躺下的機器人背部會受到一個反沖力,這個反沖力與站立情況下的受力情況是不同的[14-15]。
圖6 試驗結(jié)果殘差分布
在經(jīng)過連續(xù)3次自動標定后,NAO機器人能夠在踢滿一個半場(10min)的情況下,僅摔倒5次;而在沒有經(jīng)過標定或者長時間使用之后,NAO機器人在同一時間段內(nèi)摔倒12次??梢?,即使該方法在實際機器人身上的標定結(jié)果并不那么準確的情況下,對于提升機器人在球場上的表現(xiàn)還是有一定幫助的。比如:曾經(jīng)試驗中有一個特殊情況,一臺機器人因為摔倒和使用過度等原因,在已經(jīng)不能正常行走0.5m的情況下,經(jīng)過重新運動學(xué)標定后,該機器人可以完成半場比賽。
本文通過D-H法建立NAO機器人的運動學(xué)模型,并利用針孔模型和齊次坐標變換建立基于NAO機器人的角點投影模型,最終將標定過程轉(zhuǎn)化為用LM算法求解非線性最小二乘的問題。
在實際NAO機器人上進行標定后,雖然結(jié)果在某幾個參數(shù)上存在誤差,但從機器人比賽過程中的表現(xiàn)來看,該標定方法對于提升機器人賽前標定速度和在比賽過程中整體的運動表現(xiàn)依然有較大幫助。筆者使用該標定方法的機器人參加了2017RoboCup機器人世界杯中國賽RoboCup足球類人組項目的比賽,并獲得了一等獎。