本發(fā)明涉及蛇形機(jī)器人的控制領(lǐng)域，特別是涉及基于遺傳模擬退火算法的蛇形機(jī)器人運(yùn)動(dòng)控制方法。

背景技術(shù)：

生物蛇在大自然中是一種獨(dú)特的動(dòng)物，它相對(duì)于其它的動(dòng)物來說，最特別的地方在于它不需要腿的配合就可以完成各種所需的運(yùn)動(dòng)，如前進(jìn)、后退、翻滾、蜿蜒、攀爬等。在各種復(fù)雜的環(huán)境中，如沙漠、泥濘、湖泊、草叢等，生物蛇由于其獨(dú)特的機(jī)構(gòu)及生理特點(diǎn)，使它可以適應(yīng)各種不同的環(huán)境，在各種復(fù)雜的環(huán)境中運(yùn)動(dòng)自如。

蛇形機(jī)器人是一種模仿生物蛇、具有獨(dú)特結(jié)構(gòu)形式的仿生機(jī)器人。它與一般的足式機(jī)器人、輪式或履帶式機(jī)器人相比，最大的特點(diǎn)就是它沒有腿輪的結(jié)構(gòu)，它的運(yùn)動(dòng)完完全全就是靠機(jī)體與環(huán)境的接觸摩擦以及各個(gè)關(guān)節(jié)間運(yùn)動(dòng)的配合來實(shí)現(xiàn)的。其它結(jié)構(gòu)形式的機(jī)器人具有運(yùn)動(dòng)速度快的特點(diǎn)，但是不能很好地適應(yīng)各種復(fù)雜的環(huán)境，因此運(yùn)用的范圍有限。蛇形機(jī)器人具備了其它結(jié)構(gòu)形式的機(jī)器人所不具備的優(yōu)勢。蛇形機(jī)器人可以在各種復(fù)雜的環(huán)境下進(jìn)行運(yùn)動(dòng)，如管道、沼澤、夾縫等，因此蛇形機(jī)器人的運(yùn)用范圍更廣。

此外，其它結(jié)構(gòu)形式的機(jī)器人大多是靠輪子驅(qū)動(dòng)，或者腿和足來移動(dòng)，運(yùn)動(dòng)方式比較單一，運(yùn)動(dòng)形式相對(duì)簡單。而蛇形機(jī)器人采用模塊化的設(shè)計(jì)，可以實(shí)現(xiàn)各種二維和三維的運(yùn)動(dòng)，如蜿蜒、翻滾、行波、攀爬等，運(yùn)動(dòng)方式多樣，運(yùn)動(dòng)形式復(fù)雜。為了實(shí)現(xiàn)蛇形機(jī)器人多種多樣的運(yùn)動(dòng)，必須對(duì)其運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)進(jìn)行理論上的深入研究，因此蛇形機(jī)器人具有理論研究價(jià)值。目前的技術(shù)中，雖然也有涉及到蛇形機(jī)器人的一些運(yùn)動(dòng)控制，但是其進(jìn)行逆解的過程較為復(fù)雜，精度較低，而且可靠性較差，導(dǎo)致無法有效地對(duì)蛇形機(jī)器人進(jìn)行運(yùn)動(dòng)控制。

技術(shù)實(shí)現(xiàn)要素：

為了解決上述的技術(shù)問題，本發(fā)明的目的是提供基于遺傳模擬退火算法的蛇形機(jī)器人運(yùn)動(dòng)控制方法。

本發(fā)明解決其技術(shù)問題所采用的技術(shù)方案是：

基于遺傳模擬退火算法的蛇形機(jī)器人運(yùn)動(dòng)控制方法，包括步驟：

獲取蛇形機(jī)器人首關(guān)節(jié)的目標(biāo)位置和目標(biāo)姿態(tài)；

采用遺傳模擬退火算法對(duì)蛇形機(jī)器人進(jìn)行逆解，求解獲得蛇形機(jī)器人的各關(guān)節(jié)轉(zhuǎn)角；

蛇形機(jī)器人的驅(qū)動(dòng)器獲取求解獲得的各關(guān)節(jié)轉(zhuǎn)角并控制各關(guān)節(jié)進(jìn)行轉(zhuǎn)動(dòng)。

