基于ARM11的海洋浮標云臺穩定控制系統

周金金，林　志，王小英

（1.中國礦業大學信息與電氣工程學院，江蘇徐州221000；2.常熟理工學院電氣與自動化工程學院，江蘇常熟215500）

?

基于ARM11的海洋浮標云臺穩定控制系統

周金金1，2，林志2，王小英2

（1.中國礦業大學信息與電氣工程學院，江蘇徐州221000；2.常熟理工學院電氣與自動化工程學院，江蘇常熟215500）

摘要：由于載體姿態的變化和海浪等因素的影響，海洋浮標成像系統所獲得的圖像不穩定或者模糊，如何改變現狀是人們不斷探討和研究的課題。結合陀螺傳感器MPU6050和磁力計HMC5883L設計一套基于ARM11的云臺穩定控制系統，通過S3C6410的I2C接口讀取MPU6050和HMC5883L的數據，采用卡爾曼濾波算法對其進行處理，然后解算出載體的航向角和俯仰角，實現云臺攝像機姿態的反向調整。當云臺攝像機與PC相連時，對UleadVideoStudio軟件進行簡單的配置，便可看到云臺攝像機所拍攝的視頻信息。實驗結果表明：云臺將以水平速度280°/s、垂直速度100°/s完成反向偏轉，最長反饋調整時間為0.38s，滿足工程上的應用需求；該系統電路結構簡單、成本低、可視化且穩定，可以移植到無人機及船舶監控等場合，具有一定的實用性。

關鍵詞：MPU6050；HMC5883L；卡爾曼濾波；四元數；云臺

0　引言

海洋資料浮標能夠全天候、穩定不間斷地對海洋中海水的溫度、壓力及氣象等諸多數據進行采集，對海洋氣候災害的預測、海洋科學的研究以及資源的開發和利用等具有重要的現實意義[1-3]。隨著采集數據分析準確度要求的提高，可視化實時圖像數據的作用越發顯著。而在現實監測環境中，由于成像系統工作時受其載體的姿態變化和海浪等因素的影響，將導致獲得的圖像信息不穩定或模糊，極大地限制了圖像信息的有效利用。

針對這一現象，本文設計一套海洋浮標內的云臺攝像穩定控制系統，采用微機械陀螺儀模塊和磁力計組成的姿態解算模塊比采用光學及撓性陀螺儀體積更小，功耗及成本更低，且抗沖擊力更好[4]；云臺穩定控制系統采用嵌入式技術，不僅體積小，成本低，響應快，且經過多種傳感器信息融合技術處理后，有效降低了海洋環境中震動及風速等外界因素的影響。能夠消除或減輕運動攝像載體對圖像的影響，大大提高獲取圖像的信息質量，為相關海洋環境參數的分析和我國海洋維權執法等提供可視化的依據。

1　系統硬件設計

1.1系統硬件設計框架

S3C6410是由三星公司推出的基于ARM11內核（ARM1176JZF-S）的低功耗、高性能、高性價比的RSIC處理器，它擁有一條具有獨立結構的load-store技術和算術流水的8級流水線，性能與ARM9相比提高了約40%。MPU6050是由InvenSense公司推出的整合性6軸運動處理組件，相較于多組件方案，免除了組合陀螺儀與加速度計之間的軸差問題，減少了大量的包裝空間，且可由第2個I2C端口連接一個第3方的數字傳感器，如磁力計、壓力傳感器等；另外它還內嵌了1個12位的溫度傳感器和在工作環境下僅有±1%變動的振蕩器。MPU6050的角速度全格感測范圍為±250°/s、±500°/s、±1000°/s與±2000°/s，可準確追蹤快速與慢速動作，并且，用戶可程式控制的加速器全格感測范圍為±2g、±4g、±8g與±16g。HMC5883L是由Honeywell公司推出的一款具有最先進的高分辨率HMC118X系列的高集成、低成本的磁阻傳感器，并采用各向異性磁阻技術，具有較好的可靠性和高的靈敏度。上述系統硬件部分體積小、功耗低、性能好、價格也相對便宜，并且可以固態安裝，能夠有效降低云臺控制系統的體積、功耗和成本，滿足設計要求。本文設計的海洋浮標云臺穩定控制系統總體框圖如圖1所示。

