自主導(dǎo)航技術(shù)與應(yīng)用1.1引言自主導(dǎo)航技術(shù)定義:自主導(dǎo)航技術(shù)是指依靠自身設(shè)備和算法，不依賴(lài)外部信號(hào)源，實(shí)現(xiàn)自主定位、導(dǎo)航和控制的技術(shù)。該技術(shù)廣泛應(yīng)用于軍事、航空航天、機(jī)器人等領(lǐng)域，對(duì)于提高系統(tǒng)的自主性和可靠性具有重要意義。技術(shù)特點(diǎn):具有隱蔽性強(qiáng)、抗干擾性高、自主決策能力強(qiáng)等特點(diǎn)。能夠在復(fù)雜環(huán)境下實(shí)現(xiàn)精準(zhǔn)定位和導(dǎo)航，為各種任務(wù)提供可靠支持。核心能力:具有隱蔽性強(qiáng)、抗干擾性高、自主決策能力強(qiáng)等特點(diǎn)。能夠在復(fù)雜環(huán)境下實(shí)現(xiàn)精準(zhǔn)定位和導(dǎo)航，為各種任務(wù)提供可靠支持。1.2典型自主導(dǎo)航方法簡(jiǎn)介目前占主導(dǎo)地位的自主導(dǎo)航技術(shù)有慣性導(dǎo)航、地球物理場(chǎng)（地磁場(chǎng)、重力場(chǎng)）導(dǎo)航和視覺(jué)導(dǎo)航等。從應(yīng)用要求和技術(shù)研究進(jìn)展來(lái)看，自主導(dǎo)航技術(shù)在提升載體在某一環(huán)境下生存能力和滿(mǎn)足任務(wù)特殊導(dǎo)航需求等多方面具有明顯優(yōu)勢(shì)，已成為未來(lái)導(dǎo)航領(lǐng)域技術(shù)發(fā)展的重要方向。1.2.1慣性導(dǎo)航慣性導(dǎo)航系統(tǒng)通過(guò)陀螺儀和加速度計(jì)測(cè)量載體的角速度和加速度，經(jīng)過(guò)積分運(yùn)算得到載體的位置和速度信息。該方法完全自主，不依賴(lài)外部信號(hào)，具有抗電磁干擾能力強(qiáng)、短期精度高的優(yōu)點(diǎn)。慣性導(dǎo)航系統(tǒng)，簡(jiǎn)稱(chēng)慣導(dǎo)系統(tǒng)。通常由慣性測(cè)量裝置、計(jì)算機(jī)、控制顯示器等組成。測(cè)量裝置包括加速度計(jì)和陀螺儀，陀螺儀測(cè)量載體的轉(zhuǎn)動(dòng)，加速度計(jì)測(cè)量載體運(yùn)動(dòng)加速度。計(jì)算機(jī)根據(jù)測(cè)量數(shù)據(jù)計(jì)算載體的速度和位置。控制顯示器顯示各種導(dǎo)航參數(shù)。按照慣性測(cè)量器件在載體上的安裝方式不同，分為平臺(tái)式慣性導(dǎo)航系統(tǒng)和捷聯(lián)式慣性導(dǎo)航系統(tǒng)。1.2.1慣性導(dǎo)航慣性導(dǎo)航優(yōu)勢(shì)與挑戰(zhàn)

優(yōu)勢(shì)在于完全自主，抗電磁干擾能力強(qiáng)，短期精度高，數(shù)據(jù)連續(xù)。但誤差會(huì)隨時(shí)間積累，需要與其他導(dǎo)航方式組合使用以提高精度。慣性導(dǎo)航應(yīng)用領(lǐng)域

廣泛應(yīng)用于軍事裝備、機(jī)器人、航空航天等領(lǐng)域。在軍事領(lǐng)域，慣性導(dǎo)航系統(tǒng)是導(dǎo)彈、飛機(jī)等武器裝備的關(guān)鍵組成部分。1.2.2天文導(dǎo)航

天文導(dǎo)航是利用光學(xué)敏感器觀察太陽(yáng)、月球、行星和恒星等自然天體的位置，以天體的位置確定測(cè)量點(diǎn)位置的一種定位導(dǎo)航方法。天文導(dǎo)航與慣性導(dǎo)航一樣屬于自主導(dǎo)航技術(shù)。天文導(dǎo)航廣泛應(yīng)用在航天、航海、航空領(lǐng)域，是登月、載人航天和遠(yuǎn)洋航海的關(guān)鍵技術(shù)，也是衛(wèi)星和遠(yuǎn)程導(dǎo)航和運(yùn)載火箭等的重要輔助導(dǎo)航方法。1.2.2天文導(dǎo)航

天文導(dǎo)航按照觀測(cè)星體的數(shù)量多少，可分為單星、雙星和多星導(dǎo)航。單星導(dǎo)航需要星跟蹤器保持對(duì)星體的跟蹤觀測(cè)，也稱(chēng)為跟蹤式導(dǎo)航；雙星導(dǎo)航的定位需要根據(jù)雙星的觀測(cè)角度進(jìn)行解算，角度差越接近90度，定位精度越高；多星導(dǎo)航觀測(cè)三個(gè)以上的星體，定位精度優(yōu)于前二者。

天文導(dǎo)航目前主要關(guān)鍵技術(shù)包括：恒星星圖星點(diǎn)特征提取技術(shù)；太陽(yáng)圖像邊緣檢測(cè)及質(zhì)心提取技術(shù)；挖掘多視場(chǎng)、大視場(chǎng)觀測(cè)解算機(jī)理；優(yōu)化星圖處理算法策略；優(yōu)化自主導(dǎo)航濾波及異步時(shí)滯信息組合導(dǎo)航算法等。

天文導(dǎo)航不依賴(lài)于地面設(shè)備的定位導(dǎo)航技術(shù)，定位精度比較高。抗干擾能力強(qiáng)，可靠性高；可同時(shí)提供位置和姿態(tài)信息。但是天文導(dǎo)航受氣候條件的影響觀測(cè)性能，另外輸出的信息不連續(xù)。測(cè)量器件價(jià)格昂貴。1.2.3地球物理場(chǎng)導(dǎo)航地磁導(dǎo)航

地磁導(dǎo)航利用地球磁場(chǎng)強(qiáng)度和梯度的變化來(lái)確定載體的位置。通過(guò)匹配地磁場(chǎng)模型和實(shí)測(cè)數(shù)據(jù)，實(shí)現(xiàn)無(wú)源導(dǎo)航。重力導(dǎo)航

重力導(dǎo)航通過(guò)測(cè)量重力場(chǎng)的變化來(lái)確定載體的位置。該方法具有無(wú)源、穩(wěn)定的特點(diǎn)，適合長(zhǎng)航時(shí)任務(wù)。地球物理場(chǎng)導(dǎo)航應(yīng)用領(lǐng)域

主要應(yīng)用于水下和深海載體導(dǎo)航。在水下機(jī)器人和潛艇等領(lǐng)域，地球物理場(chǎng)導(dǎo)航提供了可靠的定位手段。1.2.3視覺(jué)導(dǎo)航

視覺(jué)導(dǎo)航通過(guò)攝像機(jī)對(duì)周?chē)h(huán)境進(jìn)行圖像采集、經(jīng)過(guò)圖像處理技術(shù)，提取環(huán)境信息特征，并且對(duì)特征進(jìn)行匹配跟蹤，完成自身定位。近年來(lái)，視覺(jué)導(dǎo)航與慣性導(dǎo)航相結(jié)合廣泛應(yīng)用于室內(nèi)導(dǎo)航、機(jī)器人導(dǎo)航、智能車(chē)導(dǎo)航等領(lǐng)域。1.2.3視覺(jué)導(dǎo)航

視覺(jué)導(dǎo)航根據(jù)使用的視覺(jué)傳感器不同，可分為單目、雙目及多目、RGBD視覺(jué)導(dǎo)航技術(shù)。視覺(jué)導(dǎo)航技術(shù)可以適應(yīng)多種不同的場(chǎng)景和環(huán)境，并且可以根據(jù)需要進(jìn)行靈活配置和調(diào)整。但是，視覺(jué)導(dǎo)航技術(shù)通常需要清晰的圖像和良好的光照條件，而在復(fù)雜、暗淡或模糊的環(huán)境中，其性能可能受到限制。實(shí)現(xiàn)高精度的視覺(jué)導(dǎo)航系統(tǒng)通常需要高性能的計(jì)算設(shè)備和復(fù)雜的算法實(shí)現(xiàn)，這可能導(dǎo)致系統(tǒng)的成本較高。視覺(jué)導(dǎo)航技術(shù)對(duì)于環(huán)境中的突發(fā)變化、遮擋物和光照變化等問(wèn)題的魯棒性相對(duì)較低，可能導(dǎo)致導(dǎo)航的失敗或錯(cuò)誤。1.2.3視覺(jué)導(dǎo)航優(yōu)勢(shì)與挑戰(zhàn)

總的來(lái)說(shuō)，視覺(jué)導(dǎo)航技術(shù)具有高精度、實(shí)時(shí)性和獨(dú)立性等優(yōu)點(diǎn)，但也面臨著環(huán)境要求高、成本高和魯棒性相對(duì)差的挑戰(zhàn)。隨著技術(shù)的不斷進(jìn)步和發(fā)展，這些問(wèn)題將逐漸得到解決，使得視覺(jué)導(dǎo)航技術(shù)在各種應(yīng)用領(lǐng)域中得到更廣泛的應(yīng)用。1.3自主導(dǎo)航關(guān)鍵技術(shù)簡(jiǎn)介

自主導(dǎo)航使載體具有獨(dú)立地在未知環(huán)境中進(jìn)行導(dǎo)航和移動(dòng)的能力。實(shí)現(xiàn)自主導(dǎo)航需要借助多種關(guān)鍵技術(shù)，包括自我環(huán)境智能感知、

定位與建圖、路徑規(guī)劃、智能決策等。自我環(huán)境感知：自主導(dǎo)航系統(tǒng)需要能夠感知周?chē)h(huán)境，并獲取準(zhǔn)確的環(huán)境信息，以便做出決策。機(jī)器人常用的感知傳感器如攝像頭、激光雷達(dá)、超聲波傳感器、紅外傳感器等，來(lái)感知障礙物、地形、道路狀況等。定位與建圖：自主導(dǎo)航系統(tǒng)基于對(duì)環(huán)境的感知，建立描述環(huán)境的地圖，環(huán)境地圖有多種形式，如拓?fù)涞貓D、柵格地圖、特征地圖、點(diǎn)云地圖等。在建立環(huán)境地圖上，載體能夠確定自身的準(zhǔn)確位置。1.3自主導(dǎo)航關(guān)鍵技術(shù)簡(jiǎn)介動(dòng)態(tài)路徑規(guī)劃：基于感知和定位信息，自主導(dǎo)航系統(tǒng)需要能夠進(jìn)行路徑規(guī)劃，即根據(jù)當(dāng)前環(huán)境狀態(tài)和目標(biāo)位置，選擇合適的路徑和動(dòng)作來(lái)避開(kāi)障礙物、到達(dá)目標(biāo)等。涉及到路徑規(guī)劃、避障、運(yùn)動(dòng)控制等核心技術(shù)。智能決策與控制：自主導(dǎo)航系統(tǒng)需要能夠做出實(shí)時(shí)決策，并將決策轉(zhuǎn)化為具體的控制指令，以實(shí)現(xiàn)車(chē)輛或機(jī)器人的移動(dòng)。涉及到機(jī)器學(xué)習(xí)、強(qiáng)化學(xué)習(xí)、優(yōu)化算法等關(guān)鍵技術(shù)。1.3自主導(dǎo)航關(guān)鍵技術(shù)簡(jiǎn)介多源信息融合技術(shù)：同時(shí)充分利用多種導(dǎo)航傳感器信息對(duì)慣導(dǎo)系統(tǒng)相關(guān)參數(shù)進(jìn)行誤差校正和反饋，將導(dǎo)航子系統(tǒng)和導(dǎo)航模式進(jìn)行深度耦合。高效多源融合技術(shù)可根據(jù)不同場(chǎng)景進(jìn)行傳感器自動(dòng)選擇以及失效切換等；自主在線(xiàn)重構(gòu)最優(yōu)配置技術(shù)可根據(jù)載體環(huán)境以及運(yùn)動(dòng)狀態(tài)快速收斂出最優(yōu)多源組合導(dǎo)航方式。

