一種用于深海裝備的著陸控制技術(shù)

章雪挺，魏晗冬，曾凡宗，張　明

（杭州電子科技大學(xué) 電子信息學(xué)院，浙江 杭州 310018）

一種用于深海裝備的著陸控制技術(shù)

章雪挺，魏晗冬，曾凡宗，張明

（杭州電子科技大學(xué) 電子信息學(xué)院，浙江 杭州 310018）

針對(duì)深海裝備在海洋科學(xué)調(diào)查和資源勘探等領(lǐng)域中的應(yīng)用，設(shè)計(jì)并實(shí)現(xiàn)了一種用于深海裝備的著陸控制技術(shù)，并完成了原理樣機(jī)的開發(fā)。系統(tǒng)以STM32處理器為核心，結(jié)合高度計(jì)、傾角傳感器及步進(jìn)電機(jī)，通過采用創(chuàng)新性的“二次調(diào)平”機(jī)制，在實(shí)驗(yàn)室水池中實(shí)現(xiàn)了原理樣機(jī)在復(fù)雜地形下的平穩(wěn)著陸與調(diào)平功能。經(jīng)測(cè)試，其能可靠地完成預(yù)定功能，滿足了設(shè)計(jì)的要求。

水下著陸技術(shù)；二次調(diào)平；STM32；PID算法

0　引言

深海裝備開發(fā)及其配套技術(shù)是海洋技術(shù)領(lǐng)域的重要分支，在國(guó)防安全、海洋科學(xué)調(diào)查、海洋資源勘探、水下工程等多個(gè)領(lǐng)域有著廣泛的應(yīng)用。本文根據(jù)深海裝備在海底作業(yè)中出現(xiàn)的實(shí)際需求，針對(duì)需要在海底復(fù)雜地形下平穩(wěn)著陸，保持水平姿態(tài)進(jìn)行作業(yè)的情況，提出了一種用于深海裝備的四足著陸控制技術(shù)，并制作了實(shí)現(xiàn)該技術(shù)的原理樣機(jī)。該樣機(jī)采用了傳感器技術(shù)、微處理技術(shù)、水下密封技術(shù)等構(gòu)成系統(tǒng)單元的硬件，在研究和分析了國(guó)內(nèi)外深海裝備著陸及調(diào)平技術(shù)方案的基礎(chǔ)上，采用了一種創(chuàng)新性的“二次調(diào)平”機(jī)制，并通過自適應(yīng)控制理論設(shè)計(jì)實(shí)現(xiàn)了一種水下四足著陸算法，在實(shí)驗(yàn)室水池中實(shí)現(xiàn)了原理樣機(jī)在模擬海底地形下的平穩(wěn)著陸與調(diào)平功能。與以往研究相比，該技術(shù)能實(shí)時(shí)根據(jù)地形地貌，自主、平穩(wěn)地著陸到海底地面，輔助勘探工作順利進(jìn)行，大大提高了水下資源采集的能力。

圖1　著陸控制系統(tǒng)工作示意圖

1　系統(tǒng)工作原理

