四輪移動小車運動學(xué)分析及環(huán)境建模案例目錄TOC\o"1-3"\h\u6474四輪移動小車運動學(xué)分析及環(huán)境建模案例 182711.1四輪移動小車運動學(xué)模型 1226101.2激光雷達(dá)模型 3144041.1.1激光雷達(dá)測距原理 3294241.1.2激光雷達(dá)測距數(shù)學(xué)模型 371881.3四輪移動小車的坐標(biāo)系統(tǒng) 669431.4地圖模型 7237571.4.1柵格地圖 742941.4.2拓?fù)涞貓D 8106211.4.3特征地圖 8如今汽車的很多高端功能都依賴于傳感器的感知能力，就比如汽車的自動導(dǎo)航、車自身溫度的檢測、時速表等信息的顯示都離不開傳感器的作用。傳感器在小車行駛過程中就像是人的大腦一樣，賦予了汽車視覺、觸覺、力的感知等多種功能，實現(xiàn)智能化處理。每個功能的實現(xiàn)都源自于傳感器，受傳感器采集到的數(shù)據(jù)所控制。對小車的運動學(xué)進(jìn)行分析有助于我們把握小車的運動狀態(tài)，激光雷達(dá)技術(shù)可用于探測距離、方位等有效信息，研究小車坐標(biāo)系來描述小車運動情況，地圖的構(gòu)建用以實現(xiàn)小車路徑規(guī)劃。1.1四輪移動小車運動學(xué)模型光電編碼器一般會安裝在驅(qū)動輪子的電機上。主要作用是記錄輪子轉(zhuǎn)軸轉(zhuǎn)動一圈所輸出的位移量，也就是光電編碼器的分辨率。再通過記錄輪子轉(zhuǎn)軸在某一時間段內(nèi)轉(zhuǎn)動的轉(zhuǎn)數(shù)，應(yīng)用相應(yīng)的計算公式就可算出這一時間段內(nèi)小車的行駛里程，也就是里程計的工作原理。然而，由于路面不平滑導(dǎo)致車輪抖動和噪聲對光電編碼器的影響，對搜集到的里程信息造成很大的誤差，隨著小車的行駛，誤差不斷累加。還有安裝車輪時形成的誤差，會導(dǎo)致計算精確度不夠高。光電編碼器通過測量驅(qū)動電機上碼盤的轉(zhuǎn)速μ、輪子的直徑d（單位：mm）和光電編碼器的精度ρ，推算出機器人位姿的變化量δ。采樣頻率可以設(shè)定，一般在QUOTE30Hz30Hz(1.1)里程計是通過編碼器輸出的左右驅(qū)動輪脈沖增量QUOTENLNL和QUOTENRNR，和編碼器的分辨率δ來分別計算左右驅(qū)動輪在采樣周期的位移增量QUOTE?SL?SL和?SRQUOTE?SR：(1.2)當(dāng)小車在轉(zhuǎn)彎時，由于直線模型無法直接描述其運動軌跡，故采用圓弧模型計算小車在轉(zhuǎn)彎時的運動情況。圖1.1輪式里程計圓弧模型小車的位姿變化可用t?1時刻的姿態(tài)QUOTEXt-1(xt-1,yt-1,θt-1)Xt-1(x(1.3)小車?yán)锍逃嫷妮斎肽Ｐ蜑镼UOTEut=(?Dt,?θ(1.4)里程計的誤差來源有很多，系統(tǒng)誤差有測量儀器精確度不夠高、安裝車輪時位置控制不夠準(zhǔn)確形成的誤差等，無法消除。非系統(tǒng)誤差有路面不平滑導(dǎo)致車輪抖動和噪聲對光電編碼器的影響，對搜集到的里程信息造成很大的誤差，隨著小車的行駛，誤差不斷累加，小車停止后誤差不再增加。1.2激光雷達(dá)模型1.1.1激光雷達(dá)測距原理本次課題所使用的雷達(dá)是型號名稱為A1M1的新型激光雷達(dá)RPLIDAR，由美國思嵐激光科技在2013年自主研發(fā)，目前國內(nèi)市場價格相對較低。360°掃描，掃描距離為8米，可用于機器人路徑規(guī)劃研究等領(lǐng)域。圖1.2激光雷達(dá)RPLIDARA1RPLIDARA1激光雷達(dá)系統(tǒng)的穩(wěn)定性較好，雷達(dá)轉(zhuǎn)速不會隨著光源電壓的變化而改變，因此選擇該型號激光雷達(dá)不會對路徑規(guī)劃造成很大誤差。掃描方向都是順時針方向。一般情況下，激光雷達(dá)都是安裝在小車中心位置，以激光雷達(dá)為坐標(biāo)原點建立直角坐標(biāo)系，其中QUOTEρi和diρi和di是當(dāng)前狀態(tài)下激光雷達(dá)到障礙物的遠(yuǎn)近，QUOTEθiQUOTEzi=ρi，θi=式中，N是掃描點數(shù)，可自行設(shè)定。1.1.2激光雷達(dá)測距數(shù)學(xué)模型通過對比分析光束模型和似然場模型來研究激光雷達(dá)在進(jìn)行路徑規(guī)劃時采用的數(shù)學(xué)模型，著重分析了本設(shè)計用到的似然場模型。相較于光束模型，似然場模型適用于各種環(huán)境，而且計算量小。（1）光束模型預(yù)期的光束模型QUOTEpztxt,m因此當(dāng)移動車輛處于動態(tài)環(huán)境中時，用QUOTEphit表示phit表示第一個高斯測距模型，由于地圖是靜態(tài)的，所以不包括在地圖中的物體，如行人，應(yīng)該從靜態(tài)地圖中刪除，并用指數(shù)分布QUOTEpshortpshort表示，處理這類問題的簡單方法是將其作為傳感器噪聲直接去除，此時自動激光雷達(dá)將自動返回到最大精度測量區(qū)域范圍QUOTEzmaxzmax，即能以QUOTEzmaxzmax為中心的點群分布QUOTEpmaxpmax表示，即最大精度測量區(qū)域范圍已經(jīng)明確建立。當(dāng)一個激光測距系統(tǒng)產(chǎn)生各自無法解釋的外力測量時，例如激光發(fā)射的多束激光被多個壁反彈或激光受到多個傳感器的干擾時，為了有效簡化這種測量情況，用QUOTEprandprand在激光雷達(dá)天線測距測量范圍內(nèi)的完全均勻分布曲線來精確表示。圖1.3激光雷達(dá)測距模型分布考慮到幾率和密度為1，加權(quán)平均求和時通過四個分布對應(yīng)的參數(shù)QUOTEzhitzhit、QUOTEzshortzshort、QUOTEzmaxzmax、QUOTEzrandzrand進(jìn)行的，假如四個參數(shù)和為1，則有：(1.6)從計算的角度看，由于每次掃描返回值都有上百個，而且掃描頻率也很高，導(dǎo)致計算傳感器全部返回值是很難實現(xiàn)的。又因為每次掃描后都要進(jìn)行射線投射操作，從而不能總是實時地將掃描整合到當(dāng)前的置信度中。為了解決這一問題，僅綜合激光雷達(dá)掃描過的一部分等間隔測量結(jié)果。并且不會輕易受相鄰測量的噪聲影響。基于波束的模型存在算法的局限性和光滑性不夠。當(dāng)小車身處一個復(fù)雜的障礙物運動環(huán)境中時，其在不同障礙位置的能量分布不連續(xù)。當(dāng)會議室里放置有許多小型桌椅和沙腿時，因為小車的一個細(xì)微角度變化就會導(dǎo)致小車局部坐標(biāo)值在空間中x-y的巨大局部偏移，也就是說，在這樣一個不光滑的模型中，存在著更多的小車局部極大值偏移值。由于利用光線的反射原理，激光雷達(dá)的每一個光線掃描工作周期有數(shù)百個光線數(shù)值，這些數(shù)值都可能需要進(jìn)行一個光線投影的運算，計算量很大。（2）似然場模型似然場模型計算是一種在特定計算環(huán)境下的數(shù)學(xué)假設(shè)設(shè)計算法，相似于高斯平滑的計算環(huán)境，它不要求計算是在激光雷達(dá)感應(yīng)器上數(shù)學(xué)模型的基本條件概率，其主要數(shù)學(xué)思想是通過全局三角變換將一個激光雷達(dá)的掃描坐標(biāo)點直接投影到位于地圖中心的一個全局坐標(biāo)系中。與其它波束模型方法相比，似然場模型通過一個三角形的映射將預(yù)先觀測到的結(jié)果通過投影映射到一個全局坐標(biāo)系，只有在電子探測器遇到障礙物時才會具有意義，該模型因而放棄了最大觀測距離下對觀測結(jié)果值的自動讀取。為達(dá)到模型的平滑度，測量到的結(jié)果由一個模型平均值為0的高斯函數(shù)進(jìn)行估計，測量的坐標(biāo)由歐氏模型距離間的dist來表示，即使所測量到的結(jié)果在一個相應(yīng)的小車坐標(biāo)系下與歐氏模型距離間的速度地圖上模擬重合：(1.7)給出了似然場模型的幾率分配：(1.8)測量過程中的波束噪聲由高斯概率值的估計值表示；對于一個測定未成功時，即在第二種測量情況下討論波束模型時，它由一個函數(shù)點陣組合表示；對于不能正確解釋的隨機函數(shù)測量，它是由一個統(tǒng)一的分布表示。表1.1似然場模型概率計算偽代碼似然場概率估計算法：1:q=12:forallkto3:if4:5:6:7:8:endif9:endfor10:returnq從表1.1可以看出，似然場模型中，第三行是在檢測激光雷達(dá)讀數(shù)是否為最大值，如果答案是，則可以丟棄。第4-7行主要測量將障礙物高斯模糊后的數(shù)據(jù)。第6行用歐氏距離表示測量距離。第7行整合均勻分布和點群分布。將障礙物高斯核處理后，用全局坐標(biāo)X-Y來表示測距的似然估計。采用的似然場模型優(yōu)點是對障礙物進(jìn)行高斯處理后，使觀測結(jié)果更加平滑，適用于結(jié)構(gòu)化和非結(jié)構(gòu)化環(huán)境。并且通過對地圖進(jìn)行光柵化來加快運行速度，本代碼最耗時的步驟是第6步，通過查表可以得到光柵化的地圖。1.3四輪移動小車的坐標(biāo)系統(tǒng)在小車研究領(lǐng)域中，有多個坐標(biāo)系可用來描述小車的位置和姿態(tài)信息，并且在一定條件下各坐標(biāo)系的數(shù)據(jù)可以相互轉(zhuǎn)換。系統(tǒng)中所有點的坐標(biāo)在任何時候都可以用世界坐標(biāo)系來描述，世界坐標(biāo)系是絕對的。通過將小車坐標(biāo)系QUOTEORXRYRORXRYR中的環(huán)境信息轉(zhuǎn)換為世界坐標(biāo)系圖1.4小車坐標(biāo)系圖1.4顯示了小車坐標(biāo)系與世界坐標(biāo)系的轉(zhuǎn)換關(guān)系?？赏ㄟ^公式（1.9）將m點的小車坐標(biāo)QUOTEX0=[x0，y0]TX0=（1.9）1.4地圖模型1.4.1柵格地圖柵格地圖（Gridmap）分為矩形柵格地圖和扇形柵格地圖。主要研究矩形柵格地圖，它是由一系列大小一致的矩形柵格組成，柵格大小可人為設(shè)定。如下圖1.4表示一個簡單的矩形柵格地圖，由直線分割得到。白色表示可行區(qū)域，黑色表示有障礙物，在矩形柵格地圖上可形象直觀的進(jìn)行路徑規(guī)劃研究，有利于數(shù)據(jù)的存儲和計算。圖1.5柵格地圖1.4.2拓?fù)涞貓D拓?fù)涞貓D（Toplogicalmap）就是用簡單的點線連接表示兩點之間的互通性。在室內(nèi)，節(jié)點代表門、桌子和拐角等位置，之間的連線表示可以到達(dá)。拓?fù)涞貓D分為星型、網(wǎng)狀型和簇狀型拓?fù)涞貓D，如圖1.6所示。由于空間復(fù)雜度比較低，可進(jìn)行有效的路徑