[發明專利]一種ROV推力分配與基于強化學習的運動控制方法

有效

• 申請號: 202010820394.0
• 申請日: 2020-08-14
• 公開（公告）號: CN111966118B
• 公開（公告）日: 2021-07-20
• 發明（設計）人: 孫玉山;吳新雨;張國成;李岳明;王元慶;祁彧;柴璞鑫
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G05D1/06
• 分類號: G05D1/06
• 代理公司: 哈爾濱市陽光惠遠知識產權代理有限公司 23211
• 代理人: 劉景祥

1.一種ROV推力分配與基于強化學習的運動控制方法，其特征在于，所述方法包括以下步驟：

步驟一、進行ROV動力學建模，確定ROV推進器空間布置及推進系統模型，提出ROV推力分配方法；

步驟二、在所述ROV推力分配方法的基礎上，加入DDPG控制器；

步驟三、在所述DDPG控制器的基礎上，加入抗擾控制器，

在步驟一中，具體的，建立有利于描述水下機器人運動狀態的坐標系，慣性坐標系以空間任意一點為原點，Eζ軸正向指向地心，Eξ軸指向北向、Eη指向東向，隨體坐標系O-xyz固連于ROV艇體上，原點取在ROV的對稱軸中點或在重心上，取縱軸Ox指向艇首，橫軸Oy與Ox軸正交于同一平面并以右舷為正方向，垂軸Oz指向機器人的腹部，機器人的位置和方向由慣性坐標系表示，速度和角速度在隨體坐標系中表示，

在慣性坐標系中，ROV位置和姿態表示為：

其中：R為ROV的位姿項，r為ROV的位置項，Λ為ROV的姿態項，ξ為大地坐標系縱向，η為大地坐標系橫向，ζ為大地坐標系垂向，為橫傾角，θ為縱傾角，ψ為艏向角，

在隨體坐標系中，水下機器人的動力學模型輸入為所受環境和控制力的力和力矩，輸出為水下機器人的線性加速度和姿態角加速度，在隨體坐標系中，載體的速度和加速度為：

V＝[U^T,Ω^T]^T，U＝[u,v,w]^T，Ω＝[p,q,r]^T (2)

其中：V為ROV的速度項，U為ROV的線性速度項，Ω為ROV的角速度項，u為縱向速度，v為橫向速度，w為垂向速度，p為橫滾角速度，q為縱傾角速度，r為轉艏角速度，

水下機器人在慣性系下的位置項與其隨體的速度量的動態關系可以由以下轉換方程來描述：

式中J₁，J₂是坐標轉換矩陣：

通過有限元計算或船模實驗可以對動力學方程中涉及的水動力參數進行測算；

推進系統是由多個處于不同方位的獨立推進器所組成，對單個推進器進行數學建模，

對于ROV上所使用的螺旋槳，其敞水推力的計算方法如下式所示：

T＝K_Tρn²D⁴ (6)

式中：K_T為螺旋槳推力系數，ρ為海水的密度kg/m³，D為螺旋槳直徑m，n為螺旋槳的轉速r/s，

為了獲得相應的推力，必須驅動螺旋槳轉動推出水體，螺旋槳的轉矩的計算公式為：

Q＝K_qρn²D⁵ (7)

式中：Q為螺旋槳扭矩Nm，K_q為螺旋槳轉矩系數，

水下機器人配置有6臺矢量布置的螺旋槳推進器，在水平面有四個斜向布置的水平面推進器，垂直面沿艇體縱軸關于質心對稱布置有兩個垂向推進器，

在水平方向上，4臺推進器平行于水平面O-xy布置，按照左上、右上、左下、右下依次編號為：第一推進器、第二推進器、第三推進器和第四推進器，且與隨體坐標系縱軸O-x的夾角為45°，水平方向上的4臺推進器安裝位置關于縱軸左右對稱，因此水平面的推進器為冗余布置，對于控制器計算的控制量通過各推進器的組合實現，第一推進器的螺旋漿在隨體坐標系xOy中的位置為：(237mm,-305mm)，第二推進器與第一推進器關于O-xz面對稱；第三推進器的坐標為(-300mm,-195mm)，第四推進器與第三推進器關于O-xz面對稱，

垂直面有兩個推進器，其布置在O-xz平面上，依照前后方向編號為第五推進器和第六推進器，其中第五推進器在xOz坐標系中位置為(284mm,113mm)，第六推進器與第五推進器其關于O-yz面對稱布置，

