基于嵌入式系統(tǒng)STM32的UUV通用運動控制層設(shè)計

朱衍明，林九根，宋家平，王天昊

(中國船舶工業(yè)系統(tǒng)工程研究院，北京 100094)

0 引言

海洋面積約占地球總面積的百分之七十一，并且在廣闊的海洋里蘊含著豐富的生物和礦物資源[1]。隨著人類工業(yè)化和城市化的進(jìn)程加劇，以及全球航海運輸?shù)倪M(jìn)一步發(fā)展，陸上資源日益枯竭，世界各國的能源開發(fā)正在由陸地轉(zhuǎn)向海洋，對海洋資源的爭奪和開發(fā)在21世紀(jì)將逐漸成為海洋大國的最重要研究方向之一[2]。

受海洋水下條件以及人類自身條件的約束，在對海洋的探索和開發(fā)過程中，無論在民用還是軍用產(chǎn)品上，人們通常選擇使用無人裝備，即水下機(jī)器人。無人水下航行器( UUV, unmanned underwater vehicle)正是這樣一種優(yōu)秀的水下無人裝備平臺，其運行方式為半自主式或智能自主式。能夠通過自身攜帶的能源和各種傳感器設(shè)備，完成對任務(wù)區(qū)域的探測和任務(wù)執(zhí)行并順利返航[3]。

近十幾年，隨著微電子技術(shù)的發(fā)展以及人工智能技術(shù)的突破，無人水下航行器得到了長足發(fā)展，UUV的控制系統(tǒng)也隨之越來越強(qiáng)大和復(fù)雜。傳統(tǒng)的集中式計算控制系統(tǒng)架構(gòu)已經(jīng)難以滿足復(fù)雜的任務(wù)控制需求，基于分布式分層理論的控制系統(tǒng)架構(gòu)已越來越多地被各科研機(jī)構(gòu)和開發(fā)商所采用[4-6]。本文正是在分層式UUV控制系統(tǒng)架構(gòu)的基礎(chǔ)上，提出了一種基于嵌入式系統(tǒng)STM32的UUV運動控制層設(shè)計方案，艇體采用X型舵加推進(jìn)器進(jìn)行運動執(zhí)行，能夠很好地緩解決策層的運算壓力，提高控制系統(tǒng)的整體可靠性。

1 UUV的總體設(shè)計

UUV的系統(tǒng)架構(gòu)會直接影響控制系統(tǒng)的設(shè)計和實現(xiàn)難度，并影響航行器的穩(wěn)定性、可靠性以及航行器的運動能力。合理的系統(tǒng)架構(gòu)能夠使UUV通過自身的各種傳感器獲得自身的運動信息和周圍環(huán)境的相關(guān)信息，能夠使UUV的決策系統(tǒng)做出合理的決策，并準(zhǔn)確合理地驅(qū)動實體層的具體執(zhí)行機(jī)構(gòu)，同時還具有一定的與其他系統(tǒng)相兼容的特點，并且更容易進(jìn)行系統(tǒng)能力擴(kuò)展，例如增添其他傳感器設(shè)備等。目前業(yè)界廣泛使用的系統(tǒng)架構(gòu)如圖1所示，其系統(tǒng)結(jié)構(gòu)分為4層，這4層既包含了硬件系統(tǒng)架構(gòu)，也包含了軟件系統(tǒng)架構(gòu)[7-9]。

圖1 分層式系統(tǒng)架構(gòu)

任務(wù)層是整個UUV控制系統(tǒng)的最高層，主要負(fù)責(zé)接收岸基端發(fā)來的任務(wù)指令，之后將要執(zhí)行的復(fù)雜任務(wù)解析成若干個子任務(wù)后傳遞給下一層，同時還要負(fù)責(zé)電池剩余電量的推算等相關(guān)系統(tǒng)管理任務(wù)。

決策規(guī)劃層主要涉及到UUV各種算法的實現(xiàn)，包括導(dǎo)航定位、地圖構(gòu)建、障礙檢測、狀態(tài)監(jiān)視，并據(jù)此進(jìn)行路徑規(guī)劃，實時確定出下一步的運行動作，如航行方向和航行速度。

運動控制層的作用是根據(jù)決策和規(guī)劃層得到的運行動作指令，采用一定的運動控制算法來解算出UUV各運動部件的具體運行參數(shù)，然后傳遞命令給實體層的各運動執(zhí)行部件，從而使UUV完成上層系統(tǒng)所規(guī)劃的運動路徑。

