專利名稱：：一種雙輪差動式機器人運動控制方法
技術(shù)領(lǐng)域：
：本發(fā)明涉及一種雙輪差動式機器人運動控制方法，具體地說是一種基于運動學(xué)的雙輪差動式機器人運動控制方法，可用于任何要求具有高精度、高穩(wěn)定性的雙輪差動式機器人的運動控制。
背景技術(shù)：
：機器人技術(shù)是一種面向未來的現(xiàn)代化技術(shù)，它和網(wǎng)絡(luò)技術(shù)、基因技術(shù)、通信技術(shù)、計算機技術(shù)等科學(xué)技術(shù)一樣，也屬于高新技術(shù)。早在1920年時，"ROBOT"這個詞就開始在捷克作家K.凱比克的一本科幻劇本中出現(xiàn)。在1950年，美國作家I.阿西莫夫又提出了機器人學(xué)(Roboties)這一概念，并提出了所謂的"機器人三原則"。在21世紀(jì)的今天，可以說機器人在當(dāng)今社會對于人類的作用和影響越來越大，如在軍事領(lǐng)域、醫(yī)學(xué)領(lǐng)域、環(huán)保產(chǎn)業(yè)領(lǐng)域、農(nóng)業(yè)領(lǐng)域、太空和深海探測等諸多方面都有著不同性能和不同種類的機器人從事著相關(guān)工作，并且隨著各項技術(shù)的提高，機器人將對人類的進(jìn)步和發(fā)展產(chǎn)生深遠(yuǎn)的影響。業(yè)內(nèi)周知，機器人要完成某個任務(wù)，都是通過其具體動作來實現(xiàn)的。而機器人的動作則由機器人最底層運動控制決定的，底層運動控制是完成各種復(fù)雜動作的基本保證。雙輪差動機器人也不例外，所有的雙輪差動機器人動作歸根結(jié)底是根據(jù)場上的不同情況由決策系統(tǒng)自主直接指定其左、右輪速度，實現(xiàn)機器人的直線、曲線、和原地旋轉(zhuǎn)等基本動作，再由這些基本動作組合成較為復(fù)雜的動作。由于對于雙輪差動式機器人模型，從運動學(xué)角度講它不能同時滿足非完全約束和動力學(xué)約束的條件，是一個非完整系統(tǒng)。為了實現(xiàn)機器人快速而準(zhǔn)確的運動控制，國內(nèi)外學(xué)者在這方面提出了許多方法。例如在原地轉(zhuǎn)角控制方面，現(xiàn)在普遍采用的是比例線性控制算法，即由當(dāng)前方向角與目標(biāo)方向角之間的差值A(chǔ)e作為控制量，然后通過大量測試確定一個比例系數(shù)a，將它們的乘積a，Ae作為數(shù)字控制器的輸出，作用到電機上以驅(qū)動車輪。這種方法原理簡單直觀，且容易實現(xiàn)，但在實際應(yīng)用中，由于物理慣性的影響，會使機器人產(chǎn)生強烈的震蕩，不能快速、穩(wěn)定的達(dá)到目標(biāo)角度。而在跑定點動作控制中，現(xiàn)在普遍采用的有弦切法、誤差比例控制法等，這些方法雖然都有比較明確的物理或幾何意義，但都由于沒有對于物理慣性或機器人的實際應(yīng)用環(huán)境給與特殊考慮，故在實際應(yīng)用中都存在各個方面的問題。比如應(yīng)用弦切法控制，就容易使機器人運動軌跡過長，從而延長了運動時間；而采用誤差比例控制法，就容易使機器人在目標(biāo)點附近產(chǎn)生強烈的震蕩，不能有效的定位于目標(biāo)點，而震蕩現(xiàn)象也容易引起電機故障，從而減少電機的使用壽命。
發(fā)明內(nèi)容要解決的技術(shù)問題為了避免現(xiàn)有技術(shù)的不足之處，本發(fā)明提出一種雙輪差動式機器人運動控制方法，以實現(xiàn)雙輪差動式機器人在各種復(fù)雜工作環(huán)境下快速、穩(wěn)定、精確的運動控制。技術(shù)方案4—種雙輪差動式機器人運動控制方法，其特征在于以機器人的幾何中心為坐標(biāo)原點在世界坐標(biāo)系XOY中建立本體坐標(biāo)系xoy，轉(zhuǎn)角運動的控制步驟和跑位運動的控制步驟如下轉(zhuǎn)角運動的控制步驟步驟i計算機器人的當(dāng)前方向角和目標(biāo)方向角角度差A(yù)e=|ed-e」，其中ed為機器人的目標(biāo)方向角，e。為機器人的當(dāng)前方向角；步驟2計算左右輪速值In"a<formula>formulaseeoriginaldocumentpage5</formula>、為左輪速值，VK為右輪速值；項數(shù)n和底數(shù)a的參數(shù)選擇策略如下當(dāng)A9的區(qū)間為時，n取為2、a取為1.8;當(dāng)A9的區(qū)間為(5，15]時，n取為3、a取為1.4;當(dāng)A9的區(qū)間為(15，30]時，n取為4、a取為1.21;當(dāng)A9的區(qū)間為(30，90]時，n取為5、a取為1.12;步驟3:將步驟2得到的輪速分別作用于機器人左右輪的伺服電機上，驅(qū)動機器人左右輪作相應(yīng)的轉(zhuǎn)動；跑位運動的控制步驟步驟1計算機器人當(dāng)前位置坐標(biāo)與目標(biāo)點位置坐標(biāo)的距離<formula>formulaseeoriginaldocumentpage5</formula>其中xsta、ysta為機器人當(dāng)前的位置坐標(biāo)，X(tes、ytes為目標(biāo)點的位置坐標(biāo)；步驟2計算機器人當(dāng)前方向角與當(dāng)前位置和目標(biāo)點位置連線的夾角△《=arctan(」Xfifey—、ta其中en為機器人的當(dāng)前方向角；步驟3進(jìn)行跑位震蕩檢測比較機器人運動最近三個仿真節(jié)拍的目標(biāo)點坐標(biāo)和機器人的位置坐標(biāo)，當(dāng)兩拍的數(shù)據(jù)不相等時執(zhí)行下一步驟；當(dāng)兩拍的數(shù)據(jù)相等時，斷定機器人當(dāng)前處在目標(biāo)點附近的震蕩狀態(tài)，將機器人正面朝向與X軸正方向夾角設(shè)為9p，機器人目標(biāo)位置和當(dāng)前位置的連線與X軸正方向的夾角設(shè)為、，則機器人執(zhí)行轉(zhuǎn)角運動控制步驟、做一個由9p轉(zhuǎn)向9q的原地轉(zhuǎn)角運動，然后執(zhí)行下一步驟；步驟4:以誤差比例控制方法產(chǎn)生的最大震蕩距離diSvib為分界線「^=cos闊)+g,當(dāng)dis>=disvib時，左右輪速值為<formula>formulaseeoriginaldocumentpage5</formula>(ae))和g(ae)采用擬合方法得到；^fr/，其中D—^》V的值L=r+A—當(dāng)dis<disvib時，左右輪速值為uT'i2sin(A(9)，取f(discos(Ae))，為指定的機器人運動的線速度，L是機器人兩輪之間的距離；所述的disvib為機器人的最大速度所對應(yīng)的運動距離為其值；所述的擬合方法為將若干組機器人運動距離dis、轉(zhuǎn)角量A9和運動速度v的關(guān)系dis-v和Ae-v的數(shù)據(jù)，采用擬合方法得到如下公式f(discos(A9))=0.14284*(discos(A9))3-0.66979*(discos(A9))2+12.519*(discos(A9))-()23125g(A9)=0.01178*(A9)2+0.1989*(A9)+l步驟5:將步驟4得到的輪速分別作用于機器人左右輪的伺服電機上，驅(qū)動機器人左右輪作相應(yīng)的轉(zhuǎn)動。有益效果本發(fā)明提出的雙輪差動式機器人運動控制方法，相比現(xiàn)有技術(shù)的優(yōu)越性在于(1)本發(fā)明由于基于運動學(xué)模型，因而在具體實現(xiàn)時，控制系統(tǒng)分為兩層。上層是基于運動學(xué)模型的機器人運動控制算法，下層是電機速度控制器控制驅(qū)動電機旋轉(zhuǎn)以帶動車輪運動。上層在實現(xiàn)機器人的運動控制時，可以不考慮機器人的動力學(xué)特性，即下層的電機速度控制對上層來講是透明的，這樣的建模思想直接反映了機器人的主要運動特性，便于建立合理的運動模型，而且易于設(shè)計相應(yīng)的控制器，同時也使系統(tǒng)開發(fā)模塊化，便于以后控制算法的改進(jìn)。(2)本發(fā)明由于考慮了機器人運動中慣性的影響，因而機器人的運動控制更加精確和全面。(3)本發(fā)明的指數(shù)型原地轉(zhuǎn)角控制算法中，由于可以按角度差A(yù)e的大小分成不同的區(qū)間，在各區(qū)間上分別確定待定參數(shù)，因而擴大了該算法的適用性，同時也提高了機器人的控制效果。(4)本發(fā)明綜合運用了誤差比例控制法和弦切法，從而充分發(fā)揮了其各自的優(yōu)點，同時又避免了其各自算法的不足。(5)本發(fā)明對誤差比例控制法中的線速度函數(shù)f(dis)做了部分改進(jìn)，從而提高了機器人運動的精確性。(6)本發(fā)明參考計算機操作系統(tǒng)中死鎖的防治策略，提出了應(yīng)用于機器人運功控制中的震蕩檢測方法，并說明了一種打破振蕩條件的方法。圖1是雙輪差動式機器人運動控制方法流程圖圖2是本發(fā)明用于說明機器人系統(tǒng)組成的示意圖圖3是本發(fā)明為了數(shù)學(xué)建模所建立的坐標(biāo)系示意圖圖4是本發(fā)明的描述機器人曲線運動的示意圖圖5是本發(fā)明所建立的指數(shù)型原地轉(zhuǎn)角模型的原理說明圖圖6是本發(fā)明用于建立弦切法定點運動模型的原理說明圖具體實施例方式現(xiàn)結(jié)合實施例、附圖對本發(fā)明作進(jìn)一步描述—、本發(fā)明所應(yīng)用的數(shù)學(xué)理論、控制原理及技術(shù)術(shù)語說明1、數(shù)據(jù)擬合所謂數(shù)據(jù)擬合是指已知某函數(shù)的若干離散函數(shù)值{fl，f2，…，fnh通過調(diào)整該函數(shù)中若干待定系數(shù)f(Al，A2，…，An)，使得該函數(shù)與已知點集的差別(最小二乘意義)最小。如果待定函數(shù)是線性，就叫線性擬合或者線性回歸，否則叫作非線性擬合或者非線性回歸。表達(dá)式也可以是分段函數(shù)，這種情況下叫作樣條擬合。數(shù)據(jù)擬合的工程意義在與它可以通過若干離散的數(shù)據(jù)得到一個自變量與因變量之間的連續(xù)的函數(shù)，由該函數(shù)得到的函數(shù)值與已知點集的差別最小。2、循環(huán)震蕩循環(huán)震蕩就是到定點運動控制中機器人在目標(biāo)點附近作永不停歇的震蕩運動，不能靜止在目標(biāo)點的一種現(xiàn)象，不同于普通的震蕩現(xiàn)象，同時這種現(xiàn)象還具有機器人的位置坐標(biāo)在若干拍內(nèi)形成循環(huán)的特點。這是由于雙輪差動式機器人不能同時滿足非完全約束和動力學(xué)約束的條件，是一個非完整系統(tǒng)，因此當(dāng)機器人處于某種初始狀態(tài)，再向某一目標(biāo)點運動時，通過控制算法的計算，會導(dǎo)致機器人永遠(yuǎn)不能到達(dá)目標(biāo)點，而只能在目標(biāo)點附近不斷運動，最終形成循環(huán)。3、伺服電機伺服電機是在伺服系統(tǒng)中控制機械元件運轉(zhuǎn)的發(fā)動機，是一種補助馬達(dá)間接變速裝置。它可以使控制速度，位置精度非常準(zhǔn)確。在自動控制系統(tǒng)中，它用作執(zhí)行元件，把所收到的電信號轉(zhuǎn)換成電動機軸上的角位移或角速度輸出以驅(qū)動控制對象的。二、本發(fā)明的雙輪差動式機器人運動控制方法具體實現(xiàn)本發(fā)明主要包括雙輪差動式機器人的原地轉(zhuǎn)角和機器人運動到定點的控制方法。具體步驟如下步驟一、在機器人所處環(huán)境中建立一個總的坐標(biāo)系，稱之為世界坐標(biāo)系，設(shè)為XOY，然后以機器人的幾何中心為坐標(biāo)原點建立一個坐標(biāo)系，稱之為本體坐標(biāo)系xoy，建立坐標(biāo)系后的機器人如圖3所示，在建立的坐標(biāo)系里構(gòu)建機器人的控制模型。1、設(shè)(x，y)為機器人在所建立的世界坐標(biāo)系下的位置坐標(biāo)；9為機器人的方向角，即X軸到X軸轉(zhuǎn)過的角度，并以逆時針方向為正方向；V為機器人質(zhì)心的線速度大?。籛為機器人質(zhì)心作曲線運動時的角速度大?。籸為機器人輪子的半徑；L為機器人兩輪之間的距離；V。VK為機器人左右輪轉(zhuǎn)動的線速度大??；e,、eK為機器人左右輪轉(zhuǎn)動的角速度大小。2、假定機器人左右輪子和地面之間沒有滑動，且沿直線作純滾動運動，即每瞬時與地面接觸點的速度等于零，則由鋼體運動規(guī)律可得如下方程W=——^<formula>formulaseeoriginaldocumentpage8</formula>3、當(dāng)機器人作曲線運動時，由于機器人運動的軌跡是由一系列繞瞬時圓心旋轉(zhuǎn)的小段圓弧組成，如圖4所示。根據(jù)圖三的幾何關(guān)系可得到方程^《^<formula>formulaseeoriginaldocumentpage8</formula>其中R為瞬時圓弧的半徑。對該方程作等價變換，可得到機器人作圓周運動時的圓弧半徑R為<formula>formulaseeoriginaldocumentpage8</formula>由該方程可以得到，當(dāng)、=VK時，圓弧半徑R趨向無窮大，則機器人作直線運動；當(dāng)、=_VK時，圓弧半徑R等于0，則機器人以質(zhì)心為圓心作原地旋轉(zhuǎn)；當(dāng)\#VK時，則圓弧半徑R為一個固定值，即機器人圍繞瞬時圓心作半徑為R的圓周運動。視覺子系統(tǒng)采集機器人的圖像信息，通過圖像處理后將機器人在已建立的世界坐標(biāo)系中的坐標(biāo)信息，以及機器人的方向信息等傳遞到智能決策子系統(tǒng)。智能決策子系統(tǒng)對視覺子系統(tǒng)傳遞的預(yù)處理信息做相應(yīng)的處理，并對機器人當(dāng)前應(yīng)做的動作作出判斷。如果當(dāng)前拍機器人需要作轉(zhuǎn)角控制，則轉(zhuǎn)向步驟(2);如果當(dāng)前拍機器人需要作精確跑位動作，則轉(zhuǎn)向步驟(3)。步驟二、由機器人當(dāng)前的方向角和目標(biāo)方向角作為數(shù)字控制器的輸入?yún)?shù)，經(jīng)指數(shù)型轉(zhuǎn)角控制算法得到機器人當(dāng)前拍的左輪速和右輪速大小，然后執(zhí)行步驟(4)。1、用目標(biāo)方向角減去當(dāng)前方向角，得到其差值A(chǔ)9，即<formula>formulaseeoriginaldocumentpage8</formula>其中9d為機器人的目標(biāo)方向角，e。為機器人的當(dāng)前方向角。2、考慮到機器人運動時的慣性現(xiàn)象，并為了保證機器人運動的快速性，則當(dāng)A9較大時，機器人應(yīng)有較大的左右輪速Iv」，以達(dá)到快速轉(zhuǎn)角的目的，當(dāng)ae減小時，輪速大小|v」應(yīng)急劇減小，以緩沖機器人慣性的影響，達(dá)到消除震蕩現(xiàn)象，使機器人穩(wěn)定的目的。如圖5為實際測試的某雙輪機器人靜態(tài)轉(zhuǎn)角時|v」和ae之間的關(guān)系圖(對于其他的雙輪式機器人也有類似關(guān)系)，這正符合指數(shù)曲線的特點當(dāng)x較小時y的值較小，且變化較為平緩，當(dāng)x增大時，y的值將隨之急劇增大。因而建立機器人的轉(zhuǎn)角模型為<formula>formulaseeoriginaldocumentpage8</formula>其中參數(shù)a的范圍為a〉1。3、為了便于數(shù)字控制器的實現(xiàn)，由麥克勞林公式及(ax)(n)Ix=。=axlnna|x=。=lnna「，A0fIn"aAn,，A^Inaan2Ina、^3In"a,^^tt!26故只要確定合適的項數(shù)n和底數(shù)a即可由該模型確定機器人左右輪輪速|(zhì)V」和角度差A(yù)e之間的函數(shù)關(guān)系式。4、在具體實現(xiàn)中，選定項數(shù)n和底數(shù)a可參照如下策略隨著項數(shù)n或底數(shù)a的增大，機器人的A9--|V」曲線將逐漸變得比較陡峭，即機器人的轉(zhuǎn)角速度加快，但機器人容易產(chǎn)生震蕩現(xiàn)象。隨著項數(shù)n或底數(shù)a的減小，機器人的Ae_-|V」曲線將逐漸變得比較平緩，即機器人的轉(zhuǎn)角速度減慢，但轉(zhuǎn)角過程平穩(wěn)，機器人不易出現(xiàn)震蕩現(xiàn)象。為了達(dá)到較好的控制效果，通?？梢园碅e的變化將Ae分成幾個不同的區(qū)間，然后在不同的區(qū)間上確定不同的參數(shù)。對于不同的區(qū)間ae，選定參數(shù)時，應(yīng)在以上策略的基礎(chǔ)之上，先選定項數(shù)n，然后從某一個不會引起機器人震蕩的底數(shù)a開始，在盡可能穩(wěn)定的前提下逐漸增大a的取值以提高機器人轉(zhuǎn)角的速度。對于不同區(qū)間上項數(shù)n的確定，由于當(dāng)Ae較小時，機器人的慣性對機器人的運動影響較為顯著，因而轉(zhuǎn)角就越需要平穩(wěn)；當(dāng)A9較大時，機器人的慣性對機器人的運動影響不是很明顯，且為了達(dá)到快速性的要求，轉(zhuǎn)角速度需要提高。因而對于不同的區(qū)間，隨著Ae的增大，該區(qū)間上項數(shù)n的值也應(yīng)該逐漸增大。項數(shù)n和底數(shù)a的參數(shù)選擇策略如下當(dāng)A9的區(qū)間為時，n取為2、a取為1.8;當(dāng)Ae的區(qū)間為(5，15]時，n取為3、a取為1.4;當(dāng)A9的區(qū)間為(15，30]時，n取為4、a取為1.21;當(dāng)A9的區(qū)間為(30，90]時，n取為5、a取為1.12;5、在確定了轉(zhuǎn)角模型的所有參數(shù)后，即可由該模型得到確定的機器人左右輪輪子的速度大小I、I，將該值作為數(shù)字控制器的輸出。步驟三、由機器人當(dāng)前的位置坐標(biāo)、目標(biāo)點位置坐標(biāo)和機器人的當(dāng)前方向角作為數(shù)字控制器的輸入?yún)?shù)，經(jīng)改進(jìn)后的誤差比例控制算法得到機器人當(dāng)前拍的左輪速和右輪速大小，然后執(zhí)行步驟(4)。1、由機器人當(dāng)前的位置坐標(biāo)和目標(biāo)點的位置坐標(biāo)，得到這兩點之間的歐式距離，即其中xsta、ysta為機器人當(dāng)前的位置坐標(biāo)，Xfcs、ydes為目標(biāo)點的位置坐標(biāo)。2、由數(shù)字控制器的輸入?yún)?shù)，得到機器人當(dāng)前方向角與當(dāng)前位置和目標(biāo)點位置連線的夾角，即AS=arctan(^^S-《叉血—x加其中9n為機器人當(dāng)前的方向角。3、作震蕩檢測。在實踐中發(fā)現(xiàn)，機器人到達(dá)目標(biāo)點附近時，由于慣性等各方面因素的影響，機器人總是不能有效的停止在目標(biāo)點上，而是不停的來回震蕩，這無論是對機器人9的有效運動控制還是對機器人自身的物理器件都有較為嚴(yán)重的影響。因此可以保存機器人運動過程中前三拍的目標(biāo)點坐標(biāo)和機器人的位置坐標(biāo)，只要檢測到這三拍中有兩拍的數(shù)據(jù)完全相等，則可以斷定機器人當(dāng)前處在目標(biāo)點附近的震蕩狀態(tài)。當(dāng)檢測到機器人處于震蕩狀態(tài)時，可以改變機器人的運動策略，即將朝向目標(biāo)點的到定點運動改為指向目標(biāo)點的原地轉(zhuǎn)角運動，這樣就可以打破機器人的震蕩平衡，而使機器人下一拍可以有效地朝向目標(biāo)點運動。4、采用誤差比例控制算法，即通過到目標(biāo)點的距離dis來控制機器人的線速度V，而通過與目標(biāo)方向的夾角A9來控制機器人的角速度w，建立機器人的到定點運動模型VL=f(dis)+g(A9)VK=f(dis)-g(Ae)其中f(dis)是以距離dis為自變量的函數(shù)，g(ae)是以角度差ae為自變量的函數(shù)。5、在上述基于誤差比例控制方法建立的模型中，其中g(shù)(Ae)函數(shù)可以通過多次實驗，對機器人付以不同的輪速，得到機器人在不同輪速下一拍可以轉(zhuǎn)到的角度曲線ae，然后用擬合的方法，得到ae-lv」曲線g(ae)。f(dis)可以基于同樣的方法得到，同時考慮到小車的速度方向始終與其指向一致，只有距離在連線方向上的投影才是小車運動的有效距離，因此對得到的f(dis)函數(shù)稍作修改，即可得到改進(jìn)后的基于誤差比例控制方法的到定點運動模型VL=f(discos(A9))+g(A9);VK=f(discos(A9))-g(Ae)6、由于誤差比例控制方法沒有考慮到機器人運動過程中的路徑規(guī)劃，因此在短距離的跑定點運動中，該方法顯得過于簡單，會使機器人產(chǎn)生較為劇烈的震蕩現(xiàn)象。因此，以誤差比例控制方法產(chǎn)生的最大震蕩距離id、ib為分界線，當(dāng)dis>=disvib時采用上述所述的改進(jìn)后的誤差比例控制方法。當(dāng)dis<disvib時，建立如下的基于弦切法的到定點運動模型如圖6所示，由幾何關(guān)系可以得到如下等式<3foA=<formula>formulaseeoriginaldocumentpage10</formula><formula>formulaseeoriginaldocumentpage10</formula>2sin(A。又由步驟(一)中所建立的運動模型，可以得到<formula>formulaseeoriginaldocumentpage10</formula>由以上公式聯(lián)立即可以解得機器人的左右輪輪速分別為<formula>formulaseeoriginaldocumentpage10</formula><formula>formulaseeoriginaldocumentpage11</formula>所以只要指定機器人運動的線速度V，并確定機器人兩輪之間的距離L就可以由該控制方法得到機器人運動的左右輪輪速。該控制方法在機器人的運動過程中有了相應(yīng)的路徑規(guī)劃，因此在短距離內(nèi)可以使機器人的運動更加準(zhǔn)確，但由于雙輪差動式機器人不能同時滿足非完全約束和動力學(xué)約束的條件，是一個非完整系統(tǒng)，因而該控制方法會使機器人產(chǎn)生循環(huán)震蕩現(xiàn)象，這也就是為什么第三步要先進(jìn)行循環(huán)檢測的原因。7、由以上模型確定了機器人左右輪輪速、和^后，就可將該值作為數(shù)字控制器的輸出。步驟四、將數(shù)字控制器的輸出分別作用于機器人左右輪的伺服電機上，以驅(qū)動機器人左右輪作相應(yīng)的轉(zhuǎn)動，以達(dá)到期望的運動結(jié)果。權(quán)利要求一種雙輪差動式機器人運動控制方法，其特征在于以機器人的幾何中心為坐標(biāo)原點在世界坐標(biāo)系XOY中建立本體坐標(biāo)系xoy，轉(zhuǎn)角運動的控制步驟和跑位運動的控制步驟如下轉(zhuǎn)角運動的控制步驟步驟1計算機器人的當(dāng)前方向角和目標(biāo)方向角角度差Δθ＝|θd-θn|，其中θd為機器人的目標(biāo)方向角，θn為機器人的當(dāng)前方向角；步驟2計算左右輪速值<mfencedopen='{'close=''><mtable><mtr><mtd><msub><mi>V</mi><mi>L</mi></msub><mo>=</mo><mn>1</mn><mo>+</mo><mi>ln</mi><mi>a</mi><mo>·</mo><mi>Δθ</mi><mo>+</mo><mfrac><mrow><msup><mi>ln</mi><mn>2</mn></msup><mi>a</mi></mrow><mn>2</mn></mfrac><mi>Δ</mi><msup><mi>θ</mi><mn>2</mn></msup><mo>+</mo><mfrac><mrow><msup><mi>ln</mi><mn>3</mn></msup><mi>a</mi></mrow><mn>6</mn></mfrac><mi>Δ</mi><msup><mi>θ</mi><mn>3</mn></msup><mo>+</mo><mo>·</mo><mo>·</mo><mo>·</mo><mo>+</mo><mfrac><mrow><msup><mi>ln</mi><mi>n</mi></msup><mi>a</mi></mrow><mrow><mi>n</mi><mo>!</mo></mrow></mfrac><mi>Δ</mi><msup><mi>θ</mi><mi>n</mi></msup></mtd></mtr><mtr><mtd><msub><mi>V</mi><mi>R</mi></msub><mo>=</mo><mo>-</mo><mrow><mo>(</mo><mn>1</mn><mo>+</mo><mi>ln</mi><mi>a</mi><mo>·</mo><mi>Δθ</mi><mo>+</mo><mfrac><mrow><msup><mi>ln</mi><mn>2</mn></msup><mi>a</mi></mrow><mn>2</mn></mfrac><mi>Δ</mi><msup><mi>θ</mi><mn>2</mn></msup><mo>+</mo><mfrac><mrow><msup><mi>ln</mi><mn>3</mn></msup><mi>a</mi></mrow><mn>6</mn></mfrac><mi>Δ</mi><msup><mi>θ</mi><mn>3</mn></msup><mo>+</mo><mo>·</mo><mo>·</mo><mo>·</mo><mo>+</mo><mfrac><mrow><msup><mi>ln</mi><mi>n</mi></msup><mi>a</mi></mrow><mrow><mi>n</mi><mo>!</mo></mrow></mfrac><mi>Δ</mi><msup><mi>θ</mi><mi>n</mi></msup><mo>)</mo></mrow></mtd></mtr></mtable></mfenced>VL為左輪速值，VR為右輪速值，項數(shù)n和底數(shù)a的參數(shù)選擇策略如下當(dāng)Δθ的區(qū)間為時，n取為2、a取為1.8；當(dāng)Δθ的區(qū)間為(5，15]時，n取為3、a取為1.4；當(dāng)Δθ的區(qū)間為(15，30]時，n取為4、a取為1.21；當(dāng)Δθ的區(qū)間為(30，90]時，n取為5、a取為1.12；步驟3將步驟2得到的輪速分別作用于機器人左右輪的伺服電機上，驅(qū)動機器人左右輪作相應(yīng)的轉(zhuǎn)動；跑位運動的控制步驟步驟1計算機器人當(dāng)前位置坐標(biāo)與目標(biāo)點位置坐標(biāo)的距離<mrow><mi>dis</mi><mo>=</mo><msqrt><msup><mrow><mo>(</mo><msub><mi>x</mi><mi>sta</mi></msub><mo>-</mo><msub><mi>x</mi><mi>des</mi></msub><mo>)</mo></mrow><mn>2</mn></msup><mo>+</mo><msup><mrow><mo>(</mo><msub><mi>y</mi><mi>sta</mi></msub><mo>-</mo><msub><mi>y</mi><mi>des</mi></msub><mo>)</mo></mrow><mn>2</mn></msup></msqrt></mrow>其中xsta、ysta為機器人當(dāng)前的位置坐標(biāo)，xdes、ydes為目標(biāo)點的位置坐標(biāo)；步驟2計算機器人當(dāng)前方向角與當(dāng)前位置和目標(biāo)點位置連線的夾角<mrow><mi>Δθ</mi><mo>=</mo><mi>arctan</mi><mrow><mo>(</mo><mfrac><mrow><msub><mi>y</mi><mi>des</mi></msub><mo>-</mo><msub><mi>y</mi><mi>sta</mi></msub></mrow><mrow><msub><mi>x</mi><mi>des</mi></msub><mo>-</mo><msub><mi>x</mi><mi>sta</mi></msub></mrow></mfrac><mo>)</mo></mrow><mo>-</mo><msub><mi>θ</mi><mi>n</mi></msub></mrow>其中θn為機器人的當(dāng)前方向角；步驟3進(jìn)行跑位震蕩檢測比較機器人運動最近三個仿真節(jié)拍的目標(biāo)點坐標(biāo)和機器人的位置坐標(biāo)，當(dāng)兩拍的數(shù)據(jù)不相等時執(zhí)行下一步驟；當(dāng)兩拍的數(shù)據(jù)相等時，斷定機器人當(dāng)前處在目標(biāo)點附近的震蕩狀態(tài)，將機器人正面朝向與X軸正方向夾角設(shè)為θp，機器人目標(biāo)位置和當(dāng)前位置的連線與X軸正方向的夾角設(shè)為θq，則機器人執(zhí)行轉(zhuǎn)角運動控制步驟、做一個由θp轉(zhuǎn)向θq的原地轉(zhuǎn)角運動，然后執(zhí)行下一步驟；步驟4以誤差比例控制方法產(chǎn)生的最大震蕩距離disvib為分界線當(dāng)dis＞＝disvib時，左右輪速值為<mrow><mfencedopen='{'close=''><mtable><mtr><mtd><msub><mi>V</mi><mi>L</mi></msub><mo>=</mo><mi>f</mi><mrow><mo>(</mo><mi>dis</mi><mo>·</mo><mi>cos</mi><mrow><mo>(</mo><mi>Δθ</mi><mo>)</mo></mrow><mo>)</mo></mrow><mo>+</mo><mi>g</mi><mrow><mo>(</mo><mi>Δθ</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>V</mi><mi>R</mi></msub><mo>=</mo><mi>f</mi><mrow><mo>(</mo><mi>dis</mi><mo>·</mo><mi>cos</mi><mrow><mo>(</mo><mi>Δθ</mi><mo>)</mo></mrow><mo>)</mo></mrow><mo>-</mo><mi>g</mi><mrow><mo>(</mo><mi>Δθ</mi><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>,</mo></mrow>其中f(dis·cos(Δθ))和g(Δθ)采用擬合方法得到；當(dāng)dis＜disvib時，左右輪速值為<mrow><mfencedopen='{'close=''><mtable><mtr><mtd><msub><mi>V</mi><mi>L</mi></msub><mo>=</mo><mi>V</mi><mo>-</mo><mfrac><mrow><mi>L</mi><mo>·</mo><mi>V</mi></mrow><mrow><mn>2</mn><mi>R</mi></mrow></mfrac></mtd></mtr><mtr><mtd><msub><mi>V</mi><mi>R</mi></msub><mo>=</mo><mi>V</mi><mo>+</mo><mfrac><mrow><mi>L</mi><mo>·</mo><mi>V</mi></mrow><mrow><mn>2</mn><mi>R</mi></mrow></mfrac></mtd></mtr></mtable></mfenced><mo>,</mo></mrow>其中<mrow><mi>R</mi><mo>=</mo><mfrac><mi>dis</mi><mrow><mn>2</mn><mi>sin</mi><mrow><mo>(</mo><mi>Δθ</mi><mo>)</mo></mrow></mrow></mfrac><mo>,</mo></mrow>V的值取f(dis·cos(Δθ))，為指定的機器人運動的線速度，L是機器人兩輪之間的距離；所述的disvib為機器人的最大速度所對應(yīng)的運動距離為其值；所述的擬合方法為將若干組機器人運動距離dis、轉(zhuǎn)角量Δθ和運動速度v的關(guān)系dis-v和Δθ-v的數(shù)據(jù)，采用擬合方法得到如下公式f(dis·cos(Δθ))＝0.14284*(dis·cos(Δθ))3-0.66979*(dis·cos(Δθ))2+12.519*(dis·cos(Δθ))-0.23125g(Δθ)＝0.01178*(Δθ)2+0.1989*(Δθ)+1步驟5將步驟4得到的輪速分別作用于機器人左右輪的伺服電機上，驅(qū)動機器人左右輪作相應(yīng)的轉(zhuǎn)動。全文摘要本發(fā)明涉及一種雙輪差動式機器人運動控制方法，技術(shù)特征在于以機器人的幾何中心為坐標(biāo)原點在世界坐標(biāo)系XOY中建立本體坐標(biāo)系xoy；基于運動學(xué)的雙輪差動式機器人運動控制方法，該方法針對所有輪式機器人運動中最基本、最主要的兩個動作，即輪式機器人原地轉(zhuǎn)角動作和運動到定點動作，實現(xiàn)了對這兩個動作的運動控制。雙輪差動式機器人的所有復(fù)雜動作都可以由這兩個基本動作合成。本發(fā)明提出采用基于誤差比例控制法對其中的線速度函數(shù)作了合理的改進(jìn)，同時結(jié)合弦切法，對機器人運動到定點進(jìn)行綜合控制。綜合控制極大發(fā)揮了這兩種方法各自的優(yōu)點，避免了各自缺陷對機器人運動的影響。本發(fā)明中還針對循環(huán)震蕩現(xiàn)象，提出了其相應(yīng)的解決策略。文檔編號B25J13/00GK101791800SQ20101001364公開日2010年8月4日申請日期2010年1月21日優(yōu)先權(quán)日2010年1月21日發(fā)明者于竹君,史豪斌,張磊,李文彬,潘煒申請人:西北工業(yè)大學(xué)