遙控水下機器人具有六個推進器，其中要控制的量為縱向橫移垂向方向的運動和轉艏，其中垂直方向的控制器有兩個均勻布置，推力分配時只需將控制力平均分配即可，水平方向有四個推進器，分布在四個角，與隨體坐標系成45°排布，在分配水平方向的力時，推進器數目大于目標力和力矩的數量，推進器冗余布置，因此在滿足推力分配時，可以有無數種分配方式，這要求在進行推力分配的同時，應當兼顧各推進器輸出幅值的偏差以及推力分配系統的能耗，

建立水下機器人推力分配的數學模型：

τ＝B(α)T (8)

式(8)為推力分配問題的等式約束，B表示推力分配的空間位置約束，τ＝{τ_x,τ_y,τ_r}為控制器輸出的三個自由度上的力和力矩，T＝{T₁ T₂ T₃ T₄}為推進器的推力，α＝{α₁ α₂ α₃α₄}為推進器轉角，

其中l_xi、l_yi分別為第i個推進器在x、y方向上距離機器人重心的距離；

遺傳算法借鑒生物進化理論的機理，以保留更適應環境的生物體為篩選原則的一種隨機算法，先使用隨機選取的一組解作為初始種群，該種群中的每個解決方案被稱為個體或染色體，種群大小是其中個體數目，遺傳算法對變量進行編碼，遺傳算法中每個染色體的適應值，具有評估新產生的種群與上一代種群關系的能力，為了得到每個染色體的適應值，計算同一染色體的優化函數的值，所述優化函數叫做目標函數，通過遺傳算法中主要的選擇、交叉和變異這種處理使種群個體進行更新，得到下一次迭代的解群，新產生的種群叫子代，上一代種群叫父代，遺傳算法的這個迭代過程直到滿足停止條件才會終止，在最后一代中具有最佳適應值的染色體被稱為最優解，

根據推進器系統模型，建立遺傳算法的優化目標方程：

其中P為推進系統的能耗方程，k_i為第i個推進器占總能耗的占比權值，在本目標機器人中，總功率為各個推進器功率之和，根據電機功率公式，其中n為轉速，Q為扭矩：

P＝nQ (10)

將上式與螺旋槳的敞水推力的計算公式(11)、螺旋槳的轉矩公式(12)結合：

T＝K_Tρn²D⁴ (6)

式中，K_T為螺旋槳推力系數，ρ為海水的密度kg/m3，D為螺旋槳直徑m，n為螺旋槳的轉速r/s，

為了獲得相應的推力，必須驅動螺旋槳轉動推出水體，其轉矩的計算公式為：

Q＝K_qρn²D⁵ (7)

式中，Q為螺旋槳扭矩Nm，K_q為螺旋槳轉矩系數，

計算每個推進器的推力和功率量化公式：

單個推進器功率與推進器推力的3/2次方成正比，

將控制分配任務抽象成最優化問題進行描述：

開始遺傳算法計算，確定遺傳算法中優化變量為四項，即推進器的推力大小u，生成初始種群，設計種群中個體數目為50，進行500次迭代，自變量數目4，交叉概率0.8，變異概率0.15，公差為1e-8，對于個體的變量上下限為[-20，20]，初始種群的個體隨機產生，隨機范圍即為變量上下限，

在優化目標函數fun的基礎上設計遺傳算法的適度函數V并計算：

上式中Bu-τ表示當前個體關于等式約束的偏差值，W為懲罰矩陣，當個體越接近等式約束的解時，W的適應度也越大，同樣當個體越滿足優化條件時，它的適應度越大，這樣便可以求出種群中更加滿足約束條件且適應優化方程的個體，

根據適應度選擇當前種群中個體，根據適應程度對最佳個體進行分類，記錄最佳個體，并以如下公式選擇計算群體中個體的選擇概率：

式中P_i為第i個個體的選擇概率，f_i為其適應度，擁有較大適應度的解有更大的概率被選擇繼承到下一次迭代，在獲取下一次迭代種群時對選中個體進行交叉變異提高在優值周圍的探索，

對遺傳算法的終止條件進行判斷，如果當前最佳個體的適應度與上代最佳個體適應度之差小于容忍度，則認為達到最優條件，遺傳算法結束；若不滿足，判斷是否達到繁殖代數，若達到則停止計算，

對有交叉權的個體進行染色體交叉，采用浮點數交叉法，即交換父母中某一段浮點位，對有交叉權但不滿足交叉概率的保留父代，對于產生的子代，依照編譯概率對其某浮點位進行隨機數變異，在產生的子代中，如果它的變量值超過了變量上下限，則將其值修改為上下限的值，同時為了保證種群的優選性，將上代最優個體進行保留，將交叉、變異后的產生的新種群回到計算適度函數V的步驟，進行下一輪的計算，

結束計算后，獲得的最優個體u即為本次推力分配問題中能耗最小的優化推力；

