專利名稱：用于三維空間中移動(dòng)機(jī)械臂路徑規(guī)劃的主從式協(xié)進(jìn)化方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種路徑規(guī)劃方法，尤其涉及一種用于三維空間中移動(dòng)機(jī)械臂路徑規(guī)劃的主從式協(xié)進(jìn)化方法。
背景技術(shù)：
路徑規(guī)劃是移動(dòng)機(jī)械臂中的一個(gè)基本問(wèn)題，即尋找起始位姿和終態(tài)位姿之間可行的連續(xù)路徑。此外，還需要考慮到相關(guān)指標(biāo)如路徑、時(shí)間等的優(yōu)化以及障礙物避碰等。移動(dòng)機(jī)械臂有很高的自由度，且要在三維空間而不是平面中進(jìn)行規(guī)劃，因此加大了路徑規(guī)劃的難度。移動(dòng)機(jī)械臂是一個(gè)包含機(jī)械臂和移動(dòng)平臺(tái)的組合系統(tǒng)，在規(guī)劃中如何協(xié)調(diào)這兩者的關(guān)系成為重點(diǎn)。前人提出了很多解決方法Pin和culioli利用變換位姿將其轉(zhuǎn)換為多目標(biāo)優(yōu)化問(wèn)題，但對(duì)兩個(gè)機(jī)械臂和移動(dòng)平臺(tái)使用一個(gè)目標(biāo)函數(shù)進(jìn)行優(yōu)化，難以實(shí)現(xiàn)整體性能最優(yōu)。yamamoto提出首選操作區(qū)的概念，實(shí)際是先移動(dòng)平臺(tái)至合適位置再移動(dòng)機(jī)械臂， 未能發(fā)揮出移動(dòng)機(jī)械臂在可移動(dòng)性和可操作性上的優(yōu)勢(shì)，且很容易造成行進(jìn)中臂的碰撞。 saraji把整個(gè)系統(tǒng)視為一個(gè)冗余的機(jī)械臂來(lái)進(jìn)行規(guī)劃，忽略了機(jī)械臂和移動(dòng)平臺(tái)在性能上的差異。李新春等提出了一種分級(jí)協(xié)調(diào)的路徑規(guī)劃方法，將平臺(tái)和機(jī)械手分別選擇目標(biāo)函數(shù)進(jìn)行規(guī)劃，但其編碼方式過(guò)于簡(jiǎn)單，且并未考慮到機(jī)械臂對(duì)平臺(tái)所造成的影響。為了考慮移動(dòng)平臺(tái)和機(jī)械臂相互之間的影響，我們想到了協(xié)進(jìn)化遺傳算法。協(xié)進(jìn)化遺傳方法是在傳統(tǒng)遺傳方法基礎(chǔ)上改進(jìn)得到的，傳統(tǒng)遺傳方法中將問(wèn)題的解編碼為個(gè)體，進(jìn)行選擇、交叉、變異等操作，并根據(jù)適應(yīng)度函數(shù)來(lái)對(duì)進(jìn)化后的個(gè)體進(jìn)行篩選，最終搜索出解空間里較優(yōu)的解。而協(xié)進(jìn)化遺傳方法不僅考慮到了種群內(nèi)的進(jìn)化，還考慮到不同種群對(duì)各自進(jìn)化的影響，各種群獨(dú)立進(jìn)化后會(huì)選出代表，進(jìn)入環(huán)境模型進(jìn)行共同進(jìn)化，得到新的適應(yīng)度并返回原種群，影響原種群的進(jìn)化，從而達(dá)到共同進(jìn)化的目標(biāo)。協(xié)進(jìn)化方法的基本步驟如圖2。協(xié)進(jìn)化遺傳方法中的不足之處在于第一，對(duì)于差異明顯的兩個(gè)種群，協(xié)作的難度較大，因?yàn)閰f(xié)進(jìn)化方法是不影響種群內(nèi)的進(jìn)化的，它只是利用環(huán)境模型對(duì)各種群代表所組成的“聯(lián)合種群”進(jìn)行了進(jìn)化操作并得出新的適應(yīng)度返回原種群，如果種群間差異過(guò)大，可以導(dǎo)致無(wú)法對(duì)“聯(lián)合種群”進(jìn)行進(jìn)化操作行為。第二，該方法是一種并行操作的方法，即各種群各自單獨(dú)進(jìn)化，再進(jìn)行共同進(jìn)化，因此對(duì)于強(qiáng)耦合的個(gè)體無(wú)法使用，而本發(fā)明所針對(duì)的實(shí)際系統(tǒng)正是一個(gè)強(qiáng)耦合的系統(tǒng)。