深海裝備的著陸控制系統(tǒng)的工作示意圖如圖1所示。系統(tǒng)分為水上的甲板供電單元和水下作業(yè)平臺(tái)兩個(gè)部分。母船行駛到指定海域后，下放搭載了著陸控制系統(tǒng)的深海裝備進(jìn)行水下作業(yè)。當(dāng)整個(gè)裝備離底10 m時(shí)，著陸控制中的距離調(diào)平系統(tǒng)被觸發(fā)，4個(gè)安裝在支腿底部的距離傳感器開始測(cè)距，并通過RS-232總線將數(shù)據(jù)傳送到主控模塊。主控模塊進(jìn)行數(shù)據(jù)處理計(jì)算后，輸出PWM信號(hào)給電機(jī)驅(qū)動(dòng)模塊，從而通過電機(jī)的轉(zhuǎn)動(dòng)調(diào)整各個(gè)支腿的長(zhǎng)度，完成第一次調(diào)平工作。隨著整個(gè)裝備的繼續(xù)下降，當(dāng)離底距離小于 0.5 m時(shí)，距離調(diào)平系統(tǒng)停止工作。隨后裝備坐底，角度調(diào)平系統(tǒng)被觸發(fā)，安裝在平臺(tái)中央的角度傳感器將實(shí)時(shí)的水平X、Y兩個(gè)方向的角度通過I2C總線傳送給主控模塊，通過數(shù)據(jù)融合解算，反復(fù)調(diào)節(jié)各個(gè)支腿的伸縮長(zhǎng)度，使整個(gè)裝備盡可能地水平著陸在指定的海底。當(dāng)X、Y的角度同時(shí)小于1°時(shí)，角度調(diào)平系統(tǒng)暫停工作，從而完成整個(gè)裝備的第二次調(diào)平工作。這一調(diào)平技術(shù)可為該深海裝備的執(zhí)行機(jī)構(gòu)平穩(wěn)、可靠地進(jìn)行水下作業(yè)(如鉆機(jī)鉆具下行、CPT探針下行、ROV或HOV機(jī)械手取樣等)提供一系列的保障。

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

系統(tǒng)的硬件設(shè)計(jì)主要包含主控模塊、傳感器模塊和步進(jìn)電機(jī)驅(qū)動(dòng)模塊，如圖2所示。

圖2　水下四足機(jī)器人的整體框圖

本系統(tǒng)選擇意法半導(dǎo)體公司生產(chǎn)的STM32F103VCT6作為控制模塊處理器，其工作頻率高達(dá)72 MHz，內(nèi)部集成嵌套矢量中斷控制器 NVIC、64 Kb閃存、20 Kb的RAM，支持 SWD調(diào)試，多個(gè) 16位定時(shí)器可以實(shí)現(xiàn)輸入捕獲、PWM輸出等功能，另外芯片內(nèi)部集成 2個(gè)SPI外設(shè)接口、2個(gè) I2C接口、5個(gè) U(S)ART等外設(shè)，完全能滿足本系統(tǒng)設(shè)計(jì)的資源要求[1]。

角度傳感器采用的是ADI公司生產(chǎn)的采用MEMS技術(shù)的三軸加速度計(jì) ADXL345，具有小巧輕薄、超低功耗、可變量程、高分辨率等特點(diǎn)；具有 SPI和 I2C數(shù)字輸出功能；最大量程可達(dá)±16 g，另可選擇±2、±4、±8 g量程，可采用固定的 4 mg/LSB分辨率模式，該分辨率可測(cè)得0.25°的傾角變化。在系統(tǒng)工作時(shí)，微處理器通過SPI總線訪問ADXL345的寄存器中的 X、Y、Z的值，經(jīng)過處理得到水平 X、Y方向的傾角值[2]。

距離傳感器是直接采用 Tritech公司生產(chǎn)的 Micro Sounder高度計(jì)，其工作頻率為500 kHz，測(cè)量范圍0.5 m～50 m，分辨率達(dá)到1mm，完全符合本系統(tǒng)設(shè)計(jì)要求。其通過 RS232總線將高度值以 ASCII碼的形式傳送給微處理器。

電機(jī)驅(qū)動(dòng)模塊采用的是以THB7128作為驅(qū)動(dòng)芯片，其特點(diǎn)是：低功耗、多種細(xì)分、高細(xì)分，且電機(jī)運(yùn)行穩(wěn)定，無噪聲，不失步。采用兩片6N137高速光耦隔離輸入，在保護(hù)控制器的同時(shí)，更高的傳輸速率讓步進(jìn)電機(jī)工作更加穩(wěn)定準(zhǔn)確。在系統(tǒng)工作時(shí)，微處理器通過2個(gè)I/O與一路驅(qū)動(dòng)電路相連，分別控制步進(jìn)電機(jī)的轉(zhuǎn)速和轉(zhuǎn)向。

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

整個(gè)著陸控制系統(tǒng)以STM32微處理器為控制核心，使用C語(yǔ)言編寫。著陸控制系統(tǒng)軟件包括高度計(jì)和姿態(tài)模塊數(shù)據(jù)的采集、數(shù)據(jù)處理和PID控制三個(gè)部分。

