《移動(dòng)機(jī)器人目標(biāo)定位跟蹤濾波算法研究》9800字

移動(dòng)機(jī)器人目標(biāo)定位跟蹤濾波算法研究目錄TOC\o"1-1"\h\u5972移動(dòng)機(jī)器人目標(biāo)定位跟蹤算法研究 117422第一章緒論 127677第二章目標(biāo)跟蹤模型及濾波器算法理論 430191第三章基于卡爾曼濾波（KF）的目標(biāo)跟蹤研究 1127759第四章基于擴(kuò)展卡爾曼濾波（EKF）的目標(biāo)跟蹤研究 1416185第五章基于無跡卡爾曼濾波（UKF）的目標(biāo)跟蹤研究 183432第六章結(jié)論與展望 21第一章緒論1.1課題背景隨著計(jì)算機(jī)技術(shù)的進(jìn)一步發(fā)展,機(jī)器人在軍用領(lǐng)域和民用領(lǐng)城也會(huì)發(fā)揮巨大的功能。機(jī)器人能夠替代人類進(jìn)行"無聊,重復(fù),危險(xiǎn)"的工作任務(wù),從而減少了人員的工作壓力以及暴露于危險(xiǎn)的環(huán)境條件下的時(shí)間。在當(dāng)今社會(huì),由于科技的迅勐猛進(jìn),機(jī)器人在我們?nèi)粘Ｉ钪性缫褵o處不在,并幾乎走進(jìn)了各行各業(yè),如家庭生活中的清掃機(jī)器人,制造業(yè)中的搬運(yùn)機(jī)器人，，快遞行業(yè)中的分揀機(jī)器人，軍事領(lǐng)域的拆彈機(jī)器人等。特別是隨著世界人口呈現(xiàn)嚴(yán)重的老齡化，人口紅利逐步下降，人工成本加劇，多數(shù)行業(yè)呈現(xiàn)用工難的問題，機(jī)器人取代人類完成工作的需要日益加大。近年來的席卷全球的新冠疫情，更是讓機(jī)器人的無人化，智能化成為熱點(diǎn)話題。移動(dòng)機(jī)器人是機(jī)器人技術(shù)最開始的分支，在軍事，工程，醫(yī)療，教育等方面有著廣闊前景，所以世界上都開始重視研究移動(dòng)機(jī)器人。因此。不論在未來的民用還是軍用方面，若移動(dòng)機(jī)器人能根據(jù)設(shè)定的任務(wù)，準(zhǔn)確的進(jìn)行目標(biāo)定位跟蹤，就可以極大的提高任務(wù)完成效率。1.2研究現(xiàn)狀移動(dòng)機(jī)器人的目標(biāo)定位跟蹤主要的兩個(gè)主要研究方面是：一是地面軌跡跟蹤問題，對自身系統(tǒng)狀態(tài)的估計(jì)，調(diào)整傳感器平臺位置確保對目標(biāo)的覆蓋：二是移動(dòng)機(jī)器人路徑規(guī)劃問題，對為了滿足既定任務(wù)要求，如生成最優(yōu)路徑和規(guī)避威脅區(qū)域等。1.2.1移動(dòng)機(jī)器人目標(biāo)定位跟蹤研究概述目標(biāo)定位跟蹤的要解決的第一個(gè)問題就是目標(biāo)狀態(tài)估計(jì)，是指通過傳感器信息收集系統(tǒng)得到的觀測狀態(tài)和目標(biāo)的觀測信息，從而確定目標(biāo)設(shè)定坐標(biāo)系下信息：位置、方向、速度等。目標(biāo)定位跟蹤的目的是為了，準(zhǔn)確估計(jì)目標(biāo)狀態(tài)，然后移動(dòng)機(jī)器人將收集到的目標(biāo)狀態(tài)估計(jì)值作為其控制模塊的參考。當(dāng)前在目標(biāo)跟蹤定位方面常用的估計(jì)技術(shù)有，a-B和a-B-y法波，Wiener法波卡爾曼濾波，擴(kuò)展卡爾曼濾波，無跡卡爾曼濾波，加權(quán)最小二乘法等濾波器。（1）Wiener濾波：是一種用在頻域設(shè)計(jì)的最優(yōu)秀的濾波器，利用最小均方誤差條件試驗(yàn)和確定濾波器的沖激響應(yīng)H和系統(tǒng)兩數(shù)W，轉(zhuǎn)換為求取方程最優(yōu)解的的問題。Wiener濾波對于一維的穩(wěn)定的信號比較適用。（2）加權(quán)最小二乘法，是一種根據(jù)事先設(shè)定的信息的收集使用的濾波，當(dāng)先驗(yàn)統(tǒng)計(jì)特性未知時(shí)，采用最小二乘法濾波，當(dāng)只知道觀測誤差的統(tǒng)計(jì)特性，選擇馬爾可夫估計(jì)。（3）a-B和a-B-y法波，是兩種工程常用的跟蹤濾波器算法，優(yōu)點(diǎn)是少量的循環(huán)濾波計(jì)算和離線計(jì)算的增益矩陣。（4）卡爾曼濾波，擴(kuò)展卡爾曼濾波，無跡卡爾曼濾波的濾波器將在后續(xù)章節(jié)進(jìn)行詳細(xì)說明。1.2.2各國移動(dòng)機(jī)器人發(fā)展現(xiàn)狀第一是美國，曲線的發(fā)展道路。第一個(gè)工業(yè)機(jī)器人是美國制造的，60年代就有商品機(jī)器人銷售，但是之后沒有重視機(jī)器人技術(shù)，偏重于理論，戰(zhàn)略意識不強(qiáng)，導(dǎo)致在1973年美國開始從日本進(jìn)口機(jī)器人，來滿足自身市場需求。2006年之后，由于機(jī)器人市場份額大，發(fā)展前景好，美國又開始重視機(jī)器人的技術(shù)發(fā)展。第二是日本，技術(shù)和國家經(jīng)濟(jì)發(fā)展相融合的實(shí)用的發(fā)展道路。日本于1967年開始在美國引進(jìn)機(jī)器人，到80年代初，已經(jīng)發(fā)展成為機(jī)器人王國，機(jī)器人眾多，機(jī)器人生產(chǎn)線規(guī)范，工業(yè)體系完善，處于世界領(lǐng)先水品。第三是德國，技術(shù)促進(jìn)經(jīng)濟(jì)的發(fā)展道路。1986年，德國機(jī)器人數(shù)量位列世界前三，超過10000臺.1987年德國已經(jīng)有42家機(jī)器人公司。現(xiàn)在機(jī)器人產(chǎn)業(yè)給德國產(chǎn)生的經(jīng)濟(jì)收入占到國家收入的1.2％。第四是中國，政策作用下的高速發(fā)展道路。2018年開始，5年內(nèi)年均增速接近30%，發(fā)展非常強(qiáng)勁，同時(shí)伴隨著市場規(guī)模的不斷擴(kuò)大、應(yīng)用領(lǐng)域的快速拓展、不斷加快核心部位零件，部分領(lǐng)域的突出，特別是在創(chuàng)新型企業(yè)的大量涌現(xiàn)、企業(yè)，我國移動(dòng)機(jī)器人機(jī)器人在產(chǎn)業(yè)發(fā)展上呈現(xiàn)三個(gè)明顯的特征：是三類機(jī)器人發(fā)展不斷提速。是產(chǎn)業(yè)集群現(xiàn)象愈發(fā)明顯。區(qū)域產(chǎn)業(yè)各自優(yōu)勢加速凸顯。在國內(nèi)的產(chǎn)業(yè)集聚過程中，。1.2.3目標(biāo)定位跟蹤的傳感器移動(dòng)機(jī)器人的目標(biāo)定位跟蹤往往需要滿足較高的定位精度、較低的定位延遲、較高的定位輸出頻率、易融合多種傳感器數(shù)據(jù)，以及安裝部署容易，對環(huán)境改動(dòng)小等要求。激光雷達(dá)傳感器很符合移動(dòng)機(jī)器人目標(biāo)定位跟蹤的要求。激光傳感器就是利用激光技術(shù)來測量信息的一種傳感器。它由三部分組成，一是發(fā)出激光信號的激光器，二是接收激光信號的激光檢測器，三是對收集信息進(jìn)行測量的測量電路。他的顯著特色就速度快，精度高，測量程度高，抵抗光和溫度的能力強(qiáng)。移動(dòng)機(jī)器人定位方面的激光傳感器最多的就是激光雷達(dá)傳感器。激光雷達(dá)數(shù)據(jù)進(jìn)行各種分類識別，進(jìn)行跟蹤處理，能夠非常有效的獲得周圍環(huán)境最主要信息。1.3本文主要工作內(nèi)容及結(jié)構(gòu)安排本文主要研究的是基于卡爾曼濾波的移動(dòng)機(jī)器人目標(biāo)定位跟蹤的方法，基于勻速直線運(yùn)動(dòng)模型，進(jìn)行真實(shí)軌跡的預(yù)測，并且在PC平臺上實(shí)現(xiàn)定位過程的仿真。然后比較卡爾曼濾波，擴(kuò)展卡爾曼濾波，無跡卡爾曼濾波對移動(dòng)機(jī)器人目標(biāo)定位的仿真效果。本文分為六章，各章的具體研究內(nèi)容如下：第一章：簡要介紹了本課題研究的意義和背景，分別介紹了主流國家機(jī)器人發(fā)展現(xiàn)狀和趨勢，以及移動(dòng)機(jī)器人目標(biāo)定位的傳感器，并說明本文的研究內(nèi)容和結(jié)構(gòu)。第二章：詳細(xì)介紹了目標(biāo)跟蹤的濾波原理，重點(diǎn)闡述了卡爾曼濾波器，擴(kuò)展卡爾曼濾波器，無跡卡爾曼濾波器的原理和算法步驟；設(shè)定移動(dòng)機(jī)器人的仿真運(yùn)動(dòng)模型和主要參數(shù)設(shè)置。第三章基于卡爾曼濾波的目標(biāo)跟蹤研究，主要介紹了卡爾曼濾波的算法實(shí)現(xiàn)，基于MATLAB的仿真實(shí)現(xiàn)，并對卡爾曼濾波前后的實(shí)驗(yàn)數(shù)據(jù)進(jìn)行分析和比較。第四章基于擴(kuò)展卡爾曼濾波的目標(biāo)跟蹤研究，主要介紹了擴(kuò)展卡爾曼濾波的算法實(shí)現(xiàn)，基于MATLAB的仿真實(shí)現(xiàn)，并對卡爾曼濾波和EKF的實(shí)驗(yàn)數(shù)據(jù)進(jìn)行分析和比較。第五章基于無跡卡爾曼濾波的目標(biāo)跟蹤研究，主要介紹了無跡卡爾曼濾波的算法實(shí)現(xiàn)，基于MATLAB的仿真實(shí)現(xiàn)，并比較UKF和EKF的實(shí)驗(yàn)數(shù)據(jù)進(jìn)行分析和比較。第六章主要是對全文的總結(jié)，分析實(shí)驗(yàn)不足，并且在之后工作的改進(jìn)。第二章目標(biāo)跟蹤模型及濾波器算法理論2.1機(jī)動(dòng)目標(biāo)跟蹤原理目標(biāo)跟蹤過程為：量測值Y和狀態(tài)預(yù)測向量HX(K+1/k)構(gòu)成殘差向量ｄ，機(jī)動(dòng)檢測或機(jī)動(dòng)辨識根據(jù)ｄ的改變進(jìn)行更新，然后設(shè)定的算法邏輯調(diào)整濾波增益與協(xié)方差矩陣，最后由卡爾曼濾波算法計(jì)算目標(biāo)的系統(tǒng)估計(jì)狀態(tài)和預(yù)測狀態(tài)數(shù)值，就完成了整個(gè)的目標(biāo)跟蹤過程。表2-1機(jī)動(dòng)目標(biāo)跟蹤原理圖如圖2-1所示，圖中目標(biāo)動(dòng)態(tài)特性由包含位置、速度和加速度的狀態(tài)向量x表示，量測值Y被假定為含有量測噪聲V的狀態(tài)向量的線性組合(HX＋V)。殘差向量d為量測值丫與狀態(tài)預(yù)測量HX(k+1/6)之差。2.2建立移動(dòng)機(jī)器人移動(dòng)模型在移動(dòng)機(jī)器人領(lǐng)域中，移動(dòng)機(jī)器人的路徑優(yōu)劣與移動(dòng)機(jī)器人的模型的精準(zhǔn)程度關(guān)系重大。通常把移動(dòng)機(jī)器人當(dāng)成一個(gè)設(shè)定坐標(biāo)系中的質(zhì)點(diǎn)，但是移動(dòng)機(jī)器人的真實(shí)物理特征影響，因此本文采用簡單的勻速直線運(yùn)動(dòng)模型。表2-1坐標(biāo)系統(tǒng)為了實(shí)現(xiàn)更好的實(shí)現(xiàn)移動(dòng)機(jī)器人的定位，我選用的是勻速直線運(yùn)動(dòng)的移動(dòng)機(jī)器人來做運(yùn)動(dòng)的模型，使用二維的平面展示，提供四個(gè)維度的參數(shù)：X方向的機(jī)器人的速度和距離，Y方向的機(jī)器人的速度和距離，即（x,y,V(x),V(y))。2.3濾波器算法理論本節(jié)主要是介紹基于卡爾曼濾波目標(biāo)定位跟蹤中的數(shù)學(xué)原理知識，包含了卡爾曼濾波的數(shù)學(xué)理論知識，擴(kuò)展卡爾曼濾波的數(shù)學(xué)理論知識，無跡卡爾曼濾波的數(shù)學(xué)理論知識，對于線性的目標(biāo)跟蹤用到是卡爾曼濾波，對于非線性的目標(biāo)跟蹤用到EKF和UKF。2.3.1卡爾曼濾波(KF)卡爾曼濾波常用于動(dòng)態(tài)多變化系統(tǒng)中的狀態(tài)估計(jì)，是一種通用性強(qiáng)的自回歸濾波器。其發(fā)明者魯?shù)婪?E.卡爾曼在一次訪問NASA的時(shí)候，發(fā)現(xiàn)阿波羅計(jì)劃中一個(gè)難點(diǎn)，所以提出一種用來預(yù)測軌跡的濾波器，他可以幫助高效預(yù)測軌跡，輔助導(dǎo)航。NASA最終使用了這個(gè)濾波器，完成阿波羅登月任務(wù)，卡爾曼濾波器由此得名。卡爾曼濾波（KF）在數(shù)學(xué)中是用來估計(jì)線性系統(tǒng)最優(yōu)狀態(tài)的一種方法。在移動(dòng)機(jī)器人定位方面主要是通過傳感器的觀測信號，形成的觀測狀態(tài)，和系統(tǒng)算法估計(jì)出來的估計(jì)狀態(tài)進(jìn)行分配比例，從而達(dá)到最接近真實(shí)的實(shí)際定位的參數(shù)，并且配合進(jìn)行位置預(yù)測和觀察系統(tǒng)的輸入輸出數(shù)據(jù)，從而構(gòu)成系統(tǒng)真實(shí)狀態(tài)的線性方程。傳統(tǒng)的卡爾曼濾波算法（FK）適用于高斯分布的系統(tǒng)模型上，不適用于復(fù)雜的非線性系統(tǒng).狀態(tài)方程描述：線性卡爾曼濾波的狀態(tài)方程： X(k+1)=δX(k)+σΓW(k) （2-1）線性卡爾曼濾波的觀測方程： Y(k)=HX（k)+V(k) （2-2）公式（2-1），（2-2）中，ｋ代表的是離散時(shí)間，系統(tǒng)在時(shí)刻k的狀態(tài)就是X(k)，系統(tǒng)在ｋ是時(shí)刻的觀測狀態(tài)為Y（ｋ），系統(tǒng)輸入為W(k)，系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣為δ，系統(tǒng)噪聲驅(qū)動(dòng)舉證為Γ，系統(tǒng)觀測矩陣為H。首先預(yù)測下一狀態(tài)，根據(jù)系統(tǒng)給定的過程模型：對狀態(tài)方程取影射： X(k+1|k)=?X對觀測方程取影射： Y(k+1|)=HX于是就有了 Y(k+1|k)=HX正常運(yùn)行時(shí)卡爾曼錄波器運(yùn)算過程如下：狀態(tài)的一步預(yù)測方程： X(k+1|k)=?X然后是經(jīng)過系統(tǒng)，進(jìn)行狀態(tài)更新： X(k+1|k+1)=X(k+1|k)+K(k+1)ε濾波的增益矩陣： K(k+1)=P(k+1|k)HT[HP(k+1|k)H進(jìn)行下一步的協(xié)方差陣： P(k+1|K)=?P(k|k)?協(xié)方差更新： X(0|0)=u在一個(gè)卡爾曼濾波的周期里面，卡爾曼濾波有兩方面的更新，時(shí)間的更新?lián)Q代和觀測的更新?lián)Q代。上面的公式2-6在計(jì)算過程中對于狀態(tài)的移動(dòng)數(shù)學(xué)矩陣和高斯白噪聲輸入數(shù)學(xué)矩陣和過程噪聲方差的數(shù)學(xué)矩陣，都是進(jìn)一步轉(zhuǎn)移的。噪聲的特殊情況處理，當(dāng)含有控制量的線性系統(tǒng)描述時(shí)改為下列公式：線性卡爾曼濾波狀態(tài)方程： X(k)=AX(k?1)+BU(k?1)+σW(k-1) （2-10）線性卡爾曼濾波觀測方程： Y(k)=HX(k)+V(k) （2-11）2.3.2擴(kuò)展卡爾曼濾波（EKF）擴(kuò)展卡爾曼濾波（EKF）是對卡爾曼濾波的進(jìn)一步創(chuàng)新，是對非線性體統(tǒng)進(jìn)行線性化，通過展開Taylor級數(shù)，然后忽略二階以上的高級項(xiàng)，以低介導(dǎo)師對非線性系統(tǒng)逼近，再利用卡爾曼濾波理論求取近似的線性系統(tǒng)方程。基于EKF的目標(biāo)定位跟蹤需要主要有三個(gè)方面：系統(tǒng)的狀態(tài)預(yù)測、系統(tǒng)的觀測更新和系統(tǒng)的狀態(tài)放大。對于移動(dòng)機(jī)器人的目標(biāo)定位跟蹤來說，首先確定移動(dòng)機(jī)器人的初始位置，然后經(jīng)過系統(tǒng)的預(yù)測更新算法得出預(yù)測位置，然后觀察收集關(guān)于周圍環(huán)境的標(biāo)志信息，利用之前計(jì)算的觀測值和實(shí)際得到的觀測值進(jìn)行推導(dǎo)，計(jì)算的到參數(shù)K，再把K帶入到系統(tǒng)進(jìn)行矯正，得到更新的協(xié)方差方程，運(yùn)動(dòng)位置和運(yùn)動(dòng)速度。再這個(gè)過程中，系統(tǒng)在不斷循環(huán)矯正，從而確保了移動(dòng)機(jī)器人的精確定位。擴(kuò)展卡爾曼濾波（EKF）算法是對非線性系統(tǒng)線性化，基本過程是：Taylor級數(shù)展開，舍棄非線性部分，然后對狀態(tài)變量求取雅各比矩陣，進(jìn)行替代系統(tǒng)的狀態(tài)矩陣和觀測矩陣，這時(shí)系統(tǒng)的就可以按照卡爾曼濾波算法進(jìn)行處理。非線性函數(shù)在Taylor展開式的矩陣形式中會(huì)用到Hessian矩陣，所以以下先列出使用EKF算法所需要用到的數(shù)學(xué)理論。Ａ：泰勒(Taylor）級數(shù)：在數(shù)學(xué)中，泰勒級數(shù)（英語：Taylorseries）用無限項(xiàng)連加式——級數(shù)來表示一個(gè)函數(shù)，這些相加的項(xiàng)由函數(shù)在某一點(diǎn)的導(dǎo)數(shù)求得.Taylor級數(shù)展開的數(shù)學(xué)理論是將一個(gè)在（x=x0)關(guān)于（x=x0)的n次多項(xiàng)式通近函 f(x)=f(x0)0!+ Rn(x)=Taylor展開式在等式右邊，剩余的RnxＢ：雅可比（Jacobian）矩陣向量分析中，雅可比矩陣是函數(shù)的一階偏導(dǎo)數(shù)以一定方式排列成的矩陣，其行列式稱為雅可比行列式。是一個(gè)從n維歐氏空間映射到到m維歐氏空間的函數(shù)。這個(gè)函數(shù)由m個(gè)實(shí)函數(shù)組成，這些函數(shù)的偏導(dǎo)數(shù)(如果存在)可以組成一個(gè)m行n列的矩陣，這個(gè)矩陣就是所謂的雅可比矩陣狀態(tài)空間模型： ?y1?x1?狀態(tài)和觀測方程 F(x)≈F(p)+J Xk+1=f(Xk Zk=?(Xk為了取得狀態(tài)轉(zhuǎn)移矩陣，要在Wk=0點(diǎn)取值；為了取得觀測矩陣，要在Ｃ：海森矩陣（Hessianmatrix）是一個(gè)多變量實(shí)值函數(shù)的二階偏導(dǎo)數(shù)組成的方塊矩陣，假設(shè)有一實(shí)數(shù)函數(shù)f(x1,x2,…,xn),其中x=(x1,x2,...xn),即H(f)為： ?2fD：擴(kuò)展卡爾曼濾波（EKF）算法實(shí)現(xiàn)步驟如下：非線性系統(tǒng)局部線性化：離散非線性系統(tǒng)動(dòng)態(tài)的方程可以表示為： X(k+1)=f[k,X(k)]+G(k)W(k) （2-19） Z(k)=h[k,X(k)]+V(k) （2-20）擴(kuò)展卡爾曼濾波是利用非線性數(shù)學(xué)函數(shù)的局部線性化的特殊性質(zhì)，將非線性系統(tǒng)線性化。將公式進(jìn)行泰勒級數(shù)展開得狀態(tài)方程為： X(k+1)=?(k+1|k)X(k)+G(k)W(k)+?同卡爾曼濾波相比，增加了狀態(tài)方程?(k)將公式進(jìn)行泰勒級數(shù)展開得觀測方程為： Z(k)=H(k)X(k)+y(k)+V(k) （2-22）擴(kuò)展卡爾曼濾波的遞推公式為： X(k|k+1)=f(X(k|k)) P(k+1|k)=?(k+1|k)P(k|k)?T K(k+1)=P(k+1|k)HT(k+1)[H(k+1)P(k+1|k) P(k+1)=[I?K(k+1)H(k+1)]P(k+1|k) （2-26）上述公式中，濾波初值和濾波誤差方差矩陣的初值分別為： X(0)=E(X(0)],P(0)=var[X(0)] （2-27）假設(shè)狀態(tài)變量有n維，X=[ ?(k+1)=?F? H(k+1)=?h?x2.3.3無跡卡爾曼濾波（UKF）無跡Kalman濾波，是對于卡爾曼濾波中的一步預(yù)測方程，使用UT變換，用一系列確定樣本來逼近狀態(tài)的后驗(yàn)概率密度，不需要對Jacobian矩陣進(jìn)行求導(dǎo)。是一種采樣點(diǎn)逐漸逼近真實(shí)非線性分布的方法。Ａ：UT變化UT變換是用固定數(shù)量的參數(shù)支近似一個(gè)高斯分布，其實(shí)現(xiàn)原理為：確定點(diǎn)集Q來近似代表系統(tǒng)輸入狀態(tài)的特征，然后對每一個(gè)SIGMA點(diǎn)進(jìn)行非線性變換，再根據(jù)加權(quán)計(jì)算得到變化后的統(tǒng)計(jì)特征。Ｂ：UT變化的算法實(shí)現(xiàn)非線性系統(tǒng)，狀態(tài)向量為N，用他的均值和方差計(jì)算2n+1個(gè)Sigma點(diǎn)X的權(quán)值ω來計(jì)算y.計(jì)算2n+1個(gè)Sigma點(diǎn)的計(jì)算公式：x公式中，（P）T計(jì)算采樣點(diǎn)的權(quán)值：ω其中降低預(yù)期狀態(tài)誤差的是：λ=a2(n+k)?n,要想控制分布狀態(tài)，需要對a進(jìn)行適當(dāng)?shù)倪x取，k備用參數(shù)，是保障矩陣（n+對于非線性系統(tǒng)：X(k+1)=f(x(k)),W(k)計(jì)算UKF的幾步步驟為：第一步：獲得采樣點(diǎn)和權(quán)值X第二步：計(jì)算2n+1個(gè)點(diǎn)的預(yù)測X第三步：狀態(tài)一步預(yù)測和其對應(yīng)的協(xié)方差，對比傳統(tǒng)的卡爾曼濾波，多出加權(quán)平均值，根據(jù)一步預(yù)測的值，進(jìn)行UT變換，通過加權(quán)算法，計(jì)算出系統(tǒng)的均值和系統(tǒng)的協(xié)方差。第四步：計(jì)算增益矩陣。第五步：計(jì)算得系統(tǒng)新的狀態(tài)和協(xié)方差第三章基于卡爾曼濾波（KF）的目標(biāo)跟蹤研究3.1卡爾曼濾波（KF）目標(biāo)跟蹤算法運(yùn)動(dòng)模型狀態(tài)包括縱向、橫向的速度和位置，設(shè)定系統(tǒng)狀態(tài)算法方程如下： X(k)=[x(k)x(k)y(k)y機(jī)器人所在的直角坐標(biāo)系的存在這二維的兩個(gè)分量，即X和方向的位置和速度分量，機(jī)器人狀態(tài)有四個(gè)，分別是：X方向的位置、X方向的速度、Y方向的位置、Y方向的速度。系統(tǒng)方程用下面的公式表示： x(k)x(k)y(k)y(k)+ Z(k)=1003.2卡爾曼濾波（KF）目標(biāo)跟蹤實(shí)驗(yàn)移動(dòng)機(jī)器人是在一個(gè)二維的平面進(jìn)行勻速直線運(yùn)動(dòng)，初始的位置我設(shè)定為（-50米，100米），對于X方向的移動(dòng)機(jī)器人的速度設(shè)定為2米每秒，對于Y方向的移動(dòng)機(jī)器人的速度設(shè)定為100米每秒，傳感器的掃描的T=0.5秒，觀測的噪聲均值是零，方差設(shè)定為5，根據(jù)設(shè)定的數(shù)學(xué)模型和FK的數(shù)學(xué)算法，應(yīng)用MATLAB仿真軟件，進(jìn)行FK算法對目標(biāo)定位跟蹤研究。研究結(jié)果如下：3.2.1卡爾曼濾波（KF）濾波軌跡和真實(shí)軌跡比較圖片的橫坐標(biāo)參數(shù)代表的是移動(dòng)機(jī)器人在勻速直線運(yùn)動(dòng)時(shí)的X方向的位移和Y方向的位移。圖3-1卡爾曼濾波（KF）軌跡、觀測軌跡、真實(shí)軌跡對比經(jīng)過仿真圖3-1可以看出：移動(dòng)機(jī)器人通過傳感器的收集到的觀測軌跡跟實(shí)際軌跡相差比較大，振蕩很明顯，不適用精準(zhǔn)的移動(dòng)機(jī)器人定位。雖然運(yùn)動(dòng)模型是簡單的勻速直線運(yùn)動(dòng)也存在較大誤差。而相對經(jīng)過卡爾曼濾波（KF）濾波之后的軌跡，就會(huì)明顯好于直接觀測的值。點(diǎn)（-15，450）為零界點(diǎn)，這個(gè)點(diǎn)之前的卡爾曼濾波（FK）幾乎于真實(shí)軌跡重合，很符合移動(dòng)機(jī)器人對于精準(zhǔn)定位的要求，但是在點(diǎn)（15，450）之后，F(xiàn)K濾波軌跡的振蕩開始增加，并且隨著距離的增加變得偏離真實(shí)的軌跡。3.2.2卡爾曼濾波（KF）濾波前后位置誤差曲線圖片是濾波前的系統(tǒng)誤差和濾波后的系統(tǒng)誤差，X軸是觀測的時(shí)間，Y軸是濾波前后的系統(tǒng)誤差值。圖3-2卡爾曼濾波（KF）濾波前后系統(tǒng)誤差通過仿真圖3-2可以看出：對于沒有濾波的系統(tǒng)，整體的誤差較大，在10秒時(shí)，誤差達(dá)到最大值，為7.683米，在83秒時(shí)有一個(gè)最小值，為0.2米，整體普遍的位置估計(jì)誤差在2.603，說明濾波前振蕩嚴(yán)重，誤差波動(dòng)頻繁，且單次差距很大，最大差距可達(dá)2.3米。而經(jīng)過卡爾曼濾波的濾波后的位置誤差，在0時(shí)刻誤差最小，在148秒時(shí)，誤差達(dá)到最大值，為2.929米。整體誤差在1.324米，單次最大誤差可以達(dá)到1.16米，位置誤差振蕩相對不嚴(yán)重，但是誤差波動(dòng)也較頻繁。說明對于移動(dòng)機(jī)器人目標(biāo)跟蹤中，卡爾曼濾波能起到優(yōu)化誤差和軌跡的效果，但是適合相對粗糙的要求，在一定誤差范圍內(nèi)是不錯(cuò)的選擇。3.2.3KF濾波前誤差跟KF濾波后誤差的仿真實(shí)驗(yàn)數(shù)據(jù)對比選取地44到50采樣點(diǎn)進(jìn)行數(shù)據(jù)的收集，進(jìn)行對經(jīng)過卡爾曼濾波（FK）系統(tǒng)前后誤差的比較，對比性能的好壞。濾波前誤差4.28281.440743.897642.080611.298841.39213.70329濾波后誤差1.174560.862141.038820.7539890.2947270.2547020.62507表3-1KF濾波前后的誤差仿真實(shí)驗(yàn)數(shù)據(jù)表3-1是隨機(jī)抽取的，主要是位于中前半段的的濾波前和濾波后的數(shù)據(jù)。根據(jù)所采集的樣本數(shù)據(jù)能夠看出，在濾波運(yùn)行的過程中，KF濾波前的偏差都要小于濾波后和偏差。第四章基于擴(kuò)展卡爾曼濾波（EKF）的目標(biāo)跟蹤研究4.1擴(kuò)展卡爾曼濾波（EKF）目標(biāo)跟蹤算法機(jī)器人所在的直角坐標(biāo)系的存在這二維的兩個(gè)分量，即X和方向的位置和速度分量，機(jī)器人狀態(tài)有四個(gè)，分別是：X方向的位置、X方向的速度、Y方向的位置、Y方向的速度。機(jī)器人在移動(dòng)過程中的隨機(jī)的擾動(dòng)定義為U（k），系統(tǒng)表示為： x(k+1)=x(k)+vx(k)T+1移動(dòng)機(jī)器人的系統(tǒng)狀態(tài)方程為： x(k+1)=?x(k+1)+γu(k) （4-2）狀態(tài)方程的矩陣分別為： ?=1T0 γ=T2激光雷達(dá)傳感器進(jìn)行信息的搜集和探測，設(shè)定位置為（x0,觀測方程為： Z(k)=(x(k)?x0)公式（4-5）中的V（k）為激光雷達(dá)傳感器自己本身的測量誤差，我把R設(shè)定為測量誤差方差。4.2擴(kuò)展卡爾曼濾波（EKF）目標(biāo)跟蹤實(shí)驗(yàn)移動(dòng)機(jī)器人是在一個(gè)二維的平面進(jìn)行勻速直線運(yùn)動(dòng)，初始的位置我設(shè)定為（-50米，100米），對于X方向的移動(dòng)機(jī)器人的速度設(shè)定為2米每秒，對于Y方向的移動(dòng)機(jī)器人的速度設(shè)定為100米每秒，傳感器的掃描的T=0.5秒，觀測的噪聲均值是零，方差設(shè)定為5，根據(jù)設(shè)定的數(shù)學(xué)模型和EKF的數(shù)學(xué)算法，應(yīng)用MATLAB仿真軟件，進(jìn)行EKF算法對目標(biāo)定位跟蹤研究。研究結(jié)果如下：4.2.1擴(kuò)展卡爾曼濾波（EKF）濾波軌跡和真實(shí)軌跡比較圖4-1的橫坐標(biāo)參數(shù)代表的是移動(dòng)機(jī)器人在勻速直線運(yùn)動(dòng)時(shí)的X方向的位移和Y方向的位移。圖4-1擴(kuò)展卡爾曼濾波（EKF）軌跡與真實(shí)軌跡對比從圖4-1可以看出，在橫坐標(biāo)-20之前，擴(kuò)展卡爾曼濾波（EKF）軌跡幾乎和真實(shí)軌跡重合，并且濾波的波動(dòng)比較小，符合移動(dòng)機(jī)器人目標(biāo)跟蹤要求。但是在橫坐標(biāo)-20點(diǎn)之后，濾波軌跡開始偏離真實(shí)軌跡，并且波動(dòng)增加，在橫坐標(biāo)點(diǎn)40左右波動(dòng)明顯加大。在橫坐標(biāo)-60點(diǎn)之后，濾波軌跡已經(jīng)完全偏離真實(shí)軌跡，這時(shí)波動(dòng)已經(jīng)超出了之前數(shù)倍?？梢钥闯?，對于勻速直線運(yùn)動(dòng)的機(jī)器人，擴(kuò)展卡爾曼濾波在一定的距離內(nèi)是很準(zhǔn)確和穩(wěn)定的，但是隨著運(yùn)動(dòng)距離變化，開始逐步失真，所以擴(kuò)展卡爾曼濾波適合短距離的移動(dòng)機(jī)器人目標(biāo)跟蹤。4.2.2擴(kuò)展卡爾曼濾波（EKF）位置誤差曲線圖4-2為擴(kuò)展卡爾曼濾波位置誤差曲線，橫坐標(biāo)時(shí)時(shí)間，縱坐標(biāo)是位置估計(jì)偏差：圖4-2擴(kuò)展卡爾曼濾波（EKF）位置誤差曲線從圖4-2可以看出，EKF濾波效果一般，在前20秒時(shí)間內(nèi)，位置誤差很小，幾乎為零，但是20秒之后誤差開始變大，并且振蕩加劇，在109秒這個(gè)點(diǎn)，達(dá)到最高誤差距離23.9米，可以說在40秒之后，濾波軌跡已經(jīng)理真實(shí)軌跡偏離的很嚴(yán)重。對于機(jī)器人定位來說，在短時(shí)間的運(yùn)動(dòng)跟蹤，EKF可以做好的很完美的精確定位，但是經(jīng)過幾次算法的迭代，一段時(shí)間之后濾波軌跡就會(huì)偏離真實(shí)軌跡，誤差也會(huì)隨著時(shí)間繼續(xù)加大。在一定的時(shí)間內(nèi)，擴(kuò)展卡爾曼濾波是很好的選擇。想要一直跟蹤移動(dòng)機(jī)器人定位是比較苦難的。4.2.3EKF濾波誤差跟FK濾波誤差的仿真實(shí)驗(yàn)數(shù)據(jù)對比選取地16到23采樣點(diǎn)進(jìn)行數(shù)據(jù)的收集，進(jìn)行UKF和EKF位置偏離誤差的比較，對比性能的好壞：EKF誤差0.161290.3690610.538080.5466220.2139450.1797520.152328FK誤差1.3140.862141.38821.96932.049682.139682.45538表4-1EKF濾波誤差與FK濾波誤差的實(shí)驗(yàn)仿真數(shù)據(jù)從上表可以看出，擴(kuò)展卡爾曼濾波有效的降低了位置誤差，并且在20秒之內(nèi)的定位很誤差小，對比卡爾曼濾波定位也更準(zhǔn)確，并且穩(wěn)定。不過根據(jù)之前的仿真可以看出，擴(kuò)展卡爾曼濾波雖然比卡爾曼濾波目標(biāo)跟蹤更準(zhǔn)確，振幅小和穩(wěn)定，但是不超過設(shè)定范圍，超過了時(shí)間范圍誤差就會(huì)加大，軌跡就會(huì)偏離真實(shí)軌跡。第五章基于無跡卡爾曼濾波（UKF）的目標(biāo)跟蹤研究5.1無跡卡爾曼濾波（UKF）目標(biāo)跟蹤算法運(yùn)動(dòng)模型方程為： X(k+1)=?X(k)+ΓW(k) （5-1） Z(k)=(x(k)?x0)過程驅(qū)動(dòng)矩陣： ?=110研究移動(dòng)機(jī)器人的目標(biāo)運(yùn)動(dòng)方程為： Γ=0.5010目標(biāo)狀態(tài)方程： Xukf=誤差的平凡根： RMSM(k)=(XUKF5.2無跡卡爾曼濾波（UKF）目標(biāo)跟蹤實(shí)驗(yàn)移動(dòng)機(jī)器人是在一個(gè)二維的平面進(jìn)行勻速直線運(yùn)動(dòng)，初始的位置我設(shè)定為（-50米，100米），對于X方向的移動(dòng)機(jī)器人的速度設(shè)定為2米每秒，對于Y方向的移動(dòng)機(jī)器人的速度設(shè)定為100米每秒，傳感器的掃描的T=0.5秒，觀測的噪聲均值是零，方差設(shè)定為5，根據(jù)上文設(shè)定的數(shù)學(xué)模型和FK的數(shù)學(xué)算法，應(yīng)用MATLAB仿真軟件，進(jìn)行FK算法對目標(biāo)定位跟蹤研究。研究結(jié)果如下：5.2.1無跡卡爾曼濾波（EKF）濾波軌跡和真實(shí)軌跡比較圖5-1的橫坐標(biāo)參數(shù)代表的是移動(dòng)機(jī)器人在勻速直線運(yùn)動(dòng)時(shí)的X方向的位移和Y方向的位移圖5-1無跡卡爾曼濾波（UKF）軌跡與真實(shí)軌跡對比從圖5-1可以看出，擴(kuò)展卡爾曼濾波（UKF）軌跡整體很符合真實(shí)軌跡。在橫坐標(biāo)0之前，UKF軌跡是有波動(dòng)的，離實(shí)際軌跡有一定誤差，但是隨著迭代算法的更新，UKF軌跡越來越符合真實(shí)軌跡，能