以上技術(shù)是實(shí)現(xiàn)自主導(dǎo)航的關(guān)鍵，它們相互協(xié)作可以使載體在未知環(huán)境中具有安全、高效的自主導(dǎo)航能力。1.4自主導(dǎo)航技術(shù)的應(yīng)用領(lǐng)域自動(dòng)駕駛汽車(chē)及智能導(dǎo)航設(shè)備：通過(guò)感知、定位、路徑規(guī)劃和控制等技術(shù)，自主導(dǎo)航系統(tǒng)可以實(shí)現(xiàn)汽車(chē)的自主駕駛，提高交通安全性、道路容量利用率和駕乘舒適度。自主導(dǎo)航技術(shù)可以應(yīng)用于智能導(dǎo)航設(shè)備，提供行人、車(chē)輛和公共交通的導(dǎo)航服務(wù)，幫助用戶(hù)快速準(zhǔn)確地找到目的地。航空航天領(lǐng)域：自主導(dǎo)航技術(shù)在無(wú)人機(jī)和自動(dòng)飛行器領(lǐng)域也有廣泛應(yīng)用。無(wú)人機(jī)可以利用自主導(dǎo)航技術(shù)進(jìn)行航跡規(guī)劃、飛行路徑調(diào)整和障礙物避障等。實(shí)現(xiàn)自主導(dǎo)航不僅能夠降低航天器對(duì)地面測(cè)控的依賴(lài)程度，提高自主生存能力，還能緩解國(guó)土面積有限對(duì)地面測(cè)控站布局的制約，提升航天器在測(cè)控區(qū)外的任務(wù)能力。1.4自主導(dǎo)航技術(shù)的應(yīng)用領(lǐng)域智能機(jī)器人：自主導(dǎo)航技術(shù)可應(yīng)用于物流與倉(cāng)儲(chǔ)機(jī)器人，使其具備自主尋找貨物、搬運(yùn)物品、避開(kāi)障礙物等能力，提高物流效率。應(yīng)用于家庭服務(wù)機(jī)器人，使其能夠在家庭環(huán)境中自主移動(dòng)，執(zhí)行家務(wù)任務(wù)如清潔、照料老人和兒童等。應(yīng)用于礦山和工業(yè)領(lǐng)域中的智能機(jī)器人，實(shí)現(xiàn)地下或危險(xiǎn)環(huán)境下的自主勘探、運(yùn)輸、維護(hù)等任務(wù)。可以用于農(nóng)業(yè)機(jī)械的自主操作，如精準(zhǔn)播種、噴灑農(nóng)藥、收獲等，提高農(nóng)業(yè)生產(chǎn)效率和減少人力成本。1.4自主導(dǎo)航技術(shù)的應(yīng)用領(lǐng)域深海探測(cè)領(lǐng)域：深海感知、導(dǎo)航技術(shù)的發(fā)展具有較大發(fā)展空間。未來(lái)結(jié)合光學(xué)陀螺儀、無(wú)線(xiàn)電羅盤(pán)、聲吶等多學(xué)科導(dǎo)航技術(shù)的深海導(dǎo)航將成為重點(diǎn)關(guān)注領(lǐng)域。暗、湍流海況下執(zhí)行搜索、維修等任務(wù)，其關(guān)鍵技術(shù)挑戰(zhàn)包括高分辨率傳感技術(shù)、未知參數(shù)物體的感知和操縱策略，以及長(zhǎng)時(shí)間自主控制方法。武器裝備：隨著技術(shù)發(fā)展，對(duì)高機(jī)動(dòng)導(dǎo)彈的實(shí)時(shí)機(jī)動(dòng)性能提出了更高需求。近年來(lái)，隨著自標(biāo)定、自瞄準(zhǔn)、自檢測(cè)的光學(xué)捷聯(lián)慣導(dǎo)系統(tǒng)的發(fā)展，精確制導(dǎo)的能力也在不斷的提升，提高了武器系統(tǒng)的實(shí)戰(zhàn)化水平。1.5自主導(dǎo)航技術(shù)的趨勢(shì)1）導(dǎo)航技術(shù)的高可靠性、高集成化

現(xiàn)有成熟的導(dǎo)航、制導(dǎo)與控制系統(tǒng)架構(gòu)多是通過(guò)簡(jiǎn)單的設(shè)備冗余備份關(guān)系提高系統(tǒng)的可靠性，但這都給系統(tǒng)的重量、功耗、體積、實(shí)時(shí)性等指標(biāo)帶來(lái)不利的影響。因此，未來(lái)還需要從系統(tǒng)高可靠性、高集成化等方面設(shè)計(jì)適應(yīng)于小型載體需求的導(dǎo)航系統(tǒng)體系。即要有高度的硬件集成，同時(shí)在軟件上要有基于硬件平臺(tái)的綜合管理系統(tǒng)、多源信息融合、導(dǎo)航、制導(dǎo)與控制算法、協(xié)同組網(wǎng)軟件模塊等。1.5自主導(dǎo)航技術(shù)的趨勢(shì)２）導(dǎo)航技術(shù)的高自主化、高智能化

隨著人工智能的高速發(fā)展，強(qiáng)化學(xué)習(xí)、深度學(xué)習(xí)等智能化的方法將是實(shí)現(xiàn)自主導(dǎo)航的重要手段。為了更好地理解和適應(yīng)復(fù)雜環(huán)境，自主導(dǎo)航系統(tǒng)需要具備語(yǔ)義理解和場(chǎng)景理解的能力。對(duì)圖像、語(yǔ)音等數(shù)據(jù)的語(yǔ)義分析和識(shí)別，以及對(duì)環(huán)境中的物體、行為、語(yǔ)境等進(jìn)行更深入的理解。通過(guò)訓(xùn)練智能體從環(huán)境中不斷學(xué)習(xí)和改進(jìn)，提高自主導(dǎo)航系統(tǒng)的性能和適應(yīng)能力。1.5自主導(dǎo)航技術(shù)的趨勢(shì)3）多機(jī)器人協(xié)同導(dǎo)航

隨著多機(jī)器人應(yīng)用的增多，多機(jī)器人協(xié)同導(dǎo)航成為一個(gè)重要的發(fā)展方向。未來(lái)的自主導(dǎo)航技術(shù)將更加注重多機(jī)器人之間的通信與協(xié)作，實(shí)現(xiàn)多機(jī)器人的智能、高效協(xié)同導(dǎo)航。第2章

導(dǎo)航理論基礎(chǔ)導(dǎo)航，顧名思義就是引導(dǎo)航行的意思，也就是正確地引導(dǎo)載體沿著預(yù)定的航線(xiàn)，以要求的精度，在指定的時(shí)間內(nèi)到達(dá)目的地。為了成功完成預(yù)定的航行任務(wù)，除了需要知道起始點(diǎn)和目標(biāo)位置外，還必須實(shí)時(shí)了解載體的位置、速度、姿態(tài)和航向等導(dǎo)航參數(shù)。本章以小型載體為示例，重點(diǎn)介紹導(dǎo)航的基礎(chǔ)知識(shí)，為后續(xù)自主導(dǎo)航技術(shù)的學(xué)習(xí)提供相關(guān)理論支持。2.1常用坐標(biāo)系宇宙間的物體都是不斷運(yùn)動(dòng)的，運(yùn)動(dòng)只能在相對(duì)的意義下討論。一個(gè)物體在空間中的位置，必須相對(duì)于另一個(gè)物體進(jìn)行確定，或者說(shuō)，一個(gè)坐標(biāo)系的位置只能相對(duì)于另一個(gè)坐標(biāo)系來(lái)確定。坐標(biāo)系是導(dǎo)航計(jì)算的基礎(chǔ)。根據(jù)運(yùn)載體運(yùn)動(dòng)情況不同的導(dǎo)航需求，導(dǎo)航中常用的坐標(biāo)系主要有慣性參考坐標(biāo)系、地球坐標(biāo)系、地理坐標(biāo)系、載體坐標(biāo)系以及傳感器坐標(biāo)系。2.1.1常用坐標(biāo)系的定義

圖2-1地心慣性坐標(biāo)2.1.1常用坐標(biāo)系的定義

圖2-2地球坐標(biāo)系1—北極2—參考子午線(xiàn)3—目標(biāo)投影點(diǎn)4—赤道2.1.1常用坐標(biāo)系的定義

圖2-3地理坐標(biāo)系當(dāng)載體在地球上航行時(shí)，載體相對(duì)于地球的位置不斷發(fā)生改變；而地球上不同地點(diǎn)的地理坐標(biāo)系，其相對(duì)地球坐標(biāo)系的位置是不相同的。也就是說(shuō)，載體相對(duì)地球運(yùn)動(dòng)將引起地理坐標(biāo)系相對(duì)地球坐標(biāo)系轉(zhuǎn)動(dòng)。2.1.1常用坐標(biāo)系的定義

圖2-4載體坐標(biāo)系(a)(b)2.1.1常用坐標(biāo)系的定義

圖2-5陀螺坐標(biāo)系2.1.1常用坐標(biāo)系的定義

圖2-6相機(jī)坐標(biāo)系圖2-7激光雷達(dá)坐標(biāo)系2.1.2坐標(biāo)轉(zhuǎn)換

圖2-8二維坐標(biāo)系旋轉(zhuǎn)圖2-9位置矢量反向旋轉(zhuǎn)2.1.2坐標(biāo)轉(zhuǎn)換

圖2-10位置矢量反向旋轉(zhuǎn)2.1.2坐標(biāo)轉(zhuǎn)換

圖2-11坐標(biāo)軸相對(duì)位置圖2-12第一次轉(zhuǎn)動(dòng)后的位置2.1.2坐標(biāo)轉(zhuǎn)換

圖2-13第二次轉(zhuǎn)動(dòng)后的位置圖2-14第三次轉(zhuǎn)動(dòng)后的位置圖2-15坐標(biāo)系相對(duì)位置綜合圖2.1.2坐標(biāo)轉(zhuǎn)換利用3個(gè)歐拉角可表示任意兩個(gè)坐標(biāo)系之間的方向余弦矩陣。2.1.2坐標(biāo)轉(zhuǎn)換

圖2-16地心慣性坐標(biāo)系和地球坐標(biāo)系之間的角度關(guān)系2.1.2坐標(biāo)轉(zhuǎn)換

2.1.2坐標(biāo)轉(zhuǎn)換

2.2地球物理特性在近地慣性導(dǎo)航中，運(yùn)載體相對(duì)于地球定位，地球的形狀參數(shù)、重力特性對(duì)具體的導(dǎo)航參數(shù)的解算有直接的影響。因此，必須對(duì)地球的形狀及其重力場(chǎng)特性有一定的了解。2.2.1地球參考橢球與曲率半徑 1.地球參考橢球人類(lèi)賴(lài)以生存的地球，實(shí)際上是一個(gè)質(zhì)量非均勻分布、形狀不規(guī)則的幾何體。從整體來(lái)看，地球近似一個(gè)對(duì)稱(chēng)于極軸的扁平旋轉(zhuǎn)橢球體，如圖2-17所示。其截面的輪廓是一個(gè)扁平橢圓，沿赤道方向?yàn)殚L(zhǎng)軸，沿極軸方向?yàn)槎梯S。對(duì)地球表面的每一質(zhì)點(diǎn)，一方面受到地心引力的作用，另一方面又受到離心力的作用。正是在后者的作用下，使地球在靠近赤道的部分向外膨脹，直到各處質(zhì)量所受到的引力與離心力的合力—重力的方向達(dá)到與當(dāng)?shù)厮矫娲怪睘橹埂＿@樣，地球的形狀就成為一個(gè)扁平的旋轉(zhuǎn)橢球體。圖2-17從整體看的地球形狀2.2.1地球參考橢球與曲率半徑

圖2-18地球橢球體的曲率半徑圖1—子午圈2—卯酉圈

3—赤道2.2.2垂線(xiàn)、緯度與高程

圖2-19垂線(xiàn)和緯度地理垂線(xiàn)2—引力垂線(xiàn)

3—地心垂線(xiàn)2.2.2垂線(xiàn)、緯度與高程

圖2-20垂線(xiàn)偏差2.2.2垂線(xiàn)、緯度與高程

圖2-21高程的定義2.2.3地球重力場(chǎng)

圖2-22重力矢量圖

2.3運(yùn)動(dòng)方程及表示2.3.1.輪式機(jī)器人運(yùn)動(dòng)方程及表示1.坐標(biāo)系定義與運(yùn)動(dòng)映射輪式移動(dòng)機(jī)器人的每個(gè)獨(dú)立的輪子即起到運(yùn)動(dòng)的作用，也對(duì)運(yùn)動(dòng)施加了約束。輪式移動(dòng)運(yùn)動(dòng)學(xué)建模有兩種方法：基于作用合成的建模方法和基于運(yùn)動(dòng)約束的建模方法。在輪式機(jī)器人建模的過(guò)程中，首先將機(jī)器人看作是建立在輪子上的剛體，在水平面上運(yùn)動(dòng)。機(jī)器人底盤(pán)在平面上有3個(gè)維度，其中兩個(gè)是平面位置，一個(gè)是繞垂直軸的旋轉(zhuǎn)方向，垂直軸和平面正交。輪子軸、輪子轉(zhuǎn)向關(guān)節(jié)和輪子車(chē)轆關(guān)節(jié)存在額外的自由度，但將機(jī)器人看作剛體，從而可以忽略機(jī)器人內(nèi)部和輪子的關(guān)節(jié)和自由度。2.3.1.輪式機(jī)器人運(yùn)動(dòng)方程及表示

圖2-23坐標(biāo)系定義2.3.1.輪式機(jī)器人運(yùn)動(dòng)方程及表示

2.3.1.輪式機(jī)器人運(yùn)動(dòng)方程及表示

圖2-24差分驅(qū)動(dòng)移動(dòng)機(jī)器人2.3.1.輪式機(jī)器人運(yùn)動(dòng)方程及表示

2.3.2.小型無(wú)人機(jī)運(yùn)動(dòng)方程小型無(wú)人機(jī)的運(yùn)動(dòng)方程如下式，無(wú)人機(jī)的受力示意圖如圖2-25所示。

圖2-25無(wú)人機(jī)受力示意圖2.4載體姿態(tài)及表示

2.4.1輪式機(jī)器人的姿態(tài)表示

(a)(b)(c)圖2-26X-Y-Z固定角坐標(biāo)系，按照Rx(γ)，Ry(β)，Rz(α)的順序旋轉(zhuǎn)2.4.1輪式機(jī)器人的姿態(tài)表示

2.4.2小型無(wú)人機(jī)的姿態(tài)表示

圖2-27小型無(wú)人機(jī)姿態(tài)圖2.4.2小型無(wú)人機(jī)的姿態(tài)表示

2.4.2小型無(wú)人機(jī)的姿態(tài)表示