從圖中可以看出，該系統按其功能結構可分為：傳感器單元、主控制單元及云臺攝像機通信單元。系統微處理器S3C6410通過I2C接口讀取由傳感器單元ADC所采集的3軸角速度分量、3軸加速度分量、3軸地磁場強度分量及溫度10軸數據，對其進行處理得到載體所對應的航向角和俯仰角等姿態信息并轉化成相應的運動指令，然后通過串口按照VISCA協議反饋給海洋浮標上的云臺，云臺做出相應的動作，使得相機等設備的視軸和目標視線在空間的同一個位置上，從而保證對目標圖像的穩定控制。而在云臺上的攝像機通過USB線將視頻信號實時傳送到PC端，并通過Ulead VideoStudio軟件顯示。

圖1　系統框圖

1.2系統硬件連接

1.2.1主控制單元與傳感器單元的連接

系統硬件電路原理圖如圖2所示。通過查看OK6410開發板的底板原理圖可以發現，用于外接攝像頭的20針接口JP1口有備用的IIC接口。將1號引腳I2C SDA0和2號引腳I2C SCL0分別連接MPU6050的雙向數據線接口SDA和時鐘線接口SCL，17號引腳VDD3.3 V和20號引腳地則分別對應MPU6050 和HMC5883L的VCC和GND。而HMC5883L的I2C接口與MPU6050的輔助接口相連，由于MPU6050模塊的接口是2.54mm的間距，而OK6410開發板在設計考慮時為了精簡開發板的尺寸大小采用的接口都是2.0mm的間距，所以這里需要再通過2.54mm間距轉2.00mm間距的杜邦線來轉接。

1.2.2主控制單元與云臺攝像機單元的連接

云臺的控制接口RS232與OK6410開發板在串口延長線的基礎上，通過3根杜邦線將對應的TXD、RXD、GND簡單對接，即可形成基本的讀寫通信，兩者的實物連接如圖3所示。

2　系統軟件設計

系統軟件框架設計如圖4所示，本系統主要實現下位機和上位機之間的通信。PC機作為上位機，主要是在VMwareWorkstation虛擬機環境下構造基于Linux的Fedora系統，并在該系統內配置armlinux-gcc交叉編譯環境進行編程，生成的二進制文件通過超級終端SecureCRT傳輸給開發板。基于ARM11的OK6410開發板作為下位機，在板內燒寫基于Linux3.0.1的Qtopia系統，通過調用內置的終端命令軟件運行二進制文件，實現下位機模塊之間的通信。在PC端，進行簡單的配置后即可通過Ulead VideoStudio軟件獲取云臺攝像機圖像信號。

圖2　系統硬件電路圖

圖3　云臺攝像機與主控制單元的連接

圖4　系統軟件框架圖

2.1云臺通信配置

在云臺和主機通信之前，要對云臺攝像機的工作地址和模式進行配置。SW1的8位全部置0，表示任何地址的代碼都可以控制云臺攝像機，方便之后的軟件調試。SW2的撥碼開關要配置如表1和表2所示，選擇通過VISCA協議傳輸，通信方式為RS232C串口，波特率為9600b/s。

2.2系統程序設計

系統程序流程圖如圖5所示，本程序可以大致分為MPU6050和HMC5883L模塊初始化子程序，串口初始化子程序、傳感器模塊讀取子程序、算法處理子程序、串口通信子程序等。

表1　SW2（3～4位）撥碼開關狀態

表2　SW2（5～6位）撥碼開關狀態

圖5　系統程序流程圖

2.2.1傳感器模塊程序設計