進(jìn)一步，所述采用遺傳模擬退火算法對(duì)蛇形機(jī)器人進(jìn)行逆解，求解獲得蛇形機(jī)器人的各關(guān)節(jié)轉(zhuǎn)角的步驟，包括：

S1、初始化遺傳模擬退火算法的初始種群、初始溫度以及退溫操作函數(shù)；

S2、計(jì)算種群中每一個(gè)個(gè)體的被選中概率，并根據(jù)計(jì)算出的被選中概率對(duì)種群進(jìn)行隨機(jī)選擇；

S3、對(duì)隨機(jī)選擇后的種群進(jìn)行交叉操作后，再進(jìn)行變異操作；

S4、判斷是否滿足預(yù)設(shè)的停止準(zhǔn)則，若是，則結(jié)束遺傳算法并輸出最優(yōu)個(gè)體作為蛇形機(jī)器人的轉(zhuǎn)角矩陣，反之，對(duì)種群中的個(gè)體進(jìn)行更新；

S5、更新迭代參數(shù)，將遺傳模擬退火算法的溫度t_k下降、遺傳代數(shù)k增加以及種群數(shù)組更新后，返回步驟S2進(jìn)行迭代。

進(jìn)一步，所述步驟S1，包括：

S11、根據(jù)蛇形機(jī)器人的關(guān)節(jié)數(shù)，生成遺傳模擬退火算法的初始種群，所述初始總?cè)菏怯上率降某跏紓€(gè)體構(gòu)成的：

上式中，n表示蛇形機(jī)器人的關(guān)節(jié)數(shù)，R表示變異因子，且R中的每個(gè)元素均為隨機(jī)生成的處于[0,1]范圍內(nèi)的隨機(jī)數(shù)，Q表示解空間的向量，且Q中的第i個(gè)元素滿足下式：

S12、響應(yīng)于用戶輸入的數(shù)據(jù)初始化遺傳模擬退火算法的初始溫度，并設(shè)定如下式的退溫操作函數(shù)：

t_k+1＝λ·t_k

上式中，t_k表示當(dāng)前溫度，t_k+1表示退溫操作后的溫度，λ表示衰減系數(shù)，其取值范圍為0.5～0.99。

進(jìn)一步，所述步驟S2，包括：

S21、設(shè)定遺傳模擬退火算法的適應(yīng)度函數(shù)；

S22、根據(jù)適應(yīng)度函數(shù)，采用下式計(jì)算種群中每一個(gè)個(gè)體的被選中概率，并根據(jù)計(jì)算出的被選中概率對(duì)種群進(jìn)行隨機(jī)選擇：

其中，f_min代表種群中個(gè)體適應(yīng)度的最小值，f(i)代表第i個(gè)個(gè)體的適應(yīng)度值，f_i(t_k)代表在當(dāng)前溫度t_k下該個(gè)體被選中的概率。

進(jìn)一步，所述步驟S21，其具體為：

根據(jù)下式，設(shè)定遺傳模擬退火算法的適應(yīng)度函數(shù)f(Q)：

其中，ΔE(Q)表示預(yù)設(shè)的目標(biāo)函數(shù)，其表達(dá)式如下：

上式中，P_j和θ_i分別代表蛇形機(jī)器人的齊次變化矩陣⁰T_n的位置元素和姿態(tài)元素，和分別表示蛇形機(jī)器人的目標(biāo)位置和目標(biāo)姿態(tài)，α和β均表示預(yù)設(shè)的權(quán)重因子。

進(jìn)一步，所述步驟S3，包括：

S31、按照設(shè)定的交叉率，采用賭輪盤法對(duì)種群中的個(gè)體進(jìn)行選擇；

S32、對(duì)選擇后的種群進(jìn)行交叉操作；

S33、對(duì)交叉操作后的種群進(jìn)行變異操作。

進(jìn)一步，所述步驟S31，包括：

S311、按照設(shè)定的交叉率P_c在種群中進(jìn)行搜索，計(jì)算出每一個(gè)個(gè)體的適應(yīng)度值后，按從大到小的順序進(jìn)行排序，所述交叉率P_c為：