2.4.2小型無(wú)人機(jī)的姿態(tài)表示姿態(tài)表示方法里面，歐拉角是比較直觀的方法，用三個(gè)角度來(lái)衡量機(jī)體坐標(biāo)和地面坐標(biāo)之間的姿態(tài)傾轉(zhuǎn)角，在小角度時(shí)，基本上姿態(tài)角就等于傾角，但是它的缺點(diǎn)是非線(xiàn)性特點(diǎn)，在俯仰角接近90°的時(shí)候，歐拉角變量會(huì)呈現(xiàn)較大的非線(xiàn)性，在90°具有奇異點(diǎn)，不利于計(jì)算，它的特點(diǎn)是意義直觀，所以歐拉角通常用于向用戶(hù)表示姿態(tài)，而在內(nèi)部計(jì)算和程序?qū)崿F(xiàn)中，則通常采用四元數(shù)或方向余弦矩陣方法。四元數(shù)和方向余弦矩陣的表示方法，都沒(méi)有奇異點(diǎn)的問(wèn)題，能夠性能一致的表示360°全方位的姿態(tài)，在計(jì)算過(guò)程中，直接對(duì)四元數(shù)或者方向余弦矩陣數(shù)值進(jìn)行迭代，相比之下四元數(shù)的表示更加簡(jiǎn)潔，計(jì)算量更小，而方向余弦矩陣則多一些冗余度和計(jì)算值，但是在無(wú)人機(jī)涉及的坐標(biāo)轉(zhuǎn)換計(jì)算方面更加便利直接，兩者都有應(yīng)用。2.5本章小結(jié)本章介紹導(dǎo)航的基礎(chǔ)理論，包括常用的導(dǎo)航坐標(biāo)系及坐標(biāo)系間的轉(zhuǎn)換方法、地球的物理特性、以輪式機(jī)器人和四軸無(wú)人機(jī)為代表的小型機(jī)器人的運(yùn)動(dòng)方程和姿態(tài)表示。這些都是自主導(dǎo)航技術(shù)中需要理解和掌握的內(nèi)容。本章的內(nèi)容為后續(xù)章節(jié)的學(xué)習(xí)提供了理論基礎(chǔ)。第三章慣性導(dǎo)航方法與原理慣性導(dǎo)航技術(shù)是慣性?xún)x表、慣性穩(wěn)定、慣性系統(tǒng)、慣性制導(dǎo)與慣性測(cè)量等及其相關(guān)技術(shù)的總稱(chēng)。廣泛應(yīng)用于航空、航天、航海、陸地導(dǎo)航、大地測(cè)量、鉆井開(kāi)隧道、地質(zhì)勘探、機(jī)器人、車(chē)輛、醫(yī)療設(shè)備以及照相機(jī)、手機(jī)、玩具等領(lǐng)域。本章重點(diǎn)介紹慣性傳感器、慣性導(dǎo)航系統(tǒng)的組成及工作原理，是自主導(dǎo)航的關(guān)鍵技術(shù)。3.1慣性傳感器慣性的定義慣性是指物體保持靜止?fàn)顟B(tài)或勻速直線(xiàn)運(yùn)動(dòng)狀態(tài)的性質(zhì)，是物體的一種固有屬性，表現(xiàn)為物體對(duì)其運(yùn)動(dòng)狀態(tài)變化的一種阻抗程度。慣性敏感器的定義慣性敏感器是指利用慣性原理敏感運(yùn)動(dòng)物體的位置和姿態(tài)變化的裝置。關(guān)鍵部件主要包含陀螺儀和加速度計(jì)3.1.1陀螺儀1.機(jī)械轉(zhuǎn)子陀螺儀定義：陀螺是一個(gè)高速旋轉(zhuǎn)的轉(zhuǎn)子，轉(zhuǎn)子繞對(duì)稱(chēng)軸的旋轉(zhuǎn)稱(chēng)為陀螺轉(zhuǎn)子的自轉(zhuǎn)。把高速旋轉(zhuǎn)的陀螺安裝在一個(gè)懸掛裝置上，使陀螺主軸在空間具有一個(gè)或兩個(gè)轉(zhuǎn)動(dòng)自由度，就構(gòu)成了機(jī)械轉(zhuǎn)子陀螺儀。自由度：確定一個(gè)物體在某坐標(biāo)系中的位置所需要的獨(dú)立坐標(biāo)的數(shù)目，稱(chēng)為該物體的自由度。陀螺儀的自由度數(shù)目，通常是指自轉(zhuǎn)軸可繞其自由旋轉(zhuǎn)的正交軸的數(shù)目。分類(lèi)：二自由度陀螺儀

單自由度陀螺儀3.1.1陀螺儀（1）兩自由度陀螺儀兩自由度陀螺儀由轉(zhuǎn)子、內(nèi)環(huán)、外環(huán)和儀表殼體組成。轉(zhuǎn)子軸、內(nèi)環(huán)軸、外環(huán)軸相交于一點(diǎn)O，O點(diǎn)是既是陀螺儀的重心又是支架中心，它是陀螺儀的不動(dòng)點(diǎn)，陀螺儀的運(yùn)動(dòng)就是圍繞O點(diǎn)進(jìn)行的，可見(jiàn)，陀螺儀的運(yùn)動(dòng)就是一個(gè)剛體繞定點(diǎn)的運(yùn)動(dòng)。陀螺儀轉(zhuǎn)子軸相對(duì)基座（殼體）有兩個(gè)運(yùn)動(dòng)自由度，因此，這種陀螺儀叫做兩自由度基本陀螺儀。圖3-1兩自由度陀螺儀框架1—轉(zhuǎn)子2—內(nèi)環(huán)3—外環(huán)4—?dú)んw3.1.1陀螺儀（1）兩自由度陀螺儀當(dāng)陀螺儀轉(zhuǎn)子高速旋轉(zhuǎn)，同時(shí)又受有不與轉(zhuǎn)子軸方向相重合的外力矩作用時(shí)，轉(zhuǎn)子軸將在外力矩平面相垂直的平面內(nèi)運(yùn)動(dòng)，這種運(yùn)動(dòng)叫進(jìn)動(dòng)，陀螺儀的這種特性叫進(jìn)動(dòng)性。繞OY軸的進(jìn)動(dòng)角速度為：圖3-2在外力矩作用下動(dòng)量矩的變化繞OX軸的進(jìn)動(dòng)角速度為：當(dāng)轉(zhuǎn)子軸與外環(huán)軸垂直時(shí)()，兩表達(dá)式完全相同，當(dāng)值很小，如

時(shí)，

