基于自適應PID控制的水下機器人推進器控制裝置及其控制方法與流程

本發明屬于水下機器人推進器技術，具體涉及一種基于自適應pid控制的水下機器人推進器控制裝置及其控制方法。

背景技術：

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

推進器是水下機器人的主要組成部分，將其它形式的能源轉化成機械能從而推動水下機器人運動，是水下機器人的動力源泉。水下機器人在執行作業需要完成指定的姿態和動作時，推進器發揮著關鍵性的作用。良好的水下推進器控制系統是水下機器人姿態、航程、航速等相關性能的有力保證。因此高機動性、高可靠性、高準確性的水下機器人推進器控制系統對我們開發、利用海洋資源顯得十分重要。無刷直流電機由于其高效率、高可靠性以及維護成本低等優點，在中小型水下機器人中深受青睞。

目前針對于水下機器人推進器的控制裝置不多，傳統的水下機器人推進器的控制方式為，直接將推進器連接到水下機器人主控制器的i/o口，這樣的結構大量占用水下機器人主控制器資源，且接線復雜，同時，推進器自身無法形成轉速閉環，控制響應慢、精度低，而且，無法獲得推進器的轉速、溫度等信息。

目前的一些水下推進器要么計算復雜，對處理器要求比較高，不便編程與實現，運算速度慢，無法取得理想控制效果，不利于應用于實際；要么需借助工程經驗數據為依據，建立專家數據庫，應用范圍窄，系統算法龐大，不具有普遍應用價值，且未給出控制系統的具體實現方式。

技術實現要素：

發明目的：本發明的目的在于解決現有技術中存在的不足，提供一種基于自適應pid控制的水下機器人推進器控制裝置及其控制方法。

技術方案：本發明的一種基于自適應pid控制的水下機器人推進器控制裝置，包括數據隔離模塊、can數據收發模塊、光電隔離驅動模塊、控制器模塊以及溫度檢測模塊；水下機器人主控制器發送給推進器的設定轉速經過數據隔離模塊的隔離作用后傳輸到can數據收發模塊，can數據收發模塊接收數據后傳輸給控制器模塊，控制器模塊根據轉速設定值輸出相應占空比的pwm波，并傳輸到光電隔離驅動模塊，驅動水下機器人推進器動作；水下機器人推進器的霍爾傳感器信號經過光電隔離驅動模塊的隔離后傳輸到控制器模塊，由控制器模塊計算出推進器的實時轉速；控制器模塊以設定轉速和實時轉速為依據，運用pid算法計算出相應占空比的pwm波，調整水下機器人推進器的轉速；所述溫度檢測模塊緊貼光電隔離驅動模塊的mos管，實時檢測mos管的溫度，mos管實時溫度高于上限值時，控制器模塊啟動推進器保護動作；mos管的實時溫度和推進器的實時轉速通過can數據收發模塊上傳到水下機器人主控制器。

進一步的，所述數據隔離模塊由數字隔離芯片構成隔離電路，用以隔離推進器與主控制器之間的can數據信號。

進一步的，所述can數據收發模塊由can數據收發器芯片構成數據收發電路，完成can數據包的接收與發送，此處的can數據包包括實時溫度和轉速數據。

進一步的，所述光電隔離驅動模塊由光電耦合器、spwm驅動電路和mos管構成隔離驅動電路，完成控制器模塊與推進器之間的完全電氣隔離以及推進器的驅動，抗干擾能力強，避免推進產生的干擾信號傳輸到控制裝置。

進一步的，所述控制器模塊由控制器芯片構成最小系統電路，完成can數據包的解析和打包處理、霍爾傳感器信號的解算、控制算法的實現、pwm波的產生以及實時溫度的計算。

本發明還公開了一種基于自適應pid控制的水下機器人推進器控制裝置的控制方法，依次包括接收設定轉速、實際轉速的計算、控制算法的實現和pwm波的輸出，具體包括以下步驟：

步驟一：水下機器人主控制器通過can數據收發模塊將推進器的設定轉速以及溫度上限設定值放在can數據包中，控制器模塊接收并解析指定的can數據包，將推進器設定轉速以及溫度上限設定值保存到固定空間；

步驟二：控制器模塊采集推進器的霍爾傳感器信號，實時計算推進器的實時轉速，將推進器的實時轉速保存到固定空間；

步驟三：控制器模塊采集溫度檢測模塊的溫度信號，實時計算光電隔離驅動模塊的mos管溫度，將mos管的實時溫度保存到固定空間；

步驟四：控制器模塊根據設定轉速和實時轉速，運行pid算法，輸出相應占空比的pwm波，pwm波傳輸到光電隔離驅動模塊后，驅動推進器動作，當推進器轉速超調過大或響應時間過長時，則控制模塊啟動自適應pid參數整定程序，運用參數逼近原則，對系統的pid參數進行重新調整，其中，超調范圍和響應時間根據需求而定；