發(fā)明內(nèi)容
本發(fā)明的目的在于針對(duì)現(xiàn)有技術(shù)的不足，提供一種用于三維空間中移動(dòng)機(jī)械臂路徑規(guī)劃的主從式協(xié)進(jìn)化方法。為了實(shí)現(xiàn)上述的目的，本發(fā)明采取如下技術(shù)方案一種用于三維空間中移動(dòng)機(jī)械臂路徑規(guī)劃的主從式協(xié)進(jìn)化方法，包括以下步驟(1)利用視覺(jué)系統(tǒng)得到環(huán)境的整體信息。(2)構(gòu)建環(huán)境模型和移動(dòng)機(jī)械臂模型，并對(duì)個(gè)體進(jìn)行編碼。(3)以移動(dòng)平臺(tái)作為規(guī)劃主導(dǎo)，根據(jù)線性同余法為系數(shù)隨機(jī)產(chǎn)生表示移動(dòng)平臺(tái)初始路徑的路徑序列。(4)對(duì)初始路徑進(jìn)行遺傳算法操作，進(jìn)化100代后，得到適應(yīng)度高且無(wú)碰撞的路徑集。(5)從路徑集中選出適應(yīng)度高的進(jìn)行離散操作。(6)在離散路徑的基礎(chǔ)上進(jìn)行機(jī)械臂的遺傳算法操作，得到臂的最優(yōu)角度序列。(7)查看所得序列是否會(huì)引起碰撞或角度變化劇烈，若有則對(duì)機(jī)械臂角度進(jìn)行光滑處理，并調(diào)換角色，以臂作為主導(dǎo)對(duì)移動(dòng)平臺(tái)重新進(jìn)行路徑規(guī)劃，若無(wú)則轉(zhuǎn)入下一步。(8)評(píng)估機(jī)械臂和移動(dòng)平臺(tái)的整體性能，若達(dá)到所設(shè)定的閾值則算法結(jié)束，若未達(dá)到所設(shè)定的閾值則從移動(dòng)平臺(tái)的路徑集中重新選擇次優(yōu)路徑進(jìn)行機(jī)械臂的進(jìn)化，如此直到到達(dá)閾值要求，算法結(jié)束。(9)將最終所得結(jié)果應(yīng)用于實(shí)際移動(dòng)機(jī)械臂平臺(tái)，使其可以按較優(yōu)路徑從起始態(tài)到達(dá)目標(biāo)態(tài)。本發(fā)明的有益效果是，能夠保證移動(dòng)機(jī)械臂系統(tǒng)的整體性能較優(yōu)，且由于平臺(tái)先獨(dú)立進(jìn)行進(jìn)化，因此更易找到平臺(tái)在全局空間中的最優(yōu)解，并通過(guò)主從角色的調(diào)換，可以有效進(jìn)行機(jī)械臂和移動(dòng)平臺(tái)的協(xié)作，防止空間內(nèi)發(fā)生的碰撞。


