專(zhuān)利名稱(chēng)：水下航行器組合導(dǎo)航與控制半實(shí)物仿真試驗(yàn)系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種半實(shí)物仿真系統(tǒng)，具有捷聯(lián)慣性導(dǎo)航+GPS/北斗(GPS/BD)+多普 勒測(cè)速儀+深度傳感器的組合導(dǎo)航與控制系統(tǒng)仿真功能，適用于多種水下航行器的組合導(dǎo) 航控制系統(tǒng)的理論方法研究、工程應(yīng)用技術(shù)開(kāi)發(fā)和系統(tǒng)性能檢測(cè)與驗(yàn)證。
背景技術(shù)：
半實(shí)物仿真試驗(yàn)又稱(chēng)為硬件在回路中仿真，它是指在系統(tǒng)研究中，把數(shù)學(xué)模型、實(shí) 體模型(物理效應(yīng)模型)和系統(tǒng)的實(shí)際設(shè)備(實(shí)物)聯(lián)系在一起運(yùn)轉(zhuǎn)。利用半實(shí)物仿真系 統(tǒng)，可以檢驗(yàn)系統(tǒng)中某些部件的性能，可以進(jìn)一步校正系統(tǒng)的數(shù)學(xué)模型，從而在實(shí)驗(yàn)室內(nèi)全 面地檢驗(yàn)和評(píng)定系統(tǒng)設(shè)計(jì)的合理性和各部件工作的協(xié)調(diào)性，在武器系統(tǒng)研制中發(fā)揮了重要 作用，具有經(jīng)濟(jì)性和安全性的特點(diǎn)。水下航行器導(dǎo)航技術(shù)與飛機(jī)、導(dǎo)彈等飛行器導(dǎo)航技術(shù)相比，具有工作時(shí)間長(zhǎng)、環(huán)境 復(fù)雜、信息源少、隱蔽性要求高等特點(diǎn)，因此水下導(dǎo)航具有更大的難度。在水下航行器導(dǎo)航 與控制系統(tǒng)半實(shí)物仿真技術(shù)研究方面，盧淑娟等人在《水下航行器組合導(dǎo)航仿真系統(tǒng)設(shè)計(jì) 與實(shí)現(xiàn)》中提出的自主水下航行器(AUV) SINS/DVL組合導(dǎo)航的實(shí)時(shí)仿真系統(tǒng)由載體計(jì)算機(jī) 和導(dǎo)航計(jì)算機(jī)組成，其中AUV模型解算主要由載體計(jì)算機(jī)完成，導(dǎo)航計(jì)算機(jī)負(fù)責(zé)解算AUV導(dǎo) 航系統(tǒng)的導(dǎo)航參數(shù)。嚴(yán)衛(wèi)生在《水下航行器導(dǎo)航與控制實(shí)時(shí)仿真系統(tǒng)》一文中建立的仿真 系統(tǒng)由兩部分組成導(dǎo)航與控制實(shí)時(shí)仿真系統(tǒng)和自動(dòng)駕駛儀的導(dǎo)航控制計(jì)算機(jī)，其中實(shí)時(shí) 仿真系統(tǒng)由兩臺(tái)聯(lián)網(wǎng)的計(jì)算機(jī)和兩個(gè)接口轉(zhuǎn)換器組成，以模擬多普勒測(cè)速儀、羅經(jīng)和深度 傳感器等外圍設(shè)備信息，并將模擬的信息參數(shù)提供給導(dǎo)航控制計(jì)算機(jī)。以上仿真系統(tǒng)都是 基于計(jì)算機(jī)的數(shù)字仿真，用計(jì)算機(jī)模擬傳感器設(shè)備為控制系統(tǒng)提供虛擬信號(hào)，沒(méi)有接入真 實(shí)的導(dǎo)航與控制組件。王彥愷在《基于SINS/GPS/DVL組合導(dǎo)航的AUV半實(shí)物仿真系統(tǒng)設(shè)計(jì)》中設(shè)計(jì)了一 種AUV半實(shí)物仿真系統(tǒng)。從文中所列的AUV半實(shí)物仿真系統(tǒng)結(jié)構(gòu)圖和AUV導(dǎo)航與控制流程 圖可以看出，該系統(tǒng)的多普勒信號(hào)仿真器和GPS仿真器仍采用計(jì)算機(jī)數(shù)字信號(hào)模擬，而且 在系統(tǒng)中沒(méi)有接入水壓仿真器的壓力信號(hào)，未構(gòu)成完整的閉環(huán)仿真系統(tǒng)。謝攀在《一種新型 UUV半實(shí)物仿真系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)》中給出了 GPS模擬子系統(tǒng)、多普勒測(cè)速模擬裝置和水壓仿 真器子系統(tǒng)的測(cè)試結(jié)果，但沒(méi)有給出閉環(huán)仿真系統(tǒng)的實(shí)驗(yàn)結(jié)果，沒(méi)有解決捷聯(lián)慣導(dǎo)系統(tǒng)的 線加速度模擬問(wèn)題以及模型解算和導(dǎo)航解算的同步問(wèn)題。專(zhuān)利200610011580. X提出了一種SINS/CNS/GPS組合導(dǎo)航半實(shí)物仿真系統(tǒng)，專(zhuān)利 200610089437. 2中提出了一種捷聯(lián)慣導(dǎo)/天文組合導(dǎo)航半實(shí)物仿真系統(tǒng)。這兩種半實(shí)物仿 真系統(tǒng)利用軌跡發(fā)生器生成標(biāo)稱(chēng)軌跡數(shù)據(jù)，然后疊加上SINS、CNS、GPS等器件的誤差數(shù)據(jù) 來(lái)研究組合導(dǎo)航系統(tǒng)的動(dòng)態(tài)性能。但是這兩種半實(shí)物仿真系統(tǒng)未考慮航行器的動(dòng)力學(xué)和運(yùn) 動(dòng)學(xué)模型，而且這兩種半實(shí)物仿真系統(tǒng)沒(méi)有構(gòu)成導(dǎo)航控制系統(tǒng)的全閉環(huán)仿真試驗(yàn)，也沒(méi)有 深度模擬裝置，不適合水下航行器的組合導(dǎo)航。

