本發(fā)明屬于水下機(jī)器人推進(jìn)器技術(shù)，具體涉及一種基于自適應(yīng)pid控制的水下機(jī)器人推進(jìn)器控制裝置及其控制方法。

背景技術(shù)：

21世紀(jì)人類(lèi)面臨人口膨脹和生存空間有限的矛盾，占地球表面積71％的海洋，是一個(gè)富饒而遠(yuǎn)未得到開(kāi)發(fā)的寶庫(kù)。水下機(jī)器人的出現(xiàn)為人類(lèi)進(jìn)行深海資源的研究與開(kāi)發(fā)提供了強(qiáng)有力的工具。目前，水下機(jī)器人廣泛應(yīng)用于海洋救助與打撈、深海資源調(diào)查、海洋石油開(kāi)采、水下工程施工、軍事和國(guó)防建設(shè)等諸多方面，己經(jīng)產(chǎn)生了巨大的經(jīng)濟(jì)效益和社會(huì)效益，具有潛在的應(yīng)用前景。水下機(jī)器人作為人們探測(cè)水下世界的一種重要工具日益顯示著它的重要性

推進(jìn)器是水下機(jī)器人的主要組成部分，將其它形式的能源轉(zhuǎn)化成機(jī)械能從而推動(dòng)水下機(jī)器人運(yùn)動(dòng)，是水下機(jī)器人的動(dòng)力源泉。水下機(jī)器人在執(zhí)行作業(yè)需要完成指定的姿態(tài)和動(dòng)作時(shí)，推進(jìn)器發(fā)揮著關(guān)鍵性的作用。良好的水下推進(jìn)器控制系統(tǒng)是水下機(jī)器人姿態(tài)、航程、航速等相關(guān)性能的有力保證。因此高機(jī)動(dòng)性、高可靠性、高準(zhǔn)確性的水下機(jī)器人推進(jìn)器控制系統(tǒng)對(duì)我們開(kāi)發(fā)、利用海洋資源顯得十分重要。無(wú)刷直流電機(jī)由于其高效率、高可靠性以及維護(hù)成本低等優(yōu)點(diǎn)，在中小型水下機(jī)器人中深受青睞。

目前針對(duì)于水下機(jī)器人推進(jìn)器的控制裝置不多，傳統(tǒng)的水下機(jī)器人推進(jìn)器的控制方式為，直接將推進(jìn)器連接到水下機(jī)器人主控制器的i/o口，這樣的結(jié)構(gòu)大量占用水下機(jī)器人主控制器資源，且接線(xiàn)復(fù)雜，同時(shí)，推進(jìn)器自身無(wú)法形成轉(zhuǎn)速閉環(huán)，控制響應(yīng)慢、精度低，而且，無(wú)法獲得推進(jìn)器的轉(zhuǎn)速、溫度等信息。

目前的一些水下推進(jìn)器要么計(jì)算復(fù)雜，對(duì)處理器要求比較高，不便編程與實(shí)現(xiàn)，運(yùn)算速度慢，無(wú)法取得理想控制效果，不利于應(yīng)用于實(shí)際；要么需借助工程經(jīng)驗(yàn)數(shù)據(jù)為依據(jù)，建立專(zhuān)家數(shù)據(jù)庫(kù)，應(yīng)用范圍窄，系統(tǒng)算法龐大，不具有普遍應(yīng)用價(jià)值，且未給出控制系統(tǒng)的具體實(shí)現(xiàn)方式。

技術(shù)實(shí)現(xiàn)要素：

發(fā)明目的：本發(fā)明的目的在于解決現(xiàn)有技術(shù)中存在的不足，提供一種基于自適應(yīng)pid控制的水下機(jī)器人推進(jìn)器控制裝置及其控制方法。