，仍接近原來(lái)的數(shù)值；當(dāng)值較大，如時(shí)，則，僅為原來(lái)值的一半；當(dāng)時(shí)，，轉(zhuǎn)子軸與外環(huán)軸重合，陀螺儀失去一個(gè)轉(zhuǎn)動(dòng)自由度。這時(shí)，作用在外環(huán)軸上的外力矩將使外環(huán)連同內(nèi)環(huán)繞外環(huán)軸轉(zhuǎn)動(dòng)起來(lái)，陀螺儀變得與一般剛體沒(méi)有區(qū)別了，這種現(xiàn)象叫做環(huán)架自鎖。3.1.1陀螺儀（1）兩自由度陀螺儀兩自由度陀螺儀的進(jìn)動(dòng)性，只有在自轉(zhuǎn)軸與外環(huán)軸不重合的情況下才表現(xiàn)出來(lái)，一旦出現(xiàn)了環(huán)架自鎖，也就沒(méi)有進(jìn)動(dòng)性了。因此，在兩自由度陀螺儀的應(yīng)用中，總是把繞內(nèi)環(huán)軸的進(jìn)動(dòng)限制在一定范圍內(nèi)。3.1.1陀螺儀（1）兩自由度陀螺儀從上面的分析，可得出進(jìn)動(dòng)性特點(diǎn):1)進(jìn)動(dòng)方向定律：陀螺儀進(jìn)動(dòng)的方向，就是動(dòng)量矩沿著最短途徑趨向外力矩的右手螺旋方向。2)進(jìn)動(dòng)角速度的大小：當(dāng)為常值時(shí)，進(jìn)動(dòng)角速度與外力矩成正比；當(dāng)外力矩一定時(shí)，進(jìn)動(dòng)角速度與動(dòng)量矩成反比；當(dāng)轉(zhuǎn)子軸與外環(huán)軸的夾角為90°時(shí)，其進(jìn)動(dòng)角速度最小。顯然，進(jìn)動(dòng)角速度的大小與有關(guān)。3)進(jìn)動(dòng)規(guī)律：繞陀螺儀外環(huán)軸加力矩，陀螺繞內(nèi)環(huán)軸進(jìn)動(dòng)。反之，繞內(nèi)環(huán)軸加力矩陀螺繞外環(huán)軸進(jìn)動(dòng)。3.1.1陀螺儀（1）兩自由度陀螺儀當(dāng)兩自由度陀螺儀轉(zhuǎn)子高速度旋轉(zhuǎn)時(shí)，具有很高的抵抗外干擾力矩的能力，使轉(zhuǎn)子軸相對(duì)慣性空間保持穩(wěn)定，這種抵抗外干擾力矩的能力，叫做陀螺儀的穩(wěn)定性或定軸性。把陀螺儀在干擾力矩作用下的緩慢進(jìn)動(dòng)，叫做漂移。其漂移角速度為:由于干擾力矩很小，陀螺動(dòng)量矩很大，在儀表有限的使用時(shí)間內(nèi)轉(zhuǎn)子軸空間位置的改變甚小；另一方面，在外力矩作用下陀螺儀是作角速度進(jìn)動(dòng)，而一般剛體則是作加速度轉(zhuǎn)動(dòng)，在同樣外力矩作用下陀螺儀的運(yùn)動(dòng)比一般剛體慢得多，所以陀螺儀具有很好的穩(wěn)定性。3.1.1陀螺儀（2）單自由度陀螺儀兩自由度陀螺儀具有內(nèi)、外兩個(gè)環(huán)架，因此，內(nèi)外環(huán)繞其軸轉(zhuǎn)動(dòng)時(shí),都將改變轉(zhuǎn)子軸相對(duì)基座（儀表殼體）的方位，這使陀螺儀轉(zhuǎn)子軸具有兩個(gè)角自由度。如果把外環(huán)和儀表基座固定在一起，兩自由度基本陀螺儀將失去一個(gè)外環(huán)和一個(gè)角自由度，這時(shí)，陀螺儀轉(zhuǎn)子相對(duì)儀表基座的運(yùn)動(dòng)只限于繞內(nèi)環(huán)軸的轉(zhuǎn)動(dòng)運(yùn)動(dòng)，這種轉(zhuǎn)子軸相對(duì)儀表基座只有一個(gè)角自由度的陀螺儀，叫做單自由度基本陀螺儀。3.1.1陀螺儀（2）單自由度陀螺儀對(duì)于單自由度基本陀螺儀而言，內(nèi)環(huán)架在結(jié)構(gòu)功用上仍起支承陀螺儀轉(zhuǎn)子的作用，它的運(yùn)動(dòng)特性如圖3-3所示。圖3-3單自由度陀螺儀的運(yùn)動(dòng)示意圖a）坐標(biāo)系重合時(shí)b）坐標(biāo)系不重合時(shí)3.1.1陀螺儀（2）單自由度陀螺儀單自由度陀螺儀的具有如下特點(diǎn):1)在載體相對(duì)單自由度陀螺儀有轉(zhuǎn)動(dòng)運(yùn)動(dòng)(除繞OX軸及XZ軸外)時(shí)，將強(qiáng)迫陀螺儀與載體一起轉(zhuǎn)動(dòng)，同時(shí)，陀螺儀還將產(chǎn)生繞其內(nèi)環(huán)軸的進(jìn)動(dòng)運(yùn)動(dòng)。因此，單自由度陀螺儀喪失了兩自由度基本陀螺儀的穩(wěn)定性。2)在繞內(nèi)軸上有外力矩作用在單自由度陀螺儀上時(shí)，由于繞軸向運(yùn)動(dòng)的喪失，所以陀螺儀將如同一般剛體一樣，將產(chǎn)生繞內(nèi)環(huán)軸的轉(zhuǎn)動(dòng)運(yùn)動(dòng)。3.1.1陀螺儀2.撓性陀螺儀機(jī)械轉(zhuǎn)子陀螺儀是采用環(huán)架裝置并由滾珠軸承來(lái)支承使轉(zhuǎn)子獲得所需的轉(zhuǎn)動(dòng)自由度。但是滾珠軸承不可避免地存在有摩擦力矩而造成陀螺漂移。盡管在工藝上可以把滾珠軸承做得很精密，或者可以采用旋轉(zhuǎn)軸承的辦法來(lái)減小摩擦力矩，然而要使陀螺漂移達(dá)到(0.01°~0.001°)/h以至更小，即達(dá)到慣導(dǎo)級(jí)陀螺的精度要求，則是難以辦到的。因此，要滿(mǎn)足慣導(dǎo)系統(tǒng)對(duì)陀螺精度的要求，關(guān)鍵問(wèn)題之一是必須在支承上進(jìn)行改革。出現(xiàn)了液體支承的液浮陀螺儀、氣浮支承的氣浮陀螺儀，撓性支承的撓性陀螺儀，以及靜電支承的靜電陀螺儀等等。3.1.1陀螺儀2.撓性陀螺儀撓性陀螺儀的轉(zhuǎn)子是由撓性接頭來(lái)支承的。撓性接頭是一種無(wú)摩擦的彈性支承，最簡(jiǎn)單的結(jié)構(gòu)是做成細(xì)軸頸。轉(zhuǎn)子借助于撓性接頭與驅(qū)動(dòng)軸相連，如圖3-4a所示。當(dāng)基座繞著垂直于自轉(zhuǎn)軸的方向出現(xiàn)偏轉(zhuǎn)角時(shí)，將帶動(dòng)驅(qū)動(dòng)軸一起偏轉(zhuǎn)過(guò)同一角度，但陀螺自轉(zhuǎn)軸仍然保持原來(lái)的空間方位穩(wěn)定，如圖3-4b所示。圖3-4撓性接頭支承轉(zhuǎn)子的原理(a)(b)3.1.1陀螺儀3.靜電陀螺儀靜電陀螺儀轉(zhuǎn)子的支承則由靜電吸力來(lái)實(shí)現(xiàn)，在靜電陀螺儀中，轉(zhuǎn)子做成球形，并放置在超高真空的強(qiáng)電場(chǎng)內(nèi)，由強(qiáng)電場(chǎng)所產(chǎn)生的靜電吸力將其支承或稱(chēng)懸浮起來(lái)，如圖3-5所示。圖3-5球面電極對(duì)球轉(zhuǎn)子的靜電吸力3.1.1陀螺儀3.靜電陀螺儀優(yōu)點(diǎn)：靜電陀螺儀是一種精度很高，結(jié)構(gòu)較筒單的慣導(dǎo)系統(tǒng)陀螺儀，尤其適合于作為高精度慣導(dǎo)系統(tǒng)的敏感元件。無(wú)論是對(duì)于艦船、潛艇的慣性導(dǎo)航系統(tǒng)，或是對(duì)于飛機(jī)慣性導(dǎo)航系統(tǒng)以及導(dǎo)彈慣性制導(dǎo)系統(tǒng)，都是適用的。它不僅適用于平臺(tái)式慣導(dǎo)系統(tǒng)，而且特別適用于捷聯(lián)式慣導(dǎo)系統(tǒng)。缺點(diǎn)：靜電陀螺儀制造成本較高。此外，它的角度輸出的精度較低，而且對(duì)漂移誤差的補(bǔ)償也比較復(fù)雜。3.1.1陀螺儀4.激光陀螺儀激光陀螺儀是20世紀(jì)70年代發(fā)展起來(lái)的一種全固態(tài)陀螺儀。不論是框架式陀螺儀還是撓性陀螺儀，都是基于剛體高速旋轉(zhuǎn)時(shí)的陀螺效應(yīng)、利用力學(xué)原理工作的，激光陀螺儀則不同，它是基于光學(xué)的塞格納克效應(yīng)、利用光電原理來(lái)工作的。圖3-6塞格納克原理分析圖3.1.1陀螺儀4.激光陀螺儀圖3-7描述的是四邊形光路構(gòu)成的單自由度激光陀螺光學(xué)原理圖，同一個(gè)激光射束經(jīng)分束器產(chǎn)生兩個(gè)光束，沿著由三面反射鏡(s)形成的四邊形光路分別朝順時(shí)針和逆時(shí)針兩個(gè)相反的方向傳播，四邊形光路外接圓半徑為r。兩個(gè)光束經(jīng)過(guò)分束器匯合后，會(huì)在光檢測(cè)器中產(chǎn)生干涉條紋。圖3-7單自由度激光陀螺原理示意圖3.1.1陀螺儀4.激光陀螺儀優(yōu)點(diǎn)：激光陀螺儀基于光學(xué)原理工作，沒(méi)有剛體轉(zhuǎn)子、環(huán)架等活動(dòng)部件，無(wú)需考慮摩擦力矩補(bǔ)償問(wèn)題；并且對(duì)加速度不敏感，不會(huì)引起加速度誤差，適用于在大加速度環(huán)境下工作；具有更寬的動(dòng)態(tài)測(cè)量范圍，這一點(diǎn)對(duì)捷聯(lián)慣導(dǎo)系統(tǒng)很重要；能夠直接提供數(shù)字量輸出，便于實(shí)現(xiàn)與數(shù)字計(jì)算機(jī)的信號(hào)交聯(lián)；整體結(jié)構(gòu)相對(duì)簡(jiǎn)單，成本低廉，并且啟動(dòng)快、壽命長(zhǎng)、可靠性高。由于激光陀螺儀具有獨(dú)特的優(yōu)點(diǎn)，它極大地促進(jìn)了捷聯(lián)慣導(dǎo)系統(tǒng)的發(fā)展。缺點(diǎn)：激光陀螺儀的諧振腔必須嚴(yán)格密封且其中氣體組成濃度恒定；反射鏡鍍膜工藝要求高；閉鎖問(wèn)題需要解決等。3.1.1陀螺儀5.光纖陀螺儀光纖陀螺儀依據(jù)的物理原理和激光陀螺儀一樣，也是塞格納克效應(yīng)，只不過(guò)其閉合光路是由纏繞在卷軸上光纖線(xiàn)圈構(gòu)成的，如圖3-8所示。圖3-8光纖陀螺示意圖3.1.1陀螺儀5.光纖陀螺儀優(yōu)點(diǎn)：光纖陀螺儀除了具有激光陀螺儀所有的優(yōu)點(diǎn)外，還不需要精確加工、嚴(yán)格密封的光學(xué)諧振腔和高質(zhì)量的反射鏡，減少了復(fù)雜性，降低了成本；可以通過(guò)改變光纖的長(zhǎng)度或光在線(xiàn)圈中的循環(huán)傳播次數(shù)，可以實(shí)現(xiàn)不同的精度，具有非常實(shí)用的設(shè)計(jì)靈活性；繞制的光纖增長(zhǎng)了激光束的檢測(cè)光路，使檢測(cè)靈敏度和分辨率比激光陀螺儀提高了好幾個(gè)數(shù)量級(jí)，有效地克服了激光陀螺儀的閉鎖問(wèn)題。缺點(diǎn)：主要包括溫度瞬態(tài)影響、振動(dòng)影響、偏振影響等，這些問(wèn)題影響了光纖陀螺的精度和穩(wěn)定性，限制了其應(yīng)用的廣泛性。3.1.1陀螺儀6.微機(jī)電陀螺儀微機(jī)電陀螺儀是利用微機(jī)電技術(shù)制作的陀螺儀。與現(xiàn)有機(jī)械轉(zhuǎn)子式陀螺儀或光學(xué)陀螺儀相比，微機(jī)電陀螺儀主要特征有體積和能耗小；成本低廉，適合大批量生產(chǎn)；動(dòng)態(tài)范圍大，可靠性高，可用于惡劣力學(xué)環(huán)境；準(zhǔn)備時(shí)間短，適合快速響應(yīng)；中低精度，適合短時(shí)應(yīng)用或與其他系統(tǒng)組合應(yīng)用。3.1.1陀螺儀6.微機(jī)電陀螺儀振動(dòng)式微機(jī)電陀螺儀按振動(dòng)結(jié)構(gòu)、材料、驅(qū)動(dòng)方式、檢測(cè)方式、工作模式和加工方式等方式進(jìn)行劃分，可以分為以下幾種類(lèi)型。（1）振動(dòng)結(jié)構(gòu)：可分為線(xiàn)振動(dòng)結(jié)構(gòu)和角振動(dòng)結(jié)構(gòu)，常用的包括振梁結(jié)構(gòu)、雙框架結(jié)構(gòu)、平面對(duì)稱(chēng)結(jié)構(gòu)、橫向音叉結(jié)構(gòu)、梳狀音叉結(jié)構(gòu)和梁島結(jié)構(gòu)等。音叉式結(jié)構(gòu)是典型的利用線(xiàn)振動(dòng)來(lái)產(chǎn)生陀螺效應(yīng)的；雙框架結(jié)構(gòu)是典型的利用角振動(dòng)來(lái)產(chǎn)生陀螺效應(yīng)的。3.1.1陀螺儀6.微機(jī)電陀螺儀（2）材料：可分為硅材料和非硅材料。其中，在硅材料陀螺儀中又可以分成單晶硅陀螺儀和多晶硅陀螺儀；在非硅材料中包括石英材料陀螺儀和其他材料陀螺儀。（3）驅(qū)動(dòng)方式：可分為靜電驅(qū)動(dòng)式、電磁驅(qū)動(dòng)式和壓電驅(qū)動(dòng)式等。（4）檢測(cè)方式：可分為電容性檢測(cè)、壓阻性檢測(cè)、壓電性檢測(cè)、光學(xué)檢測(cè)和隧道效應(yīng)檢測(cè)。（5）工作方式：可分為速率陀螺儀和速率積分陀螺儀。（6）加工方式：可分為體微機(jī)械加工、表面機(jī)械加工和LIGA加工方式等。3.1.1陀螺儀6.微機(jī)電陀螺儀目前大部分微機(jī)電陀螺儀都采用振動(dòng)式結(jié)構(gòu)。圖3-9示意了振動(dòng)式微機(jī)電陀螺的工作原理。圖3-10所示為采用雙端音叉式結(jié)構(gòu)的振動(dòng)陀螺儀工作原理框圖。圖3-9振動(dòng)式陀螺儀工作原理圖3-10雙端音叉式振動(dòng)陀螺儀工作原理框圖3.1.2加速度計(jì)加速度計(jì)是慣性導(dǎo)航系統(tǒng)中測(cè)量加速度的敏感元件，它是用來(lái)測(cè)量載體（如艦船飛機(jī)、導(dǎo)彈和其他宇宙飛行器等）相對(duì)于慣性空間的線(xiàn)加速度在某一選定參考坐標(biāo)系軸向的分量。隨著慣性導(dǎo)航技術(shù)的發(fā)展，出現(xiàn)了各種結(jié)構(gòu)和類(lèi)型的加速度計(jì)。按測(cè)量系統(tǒng)的組成形式，可分為開(kāi)環(huán)式加速度計(jì)和閉環(huán)式加速度計(jì)；按檢測(cè)質(zhì)量的支承方式，分為滾珠軸承加速度計(jì)、液浮加速度計(jì)、氣浮加速度計(jì)、磁懸浮加速度計(jì)、撓性加速度計(jì)和靜電加速度計(jì)等；按工作原理可分為擺式加速度計(jì)和非擺式加速度計(jì)。3.1.2加速度計(jì)

圖3-11液浮擺式加速度計(jì)原理示意圖圖3-12擺組件的擺性3.1.2加速度計(jì)2.撓性加速度計(jì)撓性加速度計(jì)也是一種擺式加速度計(jì)，它與液浮加速度計(jì)的主要區(qū)別在于，它的擺組件不是懸浮在液體中，而是彈性地聯(lián)接在某種類(lèi)型的撓性支承上。撓性支承消除了軸承的摩擦力矩，當(dāng)擺組件的偏轉(zhuǎn)角很小時(shí)，由此引入的微小的彈性力矩往往可以忽略。圖3-14撓性加速度計(jì)結(jié)構(gòu)示意圖3.1.2加速度計(jì)3.硅微加速度計(jì)硅微機(jī)電加速度計(jì)（MicromachinedSiliconAccelerometer）是微機(jī)電系統(tǒng)（Micro-Electro-MechanicalSystems,MEMS）技術(shù)最成功的應(yīng)用領(lǐng)域之一。硅微機(jī)電加速度計(jì)按敏感信號(hào)方式分類(lèi)，可分為硅微電容式加速度計(jì)和硅微諧振式加速度計(jì)等；按結(jié)構(gòu)形式分類(lèi)，可分為硅微撓性梳齒式電容加速度計(jì)、硅微撓性“蹺蹺板”擺式加速度計(jì)、硅微撓性“三明治”式加速度計(jì)和硅微靜電懸浮式加速度計(jì)。3.1.2加速度計(jì)3.硅微加速度計(jì)硅微加速度計(jì)按照定齒的配置可以分為定齒均勻配置梳齒電容加速度計(jì)和定齒偏置結(jié)構(gòu)的梳齒電容加速度計(jì)；按照加工方式的不同可分為表面加工梳齒式電容加速度計(jì)和體硅加工梳齒式電容加速度計(jì)。圖3-16定齒均勻配置梳齒式微機(jī)電加速度計(jì)結(jié)構(gòu)示意圖圖3-17定齒偏置梳齒式微機(jī)電加速度計(jì)結(jié)構(gòu)示意圖3.2慣性導(dǎo)航系統(tǒng)3.2.1平臺(tái)式慣性導(dǎo)航系統(tǒng)導(dǎo)航系統(tǒng)在工作過(guò)程中，需要計(jì)算出一系列的導(dǎo)航參數(shù)，如載體的位置(經(jīng)度和緯度)、載體的地速和高度、載體的航向角和姿態(tài)角等。平臺(tái)式慣性導(dǎo)航系統(tǒng)通過(guò)加速度計(jì)測(cè)量的載體加速度信息和平臺(tái)框架上取得的載體的姿態(tài)角信息，就可以計(jì)算出全部的導(dǎo)航參數(shù)。3.2.1平臺(tái)式慣性導(dǎo)航系統(tǒng)圖3-20為慣性導(dǎo)航系統(tǒng)簡(jiǎn)單的原理框圖。圖3-21為平臺(tái)式慣性導(dǎo)航系統(tǒng)各組成部分相互關(guān)系的示意圖。由加速度計(jì)、慣性導(dǎo)航平臺(tái)、導(dǎo)航計(jì)算機(jī)、控制器、速度計(jì)、顯示器組成。圖3-20慣性導(dǎo)航系統(tǒng)簡(jiǎn)單的原理框圖圖3-21慣性導(dǎo)航系統(tǒng)各組成部分示意圖3.2.1平臺(tái)式慣性導(dǎo)航系統(tǒng)平臺(tái)式慣性導(dǎo)航系統(tǒng)按所選用的導(dǎo)航坐標(biāo)系可分為以下幾種：1.當(dāng)?shù)厮矫鎽T性導(dǎo)航系統(tǒng)2.空間穩(wěn)定慣性導(dǎo)航系統(tǒng)由于載體在空間作任意運(yùn)動(dòng)，要測(cè)出載體的位置和有關(guān)參數(shù)，慣性導(dǎo)航系統(tǒng)必須具有三個(gè)通道與三維空間相對(duì)應(yīng)。如圖3-22所示的慣性導(dǎo)航平臺(tái)是由三個(gè)單自由度的陀螺儀組成的三軸平臺(tái)。圖3-22平臺(tái)式慣性導(dǎo)航系統(tǒng)的組成結(jié)構(gòu)圖3.2.2平臺(tái)式慣性導(dǎo)航系統(tǒng)力學(xué)編排方程