在步驟二中，針對的水下機器人在水平面操縱上具有縱軸和艏向上的執行機構，通過調整前后方向的推力和轉向使機器人趨近目標點，控制原理為獲得目標值和實際值的差值，將偏差作為強化學習的狀態值，使用策略網絡選擇所需執行的動作，其中動作空間為歸一化以后的x方向推力和繞z軸旋轉的力矩，

在控制器設計時，建立Actor網絡作為策略網絡μ，某一時刻的慣性坐標系位置和隨體坐標系速度R、V作為狀態空間的輸入通過神經網絡輸出控制指令，其包括縱向、橫向和轉艏方向上歸一化的力和力矩，動作空間為：a＝{τ_x,τ_y,τ_r}將選擇的動作交由推力分配系統生成實際的推力分配和推進器轉速控制方案，通過調節機器人縱向和橫向的速度和艏向角使機器人向目標點靠攏，

控制器狀態選擇隨體坐標系下的誤差s＝{e_xr,e_yr,e_ψ,u,v,r}，e_xr為隨體坐標系下縱向誤差：e_xr＝x_Target-x_r，e_yr為橫向誤差，e_ψ為大地坐標系下目標艏向和當前艏向的差值，目標艏向：ψ_Target＝arctan(dy/dx)，因此艏向誤差可表示為：

e_ψ＝ψ_Target-ψ (17)

以點鎮定為目標，設置初始目標艏向角為0，偏差為目標點在機器人艇體坐標系縱向距離，將目標速度和角速度設置為0，則設置強化學習的獎勵函數為：

當智能體越靠近智能體，且智能體的速度越慢時，獎勵值就越大，以累積折扣獎勵值最大為目標，通過DDPG方法使策略網絡選擇最優策略，當某一時刻輸入智能體狀態時，它能輸出最佳動作，進而完成機器人點鎮定的控制問題，

當傳感器獲得水下機器人狀態信息s后，根據狀態s，策略網絡會選擇一個動作控制水下機器人運動，在下一采樣時刻獲得狀態信息s’，同時根據獎勵函數計算當前狀態的獎勵值r，將s,a,r,s'作為一個采樣點儲存在記憶庫中，在參數訓練的過程中，從記憶庫小批量采樣，使用策略網絡進行決策，并用目標評價計算采樣動作的目標Q值，通過現實評價估計Q值與目標Q值的均方誤差調整評價網絡參數更新，同時使用目標Q函數計算更新策略網絡，采用軟更新的方式，異步的調整目標神經網絡的參數，使目標神經網絡向接近現實神經網絡的方向緩慢更新，在訓練過程中使用策略網絡以狀態信息s為輸入，輸出3個自由度上的力和力矩a，并加入高斯噪聲以實現對環境的探索，將策略網絡選擇的動作a發送給推力分配模塊，進而驅動ROV推進器運轉，實現動作控制；

在步驟三中，具體的，抗擾控制器以狀態觀測器為基礎，依據觀測器估計的狀態和測量狀態誤差為輸入量調整控制器，水下機器人在控制力的作用下由狀態s_t轉移至s_t+1，而由于外部擾動和系統不確定性的存在使狀態轉移至s'_t+1，將干擾后的狀態量與估計狀態量的誤差作為輸入，加入控制力作為線性補償以改善控制器的抗擾性能，

以隨體坐標系下位置量為被控狀態量s＝{x,y,ψ}，根據2個采樣時刻狀態量{s_k-1,s_k}和控制量{τ_k-1,τ_k}使用SVR方法狀態估計水下機器人狀態s_k+1，在無干擾情況下采集運動狀態數據作為數據集訓練SVR收斂，并以此來估計理想狀態下被控狀態，則狀態誤差為測量的狀態與估計狀態的差值：

根據反饋控制的思想，使用非線性反饋控制器對系統誤差進行補償，對于非線性系統加入非線性補償控制量：

其中，x為非線性系統狀態量，w為時域干擾項，τ為控制量，g(e)為以估計誤差為輸入的補償控制量，通過調整控制量來調整期望狀態與系統輸出的誤差，對于一階誤差輸入，取g(·)為fal函數[110]，

其中，β為比例參數，其反映了補償控制力在總控制力中的比重，δ為反饋函數線性段的范圍，當|e_s|＜δ時，反饋函數處于線性區間，避免了誤差在0點附近時的高頻振蕩，減小穩態誤差，取α在0～1之間，反映了系統的階次，在|e_s|＞δ的區間中，當誤差增幅大時，減小系統增益防止發散，δ取值較小但其值過小會導致線性段縮短，使系統出現高頻抖震，取值過大會導致線性段延長，使非線性段克服系統穩態誤差的特性降低。