MPU6050和HMC5883L均是I2C通信，而Linux內核中有能夠普遍使用的I2C總線結構的讀寫指令，那么就需要了解Linux內核中的IIC基本讀寫程序的規范和調用規則。首先要定義和內核一致的struct i2c_rdwr_ioctl_data子程序和struct i2c_msg子程序。然后定義操作時序中的操作信號數mpu6050_data.nmsgs，接下來用打開文件的方式打開MPU6050設備，并用讀寫子函數來讀寫寄存器的值。另外由于MPU6050的每一個軸有16位數據，分為高低各8位，故在讀取傳感器數據的時候還需要合并高低位數據。

HMC5883L傳感器連接在MPU6050的輔助I2C接口上，只需要設置MPU6050的兩個寄存器0×6A、0×37的值為0×20、0×00，將MPU6050的I2C master模式打開，HMC5883L看成從設備I2C Slave0，然后通過讀取Slave0的寄存器值即可讀出HMC5883L的數據。

2.2.2串口通信程序設計

Linux系統通過把串口設備當做文件來打開，即fd1=open（“/dev/ttySAC0”，O_RDWR），并標記為可讀可寫，然后按照termios頭文件的描述來設置串口通信的基本參數，配置完畢后只需要調用Write函數即可向串口設備發送信號，最后在程序末尾用close關閉設備。

2.2.3算法處理程序設計

陀螺儀測姿短時間內準確度很高，但由于陀螺儀存在溫漂等緩慢變化的誤差[5]，隨著時間推移產生的誤差在積分運算過程中不斷累積，使得誤差越來越大，從而影響姿態解算的精度，故不利于長期運行。加速度計測姿長期穩定，但受限于載體運動狀態，磁力計受浮標環境影響，動態性能較差[6]。為了得到較高精度的姿態角，且又能長期穩定地工作，就必須對各傳感器輸出數據進行信息融合，本文采用擴展卡爾曼濾波算法對傳感器單元進行最優化求解。

姿態解算單元流程圖如圖6所示。

圖6　姿態解算單元流程圖

采用矩陣法[7-10]確定云臺的姿態和航向時，從云臺載體坐標系和地理坐標系之間的方向余弦矩陣即可求出俯仰角θ、橫滾角γ和俯仰角ψ，旋轉矩陣Cab如下式所示：

由于三角函數計算量大，故類比余弦矩陣，四元數矩陣如下式所示：

由式（1）和式（2）可得：

3　系統測試

本文通過對靜止狀態姿態角的平均誤差和均方誤差[6]來檢驗算法的有效性。由于本云臺穩定控制系統放置在海洋浮標上，只需要考慮航向角和俯仰角即可，將姿態解算模塊水平放置在桌面上，調整面板使其航向角為58.2°，俯仰角為0.4°，采集到的姿態角曲線如圖7和圖8所示，從圖中可看出航向角在±0.2°波動，俯仰角在±0.1°波動，通過計算航向角的平均誤差和均方差為0.00370°和0.08980°，俯仰角的平均誤差和均方差為-0.00150°和0.02070°，基本準確的反應了實際的偏轉角度。

圖7　俯仰角曲線圖

圖8　航向角曲線圖

在測得云臺偏轉角度后，云臺將以水平速度280°/s、垂直速度100°/s的角速度完成反饋偏轉，由于云臺自身速度有限，實際測得最長反饋調整時間為0.38s，基本達到實時穩定控制的效果。當云臺的視頻輸出線與PC端連接時，打開Ulead VideoStudio軟件并進行簡單的配置，便可看到云臺攝像機所拍攝的視頻信息。

4　結束語

本文詳細介紹了基于ARM11的海洋浮標云臺穩定控制系統的軟硬件設計，利用微機械陀螺儀和磁阻傳感器組成的測姿儀實時測得云臺的角速率、加速度及磁場強度，然后利用四元數計算出航向角和俯仰角，并結合高階卡爾曼濾波法對測得的數據進行信息融合來更新云臺攝像機的姿態。云臺將以水平速度280°/s、垂直速度100°/s的角速度完成反饋偏轉，使得相機等設備的視軸和目標視線在空間的同一個位置上，實際測得最長反饋調整時間為0.38s，基本達到實時穩定控制的效果。在今后的研究中可以加入WIFI模塊，以實現遠程控制和海洋環境圖像的遠程傳輸；同時，在云臺穩定控制的基礎上可進一步研究并實現云臺視頻序列的穩定輸出。