實體層指安裝在UUV上的推進(jìn)器、航向控制舵機(jī)等運動執(zhí)行設(shè)備以及其他傳感器等設(shè)備，所涉及的傳感器通常有深度計、高度計、多普勒測速儀、光纖陀螺儀、三軸加速度計、北斗、攝像頭、前視聲吶和側(cè)掃聲吶等。

2 運動控制層的設(shè)計

2.1 通信總線的選擇

UUV控制系統(tǒng)中的各層之間的通信以及傳感器的通信通常選用工業(yè)控制領(lǐng)域成熟的總線，如RS232總線、RS485總線、CAN總線和EtherCAT總線。其中CAN總線因相較RS232、RS485總線有更高的可靠性；相較EtherCAT總線具有低成本和易用性，因而在工業(yè)控制領(lǐng)域中得到了更廣泛的應(yīng)用，常被用在工業(yè)自動化、汽車、船舶、醫(yī)療設(shè)備、工業(yè)設(shè)備等領(lǐng)域[10]。

CAN(controller area network)總線是由德國Bosch公司研制開發(fā)的一種串行數(shù)據(jù)通信協(xié)議，應(yīng)用在汽車內(nèi)部在各部件之間進(jìn)行數(shù)據(jù)交互。CAN總線已被ISO國際標(biāo)準(zhǔn)組織制定為國際標(biāo)準(zhǔn)，并且廣泛應(yīng)用于嵌入式系統(tǒng)和分布式控制領(lǐng)域。CAN總線由OSI模型發(fā)展而來，但是CAN總線只有物理層、數(shù)據(jù)鏈路層和應(yīng)用層協(xié)議，較簡練的協(xié)議使得CAN總線具有較高的實時性[11]。CAN總線是對等總線，總線上每個節(jié)點都可以主動發(fā)起總線訪問，每個節(jié)點都有優(yōu)先級設(shè)定，并支持點對點、點對多和廣播等通信模式。CAN總線的傳輸介質(zhì)通常是傳統(tǒng)的雙絞非屏蔽線，總線通信速率會影響總線通信長度，在最高速率1 Mbps下的通信距離可達(dá)40 m。CAN總線節(jié)點在檢測出錯誤后會自動退出總線，使得其傳輸可靠性很高[12]。

UUV的控制系統(tǒng)具有分布式節(jié)點數(shù)量大的特點，因為UUV為獲得豐富的外界環(huán)境信息，通常會掛載眾多的各種傳感器設(shè)備。眾多傳感器使UUV具備更好的任務(wù)執(zhí)行能力，智能UUV則更是如此。CAN總線最高1 Mbps的傳輸速率對于UUV控制系統(tǒng)的運動控制信號和相關(guān)傳感器數(shù)據(jù)信號足以滿足需求。UUV所攜帶的水下傳感器一般比較昂貴，且UUV所在的水下工作環(huán)境相對比較惡劣，因而對控制系統(tǒng)通信鏈路的可靠性要求相對較高，因為一旦通信環(huán)節(jié)出現(xiàn)故障，將很可能會對UUV造成難以挽回的損失。而CAN總線協(xié)議的傳輸層和數(shù)據(jù)鏈路層具有完善的錯誤檢測和數(shù)據(jù)重發(fā)功能，保障了數(shù)據(jù)通信的高靠性，很好地滿足了工作環(huán)境要求。

因此本文設(shè)計的運動控制層選擇CAN總線完成與決策規(guī)劃層的控制指令數(shù)據(jù)交互，并通過CAN總線實現(xiàn)推進(jìn)器的運動控制通信和其他傳感器的讀取。

2.2 核心控制器的選擇

意法半導(dǎo)體的STM32系列處理器是專為要求高性能、低成本、低功耗的嵌入式應(yīng)用而設(shè)計的基于ARM公司Cortex-M的32位Flash微控制器。它為用戶提供了很高的開發(fā)自由度，既可以使用底層寄存器進(jìn)行編程，也可以很方便地使用官方提供的庫文件進(jìn)行編程，兩種編程方式都可以很方便地移植到同系列的32位產(chǎn)品中。STM32系列處理器具有高性能、實時性好、低功耗和低電壓操作等特性，同時還具有易于開發(fā)和集成度高的特點。按內(nèi)核架構(gòu)分為主流產(chǎn)品、超低功耗產(chǎn)品和高性能產(chǎn)品。