技術(shù)方案：本發(fā)明的一種基于自適應(yīng)pid控制的水下機(jī)器人推進(jìn)器控制裝置，包括數(shù)據(jù)隔離模塊、can數(shù)據(jù)收發(fā)模塊、光電隔離驅(qū)動(dòng)模塊、控制器模塊以及溫度檢測(cè)模塊；水下機(jī)器人主控制器發(fā)送給推進(jìn)器的設(shè)定轉(zhuǎn)速經(jīng)過(guò)數(shù)據(jù)隔離模塊的隔離作用后傳輸?shù)絚an數(shù)據(jù)收發(fā)模塊，can數(shù)據(jù)收發(fā)模塊接收數(shù)據(jù)后傳輸給控制器模塊，控制器模塊根據(jù)轉(zhuǎn)速設(shè)定值輸出相應(yīng)占空比的pwm波，并傳輸?shù)焦怆姼綦x驅(qū)動(dòng)模塊，驅(qū)動(dòng)水下機(jī)器人推進(jìn)器動(dòng)作；水下機(jī)器人推進(jìn)器的霍爾傳感器信號(hào)經(jīng)過(guò)光電隔離驅(qū)動(dòng)模塊的隔離后傳輸?shù)娇刂破髂K，由控制器模塊計(jì)算出推進(jìn)器的實(shí)時(shí)轉(zhuǎn)速；控制器模塊以設(shè)定轉(zhuǎn)速和實(shí)時(shí)轉(zhuǎn)速為依據(jù)，運(yùn)用pid算法計(jì)算出相應(yīng)占空比的pwm波，調(diào)整水下機(jī)器人推進(jìn)器的轉(zhuǎn)速；所述溫度檢測(cè)模塊緊貼光電隔離驅(qū)動(dòng)模塊的mos管，實(shí)時(shí)檢測(cè)mos管的溫度，mos管實(shí)時(shí)溫度高于上限值時(shí)，控制器模塊啟動(dòng)推進(jìn)器保護(hù)動(dòng)作；mos管的實(shí)時(shí)溫度和推進(jìn)器的實(shí)時(shí)轉(zhuǎn)速通過(guò)can數(shù)據(jù)收發(fā)模塊上傳到水下機(jī)器人主控制器。

進(jìn)一步的，所述數(shù)據(jù)隔離模塊由數(shù)字隔離芯片構(gòu)成隔離電路，用以隔離推進(jìn)器與主控制器之間的can數(shù)據(jù)信號(hào)。

進(jìn)一步的，所述can數(shù)據(jù)收發(fā)模塊由can數(shù)據(jù)收發(fā)器芯片構(gòu)成數(shù)據(jù)收發(fā)電路，完成can數(shù)據(jù)包的接收與發(fā)送，此處的can數(shù)據(jù)包包括實(shí)時(shí)溫度和轉(zhuǎn)速數(shù)據(jù)。

進(jìn)一步的，所述光電隔離驅(qū)動(dòng)模塊由光電耦合器、spwm驅(qū)動(dòng)電路和mos管構(gòu)成隔離驅(qū)動(dòng)電路，完成控制器模塊與推進(jìn)器之間的完全電氣隔離以及推進(jìn)器的驅(qū)動(dòng)，抗干擾能力強(qiáng)，避免推進(jìn)產(chǎn)生的干擾信號(hào)傳輸?shù)娇刂蒲b置。

進(jìn)一步的，所述控制器模塊由控制器芯片構(gòu)成最小系統(tǒng)電路，完成can數(shù)據(jù)包的解析和打包處理、霍爾傳感器信號(hào)的解算、控制算法的實(shí)現(xiàn)、pwm波的產(chǎn)生以及實(shí)時(shí)溫度的計(jì)算。

本發(fā)明還公開(kāi)了一種基于自適應(yīng)pid控制的水下機(jī)器人推進(jìn)器控制裝置的控制方法，依次包括接收設(shè)定轉(zhuǎn)速、實(shí)際轉(zhuǎn)速的計(jì)算、控制算法的實(shí)現(xiàn)和pwm波的輸出，具體包括以下步驟：

步驟一：水下機(jī)器人主控制器通過(guò)can數(shù)據(jù)收發(fā)模塊將推進(jìn)器的設(shè)定轉(zhuǎn)速以及溫度上限設(shè)定值放在can數(shù)據(jù)包中，控制器模塊接收并解析指定的can數(shù)據(jù)包，將推進(jìn)器設(shè)定轉(zhuǎn)速以及溫度上限設(shè)定值保存到固定空間；

步驟二：控制器模塊采集推進(jìn)器的霍爾傳感器信號(hào)，實(shí)時(shí)計(jì)算推進(jìn)器的實(shí)時(shí)轉(zhuǎn)速，將推進(jìn)器的實(shí)時(shí)轉(zhuǎn)速保存到固定空間；

步驟三：控制器模塊采集溫度檢測(cè)模塊的溫度信號(hào)，實(shí)時(shí)計(jì)算光電隔離驅(qū)動(dòng)模塊的mos管溫度，將mos管的實(shí)時(shí)溫度保存到固定空間；