P_c＝0.7-w×N/t_k

其中，w代表比例系數(shù)，取值在0到1之間，N為種群規(guī)模；

S312、根據(jù)下式計(jì)算每一個(gè)個(gè)體的適應(yīng)度值占總適應(yīng)度值的比例P_k(Q_i)后，組成賭輪盤：

上式中，f(Q_i)表示第i個(gè)個(gè)體的適應(yīng)度值；

S313、隨機(jī)產(chǎn)生一個(gè)0～1之間的隨機(jī)數(shù)，根據(jù)隨機(jī)數(shù)在賭輪盤上所處的群尖選擇對(duì)應(yīng)的個(gè)體；

S314、重復(fù)執(zhí)行步驟S313直到選取的個(gè)體達(dá)到種群容量的最大值。

進(jìn)一步，所述步驟S32，包括：

S321、從選擇后的種群中選擇兩個(gè)個(gè)體作為父代個(gè)體，進(jìn)行染色體交換從而產(chǎn)生子代個(gè)體；

S322、根據(jù)下式計(jì)算出子代的變異因子R^*：

上式中，下標(biāo)1、2表示個(gè)體的序號(hào)，R表示父代個(gè)體的變異因子，rand()是0～1間的隨機(jī)數(shù)；

S323、根據(jù)下式計(jì)算新的解向量Q^*：

上式中，下標(biāo)1、2表示個(gè)體的序號(hào)，Q表示父代個(gè)體的解向量。

進(jìn)一步，所述步驟S33，包括：

S331、對(duì)交叉操作后的種群，在其個(gè)體中按照設(shè)定的變異概率P_m隨機(jī)選擇一個(gè)個(gè)體Q_j(θ_j1,θ_j2,…,θ_ji,…,θ_jn)，所述變異概率P_m為：

P_m＝t_k/K

其中，K為常數(shù)，滿足在算法開始時(shí)讓P_m等于0.7；

S332、通過下式對(duì)選擇的個(gè)體進(jìn)行變異：

其中，P_s為變異系數(shù)，其表達(dá)式為：

P_s＝D+(f_ave-f(Q_j))/f_ave

上式中，D＝(f_max-f_ave)/f_max，f_max為本代個(gè)體中適應(yīng)度的最大值，f_ave為本代個(gè)體中適應(yīng)度的平均值。

進(jìn)一步，所述步驟S4中所述判斷是否滿足預(yù)設(shè)的停止準(zhǔn)則的步驟，其具體為：

判斷是否達(dá)到最大遺傳代數(shù)，或者是否滿足以下條件：

上式中，f(Q_i^*)表示第i個(gè)個(gè)體的適應(yīng)度值，Q^*表示變異操作后的新的解向量。

本發(fā)明的有益效果是：本發(fā)明的基于遺傳模擬退火算法的蛇形機(jī)器人運(yùn)動(dòng)控制方法，包括步驟：獲取蛇形機(jī)器人首關(guān)節(jié)的目標(biāo)位置和目標(biāo)姿態(tài)；采用遺傳模擬退火算法對(duì)蛇形機(jī)器人進(jìn)行逆解，求解獲得蛇形機(jī)器人的各關(guān)節(jié)轉(zhuǎn)角；蛇形機(jī)器人的驅(qū)動(dòng)器獲取求解獲得的各關(guān)節(jié)轉(zhuǎn)角并控制各關(guān)節(jié)進(jìn)行轉(zhuǎn)動(dòng)。本方法通過遺傳模擬退火算法對(duì)蛇形機(jī)器人進(jìn)行逆解，用遺傳算法的搜索策略彌補(bǔ)模擬退火算法的精度問題，用模擬退火算法的全局搜索能力避免遺傳算法陷入局部最優(yōu)的困境。遺傳模擬退火算法把兩種算法的優(yōu)勢有機(jī)地結(jié)合起來，不僅能使算法的效率得到提高，還能增強(qiáng)算法的全局把控能力?；诖怂惴▓?zhí)行的蛇形機(jī)器人運(yùn)動(dòng)控制方法，精確度高，可靠性較高，可以有效地對(duì)蛇形機(jī)器人進(jìn)行運(yùn)動(dòng)控制。