本文選用STM32F407ZGT6型高性能微控制器作為UUV運動控制層的核心計算機(jī)，它采用Cortex M4內(nèi)核，帶FPU和DSP指令集，最高運行頻率可達(dá)168 Mhz，并且擁有2個CAN、3個12位ADC、3個SPI、2個全雙工I2S、3個IIC、6個串口、1個10/100 M以太網(wǎng)MAC控制器、12個16位定時器、2個32位定時器、1個RTC等眾多接口和資源，完全能夠滿足運動控制層的接口需求和算力需求。

一般分布式控制系統(tǒng)從縱向看具有分層的特點，且層與層之間通過網(wǎng)絡(luò)總線通信，從橫向看則具有模塊化分散的特點，每個模塊之間也是通過各種工業(yè)總線進(jìn)行數(shù)據(jù)通信的。本文設(shè)計的UUV運動控制層通過CAN總線與決策規(guī)劃層通信，接收其下發(fā)的運控控制指令，通過另外一條CAN總線與實體層的推進(jìn)器以及其他傳感器進(jìn)行通信，進(jìn)行推進(jìn)器驅(qū)動和傳感器數(shù)據(jù)采集。一些傳感器設(shè)備亦可通過RS232或者其他工業(yè)總線與更高的其他層進(jìn)行數(shù)據(jù)交互。具體的運動控制層總線網(wǎng)絡(luò)架構(gòu)如圖2所示。

圖2 運動控制層總線網(wǎng)絡(luò)架構(gòu)

以STM32F407ZGT6為運動控制層核心控制器，設(shè)計兩路CAN總線、2路RS232總線和1路485總線、4路PWM輸出，并利用IIC接口外接一片2 Kbit存儲空間的EEPROM，用以保存舵機(jī)的初始偏差等掉電不丟失的參數(shù)。其中EEPROM實際電路如圖3所示。

圖3 EEPROM存儲電路

2.3 舵機(jī)的控制設(shè)計

所謂舵機(jī)，其本質(zhì)就是自帶簡易反饋系統(tǒng)的伺服電機(jī)，常見的舵機(jī)主要指的直流伺服電機(jī)，通過調(diào)節(jié)電機(jī)輸入信號的PWM占空比可以實現(xiàn)電機(jī)較精準(zhǔn)的舵機(jī)轉(zhuǎn)動角度控制。PWM(pulse width modulation)就是脈沖寬度調(diào)制，也就是占空比可變的脈沖波形。PWM的占空比，就是指高電平保持的時間，與該PWM時鐘周期時間之比。

舵機(jī)的控制一般需要一個20 ms左右的時基脈沖，該脈沖的高電平部分一般為0.5～2.5 ms內(nèi)的角度控制脈沖部分，總間隔為2 ms的時間長度。

本文使用D30型水下舵機(jī)，該舵機(jī)是一種空心杯直流電機(jī)，耐壓深度可達(dá)水下300 m，額定工作電壓為7.4 V，額定扭矩為15 kg·cm，轉(zhuǎn)動角度范圍為270°，信號范圍為0.5～2.5 ms，信號頻率為50～300 Hz。

該舵機(jī)的接線定義為紅色線為電源正極，黑色線為電源負(fù)極，白色線為控制信號輸入線即PWM信號線。STM32通過輸出PWM波即可完成舵機(jī)轉(zhuǎn)動角度的控制。

STM32F407ZGT6通過定時器產(chǎn)生PWM波形，其定時器除了TIM6和TIM7，其他的定時器都可以用來產(chǎn)生PWM輸出。其中高級定時器TIM1和TIM8可以同時產(chǎn)生多達(dá)7路的PWM輸出。而通用定時器也能同時產(chǎn)生4路的PWM輸出。本文設(shè)計的UUV運動控制層需要輸出4路獨立的PWM波形來分別控制4個舵機(jī)。因此STM32F407ZGT6有足夠的定時器資源來輸出4個獨立的PWM波。

STM32F407ZGT6通過定時器產(chǎn)生PWM波形的原理如圖4所示。圖中，我們假定定時器工作在向上計數(shù)PWM模式，且當(dāng)CNT

圖4 定時器產(chǎn)生PWM波形的原理

2.4 CAN總線設(shè)計