步驟四：控制器模塊根據(jù)設(shè)定轉(zhuǎn)速和實(shí)時(shí)轉(zhuǎn)速，運(yùn)行pid算法，輸出相應(yīng)占空比的pwm波，pwm波傳輸?shù)焦怆姼綦x驅(qū)動(dòng)模塊后，驅(qū)動(dòng)推進(jìn)器動(dòng)作，當(dāng)推進(jìn)器轉(zhuǎn)速超調(diào)過(guò)大或響應(yīng)時(shí)間過(guò)長(zhǎng)時(shí)，則控制模塊啟動(dòng)自適應(yīng)pid參數(shù)整定程序，運(yùn)用參數(shù)逼近原則，對(duì)系統(tǒng)的pid參數(shù)進(jìn)行重新調(diào)整，其中，超調(diào)范圍和響應(yīng)時(shí)間根據(jù)需求而定；

步驟五：溫度檢測(cè)模塊實(shí)時(shí)檢測(cè)光電隔離驅(qū)動(dòng)模塊的mos管度，當(dāng)mos管溫度高于上限值時(shí)，則說(shuō)明推進(jìn)器功率過(guò)高，發(fā)生故障，此時(shí)，控制器模塊停止pwm波的輸出，并將故障信息通過(guò)can通信發(fā)送到水下機(jī)器人主控制器，其余模塊正常工作，當(dāng)溫度回到正常范圍時(shí)，控制器模塊正常輸出pwm波，故障信號(hào)解除；

步驟六：控制器模塊將mos管的實(shí)時(shí)溫度以及推進(jìn)器的實(shí)時(shí)轉(zhuǎn)速放置到can數(shù)據(jù)包中，由can數(shù)據(jù)收發(fā)模塊將數(shù)據(jù)包發(fā)送到水下機(jī)器人主控制器。

有益效果：與現(xiàn)有技術(shù)相比，本發(fā)明具有以下優(yōu)點(diǎn)：

(1)本發(fā)明采用自適應(yīng)pid控制算法，既有常規(guī)pid控制算法的結(jié)構(gòu)簡(jiǎn)單、工作穩(wěn)定、魯棒性較強(qiáng)等優(yōu)點(diǎn)，又具有根據(jù)誤差大小自適應(yīng)調(diào)整控制器參數(shù)、能夠適應(yīng)被控過(guò)程參數(shù)變化等優(yōu)點(diǎn)；

(2)本發(fā)明采用can總線(xiàn)的通信方式，方便多個(gè)推進(jìn)器控制裝置的布線(xiàn)與連接，占用水下機(jī)器人主控制器的io口資源較少，且通信速度快，穩(wěn)定性高；

(3)本發(fā)明采用光電隔離驅(qū)動(dòng)模塊，實(shí)現(xiàn)了主控制器與推進(jìn)器之間的完全電氣隔離，避免推進(jìn)器的干擾信號(hào)傳遞到控制器模塊，提高了控制器模塊的穩(wěn)定性和精度；

(4)本發(fā)明可以實(shí)時(shí)獲得推進(jìn)器的轉(zhuǎn)速、溫度等信息，為水下機(jī)器人主控制器實(shí)現(xiàn)各種算法提供數(shù)據(jù)支持，且具有故障保護(hù)功能，安全系數(shù)高；

(5)本發(fā)明所用器件較少，體積小，成本低，方便安裝使用。

附圖說(shuō)明

圖1為本發(fā)明的數(shù)據(jù)流向圖；

圖2為本發(fā)明的結(jié)構(gòu)示意圖。

具體實(shí)施方式

下面對(duì)本發(fā)明技術(shù)方案進(jìn)行詳細(xì)說(shuō)明，但是本發(fā)明的保護(hù)范圍不局限于所述實(shí)施例。

如圖1所示，本發(fā)明一種基于自適應(yīng)pid控制的水下機(jī)器人推進(jìn)器控制裝置，包括數(shù)據(jù)隔離模塊1、can數(shù)據(jù)收發(fā)模塊2、控制器模塊3、光電隔離驅(qū)動(dòng)模塊4以及溫度檢測(cè)模塊5。如圖2所示，本發(fā)明中的整個(gè)控制裝置放置于外殼(未圖示)內(nèi)，為了保證溫度的準(zhǔn)確測(cè)量，將溫度傳感器模塊緊貼光電隔離驅(qū)動(dòng)模塊的mos管放置。數(shù)據(jù)隔離模塊1與can數(shù)據(jù)收發(fā)模塊2連接，can數(shù)據(jù)收發(fā)模塊2與控制器模塊3連接，控制器模塊3同時(shí)與光電隔離驅(qū)動(dòng)模塊4、溫度檢測(cè)模塊5連接。