圖3-23地理坐標(biāo)系和地球坐標(biāo)系3.2.2平臺(tái)式慣性導(dǎo)航系統(tǒng)力學(xué)編排方程

3.2.2平臺(tái)式慣性導(dǎo)航系統(tǒng)力學(xué)編排方程(2).速度計(jì)算載體在當(dāng)?shù)厮矫?地平面)內(nèi)的速度，即地速為(3).經(jīng)度和緯度計(jì)算載體所在位置的地理緯度和經(jīng)度可由下列方程求得：指北方位慣性導(dǎo)航系統(tǒng)原理圖，如圖3-24所示。圖3-24指北方位慣性導(dǎo)航系統(tǒng)原理方框圖3.2.2平臺(tái)式慣性導(dǎo)航系統(tǒng)力學(xué)編排方程

圖3-25游動(dòng)系與地理系的關(guān)系

3.2.2平臺(tái)式慣性導(dǎo)航系統(tǒng)力學(xué)編排方程

3.2.2平臺(tái)式慣性導(dǎo)航系統(tǒng)力學(xué)編排方程

3.2.2平臺(tái)式慣性導(dǎo)航系統(tǒng)力學(xué)編排方程

采用方向余弦法的游動(dòng)方位系統(tǒng)的慣性導(dǎo)航原理方框圖如圖3-26所示。

圖3-26游動(dòng)方位平臺(tái)慣性導(dǎo)航系統(tǒng)力學(xué)編排框圖3.2.2平臺(tái)式慣性導(dǎo)航系統(tǒng)力學(xué)編排方程3.自由方位慣性導(dǎo)航系統(tǒng)的力學(xué)編排方程圖3-27自由方位平臺(tái)坐標(biāo)系

3.2.2平臺(tái)式慣性導(dǎo)航系統(tǒng)力學(xué)編排方程

3.2.2平臺(tái)式慣性導(dǎo)航系統(tǒng)力學(xué)編排方程(4).

平臺(tái)指令角速度如圖3-28所示給出了采用方向余弦法的自由方位慣性導(dǎo)航系統(tǒng)的原理方框圖。圖3-28自由方位慣性導(dǎo)航系統(tǒng)原理方框圖3.2.2平臺(tái)式慣性導(dǎo)航系統(tǒng)力學(xué)編排方程慣性導(dǎo)航系統(tǒng)無(wú)論在元器件特性、結(jié)構(gòu)安裝或其他工程環(huán)節(jié)都不可避免地存在誤差。這些誤差因素稱(chēng)為誤差源，大體上可分為以下幾類(lèi)：①元件誤差；②安裝誤差；③初始條件誤差；④原理誤差；根據(jù)一般情況，所有誤差源均可看成是對(duì)理想特性的小擾動(dòng)，因而各個(gè)誤差量都是對(duì)系統(tǒng)的一階小偏差輸入量。⑤計(jì)算誤差；⑥運(yùn)動(dòng)干擾；⑦其他誤差3.2.2平臺(tái)式慣性導(dǎo)航系統(tǒng)力學(xué)編排方程4.平臺(tái)式慣性導(dǎo)航系統(tǒng)的初始對(duì)準(zhǔn)平臺(tái)對(duì)準(zhǔn)的方法可分為兩類(lèi)。一是引入外部基準(zhǔn)，通過(guò)光學(xué)或機(jī)電方法，將外部參考坐標(biāo)系引入平臺(tái)，平臺(tái)對(duì)準(zhǔn)外部提供的姿態(tài)基準(zhǔn)方向；二是利用慣性導(dǎo)航系統(tǒng)本身的敏感元件——陀螺儀與加速度計(jì)——測(cè)得的信號(hào)，結(jié)合系統(tǒng)作用原理進(jìn)行自動(dòng)對(duì)準(zhǔn)，也就是自主式對(duì)準(zhǔn)。根據(jù)對(duì)準(zhǔn)精度要求，把初始對(duì)準(zhǔn)過(guò)程分為粗對(duì)準(zhǔn)與精對(duì)準(zhǔn)兩個(gè)步驟。首先進(jìn)行粗對(duì)準(zhǔn)、這時(shí)縮短對(duì)準(zhǔn)時(shí)間是主要的。要求盡快地將平臺(tái)對(duì)準(zhǔn)在一定精度范圍之內(nèi)。完成粗對(duì)準(zhǔn)之后，接著進(jìn)行精對(duì)準(zhǔn)，即要求實(shí)際平臺(tái)系與理想平臺(tái)系之間的偏差在要求的精度指標(biāo)以?xún)?nèi)。精對(duì)準(zhǔn)結(jié)束時(shí)的精度就是平臺(tái)進(jìn)入導(dǎo)航狀態(tài)時(shí)的初始精度。3.2.3捷聯(lián)式慣性導(dǎo)航系統(tǒng)在前述的平臺(tái)式慣性導(dǎo)航系統(tǒng)中，慣性平臺(tái)本身體積和質(zhì)量較大，平臺(tái)本身又是一個(gè)高精度且結(jié)構(gòu)十分復(fù)雜的機(jī)電控制系統(tǒng)，它所需的加工制造成本較高。特別是由于結(jié)構(gòu)復(fù)雜，故障率較高，因而慣性導(dǎo)航系統(tǒng)工作的可靠性受到很大影響。正是出于這方面的考慮，在發(fā)展平臺(tái)式慣性導(dǎo)航系統(tǒng)的同時(shí)，人們就開(kāi)始了對(duì)另一種慣性導(dǎo)航系統(tǒng)的研究，這就是捷聯(lián)式慣性導(dǎo)航系統(tǒng)。捷聯(lián)式慣性導(dǎo)航系統(tǒng)面臨兩方面的技術(shù)難點(diǎn)。一是對(duì)慣性器件特別是陀螺儀的技術(shù)要求更加嚴(yán)格和苛刻，二是對(duì)計(jì)算機(jī)的計(jì)算速度和精度也提出了相當(dāng)高的要求。3.2.3捷聯(lián)式慣性導(dǎo)航系統(tǒng)與平臺(tái)式慣性導(dǎo)航系統(tǒng)相比，捷聯(lián)式慣性導(dǎo)航系統(tǒng)有如下特點(diǎn):①取消了實(shí)體平臺(tái)，代之以大量的實(shí)時(shí)軟件，大大降低了系統(tǒng)的體積質(zhì)量和成本。②取消了實(shí)體平臺(tái)，減少了系統(tǒng)中的機(jī)電元件，而對(duì)加速度計(jì)和陀螺儀容易實(shí)現(xiàn)多余度配置方案，因此系統(tǒng)工作的可靠性大大提高。③較平臺(tái)系統(tǒng)維護(hù)簡(jiǎn)便，故障率低。④由于動(dòng)態(tài)環(huán)境惡劣，因而對(duì)慣性器件的要求比平臺(tái)系統(tǒng)高，也沒(méi)有平臺(tái)系統(tǒng)標(biāo)定慣性器件的方便條件。為此，器件要求有較高的參數(shù)穩(wěn)定性。3.2.3捷聯(lián)式慣性導(dǎo)航系統(tǒng)的力學(xué)編排方程捷聯(lián)式慣性導(dǎo)航系統(tǒng)就其程序編排而言，可分為兩種：一種是在慣性坐標(biāo)系中求解導(dǎo)航方程式，另一種是在導(dǎo)航坐標(biāo)系中求解導(dǎo)航方程式。這種情形是與平臺(tái)式慣性導(dǎo)航系統(tǒng)相類(lèi)似的。

圖3-29游動(dòng)方位坐標(biāo)系與載體坐標(biāo)系之間的關(guān)系3.2.3捷聯(lián)式慣性導(dǎo)航系統(tǒng)的力學(xué)編排方程

3.2.3捷聯(lián)式慣性導(dǎo)航系統(tǒng)的力學(xué)編排方程

3.2.3捷聯(lián)式慣性導(dǎo)航系統(tǒng)的力學(xué)編排方程

一個(gè)坐標(biāo)系相對(duì)另一個(gè)坐標(biāo)系的轉(zhuǎn)動(dòng)可以用四元數(shù)唯一地表示出來(lái)。用四元數(shù)來(lái)描述載體坐標(biāo)系相對(duì)游動(dòng)方位坐標(biāo)系的轉(zhuǎn)動(dòng)運(yùn)動(dòng)時(shí)，可得3.2.3捷聯(lián)式慣性導(dǎo)航系統(tǒng)的力學(xué)編排方程4.四元數(shù)微分方程四元數(shù)運(yùn)動(dòng)學(xué)微分方程為

3.2.3捷聯(lián)式慣性導(dǎo)航系統(tǒng)的力學(xué)編排方程4.捷聯(lián)式慣性導(dǎo)航系統(tǒng)誤差分析與初始對(duì)準(zhǔn)捷聯(lián)式慣性導(dǎo)航系統(tǒng)和平臺(tái)式慣性導(dǎo)航系統(tǒng)的主要區(qū)別是，前者用“數(shù)學(xué)平臺(tái)”，而后者用實(shí)體的物理平臺(tái)。從基本原理上講，兩種系統(tǒng)沒(méi)有本質(zhì)的區(qū)別。但是，捷聯(lián)式慣性導(dǎo)航系統(tǒng)的一些特點(diǎn)，使它在性能上和平臺(tái)式慣性導(dǎo)航系統(tǒng)有所不同。

3.3組合導(dǎo)航系統(tǒng)在實(shí)際使用中，慣導(dǎo)系統(tǒng)的缺點(diǎn)也十分明顯：首先是初始對(duì)準(zhǔn)完成后，系統(tǒng)的導(dǎo)航精度隨飛行時(shí)間增加而不斷下降，難以滿(mǎn)足遠(yuǎn)距離、高精度的導(dǎo)航需求；此外，一般慣導(dǎo)系統(tǒng)加溫和初始對(duì)準(zhǔn)所需的時(shí)間比較長(zhǎng)，不利于某些特定條件下的快速反應(yīng)。在當(dāng)前對(duì)導(dǎo)航系統(tǒng)性能要求越來(lái)越高的情況下，可以通過(guò)將不同導(dǎo)航系統(tǒng)按某種方式結(jié)合在一起構(gòu)成組合導(dǎo)航系統(tǒng)，從而實(shí)現(xiàn)多信息組合，提高導(dǎo)航系統(tǒng)的各項(xiàng)性能指標(biāo)。組合導(dǎo)航系統(tǒng)在具體實(shí)施方法上可以分為兩種：一種是應(yīng)用古典自動(dòng)控制理論的方法，即采用控制系統(tǒng)反饋校正方法來(lái)抑制系統(tǒng)誤差；另一種方法是應(yīng)用卡爾曼濾波技術(shù)的方法，即通過(guò)卡爾曼濾波方法估計(jì)出系統(tǒng)誤差，并利用誤差估計(jì)值去校正系統(tǒng)，這是目前應(yīng)用最為廣泛的方法。3.3.1組合導(dǎo)航的組合類(lèi)型圖3-31慣性—多普勒雷達(dá)組合示意圖1.慣性-速度組合系統(tǒng)所謂慣性-速度組合系統(tǒng)，就是把慣性導(dǎo)航系統(tǒng)的速度信息與另一種導(dǎo)航系統(tǒng)的速度信息，組合在一起所構(gòu)成的導(dǎo)航系統(tǒng)。為便于說(shuō)明這種組合導(dǎo)航方式的原理，現(xiàn)以慣性-多普勒雷達(dá)組合系統(tǒng)為例，進(jìn)行簡(jiǎn)單分析。系統(tǒng)的原理框圖如圖3-31所示。3.3.1組合導(dǎo)航的組合類(lèi)型圖3-32慣性—位置組合系統(tǒng)原理2.慣性-位置組合系統(tǒng)所謂慣性-位置組合系統(tǒng)，指的是把慣性導(dǎo)航系統(tǒng)的位置信息與另一種導(dǎo)航系統(tǒng)的位置信息組合起來(lái)所構(gòu)成的導(dǎo)航系統(tǒng)。圖3-32所示的是慣性-位置直接組合的一種方案，該方案把其他導(dǎo)航系統(tǒng)所得到的位置信息(緯度)與慣導(dǎo)系統(tǒng)所輸出的緯度信號(hào)進(jìn)行比較，然后以其差值，通過(guò)這三個(gè)環(huán)節(jié)反饋到慣導(dǎo)系統(tǒng)的相應(yīng)環(huán)節(jié)。3.3.1組合導(dǎo)航的組合類(lèi)型圖3-33直接法濾波示意圖

3.3.1組合導(dǎo)航的組合類(lèi)型