ISO 國際標(biāo)準(zhǔn)組織先后制定了ISO11898和ISO11519-2兩種CAN 總線協(xié)議標(biāo)準(zhǔn)，其中CAN 總線的高速通信標(biāo)準(zhǔn)為ISO11898 標(biāo)準(zhǔn)，其通信速度范圍為125 kbps～1 Mbps，CAN 總線的低速通信標(biāo)準(zhǔn)為ISO11519 標(biāo)準(zhǔn)，其通信速度范圍為 125 kbps 以下。兩種標(biāo)準(zhǔn)的數(shù)據(jù)鏈路層的定義是相同的，但是物理層定義不同。本文采用通信速率較高的ISO11898 標(biāo)準(zhǔn)設(shè)計，其物理層特征如圖5所示。

圖5 ISO11898標(biāo)準(zhǔn)CAN總線物理層特性

從圖5的ISO11898 標(biāo)準(zhǔn)物理層特性可以看出，顯性電平是CAN_H和CAN_L兩根信號線電壓之差2.5 V左右，對應(yīng)邏輯為0；而隱性電平是CAN_H和CAN_L兩根信號線電壓之差為0 V，對應(yīng)邏輯為1。CAN總線具有線與邏輯的特點，當(dāng)總線上某一個CAN節(jié)點輸出顯性電平，即邏輯0時，那么總線上的線與邏輯就總為邏輯0，即總為顯性電平，因而CAN總線上顯性電平具有優(yōu)先權(quán)。同理隱性電平則需要總線上所有節(jié)點都輸出隱性電平。另外，ISO11898 標(biāo)準(zhǔn)CAN總線協(xié)議的物理層要求在總線的兩端各加一個120 Ω的終端阻抗匹配電阻，以減少傳輸線上的回波反射，從而提高信號的完整性。因此本文的CAN收發(fā)電路設(shè)計如圖6所示。

圖6 CAN收發(fā)電路

如圖6所示,STM32F407ZGT6的兩個CAN口都被利用起來，一個CAN口用于與決策規(guī)劃層通信，另一個CAN口用于與實體層的推進(jìn)器或者其他傳感器通信。CAN物理層接口芯片采用TJA1050，它是控制器區(qū)域網(wǎng)絡(luò)(CAN)協(xié)議控制器和物理總線之間的接口，該器件完全兼容ISO11898標(biāo)準(zhǔn),為總線提供差分發(fā)射能力并為CAN控制器提供差分接收能力。

3 X型舵的控制

3.1 X舵的優(yōu)點

當(dāng)代UUV的日益關(guān)注水下機(jī)動性能，除了追求高航速之外，航行器的機(jī)動靈活性也成為衡量航行器戰(zhàn)術(shù)性能的重要指標(biāo)，而航行器尾舵作為航行的關(guān)鍵裝置，經(jīng)過近幾十年的演化和發(fā)展，逐漸形成“十字型”和“X型”兩種流派，尤其是近年X型尾舵日漸獲得新一代航行器的青睞，逐漸成為先進(jìn)UUV的標(biāo)配[13-14]。

隨著科技的發(fā)展，X型尾舵的操控性復(fù)雜性、可靠性和安全性不可同日而語，在克服控制方式復(fù)雜度等固有缺點之后，X型舵操控的優(yōu)勢愈加突出。X型尾舵的舵效要高于十字型尾舵，因為X型舵4個舵面可以同時參加航行器單一方向的機(jī)動，會提供更大的能量，而且每個舵面都可以單獨動作，即便出現(xiàn)其中一兩個舵面損壞的極端情況，航行器仍然能夠正常工作，這便大大提高了控制系統(tǒng)的冗余性和安全性[15]。

X型舵的4個舵面都能在水平面和垂直面產(chǎn)生操縱力，且4個舵面布置在航行器尾部的對角線上，因此在不突出筒體艇身筒體尺寸的情況下，舵面可以更好地讓航行器完成坐底海床的動作，不會撞壞底部舵面。因此更適合在淺海海域活動，靠岸也更安全，同時十分有利于UUV的布放和回收操作。因此本文的UUV運動控制層針對X型舵設(shè)計完成打舵控制。

3.2 X舵到十字舵的等效控制

X舵布局方式如圖7(a)所示，從尾部按順時針方向看，4個獨立舵舵角分別標(biāo)記為δ1、δ2、δ3、δ4，且規(guī)定向右打舵為正，向左打舵為負(fù)。圖7(b)為十字舵布局方式，其中δs、δr分別表示尾升降舵角和方向舵角[16-18]。

圖7 X舵與十字舵布局方式

