.五邑大學(xué)畢業(yè)設(shè)計說明書畢業(yè)設(shè)計題目：六足機器人的運動分析及路徑規(guī)劃院 系 機電工程學(xué)院 專 業(yè) 機械工程及自動化 學(xué) 號 AP0808340 學(xué)生姓名 諸煥城 學(xué)生電話 13119689195 指導(dǎo)教師 李昌明 副教授 完成日期 2012年5月20日 .摘 要六足步行機器人機動性強，適應(yīng)能力高，能代替多種機器人完成工作，其研究具有重要的科學(xué)意義和實際應(yīng)用價值。本文針對六足步行機器人的機體設(shè)計、步態(tài)規(guī)劃、運動學(xué)分析、足端軌跡規(guī)劃中的空間插值方法及避障路徑規(guī)劃算法等理論和技術(shù)問題，開展了較為系統(tǒng)的研究工作。首先，對六足昆蟲進(jìn)行機械建模，確定選用橢圓形身體布局后，進(jìn)一步對六足步行機器人在三角形步態(tài)下的爬行穩(wěn)定性進(jìn)行詳細(xì)地分析；然后，求解機器人步行足運動學(xué)的正逆解問題，利用求解結(jié)果輔助規(guī)劃機器人的足端軌跡。MATLAB的分析仿真發(fā)現(xiàn)，在六次多項式函數(shù)的足端軌跡曲線下，步行足具有較好的運動特性；最后，先簡單介紹了人工勢場和蟻群算法，再合理地對兩種算法進(jìn)行了有效地融合與改進(jìn)，揚長避短，得到了一種更高效智能的路徑軌跡規(guī)劃算法。MATLAB的仿真實驗結(jié)果證明了該算法的有效性。關(guān)鍵詞： 六足步行機器人；步態(tài)規(guī)劃；運動學(xué)；軌跡規(guī)劃；人工勢場；蟻群算法AbstractDue to the great mobility and adaptability of hexapod walking robot, and their high performances in various robotic tasks, the research on it is of momentous scientific significance and practical application value. This thesis addresses body design, gait planning and kinematics analysis, polynomial interpolation method of foot trajectory planning, and obstacle path planning algorithm for hexapod walking robot. In order to solve these problems, a systematic study for the robots is presented. Firstly, the oval body configuration is chosen based on the structure and motion characteristic of insect, and then drive deeper into the stability of crawl locomotion under the tripod gait movement. Secondly, after solving forward and inverse kinematics of swinging leg, polynomial interpolation method is adopted to find a better curve of foot trajectory. MATLAB is used to do this simulation. The solution shows that swinging leg possesses the excellent kinetic characteristic under the six-order polynominal function curve. Finally, a brief description of artificial potential field method(PFM) and ant colony algorithm(ACO) exposes the imperfection of them. A new algorithm is proposed by combining PFM with ACO effectivelySimulation results testify the validity of this method for robot path planningKey words：Hexapod walking robot Gait planning Kinematics Trajectory planning Artificial potential field Ant colony algorithm目錄摘 要IAbstractII第1章 緒論11.1 課題的來源及研究的目的與意義11.2 文獻(xiàn)綜述11.2.1 國外仿生多足機器人研究概況11.2.2 國內(nèi)仿生多足機器人研究概況41.3 本課題研究的主要內(nèi)容51.4 本章小結(jié)5第2章 仿生六足機器人機構(gòu)建模62.1 仿生六足機器人機構(gòu)模型62.2 基于螺旋理論的機構(gòu)自由度分析62.3 機器人機體結(jié)構(gòu)82.4 本章小結(jié)9第3章 六足機器人靜態(tài)步態(tài)規(guī)劃分析103.1 步態(tài)的相關(guān)概念103.2 六足機器人的步態(tài)分析103.3 三角形步態(tài)113.3.1 三角形步態(tài)的穩(wěn)定性分析123.3.2 六足機器人的步長設(shè)計133.3.3 六足機器人著地點的優(yōu)化143.4 本章小結(jié)15第4章 六足機器人的運動學(xué)分析164.1 D-H變換164.2 步行足坐標(biāo)系的建立174.3 運動學(xué)正解174.4 運動學(xué)逆解184.5 基于微分變換法的雅可比矩陣204.6 本章小結(jié)20第5章 機器人的足端軌跡規(guī)劃225.1 步行足的擺動軌跡分析225.2 步行足的擺動軌跡生成225.3 足端軌跡仿真分析255.4 本章小結(jié)28第6章 六足機器人避障路徑軌跡規(guī)劃296.1 人工勢場法路徑規(guī)劃296.1.1 人工勢場法原理296.1.2 受力分析316.2 蟻群算法路徑規(guī)劃326.2.1 蟻群算法原理326.2.2 基本蟻群算法的數(shù)學(xué)模型336.3 勢場和蟻群算法結(jié)合與改進(jìn)356.3.1 啟發(fā)信息的構(gòu)造356.3.2 期望啟發(fā)式因子的改進(jìn)366.4 算法步驟366.5 基于勢場蟻群算法路徑規(guī)劃的仿真實現(xiàn)366.6 本章小結(jié)38結(jié) 論39參考文獻(xiàn)40致謝43附錄A 運動分析的相關(guān)程序441 計算兩組支撐三角形最大重疊面積442 轉(zhuǎn)換矩陣生成程序443 足端軌跡的生成和計算程序454 求運動逆解問題48附錄B 路徑規(guī)劃的相關(guān)程序491 路徑規(guī)劃的主程序492 計算引力、斥力與x軸的角度503 計算引力大小504 計算斥力大小515 計算合力在在八個可行方向上的分量526 計算由合力引起的啟發(fā)信息537 地圖生成程序568 勢場蟻群算法程序57第1章 緒論1.1 課題的來源及研究的目的與意義機器人自問世以來，伴隨著電子計算機的發(fā)展，整合多科學(xué)領(lǐng)域里的新成果，已經(jīng)成為一種現(xiàn)代科學(xué)技術(shù)的典型產(chǎn)物,在工業(yè)、農(nóng)業(yè)、娛樂、軍事等行業(yè)中均扮演著舉足輕重的角色。當(dāng)今，隨著科學(xué)迅猛發(fā)展，人類探索研究范圍逐漸擴展到一些人類無法到達(dá)或可能危及生命的特殊場合，例如外星球表面、核反應(yīng)堆、戰(zhàn)場、消防及營救等。面對這艱難的挑戰(zhàn)，尋求一條解決問題的可行途徑已是科學(xué)技術(shù)發(fā)展和人類社會進(jìn)步的迫在眉睫的任務(wù)。地形不規(guī)則或難以預(yù)測是這些環(huán)境的共同特點。從而使輪式機器人和履帶式機器人的應(yīng)用受到一定的限制1。以往的研究表明輪式機器人的結(jié)構(gòu)相對也較簡單，在相對平坦的地形上行駛時具有運動速度迅速、平穩(wěn)和控制簡單的優(yōu)點。但是由于輪式機器人運動時需要連續(xù)的地面支撐，在不平坦或松軟的地形上行駛時，能耗大大增加，運動性能也極速降低有時甚至完全喪失移動能力。履帶式機器人雖然支撐面積大，牽引附著性能優(yōu)越，在松軟或泥濘的場地行駛時的適應(yīng)性比輪式有較大的改善，但是在不平地面上行駛時機身晃動嚴(yán)重2，機動性仍然差強人意。而多足步行機器人可以利用獨立的地面支撐，即使是一系列孤立的點也可以成為它的運動軌跡，所以，與輪式、履帶式移動機器人相比，多足步行機器人面對復(fù)雜的非結(jié)構(gòu)環(huán)境時適應(yīng)性強和靈活性高，可以代替人類完成很多危險的作業(yè)，具有廣泛的應(yīng)用前景。近年來，得益于仿生科學(xué)的進(jìn)步，仿生多足步行機器人如雨后春筍般快速地發(fā)展起來，現(xiàn)在已經(jīng)成為當(dāng)前各國科學(xué)家開發(fā)研究的重點課題之一。1.2 文獻(xiàn)綜述1.2.1 國外仿生多足機器人研究概況自從第一次工業(yè)革命以后，隨著機械學(xué)的不斷成熟，機器人逐漸地從人類的幻想進(jìn)入實現(xiàn)生活中。國外有據(jù)可查的記載是1893年Rygg設(shè)計的機械馬。此后步行機器人歷經(jīng)了一百多年的發(fā)展，取得了長足的進(jìn)步3。在20世紀(jì)60年代，美國的Shigley和Baldwin都使用的凸輪連桿機構(gòu)設(shè)計出比輪式車和履帶式車更為靈活的四足步行機“ walking Truck”，如圖1-1所示，被視為現(xiàn)代多足步行機器人發(fā)展史上的一個里程碑1。但由于受到當(dāng)時技術(shù)水平及控制技術(shù)的限制，步行機不僅效率低適應(yīng)性差，而且依賴于人的手腳對液壓伺服系統(tǒng)的控制來操縱整個步行機的運動。從步態(tài)規(guī)劃的角度看，這種步行機只能算是人操作的機械移動裝置。圖1-1 四足機器人“Walking Truck”圖1-2 六足機器人“ODEX- I”在上世紀(jì)70年代末以來，隨著電子技術(shù)及計算機技術(shù)的快速發(fā)展，多足步行機器人超越了單純依靠機構(gòu)控制模式而實現(xiàn)了基于電子計算機技術(shù)的控制。其中以美國的McGhee與Frank研制的四足步行機器人“Phony Pony” 最具典型和開創(chuàng)性。該機器人具有較好的步態(tài)運動穩(wěn)定性，但由于其關(guān)節(jié)是由邏輯電路組成的狀態(tài)機控制的，因此機器人的行為受到限制，只能呈現(xiàn)固定的運動形式。隨后他們又研制出具有自主避障功能的六足步行機器人“OSU”，這種機器人每條腿具有三個自由度，靈活性高。此外，具有代表性的機器人還包括1983年美國研制的六足步行機器人“ODEX-I” ，如圖1-2所示，其6條腿沿圓周方向布置，每條腿上有三個自由度，適于在狹小空間運動，可以上下臺階。1990年卡內(nèi)基美隆大學(xué)的Whittaker等人研制的用于外星探測的六足步行機器人“AMBLER”， 圖1-3, 該機器人機構(gòu)的特點是軀體分成兩部分，腿可從中間穿過，從而使得后部的腿可邁到前部，地面適應(yīng)能力增強。1993年該大學(xué)又開發(fā)出用于火山考察的八足步行機器人“DANTE，圖1-4是其改進(jìn)型“DANTE-II”，在對斯伯火山的考察中得到了實際應(yīng)用，傳回了許多珍貴的數(shù)據(jù)和圖像4。圖1-3 六足機器人“AMBLER”圖1-4 八足機器人“DANTE-II”從美國引進(jìn)機器人技術(shù)后，日本很快成為這方面的強國。自20世紀(jì)80年代開始，日本東京工業(yè)大學(xué)的Shigeo Hirose教授領(lǐng)導(dǎo)的實驗室，成功地開發(fā)出8代的TITAN系列四足步行機器人。其中，“TITAN-III”的足由形狀記憶合金組成，裝有傳感器和信號處理系統(tǒng)，可以自動檢測與地面接觸的狀態(tài)。姿態(tài)傳感器和姿態(tài)控制系統(tǒng)根據(jù)傳感信息作出控制決策，實現(xiàn)在不平地面的自適應(yīng)靜態(tài)步行。1994年研制的“TITAN-VII”作為移動的平臺，能夠靈活自由地在崎嶇和陡峭的地方步行。圖1-5是最新研制的四足機器人“TITAN-VIII”5，它具有高度的地面自適應(yīng)能力，“TITAN-VIII”的腿能夠成為有力的工作臂，用于探測地雷和進(jìn)行排雷操作。圖1-5 四足機器人“TITAN-VIII”自本世紀(jì)以來，仿生學(xué)、材料學(xué)及計算機信息學(xué)等學(xué)科的發(fā)展和交叉融合，把多足步行機器人的研究推向新的高潮。這一時期，具有多功能性和自主性對機器人技術(shù)進(jìn)入到新的發(fā)展階段。圖1-6所示的是2002年印度研制了六足行走式機器人“舞王”6，其基座為一六角形的底盤，裝有6 條長腿，由18個軸控制，分別安裝在底盤的6 個角上，在18臺電子發(fā)動機的帶動下，每條腿都轉(zhuǎn)動，形似一個巨大的蜘蛛。此外，“舞王”的基座上還裝有用于控制和監(jiān)視的電腦。據(jù)報道稱，“舞王”在一臺無線臺式電腦的遙控下，不僅可以行走、轉(zhuǎn)彎，還可以爬梯子、翻越45厘米高的障礙物。又如美國斯坦福大學(xué)于2006年研發(fā)的仿壁虎機器人“stickybot”，如圖1-7。該機器人從吸附原理、運動形式、結(jié)構(gòu)外形上都比較接近真實的壁虎，可以輕松地在墻避上爬行。圖1-6 六足機器人“舞王”圖1-7 仿壁虎機器人“stickybot”1.2.2 國內(nèi)仿生多足機器人研究概況 我國對多足步行機器人的研究是從20世紀(jì)80年代開始的，經(jīng)過三十多年的努力，在研究和應(yīng)用方面己經(jīng)取得比較好的成果。中國科學(xué)院長春光學(xué)精密機械研究所、中國科學(xué)院沈陽自動化研究所、清華大學(xué)、北京航空航天大學(xué)、上海交通大學(xué)等單位和院校都先后開展了對多足步行機器人技術(shù)的研究。其中比較有代表性的有上海交通大學(xué)研制的小型六足仿生機器人，長30mm、寬40mm、高20mm，質(zhì)量6.3g，步行速度達(dá)到3mm/s8。該學(xué)校還研制了一種仿哺乳動物的關(guān)節(jié)式 “JTUWM”系列四足步行機器人，它能以對角線步態(tài)行走。其足底安裝了PVDF測力傳感器，上位機利用模糊神經(jīng)網(wǎng)絡(luò)系統(tǒng)對力反饋信息進(jìn)行處理，調(diào)整步行參數(shù)，提高了步行的穩(wěn)定性。2000年，他們雙開發(fā)了一種形狀記憶合金(SMA)驅(qū)動的微型六足步行機器人，如圖1-8所示，后來對SMA驅(qū)動的微型六足步行機器人進(jìn)行了改進(jìn)，創(chuàng)新設(shè)計了雙三足機器人的身體轉(zhuǎn)動關(guān)節(jié)，并首次提出組合偏動SMA驅(qū)動器，使機器人的剛性軀體柔性化從而實現(xiàn)了微型雙三足步行機器人的全方位運動。此外，還有中國科學(xué)院沈陽自動化所成功研制了水下全方位六足步行機器人LR-1；清華大學(xué)開發(fā)了“DTWM”框架式雙三足步行機器人、五足爬桿機器人；哈爾濱工程大學(xué)研制開發(fā)的仿生機器蟹；中國科學(xué)院長春光學(xué)精密機械研究所研制了“4+2“多足步行機器人和圖1-9所示的MiniQuad可重構(gòu)多足步行機器人9等。與國外相比，我國的步行機器人技術(shù)的研究進(jìn)展緩慢，各方面技術(shù)還不成熟，大多數(shù)研究開發(fā)工作只停留在實驗室中。圖1-8 SMA六足微型機器人圖1-9 MiniQuad可重構(gòu)多足機器人1.3 本課題研究的主要內(nèi)容本文以六足步行機器人為研究對象，融合仿生技術(shù)、機械原理、運動控制、建模仿真等設(shè)計方法對仿生六足機器人的機構(gòu)特點和功能進(jìn)行針對性的設(shè)計和分析。本文主要結(jié)構(gòu)如下：（1） 緒論，簡述多足步行機器人研究意義和國內(nèi)外發(fā)展現(xiàn)狀。（2） 六足步行機器人自由度計算和機構(gòu)設(shè)計。（3） 六足步行機器人的步態(tài)規(guī)劃，采用三角形步態(tài)，并對該步態(tài)的穩(wěn)定性和著地方點進(jìn)行分析和優(yōu)化。（4） 六足步行機器人的運動學(xué)分析，介紹機器人與參考坐標(biāo)系之間的坐標(biāo)變換關(guān)系，腿的運動學(xué)以及機器人的正、逆運動學(xué)求解模型。（5） 對六足步行機器人的足端運動軌跡進(jìn)行設(shè)計，并利用MATLAB進(jìn)行仿真，力求運動平滑，各關(guān)節(jié)的運動無突變無沖擊。（6） 設(shè)計機器人避障的算法，使能使機器人成功避開障礙物，并能找出機器人從始點到目標(biāo)點的最佳爬行路徑。最后，用MATLAB軟件對算法進(jìn)行仿真。1.4 本章小結(jié)本章通過查閱相關(guān)文獻(xiàn)和技術(shù)資料，概述了多足步行機器人的研究意義和國內(nèi)外發(fā)展現(xiàn)狀，明確了本課題的研究內(nèi)容以及選題意義。第2章 仿生六足機器人機構(gòu)建模機構(gòu)設(shè)計是仿生多足機器人系統(tǒng)設(shè)計和分析的基礎(chǔ)，整機機械結(jié)構(gòu)、自由度數(shù)和驅(qū)動方式等都會直接影響到機器人的運動和動力性能。仿生多足步行機器人的機構(gòu)由于軀體和腿兩部分組成，腿的數(shù)量及其配置是整體設(shè)計的主要問題?，F(xiàn)有多足步行機器人的足數(shù)包括三足、四足、六足、八足甚至更多，足的數(shù)目較多的適合重載和慢速運動，而足數(shù)較少的運動靈活性相對較高。腿的配置是指步行機器人的足相對機體的位置和方位的布置，腿的配置合理與否直接影響到機器人的整體性能。2.1 仿生六足機器人機構(gòu)模型生活中，如果用心觀察大量的“六足綱”昆蟲（蟑螂，螞蟻等），我們?nèi)菀装l(fā)現(xiàn)昆蟲的六足成對稱分布，每條腿有4個關(guān)節(jié)，分別為根關(guān)節(jié)、髖關(guān)節(jié)、膝關(guān)節(jié)和踝關(guān)節(jié)，其中前3個關(guān)節(jié)為驅(qū)動關(guān)節(jié)，各關(guān)節(jié)之間的連桿分別稱為基節(jié)、股節(jié)和脛節(jié)，如圖2-1所示。本設(shè)計基于六足昆蟲的外形，經(jīng)過合理的簡化，把六足步行機器人設(shè)計成如圖2-2所示的模型。圖2-1 昆蟲腿部結(jié)構(gòu)示意圖圖2-2 六足機器人模型2.2 基于螺旋理論的機構(gòu)自由度分析螺旋（s；s0）由兩個矢量對偶組成，也可用Plcker坐標(biāo)表示為（L，M，N；P，Q，R），（L，M，N）和（P，Q，R）分別代表了矢量s和矢量s0。這里，s代表空間一個矢量，稱為對偶矢量原部表示了該矢量在空間的位置，其中稱為對偶矢量的對偶部，是由原點至該空間矢量任一點的矢徑，稱為螺旋的節(jié)距11，是原點不變量。當(dāng)h=0時，螺旋退化為線矢量，當(dāng)h=時，螺旋退化為自由矢量，也稱為偶量，其在空間只有方向而沒有位置的概念。機構(gòu)的所有運動副都可以用移動副和轉(zhuǎn)動副單獨表示或者組合而成，而移動副和轉(zhuǎn)動副則可分別用移動速度偶量和角速度線矢量來表示。如果兩螺旋和r滿足r=0 時，則稱和r互為反螺旋。式中“”表示互易積。對于運動螺旋來說，其反螺旋表示機械系統(tǒng)對物體的約束。仿生六足機器人在正常行走條件下，假設(shè)各支撐腿與地面之間不存在滑動現(xiàn)象，因此可以簡化為點接觸，即相當(dāng)于機構(gòu)學(xué)上的3自由度球面副。另外，根關(guān)節(jié)、髖關(guān)節(jié)及膝關(guān)節(jié)都相當(dāng)于單自由度轉(zhuǎn)動副。因此，整個機構(gòu)的自由度可由下面的公式11計算：（2-1）這里，M表示機構(gòu)的自由度；d表示機構(gòu)的階數(shù)，它依賴于公共約束，d=6；表示公共約束；n表示包括機架的構(gòu)件數(shù)目；g表示運動副的數(shù)目；fi表示第i個運動副的自由度；v表示并聯(lián)冗余約束，它等于在去除公共約束的因素后的獨立冗余約束的數(shù)目。假設(shè)六足步行機器人某一時刻處于支撐相的腿數(shù)為n(n6) ，則此時模型為具有 n 個分支的空間多環(huán)并聯(lián)機構(gòu)（n-SRRR型）。在分析整機體的自由度時，取出一個分支，建立分支坐標(biāo)系o-xyz。如圖2-3。以此坐標(biāo)系可以建立該分支的6個螺旋的表達(dá)式如下：1=（1 0 0 ； 0 0 0）2=（0 1 0 ； 0 0 0）3=（0 0 1 ； 0 0 0）4=（0 0 1 ； P4 Q4 0）5=（0 0 1 ； P5 Q5 0） 6=（0 1 0 ； P6 0 R6）其中，Pi，Qi，Ri是與運動副線軸線位置有關(guān)的變量，其大小與問題本身無關(guān)。圖2-3 分支螺旋系易知，該螺旋系的序為6，該螺旋系不存在反螺旋。加上機構(gòu)中的6個相互獨立的SRRR分支完全相同，所以機構(gòu)不存在公共約束，其階數(shù)d=6；并聯(lián)冗余約束v=0。將以上參數(shù)代入（2-1）式，得由此可知，仿生六足機器人整個機構(gòu)是具有 6 自由度的空間多環(huán)并聯(lián)機構(gòu)，無論其采取的步態(tài)及地面狀況如何，軀體在一定范圍內(nèi)均可靈活地實現(xiàn)任意的位置和姿態(tài)。2.3 機器人機體結(jié)構(gòu)通過對自然界六足動物的細(xì)心觀察，發(fā)現(xiàn)常見的昆蟲的腿的分布情況往往如圖2-4所示，足部大多落在畫出的橢圓上。圖2-4 昆蟲足部位置示意圖一般研究通常把機體設(shè)計成長方形。但現(xiàn)實生活中大多數(shù)的昆蟲自體并不是長方形，而是呈橢圓形，后者運動性能往往比前者好。圖2-5為等長寬比、不同機體的髖關(guān)節(jié)擺動范圍及支撐三角形，其中箭頭表示重心和移動方向，陰影部分表示髖關(guān)節(jié)可達(dá)到的轉(zhuǎn)角范圍。為了行走的穩(wěn)定起見，今生六足機器人在行走過程中，機體重心的投影必需在三條支撐點所構(gòu)成的三角形區(qū)域內(nèi)。圖2-6是支撐三角形對比，DE與DE為表征機器人運動穩(wěn)定性的重要指標(biāo)，SL縱向穩(wěn)定裕量?？梢钥闯觯瑱E圓形布局的機體的步行機器人具有兩方面明顯的優(yōu)勢：一是減少了腿的碰撞，增大了髖關(guān)節(jié)轉(zhuǎn)動范圍，提高了運動靈活性；二是增大了機體的縱向穩(wěn)定裕量，提高了機器人的運動穩(wěn)定性。因此，本研究中的仿生蟑螂機器人采用橢圓型的身體結(jié)構(gòu)。a 長方形機體b 橢圓機體圖2-6 支撐三角形對比圖2-5 兩種機體髖關(guān)節(jié)擺動范圍及支撐三角形2.4 本章小結(jié)本章通過模仿六足昆蟲，得到六足機器人的機構(gòu)模型，隨后用螺旋理論對設(shè)計出的機構(gòu)的自由度進(jìn)行核算。最后，通過比較長方形機體和橢圓機體，得出橢圓機體的優(yōu)勢性。第3章 六足機器人靜態(tài)步態(tài)規(guī)劃分析步態(tài)是步行機器人各條腿間協(xié)調(diào)運行的規(guī)律，步態(tài)的穩(wěn)定性和可操作性是實現(xiàn)機器人穩(wěn)定行走的前提，因此步態(tài)分析是研究機器人的基礎(chǔ)。作為影響仿人機器人技術(shù)進(jìn)步的重要課題之一，步態(tài)規(guī)劃是成功和有效的實現(xiàn)多足穩(wěn)定步行的理論基礎(chǔ)和關(guān)鍵技術(shù)。3.1 步態(tài)的相關(guān)概念步態(tài)是指在運動過程中，人、動物或者機器的肢體在時間和空間上的一種協(xié)調(diào)關(guān)系，是腿有規(guī)律的按重復(fù)順序與方式移動，即機器人的每條腿按一定的順序和軌跡的運動過程，由此實現(xiàn)了移動機器人或是足式機器人的步行運動。它包括各條腿的抬腿和放腿的順序，還包括了機器人占空系數(shù)分析、足端的軌跡選擇等。支撐相是指步行機器人腿部著地的狀態(tài)。擺動相是指步行機器人腿部懸空的狀態(tài)。步態(tài)周期是指足式機器人完成一個步態(tài)所需要的時間，即機器人所有腿輪流完成一次“提起，擺動，放下”的動作所耗費的時間，就是指從某一條腿開始擺動到了下一次使用同一條腿時開始擺動所用的時間。步長S是指絕對坐標(biāo)系中重心在每個周期前進(jìn)的距離，即機器人的腿從當(dāng)前的支撐點到下一個步態(tài)的支撐點間的距離。規(guī)則步態(tài)是指機器人的腿按固定的順序與軌跡所進(jìn)行步行的過程。這種步態(tài)使得機器人只可在平坦的路面上行走。非規(guī)則步態(tài)是指機器人腿的擺動的順序和足端擺動的軌跡不固定，機器人可以依據(jù)行走環(huán)境的變化，來改變各條腿擺動的順序和足端的運動軌跡。因此，理想的非規(guī)則步態(tài)可稱作自適應(yīng)步態(tài)或者智能步態(tài)。占空系數(shù)是指一個周期中處于支撐相的時間與步態(tài)周期的比值。支撐多邊形是指支撐足著地點用凸形輪廓所以構(gòu)成的凸多邊形在水平面上的投影。靜態(tài)步行是指步行機器人合成重心始終保持在水平面上的垂直投影點處于支撐多邊形內(nèi)的行走方式。動態(tài)步行是一直保持著動態(tài)性的平衡的行走。一邊以一條腿或兩條腿著地，一邊移動，因此需通過對重心的控制，使得重心與慣性力的合力的向量通過腿的支撐點或者連接兩個腿的支撐點的線段上。以上介紹的是在機器人步態(tài)研究過程中本文需用到的相關(guān)定義12。3.2 六足機器人的步態(tài)分析為了使六足步行機器人步態(tài)設(shè)計實現(xiàn)較理想的靜態(tài)步行，需要考慮一些必要的要求，如行走時平穩(wěn)協(xié)調(diào)，進(jìn)退自如，無左右搖擺，機體保持與地面基本平行，沒有較為明顯的上下波動等。足式步行機器人在運動過程中，各條腿交替的呈現(xiàn)出兩種不同的狀態(tài)，即支撐相和擺動相。根據(jù)上文介紹的占空系數(shù)13的定義，見公式(3-1)所示。（3-1）式中，i為足式步行機器人的某一條腿 ( i=1,2,3 ) 。取值在 01 之間。若所有腿都為一個相同的占空系數(shù)時，此步態(tài)即為規(guī)則步態(tài)。對于本文研究的六足步行機器人而言，按照其占空系數(shù)的不同大概可分以下幾種情況。（1） 當(dāng)占空系數(shù)0<<1/2 時，六足機器人任何時刻只有不足三條腿處于支撐相，此時六足步行機器人處于小跑或跳躍運動狀態(tài)。（2） 當(dāng)占空系數(shù) =1/2 時，六足機器人任何時刻都有三條腿處于支撐相，各條腿處于支撐相或擺動相的時間相同，具有較高的步行效率。（3） 當(dāng)占空系數(shù) 1/2<<2/3 時，六足機器人有三條或四條腿處于支撐相，各條腿處于支撐相或擺動相的時間不一定相同。由于這種步態(tài)相對緩慢，機器人可以承受較大的載荷。（4） 當(dāng)占空系數(shù) =2/3 時，六足機器人任何時刻都有四條腿處于支撐相，各條腿處于支撐相或擺動相的時間相同。（5） 當(dāng)占空系數(shù) 2/3<<5/6 時，六足機器人有四條或五條腿處于支撐相，各條腿處于支撐相或擺動相的時間不一定相同。（6） 當(dāng)占空系數(shù) =5/6 時，六足機器人任何時刻都有五條腿處于支撐相，各條腿處于支撐相或擺動相的時間相同。（7） 當(dāng)占空系數(shù) 5/6<<1 時，六足機器人有五條或六條腿處于支撐相，各條腿處于支撐相或擺動相的時間不一定相同。此時機器人呈現(xiàn)一種波動步態(tài)，這種步態(tài)行走速度最慢，但運動平穩(wěn)性最高。3.3 三角形步態(tài)本文所研究的六足步行機器人的移動方式為靜態(tài)步行，采用三角步態(tài)，自始至終有三條腿著地行走，即占空系數(shù) =1/2。這種行走方式的特點是：在行走的過程中把六條腿分為兩組，以身體一側(cè)的前足、后足與另一側(cè)的中足為一組，剩余的三條腿為另一組。每一組的動作一致，或處于擺動相或處于支撐相。由于兩組足交替地擺動和支撐，在每個時刻總有三條腿著地，保持身體的平衡，所以這種步態(tài)的穩(wěn)定裕度大。腿的每一個支撐過程使身體前進(jìn)一個步長。如圖3-1所示，大腿關(guān)節(jié)在平坦無阻的地面上運動。此時，黑色的腳為擺動腳，白色的腳原地不動，只是支撐身體向前。圖3-1 三角步態(tài)示意圖3.3.1 三角形步態(tài)的穩(wěn)定性分析在機器人技術(shù)領(lǐng)域，靜態(tài)步態(tài)規(guī)劃有別于動態(tài)規(guī)劃，其區(qū)別在于靜態(tài)步態(tài)規(guī)劃不考慮慣性力的影響，任何時刻都能保持靜態(tài)穩(wěn)定。圖3-2所示的ABC為機器人的支撐多邊形，當(dāng)機器人的重心垂直投影位于此區(qū)域時，我們就說機器人是靜態(tài)穩(wěn)定的，否則就說機器人處于不穩(wěn)定狀態(tài)。取機器人的重心垂直投影點O為坐標(biāo)原點，d1、d2、d3為機器人重心投影點O到支撐三角形ABC各邊的距離。設(shè)三點的水平面坐標(biāo)為則AB的直線方程為（3-2）所以原點到直線AB的距離d3為（3-3）同理可以求得d2、d3。則六足機器人以三角形步態(tài)行走時，其最小穩(wěn)定裕量判據(jù)為：（3-4）圖3-2 穩(wěn)定裕度計算圖3.3.2 六足機器人的步長設(shè)計六足機器人腿的初始姿態(tài)如圖3-3的實線所示，這時設(shè)根關(guān)節(jié)、髖關(guān)節(jié)、膝關(guān)節(jié)的角度分別為。于是有：（3-5）其中l(wèi)1、l2、l3分別基節(jié)、股節(jié)、脛節(jié)的的長度，L0、H0分別是初始姿態(tài)時機器人腿的伸展量和機體的重心高度。機器人最大伸展量如圖3-3中的虛線所示，可有：（3-6）圖3-4是機器人某只腿在向前邁進(jìn)時在水平面的投影，由圖可以求得機器人的允許的最大步長的大小。（3-7）圖3-3 腿的最大伸展量圖3-4 機器人的最大步長由圖3-2可知，為了保證六足機器人在步行過程中不失穩(wěn)，重心應(yīng)保持在支撐三角形內(nèi)，則須滿足S/2min(OE，OF) ，而OE、OF主要由機器人的結(jié)構(gòu)布局所決定。因此，機器人不失穩(wěn)時的步長必須滿足S2min(OE，OF)。3.3.3 六足機器人著地點的優(yōu)化為了保證機器人運動時，擺動腿和支撐腿選擇或轉(zhuǎn)變的靈活性和穩(wěn)定性，我們力求機器人的兩組腿形成的支撐三角形的重合面積最大，即圖3-5中多邊形defgij的面積最大。這樣，只要機器人的重心在重疊區(qū)域內(nèi)，機器人可以選擇任意一組腿擺動。重合面積越大，機器人的重心的活動范圍越大。圖3-5 機器人支撐點的分布作平面坐標(biāo)系如圖所示。為了分析方便，設(shè)兩個三角形全等且為等腰三角形；三角形在x軸方向的高為h；直線BA的斜率k，則直線AB的斜率為(-k)；點B到線AC的距離或點B到線AC的距離為a；點B到點B在y軸方向上的距離為b。由于多邊形defgij具有對稱性，要求其的面積最大，只需求四邊形edji的面積最大。易求得直線BA的表達(dá)式：（3-8）直線AB的表達(dá)式：（3-9）根據(jù)式（3-8）和式（3-9）可求出點e、d、j、i的坐標(biāo)值：（3-10）用MATLAB中的polyarea命令求出四邊形edji的面積，并用其它命令求出最大值（程序見附錄A-1）。結(jié)果為：當(dāng)a=h/3時，四邊形edji的面積取得最大，即（3-11）由式（3-11）可看出，為了提高兩三角形的重合度，可增大側(cè)面兩個角（B和B）的大小，和橫向的寬度（h的值），但增量要適當(dāng)，否則可能增大機器人的重量或?qū)е赂鱾€方向的穩(wěn)定裕度相差懸殊。從式（3-11）還可以看出，步行機器人的步長（b的值）越大，兩三角形的重疊面積越小，因此，為了提高步行機器人的穩(wěn)定性和腳步轉(zhuǎn)換的靈活性，可適當(dāng)減小步長。3.4 本章小結(jié)本章節(jié)首先簡單介紹機器人步態(tài)分析的相關(guān)概念，然后結(jié)合占空比對六足機器人的各種步態(tài)進(jìn)行了合理的分析，對采用三角形步態(tài)的六足機器人的穩(wěn)定性的進(jìn)行了計算，并確定了在穩(wěn)定條件下的六足機器人的步長的范圍。最后，對六足機器人的著地點進(jìn)行了優(yōu)化，計算出了機器人步行時的最佳的落地點。第4章 六足機器人的運動學(xué)分析機器人運動學(xué)研究的目的是要建立機器人各個運動構(gòu)件秘末端執(zhí)行器空間位置之間的關(guān)系，進(jìn)而建立各構(gòu)件運動的數(shù)學(xué)模型，并在此基礎(chǔ)上，首先確定各構(gòu)件的位置和軌跡，以決定機構(gòu)的行程或外形尺寸，然后確定各構(gòu)件的速度、加速度等運動參數(shù)，以便評價機器人的運動學(xué)和動力學(xué)性能，最終通過運動學(xué)分析，對機構(gòu)做出評判、決策和改進(jìn)。4.1 D-H變換為描述相鄰桿件間平移和轉(zhuǎn)動的關(guān)系，Denavit和Hartenberg提出了一種為關(guān)節(jié)鏈中的每一桿件建立附體坐標(biāo)系的矩陣方法。D-H方法是為每個關(guān)節(jié)處的桿件坐標(biāo)系建立44齊次變換矩陣，表示它與前一桿件坐標(biāo)系的關(guān)系, 其原理14如下：與固定坐標(biāo)相連的固定參考坐標(biāo)系, 稱為基坐標(biāo)系。：與機器人的第i 個桿件相固連, 坐標(biāo)原點在第i+ 1關(guān)節(jié)的中心點處。確定和建立每個坐標(biāo)系遵循以下三條規(guī)則:（1） 軸沿著第i 關(guān)節(jié)的運動軸；（2） 軸垂直于zi-1軸及zi 軸并指向離開zi-1軸的方向；（3） 軸按右手坐標(biāo)系的要求建立。按照上述規(guī)定，剛性連桿參數(shù)可以定義如下： = 繞zi軸，從xi-1旋轉(zhuǎn)到xi的角度； = 沿zi軸，從xi-1移動到xi的距離； = 沿xi軸，從zi移動到zi+1的距離； = 繞xi軸，從zi旋轉(zhuǎn)到zi+1的角度。圖4-1 相鄰連桿之間連接關(guān)系圖4.2 步行足坐標(biāo)系的建立足式機器人的每個步行足可以看作一個串聯(lián)的開式運動鏈，各關(guān)節(jié)的連桿通過轉(zhuǎn)動副串聯(lián)連接在一起。為了研究機器人的腿的運動，可以建立圖4-2所示的D-H剛體坐標(biāo)系。其連桿D-H參數(shù)及關(guān)節(jié)變量如表4-1所示。表4-1 單腿的連桿參數(shù)表iidiai-1i-111000220l1/2330l20400l30圖4-2 單足的運動參數(shù)和坐標(biāo)系分布4.3 運動學(xué)正解所謂運動學(xué)正解, 就是對于一機器人, 給定桿件的幾何參數(shù)和關(guān)節(jié)的位移, 求解末端連桿坐標(biāo)系相對于基坐標(biāo)系的位姿。為求解運動學(xué)方程式, 我們用齊次變換矩陣i-1iT來描述第i 坐標(biāo)系相對于(i -1) 坐標(biāo)系的位置和方位，記作19：（4-1）式中c是cos的簡寫, s是sin的簡寫等等（下文同理）。則第i 坐標(biāo)系相對于基坐標(biāo)系的變換矩陣19為（4-2）分別將表4-1中的各關(guān)節(jié)連桿參數(shù)代入上式得：（4-3）（4-4）（4-5）（4-6）將上面的式（4-3）至（4-6）代入式（4-2）得：（4-7）因為已知機器人的足末端A在坐標(biāo)系4中的位置矢量4PA=px4 py4 pz4 1T =0 0 0 1T，所以可根據(jù)變換公式求得足末端A在基坐標(biāo)系0中的位置矢量為：（4-8）4.4 運動學(xué)逆解機器人運動學(xué)逆解就是已知末端連桿的位置和方位， 求得機器人的各個關(guān)節(jié)變量。運動學(xué)逆解在機器人運動分析、運動仿真、離線編程和軌跡規(guī)劃中占有重要地位。在機器人控制中，只有使各關(guān)切嚴(yán)格按照逆解中求得的數(shù)值量進(jìn)行移動或轉(zhuǎn)動，才能使末端執(zhí)行器達(dá)到工作所要求的位置和姿態(tài)，所以運動學(xué)逆解是機器人控制的關(guān)鍵環(huán)節(jié)之一。根據(jù)式（4-8）得：（4-9）由方程組（4-9）計算（2）/（1）并解得：（4-10）令;后可將方程組（4-9）變形成下面形式：（4-11）由方程組（4-11）計算（1）+（2）+（3）并解得（4-12）3取負(fù)值才符合實際情況。由于用代數(shù)解法難以解出超越方程中2的解，因此，下面用幾何法直觀求解2。圖4-3 連桿l1、l2平面幾何關(guān)系從圖4-3可以看出，即（4-13）4.5 基于微分變換法的雅可比矩陣操作空間速度和關(guān)節(jié)空間速度之間的關(guān)系可用下式15表示：（4-14）式中，稱為末端在操作空間的廣義速度，簡稱操作速度；，稱為關(guān)節(jié)速度；J(q)是6n的偏導(dǎo)數(shù)矩陣，稱為機器人的雅可比矩陣。機器人的雅可比矩陣J(q)表示末端的操作速度與關(guān)節(jié)速度的線性變換，也可以看成是兩者之間的傳動比。它的行數(shù)等于機器人操作空間的維數(shù)，而列數(shù)等于機器人的關(guān)節(jié)數(shù)。六足機器人中端的雅可比矩陣是一個63的方陣。雅克比矩陣的求法可以分為矢量積法和微分變換法，在這里我們采用微分變換法。在微分變換法中，機器人的雅可比矩陣的第 i 列元素由決定。對只有轉(zhuǎn)動關(guān)節(jié)的機器人來說，有：（4-15）式中，n、o 、a和 p 是 的四個列向量。通過這些列向量，就可以按照構(gòu)造性方法自動生成雅可比矩陣。其生成步驟如下：（1） 計算各連桿變換。（2） 計算各連桿至末端連桿的變換。（3） 因為第i列由決定，所以可根據(jù)公式（4-15）計算的各列元素。利用MATLAB軟件，計算出六足機器人單只足的雅可比矩陣為：（4-16）4.6 本章小結(jié)本章基于D-H變換，建立了運動學(xué)的簡化計算模型，通過計算運動學(xué)的正解和逆解，得出了六足機器人的足端與根關(guān)節(jié)、髖關(guān)節(jié)、膝關(guān)節(jié)的運動關(guān)系，為六足機器人的足端軌跡的規(guī)劃奠定了基礎(chǔ)。最后，基于微分變換的方法得出了機器人單足的雅可比矩陣。第5章 機器人的足端軌跡規(guī)劃為了使多足機器人在地面、臺階上能夠運動平穩(wěn)，行動快速，并成功地完成規(guī)定作業(yè)任務(wù)的要求，這就要求多足機器人具有非常強的適應(yīng)環(huán)境的能力，使其既能夠跨越普通障礙物和溝壑等，同時其足端還具有路面識別和勘測的功能。為了滿足上述這些功能，需要對多足步行機器人的步足足端運動軌跡提出嚴(yán)格要求，因此對其進(jìn)行合理規(guī)劃就顯得非常重要。5.1 步行足的擺動軌跡分析多足步行機器人是通過足的運動來適應(yīng)復(fù)雜環(huán)境的，因此，在選擇軌跡曲線時，應(yīng)該關(guān)鍵考慮以下問題3。（1） 軌跡曲線的高寬比。曲線的高寬比直接反映出曲線的運動特性。該比值越大、越高，則能力越強，但相應(yīng)的運動速度會越差。（2） 軌跡曲線的弧長。在曲線寬度一定的情況下，曲線長度越長，在空中運動的時間就越長，這將直接影響擺動腿的速度，進(jìn)而影響到機器人的速度。曲線弧越短，運動時間越短，但相應(yīng)的跨越能力越差。（3） 軌跡曲線弧計算難易和曲線表達(dá)式的復(fù)雜性。曲線弧長表達(dá)式越復(fù)雜，計算所花的時間越長，CPU的負(fù)擔(dān)越重，這樣直接影響到六足機器人的反應(yīng)速度。足端軌跡規(guī)劃時，一般采用弧長易于計算的曲線。（4） 不同路面對軌跡曲線的要求。對平面要求要有一定的運動速度；對臺階要求要能夠抬起并安全地越過；對障礙要求要順利跨越等。因為經(jīng)過兩個立足點之間的軌跡曲線有窮多條，所以機器人足端的運動軌跡并不是唯一的。常見的運動軌跡曲線有：拋物線、擺線、心形線、直線段等。在實際的應(yīng)用中，沒有哪個曲線是十全十美的，每種曲線都存在這樣或那樣的優(yōu)點和缺點。比如若取拋物線作為步跡曲線，則其高度可變，起角和落角可以改變，即可適應(yīng)平坦路面也可適應(yīng)階梯路面，但在起落時有沖擊現(xiàn)象存在；若取擺線作為步跡曲線，機器人足端的起落角均與地面垂直，這對具有一定彈性的腿來說，可以有較好的交換特性和不易產(chǎn)生打滑現(xiàn)象的優(yōu)點。但步長與步高的關(guān)系固定不變；若取心形線作為步跡曲線，機器人足端的起角為鈍角，有利于抵消軀體的牽連運動，但是其落角較小，而且步長與步高的關(guān)系是固定不變的；若取直線段作為步跡曲線，則非常容易用直線組成給定的任意步跡曲線，靈活可調(diào)性高，但由于直線間的連接不連續(xù)，速度、加速度存在沖擊。5.2 步行足的擺動軌跡生成因為空間中的任意一條連續(xù)且處處可導(dǎo)的空間曲線都可以用多項式來進(jìn)行插值運算，插值的精度可以由多項式的階次來保證，因此，在本研究中，采用多項式對機器人足端點軌跡進(jìn)行插值運算。軌跡方程可用下面的多項式來表示：（5-1）式中，表示隨時間變化機器人足端點在空間中所經(jīng)過的一系列點的坐標(biāo)，是t的多項式的系數(shù)矢量。由于機器人6條步行足的機械結(jié)構(gòu)和幾何參數(shù)完全相同，因此，只需對任意一足的足端進(jìn)行軌跡規(guī)劃。設(shè)機器人擺動足完成擺動過程所用的時間為t2，經(jīng)過軌跡最高點的時間為t1，這樣就可預(yù)先給出7個約束條件：起始位置：終點位置：中間點位置：起始點速度：終點速度：起始點加速度：終點加速度：以上7個約束條件，可以唯一確定出一條關(guān)于t的六次多項式，該多項式形成的曲線就是擺動足的軌跡。將上述條件代入式（5-1）中，可得到下面的方程組：（5-2）用MATLAB軟件解該方程組可得：（5-3）將方程組（5-3）代入式（5-1）中即可得到六足機器人擺動足足端的軌跡曲線。對于六足機器人的一條步行足來說，在其處于擺動相時，足端的軌跡相對機器人軀體坐標(biāo)系可給定以下約束條件：起始位置：終點位置：中間點位置：起始點速度：終點速度：起始點加速度：終點加速度：同時，設(shè)t1=0.3s，t2=0.6s，將上述約束條件代入式（5-1）和式（5-3）中，可以解出滿足給定約束條件的六足機器人擺動相運動相足端的路徑為：（5-4）速度函數(shù)為：（5-5）加速度函數(shù)為：（5-6）5.3 足端軌跡仿真分析利用MATLAB軟件，在計算機中對所得路徑進(jìn)行分析，繪出曲線分別如圖5-1、圖5-2所示。 圖5-1 x，y，z軸的位移、速度和加速度圖圖5-2 足端點路徑的空間投影曲線由圖中的曲線可以看出，用六次多項式插值成的六足機器人的足端軌跡具有良好的平滑性，足端在經(jīng)過整條軌跡的過程中表現(xiàn)出了較好的運動特性。下面，驗證在該軌跡下的運動，機器人的各個關(guān)節(jié)的特性。采用文獻(xiàn)13中優(yōu)化結(jié)論，設(shè)基節(jié)長，股節(jié)長，脛節(jié)長，結(jié)合求六足機器人運動學(xué)逆解中的式（4-10）、式（4-12）、式（4-13），將足端軌跡中的一系列點的坐標(biāo)代入，再利用MATLAB軟件仿真，即可得到各關(guān)節(jié)的轉(zhuǎn)動參數(shù)曲線，如圖5-3所示。圖5-3 六足機器人擺動足的關(guān)節(jié)在對應(yīng)軌跡下的運動參數(shù)由上圖中的曲線可以看出，擺動足的各關(guān)節(jié)的速度、加速度過渡平滑，不存在沖擊的現(xiàn)象。所以，六次多項式插值成的足端軌跡具有良好的運動特性。5.4 本章小結(jié)本章首先對常用的各種多足步行機器人足端曲線進(jìn)行了理論分析和比較，提出采用多項式生成足端軌跡的問題，并通過MATLAB軟件對足端曲線進(jìn)行了多項式插值計算，基于插值得到的六次多項式曲線計算出擺動腿的足端速度、加速度和各關(guān)節(jié)的角速度、角加速度，得到了較為滿意的運動特性。第6章 六足機器人避障路徑軌跡規(guī)劃當(dāng)移動機器人處于一個簡單或復(fù)雜、靜態(tài)或動態(tài)、已知或未知的環(huán)境中時，機器人的首要任務(wù)是感知環(huán)境，避開障礙物，然后以最小或較少的消耗（時間、空間或者能量）完成相應(yīng)的任務(wù)，這個過程的基礎(chǔ)所在就是路徑規(guī)劃，如圖6-1所示。圖6-1 機器人避障示意圖所謂路徑規(guī)劃是指按照一定的性能指標(biāo)，機器人從所處的環(huán)境中搜索到一條從初始位置目標(biāo)位置最優(yōu)或次優(yōu)的路徑的過程。路徑規(guī)劃問題自提出以來，產(chǎn)生了大量的研究成果。目前求解路徑規(guī)劃的方法主要有人工勢場法和基于人工智能的啟發(fā)式方法，如遺傳算法、蟻群算法等。其中每種方法都有其自身的特點和不足之處，其應(yīng)用都會受到一定的限制。因此，人們一直在不斷探索復(fù)雜環(huán)境下機器人路徑規(guī)劃的有效方法。6.1 人工勢場法路徑規(guī)劃6.1.1 人工勢場法原理人工勢場法是由Khatib等于1985年提出的一種虛擬方法, 是迄今為止應(yīng)用最廣泛的移動機器人路徑規(guī)劃方法之一32。該方法的特點是計算簡潔、實時性強, 便于數(shù)學(xué)描述, 主要用于解決局部路徑的避障規(guī)劃問題。其基本理論指出，對處在任何一個環(huán)境下的目標(biāo)導(dǎo)向機器人，無論該環(huán)境包含靜止的障礙或者動態(tài)移動障礙，都可以定義并計算出一個人工勢能場圖。實際的實現(xiàn)中把目標(biāo)位置當(dāng)作一個吸引極，產(chǎn)生吸引力，障礙物的表面產(chǎn)生排斥力。在任意一個狀態(tài)下，機器人的位姿可以用q表示，勢場可以用表示，目標(biāo)狀態(tài)位姿可用來表示，并定義和目標(biāo)位姿相關(guān)聯(lián)的吸引勢，以及和障礙物，相關(guān)聯(lián)的排斥勢。那么，位姿空間中某一位姿的勢能場可以用下面的公式16表示出來：（6-1）又規(guī)定對于所處空間中的每一個位姿， 都必須是可微分的。那么，移動機器人所受到的虛擬力為目標(biāo)位姿的吸引力和障礙物的斥力的合力，可用下式表示：（6-2）式中表示U在q處的梯度。在二維空間中的位姿來說，有（6-3）對于勢場的定義方式可以有很多種，對于吸引勢和排斥勢本研究采用下面的表達(dá)式33：（6-4）式中是從位姿q到目標(biāo)的歐氏距離；是引力增益系數(shù)；是斥力增益系數(shù)；表示障礙物區(qū)域可對機器人運動產(chǎn)生影響的最大距離；是障礙區(qū)域到位姿q的最小距離。結(jié)合公式（6-3）和公式（6-4），可以求出機器人所受到吸引力的表達(dá)式為：（6-5）排斥力的表達(dá)式為（6-6）其中（6-7）矢量的方向為從障礙物指向機器人，矢量的方向為從機器人指向目標(biāo)。斥力與它的兩個分量的關(guān)系如圖6-2所示。很顯然，當(dāng)對機器人產(chǎn)生斥力時，對其產(chǎn)生朝向目標(biāo)的吸引力。圖6-2 機器人在人工勢場中的受力圖機器人所受到的合力為：（6-8）6.1.2 受力分析當(dāng)機器人處于二維的運動環(huán)境中時，設(shè)機器人在運動空間中的任意位置為，目標(biāo)位置，即終止坐標(biāo)為，則可計算出目標(biāo)對機器人的引力與x軸之間的夾角（6-9）引用公式（6-5）可以得到目標(biāo)對機器人的引力函數(shù)在x，y軸的分量分別為（6-10）假設(shè)運動空間中有N個障礙物，分別位于，則可計算出第i ( i=1n ) 個障礙物對機器人的斥力與x軸之間的夾角（6-11）引用公式（6-6）可以得到第i個障礙物對機器人的斥力函數(shù)在x，y軸的分量分別為（6-12）由式（6-11）和式（6-12）可以得到在位置q機器人受到的合力在x，y軸的分量分別為：（6-13）合力為（6-14）所以機器人受到的合力與x 軸的夾角為（6-15）6.2 蟻群算法路徑規(guī)劃6.2.1 蟻群算法原理蟻群算法是受自然界中真實蟻群的集體覓食行為的啟發(fā)而發(fā)展起來的一種基于群體的模擬進(jìn)化算法，屬于隨機搜索算法。蟻群算法通過模擬蟻群搜索食物的過程，達(dá)到求解比較困難的組合優(yōu)化問題的目的。蟻群算法的主要特征是采用正反饋搜索機制，分布式計算方法，用來解決各種分布式環(huán)境下的組合優(yōu)化問題螞蟻屬于群居昆蟲，個體行為極其簡單，但由單個簡單的個體所組成的群體卻表現(xiàn)出極其復(fù)雜的行為。螞蟻個體之間是通過一種稱之為信息素的物質(zhì)進(jìn)行信息傳遞的。螞蟻在運動過程中，能夠在它所經(jīng)過的路徑上留下該種物質(zhì)，而且螞蟻在運動過程中能夠感知這種物質(zhì)的存在及其強度，并以此指導(dǎo)自己的運動方向，螞蟻傾向于朝著該物質(zhì)強度高的方向移動。同時，該物質(zhì)隨著時間的推移會逐漸揮發(fā)掉，于是路徑的長短及該路徑上通過的螞蟻的多少就對殘余信息素的強度產(chǎn)生影響，反過來信息素的強弱又指導(dǎo)其它螞蟻的行動方向。因此，某一路徑上走過的螞蟻越多，則后來者選擇該路徑的概率就越大。這就構(gòu)成了螞蟻群體行為表現(xiàn)出的一種信息正反饋現(xiàn)象。螞蟻個體之間就是通過這種信息交流達(dá)到最快捷搜索到食物源的目的。這里用如圖6-3所示的圖例具體地說明蟻群系統(tǒng)的原理。abc圖6-3 自然界中的螞蟻覓食模擬圖6-3中，設(shè)A點是蟻巢，D點是食物源，EF為一障礙。由于障礙物的存在，螞蟻只能由A經(jīng)E或F到達(dá)D，或由D到達(dá)A，各點之間的距離如圖6.3a所示。假設(shè)每個時間單位有30只螞蟻由A 到達(dá)D點，有30只螞蟻由于D到達(dá)A點，螞蟻過后留下的信息為1。為了方便起見，高該物質(zhì)停留時間為1.在初始時刻，由于路徑BF、FC、BE、EC上均無信息存在，位于A和D的螞蟻可以隨機選擇路徑，從統(tǒng)計學(xué)的角度只可以認(rèn)為螞蟻以相同的概率選擇BF、FC、BE、EC，如圖6.3b所示。經(jīng)過一個時間單位后，在路徑BFC上的信息量是路徑BEC上信息的2倍。又經(jīng)過一段時間，將20只螞蟻由B、F和C點到達(dá)D，如圖6.3c所示。隨著時間的推移，螞蟻將會越來越大的概率選擇路徑BFC，最終將會完全選擇路徑BFC，從而找到由蟻巢到食物源的最短路徑 34 。6.2.2 基本蟻群算法的數(shù)學(xué)模型 為了闡述螞蟻系統(tǒng)的數(shù)學(xué)模型，首先引入經(jīng)典的TSP問題(旅行商問題)。TSP問題是指給定n個城市以及兩兩城市之間的距離，要求確定一條經(jīng)過各個城市當(dāng)且僅當(dāng)一次的最短路線。其圖論描述為：給定圖G=(V，A)，其中V為頂點集，A為各個頂點相互連接組成的邊集，己知各個頂點之間的連接距離，要求確定一條長度最短的Hamilton回路，即遍歷所有頂點當(dāng)且僅當(dāng)一次的最短回路。引入以下記號來模擬實際螞蟻的行為：蟻群中螞蟻數(shù)量；：t時刻位于城市 i 的螞蟻個數(shù)；：兩城市 i 和 j之間的距離；：啟發(fā)函數(shù)，即路徑 ( i，j ) 的能見度，反映由城市 i 轉(zhuǎn)移到城市 j的啟發(fā)程度，在TSP問題中一般取。，這個量在螞蟻系統(tǒng)的運行中不改變；：路徑(i，j)上的信息素軌跡強度；：螞蟻k在邊(i，j)上留下的單位長度軌跡信息素量；：螞蟻k在城市i選擇城市j的轉(zhuǎn)移概率。注：以上符號中的i和j都是城市序號，且i，j=1n。k是螞蟻序號，且k=1m。下同。初始時刻，各條路徑上的信息素量相等，設(shè) (C為常數(shù))。螞蟻k在運動過程中根據(jù)各條路徑的能見度和路徑上的信息素量決定轉(zhuǎn)移方向。螞蟻系統(tǒng)所使用的狀態(tài)轉(zhuǎn)移規(guī)則稱為隨機比例規(guī)則，它給出了t時刻位于城市i的螞蟻k選擇移動到城市