發(fā)明內(nèi)容
現(xiàn)有的仿真系統(tǒng)沒(méi)有解決慣導(dǎo)系統(tǒng)線加速度信號(hào)仿真問(wèn)題，無(wú)法構(gòu)成導(dǎo)航控制系 統(tǒng)的全閉環(huán)仿真試驗(yàn)，本發(fā)明提供一種水下航行器組合導(dǎo)航與控制半實(shí)物仿真試驗(yàn)系統(tǒng)， 實(shí)現(xiàn)了對(duì)IMU慣導(dǎo)組件、多普勒測(cè)速儀組件、深度傳感器組件的性能測(cè)試，可進(jìn)行導(dǎo)航控制 算法的研究，全部實(shí)物與仿真設(shè)備連接構(gòu)成半實(shí)物閉環(huán)仿真系統(tǒng)，實(shí)現(xiàn)了水下航行器組合 導(dǎo)航控制系統(tǒng)的聯(lián)調(diào)與測(cè)試。本發(fā)明解決其技術(shù)問(wèn)題所采用的技術(shù)方案是仿真試驗(yàn)系統(tǒng)包括ADI實(shí)時(shí)仿真計(jì) 算機(jī)、捷聯(lián)慣性導(dǎo)航子系統(tǒng)(SINS子系統(tǒng))、衛(wèi)星導(dǎo)航子系統(tǒng)、導(dǎo)航控制計(jì)算機(jī)、舵機(jī)、多普 勒(DVL)導(dǎo)航子系統(tǒng)、虛擬線加速度計(jì)和深度模擬子系統(tǒng)。ADI實(shí)時(shí)仿真計(jì)算機(jī)解算水下航行器的動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)數(shù)學(xué)模型，計(jì)算出水下航 行器的位置、姿態(tài)、速度和航向信息。SINS子系統(tǒng)包括反射內(nèi)存通訊板、角運(yùn)動(dòng)模擬器和IMU慣性測(cè)量裝置。IMU慣性 測(cè)量裝置固定在角運(yùn)動(dòng)模擬器上，實(shí)時(shí)敏感水下航行器在載體坐標(biāo)系下的三個(gè)角加速度和 角速度，并送到導(dǎo)航控制計(jì)算機(jī)。角運(yùn)動(dòng)模擬器通用反射內(nèi)存接收來(lái)自ADI實(shí)時(shí)仿真計(jì)算 機(jī)的水下航行器姿態(tài)信息，模擬水下航行器的姿態(tài)運(yùn)動(dòng)，為慣性組件提供虛擬試驗(yàn)環(huán)境。衛(wèi)星導(dǎo)航子系統(tǒng)包括衛(wèi)星導(dǎo)航模擬器和衛(wèi)星導(dǎo)航接收機(jī)。衛(wèi)星導(dǎo)航模擬器可實(shí) 現(xiàn)對(duì)真實(shí)衛(wèi)星導(dǎo)航系統(tǒng)的模擬——根據(jù)ADI實(shí)時(shí)仿真計(jì)算機(jī)發(fā)出的水下航行器的運(yùn)動(dòng)信息 實(shí)時(shí)生成并發(fā)射GPS/BD信號(hào)載波，衛(wèi)星導(dǎo)航接收機(jī)通過(guò)射頻天線實(shí)時(shí)地接收衛(wèi)星導(dǎo)航模 擬器發(fā)射的衛(wèi)星導(dǎo)航信號(hào)，然后把水下航行器所處的位置和在導(dǎo)航坐標(biāo)下的速度信息通過(guò) RS232串口送給導(dǎo)航控制計(jì)算機(jī)。深度模擬子系統(tǒng)包括深度傳感器、I/O通訊模塊、反射內(nèi)存通訊板和深度模擬器。 深度傳感器安裝在深度模擬器上，深度模擬器通過(guò)反射內(nèi)存接收來(lái)自ADI實(shí)時(shí)仿真計(jì)算機(jī) 發(fā)送的水下航行器航行深度信號(hào)，將其轉(zhuǎn)換為相應(yīng)的壓力信號(hào)，深度傳感器將敏感到深度 信息通過(guò)I/O通訊模塊輸出至導(dǎo)航控制計(jì)算機(jī)。DVL導(dǎo)航子系統(tǒng)包括DVL測(cè)速仿真裝置、聲對(duì)接裝置和多普勒測(cè)速儀。ADI實(shí)時(shí)仿 真計(jì)算機(jī)通過(guò)反射內(nèi)存將水下航行器的速度信息傳遞給DVL測(cè)速模擬裝置，DVL測(cè)速模擬 裝置為多普勒測(cè)速儀提供水聲傳播環(huán)境，并通過(guò)聲對(duì)接裝置與多普勒測(cè)速儀連接，多普勒 測(cè)速儀根據(jù)發(fā)射的聲波與經(jīng)過(guò)反射回的聲波之間的頻率差求解水下航行器的速度，并傳送 給導(dǎo)航控制計(jì)算機(jī)。虛擬線加速度計(jì)包括哥氏加速度計(jì)算裝置、重力加速度計(jì)算裝置和數(shù)據(jù)融合裝 置。哥氏加速度計(jì)算裝置和重力加速度計(jì)算裝置分別接收來(lái)自ADI實(shí)時(shí)仿真計(jì)算機(jī)的信 號(hào)，并計(jì)算得到哥氏加速度和當(dāng)?shù)氐闹亓铀俣龋ㄟ^(guò)數(shù)據(jù)融合裝置得到水下航行器實(shí)時(shí) 的比力信息，而后送至導(dǎo)航控制計(jì)算機(jī)。導(dǎo)航控制計(jì)算機(jī)實(shí)時(shí)并行采集IMU慣導(dǎo)組件、衛(wèi)星導(dǎo)航接收機(jī)、深度傳感器、DVL 測(cè)速儀輸出的姿態(tài)、位置、深度、速度信息及虛擬線加速度計(jì)輸出的比力信息，完成慣性導(dǎo) 航和控制系統(tǒng)解算，輸出水下航行器的操舵指令，驅(qū)動(dòng)舵機(jī)運(yùn)動(dòng)。舵機(jī)接收導(dǎo)航控制計(jì)算機(jī)輸出的操作指令，模擬水下航行器垂直舵和水平舵的動(dòng) 作。舵機(jī)上的舵角反饋電位計(jì)將舵角信息反饋給ADI實(shí)時(shí)仿真計(jì)算機(jī)，構(gòu)成整個(gè)系統(tǒng)的閉 環(huán)反饋。
上述設(shè)備模擬了水下航行器在航行過(guò)程中的姿態(tài)、位置、速度、航行深度等信息， 在實(shí)驗(yàn)室內(nèi)構(gòu)建了水下航行器的半實(shí)物仿真試驗(yàn)環(huán)境。在本發(fā)明中，通過(guò)引入開(kāi)機(jī)信號(hào)作為仿真開(kāi)始信號(hào)實(shí)現(xiàn)了導(dǎo)航控制計(jì)算機(jī)的慣導(dǎo) 解算與ADI實(shí)時(shí)仿真計(jì)算機(jī)的模型解算及虛擬加速度計(jì)的同步。開(kāi)機(jī)信號(hào)由穩(wěn)壓電源提 供，分別通過(guò)I/O通訊模塊與ADI實(shí)時(shí)仿真計(jì)算機(jī)、導(dǎo)航控制計(jì)算機(jī)和虛擬線加速度計(jì)連 接，確保導(dǎo)航控制計(jì)算機(jī)的慣導(dǎo)解算與ADI實(shí)時(shí)仿真計(jì)算機(jī)的模型解算及虛擬線加速度計(jì) 的同步，保證了模型解算和導(dǎo)航控制計(jì)算機(jī)的積分時(shí)間相同，使積分誤差達(dá)到最小。上述的時(shí)間同步連接在使用時(shí)包括以下步驟(I)ADI仿真主機(jī)、角運(yùn)動(dòng)模擬器、深度模擬器、多普勒測(cè)速模擬裝置和導(dǎo)航控制計(jì) 算機(jī)設(shè)置仿真初值；(2)角運(yùn)動(dòng)模擬器、深度模擬器分別運(yùn)動(dòng)到設(shè)定的姿態(tài)角位置和深度，導(dǎo)航控制計(jì) 算機(jī)接收IMU組件、GPS接收機(jī)和深度傳感器信息，多普勒測(cè)速儀仿真裝置接收水下航行器 的速度信號(hào)，各設(shè)備均穩(wěn)定在初值狀態(tài)，等待開(kāi)機(jī)信號(hào)；(3)打開(kāi)開(kāi)機(jī)信號(hào)，各設(shè)備構(gòu)成閉環(huán)進(jìn)行仿真運(yùn)行開(kāi)機(jī)信號(hào)由穩(wěn)壓電源提供，分 別通過(guò)I/O通訊模塊與ADI實(shí)時(shí)仿真計(jì)算機(jī)、導(dǎo)航控制計(jì)算機(jī)和虛擬線加速度計(jì)連接，實(shí)現(xiàn) 了導(dǎo)航控制計(jì)算機(jī)的慣導(dǎo)解算與ADI實(shí)時(shí)仿真計(jì)算機(jī)的模型解算及虛擬線加速度計(jì)的同 步，保證了模型解算和導(dǎo)航控制計(jì)算機(jī)的積分時(shí)間相同，使積分誤差達(dá)到最小。(4)仿真時(shí)間到，角運(yùn)動(dòng)模擬器、深度模擬器退出仿真模式，然后導(dǎo)航控制計(jì)算機(jī)、 GPS模擬器、多普勒測(cè)速儀模擬裝置和ADI仿真機(jī)退出，本次仿真結(jié)束。本發(fā)明的有益效果是本發(fā)明將更多的水下航行器實(shí)物設(shè)備(衛(wèi)星導(dǎo)航接收機(jī)、 多普勒測(cè)速儀、深度傳感器等)直接接入仿真回路，可對(duì)導(dǎo)航控制系統(tǒng)各傳感器組件功能 進(jìn)行檢驗(yàn)，還可通過(guò)模型和實(shí)物之間的切換，進(jìn)一步校準(zhǔn)數(shù)學(xué)模型。通過(guò)引入開(kāi)機(jī)信號(hào)，實(shí) 現(xiàn)了慣導(dǎo)解算與水下航行器模型解算的同步性，減小了半實(shí)物仿真系統(tǒng)的誤差，提高了系 統(tǒng)的仿真精度。虛擬線加速度計(jì)滿足半實(shí)物仿真試驗(yàn)的需求，使試驗(yàn)?zāi)茉趯?shí)驗(yàn)室條件下順 利進(jìn)行，相比車(chē)載慣性測(cè)量器件更具實(shí)用性，且不受天氣、路況等外部因素的影響。全閉環(huán) 半實(shí)物仿真試驗(yàn)系統(tǒng)能夠設(shè)計(jì)、分析和優(yōu)化水下航行器的彈道軌跡；可以檢驗(yàn)水下航行器 組合導(dǎo)航與控制系統(tǒng)軟硬件功能，完成系統(tǒng)測(cè)試，進(jìn)行導(dǎo)航控制系統(tǒng)的方案論證和誤差分 析，對(duì)水下航行器進(jìn)行性能評(píng)估；為水下航行器的研制提供了有力的技術(shù)支持，有助于縮短 研制時(shí)間，節(jié)約研究經(jīng)費(fèi)。下面結(jié)合附圖和實(shí)施例對(duì)本發(fā)明進(jìn)一步說(shuō)明。