其中，數(shù)據(jù)隔離模塊1由數(shù)字隔離芯片構(gòu)成隔離電路，完成推進(jìn)器控制裝置與水下機(jī)器人主控制器之間信號(hào)隔離。can數(shù)據(jù)收發(fā)模塊2由can數(shù)據(jù)收發(fā)器芯片構(gòu)成數(shù)據(jù)收發(fā)電路，完成can數(shù)據(jù)包的接收與發(fā)送?？刂破髂K3由控制器芯片構(gòu)成最小系統(tǒng)電路，完成can數(shù)據(jù)包的解析和打包處理、霍爾傳感器信號(hào)的解算、控制算法的實(shí)現(xiàn)、pwm波的產(chǎn)生以及實(shí)時(shí)溫度的計(jì)算。光電隔離驅(qū)動(dòng)模塊4由光電耦合器、spwm驅(qū)動(dòng)電路和mos管構(gòu)成隔離驅(qū)動(dòng)電路，完成控制裝置與推進(jìn)器之間的完全電氣隔離以及推進(jìn)器的驅(qū)動(dòng)，抗干擾能力強(qiáng)，避免推進(jìn)產(chǎn)生的干擾信號(hào)傳輸?shù)娇刂蒲b置。

水下機(jī)器人主控制器發(fā)送給推進(jìn)器的設(shè)定轉(zhuǎn)速經(jīng)過(guò)數(shù)據(jù)隔離模塊1的隔離作用后傳輸?shù)絚an數(shù)據(jù)收發(fā)模塊2，can數(shù)據(jù)收發(fā)模塊2接收后傳輸給控制器模塊3，控制器模塊3根據(jù)轉(zhuǎn)速設(shè)定值輸出相應(yīng)占空比的pwm波，傳輸?shù)焦怆姼綦x驅(qū)動(dòng)模塊，驅(qū)動(dòng)推進(jìn)器動(dòng)作；推進(jìn)器的霍爾傳感器信號(hào)經(jīng)過(guò)光電隔離驅(qū)動(dòng)模塊4的隔離后傳輸?shù)娇刂破髂K3，由控制器模塊計(jì)算出推進(jìn)器的實(shí)時(shí)轉(zhuǎn)速；控制器模塊3以設(shè)定轉(zhuǎn)速和實(shí)時(shí)轉(zhuǎn)速為依據(jù)，運(yùn)用pid算法計(jì)算出相應(yīng)占空比的pwm波，調(diào)整推進(jìn)器轉(zhuǎn)速；所述溫度檢測(cè)模塊5緊貼光電隔離驅(qū)動(dòng)模塊的mos管，實(shí)時(shí)檢測(cè)其溫度，溫度高于上限值時(shí)，控制器模塊3啟動(dòng)推進(jìn)器保護(hù)動(dòng)作；mos管實(shí)時(shí)溫度與推進(jìn)器實(shí)時(shí)轉(zhuǎn)速通過(guò)can數(shù)據(jù)收發(fā)模塊2上傳到水下機(jī)器人的主控制器中。

上述基于自適應(yīng)pid控制的水下機(jī)器人推進(jìn)器控制裝置的控制方法，具體步驟如下：

步驟一：水下機(jī)器人主控制器通過(guò)can通信的方式，將推進(jìn)器的設(shè)定轉(zhuǎn)速nd以及光電隔離驅(qū)動(dòng)模塊4的mos管的上限溫度td放在can數(shù)據(jù)包中，can數(shù)據(jù)收發(fā)模塊2接收后傳輸給控制器模塊3，控制器模塊3接收并解析指定的can數(shù)據(jù)包，經(jīng)過(guò)can數(shù)據(jù)包處理31，將推nd、td保存到固定空間；

步驟二：控制器模塊3采集推進(jìn)器的霍爾傳感器信號(hào)，經(jīng)過(guò)霍爾數(shù)據(jù)處理35，實(shí)時(shí)計(jì)算推進(jìn)器的實(shí)時(shí)轉(zhuǎn)速n，將推進(jìn)器的實(shí)時(shí)轉(zhuǎn)速n保存到固定空間；

步驟三：控制器模3采集溫度檢測(cè)模塊5的溫度信號(hào)，經(jīng)過(guò)溫度數(shù)據(jù)處理34，實(shí)時(shí)計(jì)算光電隔離驅(qū)動(dòng)模塊4的mos管t，將mos管的實(shí)時(shí)溫度t保存到固定空間；

