一種矢量推進的小型四軸水下機器人控制系統的制作方法
【專利摘要】一種矢量推進的小型四軸水下機器人控制系統，系統可劃分為人機協作層、決策層、規劃層、執行層。人機協作層由陸基/艇載監控系統構成，實現對AUV航向控制信號發送和接收傳感器數據并實時顯示于上位機監測軟件。決策層包括多傳感器信息采集融合系統用于接收航向控制信號、采集并融合傳感器數據；智能決策系統用于接收由多傳感器信息采集融合系統融合的傳感器數據與航向控制信號，通過決策算法處理生成決策指令。規劃層包括步態生成系統，負責根據決策指令進行機器人運動步態規劃并協調上下層間的數據通信。執行層包括驅動執行系統，用于實時控制舵機和直流噴水電機。本發明具有可靠性高、靈活性好、易于擴展、易于維護的特點，應用前景廣闊。
【專利說明】一種矢量推進的小型四軸水下機器人控制系統
【技術領域】
[0001]本發明涉及機器人【技術領域】，特別是關于一種矢量推進的小型四軸水下機器人的控制系統。
【背景技術】
[0002]水下機器人是一種可以在水中游動、作業或從事專門工作的水下航行裝置。目前，水下機器人在我國已經有了非常廣泛的應用，特別是在海洋湖泊的資源勘探、環境監測、地質地貌勘探調查等領域發揮著不可替代的作用。
[0003]矢量推進系統可以為小型水下機器人提供強勁的動力，還可為水下機器人自身姿態與航行狀態的調整提供有力幫助。目前應用前景較好的自治水下機器人(AUV)的矢量推進方式大多采用噴水驅動原理，其優點一是水下機器人在低速下仍然具有高可控性與高機動性；二是水下機器人能迅速改變航行方向，實現快速轉向。但水下機器人是在水中運動的具有六自由度的剛體，其本身就是一個強耦合的非線性系統，再加上矢量推進系統，使得控制問題變得復雜與困難。常規的水下機器人控制系統，特別是自治水下機器人控制系統，多采用集中式控制方式，即由主控計算機實現對所有傳感器和設備的數據采集及控制。但這種控制方式使主控計算機的工作任務十分繁重，效率較低，而且一旦主控計算機出現故障，整個系統就會癱瘓，可靠性差；由于主控計算機的各種通道、地址、中斷資源有限，很難隨意增加其他設備，可擴展性差。隨著水下機器人本身結構的復雜化以及用戶需求的多樣化，傳統的集中式控制方法已無法滿足水下機器人控制系統發展的需求。
【發明內容】
[0004]本發明的目的在于提供一種矢量推進的小型四軸水下機器人的控制系統，該系統采用與動物神經控制系統相似的結構，通過各層CPU之間的協同工作，實現對機器人各個關節的有效控制，在保證AUV具有足夠能動性和可靠性的同時，使其能夠利用傳感器信息融合系統實現對外部環境的實時感知，從而使AUV具有一定的自主運動能力。
[0005]為實現上述目的，本發明的技術方案是:
[0006]包括由數字舵機及直流噴水電機驅動的小型四軸水下機器人本體，智能決策系統、步態生成系統、驅動執行系統、多傳感器信息采集融合系統、電源系統和陸基/艇載監控系統。所述小型四軸水下機器人本體有四個結構相同的矢量推進機構，并在同一水平面均布于機器人圓盤狀機身，與四軸飛行器結構布局相同。每個矢量推進機構有兩個關節，其中，由靠近機器人中心的舵機實現一個矢量推進機構的軸向自由度，定義為內關節；遠離機器人中心的舵機實現一個矢量推進機構的徑向自由度，定義為外關節，直流噴水電機固定在機構最外端。
[0007]所述陸基/艇載監控系統包括多通道發射機、長波收發系統、上位機監測軟件，可建設于陸地、水面或搭載于艦艇上。航模發射機通過長波收發系統向AUV發送控制信號，AUV發送的傳感器數據、圖像信息由長波收發系統接收并實時顯示在上位機監測軟件上。[0008]所述多傳感器信息采集融合系統包括數字信號處理器、多通道串口擴展板、視覺傳感器、三軸加速度計、單波束前視聲吶、壓力傳感器、溫度傳感器、艙內漏水傳感器、高度計、深度計、長波收發系統，用于接收航向控制信號和采集并融合傳感器數據。
[0009]所述智能決策系統通過CAN總線接收由多傳感器信息采集融合系統采集并融合的傳感器信息與陸基/艇載監控系統發射的航向控制信號，通過決策算法模塊分析生成所需決策指令。
[0010]所述步態生成系統包括雙口 RAM控制器模塊、CPG步態規劃算法模塊、CAN總線控制器模塊，并通過CAN總線實現與所述驅動執行系統的信息傳遞。所述智能決策系統與所述步態生成系統通過雙口 RAM連接，實現二者之間的數據通信。
[0011 ] 所述驅動執行系統包括現場可編程門陣列和電機驅動器，現場可編程門陣列可以產生控制多路舵機和多路電機的PWM波，由電機驅動器驅動直流噴水電機。
[0012]本發明的一種矢量推進的小型四軸水下機器人控制系統將分級遞階控制系統與分布式控制系統(又稱為集散控制系統)有機結合，形成一種半自治集散遞階控制系統，既能實現控制任務的協調分配，又可保證系統的完整性。其核心在于AUV搭載的整個控制系統可劃分為三個層級:決策層、規劃層、執行層，每個層級由單CPU或多CPU構成。
[0013]本發明的決策層主要包括智能決策系統和多傳感器信息采集融合系統。智能決策系統由內嵌實時操作系統RT-Linux的ARM9構成，其主要功能模塊包括避障算法模塊、決策算法模塊、雙口 RAM驅動模塊。避障算法模塊又分為環境建模算法子模塊、局部避障算法子模塊。所述的多傳感器信息采集融合系統采用數字信號處理器DSP作為核心處理器，處理器內嵌傳感器驅動模塊，聲吶信號處理模塊、傳感器數據融合模塊，融合算法模塊，并通過片上接口與多通道串口擴展板串口與機載長波收發器和各個傳感器連接，用于接收陸基/艇載監控系統發出的航向控制信號和傳感器對AUV工作水域環境信息的采集及融合，保證AUV具有一定自主運動能力。在該子系統中，視覺傳感器用于實現AUV前方可見區域的三維重建；三軸加速度計用于測量AUV工作空間三個方向的線加速度；單波束前視聲吶用于探測AUV相對障礙物的位置；壓力傳感器和溫度傳感器分別用于檢測AUV工作水域的壓強和溫度；艙內漏水傳感器用于檢測AUV艙內是否進水；高度計和深度計分別測量AUV距水底和水面的距離。各類傳感器信息和航向控制信號通過多通道串口擴展板的相應接口和DSP相應接口傳輸至多傳感器信息米集融合系統的傳感器數據融合模塊,一方面將數據信息采樣并封裝，再經長波收發器發送給陸基/艇載監控系統，另一方面調用信息融合算法，產生決策算法模塊所需的數據，數據通過CAN總線傳送到智能決策系統，若檢測到障礙物，智能決策系統調用環境建模算法，采用單元數算法建立內部障礙物表示地圖模型，再調用局部避障算法，采用人工勢場算法由內部障礙物表示地圖模型中解算出自主避障局部路徑規劃，再調用決策算法處理，最后獲得相應的控制指令；若未檢測到障礙物，則經CAN總線傳輸到智能決策系統的數據直接由決策算法處理后獲得相應控制指令，該指令通過雙口RAM驅動模塊被存儲在雙口 RAM指定的決策指令地址空間內，以便步態生成系統隨時讀取。
[0014]本發明的規劃層由FPGA芯片及相應的外設接口構成，主要負責根據決策指令進行機器人運動步態的實施規劃并協調上下層之間的數據通信。步態生成系統內部集成了 Nios II軟核嵌入式處理器，利用SPOC Builder開發工具設計控制器的硬件系統，利用Quarts II設計控制器的軟件系統。步態生成系統在決策層觸發信號的引導下，對雙口 RAM決策指令地址空間進行讀取操作，獲得決策指令；CPG網絡解算模塊根據相應的決策指令利用數值求解算法進行解算，獲得各關節運動指令；CAN總線控制器模塊將運動指令封裝為CAN數據包后，放入總線通信層。基于FPGA的步態生成系統通過充分發揮FPGA的并行數據處理能力來提高機器人運動控制系統的穩定性和實時性。
[0015]本發明的執行層采用FPGA芯片作為核心處理器，通過光耦與8路舵機連接，實現舵機與FPGA完全電氣隔離，采用電機驅動芯片構成直流噴水電機驅動電路，實現對直流噴水電機的驅動控制。所述步態生成系統通過CAN總線將關節運動指令傳輸給驅動執行系統，驅動執行系統利用FPGA片內多路PWM發生器生成相應舵機和直流噴水電機的PWM控制信號，實時控制舵機和直流噴水電機的轉動。
[0016]本發明的陸基/艇載監控系統處于決策層之上，屬于人機協作層。發射機采用普通多通道航模發射機。發射機控制的是AUV的航向，其上共有兩個操縱桿，操縱桿I對應AUV水平面360 °的航向，操縱桿2對應AUV上、下兩個航向，這兩個操縱桿可以實現AUV空間任意方向上的運動。發射機發射連續的航向信號，由AUV的長波收發器接收，與傳感器信號融合后經CAN總線傳送到智能決策系統，調用決策算法處理產生AUV相應航向控制指令(如果有障礙物，則指令融合了避障路徑規劃)，通過雙口 RAM驅動模塊被存儲在雙口 RAM指定的決策指令地址空間內。上位機監測軟件采用LABVIEW及Solidworks聯合設計編寫的顯示及仿真平臺，該平臺包括采樣圖像顯示界面，障礙物提示界面，壓力、溫度、高度、深度數據顯示界面，艙內漏水報警器，三軸加速度計數據顯示界面，實時姿態仿真界面。長波收發系統接收由AUV發送的載有傳感器數據、圖像信息的長波信號，經解調后通過USB傳送給上位機并顯示在顯示仿真平臺的相應界面中。該系統利用在LABVIEW中搭建的控制器分析所傳回的姿態數據，通過LABVIEW與Solidworks的接口直接驅動Solidworks中的AUV三維模型，在顯示器上實時仿真AUV在水中的姿態。該系統還可將所采集回的所有數據、圖像信息自動進行整理并生成監測報告保存在上位機中，方便之后分析使用。
[0017]本發明有益效果表現在:
[0018]1.本發明通過使用將分級遞階控制系統與分布式控制系統有機結合，形成一種半自治集散遞階控制系統，使整個控制系統所建立的構架與動物神經控制系統一致，而且系統具有實時性高、可擴展性強、可靠性好、穩定性強、組態靈活、安全性高、易于維護等諸多優點。
[0019]2.本發明通過構建豐富的傳感器系統，使AUV能夠實現自我感知、環境預測、自主避障等功能，提高機器人對復雜應用環境的適應性。
[0020]3.本發明通過使用內嵌實時操作系統ARM9，具有并行數據處理能力的FPGA以及由高速運算率的DSP構成整個控制系統，在各個環節提高了系統的實時性，為機器人穩定運行提供了保障。
[0021]4.本發明采用雙口 RAM和CAN總線技術實現智能決策系統、步態生成系統、驅動執行系統、多傳感器信息采集融合系統之間的信息傳遞，該種方法所需外圍電路簡單，數據線使用量小，降低了系統設計的復雜性，并能保證有效信息傳遞的流暢性。
[0022]5.本發明使用航模發射機控制AUV航向，實現AUV的半自治控制，提高了 AUV的工作效率。
[0023]6.本發明所建立的人機交互界面即陸基/艇載監控系統能實時顯示AUV位姿及工作水域環境信息，可供研究人員及時做出判斷和決策。
[0024]7.本發明提供的顯示及仿真平臺實時顯示了 AUV當前水中姿態數據以及傳感器所檢測到的各種數據信息，并利用在LABVIEW中設計的的控制系統分析所傳回的姿態數據，進而通過LABVIEW與Solidworks的接口直接驅動Solidworks中的AUV三維模型,從而在顯示器上真實復現AUV在水中的姿態。
[0025]8.本發明設計的顯示及仿真平臺可對AUV傳感器數據、圖像信息自動整理并生成監測報告保存在上位機中，方便之后分析使用。
【專利附圖】
【附圖說明】
[0026]附圖1為本發明一種矢量推進的小型四軸水下機器人控制系統的整體設計框圖。
[0027]附圖2為本發明陸基/艇載監控系統結構示意圖。
[0028]附圖3為本發明航模發射機操縱桿控制方法示意圖。
[0029]附圖4為本發明智能決策系統結構示意圖。
[0030]附圖5為本發明多傳感器信息采集融合系統結構示意圖。
[0031]附圖6為本發明雙口 RAM通信方式示意圖。
[0032]附圖7為本發明步態生成系統結構示意圖。
[0033]附圖8為本發明CAN總線通信網絡結構示意圖。
[0034]附圖9為驅動執行系統功能結構示意圖。
【具體實施方式】
[0035]以下結合附圖和具體實施例對本發明的技術方案做進一步詳細描述。以下實施例不構成對本發明的限定。
[0036]圖1所示為本發明所述的一種矢量推進的小型四軸水下機器人控制系統，該系統按照結構和功能劃分為四層:人機協作層、決策層、規劃層、執行層。系統包括智能決策系統、步態生成系統、驅動執行系統、多傳感器信息采集融合系統、電源系統和陸基/艇載監控系統。從而構成一個完善的能夠實現機器人具有一定自主運動功能的控制系統。搭建在AUV內的多傳感器信息采集融合系統將采集到的數據和接收到的航向控制信號融合后通過CAN總線傳給智能決策系統，智能決策系統將數據信息進行處理運算，生成相應決策控制命令，再通過雙口 RAM傳遞給步態生成系統，步態生成系統根據相應的決策控制指令進行步態選擇與切換，經過CPG網絡解算后得到關節的運動參數，而后通過CAN總線網絡將數據包傳遞給驅動執行系統，進而驅動電機和舵機工作，實現AUV運動及避障。
[0037]圖2所示為本發明所述的陸基/艇載監控系統結構示意圖。該子系統搭載于地面、水面或艦艇，實現對AUV航向的控制及傳感器圖像數據的監視。航模發射機采用天地飛WFT07的7通道發射機。發射機通過操縱桿發射航向控制信號，經長波收發系統的短波接收機接收后傳輸給編碼調制電路及功率放大電路，將航向控制信號加載到長波上后通過水上無線發射信道向水下的AUV發射；AUV發射的長波信號通過長波收發系統的水上無線接收信道接收，傳輸給中頻解調解碼電路及放大混頻電路處理后通過USB串口傳送給上位機；上位機的監視軟件接收到數據后顯示在相應界面，姿態數據經處理后驅動AUV三維模型實時仿真AUV的真實姿態。[0038]圖3為本發明所述的航模發射機操縱桿控制方法示意圖。由圖(a)知發射機操縱桿I對應AUV水平面360 °的航向，操縱桿2對應AUV上、下兩個航向。其中每個通道所能產生的PWM波正脈寬范圍是0.5ms~2.5ms。從圖(b)可以看出航向控制方法，例如操縱
B
桿I推至圖示向量OM方向，則CHl與CH2各對應一個PWM脈寬A、B,由式0=arctan—即可
A
解算出航向角Θ的大小，其它象限亦是如此。而向量OM的模則表示了直流噴水電機推力分量在此航向下的大小。
[0039]圖4為本發明智能決策系統結構示意圖。內嵌實時操作系統RT-Linux的ARM9芯片構成系統的決策層核心處理器，芯片內部的豐富資源在保證系統功能的同時減少了硬件設備所需尺寸空間。該子系統外設中擴展一個串口用于程序測試。JTAG接口用于系統仿真及程序下載。決策層初始化數據存放在外部SD卡內，系統復位后自動讀取。決策層實時接收處理融合后的傳感器數據及航向控制信號，并通過決策算法得到相應控制指令，最后將控制指令通過雙口 RAM驅動模塊傳輸給規劃層。
[0040]附圖5為本發明多傳感器信息采集融合系統結構示意圖。該子系統采用數字信號處理器DSP作為核心處理器，采用德州儀器的TMS320C6713芯片，多通道串口擴展板由兩片四通道擴展芯片TL16C554A和一些輔助的器件組成。處理器內嵌傳感器驅動模塊，聲吶信號處理模塊、傳感器數據融合模塊，融合算法模塊，并通過片上接口與多通道串口擴展板串口與機載長波收發器和各個傳感器連接，用于接收陸基/艇載監控系統發出的航向控制信號和AUV工作水域環境信息的采集及融合，保證AUV具有一定自主運動能力。其中，視覺傳感器通過串口 6與多通道擴展板連接，5個單波束前視聲吶通過串口 I~5與多通道串口擴展板連接，長波收發器通過DSP片上串口與DSP連接，三軸加速度計通過片上I2C總線接口與DSP連接，壓力傳感器、溫度傳感器、高度計、深度計、艙內漏水傳感器分別通過片上A/D轉換接口與DSP連接。各類傳感器信息和航向控制信號通過相應的接口與多通道串口擴展板串口傳輸至多傳感器信息采集`融合系統的傳感器數據融合模塊，一方面將數據信息采樣并封裝，再經長波收發器發送給陸基/艇載監控系統；另一方面調用信息融合算法，產生決策算法模塊所需的數據，數據通過CAN總線傳送到智能決策系統。
[0041]圖6所示為本發明所述的雙口 RAM通信方式示意圖。決策層與規劃層的通信通過雙口 RAM實現。雙口 RAM是一種具有兩套完全獨立的數據線、地址線和讀寫控制線的SRAM存儲器，該存儲器允許兩個獨立的系統同時對其進行隨機訪問。硬件設計上，雙口 RAM控制器在FPGA內部通過編寫Verilog程序實現，ARM9通過相應的硬件驅動實現對雙口 RAM的訪問。由圖6可知，該模塊利用FPGA的硬件資源建立一塊數據存儲區，從而用于實現兩個控制器的數據交換，ARM9將生成的決策指令存儲在數據共享區內，并通知FPGA可以進行數據的讀取操作，當FPGA讀取相應數據后，通知ARM9可以進行數據的更新，從而保證數據的流暢性。
[0042]圖7所示為本發明所述的步態生成系統的內部功能結構圖。FPGA芯片構成步態生成系統的核心單元，FPGA強大的并行運算能力能夠很好的解決系統實時性的問題。在程序設計方面，通過硬件描述語言進行設計，并可通過軟件仿真，事先驗證程序設計的正確性。步態生成系統內部嵌入了 Nios II內核，用于實現CPG網絡解算算法及其他通信模塊的調用。本發明結構與四足機器人類似，故特別設計CPG網絡并應用于本發明的步態規劃中。在CPG網絡解算算法方面通過利用龍格庫塔法求解微分方程組的方式實現。CPG網絡解算模塊的輸入參數由雙口 RAM模塊在指定的數據存儲區獲取，解算后的結果通過CAN總線模塊進行數據封裝并傳輸到CAN總線上。步態生成系統內部各功能模塊的控制指令及數據通過Avalon總線交換。
[0043]圖8所示為本發明所述的CAN總線網絡構架結構圖。CAN總線的通信速率高，實時性好，非常適合機器人各關節軸的協調運動控制，便于構成多節點的通信網絡，同時克服了其他多點通信總線協議接線復雜的缺點。CAN遵從IS0/0SI模型，采用了其中的物理層、數據鏈路層與應用層。采用雙絞線作為通信介質，通信速率最高可達到1Mbps，直接傳輸距離最遠可達10km。由DSP和FPGA為主控芯片構成的驅動執行系統及由FPGA為主控芯片構成的步態生成系統共同組成CAN總線上的節點，每個節點設備都一個ID，設備將根據報文的ID決定接收或者屏蔽該報文。FPGA在向驅動執行系統傳遞關節運動參數前，先將數據封裝為指定的格式，DSP接收到數據后進行解碼，從而得到電機控制所需的參數。
[0044]圖9所示為本發明驅動執行系統功能結構示意圖。本發明的執行層采用FPGA芯片作為核心處理器，FPGA選擇Altera公司的EP1C3T100，通過光耦與8路舵機連接，實現舵機與FPGA完全電氣隔離，采用電機驅動芯片構成直流噴水電機驅動電路，實現對4個直流噴水電機的驅動控制。所述步態生成系統通過CAN總線將關節運動指令傳遞給驅動執行系統，驅動執行系統利用FPGA片內多路PWM發生器生成相應舵機和直流噴水電機的PWM控制信號，多路PWM發生器首先通過譯碼器譯碼地址，選擇對哪幾路PWM脈寬輸出或修改，并為每一路PWM設置一個寄存器來存儲相應的脈寬值，以此實時控制舵機和直流噴水電機的轉動。
【權利要求】
1.一種矢量推進的小型四軸水下機器人控制系統，其特征在于:系統可劃分為人機協作層、決策層、規劃層、執行層，采用CAN總線實現所有子系統間通信；人機協作層包括陸基/艇載監控系統，決策層包括多傳感器信息采集融合系統與智能決策系統，規劃層包括步態生成系統，執行層包括驅動執行系統。
2.按照權利要求1所述一種矢量推進的小型四軸水下機器人控制系統，其特征在于:陸基/艇載監控系統包括多通道航模發射機、長波收發系統、上位機監測軟件，該子系統可建設于陸地、水面或搭載于艦艇上。
3.按照權利要求2所述一種矢量推進的小型四軸水下機器人控制系統，其特征在于:航模發射機通過2.4GHz短波發射水下機器人航向控制信號，長波收發系統的短波接收機接收信號后傳輸給編碼調制電路及功率放大電路，將航向控制信號加載到長波上，再通過水上無線發射信道向水下的AUV發射；AUV發射的長波信號通過長波收發系統的水上無線接收信道接收,傳輸給中頻解調解碼電路及放大混頻電路處理后通過USB串口傳送給上位機；上位機的監視軟件接收到數據后顯示在相應界面，姿態數據經處理后驅動AUV三維模型實時仿真AUV的真實姿態。
4.按照權利要求3所述一種矢量推進的小型四軸水下機器人控制系統，其特征在于:上位機監測軟件采用LABVIEW及Solidworks聯合設計編寫的顯示及仿真平臺，該平臺包括采樣圖像顯示界面，障礙物提示界面，壓力、溫度、高度、深度數據顯示界面，艙內漏水報警器，三軸加速度計數據顯示界面，實時姿態仿真界面；長波收發系統接收由AUV發送的載有傳感器數據、圖像信息的長波信號，經解調后通過USB傳送給上位機并顯示在顯示仿真平臺的相應界面中，該系統利用在LABVIEW中搭建的控制器分析所傳回的姿態數據，通過LABVIEW與Solidworks的接口直接驅動Solidworks中的AUV三維模型，在顯示器上實時仿真AUV在水中的姿態。該系統還可將所采集回的所有數據、圖像信息自動進行整理并生成監測報告保存在上位機中。
5.按照權利要求1所述一種矢量推進的小型四軸水下機器人控制系統，其特征在于:多傳感器信息采集融合系統采用數字信號處理器DSP作為核心處理器，處理器內嵌傳感器驅動模塊，聲吶信號處理模塊、傳感器`數據融合模塊，融合算法模塊，并通過片上接口與多通道串口擴展板串口與機載長波收發器和各個傳感器連接，用于接收陸基/艇載監控系統發出的航向控制信號和傳感器對AUV工作水域環境信息的采集及融合；在該子系統中，視覺傳感器用于實現AUV前方可見區域的三維重建，三軸加速度計用于測量AUV工作空間三個方向的線加速度，單波束前視聲吶用于探測AUV相對障礙物的位置，壓力傳感器和溫度傳感器分別用于檢測AUV工作水域的壓強和溫度，艙內漏水傳感器用于檢測AUV艙內是否進水，高度計和深度計分別測量AUV距水底和水面的距離；各類傳感器信息和航向控制信號通過多通道串口擴展板的相應接口和DSP相應接口傳輸至多傳感器信息采集融合系統的傳感器數據融合模塊，一方面將數據信息采樣并封裝，再經長波收發器發送給陸基/艇載監控系統，另一方面調用信息融合算法，產生決策算法模塊所需的數據，數據通過CAN總線傳送到智能決策系統。
6.按照權利要求1所述一種矢量推進的小型四軸水下機器人控制系統，其特征在于:智能決策系統由內嵌實時操作系統RT-Linux的ARM9構成，其主要功能模塊包括避障算法模塊、決策算法模塊、雙口 RAM驅動模塊。避障算法模塊又分為環境建模算法子模塊、局部避障算法子模塊；智能決策系統接收到多傳感器信息采集融合系統發送的數據后，若檢測到障礙物，智能決策系統調用環境建模算法，采用單元數算法建立內部障礙物表示地圖模型，再調用局部避障算法，采用人工勢場算法由內部障礙物表示地圖模型中解算出自主避障局部路徑規劃，再調用決策算法處理，最后獲得相應的控制指令；若未檢測到障礙物，則經CAN總線傳輸到智能決策系統的數據直接由決策算法處理后獲得相應控制指令，該指令通過雙口 RAM驅動模塊被存儲在雙口 RAM指定的決策指令地址空間內，以便步態生成系統隨時讀取。
7.按照權利要求1所述一種矢量推進的小型四軸水下機器人控制系統，其特征在于:步態生成系統由FPGA芯片及相應的外設接口構成，內部集成了 Nios II軟核嵌入式處理器，步態生成系統在決策層觸發信號的引導下，對雙口 RAM決策指令地址空間進行讀取操作，獲得決策指令；CPG網絡解算模塊根據相應的決策指令利用數值求解算法進行解算，獲得各關節運動指令；CAN總線控制器模塊將運動指令封裝為CAN數據包后，放入總線通信層。
8.按照權利要求1或6、7所述一種矢量推進的小型四軸水下機器人控制系統，其特征在于:所述智能決策系統與步態生成系統之間的通信通過雙口 RAM實現。
9.按照權利要求1所述一種矢量推進的小型四軸水下機器人控制系統，其特征在于:采用FPGA芯片作為核心處理器，通過光耦與8路舵機連接，采用電機驅動芯片構成直流噴水電機驅動電路，實現對4個直流噴水電機的驅動控制；所述步態生成系統通過CAN總線將關節運動指令傳遞給驅動執行系統，驅動執行系統利用FPGA片內多路PWM發生器生成相應舵機和直流噴水電機的PWM控制信號，多路PWM發生器首先通過譯碼器譯碼地址，選擇對哪幾路PWM脈寬輸出或修改，并為每一路PWM設置一個寄存器來存儲相應的脈寬值，以此實時控制舵機和直流噴水電機的轉動。
10.按照權利要求2或3所述一種矢量推進的小型四軸水下機器人控制系統，其特征在于:航模發射機操縱桿控制方法為操縱桿I對應AUV水平面360°的航向，操縱桿2對應AUV上、下兩個航向。其中每個通道所能產生的PWM波正脈寬范圍是0.5ms~2.5ms。航向角的大小可由式解算出，而由操縱桿推`出的程度可解算出直流噴水電機推力分
A量在此航向下的大小。
【文檔編號】G05D1/10GK103823471SQ201410091538
【公開日】2014年5月28日 申請日期:2014年3月13日 優先權日:2014年3月13日
【發明者】羅慶生, 金誠, 梁偉棟, 張 浩, 許珂, 陳怡然 申請人:北京理工大學