參考文獻

[1]唐歆.海洋資料浮標水動力分析及結構研究[D].上海：上海海洋大學，2012.

[2]李崇.大型海洋資料浮標前置機軟件建模及實現[D].青島：中國海洋大學，2012.

[3]周金元，唐原廣，趙曙東.基于海洋資料浮標上目標探測系統的集成設計[J].氣象水文海洋儀器，2013（6）：73-76.

[4]邱寶梅，王建文.嵌入式機載攝影穩定平臺的設計[J].儀器儀表學報，2009，30（9）：1981-1984.

[5]劉娜娜.基于MEMS傳感器的姿態檢測系統設計[D].哈爾濱：哈爾濱工業大學，2011.

[6]高同躍，丁衛，高新聞.面向機載穩定云臺的微姿態參考系統研究[J].傳感技術學報，2009，22（9）：1303-1307.

[7]袁書明，楊曉東，程建華.導航系統應用數學分析方法[M].北京：國防工業出版社，2013：169-174.

[8]鄧正隆.慣性技術[M].哈爾濱：哈爾濱工業大學出版社，2006：152-159.

[9]高鐘毓.慣性導航系統技術[M].北京：清華大學出版社，2012：10-17.

[10] LI G C，HE Y F，ZHU S B，et al. The MEMS gyro stabilized platform design based on Kalman Filter [C]∥International Conference Optoelectronics and Microelectr onics（ICOM）. Harbin：2013：14-17.

（編輯：李剛）

Design of PTZ stability control system in ocean buoy based on ARM11

ZHOU Jinjin1，2，LIN Zhi2，WANG Xiaoying2
（1. School of Information and Electrical Engineering，China University of Mining and Technology，Xuzhou 221000，China；2. School of Electric and Automatic Engineering，Changshu Institute of Technology，Changshu 215500，China）

Abstract:Because of carrier changing postures and waves and other influential factors，the images acquired from the image system on an ocean buoys is unstable or fuzzy. How to solve this problem has become a topic of constant discussion and research. A set of PTZ stability control system，comprising a gyroscopic sensor MPU6050 and a magnetometer HMC5883L，was developed on an ARM11. The operating principle of the system is as follows: read the data of MPU6050 and HMC5883L through the I2C interface of S3C6410 and process these data by Kalman filtering algorithm. Next，calculate the azimuth angle and pitching angle of the carrier to adjust reversely the posture of the PTZ camera. When the PTZ is connected with PC by USB，the video information can be seen by simple configuration of Ulead VideoStudio software. The experiment results have shown that the PTZ would complete reverse deflection at a horizontal speed of 280°/s and a vertical speed of 100°/s and the longest adjustment time of feedback is 0.38s，which meets the requirements of engineering application. The system，characterized by simple circuit structure，low cost，visualization and stability，can be transplanted to unmanned aerial vehicles（UAVs）and ship monitoring places.

Keywords:MPU6050；HMC5883L；Kalman filter；quaternions；PTZ

通訊作者：王小英（1975-），女，江西波陽縣人，副教授，博士，研究方向為嵌入式實時系統、圖形圖像處理。

作者簡介：周金金（1988-），女，湖北隨州市人，碩士研究生，專業方向為嵌入式實時系統、圖形圖像處理。

基金項目：江蘇省產學研聯合前瞻性項目（BY2010126）；蘇州市科技計劃項目（SGZ2012064）

收稿日期：2015-04-10；收到修改稿日期：2015-06-08

doi：10.11857/j.issn.1674-5124.2016.01.017

文獻標志碼：A

文章編號：1674-5124（2016）01-0074-05