本發(fā)明屬于智能算法優(yōu)化領(lǐng)域，涉及一種基于市場拍賣法的多機(jī)器人系統(tǒng)編隊任務(wù)分配算法和基于切線圓法的多機(jī)器人編隊路徑規(guī)劃算法。
背景技術(shù)：
：編隊控制方法是指多機(jī)器人系統(tǒng)在到達(dá)目標(biāo)位置的過程中既保持某種隊形又可以適應(yīng)環(huán)境約束的控制方法。由于多機(jī)器人編隊的作業(yè)任務(wù)和環(huán)境各不相同，科研人員提出了多種機(jī)器人編隊的控制方法，其中最為常用的方法有基于領(lǐng)航者-跟隨者的方法、虛擬結(jié)構(gòu)法、基于行為的方法、基于圖論的方法和強(qiáng)化學(xué)習(xí)的方法。領(lǐng)航者-跟隨者方法的基本思想是在多機(jī)器人組成的編隊系統(tǒng)中，可以存在一個或者多個領(lǐng)航機(jī)器人(Leader)，其他非領(lǐng)航者機(jī)器人即為跟隨機(jī)器人(Follower)，跟隨機(jī)器人以相對其領(lǐng)航機(jī)器人的位置(相對距離L、相對角度φ)作為輸入控制量，使得跟隨機(jī)器人與領(lǐng)航機(jī)器人的相對位置無限逼近目標(biāo)值。領(lǐng)航跟隨方法的控制結(jié)構(gòu)比較簡單，但是由于領(lǐng)航機(jī)器人和跟隨機(jī)器人之間沒有位置反饋，并且是領(lǐng)航者機(jī)器人單點(diǎn)控制，導(dǎo)致容易出現(xiàn)機(jī)器人掉隊等情況，系統(tǒng)的魯棒性較差。虛擬結(jié)構(gòu)法的基本思想是將多機(jī)器人組成的系統(tǒng)看成一個假想的剛體結(jié)構(gòu)，各個機(jī)器人在參考坐標(biāo)系下的坐標(biāo)不變，即機(jī)器人之間的相對位置不變?；谛袨榈姆椒ǖ幕舅枷胧菍⒍鄼C(jī)器人編隊的各個環(huán)節(jié)看成由單個機(jī)器人的多個基本行為構(gòu)成，只需要研究各個基本行為的控制方法即可通過基本行為的組合控制多機(jī)器人形成編隊?；拘袨橐话惆繕?biāo)跟蹤、避免障礙、避免碰撞、編隊生成和編隊保持等?；谛袨榈姆椒ㄓ筛鱾€機(jī)器人相互感知控制，系統(tǒng)比較容易實(shí)現(xiàn)分布式控制，具有很好的魯棒性。但是由于無法結(jié)合準(zhǔn)確的數(shù)學(xué)模型分析，導(dǎo)致不能十分有效的保證系統(tǒng)的可靠性?；趫D論的方法的基本思想是結(jié)合圖論的知識，將各個機(jī)器人看成節(jié)點(diǎn)，然后節(jié)點(diǎn)之間按照一定的規(guī)則相互連通，控制指令在節(jié)點(diǎn)之間按照一定的方向“傳遞”，使得整個多機(jī)器人系統(tǒng)形成一種網(wǎng)狀的拓?fù)浣Y(jié)構(gòu)。強(qiáng)化學(xué)習(xí)的方法是一種比較新興的研究方法，基本思想是結(jié)合機(jī)器學(xué)習(xí)的理論，對機(jī)器人在學(xué)習(xí)階段的行為進(jìn)行評估，如果機(jī)器人的行為符合預(yù)期設(shè)定則獎勵為正，那么機(jī)器人將在以后的動作行為中更加偏向使用該行為，反之這種行為的出現(xiàn)概率將會越來越低，甚至消失。通過有效的設(shè)定行為獎懲規(guī)則，可以使得各個機(jī)器人能夠按照設(shè)定進(jìn)行長時間的“訓(xùn)練”，最終可以實(shí)現(xiàn)編隊效果。經(jīng)過對現(xiàn)有多機(jī)器人編隊控制方法的對比，可以看出國內(nèi)外學(xué)者對多機(jī)器人的編隊控制方法研究多應(yīng)用于解決二維平面內(nèi)的編隊控制問題，或者只考慮編隊形狀而不考慮形成編隊時各個機(jī)器人的姿態(tài)問題。目前，三維空間內(nèi)的編隊控制方法主要是簡化為二維平面內(nèi)的編隊問題來處理，而且三維空間內(nèi)的編隊控制算法復(fù)雜度太高，很難實(shí)現(xiàn)對多機(jī)器人系統(tǒng)的準(zhǔn)確編隊控制。因此，現(xiàn)有的編隊控制方法不能很好的適用于解決多機(jī)器人在三維空間內(nèi)的編隊控制問題。針對上述問題，提出一種基于市場拍賣法和切線圓法的新型路徑規(guī)劃算法，解決多機(jī)器人三維空間內(nèi)的編隊路徑規(guī)劃問題。技術(shù)實(shí)現(xiàn)要素：為解決上述問題，本專利提供一種基于市場拍賣法的多機(jī)器人系統(tǒng)編隊的任務(wù)分配算法和基于切線圓法的多機(jī)器人編隊路徑規(guī)劃算法?；谑袌雠馁u法的多機(jī)器人系統(tǒng)編隊的任務(wù)分配算法包括，由目標(biāo)狀態(tài)矩陣ST可以通過市場拍賣法的方式將各坐標(biāo)分配給各個機(jī)器人，并形成路徑，使各個機(jī)器人跟蹤參考路徑向形成編隊形狀的各個目標(biāo)位置運(yùn)動。傳統(tǒng)的市場拍賣法僅僅根據(jù)目標(biāo)點(diǎn)與當(dāng)前各個機(jī)器人的距離遠(yuǎn)近進(jìn)行坐標(biāo)分配，當(dāng)多機(jī)器人系統(tǒng)中的機(jī)器人個數(shù)比較少時，傳統(tǒng)的坐標(biāo)分配方法可能產(chǎn)生比較小的誤差，但是當(dāng)機(jī)器人個數(shù)明顯增加時，傳統(tǒng)的方法很可能導(dǎo)致多機(jī)器人系統(tǒng)形成編隊所需要的時間更長、耗能更多。本文提出一種改進(jìn)的市場拍賣算法進(jìn)行多機(jī)器人編隊系統(tǒng)中各個坐標(biāo)的分配計算，綜合考慮所有機(jī)器人向形成編隊的目標(biāo)位置運(yùn)動路程之和Sumdistance，機(jī)器人從接收到形成編隊指令到形成編隊所需要的時間Tcost(每個機(jī)器人的姿態(tài)調(diào)整時間Ta和機(jī)器人運(yùn)動到指定位置時間Tb之和的最大值)，分別使用Ka，Kb設(shè)置Sumdistance和Tcost的權(quán)值參數(shù)，在不同環(huán)境下可選擇不同的權(quán)值參數(shù)。為了保證編隊任務(wù)的連續(xù)性，需要在形成編隊形狀時各個機(jī)器人有相同的速度Vg和姿態(tài)角θg,因此需要計算各個機(jī)器人運(yùn)動到指定坐標(biāo)點(diǎn)所需要的時間Ti的最小值min{Ti}，設(shè)機(jī)器人角速度為ω，Li為機(jī)器人Ri到形成編隊形狀需要運(yùn)動的路程，則因此得Tcost＝max{Ti}，則每個機(jī)器人運(yùn)動到指定目標(biāo)點(diǎn)的平均速度：故需要根據(jù)目標(biāo)速度Vg和運(yùn)動時間Tcost，結(jié)合機(jī)器人的動力學(xué)公式，對機(jī)器人的速度進(jìn)行運(yùn)動學(xué)控制，使各個機(jī)器人都能在規(guī)定時間內(nèi)以準(zhǔn)確的速度和姿態(tài)到達(dá)指定位置。設(shè)編隊形成過程效率指標(biāo)為η，則：k表示第k種坐標(biāo)分配方案，0≤ηk≤2。通過設(shè)置合適的權(quán)值參數(shù)Ka、Kb可以選擇不同的編隊分配方案作為任務(wù)要求下的最優(yōu)解?；谇芯€圓法的多機(jī)器人編隊路徑規(guī)劃算法應(yīng)用于解決三維空間內(nèi)的多機(jī)器人編隊的路徑規(guī)劃和位姿控制，三維空間內(nèi)的切線圓法原理圖如圖1所示。步驟1：通過程序算法計算機(jī)器人Ri到達(dá)指定目標(biāo)位置所需要的時間ti；步驟2：用表示機(jī)器人Ri的目標(biāo)點(diǎn)，在圖1中繪出的位置，并結(jié)合目標(biāo)位置的姿態(tài)角方向向量ri和機(jī)器人Ri的初始位置坐標(biāo)可以構(gòu)建一個平面H；在平面H內(nèi)，以為切點(diǎn)可以做目標(biāo)點(diǎn)運(yùn)動軌跡直線的兩個外切圓外切圓半徑為機(jī)器人Ri的預(yù)設(shè)轉(zhuǎn)彎半徑ri；步驟3：在平面H內(nèi)，由機(jī)器人初始位置向作切線，易知有四種解法路徑直線l1、l2、l3、l4，滿足機(jī)器人運(yùn)動學(xué)要求的是直線l2、l3，計算易知直線l2路徑最優(yōu)；步驟4：由機(jī)器人初始位置Ri的初始姿態(tài)角，及直線l2可得一個平面，記為平面P，則需要在平面P內(nèi)找到一條路徑使得機(jī)器人Ri可以沿此路徑進(jìn)入到l2；步驟5：在P平面內(nèi)，過點(diǎn)，在直線l2側(cè)做外切圓外切圓半徑為ri；在直線l2上下兩側(cè)分別作出圓與圓直線l2相切，如圖1所示，僅有圓滿足要求，故可構(gòu)建路徑如圖1虛線所示。以為例計算路徑曲線。過x軸、點(diǎn)和ri可做平面則由空間平面方程求解。平面法向量：則平面的方程為：設(shè)ri與nx的夾角為δ，則：在XOY平面作出以δ為方向角的直線l0，如圖2所示，并作相切圓則圓的方程可寫成：r為轉(zhuǎn)彎半徑。則圓的圓心(a,b)由下式定義：則圓的方程可寫成：則切點(diǎn)P1坐標(biāo)定義如下：設(shè)平面與平面XOY的夾角為γ，則：則由坐標(biāo)旋轉(zhuǎn)公式，繞x軸旋轉(zhuǎn)后新坐標(biāo)滿足：故可得在平面中圓的方程為：圓的方程為：附圖說明圖1為三維空間內(nèi)的切線圓法原理圖圖2為平面XOY上幾何關(guān)系圖圖3為編隊任務(wù)分配仿真圖圖4為空間四面體編隊模型圖圖5為四面體編隊仿真圖具體實(shí)施方式定義機(jī)器人的坐標(biāo)和姿態(tài)用矩陣表示，x,y,z表示機(jī)器人在笛卡爾直角坐標(biāo)系下的位置，θ,表示機(jī)器人在笛卡爾直角坐標(biāo)系下的姿態(tài)方向。以四個機(jī)器人形成一個四面體形的多機(jī)器人編隊形狀為例。初始坐標(biāo)為：目標(biāo)編隊形狀各頂點(diǎn)坐標(biāo)為：四機(jī)器人編隊系統(tǒng)中的所有任務(wù)分配方法如表1所示。表1四機(jī)器人編隊系統(tǒng)中的所有任務(wù)分配方法kSumdistance(m)Tcost(s)η144.976220.59210.8845246.067621.43160.9133352.519021.54330.9783451.661424.67551.0382550.677224.67551.0286652.626221.54330.9793746.067620.59210.8950847.159021.43160.9239951.661420.59210.94931053.185124.79521.05561152.200924.79521.04601251.768621.43160.96861350.677220.59210.93971452.200924.79521.04601552.626221.54330.97931651.768624.67551.03921756.378224.67551.08391858.759424.79521.10961950.226824.79521.02692049.794621.43160.94952157.769624.79521.10002255.388424.67551.07432349.794624.67551.02012451.743521.54330.9708由表中數(shù)據(jù)可得，當(dāng)I＝0.8845時，系統(tǒng)有最優(yōu)解，此時花費(fèi)時間20.5921s，總運(yùn)動路程44.9762m。通過MATLAB仿真可得任務(wù)分配如圖3所示，實(shí)心圓點(diǎn)為各個機(jī)器人的初始位置，空心圓點(diǎn)為目標(biāo)點(diǎn)位置。此種解法為給定初始狀態(tài)和目標(biāo)位置的最優(yōu)解，證明了多機(jī)器人編隊過程的任務(wù)分配算法的有效性。以四面體編隊為例，最終形成的四面體編隊形狀如圖4中所示四面體G0G1G2G3，并沿x軸正向向目標(biāo)點(diǎn)Gt運(yùn)動。目標(biāo)狀態(tài)矩陣：編隊形成過程的問題即轉(zhuǎn)化為使用算法使得ST＝S，為了保證目標(biāo)矩陣的單一性，便于計算，做如下規(guī)定依次由的大小確定目標(biāo)矩陣ST的行向量，列坐標(biāo)小的行向量在前。運(yùn)用基于改進(jìn)市場拍賣法的多機(jī)器人編隊坐標(biāo)分配方法，將形成編隊形狀的G1、G2、G3坐標(biāo)分配給R1、R2、R3機(jī)器人。運(yùn)用基于切線圓法的三維平面內(nèi)路徑規(guī)劃算法和位姿控制方法，計算各個機(jī)器人的編隊路徑。最終得到仿真圖如圖5所示。從圖5可以看出，各個機(jī)器人在初始各自不同的姿態(tài)方向運(yùn)動情況下，可以使用切線圓法規(guī)劃出運(yùn)動路線和調(diào)整運(yùn)動姿態(tài)，并最終和其他機(jī)器人以相同的運(yùn)動姿態(tài)形成四面體編隊形狀，驗(yàn)證了多機(jī)器人編隊過程的路徑規(guī)劃和位姿控制算法的有效性。當(dāng)前第1頁1 2 3