采用間接法濾波時(shí)，系統(tǒng)方程中的主要部分是導(dǎo)航參數(shù)誤差方程。間接法濾波時(shí)，所謂“系統(tǒng)”實(shí)際上就是導(dǎo)航系統(tǒng)中各種誤差的“組合體”，它不參與原系統(tǒng)(導(dǎo)航系統(tǒng))的計(jì)算流程，即濾波過(guò)程是與原系統(tǒng)無(wú)關(guān)的獨(dú)立過(guò)程。對(duì)原系統(tǒng)來(lái)講，除了接收誤差估計(jì)值的校正外，系統(tǒng)也保持其工作的獨(dú)立性，這使得間接法能充分發(fā)揮各個(gè)系統(tǒng)的特點(diǎn)。所以，組合導(dǎo)航系統(tǒng)一般都采用間接法濾波。3.3.2典型慣性組合導(dǎo)航系統(tǒng)1.SINS/GPS組合導(dǎo)航系統(tǒng)捷聯(lián)慣導(dǎo)系統(tǒng)與GPS構(gòu)成的組合導(dǎo)航系統(tǒng)在實(shí)際中應(yīng)用比較廣泛。SINS/GPS系統(tǒng)的組合有多種方案，主要可分為硬件一體化組合與軟件組合。硬件一體化組合是指將慣導(dǎo)的主要部件與GPS接收機(jī)的主要部分構(gòu)成硬件一體化設(shè)備，它提高了GPS接收機(jī)快捕獲衛(wèi)星信號(hào)的能力，適用于高動(dòng)態(tài)應(yīng)用環(huán)境中。軟件組合則是指由安裝在載體上的慣導(dǎo)和GPS接收機(jī)各自獨(dú)立觀測(cè)并通過(guò)專(zhuān)用接口將觀測(cè)數(shù)據(jù)輸入中心計(jì)算機(jī)，作時(shí)空同步后，按濾波方法進(jìn)行組合處理，并通過(guò)相應(yīng)的理論及算法提取所需要的信息。軟件組合是目前研究得最多的方案，它又可分為位置組合和偽距差(偽距率)組合。位置組合利用慣性導(dǎo)航誤差模型與GPS位置誤差模型進(jìn)行組合，這種方法首先要解算GPS定位結(jié)果，組合系統(tǒng)性能依賴(lài)于GPS位置誤差模型的精確性。偽差組合方法直接利用接收機(jī)觀測(cè)的偽距數(shù)據(jù)與根據(jù)SINS解算的偽距之差作為觀測(cè)量進(jìn)行組合運(yùn)算。3.3.2典型慣性組合導(dǎo)航系統(tǒng)(1).SINS/GPS狀態(tài)模型SINS的狀態(tài)向量取為GPS的狀態(tài)向量為于是，SINS/GPS組合系統(tǒng)的狀態(tài)向量就是這兩個(gè)子系統(tǒng)狀態(tài)向量的組合，即組合系統(tǒng)狀態(tài)模型如下3.3.2典型慣性組合導(dǎo)航系統(tǒng)

由SINS/GPS組合導(dǎo)航系統(tǒng)狀態(tài)模型與量測(cè)模型，利用卡爾曼濾波方程就可以對(duì)系統(tǒng)狀態(tài)量進(jìn)行估計(jì)解算，得到系統(tǒng)定位誤差與速度誤差等參數(shù)的估計(jì)值，再采用輸出校正或反饋校正就可以完成組合系統(tǒng)的導(dǎo)航參數(shù)輸出。3.3.2典型慣性組合導(dǎo)航系統(tǒng)2.SINS/北斗組合導(dǎo)航系統(tǒng)將北斗雙星定位和捷聯(lián)慣導(dǎo)系統(tǒng)進(jìn)行組合，對(duì)于慣導(dǎo)系統(tǒng)可以實(shí)現(xiàn)慣性傳感器校準(zhǔn)、慣導(dǎo)系統(tǒng)空中對(duì)準(zhǔn)、慣導(dǎo)系統(tǒng)高度通道穩(wěn)定等，從而有效地提高慣導(dǎo)系統(tǒng)的性能和精度。因此，SINS/北斗組合導(dǎo)航系統(tǒng)也是一種比較理想的組合導(dǎo)航系統(tǒng)，是目前我國(guó)導(dǎo)航技術(shù)的發(fā)展方向之一。針對(duì)北斗雙星定位系統(tǒng)的實(shí)際特點(diǎn)，通常對(duì)雙星定位系統(tǒng)不做誤差狀態(tài)修正，即在SINS/北斗組合導(dǎo)航系統(tǒng)中北斗雙星定位系統(tǒng)仍獨(dú)立工作，組合的作用僅表現(xiàn)在利用北斗雙星定位系統(tǒng)的位置信息進(jìn)行組合，用雙星定位系統(tǒng)輔助捷聯(lián)慣導(dǎo)系統(tǒng)。這種組合模式的優(yōu)點(diǎn)是：組合工作比較簡(jiǎn)單，便于工程實(shí)現(xiàn)，而且兩個(gè)系統(tǒng)仍保持一定的獨(dú)立工作狀態(tài)，又使導(dǎo)航信息有一定的余度，具有良好的組合效果，因此也是一種普遍使用的組合模式。3.3.2典型慣性組合導(dǎo)航系統(tǒng)(1).SINS/北斗組合導(dǎo)航系統(tǒng)狀態(tài)模型SINS/北斗組合導(dǎo)航系統(tǒng)的15維狀態(tài)向量為：結(jié)合各個(gè)誤差模型方程，可以得到組合導(dǎo)航定位系統(tǒng)的狀態(tài)方程：

3.3.2典型慣性組合導(dǎo)航系統(tǒng)(2).SINS/北斗組合導(dǎo)航系統(tǒng)量測(cè)模型選取位置、速度的組合模式，因此組合系統(tǒng)的量測(cè)變量有兩種，分別為位置測(cè)量值和速度測(cè)量值。位置測(cè)量值為慣導(dǎo)系統(tǒng)解算后得到的位置信息和北斗衛(wèi)星接收機(jī)輸出的位置的差值,速度測(cè)量值為兩個(gè)子系統(tǒng)各自經(jīng)過(guò)解算后輸出的速度信息的差值。將SINS輸出的信息和BDS接收機(jī)輸出的信息作差，可以得到組合導(dǎo)航定位系統(tǒng)的量測(cè)方程為:3.4多源信息融合方法多源信息融合也叫多傳感器信息融合(multi-sensorinformationfusion)，是20世紀(jì)80年代出現(xiàn)的一個(gè)新興學(xué)科，它是將不同傳感器對(duì)某一目標(biāo)或環(huán)境特征描述的信息，綜合成統(tǒng)一的特征表達(dá)信息及其處理的過(guò)程。上述的組合導(dǎo)航系統(tǒng)，就是融合多種傳感器的測(cè)量結(jié)果，給出最優(yōu)的載體參數(shù)信息。下面我們首先討論信息融合的特點(diǎn)，然后給出信息融合的常用方法。多傳感器信息融合的基本原理如圖3-34所示。圖3-34多傳感器數(shù)據(jù)融合的基本原理3.4.1信息融合的基本概念多傳感器信息融合實(shí)際上是對(duì)人腦綜合處理復(fù)雜問(wèn)題的一種功能模擬。在多傳感器系統(tǒng)中，各種傳感器提供的信息可能具有不同的特征，如模糊的或確定的、時(shí)變的或非時(shí)變的、快變的或緩變的、實(shí)時(shí)的或非實(shí)時(shí)的、全面的或不全面的、可靠的或非可靠的、相互支持的或相互矛盾的。多傳感器信息融合的主要特點(diǎn)如下：1.提高了對(duì)目標(biāo)或環(huán)境描述的能力。2.提高了對(duì)系統(tǒng)描述的精度。3.提高了系統(tǒng)的運(yùn)行效率。4.提高了系統(tǒng)的可靠性和容錯(cuò)能力。5.可降低系統(tǒng)的成本。3.4.2信息融合的基本方法多傳感器信息融合算法有很多種，但尚無(wú)一種通用的方法能對(duì)各種傳感器信息進(jìn)行融合處理，一般都是依據(jù)具體的應(yīng)用場(chǎng)合而定。在多傳感器信息融合過(guò)程，信息處理過(guò)程的基本功能包括相關(guān)、估計(jì)和識(shí)別。相關(guān)處理要求對(duì)多源信息的相關(guān)性進(jìn)行定量分析，按照一定的判據(jù)原則，將信息分成不同的集合，每個(gè)集合的信息都與同一源(目標(biāo)或事件)關(guān)聯(lián)，其處理方法常有：最近鄰法則、極大似然法、最優(yōu)差別法、統(tǒng)計(jì)關(guān)聯(lián)法、聚類(lèi)分析法等估計(jì)處理是通過(guò)對(duì)各種已知信息的綜合處理來(lái)實(shí)現(xiàn)對(duì)待測(cè)參數(shù)及目標(biāo)狀態(tài)的估計(jì)，通常有如下的幾種處理方法：（1）加權(quán)平均法。（2）數(shù)理統(tǒng)計(jì)法。（3）證據(jù)決策理論。（4）選舉決策法。（5）產(chǎn)生式規(guī)則。（6）卡爾曼濾波。（7）自適應(yīng)人工神經(jīng)網(wǎng)絡(luò)法。3.4.2KALMAN濾波算法1.卡爾曼濾波在多傳感器跟蹤系統(tǒng)中廣泛使用的狀態(tài)估計(jì)技術(shù)是卡爾曼濾波方法，它是研究多傳感器信息融合估計(jì)的基礎(chǔ)。狀態(tài)估計(jì)的目的是對(duì)目標(biāo)過(guò)去的運(yùn)動(dòng)狀態(tài)進(jìn)行平滑，對(duì)目標(biāo)現(xiàn)在的運(yùn)動(dòng)狀態(tài)進(jìn)行濾波，對(duì)目標(biāo)未來(lái)的運(yùn)動(dòng)狀態(tài)進(jìn)行預(yù)測(cè)。卡爾曼濾波是對(duì)隨機(jī)信號(hào)作估計(jì)的算法之一。與最小二乘、維納濾波等諸多估計(jì)算法相比，卡爾曼濾波具有顯著的優(yōu)點(diǎn)；采用狀態(tài)空間法在時(shí)域內(nèi)設(shè)計(jì)濾波器，用狀態(tài)方程描述任何復(fù)雜多維信號(hào)的動(dòng)力學(xué)特性，避開(kāi)了在頻域內(nèi)對(duì)信號(hào)功率譜作分解帶來(lái)的麻煩，濾波器設(shè)計(jì)簡(jiǎn)單；采用遞推算法，實(shí)時(shí)觀測(cè)信息進(jìn)行估計(jì)，而不必存儲(chǔ)時(shí)間過(guò)程中的所有測(cè)量。3.4.2KALMAN濾波算法1.卡爾曼濾波根據(jù)系統(tǒng)方程的不同，卡爾曼濾波基本方程包括連續(xù)型卡爾曼濾波方程與離散型卡爾曼濾波方程，在工程上常用的是離散型卡爾曼濾波方程。假定被估計(jì)系統(tǒng)方程為則離散型卡爾曼濾波基本方程為3.4.2KALMAN濾波算法2.擴(kuò)展卡爾曼濾波卡爾曼濾波是在線(xiàn)性高斯情況下利用最小均方差準(zhǔn)則獲得目標(biāo)的動(dòng)態(tài)估計(jì)，但在實(shí)際系統(tǒng)中，許多情況下觀測(cè)數(shù)據(jù)與目標(biāo)動(dòng)態(tài)參數(shù)間的關(guān)系是非線(xiàn)性的。對(duì)于非線(xiàn)性濾波問(wèn)題，至今尚未得到完善的解法。通常的處理方法是利用線(xiàn)性化技巧將非線(xiàn)性濾波問(wèn)題轉(zhuǎn)化為一個(gè)近似的線(xiàn)性濾波問(wèn)題，套用線(xiàn)性濾波理論得到求解原非線(xiàn)性濾波問(wèn)題的次優(yōu)濾波算法，其中最常用的線(xiàn)性化方法是泰勒級(jí)數(shù)展開(kāi)，所得到的濾波方法是擴(kuò)展卡爾曼濾波(EKF)。擴(kuò)展卡爾曼濾波(EKF)通過(guò)泰勒級(jí)數(shù)的一階或者二階展開(kāi)式獲得，忽略了泰勒展開(kāi)的高階項(xiàng)，將非線(xiàn)性濾波問(wèn)題轉(zhuǎn)換為近似線(xiàn)性濾波問(wèn)題，再使用線(xiàn)性濾波的方法來(lái)進(jìn)行處理。3.4.2KALMAN濾波算法EKF算法如下：狀態(tài)一步預(yù)測(cè)為：協(xié)方差一步預(yù)測(cè)為：量測(cè)一步預(yù)測(cè)為：狀態(tài)更新方程為：協(xié)方差更新方程為：3.4.2KALMAN濾波算法3.不敏卡爾曼濾波不敏卡爾曼濾波（UnscentedKalmanFilter,UKF）是一種非線(xiàn)性濾波方法。UKF對(duì)狀態(tài)向量的PDF進(jìn)行近似化，表現(xiàn)為一系列選取好的采樣點(diǎn)。這些采樣點(diǎn)完全體現(xiàn)了高斯密度的真實(shí)均值和協(xié)方差。當(dāng)這些點(diǎn)經(jīng)過(guò)任何非線(xiàn)性系統(tǒng)的傳遞后，得到的后驗(yàn)均值和協(xié)方差都能夠精確到二階（即對(duì)系統(tǒng)的非線(xiàn)性強(qiáng)度不敏感）。UKF算法的基本思想是將系統(tǒng)的統(tǒng)計(jì)特性（如均值和協(xié)方差），通過(guò)一系列精確選擇的求積點(diǎn)和各求積點(diǎn)相對(duì)應(yīng)的權(quán)值進(jìn)行傳遞，避免了線(xiàn)性化過(guò)程中產(chǎn)生的舍入誤差等情況，既保留了原系統(tǒng)的統(tǒng)計(jì)特性，同時(shí)也能提高算法的精度，在非線(xiàn)性系統(tǒng)濾波估計(jì)問(wèn)題中的應(yīng)用十分廣泛。3.4.2KALMAN濾波算法基于不敏（Unscented）變換的UKF算法具體步驟如下：

