• <tr id="yyy80"></tr>
  • <sup id="yyy80"></sup>
  • <tfoot id="yyy80"><noscript id="yyy80"></noscript></tfoot>
  • 99热精品在线国产_美女午夜性视频免费_国产精品国产高清国产av_av欧美777_自拍偷自拍亚洲精品老妇_亚洲熟女精品中文字幕_www日本黄色视频网_国产精品野战在线观看 ?

    基于動(dòng)態(tài)種群分布的雙變異優(yōu)化FastSLAM算法的改進(jìn)

    2020-12-08 03:48:34梁雪慧張瑞杰程云澤
    化工自動(dòng)化及儀表 2020年6期
    關(guān)鍵詞:位姿適應(yīng)度無(wú)人

    梁雪慧 張瑞杰 趙 菲 程云澤

    (天津理工大學(xué)電氣電子工程學(xué)院)

    無(wú)人駕駛車(chē)輛最重要的功能之一是建立周?chē)h(huán)境的地圖并且通過(guò)地圖進(jìn)行導(dǎo)航,這通常被稱(chēng)為SLAM (同時(shí)定位與建圖)[1]。 然而, 在實(shí)施SLAM時(shí)同時(shí)讓無(wú)人車(chē)進(jìn)行定位和建立地圖存在一定的困難,難點(diǎn)在于如果要使無(wú)人車(chē)知道其真實(shí)位置, 必須在定位之前構(gòu)建非常精確的地圖;但是在建立精確的地圖之前,車(chē)輛必須準(zhǔn)確對(duì)自身定位,否則會(huì)導(dǎo)致誤差不斷累積從而影響結(jié)果的精確性。 車(chē)輛在行駛過(guò)程中的任何摩擦、控制損失或者小障礙都往往是導(dǎo)致錯(cuò)誤測(cè)距的原因,從而對(duì)真實(shí)位置估計(jì)不當(dāng)。

    SLAM算法最早由Smith R等提出[2],它利用擴(kuò)展卡爾曼濾波估計(jì)機(jī)器人的位姿、路標(biāo)特征和聯(lián)合分布,但由于在實(shí)際環(huán)境中噪聲與環(huán)境狀態(tài)多為非線性參量,且計(jì)算量會(huì)隨著迭代次數(shù)的增加急劇上升, 因此Martinez-Cantin R和Castellanos J A又提出了基于無(wú)跡卡爾曼濾波的SLAM 算法[3],雖然具有一定的調(diào)節(jié)能力,但是算法只能用于高斯環(huán)境。為了不局限于高斯環(huán)境,Doucet A等提出了基于粒子濾波器 (Rao-Blackwellized)的SLAM算法[4],從而打破了線性和高斯環(huán)境的限制。另一方面,在此基礎(chǔ)上Montemerlo M等又提出了針對(duì)特征地圖的FastSLAM算法[5],把SLAM分解為兩個(gè)“獨(dú)立”的問(wèn)題,并分別采用不同的濾波方式使算法能夠在大規(guī)模復(fù)雜的環(huán)境中運(yùn)用。 但是在計(jì)算過(guò)程中, 隨著粒子種群不斷更新權(quán)重,這其中有大部分的粒子權(quán)重會(huì)非常小,甚至權(quán)重會(huì)集中在種群中的幾個(gè)甚至單一粒子上,最終出現(xiàn)“粒子退化”的問(wèn)題。 粒子濾波中的重采樣雖然可以降低粒子的退化程度, 提升有效粒子數(shù)目,但是會(huì)出現(xiàn)一些問(wèn)題,一方面使得粒子的多樣性有所減弱,另一方面會(huì)帶來(lái)粒子的衰竭,計(jì)算后只有權(quán)重最大的粒子生存下來(lái), 影響結(jié)果的精度。Sim R等提出的粒子群優(yōu)化算法(PSO)雖然已經(jīng)廣泛應(yīng)用于各種群智能問(wèn)題[6],但仍然很難保證粒子的多樣性和結(jié)果的精確性。 為了解決這些問(wèn)題,寧國(guó)忠提出了一種自適應(yīng)的粒子群算法(APSO)[7], 該算法能夠根據(jù)運(yùn)算過(guò)程中的粒子種群收斂情況自適應(yīng)調(diào)節(jié)全局概率,加強(qiáng)全局最優(yōu)粒子的局部搜索性能, 從而避免粒子陷入局部最優(yōu),提高計(jì)算精度。

    目前已經(jīng)有學(xué)者指出APSO算法不一定能得出問(wèn)題的最優(yōu)解[7],一方面是粒子種群是固定不變的,假若粒子種群規(guī)模較大,則會(huì)很大程度上增加計(jì)算量; 另一方面是粒子群陷入局部最優(yōu)時(shí),隨著計(jì)算的不斷進(jìn)行,搜索范圍會(huì)越來(lái)越小,這兩方面的問(wèn)題往往會(huì)導(dǎo)致結(jié)果不理想甚至失敗。 筆者采用種群動(dòng)態(tài)分布策略,并且引入雙變異算子,將它應(yīng)用到無(wú)人車(chē)的FastSLAM中,提出一種基于種群動(dòng)態(tài)分布雙變異優(yōu)化的FastSLAM算法。 在計(jì)算過(guò)程中對(duì)粒子種群進(jìn)行判定,如果種群收斂,則通過(guò)動(dòng)態(tài)種群策略的調(diào)整對(duì)粒子種群進(jìn)行增加或刪減粒子來(lái)保證粒子多樣性,并且對(duì)種群中適應(yīng)度最差和最優(yōu)的粒子分別進(jìn)行高斯變異,從而加強(qiáng)在其附近搜索的能力,使算法得到理想的最優(yōu)解。 最后,將改進(jìn)后的FastSLAM算法應(yīng)用于無(wú)人駕駛車(chē)輛,并進(jìn)行仿真驗(yàn)證。

    1 FastSLAM算法

    1.1 FastSLAM算法的流程

    基于粒子濾波器和擴(kuò)展卡爾曼濾波的Fast-SLAM算法步驟如下。

    k時(shí)刻粒子集sk由該時(shí)刻所有粒子

    預(yù)測(cè)機(jī)器人位姿, 根據(jù)k-1時(shí)刻粒子集中的每個(gè)粒子及其運(yùn)動(dòng)模型來(lái)計(jì)算k時(shí)刻的粒子集sk。

    計(jì)算權(quán)重,k時(shí)刻第i個(gè)粒子的權(quán)值由k-1時(shí)刻第i個(gè)粒子的權(quán)值和k時(shí)刻第i個(gè)粒子本身的值所估計(jì)的k時(shí)刻粒子位姿zk得出,具體計(jì)算式為:

    根據(jù)觀測(cè)模型計(jì)算粒子集sk-1對(duì)應(yīng)的特征點(diǎn)坐標(biāo),并將它與環(huán)境陸標(biāo)集中的某一陸標(biāo)mk進(jìn)行關(guān)聯(lián)。

    對(duì)粒子集權(quán)值進(jìn)行更新。

    重采樣。

    更新粒子集權(quán)重。

    更新環(huán)境地圖。

    更新粒子k時(shí)刻的位姿,若無(wú)人車(chē)?yán)^續(xù)運(yùn)行,則轉(zhuǎn)到預(yù)測(cè)機(jī)器人位姿步驟,否則結(jié)束定位。

    1.2 FastSLAM算法的缺陷

    FastSLAM由于算法本身的估計(jì)誤差會(huì)隨著時(shí)間的推移而累積[8],因此產(chǎn)生了廣泛存在的權(quán)值集中在少數(shù)粒子、粒子的多樣性無(wú)法得到保證等問(wèn)題。 粒子退化意味著計(jì)算效率在大部分對(duì)于計(jì)算結(jié)果僅有微小作用的粒子影響下會(huì)不斷降低,且由于誤差的累積而導(dǎo)致結(jié)果精度也受到影響。 但是每一個(gè)粒子所包含的位姿信息是所有時(shí)刻的位姿信息,然而,算法每一次的迭代過(guò)程只需要當(dāng)即時(shí)刻的位姿信息, 進(jìn)行重采樣之后,雖然淘汰了大多數(shù)權(quán)重小的粒子,但同時(shí)也刪除了這些粒子中所包含的過(guò)去時(shí)刻的信息, 并且,權(quán)重大的粒子被多次復(fù)制,那么復(fù)制出的粒子也許都繼承的同一個(gè)原始粒子的信息,這樣雖然保證了一定數(shù)量權(quán)值大的粒子參與計(jì)算但是大量的粒子位姿信息被刪除,從而會(huì)大幅削弱地圖估計(jì)的精確性,引起粒子缺乏多樣性的問(wèn)題。 這使得FastSLAM算法存在粒子多樣性嚴(yán)重退化的顯著缺點(diǎn)[9]。

    另一個(gè)方面,在FastSLAM算法中粒子種群數(shù)量是固定不變的,如果種群數(shù)量過(guò)大,則會(huì)很大程度上影響運(yùn)算效率; 相反如果種群數(shù)量過(guò)小,則會(huì)導(dǎo)致運(yùn)算得出的最優(yōu)解不是理想結(jié)果,無(wú)人車(chē)實(shí)際路徑與預(yù)設(shè)路徑偏差過(guò)大。

    2 基于種群動(dòng)態(tài)分布的雙變異優(yōu)化FastSLAM算法

    傳統(tǒng)的FastSLAM算法中由于需要對(duì)粒子進(jìn)行重采樣,會(huì)導(dǎo)致種群中只有少數(shù)幾個(gè)粒子甚至單一高權(quán)重粒子參與到后續(xù)計(jì)算中[10],為了改善粒子集的多樣性,同時(shí)避免自適應(yīng)粒子群優(yōu)化算法中容易出現(xiàn)的局部最優(yōu)現(xiàn)象,使無(wú)人車(chē)的狀態(tài)估計(jì)更加準(zhǔn)確,筆者將種群動(dòng)態(tài)分布策略和動(dòng)態(tài)雙變異粒子群算法引入到FastSLAM算法中,在保證計(jì)算效率和結(jié)果精度的前提下得到更加優(yōu)秀的粒子集[11]。

    2.1 動(dòng)態(tài)種群分布策略

    針對(duì)傳統(tǒng)FastSLAM算法中粒子種群固定不變的特性,引入了動(dòng)態(tài)種群分布策略來(lái)提高優(yōu)化性能。 加入動(dòng)態(tài)種群分布策略之后,種群規(guī)模、慣性權(quán)重和學(xué)習(xí)因子都在搜索過(guò)程中能自適應(yīng)地改變數(shù)值大小來(lái)進(jìn)一步優(yōu)化算法。 在計(jì)算過(guò)程中, 提出一種新的實(shí)時(shí)種群分布估計(jì)(Particle Swarm Distribution Estimation,PSDE), 根據(jù)粒子種群的分布情況進(jìn)行判定,共有兩個(gè)狀態(tài)——收斂和分散,按照動(dòng)態(tài)種群分布策略的設(shè)計(jì),如果當(dāng)前粒子種群的分布狀態(tài)為收斂,那么種群則增加一個(gè)粒子來(lái)增加粒子群的多樣性;如果判定粒子種群此時(shí)刻的狀態(tài)為分散,那么種群減少一個(gè)粒子,這樣在保證采樣多樣性的同時(shí)也減少了運(yùn)算時(shí)間,提高了運(yùn)算效率;如果判定粒子種群為不收斂,則種群減少一個(gè)粒子來(lái)降低計(jì)算次數(shù)和運(yùn)算時(shí)間。

    根據(jù)PSDE,計(jì)算每一代粒子到全局適應(yīng)度最優(yōu)粒子的距離時(shí),將它與全局適應(yīng)度最差粒子到最優(yōu)粒子之間的距離進(jìn)行比較,假如距離大于最優(yōu)與最差粒子距離的粒子數(shù)量大于粒子總數(shù)的60%,則種群此時(shí)的狀態(tài)為分散,那么種群將隨機(jī)刪除一個(gè)除全局最優(yōu)粒子之外的粒子; 反之,假如距離小于最優(yōu)與最差粒子距離的粒子數(shù)量大于粒子總數(shù)的60%, 那么種群此時(shí)的狀態(tài)判定為收斂, 且極有可能已經(jīng)陷入局部最優(yōu)的狀態(tài),此時(shí)將增加一個(gè)粒子,增加的粒子由下式?jīng)Q定:

    其中,newPos為增加粒子的位置,Xmax和Xmin分別是搜索空間的上、 下限,gbest為全局最優(yōu)粒子,Gaussian是一個(gè)均值為μ(μ=0)、標(biāo)準(zhǔn)差為σ的高斯分布隨機(jī)數(shù)。

    2.2 動(dòng)態(tài)雙變異優(yōu)化

    在PSDE-FastSLAM算法中針對(duì)粒子種群的分布做出了動(dòng)態(tài)調(diào)整, 但為了避免出現(xiàn)如圖1所示的粒子分布(狀態(tài)估計(jì)判定為分散,位姿信息全部集中在某一個(gè)或者少數(shù)幾個(gè)粒子上), 加入了兩個(gè)變異算子(Dynamic Dual Mutation,DD)[12],通過(guò)對(duì)適應(yīng)度最優(yōu)的粒子和適應(yīng)度最差的粒子進(jìn)行高斯變異來(lái)控制算法, 提高算法的搜索能力,同時(shí)避免動(dòng)態(tài)種群分布策略產(chǎn)生的過(guò)度分散的情況。

    圖1 粒子種群過(guò)度分散

    加入的第1個(gè)變異算子是最差適應(yīng)度變異算子,對(duì)適應(yīng)度最差的粒子在迭代的每一代都進(jìn)行較大概率的高斯變異, 用來(lái)增加粒子的搜索空間。 在該算法中,對(duì)最差適應(yīng)度粒子以變異概率pm1進(jìn) 行 變 異,pm1值 根 據(jù) 經(jīng) 驗(yàn) 確 定, 一 般 在[0.08,0.50]取值,粒子變異過(guò)程為:

    其中,xi為變異后的新粒子,gworsti為變異前的最差適應(yīng)度粒子,ζ為服從高斯分布的隨機(jī)量。

    第2個(gè)變異算子是最優(yōu)適應(yīng)度變異算子,隨著計(jì)算的不斷進(jìn)行,粒子種群的分布逐漸趨于收斂,運(yùn)算后期算法出現(xiàn)局部早熟收斂甚至最優(yōu)值不再進(jìn)行更新,于是對(duì)最優(yōu)適應(yīng)度粒子以變異概率pm2進(jìn)行變異,pm2值也根據(jù)經(jīng)驗(yàn)確定, 一般在[0.01,0.06]取值,粒子變異過(guò)程為:

    其中,gbesti為變異前的最優(yōu)適應(yīng)度粒子,γ為服從高斯分布的隨機(jī)量。

    2.3 PSDE-FastSLAM-DD算法的實(shí)現(xiàn)

    PSDE-FastSLAM-DD算法的實(shí)現(xiàn)步驟如下:

    a. 根據(jù)初始坐標(biāo),生成具有Num個(gè)粒子的粒子集s0和相應(yīng)的權(quán)值ω0,如前文式(1)~(3);

    b. 計(jì)算粒子適應(yīng)度值,根據(jù)適應(yīng)度來(lái)選擇局部歷史最優(yōu)適應(yīng)度粒子和全局最優(yōu)適應(yīng)度粒子;

    c. 進(jìn)行PSDE步驟;

    d. 對(duì)最差適應(yīng)度粒子進(jìn)行高斯變異,幫助粒子種群進(jìn)行收斂,避免過(guò)度分散;

    e. 更新粒子適應(yīng)度以及局部歷史最優(yōu)適應(yīng)度粒子、全局最優(yōu)適應(yīng)度粒子;

    f. 對(duì)最優(yōu)適應(yīng)度粒子進(jìn)行高斯變異, 幫助粒子種群跳出局部收斂狀態(tài),保證運(yùn)算結(jié)果的精確性;

    g. 更新環(huán)境地圖;

    h. 計(jì)算粒子k時(shí)刻的位姿, 若無(wú)人車(chē)?yán)^續(xù)運(yùn)行,則轉(zhuǎn)到步驟b,否則結(jié)束更新。

    3 仿真結(jié)果及實(shí)驗(yàn)分析

    3.1 仿真模型

    無(wú)人車(chē)的里程計(jì)數(shù)學(xué)模型如下[13,14]:

    其中,Xk+1表示無(wú)人車(chē)在k+1時(shí)刻的位姿狀態(tài),uk+1表示無(wú)人車(chē)在k+1時(shí)刻里程計(jì)的控制輸入,θk為k時(shí)刻無(wú)人車(chē)轉(zhuǎn)過(guò)的航向角,xk和yk分別為k時(shí)刻無(wú)人車(chē)的坐標(biāo)值,D為無(wú)人車(chē)的車(chē)長(zhǎng),Δdk+1為單位時(shí)間內(nèi)無(wú)人車(chē)走過(guò)的路徑長(zhǎng)度,Δθk+1為單位時(shí)間內(nèi)無(wú)人車(chē)的偏轉(zhuǎn)角度。

    觀測(cè)模型為:

    其中,r和θ分別表示檢測(cè)到的環(huán)境特征與無(wú)人車(chē)之間的距離和運(yùn)動(dòng)航向角,xl和yl表示里程計(jì)接收的路標(biāo)信息,ψk表示觀測(cè)噪聲。

    3.2 MATLAB結(jié)果分析

    3.2.1 MATLAB仿真環(huán)境

    為了驗(yàn)證PSDE-FastSLAM-DD算法的實(shí)際有效性,在實(shí)驗(yàn)中,針對(duì)天津理工大學(xué)實(shí)際地圖來(lái)創(chuàng)建仿真環(huán)境, 選取學(xué)校東門(mén)作為路徑起點(diǎn),無(wú)人車(chē)車(chē)輛途徑體育館, 最后到達(dá)機(jī)械工程學(xué)院,對(duì)此創(chuàng)建了如圖2所示的一個(gè)基于路標(biāo)點(diǎn)和導(dǎo)航點(diǎn)的仿真環(huán)境, 環(huán)境中設(shè)置34個(gè)路標(biāo),24個(gè)導(dǎo)航點(diǎn)。

    圖2 仿真路徑

    仿真中時(shí)間ΔT=0.025s, 運(yùn)動(dòng)過(guò)程噪聲為0.3m/s,仿真過(guò)程中取20個(gè)粒子,仿真結(jié)果如圖3所示。

    圖3 仿真結(jié)果

    在圖3中,黑色方塊為仿真對(duì)象無(wú)人車(chē),綠色星號(hào)點(diǎn)為路標(biāo)點(diǎn),紅色圓圈點(diǎn)為導(dǎo)航點(diǎn),紅色細(xì)實(shí)線為預(yù)設(shè)路徑,藍(lán)色實(shí)線為無(wú)人車(chē)的實(shí)際行駛路徑, 黃色顯示為無(wú)人車(chē)在該點(diǎn)監(jiān)測(cè)到的路標(biāo)點(diǎn),從仿真結(jié)果可以看出,無(wú)人車(chē)基本按照預(yù)設(shè)路徑完成了行駛過(guò)程。

    3.2.2 仿真參數(shù)的選取

    筆者在MATLAB仿真平臺(tái)上采用獨(dú)立實(shí)驗(yàn)分別 對(duì) 傳 統(tǒng) 的FastSLAM 算 法、 基 于APSO 的FastSLAM算法和改進(jìn)后的PSDE-FastSLAM-DD算法這3種不同的算法進(jìn)行仿真對(duì)比分析來(lái)驗(yàn)證改進(jìn)后算法在實(shí)際應(yīng)用中的有效性。

    采用一個(gè)樣本空間, 粒子種群規(guī)模設(shè)定為30,算法迭代500次結(jié)束,選取Rastrigrin為測(cè)試函數(shù),針對(duì)算法的均最小值、均耗時(shí)兩個(gè)方面進(jìn)行測(cè)試,從測(cè)試結(jié)果中得到最優(yōu)的參數(shù)。

    由測(cè)試結(jié)果表1可得,選取最差適應(yīng)度粒子的變異概率pm1為0.20, 最優(yōu)適應(yīng)度粒子的變異概率pm2為0.03, 其他變異概率的取值都不利于算法的收斂。

    表1 變異概率對(duì)算法性能影響的測(cè)試結(jié)果

    3.2.3 3種算法對(duì)比

    為了驗(yàn)證算法的優(yōu)越性, 采用一個(gè)樣本空間, 對(duì)比了傳統(tǒng)的FastSLAM算法、 基于APSO的FastSLAM算法和PSDE-FastSLAM-DD算法的無(wú)人車(chē)實(shí)際運(yùn)動(dòng)路徑與預(yù)設(shè)路徑之間的偏差,結(jié)果如圖4所示,結(jié)果顯示PSDE-FastSLAM-DD算法的無(wú)人車(chē)的運(yùn)動(dòng)路徑與真實(shí)值相比偏差最小,說(shuō)明它具有較高的計(jì)算精度。

    圖4 3種算法路徑偏差的對(duì)比

    實(shí)驗(yàn)又對(duì)比了3種算法的估計(jì)精度和運(yùn)行時(shí)間(表2)。 由表2可知,改進(jìn)后的PSDE-FastSLAMDD算法不僅估計(jì)精度比其他兩種算法有所提高,運(yùn)行效率也有了提升。

    表2 3種算法估計(jì)精度和運(yùn)行時(shí)間

    4 結(jié)束語(yǔ)

    在傳統(tǒng)的FastSLAM算法中, 對(duì)粒子集進(jìn)行重采樣會(huì)引起“粒子耗盡”問(wèn)題,而單一的PSO算法會(huì)隨著運(yùn)算而發(fā)生“局部最優(yōu)”的情況,若采用APSO算法來(lái)替代FastSLAM算法中的粒子濾波同樣難以保證粒子種群不陷入 “局部收斂” 的情況。 針對(duì)這兩種算法的特點(diǎn), 改進(jìn)了FastSLAM算法, 在算法初期運(yùn)用動(dòng)態(tài)種群分布策略根據(jù)粒子群的實(shí)時(shí)進(jìn)化狀態(tài)來(lái)改變其規(guī)模, 并且在運(yùn)算中加入針對(duì)最優(yōu)適應(yīng)度粒子和最差適應(yīng)度粒子的變異算子, 增強(qiáng)這兩種粒子附近的搜索范圍,既避免陷入了“局部最優(yōu)”,又避免了粒子群過(guò)度分散、 粒子信息單一的缺陷, 保證了粒子的多樣性。 理論分析和實(shí)驗(yàn)表明,改進(jìn)后的PSDE-FastSLAM-DD算法在估計(jì)精度和運(yùn)行效率方面的性能都較為突出, 保障了無(wú)人車(chē)的實(shí)時(shí)精準(zhǔn)運(yùn)行。

    猜你喜歡
    位姿適應(yīng)度無(wú)人
    改進(jìn)的自適應(yīng)復(fù)制、交叉和突變遺傳算法
    無(wú)人戰(zhàn)士無(wú)人車(chē)
    反擊無(wú)人機(jī)
    詩(shī)到無(wú)人愛(ài)處工
    無(wú)人超市會(huì)流行起來(lái)嗎?
    基于共面直線迭代加權(quán)最小二乘的相機(jī)位姿估計(jì)
    基于CAD模型的單目六自由度位姿測(cè)量
    基于空調(diào)導(dǎo)風(fēng)板成型工藝的Kriging模型適應(yīng)度研究
    小型四旋翼飛行器位姿建模及其仿真
    基于幾何特征的快速位姿識(shí)別算法研究
    九龙城区| 闵行区| 洛浦县| 阳谷县| 方城县| 德昌县| 竹溪县| 吴川市| 武胜县| 甘孜县| 姚安县| 千阳县| 泾源县| 饶平县| 孟津县| 山东省| 诸暨市| 靖江市| 承德县| 新田县| 永嘉县| 镇沅| 武鸣县| 嵊泗县| 甘谷县| 汤阴县| 长春市| 富民县| 平阳县| 察隅县| 黄山市| 福清市| 安塞县| 泗阳县| 红河县| 西丰县| 赤壁市| 五常市| 红安县| 南江县| 棋牌|