圖1為本發(fā)明的結(jié)構(gòu)組成示意圖；圖2為本發(fā)明的SINS子系統(tǒng)的組成結(jié)構(gòu)圖；圖3為本發(fā)明的深度模擬子系統(tǒng)的組成結(jié)構(gòu)圖；圖4為本發(fā)明的DVL導(dǎo)航子系統(tǒng)的組成結(jié)構(gòu)圖；圖5為虛擬線加速度計(jì)的結(jié)構(gòu)圖；圖6為本發(fā)明的信號(hào)連接圖；圖7為水下航行器在SINS+DVL組合導(dǎo)航情況下，慣導(dǎo)解算的緯度與真實(shí)值對(duì)比曲線；圖8為水下航行器在SINS+DVL組合導(dǎo)航情況下，慣導(dǎo)解算的經(jīng)度與真實(shí)值對(duì)比曲 線；圖9為水下航行器在SINS+DVL組合導(dǎo)航情況下，航向角隨時(shí)間的變化曲線；圖10為水下航行器在SINS+DVL組合導(dǎo)航情況下，北向速度隨時(shí)間的變化曲線；圖11為水下航行器在SINS+DVL組合導(dǎo)航情況下，東向速度隨時(shí)間的變化曲線；圖12為水下航行器在SINS+DVL組合導(dǎo)航情況下，深度隨時(shí)間的變化曲線；圖13為水下航行器在SINS+GPS衛(wèi)星導(dǎo)航情況下，緯度誤差隨時(shí)間的變化曲線；圖14為水下航行器在SINS+GPS衛(wèi)星導(dǎo)航情況下，經(jīng)度誤差隨時(shí)間的變化曲線；
具體實(shí)施例方式利用本發(fā)明完成了水下航行器導(dǎo)航控制系統(tǒng)半實(shí)物仿真實(shí)驗(yàn)。(1)系統(tǒng)組成水下航行器組合導(dǎo)航與控制半實(shí)物仿真試驗(yàn)系統(tǒng)由IMU慣性測(cè)量裝置、多普勒測(cè) 速儀、深度傳感器、衛(wèi)星導(dǎo)航接收機(jī)和舵機(jī)組件等組件實(shí)物和ADI實(shí)時(shí)仿真計(jì)算機(jī)、多普勒 測(cè)速模擬裝置、深度模擬器、角運(yùn)動(dòng)模擬器、衛(wèi)星導(dǎo)航模擬器、虛擬線加速度計(jì)等仿真設(shè)備 以及導(dǎo)航控制計(jì)算機(jī)組成。水下航行器在水下航行時(shí)，采用SINS+DVL組合導(dǎo)航方式；在水 面航行時(shí)，采用SINS+GPS/北斗(GPS/BD)組合導(dǎo)航方式。系統(tǒng)的工作原理是首先，由ADI實(shí)時(shí)仿真計(jì)算機(jī)給角運(yùn)動(dòng)模擬器、深度模擬器、衛(wèi)星導(dǎo)航模擬器和多 普勒測(cè)速模擬裝置設(shè)置初值，待所有仿真設(shè)備達(dá)到初值后，啟動(dòng)開(kāi)機(jī)信號(hào)，仿真系統(tǒng)開(kāi)始運(yùn) 行。ADI實(shí)時(shí)仿真計(jì)算機(jī)實(shí)時(shí)解算水下航行器的動(dòng)力學(xué)、運(yùn)動(dòng)學(xué)方程，并將解算出的三個(gè)姿 態(tài)角、深度及速度信息分別送至角運(yùn)動(dòng)模擬器、深度模擬器和多普勒測(cè)速模擬裝置，同時(shí)將 水下航行器的位置(包括經(jīng)度、緯度、深度)和速度信息通過(guò)網(wǎng)絡(luò)送至衛(wèi)星導(dǎo)航模擬器。IMU 慣性測(cè)量裝置、深度傳感器、衛(wèi)星導(dǎo)航接收機(jī)和多普勒測(cè)速儀將采集到的水下航行器的姿 態(tài)角速率、深度信息、位置信息及速度信息分別輸出至導(dǎo)航控制計(jì)算機(jī)。導(dǎo)航控制計(jì)算機(jī)通 過(guò)導(dǎo)航解算得出操舵信號(hào)，控制舵機(jī)運(yùn)動(dòng)，舵反饋信號(hào)送入ADI實(shí)時(shí)仿真計(jì)算機(jī)，從而構(gòu)成 一個(gè)閉環(huán)的半實(shí)物仿真系統(tǒng)。系統(tǒng)的工作流程是①對(duì)系統(tǒng)進(jìn)行初始化，包括對(duì)各仿真設(shè)備和參試實(shí)物的初值設(shè)置、軟件的初始 化；②在檢測(cè)到開(kāi)機(jī)信號(hào)發(fā)出后，虛擬線加速度計(jì)輸出水下航行器的線加速度，同時(shí)， 模型解算模塊解算出水下航行器的運(yùn)動(dòng)信息；③通過(guò)水下航行器是否上浮判斷是SINS+DVL組合導(dǎo)航還是SINS+衛(wèi)星組合導(dǎo)航， 如是前者，則導(dǎo)航控制計(jì)算機(jī)通過(guò)RS422串口接收DVL測(cè)速儀送出的水下航行器的速度信 息，同時(shí)通過(guò)RS232串口接收ADI實(shí)時(shí)仿真計(jì)算機(jī)解算出的水下航行器的運(yùn)動(dòng)信息及虛擬 線加速度計(jì)送出的水下航行器的線加速度，解算后送出操舵指令；否則確定是否進(jìn)行GPS/ BD位置修正，若是，則接收衛(wèi)星信號(hào)接收機(jī)的數(shù)據(jù)，進(jìn)行GPS/BD星座定位解算，導(dǎo)航控制計(jì) 算機(jī)通過(guò)SINS/衛(wèi)星組合濾波校正慣導(dǎo)系統(tǒng)的導(dǎo)航誤差，控制水下航行器回歸設(shè)定航跡；
④舵機(jī)在操舵指令的控制下，驅(qū)動(dòng)舵機(jī)動(dòng)作，舵角反饋電位計(jì)將實(shí)時(shí)舵角信息送 到模型解算模塊，從而完成了整個(gè)半實(shí)物仿真試驗(yàn)系統(tǒng)的閉環(huán)。⑤當(dāng)水下航行器到達(dá)指定區(qū)域，或滿足其它結(jié)束條件(如航行時(shí)間到)時(shí)，系統(tǒng)結(jié) 束運(yùn)行。(2)虛擬線加速度計(jì)慣性導(dǎo)航系統(tǒng)是以陀螺和加速度計(jì)為敏感元件的導(dǎo)航參數(shù)解算系統(tǒng)。在水下航行 器導(dǎo)航控制半實(shí)物仿真試驗(yàn)系統(tǒng)中，由于IMU慣性測(cè)量裝置固定在三軸轉(zhuǎn)臺(tái)上，所以加速 度計(jì)無(wú)法提供比力信息。本發(fā)明設(shè)計(jì)的虛擬線加速度計(jì)，利用水下航行器的經(jīng)緯度、深度、 位置信息，采用公式(1)計(jì)算出水下航行器的比力信息，通過(guò)RS232串口送入導(dǎo)航控制計(jì)算 機(jī)，從而使慣性導(dǎo)航實(shí)驗(yàn)?zāi)軌蛟趯?shí)驗(yàn)室條件下進(jìn)行。JZ =Κ +(2ω +ω: )χν ~g" +V,( 1)式中，——比力在導(dǎo)航坐標(biāo)系的投影；<——'[貫性坐標(biāo)系相對(duì)于地球坐標(biāo)系的角速率在導(dǎo)航坐標(biāo)系的投影；<——地球坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)系的角速率在導(dǎo)航坐標(biāo)系的投影；K:—地球坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)系的速率度在導(dǎo)航坐標(biāo)系的投影；gn~重力加速度在導(dǎo)航坐標(biāo)系的投影；——噪聲干擾；工作原理為，通過(guò)ADI實(shí)時(shí)仿真計(jì)算機(jī)的模型結(jié)算模塊得到水下航行器的加速度 信息，經(jīng)積分后得到其速度信息；由水下航行器所處地理位置的緯度l、實(shí)時(shí)的航行深 度h可以計(jì)算得到當(dāng)?shù)氐闹亓铀俣萭n;由水下航行器所處地理位置的經(jīng)度λ和緯度L信 息，可以計(jì)算出地球坐標(biāo)系到導(dǎo)航坐標(biāo)系的位置轉(zhuǎn)換矩陣，進(jìn)而求出由于水下航行器相對(duì) 于地球運(yùn)動(dòng)(相對(duì)運(yùn)動(dòng))和地球旋轉(zhuǎn)(牽連運(yùn)動(dòng))引起的哥氏加速度2 ； ；通過(guò)計(jì)算地 球速率和矩陣變換得到由于水下航行器保持在地球表面運(yùn)動(dòng)(圓周運(yùn)動(dòng))引起的對(duì)地向心 加速度X^C。對(duì)上述信息進(jìn)行綜合、加入噪聲干擾后得到虛擬線加速度計(jì)的輸出/J。(3)陀螺儀積分時(shí)間同步技術(shù)根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)的基本原理可知，陀螺儀和虛擬線加速度計(jì)送出的加速度信息 需要經(jīng)過(guò)積分運(yùn)算才能得到水下航行器的速度信息，在半實(shí)物仿真系統(tǒng)中各仿真設(shè)備都有 自己的時(shí)鐘系統(tǒng)，如果導(dǎo)航控制計(jì)算機(jī)與ADI仿真主機(jī)不同步，慣導(dǎo)解算就會(huì)產(chǎn)生較大的 積分累積誤差，甚至引起系統(tǒng)的發(fā)散。所以如何確定積分開(kāi)始時(shí)間，即實(shí)現(xiàn)導(dǎo)航控制計(jì)算機(jī) 與ADI機(jī)的同步時(shí)間關(guān)系到半實(shí)物仿真系統(tǒng)的精確性和實(shí)時(shí)性。在本發(fā)明中，通過(guò)引入開(kāi) 機(jī)信號(hào)作為仿真開(kāi)始信號(hào)實(shí)現(xiàn)了導(dǎo)航控制計(jì)算機(jī)的慣導(dǎo)解算與ADI實(shí)時(shí)仿真計(jì)算機(jī)的模 型解算及虛擬線加速度計(jì)的同步。如圖6中的開(kāi)機(jī)信號(hào)同時(shí)送至導(dǎo)航控制計(jì)算機(jī)、ADI實(shí) 時(shí)仿真計(jì)算機(jī)及虛擬線加速度計(jì)，這就保證了 ADI實(shí)時(shí)仿真計(jì)算機(jī)的模型解算和導(dǎo)航控制 計(jì)算機(jī)的積分時(shí)間相同，使積分誤差達(dá)到最小。(4)水下SINS+DVL組合導(dǎo)航半實(shí)物實(shí)時(shí)仿真試驗(yàn)進(jìn)行水下航行器在水下航行時(shí)導(dǎo)航與控制半實(shí)物仿真試驗(yàn)，模擬水下航行器從初 始位置運(yùn)動(dòng)到設(shè)定位置，接入的實(shí)物有IMU慣性測(cè)量裝置、深度傳感器和舵機(jī)組件等。ADI實(shí)時(shí)仿真計(jì)算機(jī)通過(guò)反射內(nèi)存網(wǎng)絡(luò)與深度模擬器、角運(yùn)動(dòng)模擬器、多普勒測(cè)速 模擬裝置連接，對(duì)其初值進(jìn)行設(shè)置，仿真初值設(shè)置如表1所示。在所有仿真設(shè)備到達(dá)初始位置后，由外部穩(wěn)壓電源送出開(kāi)機(jī)信號(hào)，啟動(dòng)半實(shí)物實(shí)時(shí)仿真系統(tǒng)。仿真開(kāi)始，ADI實(shí)時(shí)仿真 機(jī)解算水下航行器的動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)模型，角運(yùn)動(dòng)模擬器接收水下航行器姿態(tài)，模擬水下 航行器的姿態(tài)運(yùn)動(dòng)，深度模擬器接收水下航行器深度信號(hào)，為深度傳感器提供相應(yīng)的壓力 信號(hào)，多普勒測(cè)速儀仿真裝置接收水下航行器的速度信號(hào)，模擬多普勒測(cè)速儀回波信號(hào)。導(dǎo) 航控制計(jì)算機(jī)通過(guò)RS232串口采集IMU慣性測(cè)量裝置、多普勒測(cè)速儀和虛擬線加速度計(jì)敏 感到的水下航行器姿態(tài)、速度和加速度信息，通過(guò)D/A接口采集深度傳感器敏感到的深度 信息，進(jìn)行導(dǎo)航和控制系統(tǒng)解算，得到水下航行器操舵指令，控制舵機(jī)運(yùn)動(dòng)，舵機(jī)反饋信號(hào) 通過(guò)A/D接口送入ADI實(shí)時(shí)仿真計(jì)算機(jī)，從而構(gòu)成閉環(huán)仿真系統(tǒng)。圖7-圖12為仿真結(jié)果 曲線。表1仿真初值設(shè)置
權(quán)利要求
1.一種水下航行器組合導(dǎo)航與控制半實(shí)物仿真試驗(yàn)系統(tǒng)，包括ADI實(shí)時(shí)仿真計(jì)算機(jī)、 捷聯(lián)慣性導(dǎo)航子系統(tǒng)、衛(wèi)星導(dǎo)航子系統(tǒng)、導(dǎo)航控制計(jì)算機(jī)、舵機(jī)、多普勒導(dǎo)航子系統(tǒng)、虛擬線 加速度計(jì)和深度模擬子系統(tǒng)，其特征在于ADI實(shí)時(shí)仿真計(jì)算機(jī)解算水下航行器的動(dòng)力學(xué)和運(yùn)動(dòng)學(xué)數(shù)學(xué)模型，計(jì)算出水下航行器 的位置、姿態(tài)、速度和航向信息；SINS子系統(tǒng)包括反射內(nèi)存通訊板、角運(yùn)動(dòng)模擬器和IMU慣性測(cè)量裝置，IMU慣性測(cè)量 裝置固定在角運(yùn)動(dòng)模擬器上，實(shí)時(shí)敏感水下航行器在載體坐標(biāo)系下的三個(gè)角加速度和角速 度，并送到導(dǎo)航控制計(jì)算機(jī)，角運(yùn)動(dòng)模擬器通用反射內(nèi)存接收來(lái)自ADI實(shí)時(shí)仿真計(jì)算機(jī)的 水下航行器姿態(tài)信息，模擬水下航行器的姿態(tài)運(yùn)動(dòng)，為慣性組件提供虛擬試驗(yàn)環(huán)境；衛(wèi)星導(dǎo)航子系統(tǒng)包括衛(wèi)星導(dǎo)航模擬器和衛(wèi)星導(dǎo)航接收機(jī)，衛(wèi)星導(dǎo)航模擬器可實(shí)現(xiàn)對(duì)真 實(shí)衛(wèi)星導(dǎo)航系統(tǒng)的模擬——根據(jù)ADI實(shí)時(shí)仿真計(jì)算機(jī)發(fā)出的水下航行器的運(yùn)動(dòng)信息實(shí)時(shí)生 成并發(fā)射GPS/BD信號(hào)載波，衛(wèi)星導(dǎo)航接收機(jī)通過(guò)射頻天線實(shí)時(shí)地接收衛(wèi)星導(dǎo)航模擬器發(fā) 射的衛(wèi)星導(dǎo)航信號(hào)，然后把水下航行器所處的位置和在導(dǎo)航坐標(biāo)下的速度信息通過(guò)RS232 串口送給導(dǎo)航控制計(jì)算機(jī)；深度模擬子系統(tǒng)包括深度傳感器、I/O通訊模塊、反射內(nèi)存通訊板和深度模擬器，深度 傳感器安裝在深度模擬器上，深度模擬器通過(guò)反射內(nèi)存接收來(lái)自ADI實(shí)時(shí)仿真計(jì)算機(jī)發(fā)送 的水下航行器航行深度信號(hào)，將其轉(zhuǎn)換為相應(yīng)的壓力信號(hào)，深度傳感器將敏感到深度信息 通過(guò)I/O通訊模塊輸出至導(dǎo)航控制計(jì)算機(jī)；DVL導(dǎo)航子系統(tǒng)包括DVL測(cè)速仿真裝置、聲對(duì)接裝置和多普勒測(cè)速儀，ADI實(shí)時(shí)仿真計(jì) 算機(jī)通過(guò)反射內(nèi)存將水下航行器的速度信息傳遞給DVL測(cè)速模擬裝置，DVL測(cè)速模擬裝置 為多普勒測(cè)速儀提供水聲傳播環(huán)境，并通過(guò)聲對(duì)接裝置與多普勒測(cè)速儀連接，多普勒測(cè)速 儀根據(jù)發(fā)射的聲波與經(jīng)過(guò)反射回的聲波之間的頻率差求解水下航行器的速度，并傳送給導(dǎo) 航控制計(jì)算機(jī)；虛擬線加速度計(jì)包括哥氏加速度計(jì)算裝置、重力加速度計(jì)算裝置和數(shù)據(jù)融合裝置，哥 氏加速度計(jì)算裝置和重力加速度計(jì)算裝置分別接收來(lái)自ADI實(shí)時(shí)仿真計(jì)算機(jī)的信號(hào)，并計(jì) 算得到哥氏加速度和當(dāng)?shù)氐闹亓铀俣?，通過(guò)數(shù)據(jù)融合裝置得到水下航行器實(shí)時(shí)的比力信 息，而后送至導(dǎo)航控制計(jì)算機(jī)；導(dǎo)航控制計(jì)算機(jī)實(shí)時(shí)并行采集IMU慣導(dǎo)組件、衛(wèi)星導(dǎo)航接收機(jī)、深度傳感器、DVL測(cè)速 儀輸出的姿態(tài)、位置、深度、速度信息及虛擬線加速度計(jì)輸出的比力信息，完成慣性導(dǎo)航和 控制系統(tǒng)解算，輸出水下航行器的操舵指令，驅(qū)動(dòng)舵機(jī)運(yùn)動(dòng)；舵機(jī)接收導(dǎo)航控制計(jì)算機(jī)輸出的操作指令，模擬水下航行器垂直舵和水平舵的動(dòng)作。 舵機(jī)上的舵角反饋電位計(jì)將舵角信息反饋給ADI實(shí)時(shí)仿真計(jì)算機(jī)，構(gòu)成整個(gè)系統(tǒng)的閉環(huán)反 饋。
2.根據(jù)權(quán)利要求1所述的水下航行器組合導(dǎo)航與控制半實(shí)物仿真試驗(yàn)系統(tǒng)，其特征在 于所述的ADI實(shí)時(shí)仿真計(jì)算機(jī)、導(dǎo)航控制計(jì)算機(jī)和虛擬線加速度計(jì)分別通過(guò)I/O通訊模塊 連接由穩(wěn)壓電源提供的開(kāi)機(jī)信號(hào)，確保導(dǎo)航控制計(jì)算機(jī)的慣導(dǎo)解算與ADI實(shí)時(shí)仿真計(jì)算機(jī) 的模型解算及虛擬線加速度計(jì)的同步。
全文摘要
本發(fā)明公開(kāi)了一種水下航行器組合導(dǎo)航與控制半實(shí)物仿真試驗(yàn)系統(tǒng)，仿真計(jì)算機(jī)計(jì)算出水下航行器的位置、姿態(tài)、速度和航向信息；SINS子系統(tǒng)模擬水下航行器的姿態(tài)運(yùn)動(dòng)；衛(wèi)星導(dǎo)航子系統(tǒng)模擬水下航行器的位置和速度信息；深度模擬子系統(tǒng)模擬水下航行器航行深度信息；DVL導(dǎo)航子系統(tǒng)模擬求解水下航行器的速度；虛擬線加速度計(jì)模擬水下航行器的比力信息；導(dǎo)航控制計(jì)算機(jī)采集上述信息完成慣性導(dǎo)航和控制系統(tǒng)解算，驅(qū)動(dòng)舵機(jī)運(yùn)動(dòng)；舵機(jī)模擬動(dòng)作，并由舵角反饋電位計(jì)將舵角信息反饋給仿真計(jì)算機(jī)。本發(fā)明實(shí)現(xiàn)了水下航行器組合導(dǎo)航控制系統(tǒng)的聯(lián)調(diào)與測(cè)試。
文檔編號(hào)G01C21/16GK102004447SQ20101054493
公開(kāi)日2011年4月6日 申請(qǐng)日期2010年11月11日 優(yōu)先權(quán)日2010年11月11日
發(fā)明者康鳳舉, 張靜, 楊惠珍, 王圣潔, 謝攀 申請(qǐng)人:西北工業(yè)大學(xué)