步驟四：控制器模3根據(jù)設(shè)定轉(zhuǎn)速nd和實(shí)時(shí)轉(zhuǎn)速n，運(yùn)行pid算法32，輸出相應(yīng)占空比的pwm波33，pwm波傳輸?shù)焦怆姼綦x驅(qū)動(dòng)模塊4后，驅(qū)動(dòng)推進(jìn)器動(dòng)作，當(dāng)推進(jìn)器轉(zhuǎn)速超調(diào)過(guò)大或響應(yīng)時(shí)間過(guò)長(zhǎng)時(shí)，則控制模塊進(jìn)行pid參數(shù)自適應(yīng)整定36，具體過(guò)程如下：

此處僅對(duì)pid參數(shù)自適應(yīng)整定部分進(jìn)行詳述，常規(guī)pid算法部分不再累述。

n為推進(jìn)器的實(shí)際轉(zhuǎn)速，nd為推進(jìn)器的理想轉(zhuǎn)速，則系統(tǒng)的追蹤誤差可表達(dá)為式：

e＝nd-n

由于只有轉(zhuǎn)速信息，則濾波誤差函數(shù)為式：

其中，a為一正定矩陣，a＝a^t>0，則：

由于模型的不確定性和外部干擾未知，則系統(tǒng)不確定項(xiàng)如式：

pid控制算法如式：

其中，kp，ki，kd分別為比例、微分、積分增益。

上式亦可表示成如式：

τpid(e|θk)＝θk^tξ(e)

其中，θk^t＝[kp,ki,kd]，是可調(diào)參數(shù)向量是回歸向量。θk在控制過(guò)程中，不斷通過(guò)自適應(yīng)機(jī)制自動(dòng)調(diào)整。

pid控制器τpid(e|θk)可以不斷逼近系統(tǒng)的不確定項(xiàng)f，因此，存在一最優(yōu)參數(shù)向量θ^*，使得近似誤差有界，如式：

f＝τpid(e|θ^*)+δ(e)≤δ0

其中，θ^*滿(mǎn)足式：

其中，ωθ、ωe分別為θ、e的期望約束集。

由于f有界，則θ有界，取||θ^*||f≤θmax，即θmax有界，||θ^*||f為frobenius范數(shù)。此處僅需知道存在最優(yōu)參數(shù)向量θ^*，不需要知道該范數(shù)公式的確定值，它對(duì)控制器的設(shè)計(jì)不是必需的。

令滿(mǎn)足下式：

其中，分別為f、θ^*的估計(jì)值。

則控制器設(shè)計(jì)如下式所示：

其中，ν為濾波器的輸出，即控制模塊輸出的pwm波對(duì)應(yīng)的占空比，γ為一正定矩陣。k＝diag[k1,…,kn]，β＝diag[β1,…,βn]，tanhν＝diag[tanhν1,…,tanhνn]^t，tanhν∈rⁿ，k，β∈r^n×n，k1,…,kn，β1,…,βn，α皆為正常數(shù)。

此時(shí)，將逼近得到的pid參數(shù)值保存起來(lái)，運(yùn)用到pid算法32中，當(dāng)再次出現(xiàn)轉(zhuǎn)速超調(diào)過(guò)大或響應(yīng)時(shí)間過(guò)長(zhǎng)時(shí)，重復(fù)上述pid參數(shù)自適應(yīng)整定過(guò)程。超調(diào)范圍和響應(yīng)時(shí)間可根據(jù)需求而定。

步驟五：溫度檢測(cè)模塊5實(shí)時(shí)檢測(cè)光電隔離驅(qū)動(dòng)模塊4的mos管溫度t，當(dāng)mos管溫度t高于上限值td時(shí)，即t≥td時(shí)，則說(shuō)明推進(jìn)器功率過(guò)高，發(fā)生故障，此時(shí)，控制器模塊3停止pwm波輸出33，控制模塊3的pwm波輸出口持續(xù)低電平。當(dāng)溫度回到正常范圍內(nèi)的時(shí)候，控制器模塊正常工作；

步驟六：控制器模塊3將mos管的實(shí)時(shí)溫度t以及推進(jìn)器的實(shí)時(shí)轉(zhuǎn)速n放置到can數(shù)據(jù)包中，由can數(shù)據(jù)收發(fā)模塊2將數(shù)據(jù)包發(fā)送到水下機(jī)器人主控制器。