專利名稱：欠驅(qū)動(dòng)兩足機(jī)器人的激勵(lì)規(guī)劃及控制方法
技術(shù)領(lǐng)域：
本發(fā)明涉及機(jī)器人控制技術(shù)領(lǐng)域，具體講是一種欠驅(qū)動(dòng)兩足機(jī)器人的激勵(lì)規(guī)劃及 控制方法。
背景技術(shù)：
欠驅(qū)動(dòng)兩足機(jī)器人是本質(zhì)非線性系統(tǒng)，動(dòng)力學(xué)約束是不可積的，為二階非完整系 統(tǒng)，其軌跡規(guī)劃及控制是一個(gè)難題。由于缺乏對(duì)任意狀態(tài)空間軌跡的跟蹤能力，在欠驅(qū)動(dòng)系 統(tǒng)里，運(yùn)動(dòng)軌道的生成比一般常規(guī)系統(tǒng)難度要大得多，軌道的可實(shí)現(xiàn)性完全依賴于動(dòng)力學(xué) 關(guān)系。欠驅(qū)動(dòng)系統(tǒng)屬于不完全可控的系統(tǒng)，一般的光滑狀態(tài)反饋控制方法是無(wú)法令其鎮(zhèn)定 的。目前很多研究人員都是針對(duì)被動(dòng)關(guān)節(jié)(欠驅(qū)動(dòng)關(guān)節(jié))含有制動(dòng)器的情況進(jìn)行研究 的，而對(duì)于被動(dòng)關(guān)節(jié)沒(méi)有制動(dòng)器的情況，仍然沒(méi)有實(shí)時(shí)性高、工程實(shí)用性強(qiáng)的欠驅(qū)動(dòng)兩足機(jī) 器人的激勵(lì)規(guī)劃及控制方法出現(xiàn)。

發(fā)明內(nèi)容
本發(fā)明要解決的技術(shù)問(wèn)題是，克服以上現(xiàn)有技術(shù)的缺點(diǎn)，提供一種實(shí)時(shí)性高、工程 實(shí)用性強(qiáng)的欠驅(qū)動(dòng)兩足機(jī)器人的激勵(lì)規(guī)劃及控制方法。本發(fā)明的技術(shù)方案是，提供一種欠驅(qū)動(dòng)兩足機(jī)器人的激勵(lì)規(guī)劃及控制方法，由以 下步驟實(shí)現(xiàn)(1)建立欠驅(qū)動(dòng)兩足機(jī)器人的數(shù)學(xué)模型，并將該數(shù)學(xué)模型轉(zhuǎn)化為系統(tǒng)狀態(tài)方程；(2)選定激勵(lì)信號(hào)間斷性振蕩力矩正弦函數(shù)，或脈沖函數(shù)；(3)輸入激勵(lì)信號(hào)，根據(jù)欠驅(qū)動(dòng)兩足機(jī)器人的數(shù)學(xué)模型計(jì)算輸出關(guān)節(jié)角度位置、關(guān) 節(jié)角度速度、關(guān)節(jié)角度加速度；(4)激勵(lì)規(guī)劃把步驟(3)中得到的關(guān)節(jié)角度位置、關(guān)節(jié)角度速度、關(guān)節(jié)角度加速 度作為實(shí)際欠驅(qū)動(dòng)兩足機(jī)器人輸出的給定值，即欠驅(qū)動(dòng)兩足機(jī)器人的角度激勵(lì)規(guī)劃；記被 動(dòng)關(guān)節(jié)的角度激勵(lì)規(guī)劃為q/，qpd就是被動(dòng)關(guān)節(jié)角度的期望值，欠驅(qū)動(dòng)關(guān)節(jié)的實(shí)際輸出角度 為Qp ；(5)由欠驅(qū)動(dòng)兩足機(jī)器人的傳感器按采樣周期采樣并計(jì)算實(shí)際欠驅(qū)動(dòng)兩足機(jī)器人 的欠驅(qū)動(dòng)關(guān)節(jié)角度qp，欠驅(qū)動(dòng)關(guān)節(jié)角度速度t，欠驅(qū)動(dòng)關(guān)節(jié)角度加速度t ； (6)在每個(gè)采樣周期都將qpd與實(shí)際欠驅(qū)動(dòng)兩足機(jī)器人輸出的qp進(jìn)行比較，得到誤 差，記該誤差為e，由表達(dá)式e = qdp~ %確定，并進(jìn)行滑模變結(jié)構(gòu)控制校正，使e趨向于零；(7)經(jīng)滑模變結(jié)構(gòu)控制校正運(yùn)算后輸出主動(dòng)關(guān)節(jié)控制量，記該控制量為Ua，所述 欠驅(qū)動(dòng)兩足機(jī)器人根據(jù)Ua去控制欠驅(qū)動(dòng)兩足機(jī)器人的主動(dòng)關(guān)節(jié)伺服驅(qū)動(dòng)器或主動(dòng)關(guān)節(jié)電 機(jī)；(8)重復(fù)步驟(3) (7)，得到每個(gè)采樣周期主動(dòng)關(guān)節(jié)的實(shí)際控制量，同時(shí)根據(jù)實(shí) 際控制量實(shí)時(shí)控制所述欠驅(qū)動(dòng)兩足機(jī)器人的運(yùn)動(dòng)。
采用上述方法后，本發(fā)明與現(xiàn)有技術(shù)相比，具有以下顯著優(yōu)點(diǎn)及有益效果因?yàn)?本發(fā)明欠驅(qū)動(dòng)兩足機(jī)器人的激勵(lì)規(guī)劃及控制方法在被動(dòng)關(guān)節(jié)無(wú)制動(dòng)器的情況下，采用間斷 性振蕩力矩正弦函數(shù)或脈沖函數(shù)輸入激勵(lì)信號(hào)給欠驅(qū)動(dòng)兩足機(jī)器人的數(shù)學(xué)模型，并計(jì)算輸 出主動(dòng)關(guān)節(jié)角度進(jìn)行軌跡規(guī)劃，利用滑模變結(jié)構(gòu)控制校正方法實(shí)現(xiàn)期望軌跡的跟蹤控制， 克服非完整系統(tǒng)帶來(lái)的軌跡規(guī)劃及控制難題，且規(guī)劃和控制實(shí)時(shí)完成，計(jì)算時(shí)間短、計(jì)算簡(jiǎn) 單，所以本發(fā)明欠驅(qū)動(dòng)兩足機(jī)器人的激勵(lì)規(guī)劃及控制方法具有實(shí)時(shí)性高、工程實(shí)用性強(qiáng)的 優(yōu)點(diǎn)。