步驟五：溫度檢測模塊實時檢測光電隔離驅動模塊的mos管度，當mos管溫度高于上限值時，則說明推進器功率過高，發生故障，此時，控制器模塊停止pwm波的輸出，并將故障信息通過can通信發送到水下機器人主控制器，其余模塊正常工作，當溫度回到正常范圍時，控制器模塊正常輸出pwm波，故障信號解除；

步驟六：控制器模塊將mos管的實時溫度以及推進器的實時轉速放置到can數據包中，由can數據收發模塊將數據包發送到水下機器人主控制器。

有益效果：與現有技術相比，本發明具有以下優點：

(1)本發明采用自適應pid控制算法，既有常規pid控制算法的結構簡單、工作穩定、魯棒性較強等優點，又具有根據誤差大小自適應調整控制器參數、能夠適應被控過程參數變化等優點；

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

(3)本發明采用光電隔離驅動模塊，實現了主控制器與推進器之間的完全電氣隔離，避免推進器的干擾信號傳遞到控制器模塊，提高了控制器模塊的穩定性和精度；

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

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

附圖說明

圖1為本發明的數據流向圖；

圖2為本發明的結構示意圖。

具體實施方式

下面對本發明技術方案進行詳細說明，但是本發明的保護范圍不局限于所述實施例。

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

其中，數據隔離模塊1由數字隔離芯片構成隔離電路，完成推進器控制裝置與水下機器人主控制器之間信號隔離。can數據收發模塊2由can數據收發器芯片構成數據收發電路，完成can數據包的接收與發送。控制器模塊3由控制器芯片構成最小系統電路，完成can數據包的解析和打包處理、霍爾傳感器信號的解算、控制算法的實現、pwm波的產生以及實時溫度的計算。光電隔離驅動模塊4由光電耦合器、spwm驅動電路和mos管構成隔離驅動電路，完成控制裝置與推進器之間的完全電氣隔離以及推進器的驅動，抗干擾能力強，避免推進產生的干擾信號傳輸到控制裝置。

水下機器人主控制器發送給推進器的設定轉速經過數據隔離模塊1的隔離作用后傳輸到can數據收發模塊2，can數據收發模塊2接收后傳輸給控制器模塊3，控制器模塊3根據轉速設定值輸出相應占空比的pwm波，傳輸到光電隔離驅動模塊，驅動推進器動作；推進器的霍爾傳感器信號經過光電隔離驅動模塊4的隔離后傳輸到控制器模塊3，由控制器模塊計算出推進器的實時轉速；控制器模塊3以設定轉速和實時轉速為依據，運用pid算法計算出相應占空比的pwm波，調整推進器轉速；所述溫度檢測模塊5緊貼光電隔離驅動模塊的mos管，實時檢測其溫度，溫度高于上限值時，控制器模塊3啟動推進器保護動作；mos管實時溫度與推進器實時轉速通過can數據收發模塊2上傳到水下機器人的主控制器中。

上述基于自適應pid控制的水下機器人推進器控制裝置的控制方法，具體步驟如下：

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

步驟二：控制器模塊3采集推進器的霍爾傳感器信號，經過霍爾數據處理35，實時計算推進器的實時轉速n，將推進器的實時轉速n保存到固定空間；

步驟三：控制器模3采集溫度檢測模塊5的溫度信號，經過溫度數據處理34，實時計算光電隔離驅動模塊4的mos管t，將mos管的實時溫度t保存到固定空間；

步驟四：控制器模3根據設定轉速nd和實時轉速n，運行pid算法32，輸出相應占空比的pwm波33，pwm波傳輸到光電隔離驅動模塊4后，驅動推進器動作，當推進器轉速超調過大或響應時間過長時，則控制模塊進行pid參數自適應整定36，具體過程如下：

此處僅對pid參數自適應整定部分進行詳述，常規pid算法部分不再累述。

n為推進器的實際轉速，nd為推進器的理想轉速，則系統的追蹤誤差可表達為式：

e＝nd-n

由于只有轉速信息，則濾波誤差函數為式：

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

由于模型的不確定性和外部干擾未知，則系統不確定項如式：

pid控制算法如式：

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

上式亦可表示成如式：

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

其中，θk^t＝[kp,ki,kd]，是可調參數向量是回歸向量。θk在控制過程中，不斷通過自適應機制自動調整。

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

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

其中，θ^*滿足式：

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

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

令滿足下式：

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

則控制器設計如下式所示：

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

此時，將逼近得到的pid參數值保存起來，運用到pid算法32中，當再次出現轉速超調過大或響應時間過長時，重復上述pid參數自適應整定過程。超調范圍和響應時間可根據需求而定。

步驟五：溫度檢測模塊5實時檢測光電隔離驅動模塊4的mos管溫度t，當mos管溫度t高于上限值td時，即t≥td時，則說明推進器功率過高，發生故障，此時，控制器模塊3停止pwm波輸出33，控制模塊3的pwm波輸出口持續低電平。當溫度回到正常范圍內的時候，控制器模塊正常工作；

步驟六：控制器模塊3將mos管的實時溫度t以及推進器的實時轉速n放置到can數據包中，由can數據收發模塊2將數據包發送到水下機器人主控制器。