3.1高度計(jì)的數(shù)據(jù)采集及處理

測(cè)距模塊程序設(shè)計(jì)可以分為以下幾個(gè)步驟，如圖3所示。

圖3　高度計(jì)數(shù)據(jù)采集流程圖

(1)系統(tǒng)初始化

上電后，對(duì)系統(tǒng)時(shí)鐘、I/O口、UART初始化配置。

(2)微處理器接收高度數(shù)據(jù)

單片機(jī)微處理器接收中斷程序，判斷是否接收到高度計(jì)發(fā)送數(shù)據(jù)的請(qǐng)求，如果接收到發(fā)送請(qǐng)求，則單片機(jī)進(jìn)入到接收數(shù)據(jù)狀態(tài)，存入預(yù)先設(shè)定好大小的數(shù)據(jù)緩存區(qū)。

(3)數(shù)據(jù)解析

從數(shù)據(jù)緩存區(qū)提取5幀完整字符串類型的數(shù)據(jù)，將其轉(zhuǎn)化成浮點(diǎn)型數(shù)據(jù)，求出這5量。組數(shù)據(jù)的平均值，存入數(shù)組變

3.2傾角計(jì)的數(shù)據(jù)采集及處理

ADXL345寄存器中存放的X、Y、Z 3個(gè)方向的加速度值，微處理器通過SPI總線取出數(shù)據(jù)，計(jì)算得到 X、Y方向的傾角值，軟件流程圖如圖4所示。

圖4　傾角計(jì)數(shù)據(jù)采集流程圖

圖5　著陸控制系統(tǒng)主程序流程圖

3.3著陸控制系統(tǒng)主程序設(shè)計(jì)

著陸控制系統(tǒng)主程序的軟件流程圖如圖5所示。主程序?qū)⒉杉降母叨扔?jì)和傾角計(jì)數(shù)據(jù)進(jìn)行解析、判斷、運(yùn)算，結(jié)合 PID算法，獲得電機(jī)控制量。

距離調(diào)平系統(tǒng)采用向平均值靠攏的方法，設(shè)H為各個(gè)支腿的高度值，i為支腿編號(hào)，j為測(cè)量序數(shù)，P、I、D分別為PID調(diào)節(jié)中的比例、積分和微分參數(shù)，M為電機(jī)控制量。通過4個(gè)支腿的高度值Hi（i=1，2，3，4），分別計(jì)算出誤差ΔHi：

根據(jù)實(shí)驗(yàn)得出的PID參數(shù)P、I、D，分別計(jì)算出各自的控制量 MPi、MIi、MDi[3，4]：

根據(jù)式(2)～(4)計(jì)算出總的電機(jī)控制量：

角度調(diào)平系統(tǒng)采用位置誤差控制調(diào)平的方法。當(dāng)整個(gè)設(shè)備坐底后，各個(gè)支腿承受較大的力，因此采用各支腿只升不降，其他支腿向最高點(diǎn)靠攏的方式，可以快速平穩(wěn)地完成系統(tǒng)的“微調(diào)”。

在如圖6所示的模型分析圖中，OX0Y0為水平坐標(biāo)系，OXY為平臺(tái)坐標(biāo)系[5，6]。其中，平臺(tái)長(zhǎng)為 La，寬為 Lb，平臺(tái)與X、Y方向的傾角分別為琢、茁。首先根據(jù)琢、茁的大小判斷出最高的支腿；其次，設(shè)平臺(tái)的4個(gè)支點(diǎn)的坐標(biāo)為（xi，yi，0）(i=1，2，3，4)，根據(jù)數(shù)學(xué)推算，得出各個(gè)支點(diǎn)的坐標(biāo)為：

圖6　模型分析圖

當(dāng)平臺(tái)剛落地后，各個(gè)支點(diǎn)在OX0Y0中的縱坐標(biāo)的初值：

