1、目錄一、仿真背景2二、仿真算法2三、仿真結果4四、總結8圖表Figure1EKF的算法結構3Figure2真實路徑5Figure3不加EKF路徑5Figure4不加EKF誤差6Figure5加EKF路徑6Figure6加EKF誤差7Figure7真實、不加EKF、加EKF路徑對比7Figure8不加EKF、加EKF誤差對比8一、仿真背景室內(nèi)定位很難采用GPS定位的方法，由于信號受建筑物的影響而大大衰減造成的，同時由于多路徑效應也會導致定位精度較低。在新主樓室內(nèi)測試過幾種GPS模塊，發(fā)現(xiàn)只要在室內(nèi)就是不能成功定位的，但只要把GPS模塊靠近空曠側的窗，GPS信號就會恢復，但是定位精度卻比較差?？梢?/p>

2、，小范圍的室內(nèi)定位用GPS定位方法是不合適的。目前室內(nèi)定位有很多其他方法，例如室內(nèi)無線定位技術、基于計算機視覺、光跟蹤定位、基于圖像分析、磁場以及信標定位等。此外，還有基于圖像分析的定位技術、信標定位、三角定位等。與GPS定位方法比較，這些技術成熟度上還存在不足，但是這些技術有很好的應用前景，尤其是在小范圍、小空間的室內(nèi)定位中有很好的定位效果、精度也很高，目前也有很高的研究熱度。把這些技術和GPS方法結合起來，就可以發(fā)揮各自的優(yōu)長，既可以提供較好的精度和響應速度，又可以覆蓋較廣的范圍，實現(xiàn)無縫的、精確的定位，把這些不同來源的傳感器信息綜合起來，就需要依靠多源信息融合的方法實現(xiàn)。本實驗主要是對室

3、內(nèi)定位數(shù)據(jù)進行濾波，從而獲取更好的定位效果。實驗數(shù)據(jù)是基于圖像分析的方法獲取的，以實驗室天花板嵌入的燈罩作為地標，燈罩具有確定的分布以及確定的位置，可以把這些燈罩作為參考，解算出攝像頭的位置，再把攝像頭坐標系轉換到車坐標系上，這樣就實現(xiàn)了定位。實驗數(shù)據(jù)來自于老師提供的文件，文件數(shù)據(jù)導入到matlab中，使用matlab對數(shù)據(jù)進行處理，并繪圖顯示出處理效果。二、仿真算法本文使用擴展卡爾曼濾波EKF的方法，對從圖像解析出來的數(shù)據(jù)進行處理。擴展卡爾曼濾波的原理如下：在非線性系統(tǒng)中，系統(tǒng)模型是這樣的：xk+1=f(Xk)+wkzk=h(Xk)+vk這與線性系統(tǒng)的區(qū)別在于非線性系統(tǒng)的狀態(tài)向量和其系數(shù)是不

4、能夠分離的。所謂擴展卡爾曼濾波器，就是適用于非線性系統(tǒng)的卡爾曼濾波器。它與經(jīng)典的線性卡爾曼濾波器很相似，算法步驟和結構都相同。不同在于系統(tǒng)模型和矩陣A和H。在擴展卡爾曼濾波器當中用非線性系統(tǒng)模型方程代替線性系統(tǒng)墨香的系統(tǒng)方程；將系統(tǒng)模型求偏導得到新的擴展卡爾曼濾波器當中的矩陣A和H,在偏導的求解過程中，也是就是線性化的過程中用前一個時刻的估計值作為參考點。通過這樣的修改就得到了適用于非線性系統(tǒng)的擴展卡爾曼濾波器。在使用的過程中我們要時刻牢記，擴展卡爾曼濾波會發(fā)散。擴展卡爾曼濾波的算法結構如下圖，注意，將非線性系統(tǒng)中的f對x求(xk估計)處的偏導得到A,同樣的求h對x求(xk)處的偏導得到H。；

5、14Figure1EKF的算法結構利用擴展卡爾曼濾波，就可以對我們的數(shù)據(jù)進行融合估計了下面對我們的matlab程序做一些說明：1.數(shù)據(jù)存放在location.txt文件中，數(shù)據(jù)的存放格式如下：數(shù)據(jù)分別代表：0-071.5645199.2392.41.865.4946456.4132.42.43.102842.048781.54719單位時間左右輪駛過的距離特征點1的image坐標特征點1的world坐標特征點2的image坐標特征點2的world坐標真實坐標和角度2. 程序中的變量說明：ru,rv：pixelmagnificationfactors，像素放大因子xf,yf：worldframe