任意一個舵面偏轉(zhuǎn)后都會產(chǎn)生一個空間力F，該力可以分解為公式(1)：

(1)

其中，X、Y、Z為X舵舵力在艇體坐標(biāo)軸上的分量，K、M、N為坐標(biāo)軸上的舵力矩分量。艇體只做潛浮運動時，十字舵表達(dá)為δr=0、δs≠0，X舵表達(dá)如公式(2)所示：

(2)

艇體只做轉(zhuǎn)向運動時，十字舵表達(dá)為δr≠0、δs=0，X舵表達(dá)如公式(3)所示：

(3)

X型舵由于其空間布局方式，不存在直接的方向舵和升降舵，每個舵面的偏轉(zhuǎn)都會引起航行器的潛浮與轉(zhuǎn)向，因此需要多個(4個或者2個)舵面的協(xié)同控制來實現(xiàn)單一方向的航向或者深度控制。以向左轉(zhuǎn)向運動為例，同時打下舵(δ1=δ2<0)或者同時打上舵(δ3=δ4<0)或者同時打左舵(δ2=δ3<0)或者同時打右舵(δ1=δ4<0)在理論上都能實現(xiàn)艇體向左轉(zhuǎn)，但考慮到艇體尾部對水流的影響，以及為避免艇體橫傾，同時打左舵(δ2=δ3<0)是最佳方案[19-20]。同理向右轉(zhuǎn)向運動最好同時打右舵(δ1=δ4>0)，上浮運動最好同時打上舵(δ3=(-δ4)>0)，下潛運動最好同時打下舵((-δ1)=δ2>0)。

4 實驗與測試

將通用運動控制層控制器作為下位機(jī)與UUV尾段參照圖2進(jìn)行電氣連接，其中上層通訊CAN總線通過仿真器與上位機(jī)電腦相連，下層CAN總線進(jìn)行推進(jìn)器控制，PWM輸出用于舵機(jī)控制。之后將UUV尾段置于水箱中進(jìn)行運動控制測試，實測圖如圖8所示。

圖8 UUV尾段實測圖

主要測試項目和測試方法如表1所示，測試目的在于完成運動控制層的核心能力驗證，即對上層通信和對下層運動控制。

表1 測試項和測試方法

下位機(jī)通過上層通訊CAN總線與上位機(jī)通信時需要自定義控制命令格式，此處CAN通信采用1 Mbps的通信速率，并通過標(biāo)準(zhǔn)數(shù)據(jù)幀格式進(jìn)行通信，且其數(shù)據(jù)字段長度為8個字節(jié)，數(shù)據(jù)字段格式直接采用推進(jìn)器通信協(xié)議里的定義，如圖9所示。這樣下位機(jī)在收到CAN總線數(shù)據(jù)幀后進(jìn)行解析，如果是推進(jìn)器的控制指令則直接轉(zhuǎn)發(fā)給推進(jìn)器控制器，從而完成推進(jìn)器的控制測試。

圖9 數(shù)據(jù)字段格式

同樣，如果下位機(jī)在收到CAN總線數(shù)據(jù)幀后解析為打舵控制命令，則其攜帶數(shù)據(jù)被轉(zhuǎn)換為X舵的4個舵機(jī)舵角的PWM控制量輸出，從而完成舵機(jī)的控制測試。

按照以上方法進(jìn)行實驗測試。測試結(jié)果表明本，采用該設(shè)計方案完成的通用運動控制層控制器能夠?qū)崿F(xiàn)X型舵的上下左右打舵操作，以及推進(jìn)器的正推、反推控制，并且完成與上層通信的1 Mbps速率的CAN總線數(shù)據(jù)交互。達(dá)到了UUV運動控制層的設(shè)計目標(biāo)。

5 結(jié)束語

以嵌入式系統(tǒng)STM32為核心構(gòu)建的UUV運動控制層設(shè)計方案，符合分布式計算和模塊化設(shè)計的原則，能夠?qū)崿F(xiàn)UUV實體層X型舵的打舵控制和推進(jìn)器的推力控制以及同上層的通信，并能提供富裕的接口和算力進(jìn)行各類傳感器的數(shù)據(jù)交互。該設(shè)計方案具有較強(qiáng)通用性，提供了比較完整的UUV運動實體層和運動控制層解決方案。

本設(shè)計方案需要進(jìn)一步完善X型舵的控制策略，解決例如單舵卡舵情況下的航行器運動控制問題[21]。