基于ARM11的海洋浮標(biāo)云臺(tái)穩(wěn)定控制系統(tǒng)

周金金，林　志，王小英

（1.中國(guó)礦業(yè)大學(xué)信息與電氣工程學(xué)院，江蘇徐州221000；2.常熟理工學(xué)院電氣與自動(dòng)化工程學(xué)院，江蘇常熟215500）

?

基于ARM11的海洋浮標(biāo)云臺(tái)穩(wěn)定控制系統(tǒng)

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

（1.中國(guó)礦業(yè)大學(xué)信息與電氣工程學(xué)院，江蘇徐州221000；2.常熟理工學(xué)院電氣與自動(dòng)化工程學(xué)院，江蘇常熟215500）

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

關(guān)鍵詞：MPU6050；HMC5883L；卡爾曼濾波；四元數(shù)；云臺(tái)

0　引言

海洋資料浮標(biāo)能夠全天候、穩(wěn)定不間斷地對(duì)海洋中海水的溫度、壓力及氣象等諸多數(shù)據(jù)進(jìn)行采集，對(duì)海洋氣候?yàn)?zāi)害的預(yù)測(cè)、海洋科學(xué)的研究以及資源的開(kāi)發(fā)和利用等具有重要的現(xiàn)實(shí)意義[1-3]。隨著采集數(shù)據(jù)分析準(zhǔn)確度要求的提高，可視化實(shí)時(shí)圖像數(shù)據(jù)的作用越發(fā)顯著。而在現(xiàn)實(shí)監(jiān)測(cè)環(huán)境中，由于成像系統(tǒng)工作時(shí)受其載體的姿態(tài)變化和海浪等因素的影響，將導(dǎo)致獲得的圖像信息不穩(wěn)定或模糊，極大地限制了圖像信息的有效利用。

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

1　系統(tǒng)硬件設(shè)計(jì)

1.1系統(tǒng)硬件設(shè)計(jì)框架

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

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

圖1　系統(tǒng)框圖

1.2系統(tǒng)硬件連接

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

系統(tǒng)硬件電路原理圖如圖2所示。通過(guò)查看OK6410開(kāi)發(fā)板的底板原理圖可以發(fā)現(xiàn)，用于外接攝像頭的20針接口JP1口有備用的IIC接口。將1號(hào)引腳I2C SDA0和2號(hào)引腳I2C SCL0分別連接MPU6050的雙向數(shù)據(jù)線接口SDA和時(shí)鐘線接口SCL，17號(hào)引腳VDD3.3 V和20號(hào)引腳地則分別對(duì)應(yīng)MPU6050 和HMC5883L的VCC和GND。而HMC5883L的I2C接口與MPU6050的輔助接口相連，由于MPU6050模塊的接口是2.54mm的間距，而OK6410開(kāi)發(fā)板在設(shè)計(jì)考慮時(shí)為了精簡(jiǎn)開(kāi)發(fā)板的尺寸大小采用的接口都是2.0mm的間距，所以這里需要再通過(guò)2.54mm間距轉(zhuǎn)2.00mm間距的杜邦線來(lái)轉(zhuǎn)接。

1.2.2主控制單元與云臺(tái)攝像機(jī)單元的連接

云臺(tái)的控制接口RS232與OK6410開(kāi)發(fā)板在串口延長(zhǎng)線的基礎(chǔ)上，通過(guò)3根杜邦線將對(duì)應(yīng)的TXD、RXD、GND簡(jiǎn)單對(duì)接，即可形成基本的讀寫(xiě)通信，兩者的實(shí)物連接如圖3所示。

2　系統(tǒng)軟件設(shè)計(jì)

系統(tǒng)軟件框架設(shè)計(jì)如圖4所示，本系統(tǒng)主要實(shí)現(xiàn)下位機(jī)和上位機(jī)之間的通信。PC機(jī)作為上位機(jī)，主要是在VMwareWorkstation虛擬機(jī)環(huán)境下構(gòu)造基于Linux的Fedora系統(tǒng)，并在該系統(tǒng)內(nèi)配置armlinux-gcc交叉編譯環(huán)境進(jìn)行編程，生成的二進(jìn)制文件通過(guò)超級(jí)終端SecureCRT傳輸給開(kāi)發(fā)板。基于ARM11的OK6410開(kāi)發(fā)板作為下位機(jī)，在板內(nèi)燒寫(xiě)基于Linux3.0.1的Qtopia系統(tǒng)，通過(guò)調(diào)用內(nèi)置的終端命令軟件運(yùn)行二進(jìn)制文件，實(shí)現(xiàn)下位機(jī)模塊之間的通信。在PC端，進(jìn)行簡(jiǎn)單的配置后即可通過(guò)Ulead VideoStudio軟件獲取云臺(tái)攝像機(jī)圖像信號(hào)。

圖2　系統(tǒng)硬件電路圖

圖3　云臺(tái)攝像機(jī)與主控制單元的連接

圖4　系統(tǒng)軟件框架圖

2.1云臺(tái)通信配置

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

2.2系統(tǒng)程序設(shè)計(jì)

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

表1　SW2（3～4位）撥碼開(kāi)關(guān)狀態(tài)

表2　SW2（5～6位）撥碼開(kāi)關(guān)狀態(tài)

圖5　系統(tǒng)程序流程圖

2.2.1傳感器模塊程序設(shè)計(jì)

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

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

2.2.2串口通信程序設(shè)計(jì)

Linux系統(tǒng)通過(guò)把串口設(shè)備當(dāng)做文件來(lái)打開(kāi)，即fd1=open（“/dev/ttySAC0”，O_RDWR），并標(biāo)記為可讀可寫(xiě)，然后按照termios頭文件的描述來(lái)設(shè)置串口通信的基本參數(shù)，配置完畢后只需要調(diào)用Write函數(shù)即可向串口設(shè)備發(fā)送信號(hào)，最后在程序末尾用close關(guān)閉設(shè)備。