6、coordinates，世界坐標xfr,yfr：robotframecoordinates，robot坐標uO,vO：imagecoordinatesofthecamerasprincipal機投影點的圖像坐標zfc：distancefromtheopticalcenterofthecameratoceilingplane，相機中心至U天花板平面的距離d1,d2：coordinatesofOcintherobotframe,相機中心在robot坐標系中的坐標b：distancebetweenthedrivingwheels,兩輪距P,Q：論文中的測距法模型矩陣W：觀測模型矩陣u,v：coord

7、inatesofthefeatureintheimageplane,image坐標中的特征位置3. 程序中的變量定義以及融合過程完全按照文章中的步驟進行，最終將得至：a. 真實的路徑b. 不加EKF計算出的路徑c. 加EKF計算出的路徑d. 不加EKF計算和真實值之間的誤差e. 加EKF計算和真實值之間的誤差三、仿真結果根據(jù)上述數(shù)據(jù)提取以及處理的方法與流程，得至了下面的結果：1.真實路徑Figure3不加EKF路徑3.不加EKF的誤差5.加EKF的誤差Figure6加EKF誤差7.不加EKF和加EKF誤差的綜合對照四、總結通過對比我們可以看出，加EKF后融合的數(shù)據(jù)要比不加EKF的數(shù)據(jù)好的多，無

8、論是從路徑圖上還是從誤差曲線上，都有明顯的效果。可見，使用濾波的方法對傳感器數(shù)據(jù)進行處理可以為我們很好地提高數(shù)據(jù)的精度，這對于我們更精確地完成一些操作是必須的。本科階段參加過一些比賽和小制作，也經(jīng)常使用一些傳感器，但總是被傳感器精度不足困擾。多源信息融合這門課給了我新的工具與思路，通過采用多個不同、或者相同的傳感器數(shù)據(jù)進行融合，就可以很好地提高傳感器精度，獲得更好的數(shù)據(jù)。這從我們這次仿真的圖像中就可以看出來它的效果。以后再進行一些工程或者項目的時候，會盡量使用信息融合的辦法，采集不同來源的數(shù)據(jù)以提高傳感器精度。這其實是對我思維的改變。另外，這門課讓我認識到自己的數(shù)學基礎特別薄弱，而且面對文獻耐

9、心不足，這也是以后需要著力提升的方面。最后，感謝老師寬限我晚交報告附錄：matlab代碼：clearall;clc;ticdata=importdata(location.txt);ru=902.13283;rv=902.50141;u0=347.20436;v0=284.34705;zfc=2.1050;d1=-0.0668;d2=0.0536;b=0.328;I=eye(3);0025;P=0.00300;00.0030;000.01;Q_value=0.1*0.050;00.05;W=0.01*25000;02500;00250;0k0(1,1:2)=data(6,:);k0(1,3)=

10、data(7,1);d_s=zeros(2,2);route_real=zeros(2,3);dataf=zeros(2,2,2);datat=zeros(2,2,2);count=1;fori=1:size(data,1)ifmod(i,7)=3dataf(count,:,1)=data(i,:);endifmod(i,7)=5dataf(count,:,2)=data(i,:);endifmod(i,7)=6route_real(count,1:2)=data(i,:);endifmod(i,7)=0route_real(count,3)=data(i,1);count=count+1;e