(2)狀態(tài)預(yù)測(cè)及其協(xié)方差:3.4.2KALMAN濾波算法(3)量測(cè)預(yù)測(cè)值及其協(xié)方差：(4)狀態(tài)方程及其協(xié)方差矩陣更新互協(xié)方差和增益3.5本章小結(jié)慣性導(dǎo)航技術(shù)是自主導(dǎo)航的核心技術(shù)，本章從慣性傳感器入手，介紹陀螺儀和加速度計(jì)的種類(lèi)及工作原理；然后給出平臺(tái)式及捷聯(lián)式慣性導(dǎo)航系統(tǒng)的組成和力學(xué)編排方程，討論了慣性導(dǎo)航系統(tǒng)的誤差源及初始對(duì)準(zhǔn)問(wèn)題。并在此基礎(chǔ)上，融入其它導(dǎo)航方法，給出了幾種常見(jiàn)的組合導(dǎo)航系統(tǒng)。最后，介紹了目前常用的多傳感器融合方法及其數(shù)學(xué)原理。這些都是慣性導(dǎo)航技術(shù)的核心知識(shí)點(diǎn)，為后續(xù)章節(jié)的學(xué)習(xí)提供理論基礎(chǔ)。第4章

自主導(dǎo)航系統(tǒng)的環(huán)境感知前面章節(jié)介紹了自主導(dǎo)航的理論基礎(chǔ)及慣性導(dǎo)航技術(shù)，對(duì)于機(jī)器人應(yīng)用而言，僅用慣性導(dǎo)航系統(tǒng)是不完備的，需要其它信息輔助完成自主導(dǎo)航定位。對(duì)于海面艦船、航空航天等大型軍用設(shè)備，常采用慣性導(dǎo)航技術(shù)與天文、地磁、衛(wèi)星等導(dǎo)航方式進(jìn)行組合導(dǎo)航。而在小型的機(jī)器人系統(tǒng)中，常采用激光雷達(dá)及視覺(jué)傳感器等小型設(shè)備完成自主導(dǎo)航定位。本章中以小型機(jī)器人應(yīng)用為背景，介紹常用的激光雷達(dá)、視覺(jué)傳感器，以及基于二者對(duì)環(huán)境進(jìn)行感知，建立環(huán)境地圖的主要過(guò)程。4.1環(huán)境感知常用傳感器雷達(dá)是指利用探測(cè)介質(zhì)探測(cè)物體距離的設(shè)備，比如無(wú)線(xiàn)電測(cè)距雷達(dá)、激光測(cè)距雷達(dá)、超聲波測(cè)距雷達(dá)等，如圖4-1所示。由于激光具有很好的抗干擾性和直線(xiàn)傳播特性，因此激光測(cè)距具有很高的精度。激光雷達(dá)測(cè)距精度往往可以達(dá)到厘米級(jí)或毫米級(jí)，廣泛應(yīng)用于機(jī)器人導(dǎo)航避障、無(wú)人駕駛汽車(chē)、安防、智能交互等領(lǐng)域。4.1.1激光雷達(dá)原理及數(shù)據(jù)處理激光測(cè)距無(wú)線(xiàn)電測(cè)距超聲波測(cè)距圖4-1常見(jiàn)雷達(dá)種類(lèi)4.1.1激光雷達(dá)原理及數(shù)據(jù)處理激光雷達(dá)測(cè)距方式主要是三角測(cè)距和TOF(TimeofFly，飛行時(shí)間)測(cè)距兩。1. 激光雷達(dá)測(cè)距原理(1)三角測(cè)距三角測(cè)距法的原理示意如圖4-2所示，發(fā)射一束激光，被物體A反射后，照射到圖像傳感器的

位置，這樣就形成了一個(gè)三角形，通過(guò)解算可以求出物體A到激光器的距離。圖4-2三角測(cè)距原理示意4.1.1激光雷達(dá)原理及數(shù)據(jù)處理(2)TOF測(cè)距最常見(jiàn)的測(cè)距傳感器基于飛行時(shí)間原理(TOF)。這類(lèi)傳感器通常包含發(fā)射器和接收器兩個(gè)環(huán)節(jié)，發(fā)射器能夠主動(dòng)向環(huán)境發(fā)射信號(hào)，信號(hào)在遇到環(huán)境中的障礙物后被反射，進(jìn)而被接收器接收。圖4-3TOF傳感器的基本原理

4.1.1激光雷達(dá)原理及數(shù)據(jù)處理圖4-4二維激光傳感器的構(gòu)成圖4-5二維激光雷達(dá)（1線(xiàn)）圖4-6二維激光雷達(dá)掃描結(jié)果2.典型的激光雷達(dá)

當(dāng)把單線(xiàn)的激光測(cè)距儀固定安裝到電動(dòng)機(jī)上時(shí)，電動(dòng)機(jī)的轉(zhuǎn)動(dòng)可以帶來(lái)激光測(cè)距儀對(duì)周?chē)h(huán)境多個(gè)角度的測(cè)量，單線(xiàn)激光雷達(dá)掃描點(diǎn)通常處在同一平面上的360°范圍內(nèi)，如圖4-4所示，形成二維激光掃描儀，又稱(chēng)2D激光雷達(dá)。4.1.1激光雷達(dá)原理及數(shù)據(jù)處理2.典型的激光雷達(dá)

二維激光雷達(dá)的單線(xiàn)掃描只能掃描同一平面上的障礙信息，也就是環(huán)境的某一個(gè)橫截面的輪廓，這樣掃描的數(shù)據(jù)信息很有限。在垂直方向同時(shí)發(fā)射多束激光，再結(jié)合旋轉(zhuǎn)機(jī)構(gòu)，就能掃描多個(gè)橫截面的輪廓，這就是多線(xiàn)激光雷達(dá)，也叫3D激光雷達(dá)。多線(xiàn)激光雷達(dá)的原理示意如圖4-7所示。16線(xiàn)的激光雷達(dá)如圖4-8所示。圖4-8三維激光雷達(dá)（16線(xiàn)）

(a)側(cè)視圖

(b)俯視圖

圖4-7多線(xiàn)激光雷達(dá)掃描點(diǎn)4.1.1激光雷達(dá)原理及數(shù)據(jù)處理3.激光雷達(dá)的性能參數(shù)

理解激光雷達(dá)的性能參數(shù)是挑選和使用激光雷達(dá)的前提。常用的激光雷達(dá)的主要性能參數(shù)包括以下幾個(gè)：激光線(xiàn)數(shù)、測(cè)距頻率、掃描頻率、測(cè)距量程、掃描角度、距離分辨率、角度分辨率、使用壽命。常見(jiàn)的激光雷達(dá)型號(hào)以供大家選擇，如表4-1所示。表4-1

常見(jiàn)激光雷達(dá)型號(hào)公司型號(hào)德國(guó)SICKLMS111、LMS151、TIM561日本

HOKUYOURG-04LX、UST-10LX、UTM-30LX美國(guó)

VelodyneVLP-16、VLP-32C、HDL-64E、VLS-128上海恩嵐科技RPLIDAR-A1、RPLIDAR-A2、RPLIDAR-A3深圳市鐳神智能LS01A、LS01D、LS01E、LS01B深圳玩智商科技YDLIDAR-G4、YDLIDAR-X4、YDLIDAR-X2大族激光3i-LIDAR-Delta2B、3i-LIDAR-Delta3速騰聚創(chuàng)RS-LiDAR-16、RS-LiDAR-32、RS-Ruby4.1.1激光雷達(dá)原理及數(shù)據(jù)處理4.激光雷達(dá)數(shù)據(jù)處理

激光雷達(dá)的數(shù)據(jù)處理主要包括兩個(gè)過(guò)程：濾波及校正。濾波處理是應(yīng)對(duì)激光雷達(dá)在測(cè)量的過(guò)程中，有時(shí)候激光雷達(dá)數(shù)據(jù)會(huì)受到一些干擾，需要經(jīng)過(guò)一些簡(jiǎn)單的濾波處理。目前基于Ubuntu和ROS系統(tǒng)是機(jī)器人主要使用的操作系統(tǒng)，對(duì)于激光雷達(dá)的濾波處理，ROS系統(tǒng)中功能包laser_filters內(nèi)有較豐富的濾波函數(shù)可以參考，如表4-2所示。表4-2

功能包laser_filters中包含的濾波函數(shù)函數(shù)名描述LaserArrayFilter將雷達(dá)數(shù)據(jù)存入數(shù)組，便于后續(xù)處理ScanShadowsFilter濾除因自身遮擋而產(chǎn)生的干擾數(shù)據(jù)InterpolationFilter在可信任的掃描點(diǎn)LaserScanIntensityFilter濾除在設(shè)定強(qiáng)度閾值之外的數(shù)據(jù)LaserScanRangeFilter濾除在設(shè)定距離范圍之外的數(shù)據(jù)LaserScanAngularBoundsFilter濾除在設(shè)定掃描角度范圍之外的數(shù)據(jù)LaserScanAngularBoundsFilterInPlace濾除在設(shè)定掃描角度范圍之內(nèi)的數(shù)據(jù)LaserScanBoxFilter濾除在設(shè)定區(qū)域范圍之內(nèi)的數(shù)據(jù)4.1.1激光雷達(dá)原理及數(shù)據(jù)處理

由于雷達(dá)是通過(guò)旋轉(zhuǎn)進(jìn)行掃描，所以?huà)呙钄?shù)據(jù)點(diǎn)會(huì)因機(jī)器人自身移動(dòng)而產(chǎn)生偏差。當(dāng)機(jī)器人靜止時(shí)。激光雷達(dá)旋轉(zhuǎn)一圈掃描到的點(diǎn)序列都是以機(jī)器人當(dāng)前靜止位置作為參考的，激光雷達(dá)的測(cè)距誤差僅來(lái)自測(cè)距方法本身。而當(dāng)機(jī)器人處于移動(dòng)狀態(tài)時(shí)，因?yàn)榧す饫走_(dá)并不知道自身處于移動(dòng)狀態(tài)，所以激光雷達(dá)旋轉(zhuǎn)一圈掃描到的點(diǎn)序列依然是以該幀時(shí)間戳?xí)r刻的機(jī)器人位置為參考，顯然激光雷達(dá)的測(cè)距誤差除了來(lái)自測(cè)距方法本身外，還來(lái)自機(jī)器人運(yùn)動(dòng)產(chǎn)生的畸變。因此，有必要對(duì)雷達(dá)運(yùn)動(dòng)的畸變做校正。不管是單線(xiàn)激光雷達(dá)還是多線(xiàn)激光雷達(dá)，都是繞z軸旋轉(zhuǎn)進(jìn)行掃描，校正方法是一樣的，所以就只討論單線(xiàn)激光雷達(dá)的情況了。常用的方法如下：4.1.1激光雷達(dá)原理及數(shù)據(jù)處理

已知機(jī)器人中兩幀激光雷達(dá)掃描數(shù)據(jù)

和

求機(jī)器人的位置轉(zhuǎn)移關(guān)系

，也就是激光里程計(jì)的問(wèn)題，最常用的求解方法是ICP算法。通過(guò)ICP算法求前后兩幀雷達(dá)數(shù)據(jù)

和

對(duì)應(yīng)機(jī)器人位置的轉(zhuǎn)移關(guān)系

，位置的轉(zhuǎn)移關(guān)系

近似表示其當(dāng)前的運(yùn)動(dòng)

，然后利用這個(gè)運(yùn)動(dòng)信息

對(duì)當(dāng)前的雷達(dá)數(shù)據(jù)

做補(bǔ)償。ICP算法的流程如圖4-11所示。圖4-11ICP迭代過(guò)程

（1）ICP(IterativeClosestPoint，最近鄰點(diǎn)迭代)算法及改進(jìn)4.1.1激光雷達(dá)原理及數(shù)據(jù)處理

（2）里程計(jì)輔助法

里程計(jì)輔助法采用外部的IMU或者輪式里程計(jì)來(lái)提供機(jī)器人的運(yùn)動(dòng)信息，就可以直接對(duì)雷達(dá)運(yùn)動(dòng)畸變做校正。IMU雖然可以提供極高頻率的姿態(tài)更新，但是其里程計(jì)存在很高的累積誤差，所以選用輪式里程計(jì)會(huì)更穩(wěn)定，如圖4-12所示。圖4-12里程計(jì)輔助法4.1.2視覺(jué)傳感器

視覺(jué)傳感器可以感知了解移動(dòng)機(jī)器人所到之處周?chē)沫h(huán)境，并能夠同時(shí)判斷機(jī)器人在環(huán)境中的位置，是視覺(jué)環(huán)境感知建模中的重要環(huán)節(jié)。相機(jī)是機(jī)器人進(jìn)行視覺(jué)感知的傳感器，相當(dāng)于機(jī)器人的眼睛。在機(jī)器人中，常見(jiàn)3種類(lèi)型的相機(jī)，即單目相機(jī)、雙目相機(jī)和RGB-D相機(jī)，如圖4-13所示。相機(jī)由于其能感知光強(qiáng)，并且角度測(cè)量精度較高，分辨率也較高，所以也是機(jī)器人應(yīng)用中常見(jiàn)的傳感器，也可以用于測(cè)量距離。單目

