本發(fā)明涉及無人船，特別涉及激光測深無人船的路徑跟蹤方法。
背景技術(shù)：
：：1、無人船在水深測繪領(lǐng)域有著廣闊的應(yīng)用前景。為確保執(zhí)行水深測繪任務(wù)時(shí)相鄰交叉條帶的重疊率超過?30%，并滿足點(diǎn)云數(shù)據(jù)水深測繪的精度要求，必須開發(fā)一種路徑跟蹤方法，以便無人船在湖泊、水庫和海洋等不同水域環(huán)境下自主運(yùn)行。2、無人船的路徑跟蹤控制通常采用級聯(lián)系統(tǒng)，即結(jié)合制導(dǎo)系統(tǒng)和控制系統(tǒng)。在制導(dǎo)系統(tǒng)方面，國內(nèi)外普遍使用視線（los）引導(dǎo)率。los引導(dǎo)率是一種模仿舵手操舵控制無人船航行的行為進(jìn)行路徑跟蹤的方法。在該方法控制下并不是直接控制無人船朝期望航點(diǎn)行駛，而是控制無人船朝向航線點(diǎn)的一段距離的目標(biāo)點(diǎn)行駛。通過船舶當(dāng)前位置與目標(biāo)點(diǎn)位的幾何關(guān)系，可以計(jì)算出船舶的期望航向。為了解決傳統(tǒng)los引導(dǎo)率在轉(zhuǎn)彎過程中的收斂速度慢、超調(diào)量大、抗干擾性差等問題，許多學(xué)者對los引導(dǎo)率加以改進(jìn)。3、目前，專利號為cn117724468a，名稱為一種智能無人船的改進(jìn)los和增量pid的路徑跟蹤方法的發(fā)明專利。專利中改進(jìn)的los采用有限時(shí)間觀測器對側(cè)滑角進(jìn)行實(shí)時(shí)補(bǔ)償，增量式pid控制方法中通過改進(jìn)的粒子群算法對pid參數(shù)進(jìn)行優(yōu)化。4、專利號為cn111830989a，名稱為一種基于內(nèi)?？刂婆c遺傳算法的無人船路徑跟蹤控制方法。專利中采用遺傳算法對控制器參數(shù)進(jìn)行全局優(yōu)化。但粒子群算法、遺傳算法等都屬于智能算法，要求控制器需要有一定的計(jì)算能力，不利于在微控制器中應(yīng)用。5、專利號為cn?116679714a，名稱為一種基于非線性mpc的氫燃料電池?zé)o人船路徑跟蹤控制方法的發(fā)明專利。該專利提出了自適應(yīng)的前視圓半徑策略與積分los策略，保證無人船能夠準(zhǔn)確地收斂至期望路徑，并設(shè)計(jì)了mpc航向控制器，實(shí)現(xiàn)航向跟蹤控制。6、專利號為cn115494847a，名稱為無人船的mpc控制方法和控制系統(tǒng)的發(fā)明專利。專利中采用了改進(jìn)的los與mpc控制結(jié)合的方法，預(yù)測無人船在未來某一時(shí)刻的首向角參考值，使無人船快速收斂至期望路徑。mpc要求控制器需要有一定的計(jì)算能力，不利于在微控制器中應(yīng)用。7、專利號為cn109976349a，名稱為一種含約束無人船的路徑跟蹤制導(dǎo)與控制結(jié)構(gòu)的設(shè)計(jì)方法的發(fā)明專利。專利中通過采用擾動觀測器對由含約束無人船動力學(xué)建模不確定性與海洋環(huán)境帶來外部擾動組成的集總不確定性進(jìn)行估計(jì)，所需調(diào)節(jié)參數(shù)少，易于調(diào)參。8、專利號為cn116974278a，名稱為基于改進(jìn)los的滑模無人船路徑跟蹤控制系統(tǒng)及方法的發(fā)明專利。專利中在制導(dǎo)部分提出一種基于可變切換圓半徑的制導(dǎo)方法，以此獲得期望航向角，同時(shí)采用非線性干擾觀測器對外部環(huán)境擾動進(jìn)行估計(jì)與補(bǔ)償。9、專利號為cn112835369a，名稱為一種基于eso估計(jì)漂角的無人船變速曲線路徑跟蹤控制方法的發(fā)明專利。專利利用擴(kuò)張狀態(tài)觀測器對側(cè)滑角進(jìn)行估計(jì)并在los引導(dǎo)率中補(bǔ)償，實(shí)現(xiàn)對環(huán)境擾動的預(yù)測。但觀測器的加入同樣會增加微控制器的計(jì)算效率，且該方法大多數(shù)還停留在仿真階段。10、專利號為cn111506086a，名稱為一種改進(jìn)los引導(dǎo)律結(jié)合模糊pid的無人船路徑跟蹤控制方法的發(fā)明專利。專利中采用改進(jìn)los引導(dǎo)律結(jié)合模糊pid的控制方法實(shí)現(xiàn)無人船路徑跟蹤控制。pid算法雖然較易實(shí)現(xiàn)，但存在魯棒性差的問題，抗干擾能力差。11、上述專利中為了提高路徑跟蹤的精度要求，加入了觀測器估計(jì)外部環(huán)境擾動或mpc控制預(yù)測無人船航向角的方法。雖然路徑跟蹤效果得到了提升，但也提高了路徑跟蹤方法的復(fù)雜程度，不利于微型控制器的開發(fā)，降低了系統(tǒng)的實(shí)用性，而且這些路徑跟蹤方法大部分停留在仿真階段，在實(shí)際工程應(yīng)用中還需進(jìn)一步驗(yàn)證。12、綜上可知，在現(xiàn)有的無人船路徑跟蹤系統(tǒng)中存在如下問題：①傳統(tǒng)的los引導(dǎo)率抗干擾性差、超調(diào)量大，導(dǎo)致路徑跟蹤精度較低。②改進(jìn)的los引導(dǎo)率與觀測器或mpc控制相結(jié)合的控制方法計(jì)算復(fù)雜，難以運(yùn)用在微型控制器中，實(shí)現(xiàn)輕小化、便攜化。技術(shù)實(shí)現(xiàn)思路1、針對上述問題，本發(fā)明公開了一種激光測深無人船路徑跟蹤的自適應(yīng)模糊積分微分視線（afidlos）法。它在傳統(tǒng)的los引導(dǎo)率上加以改進(jìn)，加入積分項(xiàng)與微分項(xiàng)，用于抵消無人船路徑跟蹤過程中由于橫向移動產(chǎn)生側(cè)滑角所造成的影響；加入時(shí)變前視距離方程，將原本取值固定的前視距離改進(jìn)為由橫向誤差調(diào)節(jié)取值的前視距離，并用模糊控制器調(diào)節(jié)方程中的收斂率，使前視距離的取值更加合理。afidlos法使得無人船在路徑跟蹤過程中的收斂速度更快，超調(diào)量更小。此方法最終運(yùn)用在本團(tuán)隊(duì)研發(fā)的激光雷達(dá)測深無人船上。2、本發(fā)明的技術(shù)方案為結(jié)合afidlos法和lqr控制，形成一種新的路徑跟蹤方法。3、afidlos法，所述方法包括：4、建立平面坐標(biāo)系，取任意地球的一點(diǎn)為坐標(biāo)原點(diǎn)，以正北方向?yàn)閤軸，正東方向?yàn)閥軸。5、獲取無人船位置，規(guī)劃期望路徑。6、根據(jù)無人船的位置以及期望路徑，計(jì)算橫向誤差。7、根據(jù)橫向偏差、積分項(xiàng)、微分項(xiàng)與前視距離，計(jì)算期望航向角。8、lqr控制，所述控制方法包括：9、建立無人船的航向控制模型。10、根據(jù)航向控制模型，建立模型的狀態(tài)空間表達(dá)式。11、根據(jù)建立的狀態(tài)空間表達(dá)式，使用lqr控制計(jì)算出無人船的控制率。12、afidlos法和lqr控制的結(jié)合設(shè)計(jì)，包括以下步驟：13、步驟1：設(shè)計(jì)idlos法。為了抵消無人船在路徑跟蹤過程中由于外界環(huán)境影響而產(chǎn)生側(cè)滑角所帶來的影響，在傳統(tǒng)los引導(dǎo)率計(jì)算los角的公式中加入積分項(xiàng)與微分項(xiàng)：14、15、其中，為橫向誤差，?為前視距離，為積分項(xiàng)，為微分項(xiàng)，積分項(xiàng)與微分項(xiàng)的表達(dá)形式分別為：16、17、18、其中，、為積分時(shí)間，是常微分系數(shù)，為橫向誤差變化率，是可變積分系數(shù)，其計(jì)算方式為：19、20、其中，為動態(tài)可調(diào)參數(shù)。最終得到idlos法的公式為：21、22、步驟2：設(shè)計(jì)自適應(yīng)模糊los法。提出時(shí)變前視距離los引導(dǎo)策略:23、24、其中，與分別為無人船的最大前視距離與最小前視距離，根據(jù)經(jīng)驗(yàn)一般取船長的2至4倍，為收斂率。25、針對收斂率的固定取值影響無人船路徑跟蹤精度的問題，提出自適應(yīng)模糊策略。26、首先進(jìn)行模糊化，設(shè)置橫向誤差、橫向誤差變化率與收斂率27、的論域，定義模糊子集，用模糊子集來表示論域中的準(zhǔn)確值。28、其次進(jìn)行模糊推理。根據(jù)人為提出的經(jīng)驗(yàn)，設(shè)置模糊控制規(guī)則表。29、最后進(jìn)行解模糊。采用重心法對上述模糊控制規(guī)則進(jìn)行解模糊化，得到收斂率模糊輸入輸出三維曲面。30、步驟3：建立無人船控制模型。無人船的控制模型為：31、32、其中，為質(zhì)量矩陣系數(shù)，為阻尼矩陣系數(shù)，為角速度，為旋轉(zhuǎn)力矩。33、該模型中的輸入是旋轉(zhuǎn)力矩，對于本專利中研究的雙體雙推進(jìn)式無人船，旋轉(zhuǎn)力矩的取值無法直接求得，但該取值與無人船左右推進(jìn)器的推力有關(guān)，而推進(jìn)器的推力與控制器發(fā)出的控制指令相關(guān)。因此通過建立控制指令與旋轉(zhuǎn)力矩之間的關(guān)系，將輸入為旋轉(zhuǎn)力矩的控制模型轉(zhuǎn)換為控制指令作為輸入的控制模型。最終得到無人船的控制模型為：34、35、步驟4：設(shè)計(jì)lqr航向控制器，得到輸入的控制率。將控制模型改寫為狀態(tài)空間方程：36、<mstyledisplaystyle="true"mathcolor="#000000"><mrow><mo>[</mo><mtable><mtr><mtd><moveraccent="true"><mi>r</mi><mo>˙</mo></mover></mtd></mtr><mtr><mtd><mi>δ</mi><moveraccent="true"><mi>ψ</mi><mo>˙</mo></mover></mtd></mtr></mtable><mo>]</mo></mrow><mi>=</mi><mrow><mo>[</mo><mtable><mtr><mtd><mi>?</mi><mfrac><msub><mi>d</mi><mn>33</mn></msub><msub><mi>m</mi><mn>33</mn></msub></mfrac></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mi>?</mi><mn>1</mn></mtd><mtd><mn>0</mn></mtd></mtr></mtable><mo>]</mo></mrow><mrow><mo>[</mo><mtable><mtr><mtd><mi>r</mi></mtd></mtr><mtr><mtd><mi>δ</mi><mi>ψ</mi></mtd></mtr></mtable><mo>]</mo></mrow><mo>+</mo><mrow><mo>[</mo><mtable><mtr><mtd><mfrac><mi>kd</mi><msub><mi>m</mi><mn>33</mn></msub></mfrac></mtd></mtr><mtr><mtd><mn>0</mn></mtd></mtr></mtable><mo>]</mo></mrow><mi>δ</mi><mi>n</mi></mstyle>37、lqr能找到使函數(shù)取得最小值的最優(yōu)控制率。其中、為狀態(tài)變量和輸入的權(quán)重矩陣。確定、后，可使用matlab的lqr函數(shù)計(jì)算值。38、步驟5：結(jié)合afidlos法和lqr控制，形成一種新的路徑跟蹤方法，并將其應(yīng)用在微控制器中，實(shí)現(xiàn)激光測深無人船的路徑跟蹤控制。39、本發(fā)明有益效果是：①針對傳統(tǒng)los引導(dǎo)率收斂速度慢、超調(diào)量大、抗干擾性差等問題，提出一種自適應(yīng)模糊積分微分視線（afidlos）法。②與傳統(tǒng)los引導(dǎo)率相比，afidlos法中增加了積分項(xiàng)與微分項(xiàng)，用于抵消路徑跟蹤過程中產(chǎn)生的側(cè)滑角所帶來的影響，提高了引導(dǎo)率的抗干擾性。③與傳統(tǒng)los引導(dǎo)率相比，afidlos法中采用了時(shí)變前視距離自適應(yīng)調(diào)整策略，并通過模糊控制方法調(diào)節(jié)時(shí)變方程中的收斂率，使得前視距離的取值更加合理。④結(jié)合afidlos法和lqr控制，形成一種新的路徑跟蹤方法，將其應(yīng)用在微控制器中，實(shí)現(xiàn)激光測深無人船的路徑跟蹤控制。當(dāng)前第1頁12當(dāng)前第1頁12