11、ndifmod(i,7)=2datat(count,:,1)=data(i,:);endifmod(i,7)=4datat(count,:,2)=data(i,:);endifmod(i,7)=1d_s(count,:)=data(i,:);endendX=k0(1,:);count=2;fori=1:size(d_s,1)k0(count,1)=k0(count-1,1)+(d_s(i,1)+d_s(i,2)/2*cos(k0(count-1,3);k0(count,2)=k0(count-1,2)+(d_s(i,1)+d_s(i,2)/2*sin(k0(count-1,3);k0(coun

12、t,3)=k0(count-1,3)+(d_s(i,1)-d_s(i,2)/b;count=count+1;endroute_fusion=zeros(size(d_s,1),2);fori=1:size(d_s,1)X_estimate=X(1,1)+(d_s(i,1)+d_s(i,2)/2*cos(X(3,1);X(2,1)+(d_s(i,1)+d_s(i,2)/2*sin(X(3,1);X(3,1)+(d_s(i,1)-d_s(i,2)/b;hx=zeros(3,3);hx(1,1)=1;hx(2,2)=1;hx(3,3)=1;hx(1,3)=-(d_s(i,1)+d_s(i,2)/2*

13、sin(X(3,1);hx(2,3)=(d_s(i,1)+d_s(i,2)/2*cos(X(3,1);hu=1/b*ones(3,2);hu(3,2)=0-hu(3,2);hu(1,1)=0.5*cos(X(3,1);hu(2,1)=0.5*sin(X(3,1);hu(1,2)=0.5*cos(X(3,1);hu(2,2)=0.5*sin(X(3,1);Q=Q_value;Q(1,1)=Q(1,1)*abs(d_s(i,1);Q(2,2)=Q(2,2)*abs(d_s(i,2);P_estimate=hx*P*hx+hu*Q*hu;z_estimate=-ru/zfc*(-(dataf(i,1

14、,1)-X_estimate(1,1)*sin(X_estimate(3,1)+(dataf(i,2,1)-X_estimate(2,1)*cos(X_estimate(3,1)-d2)+u0;-rv/zfc*(-(dataf(i,1,1)-X_estimate(1,1)*cos(X_estimate(3,1)-(dataf(i,2,1) -X_estimate(2,1)*sin(X_estimate(3,1)+d1)+v0;-ru/zfc*(-(dataf(i,1,2)-X_estimate(1,1)*sin(X_estimate(3,1)+(dataf(i,2,2) -X_estimate

15、(2,1)*cos(X_estimate(3,1)-d2)+u0;-rv/zfc*(-(dataf(i,1,2)-X_estimate(1,1)*cos(X_estimate(3,1)-(dataf(i,2,2) -X_estimate(2,1)*sin(X_estimate(3,1)+d1)+v0;gx=ones(4,3);gx(1,1)=ru/zfc*sin(X(3,1);gx(1,2)=-ru/zfc*cos(X(3,1);gx(1,3)=ru/zfc*(-(dataf(i,1,1)-X(1,1)*cos(X(3,1)-(dataf(i,2,1)-X(2,1)*sin(X(3,1);gx

16、(2,1)=rv/zfc*cos(X(3,1);gx(2,2)=rv/zfc*sin(X(3,1);gx(2,3)=rv/zfc*(dataf(i,1,1)-X(1,1)*sin(X(3,1)-(dataf(i,2,1)-X(2,1)*cos(X(3,1) );gx(3,1)=ru/zfc*sin(X(3,1);gx(3,2)=-ru/zfc*cos(X(3,1);gx(3,3)=ru/zfc*(-(dataf(i,1,2)-X(1,1)*cos(X(3,1)-(dataf(i,2,2)-X(2,1)*sin(X(3,1);gx(4,1)=rv/zfc*cos(X(3,1);gx(4,2)=r

17、v/zfc*sin(X(3,1);gx(4,3)=rv/zfc*(dataf(i,1,2)-X(1,1)*sin(X(3,1)-(dataf(i,2,2)-X(2,1)*cos(X(3,1) );gx=gx*(-1);R=gx*P_estimate*(gx)+W;K=P_estimate*(gx)*inv(R);Z=zeros(4,1);Z(1,1)=datat(i,1,1);Z(2,1)=datat(i,2,1);Z(3,1)=datat(i,1,2);Z(4,1)=datat(i,2,2);X_fusion=X_estimate+K*(Z-z_estimate);X=X_fusion;P=(I-K*gx)*P_estimate;route_fusion(i,:)=X_fusion(1:2,1);endfigure;plot(route_real(:,1),route_real(:,2);holdon;plot(k0(:,1),k0(:,2),r);holdon;plot(route_fusion(:,1),route_fusion(:,2),k);holdoff;E_odometer=zeros(size(route_real,1),1);E_fusion=zeros(size(route_real,1),1);fori=1:size(route_rea