雙目

RGB-D

圖4-133種類(lèi)型相機(jī)4.1.2視覺(jué)傳感器1.單目相機(jī)

單目相機(jī)其實(shí)就是大家通常說(shuō)的攝像頭，由鏡頭和圖像傳感器(CMOS或CCD)構(gòu)成。相機(jī)包含鏡頭(透鏡)、光圈和感光器件三個(gè)部分，其中感光器件決定了圖像平面。根據(jù)光學(xué)原理，透鏡存在聚焦平面，透鏡和聚焦平面的距離為焦距，由透鏡決定。如圖4-14所示，物距

、焦距

和焦點(diǎn)距離透鏡的距離

之間的關(guān)系如下:

(4-2)圖4-14相機(jī)成像機(jī)理4.1.2視覺(jué)傳感器

進(jìn)一步分析單目相機(jī)成像的數(shù)學(xué)原理，假設(shè)實(shí)際環(huán)境中的物體點(diǎn)

在相機(jī)坐標(biāo)系下的坐標(biāo)值為

，物體點(diǎn)

透過(guò)光心在圖像傳感器上形成點(diǎn)

，二者的關(guān)系如圖4-15所示。圖4-15小孔成像原理

4.1.2視覺(jué)傳感器

式(4-4)就是相機(jī)的無(wú)畸變內(nèi)參模型，式中的矩陣K就是相機(jī)內(nèi)參數(shù)。在小孔成像模型中，物體點(diǎn)

是直接沿直線(xiàn)透過(guò)光心形成圖像點(diǎn)

。實(shí)際的相機(jī)前面是一個(gè)大大的鏡頭，鏡頭能讓更多的光線(xiàn)進(jìn)入以加快曝光速度，但是鏡頭會(huì)對(duì)光線(xiàn)產(chǎn)生折射，這樣成像會(huì)產(chǎn)生畸變，這種由鏡頭折射引起的圖像畸變叫徑向畸變。

(4-4)4.1.2視覺(jué)傳感器

除了上面的徑向畸變外，還有切向畸變。相機(jī)鏡頭和圖像傳感器平面由于安裝誤差導(dǎo)致不平行，因此引入了切向畸變，如圖4-16所示。

圖4-16切向畸變4.1.2視覺(jué)傳感器(4-5)

相機(jī)的成像模型就可以寫(xiě)成式(4-6)所示形式。

(4-6)

式(4-6)就是相機(jī)的外參模型，式中的矩陣

就是相機(jī)外參數(shù)。外參

就是相機(jī)在世界坐標(biāo)系下的位姿。4.1.2視覺(jué)傳感器2.雙目相機(jī)

單目相機(jī)無(wú)法測(cè)量物體點(diǎn)的深度信息。但是，用兩個(gè)單目相機(jī)組成的雙目相機(jī)就可以測(cè)量深度信息，有些地方也把雙目相機(jī)叫深度相機(jī)。為了方便理解，先說(shuō)明理想情況下雙目相機(jī)測(cè)量深度的原理，如圖4-17所示。圖4-17雙目相機(jī)測(cè)量深度原理4.1.2視覺(jué)傳感器

圖4-18非理想雙目相機(jī)模型4.1.2視覺(jué)傳感器3.RGB_D相機(jī)

雙目相機(jī)雖然能測(cè)量深度信息，但是需要事先找到同一物體點(diǎn)在左右相機(jī)中成像點(diǎn)對(duì)，也就是要先匹配。匹配過(guò)程很容易受到光照強(qiáng)度等環(huán)境因素干擾，在沒(méi)有特征的環(huán)境中，匹配會(huì)失效，深度信息將無(wú)法測(cè)量。RGB-D相機(jī)是主動(dòng)測(cè)量深度的傳感器，受環(huán)境的干擾會(huì)小一些。RGB-D相機(jī)一般有3個(gè)鏡頭：中間的鏡頭是普通的攝像頭，采集彩色圖像；另外兩個(gè)鏡頭分別用來(lái)發(fā)射紅外光和接收紅外光。圖4-19結(jié)構(gòu)光三維視覺(jué)透視攝影模型4.1.3視覺(jué)傳感器標(biāo)定方法

前面我們介紹了單目、雙目及RGBD視覺(jué)傳感器的成像原理，建立圖像像素點(diǎn)與世界坐標(biāo)下任意點(diǎn)之間的數(shù)學(xué)關(guān)系，定義了相機(jī)的內(nèi)部參數(shù)和外部參數(shù)。本節(jié)分別介紹直接線(xiàn)性標(biāo)定法和張正友標(biāo)定法。

1. 直線(xiàn)線(xiàn)性標(biāo)定法DLT標(biāo)定法需要將一個(gè)特制的立方體標(biāo)定模板放置在所需標(biāo)定相機(jī)前，其中標(biāo)定模板上的標(biāo)定點(diǎn)相對(duì)于世界坐標(biāo)系的位置已知。這樣相機(jī)的參數(shù)可以利用相線(xiàn)性模型得到。根據(jù)無(wú)畸變內(nèi)參模型公式(4-4)寫(xiě)出具體的內(nèi)參方法，如式(4-9)所示。

(4-9)(4-10)4.1.3視覺(jué)傳感器標(biāo)定方法

(4-11)

(4-12)DLT方法的優(yōu)點(diǎn)是計(jì)算速度很快，操作簡(jiǎn)單且易實(shí)現(xiàn)。缺點(diǎn)是由于沒(méi)有考慮攝像機(jī)鏡頭的畸變，因此不適合畸變系數(shù)很大的鏡頭，否則會(huì)帶來(lái)很大誤差。4.1.3視覺(jué)傳感器標(biāo)定方法2. 張正友標(biāo)定法

張正友標(biāo)定法，也稱(chēng)Zhang標(biāo)定法，是由微軟研究院的張正友博士于1998年提出的一種介于傳統(tǒng)標(biāo)定方法和自標(biāo)定方法之間的平面標(biāo)定方法。它既避免了傳統(tǒng)標(biāo)定方法設(shè)備要求高、操作繁瑣等缺點(diǎn)，又比自標(biāo)定的精度高、魯棒性好。該方法主要步驟如下:1) 打印一張黑白棋盤(pán)方格圖案，并將其貼在一塊剛性平面上作為標(biāo)定板;2) 移動(dòng)標(biāo)定板或者相機(jī)，從不同角度拍攝若干照片(理論上照片越多，誤差越小);3) 對(duì)每張照片中的角點(diǎn)進(jìn)行檢測(cè)，確定角點(diǎn)的圖像坐標(biāo)與實(shí)際坐標(biāo);4) 在不考慮徑向畸變的前提下，即采用相機(jī)的線(xiàn)性模型。根據(jù)旋轉(zhuǎn)矩陣的正交性，通過(guò)求解線(xiàn)性方程，獲得攝像機(jī)的內(nèi)部參數(shù)和第一幅圖的外部參數(shù);5) 利用最小二乘法估算相機(jī)的徑向畸變系數(shù);6) 根據(jù)再投影誤差最小準(zhǔn)則，對(duì)內(nèi)外參數(shù)進(jìn)行優(yōu)化。以下介紹上述步驟的具體實(shí)現(xiàn)過(guò)程。4.1.4圖像特征的提取匹配方法-SIFT算法David.Lowe總結(jié)了現(xiàn)有的基于不變量技術(shù)的特征檢測(cè)方法，正式提出了一種基于尺度空間的，對(duì)圖像平移、旋轉(zhuǎn)、縮放、甚至仿射變換保持不變性的圖像局部特征，以及基于該特征的描述符。并將這種方法命名為尺度不變特征變換（ScaleInvariantFeatureTransform），簡(jiǎn)稱(chēng)SIFT算法。1. 高斯尺度空間的極值檢測(cè)

特征點(diǎn)檢測(cè)的第一步是能夠識(shí)別出目標(biāo)的位置和尺度，對(duì)同一個(gè)目標(biāo)在不同的視角下這些位置和尺度可以被重復(fù)地分配。并且這些檢測(cè)到的位置是不隨圖像尺度的變化而改變的，因?yàn)樗鼈兪峭ㄟ^(guò)搜索所有尺度上的穩(wěn)定特征得到的，所應(yīng)用的工具就是被稱(chēng)為尺度空間的連續(xù)尺度函數(shù)。圖像的尺度空間用L(x,y,σ)函數(shù)表示，它是由一個(gè)變尺度的二維高斯函數(shù)G(x,y,σ)與圖像I(x,y)通過(guò)卷積產(chǎn)生的。

4.1.4圖像特征的提取匹配方法-SIFT算法

利用高斯拉普拉斯方法(LaplacianofGaussian，LoG)，即圖像的二階導(dǎo)數(shù)，能夠在不同的尺度下檢測(cè)到圖像的斑點(diǎn)特征，從而可以確定圖像的特征點(diǎn)。但LoG的效率不高，因此SIFT算法進(jìn)行了改進(jìn)，通過(guò)對(duì)兩個(gè)相鄰高斯尺度空間的圖像相減，得到一個(gè)DifferenceofGaussians(高斯差分，DoG)的響應(yīng)值圖像

來(lái)近似LoG:4.1.4圖像特征的提取匹配方法-SIFT算法

為了在連續(xù)的尺度下檢測(cè)圖像的特征點(diǎn)，需要建立DoG金字塔，而DoG金字塔的建立又離不開(kāi)高斯金字塔的建立。如圖4-20所示，左側(cè)為高斯金字塔，右側(cè)為DoG金字塔。圖4-20高斯金字塔和DoG金字塔4.1.4圖像特征的提取匹配方法-SIFT算法

極值點(diǎn)的搜索是在DoG金字塔內(nèi)進(jìn)行的，這些極值點(diǎn)就是候選的特征點(diǎn)。在搜索之前，需要在DoG金字塔內(nèi)剔除那些像素灰度值過(guò)小的點(diǎn)，因?yàn)檫@些像素具有較低的對(duì)比度，它們肯定不是穩(wěn)定的特征點(diǎn)。極值點(diǎn)的搜索不僅需要在它所在尺度空間圖像的鄰域內(nèi)進(jìn)行,還需要在它的相鄰尺度空間的圖像內(nèi)進(jìn)行，如圖4-21所示。圖4-21DoG中極值點(diǎn)的搜索4.1.4圖像特征的提取匹配方法-SIFT算法2.特征點(diǎn)位置的確定

上一步得到的極值點(diǎn)還僅僅是候選的特征點(diǎn)，因?yàn)樗鼈冞€存在一些不確定的因素。首先是極值點(diǎn)的搜索是是在離散空間內(nèi)進(jìn)行的，并且這些離散空間還是經(jīng)過(guò)不斷的降采樣得到的。如果把采樣點(diǎn)擬合成曲面后我們會(huì)發(fā)現(xiàn)，原先的極值點(diǎn)并不是真正的極值點(diǎn)，也就是離散空間的極值點(diǎn)并不一定是連續(xù)空間的極值點(diǎn)。在這里，我們是需要精確定位特征點(diǎn)的位置和尺度的，也就是要達(dá)到亞像素級(jí)精度，因此必須進(jìn)行擬合處理。4.1.4圖像特征的提取匹配方法-SIFT算法

使用泰勒級(jí)數(shù)展開(kāi)式作為擬合函數(shù)。如上所述，極值點(diǎn)是一個(gè)三維矢量，即它包括極值點(diǎn)所在的尺度，以及它的尺度圖像坐標(biāo)，即

，因此需要三維函數(shù)的泰勒級(jí)數(shù)展開(kāi)式，設(shè)在

處進(jìn)行泰勒級(jí)數(shù)展開(kāi)，則它的矩陣形式為:它的矢量表示形式為：4.1.4圖像特征的提取匹配方法-SIFT算法經(jīng)過(guò)變量變換后，又可寫(xiě)成對(duì)上式求導(dǎo)，得:讓它的導(dǎo)數(shù)為0，就可得到極值點(diǎn)下的相對(duì)于插值中心

的偏移量:4.1.4圖像特征的提取匹配方法-SIFT算法該極值點(diǎn)下的極值：4.1.4圖像特征的提取匹配方法-SIFT算法3.方向角度的確定

經(jīng)過(guò)上面兩個(gè)步驟，一幅圖像的特征點(diǎn)就可以完全找到，而且這些特征點(diǎn)是具有尺度不變性的。但為了實(shí)現(xiàn)旋轉(zhuǎn)不變性，還需要為特征點(diǎn)分配一個(gè)方向角度，也就是需要根據(jù)檢測(cè)到的特征點(diǎn)所在的高斯尺度圖像的局部結(jié)構(gòu)求得一個(gè)方向基準(zhǔn)。該高斯尺度圖像的尺度σ是已知的，并且該尺度是相對(duì)于高斯金字塔所在組的基準(zhǔn)層的尺度。而所謂局部結(jié)構(gòu)指的是在高斯尺度圖像中以特征點(diǎn)為中心，以r為半徑的區(qū)域內(nèi)計(jì)算所有像素梯度的幅角和幅值，半徑r為

式中，σ就是上面提到的相對(duì)于所在組的基準(zhǔn)層的高斯尺度圖像的尺度。4.1.4圖像特征的提取匹配方法-SIFT算法

像素梯度的幅值和幅角的計(jì)算公式為：

因?yàn)樵谝詒為