2.2.3算法處理程序設(shè)計(jì)

陀螺儀測(cè)姿短時(shí)間內(nèi)準(zhǔn)確度很高，但由于陀螺儀存在溫漂等緩慢變化的誤差[5]，隨著時(shí)間推移產(chǎn)生的誤差在積分運(yùn)算過(guò)程中不斷累積，使得誤差越來(lái)越大，從而影響姿態(tài)解算的精度，故不利于長(zhǎng)期運(yùn)行。加速度計(jì)測(cè)姿長(zhǎng)期穩(wěn)定，但受限于載體運(yùn)動(dòng)狀態(tài)，磁力計(jì)受浮標(biāo)環(huán)境影響，動(dòng)態(tài)性能較差[6]。為了得到較高精度的姿態(tài)角，且又能長(zhǎng)期穩(wěn)定地工作，就必須對(duì)各傳感器輸出數(shù)據(jù)進(jìn)行信息融合，本文采用擴(kuò)展卡爾曼濾波算法對(duì)傳感器單元進(jìn)行最優(yōu)化求解。

姿態(tài)解算單元流程圖如圖6所示。

圖6　姿態(tài)解算單元流程圖

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

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

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

3　系統(tǒng)測(cè)試

本文通過(guò)對(duì)靜止?fàn)顟B(tài)姿態(tài)角的平均誤差和均方誤差[6]來(lái)檢驗(yàn)算法的有效性。由于本云臺(tái)穩(wěn)定控制系統(tǒng)放置在海洋浮標(biāo)上，只需要考慮航向角和俯仰角即可，將姿態(tài)解算模塊水平放置在桌面上，調(diào)整面板使其航向角為58.2°，俯仰角為0.4°，采集到的姿態(tài)角曲線如圖7和圖8所示，從圖中可看出航向角在±0.2°波動(dòng)，俯仰角在±0.1°波動(dòng)，通過(guò)計(jì)算航向角的平均誤差和均方差為0.00370°和0.08980°，俯仰角的平均誤差和均方差為-0.00150°和0.02070°，基本準(zhǔn)確的反應(yīng)了實(shí)際的偏轉(zhuǎn)角度。

圖7　俯仰角曲線圖

圖8　航向角曲線圖

在測(cè)得云臺(tái)偏轉(zhuǎn)角度后，云臺(tái)將以水平速度280°/s、垂直速度100°/s的角速度完成反饋偏轉(zhuǎn)，由于云臺(tái)自身速度有限，實(shí)際測(cè)得最長(zhǎng)反饋調(diào)整時(shí)間為0.38s，基本達(dá)到實(shí)時(shí)穩(wěn)定控制的效果。當(dāng)云臺(tái)的視頻輸出線與PC端連接時(shí)，打開(kāi)Ulead VideoStudio軟件并進(jìn)行簡(jiǎn)單的配置，便可看到云臺(tái)攝像機(jī)所拍攝的視頻信息。

4　結(jié)束語(yǔ)

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

參考文獻(xiàn)

[1]唐歆.海洋資料浮標(biāo)水動(dòng)力分析及結(jié)構(gòu)研究[D].上海：上海海洋大學(xué)，2012.

[2]李崇.大型海洋資料浮標(biāo)前置機(jī)軟件建模及實(shí)現(xiàn)[D].青島：中國(guó)海洋大學(xué)，2012.

[3]周金元，唐原廣，趙曙東.基于海洋資料浮標(biāo)上目標(biāo)探測(cè)系統(tǒng)的集成設(shè)計(jì)[J].氣象水文海洋儀器，2013（6）：73-76.

[4]邱寶梅，王建文.嵌入式機(jī)載攝影穩(wěn)定平臺(tái)的設(shè)計(jì)[J].儀器儀表學(xué)報(bào)，2009，30（9）：1981-1984.

[5]劉娜娜.基于MEMS傳感器的姿態(tài)檢測(cè)系統(tǒng)設(shè)計(jì)[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2011.

[6]高同躍，丁衛(wèi)，高新聞.面向機(jī)載穩(wěn)定云臺(tái)的微姿態(tài)參考系統(tǒng)研究[J].傳感技術(shù)學(xué)報(bào)，2009，22（9）：1303-1307.

[7]袁書(shū)明，楊曉東，程建華.導(dǎo)航系統(tǒng)應(yīng)用數(shù)學(xué)分析方法[M].北京：國(guó)防工業(yè)出版社，2013：169-174.

[8]鄧正隆.慣性技術(shù)[M].哈爾濱：哈爾濱工業(yè)大學(xué)出版社，2006：152-159.

[9]高鐘毓.慣性導(dǎo)航系統(tǒng)技術(shù)[M].北京：清華大學(xué)出版社，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-），女，江西波陽(yáng)縣人，副教授，博士，研究方向?yàn)榍度胧綄?shí)時(shí)系統(tǒng)、圖形圖像處理。

作者簡(jiǎn)介：周金金（1988-），女，湖北隨州市人，碩士研究生，專業(yè)方向?yàn)榍度胧綄?shí)時(shí)系統(tǒng)、圖形圖像處理。

基金項(xiàng)目：江蘇省產(chǎn)學(xué)研聯(lián)合前瞻性項(xiàng)目（BY2010126）；蘇州市科技計(jì)劃項(xiàng)目（SGZ2012064）

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

doi：10.11857/j.issn.1674-5124.2016.01.017

文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：1674-5124（2016）01-0074-05