附圖說明

下面結(jié)合附圖和實(shí)施例對(duì)本發(fā)明作進(jìn)一步說明。

圖1是本發(fā)明的基于遺傳模擬退火算法的蛇形機(jī)器人運(yùn)動(dòng)控制方法的流程圖；

圖2是本發(fā)明的基于遺傳模擬退火算法的蛇形機(jī)器人運(yùn)動(dòng)控制方法的蛇形機(jī)器人的逆解步驟的流程圖。

具體實(shí)施方式

參照圖1，本發(fā)明提供了一種基于遺傳模擬退火算法的蛇形機(jī)器人運(yùn)動(dòng)控制方法，包括步驟：

獲取蛇形機(jī)器人首關(guān)節(jié)的目標(biāo)位置和目標(biāo)姿態(tài)；

采用遺傳模擬退火算法對(duì)蛇形機(jī)器人進(jìn)行逆解，求解獲得蛇形機(jī)器人的各關(guān)節(jié)轉(zhuǎn)角；

蛇形機(jī)器人的驅(qū)動(dòng)器獲取求解獲得的各關(guān)節(jié)轉(zhuǎn)角并控制各關(guān)節(jié)進(jìn)行轉(zhuǎn)動(dòng)。求解獲得蛇形機(jī)器人的各關(guān)節(jié)轉(zhuǎn)角后，可以根據(jù)現(xiàn)有技術(shù)的控制水平，控制各關(guān)節(jié)進(jìn)行轉(zhuǎn)動(dòng)。

進(jìn)一步作為優(yōu)選的實(shí)施方式，參照圖2，所述采用遺傳模擬退火算法對(duì)蛇形機(jī)器人進(jìn)行逆解，求解獲得蛇形機(jī)器人的各關(guān)節(jié)轉(zhuǎn)角的步驟，包括：

S1、初始化遺傳模擬退火算法的初始種群、初始溫度以及退溫操作函數(shù)；

S2、計(jì)算種群中每一個(gè)個(gè)體的被選中概率，并根據(jù)計(jì)算出的被選中概率對(duì)種群進(jìn)行隨機(jī)選擇；

S3、對(duì)隨機(jī)選擇后的種群進(jìn)行交叉操作后，再進(jìn)行變異操作；

S4、判斷是否滿足預(yù)設(shè)的停止準(zhǔn)則，若是，則結(jié)束遺傳算法并輸出最優(yōu)個(gè)體作為蛇形機(jī)器人的轉(zhuǎn)角矩陣，反之，對(duì)種群中的個(gè)體進(jìn)行更新；

S5、更新迭代參數(shù)，將遺傳模擬退火算法的溫度t_k下降、遺傳代數(shù)k增加以及種群數(shù)組更新后，返回步驟S2進(jìn)行迭代。

進(jìn)一步作為優(yōu)選的實(shí)施方式，所述步驟S1，包括：

S11、根據(jù)蛇形機(jī)器人的關(guān)節(jié)數(shù)，生成遺傳模擬退火算法的初始種群，所述初始總?cè)菏怯上率降某跏紓€(gè)體構(gòu)成的：

上式中，n表示蛇形機(jī)器人的關(guān)節(jié)數(shù)，R表示變異因子，且R中的每個(gè)元素均為隨機(jī)生成的處于[0,1]范圍內(nèi)的隨機(jī)數(shù)，Q表示解空間的向量，且Q中的第i個(gè)元素滿足下式：

S12、響應(yīng)于用戶輸入的數(shù)據(jù)初始化遺傳模擬退火算法的初始溫度，并設(shè)定如下式的退溫操作函數(shù)：

t_k+1＝λ·t_k

上式中，t_k表示當(dāng)前溫度，t_k+1表示退溫操作后的溫度，λ表示衰減系數(shù)，其取值范圍為0.5～0.99。

進(jìn)一步作為優(yōu)選的實(shí)施方式，所述步驟S2，包括：

S21、設(shè)定遺傳模擬退火算法的適應(yīng)度函數(shù)；

