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