圖1是本發(fā)明中雙目視覺(jué)中的幾何關(guān)系圖。圖2是本發(fā)明中協(xié)進(jìn)化遺傳算法的流程圖。圖3是本發(fā)明中機(jī)械臂和小車模型圖。圖4是本發(fā)明中主從式協(xié)進(jìn)化方法流程圖。
具體實(shí)施例方式本發(fā)明專利是利用主從式協(xié)進(jìn)化方法實(shí)現(xiàn)三維空間中移動(dòng)機(jī)械臂路徑規(guī)劃。該方法的原理是仿照人類的社會(huì)活動(dòng)情況，在群體活動(dòng)中總是有部分人為主導(dǎo)地位，主導(dǎo)整個(gè)群體的發(fā)展方向，另一部分人屬于從屬地位，幫助推動(dòng)群體的發(fā)展。而且這種地位并不是不變的，而是隨著所面臨的環(huán)境和個(gè)人經(jīng)驗(yàn)對(duì)群體的貢獻(xiàn)大小而發(fā)生改變的。在本文所述移動(dòng)機(jī)械臂中，包含移動(dòng)平臺(tái)和機(jī)械臂，給這兩個(gè)子系統(tǒng)分配領(lǐng)導(dǎo)者和協(xié)助者的角色。首先由移動(dòng)平臺(tái)擔(dān)任領(lǐng)導(dǎo)者的角色，完成地面上的路徑規(guī)劃，得到一條最優(yōu)路徑。 然后由協(xié)助者一機(jī)械臂在領(lǐng)導(dǎo)者完成的任務(wù)基礎(chǔ)上完成自己的任務(wù)規(guī)劃無(wú)碰撞且較優(yōu)的位姿。由于有碰撞的個(gè)體也可能在下一代的進(jìn)化中轉(zhuǎn)變成為無(wú)碰撞的，因此在算法中并沒(méi)有將有碰撞的個(gè)體去除，而只是給它定義了一個(gè)罰函數(shù)。因此當(dāng)協(xié)助者完成任務(wù)后會(huì)再次進(jìn)行一個(gè)評(píng)估，即看它是否得到無(wú)碰位姿，所得到的位姿是否不會(huì)發(fā)生劇烈震蕩，若是則得到移動(dòng)機(jī)械臂最優(yōu)解，若不是則調(diào)換角色，以機(jī)械臂作為領(lǐng)導(dǎo)者，對(duì)位姿先進(jìn)行優(yōu)化，然后車作為協(xié)助者重新進(jìn)行進(jìn)化。
最后還要再進(jìn)行移動(dòng)機(jī)械臂整體的適應(yīng)度評(píng)估，若達(dá)到一個(gè)閾值則算法結(jié)束，若達(dá)不到則在第一步所得移動(dòng)平臺(tái)路徑集中選出次優(yōu)解再次進(jìn)行上述過(guò)程。本發(fā)明用于三維空間中移動(dòng)機(jī)械臂路徑規(guī)劃的主從式協(xié)進(jìn)化方法，包括以下步驟
1.利用視覺(jué)系統(tǒng)得到環(huán)境的整體信息。通以雙目相機(jī)對(duì)同一物體從不同位置成像獲得立體圖像，融合左右相機(jī)獲得的立體圖像，并察覺(jué)它們之間的差別(視差)，根據(jù)它們自身相對(duì)的幾何學(xué)計(jì)算左右圖像的關(guān)系， 使計(jì)算機(jī)能夠?qū)С錾疃刃畔?。兩個(gè)已知內(nèi)參數(shù)的攝像機(jī)觀察同一個(gè)場(chǎng)景點(diǎn)，它的3D坐標(biāo)可以用兩條射線的交點(diǎn)計(jì)算出來(lái)，這是立體視覺(jué)(Stereo Vision)的基本原理。為了獲得3D坐標(biāo)，一般需要三個(gè)步驟獲得攝像機(jī)的內(nèi)參數(shù)；在左右圖像中的點(diǎn)之間建立對(duì)應(yīng)點(diǎn)對(duì)；重構(gòu)場(chǎng)景中點(diǎn)的深度信息。圖1給出了兩個(gè)攝像機(jī)系統(tǒng)的幾何學(xué)。其中場(chǎng)景點(diǎn)為P(AP) ,PlPr分別是P(^z)在左相機(jī)和右相機(jī)拍攝的圖像點(diǎn)，在以像素為單位的圖像坐標(biāo)系 。￥l禾口 2V2中坐標(biāo)分別為(x. y)。以左相機(jī)中心為坐標(biāo)原點(diǎn)，根據(jù)視差理論計(jì)算可得到對(duì)應(yīng)點(diǎn)的關(guān)系為
權(quán)利要求
1.一種用于三維空間中移動(dòng)機(jī)械臂路徑規(guī)劃的主從式協(xié)進(jìn)化方法，其特征在于，包括以下步驟(1)利用視覺(jué)系統(tǒng)得到環(huán)境的整體信息；(2)構(gòu)建環(huán)境模型和移動(dòng)機(jī)械臂模型，并對(duì)個(gè)體進(jìn)行編碼；(3)以移動(dòng)平臺(tái)作為規(guī)劃主導(dǎo)，根據(jù)線性同余法為系數(shù)隨機(jī)產(chǎn)生表示移動(dòng)平臺(tái)初始路徑的路徑序列；(4)對(duì)初始路徑進(jìn)行遺傳算法操作，進(jìn)化100代后，得到適應(yīng)度高且無(wú)碰撞的路徑集；(5)從路徑集中選出適應(yīng)度高的進(jìn)行離散操作；(6)在離散路徑的基礎(chǔ)上進(jìn)行機(jī)械臂的遺傳算法操作，得到臂的最優(yōu)角度序列；(7)查看所得序列是否會(huì)引起碰撞或角度變化劇烈，若有則對(duì)機(jī)械臂角度進(jìn)行光滑處理，并調(diào)換角色，以臂作為主導(dǎo)對(duì)移動(dòng)平臺(tái)重新進(jìn)行路徑規(guī)劃，若無(wú)則轉(zhuǎn)入下一步；(8)評(píng)估機(jī)械臂和移動(dòng)平臺(tái)的整體性能，若達(dá)到所設(shè)定的閾值則算法結(jié)束，若未達(dá)到所設(shè)定的閾值則從移動(dòng)平臺(tái)的路徑集中重新選擇次優(yōu)路徑進(jìn)行機(jī)械臂的進(jìn)化，如此直到到達(dá)閾值要求，算法結(jié)束；(9)將最終所得結(jié)果應(yīng)用于實(shí)際移動(dòng)機(jī)械臂平臺(tái)，使其可以按較優(yōu)路徑從起始態(tài)到達(dá)目標(biāo)態(tài)。
2.根據(jù)權(quán)利要求1所述用于三維空間中移動(dòng)機(jī)械臂路徑規(guī)劃的主從式協(xié)進(jìn)化方法，其特征在于，所述步驟(1)中，以雙目相機(jī)采集圖像，并對(duì)圖像進(jìn)行SURF特征點(diǎn)檢測(cè)和特征點(diǎn)匹配，以得到環(huán)境中障礙物所在的空間位置。
3.根據(jù)權(quán)利要求1所述用于三維空間中移動(dòng)機(jī)械臂路徑規(guī)劃的主從式協(xié)進(jìn)化方法， 其特征在于，所述步驟(2)具體為，將移動(dòng)平臺(tái)膨脹為空間中的一個(gè)點(diǎn)，該點(diǎn)有三個(gè)自由度 X, Y,航向角爐，滿足非完整約束知sm9 ->scosiP=0 ,個(gè)體編碼為(X1jJ1,^ )，()，…·，(\,凡,鉺)；將6個(gè)關(guān)節(jié)的!11討0111肌機(jī)械臂簡(jiǎn)化為三段連桿1^0、1^、1^，角度從下到上為α 1、α 2，個(gè)體編碼為()，( (^,εξ )，…，(Of, 4 )。
4.根據(jù)權(quán)利要求1所述用于三維空間中移動(dòng)機(jī)械臂路徑規(guī)劃的主從式協(xié)進(jìn)化方法，其特征在于，所述步驟(3)包括以下子步驟(A)對(duì)于移動(dòng)平臺(tái)，首先求得每段路徑下χ的最大值ImHt—S = (j-1)^mtenral/cos ^，再利用線性同余法產(chǎn)生滿足均勻分布的隨機(jī)數(shù)作為系數(shù)k，移動(dòng)平臺(tái)個(gè)體編碼中 = — , y^· = 1 / tan φ*QimU __ χ- Xj) , j 的范圍從 1 至Ij 編碼長(zhǎng)度—φ ,爐為航向角，interval為每段長(zhǎng)度下的路徑，計(jì)算方法為interval = xsf+{ye-y5f codskngtk，其中 &、yf 為平臺(tái)目標(biāo)位置處的 x、y 坐標(biāo) λ、ys為平臺(tái)初始位置處的χ、y坐標(biāo)，codelength為編碼長(zhǎng)度；(B)對(duì)于機(jī)械臂，首先求得每段路徑下χ的最大值 mlimit _χ = αζ - (j -1) *minterval / sm (atan2(l / 召))—α/ , 其中 j 的范 圍從1到編碼長(zhǎng)度mcodelengih , minterval為每段長(zhǎng)度下的角度值，計(jì)算方法為
5.根據(jù)權(quán)利要求1所述用于三維空間中移動(dòng)機(jī)械臂路徑規(guī)劃的主從式協(xié)進(jìn)化方法，其特征在于，所述步驟(4 )包括以下子步驟(A)對(duì)移動(dòng)平臺(tái)路徑序列計(jì)算適應(yīng)度；平臺(tái)的適應(yīng)度應(yīng)該包括以下指標(biāo)首先是路徑長(zhǎng)度最小，而且在該路徑長(zhǎng)度上不能與障礙物發(fā)生碰撞；關(guān)于時(shí)間最短的問(wèn)題會(huì)在計(jì)算機(jī)械臂的適應(yīng)度時(shí)再考慮，因?yàn)闀r(shí)間的取值是平臺(tái)和機(jī)械臂中較長(zhǎng)的那個(gè)；路徑長(zhǎng)度表示 為
6.根據(jù)權(quán)利要求1所述用于三維空間中移動(dòng)機(jī)械臂路徑規(guī)劃的主從式協(xié)進(jìn)化方法，其特征在于，所述步驟(5)具體為從路徑集& ??！中選擇適應(yīng)度最高的路徑序列，進(jìn)行離散化操作，即設(shè)定平臺(tái)行駛速度為常數(shù)，將所得路徑轉(zhuǎn)換為時(shí)間上平均分布的路徑。
7.根據(jù)權(quán)利要求1所述用于三維空間中移動(dòng)機(jī)械臂路徑規(guī)劃的主從式協(xié)進(jìn)化方法，其特征在于，所述步驟(6 )包含以下子步驟(A)在此離散路徑基礎(chǔ)上對(duì)機(jī)械臂進(jìn)行遺傳算法操作；適應(yīng)度函數(shù)考慮以下指標(biāo)移動(dòng)機(jī)械臂整體運(yùn)行時(shí)間T，與空間障礙物碰撞的次數(shù)運(yùn)動(dòng)過(guò)程中角度變化的劇烈程度ξ ；算法中設(shè)定機(jī)械臂的運(yùn)動(dòng)速度為一常數(shù)，因此運(yùn)行時(shí)間
8.根據(jù)權(quán)利要求1所述用于三維空間中移動(dòng)機(jī)械臂路徑規(guī)劃的主從式協(xié)進(jìn)化方法，其特征在于，所述步驟(7)具體為由于未去除有碰撞的個(gè)體，因此進(jìn)化完后在該步驟查看步驟(6)中所得到的最優(yōu)角度序列是否會(huì)發(fā)生碰撞，即Kl是否為0，同時(shí)查看角度是否發(fā)生連續(xù)兩次反向波動(dòng)，即4 <0 &<0 ,若有則對(duì)臂進(jìn)行光滑處理，得到新的序列(4，，并對(duì)移動(dòng)平臺(tái)重新進(jìn)行路徑規(guī)劃，重復(fù)步驟(3) (4)。
9.根據(jù)權(quán)利要求1所述用于三維空間中移動(dòng)機(jī)械臂路徑規(guī)劃的主從式協(xié)進(jìn)化方法，其特征在于，所述步驟(8)具體為評(píng)估機(jī)械臂和移動(dòng)平臺(tái)的整體性能
10.根據(jù)權(quán)利要求1所述用于三維空間中移動(dòng)機(jī)械臂路徑規(guī)劃的主從式協(xié)進(jìn)化方法， 其特征在于，所述步驟(9)具體為將最終所得結(jié)果應(yīng)用于實(shí)際移動(dòng)機(jī)械臂平臺(tái)一個(gè)六關(guān)節(jié)機(jī)械臂Μ0Τ0ΜΑΝ和一個(gè)滿足非完整約束的輪式移動(dòng)平臺(tái)P0WERB0T，使其可以按較優(yōu)路徑從起始態(tài)到達(dá)目標(biāo)態(tài)，并避開空間中的2個(gè)障礙物和地面上的3個(gè)障礙物。
全文摘要
本發(fā)明公開了一種用于三維空間中移動(dòng)機(jī)械臂路徑規(guī)劃的主從式協(xié)進(jìn)化方法，該方法能夠保證達(dá)到系統(tǒng)整體性能而不是子系統(tǒng)性能最優(yōu)的效果；尤其是在既有空間障礙物又有地面障礙物的情況下能有效避免碰撞并找到一條較優(yōu)的路徑。
文檔編號(hào)B25J9/02GK102161198SQ201110064969
公開日2011年8月24日 申請(qǐng)日期2011年3月18日 優(yōu)先權(quán)日2011年3月18日
發(fā)明者劉山, 馬遙 申請(qǐng)人:浙江大學(xué)