[發明專利]基于積分強化學習的多消防巡檢協作機器人系統

有效

• 申請號: 202110419574.2
• 申請日: 2021-04-19
• 公開（公告）號: CN113134187B
• 公開（公告）日: 2022-04-29
• 發明（設計）人: 陳剛;劉智
• 申請（專利權）人: 重慶大學
• 主分類號: A62C27/00
• 分類號: A62C27/00;A62C37/00;A62C37/50
• 代理公司: 北京同恒源知識產權代理有限公司 11275
• 代理人: 趙榮之

1.基于積分強化學習的多消防巡檢協作機器人系統，其特征在于：包括依次連接的硬件層、交互層、感知層和控制層；

所述硬件層采用DSP作為控制器，將里程計和陀螺儀采集到的數據送入DSP內部進行處理，實時計算出機器人在巡檢地圖中的位置；通過上位機向DSP發送速度指令，DSP將獲取到速度信息編碼后以控制伺服電機的運轉；消防巡檢機器人采用的是履帶式驅動；當機械臂需要動作時，由上位機中的ros系統通過在moveit！平臺對機械臂將要移動到的目標點進行運動軌跡規劃，將規劃好的運動軌跡離散化后發送到DSP中，DSP獲得各個軸的角速度、加速度后控制機械臂的伺服電機運動以到達目標點；

所述感知層包括用于建圖的激光雷達、避障的紅外線傳感器、檢測火焰的火焰探測器、溫度傳感器和realsenseD435i深度攝像頭、里程計和陀螺儀；

所述控制層為：

設整個消防巡檢區域下共有N個機器人協同巡檢，N個機器人從各自的初始位置(x_i0，y_i0)要到達各自的目的地(x_iD，y_iD)，i∈{1，2，…，N}，設第i個消防巡檢機器人在t時刻的位置L_i(t)＝[L_ix(t)，L_iy(t)]^T，速度V_i(t)＝[V_ix(t)，V_iy(t)]^T，控制器輸入U_i(t)＝[u_ix(t)，u_iy(t)]^T，控制輸入和未知的環境擾動W_i(t)＝[W_ix(t)，W_iy(t)]^T，為避免執行器飽和，對輸入進行約束，要求|U(t)|≤λ，其中λ為正常數；設兩個巡檢機器人之間的距離r_ij(t)＝||L_i(t)-L_j(t)||，為避免兩個巡檢機器人發生碰撞需要設置一個安全距離r_s，要求在巡檢過程中的任意時刻都要滿足r_ij(t)≥r_s，設當N個機器人到達巡檢目的地后保證r_ij(t)＞＞r_s，此時i≠j；

則考慮第i個消防巡檢機器人的二階線性動力學模型為：

其中系統矩陣為A，輸入矩陣為B，輸出矩陣為C，干擾矩陣為D，為機器人在t時刻的狀態，為輸入，y_i(t)為系統唯一輸出；

將全局動力學模型寫為：

其中為Kronecker乘積，X(t)＝[x₁(t)，x₂(t)，...，x_n(t)]^T，Y(t)＝[y₁(t)，y₂(t)，...，y_n(t)]^T，I_N為N階單位矩陣，且設L(t)＝[L_1t，L_2t，...，L_Nt]^T，L_D＝[L_1D，L_2D，...，L_ND]^T，U₀＝[U₁，U₂，...，U_N]^T分別為N個機器人的在t時刻的位置、目標點位置和控制輸入；

為使N個消防巡檢機器人在未知的擾動下實現在連續時間、連續狀態和控制輸入空間中的最小時間和能量的最優控制，并且在整個過程中要避免碰撞，考慮以下成本函數：

