HW2-基于里程計(jì)的航位推算仿真實(shí)驗(yàn)-15291122-任家輝

實(shí)驗(yàn)報(bào)告1-航位推算基于光學(xué)編碼器的移動(dòng)機(jī)器人航位推算仿真實(shí)驗(yàn)報(bào)告學(xué)號：15291122姓名：任家輝北京交通大學(xué)2014年版實(shí)驗(yàn)?zāi)康脑谝苿?dòng)機(jī)器人的傳感技術(shù)中，實(shí)現(xiàn)機(jī)器人在環(huán)境中的定位是移動(dòng)機(jī)器人要解決的關(guān)鍵技術(shù)之一。能實(shí)現(xiàn)移動(dòng)機(jī)器人的定位的方法很多，如衛(wèi)星定位、超聲定位、激光測距定位、基于視覺的機(jī)器人定位。本實(shí)驗(yàn)主要是對基于光學(xué)編碼器（里程計(jì)）的定位原理進(jìn)行學(xué)習(xí)，并利用Matlab環(huán)境實(shí)現(xiàn)其航位推算的仿真。通過仿真實(shí)驗(yàn)，加深對里程計(jì)工作原理的了解。同時(shí)對移動(dòng)機(jī)器人的定位方法有一個(gè)基礎(chǔ)的掌握。實(shí)驗(yàn)內(nèi)容及步驟本實(shí)驗(yàn)主要包括以下幾個(gè)方面的內(nèi)容：Matlab的編程學(xué)習(xí)。通過對該軟件的學(xué)習(xí)編程，掌握一些數(shù)學(xué)工具的使用。對航位推算的代碼進(jìn)行分析。結(jié)合里程計(jì)的工作原理，對計(jì)算方法進(jìn)行分析，掌握計(jì)算的基本過程，及編程中用到的Matlab函數(shù)的使用方法。學(xué)習(xí)Matlab的簡單的繪圖函數(shù)的使用。在例程的基礎(chǔ)上，進(jìn)行一定程度的修改。對于掌握的較好的同學(xué)，可以做更進(jìn)一步的實(shí)驗(yàn)：增加噪聲。觀察因?yàn)閭鞲衅鞯脑肼晫?dǎo)致機(jī)器人路徑的偏差變化。3Matlab中運(yùn)行代碼得到小車軌跡圖一寫入代碼圖二運(yùn)行程序后得到小車運(yùn)動(dòng)軌跡收獲及建議本次實(shí)驗(yàn)讓我接觸到了MATLAB，我切身感覺到MATLAB給我們實(shí)驗(yàn)仿真帶來的好處，這次實(shí)驗(yàn)也讓我得到由自己設(shè)定的運(yùn)行軌跡，甚是欣喜。附錄1：樣例代碼%****************************myDR.m**************************clc;clear;Sr=[03420050020-400000000];%左輪單位時(shí)間內(nèi)走過的距離，相當(dāng)于是左編碼器的采樣值，給出了每個(gè)時(shí)刻下編碼器的讀數(shù)Sl=[02205200523533243450];%右輪單位時(shí)間內(nèi)走過的距離，相當(dāng)于是右編碼器的采樣值O=[0000000000000000000];%小車在T時(shí)間相對于水平角度X=[0000000000000000000];%用于存儲小車當(dāng)前X坐標(biāo)（計(jì)算的定位結(jié)果）Y=[0000000000000000000];%用于存儲小車當(dāng)前Y坐標(biāo)L=10;%小車寬度fori=1:19X(i+1)=X(i)+(Sr(i)+Sl(i))*cos(O(i)+(Sr(i)+Sl(i))/(2*L))/2;%計(jì)算下一時(shí)刻XY(i+1)=Y(i)+(Sr(i)+Sl(i))*sin(O(i)+(Sr(i)-Sl(i))/(2*L))/2;%計(jì)算下一時(shí)刻YO(i+1)=O(i)+(Sr(i)-Sl(i))/L;%計(jì)算下一時(shí)刻角度endplot(X,Y,'-s',...'Color','r',...'LineWidth',2,...'MarkerEdgeColor','k',...'MarkerFaceColor','g',...'MarkerSize',8);%繪制以（X,Y）為坐標(biāo)的關(guān)鍵點(diǎn)的軌跡圖，它相當(dāng)于各個(gè)時(shí)刻下的機(jī)器人的位置（定位實(shí)現(xiàn)了?。ゞridon;xlabel('X');%給X軸加注ylabel('Y');text(0,-0.5,'小車起點(diǎn)');text(10,-3.8,'小車終點(diǎn)');title('航位推算坐標(biāo)圖');%加圖注%****************************myDR.m**************************附錄2：:航位推算基本原理的“溫習(xí)”僅僅利用里程計(jì)來實(shí)現(xiàn)定位的基本原理如圖1所示：（圖1） 設(shè)t時(shí)刻，移動(dòng)機(jī)器人的位姿為，，分別表示機(jī)器人從t到t+1時(shí)刻機(jī)器人運(yùn)動(dòng)的距離和絕對航向，相對航向定義為前后兩個(gè)時(shí)刻絕對航向之差，用ω表示。則t+1時(shí)刻移動(dòng)機(jī)器人的位姿可表示為: (3-1) 從(3-1)式可以看出，里程計(jì)作定位之用算法結(jié)構(gòu)簡單，工作時(shí)實(shí)時(shí)獲得前后時(shí)間的行駛距離s和角度變化量ω。 在實(shí)際工程中，對于移動(dòng)機(jī)器人平臺可以在左右兩側(cè)分別添加里程計(jì)進(jìn)行路徑記錄（如圖3-2所示），這樣可以通過兩側(cè)的路程差得到轉(zhuǎn)過的角度，進(jìn)行路徑的遞推，該推導(dǎo)過程如下：(圖3-2) 對實(shí)際狀態(tài)下的移動(dòng)機(jī)器人移動(dòng)情況進(jìn)行建模分析，確定形象模型進(jìn)行路徑計(jì)算的方法，圖6-1所示表示移動(dòng)機(jī)器人從點(diǎn)O(x,y,θ)開始做圓弧運(yùn)動(dòng)，在Δt時(shí)間后到達(dá)點(diǎn)M(x+Δx,y+Δy,θ+Δθ),圖中θ為初始航向角，L為移動(dòng)機(jī)器人兩輪間的寬度，R為機(jī)器人運(yùn)動(dòng)圓弧的半徑，ΔSL、ΔSR、ΔS分別表示機(jī)器人左右兩輪以及中心點(diǎn)從O到M所走過的距離，Δx、Δy、Δθ分別表示移動(dòng)機(jī)器人從O到M橫、縱坐標(biāo)以及航向角度所發(fā)生的變化，如果假定移動(dòng)機(jī)器人在行駛過程中理想，即路面平整，無打滑，則有： (3-2) 因?yàn)閳D3-2中有，則為等腰三角形，則成立，根據(jù)弦切角定理可知： (3-3) (3-4) 當(dāng)對傳感器的信息采樣時(shí)間足夠短或者移動(dòng)機(jī)器人做直線運(yùn)動(dòng)時(shí)，有以下兩個(gè)式子成立，，則根據(jù)(3-3)和(3-4)式可以化簡為： (3-5)通過對(3-2)與(3-5)式聯(lián)立求解可以得到移動(dòng)機(jī)器人的離散運(yùn)動(dòng)方程：(3-6)其中，分別表示移動(dòng)機(jī)器人左右兩側(cè)輪子從t到t+1時(shí)刻移動(dòng)的里程，將移動(dòng)機(jī)器人的位置向量設(shè)為，采樣周期為dt（dt應(yīng)該比較小，這樣才能滿足在采樣周期內(nèi)機(jī)器人做線性運(yùn)動(dòng)），其輸入控制向量（它們的噪聲可近似為，的高斯白噪聲），根據(jù)(3-6)式，考慮到控制輸入量噪聲的影響，可得移動(dòng)機(jī)器人的位置向量為：(3-7)根據(jù)（3-6）式可以計(jì)算小車的航跡，輸入量為小車左右輪在一個(gè)時(shí)間單位所走的路程。L為小車的車寬，是已知量。通過公式可以計(jì)算出小車的當(dāng)前相對位置，及X，Y。附錄3：參考資料:[1]Matlab繪圖教程(大量實(shí)