根據(jù)式(7)，這里必有最高腿存在，同時(shí)假定i=h，且zi≤zh。那么動(dòng)作過程中，各腿的高度差滿足：

若琢、茁都大于零，可判斷出支腿1為最高腿，支腿3為最低腿，則有：e1=0，e2=琢La，e3=琢La+茁Lb，e4=茁Lb。

支腿總的行程量為：

4　系統(tǒng)調(diào)試與結(jié)論

在實(shí)驗(yàn)室水池中搭建的試驗(yàn)環(huán)境如圖7所示。在水池底部模擬一個(gè)高低不平的復(fù)雜海底環(huán)境，并將整個(gè)裝置通過滑輪吊到半空中，處于模擬海底的正上方；接通電源，通過滑輪將裝置緩緩下放；下放過程中，4個(gè)支腿將會(huì)通過水底的實(shí)際環(huán)境自動(dòng)調(diào)節(jié)各個(gè)支腿的高度，進(jìn)行“粗調(diào)”；當(dāng) 4個(gè)支腿到達(dá)底部時(shí)，傾角傳感器開始工作，進(jìn)行 “微調(diào)”；當(dāng)傾斜度小于閾值時(shí)，調(diào)平結(jié)束，著陸成功。圖7所示為調(diào)平過程中各個(gè)支腿的高度曲線。結(jié)果表明，系統(tǒng)工作正常，能很好地在水下完成平穩(wěn)的著陸。

圖7　各支腿實(shí)時(shí)高度曲線圖

[1]ST Microelectronics.STM32F103xC STM32F103xD STM32-F103xE[Z].2008.

[2]肖茜，陳慶.基于 ADXL345的超低功耗傾角測(cè)量?jī)x的設(shè)計(jì)[J].電腦知識(shí)與技術(shù)，2014(16)：3923-3925.

[3]徐鋒，張嫣華.數(shù)字控制系統(tǒng)的 PID算法研究[J].機(jī)床電器，2008(6)：8-10.

[4]王虎，彭如恕，尹泉.基于STM32嵌入式模糊PID步進(jìn)電機(jī)控制系統(tǒng)的設(shè)計(jì)[J].機(jī)械工程師，2014(11)：139-141.

[5]何兵懷.多點(diǎn)調(diào)平系統(tǒng)的研究[D].成都：電子科技大學(xué)，2014.

[6]何林立.大型重載平臺(tái)的自動(dòng)調(diào)平系統(tǒng)研究[D].太原：中北大學(xué)，2010.

A kind of landing control technology used for deep-sea equipment

Zhang Xueting，Wei Handong，Zeng Fanzong，Zhang Ming
(Electronic Information College，Hangzhou Dianzi University，Hangzhou 310018，China)

For the application of deep-sea equipment in marine scientific research and in the field of resources exploration,a kind of landing control technology used for deep-sea equipment was developed and implemented,and a principle prototype was also completed.The principle prototype takes STM32 processor as the core and combines with altimeter,inclinator and step motor.Then with the adoption of innovative mechanism of"secondary leveling",the principle prototype realized the complex terrain of smooth landing and the leveling function in the environment of the laboratory pool.The test shows that it can accomplish those functions in a reliable way and meet the requirements of the design.

underwater landing technology；secondary leveling；STM32；PID algorithm

TP273+.2

A

10.16157/j.issn.0258-7998.2015.09.011

2015-04-09)

章雪挺（1978-），男，博士，副教授，主要研究方向：測(cè)量與傳感技術(shù)、海洋技術(shù)裝備系統(tǒng)集成等。

魏晗冬（1990-），男，碩士研究生，主要研究方向：海洋技術(shù)裝備系統(tǒng)集成等。

中文引用格式：章雪挺，魏晗冬，曾凡宗，等.一種用于深海裝備的著陸控制技術(shù)[J].電子技術(shù)應(yīng)用，2015，41(9)：42-44，

英文引用格式：Zhang Xueting，Wei Handong，Zeng Fanzong，et al.A kind of landing control technology used for deep-sea equipment[J].Application of Electronic Technique，2015，41(9)：42-44，