S22、根據(jù)適應(yīng)度函數(shù)，采用下式計(jì)算種群中每一個(gè)個(gè)體的被選中概率，并根據(jù)計(jì)算出的被選中概率對(duì)種群進(jìn)行隨機(jī)選擇：

其中，f_min代表種群中個(gè)體適應(yīng)度的最小值，f(i)代表第i個(gè)個(gè)體的適應(yīng)度值，f_i(t_k)代表在當(dāng)前溫度t_k下該個(gè)體被選中的概率。

進(jìn)一步作為優(yōu)選的實(shí)施方式，所述步驟S21，其具體為：

根據(jù)下式，設(shè)定遺傳模擬退火算法的適應(yīng)度函數(shù)f(Q)：

其中，ΔE(Q)表示預(yù)設(shè)的目標(biāo)函數(shù)，其表達(dá)式如下：

上式中，P_j和θ_i分別代表蛇形機(jī)器人的齊次變化矩陣⁰T_n的位置元素和姿態(tài)元素，和分別表示蛇形機(jī)器人的目標(biāo)位置和目標(biāo)姿態(tài)，α和β均表示預(yù)設(shè)的權(quán)重因子。蛇形機(jī)器人的齊次變化矩陣⁰T_n是根據(jù)旋量理論對(duì)蛇形機(jī)器人進(jìn)行正運(yùn)動(dòng)學(xué)建模所獲得的。

ΔE(Q)越小，適應(yīng)度的值就越大，分母加上0.001是為了避免零分母的出現(xiàn)。

進(jìn)一步作為優(yōu)選的實(shí)施方式，所述步驟S3，包括：

S31、按照設(shè)定的交叉率，采用賭輪盤法對(duì)種群中的個(gè)體進(jìn)行選擇；

S32、對(duì)選擇后的種群進(jìn)行交叉操作；

S33、對(duì)交叉操作后的種群進(jìn)行變異操作。

步驟S3實(shí)際執(zhí)行的是遺傳操作，遺傳操作是整個(gè)算法的關(guān)鍵部分，遺傳操作對(duì)上一代的個(gè)體進(jìn)行選擇，被選擇的個(gè)體然后通過交叉和變異產(chǎn)生下一代的個(gè)體。在這個(gè)過程中充滿了隨機(jī)性與不確定性，但是在全局范圍內(nèi)又要有把控搜索方向能力。遺傳操作包括選擇、交叉和變異三個(gè)步驟，即對(duì)應(yīng)步驟S31～S33。

進(jìn)一步作為優(yōu)選的實(shí)施方式，所述步驟S31，包括：

S311、按照設(shè)定的交叉率P_c在種群中進(jìn)行搜索，計(jì)算出每一個(gè)個(gè)體的適應(yīng)度值后，按從大到小的順序進(jìn)行排序，所述交叉率P_c為：

P_c＝0.7-w×N/t_k

其中，w代表比例系數(shù)，取值在0到1之間，N為種群規(guī)模；

交叉率P_c是交叉操作的過程中一個(gè)很重要的參數(shù)，影響著子代的多樣性以及算法的搜索范圍。如果交叉率P_c取得過高，則容易破壞優(yōu)秀的個(gè)體，不利于算法的收斂。如果交叉率取得過低，則降低了個(gè)體的多樣性使算法陷入局部最優(yōu)，得出的解缺乏質(zhì)量。在算法開始的時(shí)候溫度值較高，交叉率接近于0.7，這時(shí)算法進(jìn)行大范圍的搜索。隨著溫度下降，交叉率也在下降，符合算法后期對(duì)優(yōu)秀個(gè)體的保護(hù)原則但同時(shí)又具有一定的跳變能力。

S312、根據(jù)下式計(jì)算每一個(gè)個(gè)體的適應(yīng)度值占總適應(yīng)度值的比例P_k(Q_i)后，組成賭輪盤：

上式中，f(Q_i)表示第i個(gè)個(gè)體的適應(yīng)度值；

S313、隨機(jī)產(chǎn)生一個(gè)0～1之間的隨機(jī)數(shù)，根據(jù)隨機(jī)數(shù)在賭輪盤上所處的群尖選擇對(duì)應(yīng)的個(gè)體；