其中ζ0，用于表示巡檢過程中時間的比重，R為正定矩陣；為求解機器人最小到達時間T未知的路徑規劃問題，引入雙曲正切函數將成本函數改寫成無窮積分的形式以便求解，另外為避免執行器飽和，還想要對輸入進行約束，將U(t)^TRU(t)線性二次型改寫成非二次型性能函數φ(U(t)用于逼近最小能量成本并且捕獲輸入約束，且為避免兩個機器人之間發生碰撞引入了人工勢場函數，將成本函數近似改寫為：

其中ζ為正常數，tanh為雙曲正切函數，該函數為單調遞增的奇函數且連續可微，成本函數是IRL可解的形式；將ζ改寫為ζtanh(L(t)-L_D)^T(L(t)-L_D)，當機器人當前位置L(t)距離目標點L_D時ζtanh(L(t)-L_D)^T(L(t)-L_D)近似為ζ，到達目標點時ζtanh(L(t)-L_D)^T(L(t)-L_D)＝0，將未知時間的T積分轉化為與到達時間T無關的無窮積分，以實現對值函數的最優求解；

將U(t)^TRU(t)線性二次型改寫成非二次型性能函數φ(U(t)用于逼近最小能量成本并且捕獲輸入約束：

其中輸入約束為|U(t)|≤λ，λ和σ均為正常數，R＝diag(r₁，r₂...r_m)＞0；

為避免任何一對巡檢機器人發生碰撞，加入人工勢場函數f_R(r_ij(t))使得兩個機器人之間發出排斥勢場使得二者相互避開，為使得加入勢場函數之后的V(x(t)，U(t))有界，設計權重矩陣Λ_R(t)，用于抵消非零尾部；將排斥函數f_R(r_ij(t))定義高斯函數的形式，且該高斯函數總是大于0：

其中s越大則排斥函數的陡度就越大，σ越大排斥范圍也越大；為捕捉排斥距離r_ij(t)，求解排斥函數中的s和σ，設有：

f_R(r_s)＝K₀；f_R(r_s+Δ)＝K₁ (4-7)

其中0＜K₁＜K₀＜1；Δ為正增量，代入得：

通過權重矩陣Λ_R(t)＝[Λ₁₂(t)，Λ₁₃(t)，...，Λ_N-1，N(t)]^T來使得引入人工勢場函數后的值函數是有界的，且權重矩陣取決于與目標點的距離；

Λ_R(t)＝βtanh(||L_i(t)-L_iD||²+||L_j(t)-L_jD||²) (4-9)

當機器人遠離目標點時Λ_R(t)＝β，當機器人到達目標點時Λ_R(t)＝0，β為碰撞系數，β的大小由巡檢過程中避免碰撞的重要性決定；

下面利用(4-4)中的成本函數求解最優控制輸入，(4-4)式兩邊對t求導，貝爾曼方程寫為：

令F_ζ(t)＝ζtanh(L(t)-L_D)^T(L(t)-L_D)，定義最優值函數為：

根據(4-10)式定義HJB方程為：

其中

在穩定性條件下有(4-12)式兩邊同時對U求導得：

移項后得最優控制輸入U^*(t)為：

將(4-14)代入到(4-5)中得：

其中l為全為一的列向量，將(4-14)代入(4-15)中得：

其中將(4-16)代入(4-12)中得：

利用基于積分強化學習的策略迭代算法求解HJB方程，積分強化學習使用(t，t+T)內的信號用于學習，不需要知道系統具體的動力學模型；

首先將值函數改寫成積分差值的形式，得到如下的貝爾曼方程：

為能夠在線實時地求解(4-18)，引入actor-critic神經網絡算法來實現策略迭代過程中的實時更新；首先通過critic神經網絡對值函數V(X)進行近似逼近，因為

而其中第一項為易求得的二次型，只對第二項進行逼近，并設用神經網絡對V₀(X)進行逼近得：

其中w_c為critic神經網絡的權重，ψ_c(X)為基函數，ε_c(X)為逼近誤差；

將(4-20)兩邊對X求微分得：

將(4-20)代入到(4-18)中得到新的貝爾曼方程：

其中ε_e(t)＝ε_c(X(t+T))-ε_c(X(t))為貝爾曼方程誤差，Δψ_c(X(t)＝ψ_c(X(t+T)-ψ_c(X(t)；

為確定w_c，將(4-20)改寫成：

其中為V₀(X)的近似值，為理想的逼近系數，則(4-22)式為：

令為貝爾曼跟蹤誤差，并構造以下目標函數，通過使得ε_e(t)最小化來調整critic神經網絡的權重系數：

將(4-25)式兩邊對求導，再由鏈式法則得：

其中β_c0為學習率，為Δψ_c的近似值；

將E_e代入到(4-26)得critic神經網絡的權重系數的更新應服從：

將得到的理想權重系數代入到(4-14)中得最優控制策略，然而通過critic逼近的值函數所求得的最優策略卻并不能保證閉環系統的穩定性，要為執行器引入actor神經網絡來保證收斂到最優解的同時還能夠保證系統的穩定性：

為actor神經網絡的最優逼近系數，的更新由以下李雅普諾夫函數來確定：

當w_a滿足下式時，所逼近的策略使得系統一致最終有界，通過得到U^*(t)；

其中K₁、K₂為設計好的正常數，

基于(4-19)、(4-27)、(4-28)和(4-30)式，分別利用critic和actor算法實現對值函數和策略函數的同步更新，設計一種基于策略迭代的在線積分強化學習算法來求解HJB方程，以求解最優控制輸入；

算法：基于策略迭代的在線IRL算法

初始化：給定一個可行的執行器輸入

Step1：策略評估，給定初始利用下式求解

Step2：策略改進，將代入下式更新

Step3：令返回Step1，直至收斂到最小值。