專利名稱：基于快速搜索樹的人型機(jī)器人踢球動(dòng)作信息處理方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種機(jī)器人踢球動(dòng)作信息處理方法，尤其是涉及一種基于快速搜索樹的人型機(jī)器人踢球動(dòng)作信息處理方法。
背景技術(shù)：
近年來機(jī)器人足球賽越來越受世界各國(guó)高等院校的青睞，而在類人機(jī)器人足球賽中，魯棒、有效的踢球動(dòng)作這是十分重要的。目前大多數(shù)的類人機(jī)器人踢球動(dòng)作都是通過人工離線的編排一系列固定的關(guān)節(jié)位置來實(shí)現(xiàn)的，這種方法不僅需要大量繁瑣枯燥的人力勞動(dòng)，而且受實(shí)時(shí)環(huán)境的干擾非常大，地面的微小變化都會(huì)導(dǎo)致踢球動(dòng)作無法完成。

發(fā)明內(nèi)容
本發(fā)明的目的就是為了克服上述現(xiàn)有技術(shù)存在的缺陷而提供一種保證機(jī)器人在不確定環(huán)境下踢球動(dòng)作可靠執(zhí)行的基于快速搜索樹的人型機(jī)器人踢球動(dòng)作信息處理方法。本發(fā)明的目的可以通過以下技術(shù)方案來實(shí)現(xiàn)一種基于快速搜索樹的人型機(jī)器人踢球動(dòng)作信息處理方法，其特征在于，包括以下步驟1)機(jī)器人在不確定環(huán)境下根據(jù)關(guān)節(jié)位置傳感器、加速度傳感器和陀螺儀采集的信息，并通過正向運(yùn)動(dòng)模型得到機(jī)器人當(dāng)前狀態(tài)；2)機(jī)器人根據(jù)當(dāng)前狀態(tài)和踢球的終止?fàn)顟B(tài)，通過快速隨機(jī)搜索樹計(jì)算出所需的機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)軌跡；3)通過動(dòng)作平滑濾波器對(duì)步驟2~)中生成的關(guān)節(jié)運(yùn)動(dòng)軌跡進(jìn)行平滑處理，并動(dòng)力學(xué)濾波器進(jìn)行修正得到穩(wěn)定的最終踢球運(yùn)動(dòng)軌，并將該軌跡發(fā)送給機(jī)器人執(zhí)行機(jī)構(gòu)。所述的步驟1)中的通過正向運(yùn)動(dòng)模型得到機(jī)器人當(dāng)前狀態(tài)的具體步驟如下1)利用加速度傳感器的靜態(tài)特性和陀螺儀的動(dòng)態(tài)特性，通過補(bǔ)償濾波器獲得機(jī)器人身體的全局旋轉(zhuǎn)矩陣Rbw;2)利用正向運(yùn)動(dòng)模型由身體開始向各子連桿遞歸計(jì)算各子連桿全局位置和旋轉(zhuǎn)矩陣Pj = Pi+RibjRj = RiR(Qj)其中ρ」為待求第j連桿全局位置，第i連桿為第j連桿的母連桿，Ri為第i連桿的全局旋轉(zhuǎn)矩陣，bj為第j連桿關(guān)節(jié)在其母連桿坐標(biāo)系下的位置向量；民為待求第j連桿的全局旋轉(zhuǎn)矩陣，R(Qj)是由第j連桿關(guān)節(jié)旋轉(zhuǎn)Clj所造成的旋轉(zhuǎn)效應(yīng)。所述的步驟2、通過快速隨機(jī)搜索樹計(jì)算出所需的機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)軌跡的具體步驟如下1)根據(jù)踢球動(dòng)作的起始位置初始化搜索樹算法的起始狀態(tài)；2)在滿足無連桿碰撞和所設(shè)定支撐模式的約束條件下，使用RRT-Extend方法隨機(jī)生成擴(kuò)展?fàn)顟B(tài)并同時(shí)采用RRT-Cormect生成連接狀態(tài)；3)重復(fù)步驟幻，直到由起始狀態(tài)和終止?fàn)顟B(tài)生成的兩棵搜索樹實(shí)現(xiàn)連接為止。所述的動(dòng)作平滑濾波器通過以下步驟來實(shí)現(xiàn)關(guān)節(jié)運(yùn)動(dòng)軌跡的平滑處理1)采用計(jì)算機(jī)圖形學(xué)中的三階貝塞爾曲線進(jìn)行平滑處理，減小關(guān)節(jié)值的震蕩；2)采用直線切割的方法去除不必要的關(guān)節(jié)運(yùn)動(dòng)。所述的動(dòng)力學(xué)濾波器通過以下步驟來實(shí)現(xiàn)關(guān)節(jié)運(yùn)動(dòng)軌跡的修正處理設(shè)C為機(jī)器人質(zhì)心，是機(jī)器人關(guān)節(jié)角θ的函數(shù)，即C = Χ( θ )，則質(zhì)心的雅可
比矩陣表示為《/(的，亦即= 彡，對(duì)于微小量的變化，近似的有AC = J(e) Δ θ (t)，濾波目標(biāo)為保證質(zhì)心在地面的投影位置不變，該約束條件可表示為AC, = YiJxjAOi(0 = 0 ,ACy = ￡JyjiA^(0 = 0其中Δ θ i = Δ θ cmdi+A θ。。_，Δ θ ,表示實(shí)際角度變化值，Δ θ cmdi表示輸入命令角度變化值，Δ θ。。mpi表示濾波補(bǔ)償值；為了保證對(duì)原始姿態(tài)的動(dòng)作改變最小，設(shè)計(jì)一個(gè)評(píng)價(jià)函數(shù)
E(Aecomp) = ^1(Aecompi)2，將問題轉(zhuǎn)化為一個(gè)最優(yōu)化問題，其表述如下 (=1求取|>(輯—)2)，約束條件ACx = O 與 ACy = O。
/=1與現(xiàn)有技術(shù)相比，本發(fā)明具有采用機(jī)載傳感器計(jì)算機(jī)器人的當(dāng)前狀態(tài)，自動(dòng)規(guī)劃的方法對(duì)機(jī)器人踢球動(dòng)作進(jìn)行實(shí)時(shí)生成，提高了踢球動(dòng)作的魯棒性，保證了機(jī)器人在不確定環(huán)境下踢球動(dòng)作的可靠執(zhí)行。


圖1為本發(fā)明的流程圖；圖2為本發(fā)明的硬件結(jié)構(gòu)示意圖；圖3為本發(fā)明的機(jī)器人二叉樹結(jié)構(gòu)示意圖。
具體實(shí)施例方式下面結(jié)合附圖和具體實(shí)施例對(duì)本發(fā)明進(jìn)行詳細(xì)說明。實(shí)施例如圖1、圖2所示，一種基于快速搜索樹的人型機(jī)器人踢球動(dòng)作信息處理方法，包括以下步驟1)機(jī)器人在不確定環(huán)境下根據(jù)關(guān)節(jié)位置傳感器1、加速度傳感器2和陀螺儀3采集的信息，并通過正向運(yùn)動(dòng)模型得到機(jī)器人當(dāng)前狀態(tài)；2)機(jī)器人的處理器4根據(jù)當(dāng)前狀態(tài)和踢球的終止?fàn)顟B(tài)，通過快速隨機(jī)搜索樹計(jì)算出所需的機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)軌跡；3)通過動(dòng)作平滑濾波器對(duì)步驟2、中生成的關(guān)節(jié)運(yùn)動(dòng)軌跡進(jìn)行平滑處理，并動(dòng)力學(xué)濾波器進(jìn)行修正得到穩(wěn)定的最終踢球運(yùn)動(dòng)軌跡，并將該軌跡發(fā)送給機(jī)器人執(zhí)行機(jī)構(gòu)。
機(jī)器人位姿的計(jì)算是動(dòng)作規(guī)劃的基礎(chǔ)之一，主要包含正運(yùn)動(dòng)學(xué)和逆運(yùn)動(dòng)學(xué)計(jì)算兩部分。為了方便對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)的計(jì)算，需要將機(jī)器人抽象為一系列連桿和關(guān)節(jié)構(gòu)成的模型。同時(shí)，由于在機(jī)器人動(dòng)作規(guī)劃過程中還要滿足其與自身或環(huán)境無碰撞的條件。還需要將機(jī)器人抽象為一系列三維物體構(gòu)成的模型。人形機(jī)器人是由很多連桿和關(guān)節(jié)構(gòu)成的復(fù)雜機(jī)械系統(tǒng)，對(duì)其進(jìn)行運(yùn)動(dòng)學(xué)分析的第一步是對(duì)其連桿和關(guān)節(jié)進(jìn)行分解。劃分好關(guān)節(jié)后， 需要對(duì)連桿連接關(guān)系進(jìn)行描述，這里采用二叉樹結(jié)構(gòu)(如圖3)，左子樹表示子結(jié)點(diǎn)，右子樹表示姊妹結(jié)點(diǎn)，結(jié)構(gòu)明確固定，利于編程。人形機(jī)器人腿部股關(guān)節(jié)處有兩個(gè)在TL平面上與Z軸呈45度和135度的關(guān)節(jié)。在建模時(shí)均假設(shè)這兩個(gè)關(guān)節(jié)的關(guān)節(jié)軸相對(duì)世界坐標(biāo)系的朝向沿Z軸正方向。因此計(jì)算旋轉(zhuǎn)矩陣時(shí)須分別作特殊處理如下( 表示該關(guān)節(jié)的角度值)
權(quán)利要求
1.一種基于快速搜索樹的人型機(jī)器人踢球動(dòng)作信息處理方法，其特征在于，包括以下步驟1)機(jī)器人在不確定環(huán)境下根據(jù)關(guān)節(jié)位置傳感器、加速度傳感器和陀螺儀采集的信息， 并通過正向運(yùn)動(dòng)模型得到機(jī)器人當(dāng)前狀態(tài)；2)機(jī)器人根據(jù)當(dāng)前狀態(tài)和踢球的終止?fàn)顟B(tài)，通過快速隨機(jī)搜索樹計(jì)算出所需的機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)軌跡；3)通過動(dòng)作平滑濾波器對(duì)步驟幻中生成的關(guān)節(jié)運(yùn)動(dòng)軌跡進(jìn)行平滑處理，并通過動(dòng)力學(xué)濾波器進(jìn)行修正得到穩(wěn)定的最終踢球運(yùn)動(dòng)軌跡，并將該軌跡發(fā)送給機(jī)器人執(zhí)行機(jī)構(gòu)。
2.根據(jù)權(quán)利要求1所述的一種基于快速搜索樹的人型機(jī)器人踢球動(dòng)作信息處理方法， 其特征在于，所述的步驟1)中的通過正向運(yùn)動(dòng)模型得到機(jī)器人當(dāng)前狀態(tài)的具體步驟如下1)利用加速度傳感器的靜態(tài)特性和陀螺儀的動(dòng)態(tài)特性，通過補(bǔ)償濾波器獲得機(jī)器人身體的全局旋轉(zhuǎn)矩陣Rbw ；2)利用正向運(yùn)動(dòng)模型由身體開始向各子連桿遞歸計(jì)算各子連桿全局位置和旋轉(zhuǎn)矩陣Pj = Pi+RibjRj = RiR(Qj)其中P」為待求第j連桿全局位置，第i連桿為第j連桿的母連桿，Ri為第i連桿的全局旋轉(zhuǎn)矩陣，bj為第j連桿關(guān)節(jié)在其母連桿坐標(biāo)系下的位置向量；民為待求第j連桿的全局旋轉(zhuǎn)矩陣，R(Qj)是由第j連桿關(guān)節(jié)旋轉(zhuǎn)Clj所造成的旋轉(zhuǎn)效應(yīng)。
3.根據(jù)權(quán)利要求1所述的一種基于快速搜索樹的人型機(jī)器人踢球動(dòng)作信息處理方法， 其特征在于，所述的步驟幻通過快速隨機(jī)搜索樹計(jì)算出所需的機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)軌跡的具體步驟如下1)根據(jù)踢球動(dòng)作的起始位置初始化搜索樹算法的起始狀態(tài)；2)在滿足無連桿碰撞和所設(shè)定支撐模式的約束條件下，使用RRT-Extend方法隨機(jī)生成擴(kuò)展?fàn)顟B(tài)并同時(shí)采用RRT-Cormect生成連接狀態(tài)；3)重復(fù)步驟幻，直到由起始狀態(tài)和終止?fàn)顟B(tài)生成的兩棵搜索樹實(shí)現(xiàn)連接為止。
4.根據(jù)權(quán)利要求1所述的一種基于快速搜索樹的人型機(jī)器人踢球動(dòng)作信息處理方法， 其特征在于，所述的動(dòng)作平滑濾波器通過以下步驟來實(shí)現(xiàn)關(guān)節(jié)運(yùn)動(dòng)軌跡的平滑處理1)采用計(jì)算機(jī)圖形學(xué)中的三階貝塞爾曲線進(jìn)行平滑處理，減小關(guān)節(jié)值的震蕩；2)采用直線切割的方法去除不必要的關(guān)節(jié)運(yùn)動(dòng)。
5.根據(jù)權(quán)利要求1所述的一種基于快速搜索樹的人型機(jī)器人踢球動(dòng)作信息處理方法， 其特征在于，所述的動(dòng)力學(xué)濾波器通過以下步驟來實(shí)現(xiàn)關(guān)節(jié)運(yùn)動(dòng)軌跡的修正處理設(shè)C為機(jī)器人質(zhì)心，是機(jī)器人關(guān)節(jié)角θ的函數(shù)，即C = χ( θ )，則質(zhì)心的雅可比矩陣表示為J(的，亦即0 = /⑹々，對(duì)于微小量的變化，近似的有AC = ΚΘ)Δ θ⑴，濾波目標(biāo)為保證質(zhì)心在地面的投影位置不變，該約束條件可表示為 AC, == 0 ACy = YiJyjAm = 0/=1Z=I其中Δ θ i = Δ θ cmdi+A θ。。_，Δ θ i表示實(shí)際角度變化值，Δ θ。mdi表示輸入命令角度變化值，Δ θ。。mpi表示濾波補(bǔ)償值；為了保證對(duì)原始姿態(tài)的動(dòng)作改變最小，設(shè)計(jì)一個(gè)評(píng)價(jià)函數(shù) 22’將問題轉(zhuǎn)化為一個(gè)最優(yōu)化問題，其表述如下: 求取Mil^E(Mcomp) = |>,(Δ0_)2) ’ 約束條件 AC, = 0 與 AC廣 0。
全文摘要
本發(fā)明涉及一種基于快速搜索樹的人型機(jī)器人踢球動(dòng)作信息處理方法，包括以下步驟1)機(jī)器人在不確定環(huán)境下根據(jù)關(guān)節(jié)位置傳感器、加速度傳感器和陀螺儀采集的信息，并通過正向運(yùn)動(dòng)模型得到機(jī)器人當(dāng)前狀態(tài)；并通過正向運(yùn)動(dòng)模型得到機(jī)器人當(dāng)前狀態(tài)；2)機(jī)器人根據(jù)當(dāng)前狀態(tài)和踢球的終止?fàn)顟B(tài)，通過快速隨機(jī)搜索樹計(jì)算出所需的機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)軌跡；3)通過動(dòng)作平滑濾波器對(duì)步驟2)中生成的關(guān)節(jié)運(yùn)動(dòng)軌跡進(jìn)行平滑處理，并動(dòng)力學(xué)濾波器進(jìn)行修正得到穩(wěn)定的最終踢球運(yùn)動(dòng)軌跡。與現(xiàn)有技術(shù)相比，本發(fā)明具有保證機(jī)器人在不確定環(huán)境下踢球動(dòng)作可靠執(zhí)行等優(yōu)點(diǎn)。
文檔編號(hào)G05B19/04GK102375416SQ20101025276
公開日2012年3月14日 申請(qǐng)日期2010年8月13日 優(yōu)先權(quán)日2010年8月13日
發(fā)明者許濤, 鄧方舟, 陳啟軍 申請(qǐng)人:同濟(jì)大學(xué)