S314、重復(fù)執(zhí)行步驟S313直到選取的個(gè)體達(dá)到種群容量的最大值。

進(jìn)一步作為優(yōu)選的實(shí)施方式，所述步驟S32，包括：

S321、從選擇后的種群中選擇兩個(gè)個(gè)體作為父代個(gè)體，進(jìn)行染色體交換從而產(chǎn)生子代個(gè)體；

S322、根據(jù)下式計(jì)算出子代的變異因子R^*：

上式中，下標(biāo)1、2表示個(gè)體的序號(hào)，R表示父代個(gè)體的變異因子，rand()是0～1間的隨機(jī)數(shù)；

S323、根據(jù)下式計(jì)算新的解向量Q^*：

上式中，下標(biāo)1、2表示個(gè)體的序號(hào)，Q表示父代個(gè)體的解向量。

進(jìn)一步作為優(yōu)選的實(shí)施方式，所述步驟S33，包括：

S331、對(duì)交叉操作后的種群，在其個(gè)體中按照設(shè)定的變異概率P_m隨機(jī)選擇一個(gè)個(gè)體Q_j(θ_j1,θ_j2,…,θ_ji,…,θ_jn)，所述變異概率P_m為：

P_m＝t_k/K

其中，K為常數(shù)，滿足在算法開始時(shí)讓P_m等于0.7；

S332、通過下式對(duì)選擇的個(gè)體進(jìn)行變異：

其中，P_s為變異系數(shù)，其表達(dá)式為：

P_s＝D+(f_ave-f(Q_j))/f_ave

上式中，D＝(f_max-f_ave)/f_max，f_max為本代個(gè)體中適應(yīng)度的最大值，f_ave為本代個(gè)體中適應(yīng)度的平均值。

由此可以看出，每個(gè)個(gè)體的變異系數(shù)隨著其適應(yīng)度值而自適應(yīng)變化。適應(yīng)度大的個(gè)體變異系數(shù)就會(huì)變小，而適應(yīng)度小的個(gè)體變異系數(shù)就會(huì)增大，這避免了最優(yōu)解附近的個(gè)體受到破壞。

進(jìn)一步作為優(yōu)選的實(shí)施方式，所述步驟S4中所述判斷是否滿足預(yù)設(shè)的停止準(zhǔn)則的步驟，其具體為：

判斷是否達(dá)到最大遺傳代數(shù)，或者是否滿足以下條件：

上式中，f(Q_i^*)表示第i個(gè)個(gè)體的適應(yīng)度值，Q^*表示變異操作后的新的解向量。因此，算法收斂的判斷條件是達(dá)到最大遺傳代數(shù)，或者，當(dāng)算法迭代過程中出現(xiàn)適應(yīng)度值滿足大于500這個(gè)要求時(shí)算法終止，滿足這個(gè)適應(yīng)度值的個(gè)體就是可以近似看成所求優(yōu)化問題的最優(yōu)解。

本發(fā)明中，遺傳模擬退火算法(GSA)把遺傳算法和模擬退火算法進(jìn)行并行整合，用遺傳算法的搜索策略彌補(bǔ)模擬退火算法的精度問題，用模擬退火算法的全局搜索能力避免遺傳算法陷入局部最優(yōu)的困境。GSA把兩種算法的優(yōu)勢有機(jī)地結(jié)合起來，不僅能使算法的效率得到提高，還能增強(qiáng)算法的全局把控能力?；诖怂惴▓?zhí)行的蛇形機(jī)器人運(yùn)動(dòng)控制方法，精確度高，可靠性較高，可以有效地對(duì)蛇形機(jī)器人進(jìn)行運(yùn)動(dòng)控制。

以上是對(duì)本發(fā)明的較佳實(shí)施進(jìn)行了具體說明，但本發(fā)明創(chuàng)造并不限于所述實(shí)施例，熟悉本領(lǐng)域的技術(shù)人員在不違背本發(fā)明精神的前提下還可做出種種的等同變形或替換，這些等同的變型或替換均包含在本申請權(quán)利要求所限定的范圍內(nèi)。