附圖是本發(fā)明欠驅(qū)動(dòng)兩足機(jī)器人的激勵(lì)規(guī)劃及控制方法的原理圖。
具體實(shí)施例方式下面結(jié)合附圖和具體實(shí)施例對(duì)本發(fā)明作進(jìn)一步說(shuō)明。一般欠驅(qū)動(dòng)兩足機(jī)器人的規(guī)劃及控制包括關(guān)節(jié)(主動(dòng)關(guān)節(jié)和被動(dòng)關(guān)節(jié))角度位置 傳感器、關(guān)節(jié)角度速度傳感器、關(guān)節(jié)角度加速度傳感器、測(cè)力傳感器、控制器和伺服驅(qū)動(dòng)器， 本發(fā)明欠驅(qū)動(dòng)兩足機(jī)器人的激勵(lì)規(guī)劃及控制方法需要調(diào)用這些現(xiàn)有的硬件。本發(fā)明欠驅(qū)動(dòng)兩足機(jī)器人的激勵(lì)規(guī)劃及控制方法的控制過(guò)程如下(1)建立欠驅(qū)動(dòng)兩足機(jī)器人的數(shù)學(xué)模型，并將該數(shù)學(xué)模型簡(jiǎn)化為系統(tǒng)狀態(tài)方程，一 般欠驅(qū)動(dòng)兩足機(jī)器人的數(shù)學(xué)模型簡(jiǎn)化為以下數(shù)學(xué)模型k M」kHwJ+k(—J=k該式中，下標(biāo)a表示主動(dòng)關(guān)節(jié)，ρ表示被動(dòng)關(guān)節(jié)，q= [qa qp]T為關(guān)節(jié)變量，t是系
_ M M ‘
統(tǒng)的被動(dòng)關(guān)節(jié)加速度，..是系統(tǒng)的主動(dòng)關(guān)節(jié)加速度，““為慣量矩陣，且Map(q)
4aL Pa "^ΛΡ」
= MTpa(q) ；C= aK； \為科氏力和離心力項(xiàng) ’G= / …為重力和其他力；、為主動(dòng) P_Cp(q,q)_Gp{q,q)
關(guān)節(jié)驅(qū)動(dòng)力；被動(dòng)關(guān)節(jié)對(duì)應(yīng)的驅(qū)動(dòng)力為零(2)選定激勵(lì)信號(hào)選定激勵(lì)函數(shù)為正弦函數(shù)rfl=Jsin[iy(i-7；) +洌+ 5，其中，識(shí)
為相位角，B為偏置，Ttl為步長(zhǎng)；(3)由欠驅(qū)動(dòng)兩足機(jī)器人數(shù)學(xué)模型計(jì)算輸出關(guān)節(jié)角度位置q = [qa qp]T、關(guān)節(jié)角度 速度—k dr、關(guān)節(jié)角度加速度々=[也‘(4)把步驟(3)中得到的關(guān)節(jié)角度位置、關(guān)節(jié)角度速度、關(guān)節(jié)角度加速度作為實(shí)際 欠驅(qū)動(dòng)兩足機(jī)器人輸出的給定值qd = q = [qa %V,qd=[^ ^ =k(5)記欠驅(qū)動(dòng)關(guān)節(jié)的角度激勵(lì)規(guī)劃為qpd，欠驅(qū)動(dòng)關(guān)節(jié)的實(shí)際輸出角度qp ；(6)在每個(gè)采樣周期都將qpd與實(shí)際欠驅(qū)動(dòng)兩足機(jī)器人輸出的qp進(jìn)行比較，得到誤 差，記該誤差為e，由表達(dá)式e = qdp - %確定，并進(jìn)行滑模變結(jié)構(gòu)控制校正，使e趨向于零；(7)經(jīng)滑模變結(jié)構(gòu)控制校正運(yùn)算后輸出主動(dòng)關(guān)節(jié)控制量，記該控制量為ua，所述欠 驅(qū)動(dòng)兩足機(jī)器人根據(jù)Ua去控制欠驅(qū)動(dòng)兩足機(jī)器人的主動(dòng)關(guān)節(jié)伺服驅(qū)動(dòng)器或主動(dòng)關(guān)節(jié)電機(jī)，
<formula>formula see original document page 5</formula><formula>formula see original document page 5</formula>其中j = +眾，η，ξ和ζ為適當(dāng)系數(shù)，sgn( ·)是符號(hào)函數(shù)；(8)重復(fù)步驟(3) (7)，得到每個(gè)采樣周期主動(dòng)關(guān)節(jié)的實(shí)際控制量，同時(shí)根據(jù)實(shí) 際控制量實(shí)時(shí)控制所述欠驅(qū)動(dòng)兩足機(jī)器人的運(yùn)動(dòng)。以上僅就本發(fā)明較佳的實(shí)施例作了說(shuō)明，但不能理解為是對(duì)權(quán)利要求的限制。本 發(fā)明的結(jié)構(gòu)可以有其他變化，不局限于上述結(jié)構(gòu)，比如所述激勵(lì)函數(shù)可以為脈沖函數(shù)?？?之，凡在本發(fā)明產(chǎn)品獨(dú)立權(quán)利要求的保護(hù)范圍內(nèi)所作的各種變化均在本發(fā)明的保護(hù)范圍 內(nèi)。
權(quán)利要求
一種欠驅(qū)動(dòng)兩足機(jī)器人的激勵(lì)規(guī)劃及控制方法，其特征在于，由以下步驟實(shí)現(xiàn)(1)建立欠驅(qū)動(dòng)兩足機(jī)器人的數(shù)學(xué)模型，并將該數(shù)學(xué)模型轉(zhuǎn)化為系統(tǒng)狀態(tài)方程；(2)選定激勵(lì)信號(hào)間斷性振蕩力矩正弦函數(shù)，或脈沖函數(shù)；(3)輸入激勵(lì)信號(hào)，根據(jù)欠驅(qū)動(dòng)兩足機(jī)器人的數(shù)學(xué)模型計(jì)算輸出關(guān)節(jié)角度位置、關(guān)節(jié)角度速度、關(guān)節(jié)角度加速度；(4)激勵(lì)規(guī)劃把步驟(3)中得到的關(guān)節(jié)角度位置、關(guān)節(jié)角度速度、關(guān)節(jié)角度加速度作為實(shí)際欠驅(qū)動(dòng)兩足機(jī)器人輸出的給定值，即欠驅(qū)動(dòng)兩足機(jī)器人的角度激勵(lì)規(guī)劃；記被動(dòng)關(guān)節(jié)的角度激勵(lì)規(guī)劃為qpd，qpd就是被動(dòng)關(guān)節(jié)角度的期望值，欠驅(qū)動(dòng)關(guān)節(jié)的實(shí)際輸出角度為qp；(5)由欠驅(qū)動(dòng)兩足機(jī)器人的傳感器按采樣周期采樣并計(jì)算實(shí)際欠驅(qū)動(dòng)兩足機(jī)器人的欠驅(qū)動(dòng)關(guān)節(jié)角度qp，欠驅(qū)動(dòng)關(guān)節(jié)角度速度欠驅(qū)動(dòng)關(guān)節(jié)角度加速度(6)在每個(gè)采樣周期都將qpd與實(shí)際欠驅(qū)動(dòng)兩足機(jī)器人輸出的qp進(jìn)行比較，得到誤差，記該誤差為e，由表達(dá)式 <mrow><mi>e</mi><mo>=</mo><msubsup> <mi>q</mi> <mi>p</mi> <mi>d</mi></msubsup><mo>-</mo><msub> <mi>q</mi> <mi>p</mi></msub> </mrow>確定，并進(jìn)行滑模變結(jié)構(gòu)控制校正，使e趨向于零；(7)經(jīng)滑模變結(jié)構(gòu)控制校正運(yùn)算后輸出主動(dòng)關(guān)節(jié)控制量，記該控制量為ua，所述欠驅(qū)動(dòng)兩足機(jī)器人根據(jù)ua去控制欠驅(qū)動(dòng)兩足機(jī)器人的主動(dòng)關(guān)節(jié)伺服驅(qū)動(dòng)器或主動(dòng)關(guān)節(jié)電機(jī)；(8)重復(fù)步驟(3)～(7)，得到每個(gè)采樣周期主動(dòng)關(guān)節(jié)的實(shí)際控制量，同時(shí)根據(jù)實(shí)際控制量實(shí)時(shí)控制所述欠驅(qū)動(dòng)兩足機(jī)器人的運(yùn)動(dòng)。F2010100400063C00011.tif,F2010100400063C00012.tif
2.根據(jù)權(quán)利要求1所述的欠驅(qū)動(dòng)兩足機(jī)器人的激勵(lì)規(guī)劃及控制方法，其特征在于，選定激勵(lì)信號(hào)為正弦函數(shù)L =』&[樹(shù)/-7；) +刎+ S，其中，爐為相位角，B為偏置，T0為步長(zhǎng)。
全文摘要
本發(fā)明公開(kāi)了一種實(shí)時(shí)性高、工程實(shí)用性強(qiáng)的欠驅(qū)動(dòng)兩足機(jī)器人的激勵(lì)規(guī)劃及控制方法，(1)建立數(shù)學(xué)模型，并轉(zhuǎn)化為系統(tǒng)狀態(tài)方程；(2)選定激勵(lì)信號(hào)；(3)輸入激勵(lì)信號(hào)；(4)把步驟(3)中得到的關(guān)節(jié)角度位置、關(guān)節(jié)角度速度、關(guān)節(jié)角度加速度作為給定值；記被動(dòng)關(guān)節(jié)的角度激勵(lì)規(guī)劃為qpd，欠驅(qū)動(dòng)關(guān)節(jié)的實(shí)際輸出角度為qp；(5)由傳感器按采樣周期采樣并計(jì)算實(shí)際的欠驅(qū)動(dòng)關(guān)節(jié)角度qp，欠驅(qū)動(dòng)關(guān)節(jié)角度速度欠驅(qū)動(dòng)關(guān)節(jié)角度加速度(6)在每個(gè)采樣周期都將qpd與qp比較，得到誤差，由確定，并進(jìn)行滑模變結(jié)構(gòu)控制校正，使e趨向于零；(7)經(jīng)滑模變結(jié)構(gòu)控制校正運(yùn)算后輸出主動(dòng)關(guān)節(jié)控制量ua；(8)重復(fù)(3)～(7)，得到每個(gè)采樣周期的主動(dòng)關(guān)節(jié)控制量。
文檔編號(hào)G05B13/04GK101799663SQ20101004000
公開(kāi)日2010年8月11日 申請(qǐng)日期2010年1月12日 優(yōu)先權(quán)日2010年1月12日
發(fā)明者張智煥 申請(qǐng)人:浙江大學(xué)寧波理工學(xué)院