[發明專利]基于強化學習的機器人時間最優軌跡規劃方法及控制器

有效

• 申請號: 202010746579.1
• 申請日: 2020-07-29
• 公開（公告）號: CN111983924B
• 公開（公告）日: 2022-03-15
• 發明（設計）人: 李琳;肖佳棟;張鐵;鄒焱飚
• 申請（專利權）人: 華南理工大學;中山市華南理工大學現代產業技術研究院
• 主分類號: G05B13/04
• 分類號: G05B13/04
• 代理公司: 廣州粵高專利商標代理有限公司 44102
• 代理人: 江裕強

1.基于強化學習的機器人時間最優軌跡規劃方法，其特征在于，包括以下步驟：

S1將任務路徑的各關節參數輸入到路徑參數化模塊轉化為關于末端路徑的標量參數，具體包括以下步驟：

S11獲取任務路徑的時間序列的各關節角位移；

S12將各關節角度代入機器人正運動學方程獲得時間序列的笛卡爾空間位移；

其中，上述機器人正運動學方程為采用如下的矩陣表達式

其中p_x、p_y、p_z表示機器人末端坐標相對于基坐標的位置在極坐標x、y、z軸方向的分量，通過計算相鄰時間間隔p_x、p_y、p_z的變化得到笛卡爾空間位移；n_x、n_y、n_z表示末端姿態的X軸方向在極坐標x、y、z方向的分量；o_x、o_y、o_z表示末端姿態的Y軸方向在極坐標x、y、z方向的分量；a_x、a_y、a_z表示末端姿態的Z軸方向在極坐標x、y、z方向的分量；

矩陣Γ由各軸的相鄰坐標系的變換矩陣相乘得到，即其中為機器人總的關節數，相鄰坐標系的變換矩陣表達式如下

其中表示機器人任一關節，表示第軸的坐標系的Z軸繞著根據右手定則確定的X軸轉到第軸的坐標系的Z軸需要的角位移，表示第軸的坐標系的Z軸沿著根據右手定則確定的X軸到第軸的坐標系的Z軸的垂直距離，表示根據右手定則確定的第軸的坐標系的X軸沿著第軸的Z軸到第軸的坐標系的X軸的距離，表示根據右手定則確定的第軸的坐標系的X軸沿著第軸的Z軸轉動到第軸的坐標系的X軸的角位移；

S13根據笛卡爾空間位移計算機器人的路徑弧長，并取位移在總弧長的占比作為機器人在任一時刻的標量位移s；

S14使用分段最小二乘方法擬合關節角位移q關于標量位移s的函數q(s)；

S15對函數q(s)進行求導，獲得路徑曲率q′(s)和路徑曲率變化率q″(s)；

S2將參數化后的路徑輸入到路徑離散化模塊進行路徑離散化，具體包括以下步驟：

S21設定路徑曲率之差的閾值σ以及路徑曲率變化率之差的閾值

S22設定m＝1，κ＝n＝時間序列數，將各時間序列的路徑標量位移按時間順序構成集合其中m，κ，n均表示中間變量；

S23當m＜n時，如果或則令m＝m+1，否則取集合中第m-1個數放入集臺并令κ＝m；上式中max()表示取最大值，和表示集合中的第κ個數和第m個數，下標i表示機器人的第i個關節，表示當路徑位移為集合中的第κ個數的值時的第i個關節的路徑曲率，表示當路徑位移為集合中的第κ個數的值時的第i個關節的路徑曲率變化率，表示當路徑位移為集臺中的第m個數的值時的第i個關節的路徑曲率，表示當路徑位移為集合中的第m個數的值時的第i個關節的路徑曲率變化率；

S24重復步驟S23直到m≥n，則集合N就是離散化后的路徑標量位移的集合；

S3將離散化后的路徑輸入到強化學習模塊中構建強化學習環境，具體包括以下步驟：

S31將相平面分割為N×M的網格；所述網格分割方法具體包括以下步驟：

S311設定機器人運行時允許的各關節最大允許速度最大允許加速度最小允許速度最大允許加速度

S312根據不等式組

求得每個離散點允許的最大路徑標量速度，式中表示機器人的末端標量速度，表示機器人的末端標量加速度，q′_i(s)和q″_i(s)表示機器人的第i關節在末端標量位移為s時的函數q(s)的一階導數和二階導數；

S313設定在相平面上要劃分的網格尺寸N×M，其中N是沿著s方向要離散的網格數，沿著s方向的分割方法為步驟S2的離散化，從而將相平面沿s方向分割為N部分，M是沿著方向要離散的網格數；

S314以各離散點中最大的路徑標量速度作為速度上限以為步長，將相平面沿方向分割為M部分；

S315將相平面分割為N×M的網格，每個網格點即為一個狀態，當前網格點到下一個網格點的移動即為一個動作；

S32設定強化學習的狀態值函數，所述狀態值函數為：

Q(S_k，A_k)←Q(S_k，A_k)+α[R_k+1+γQ(S_k+1，A_k+1)-Q(S_k，A_k)]

式中S_k表示相平面上的第k個狀態，A_k表示在第k個狀態時選擇的動作，Q()表示在狀態S_k時選擇動作A_k對應的Q值表上的值，α表示學習系數，γ表示折扣因子，R_k+1表示在狀態S_k時執行動作A_k獲得的獎勵或懲罰，箭頭←表示箭頭右邊的值賦值給左邊；

S33設定強化學習的搜索策略，所述的搜索策略為貪婪策略，其具體步驟為：

S331設定貪婪因子ε，其取值范圍在0到1之間；

S332隨機生成一個在0到1之間的數λ；

S333如果λ＜ε，則執行探索，在動作范圍內隨機選取一個動作；如果λ≥ε，則選取動作范圍內具體最大路徑標量速度的動作；

S34設定強化學習的動作獎勵和懲罰，所述動作獎勵和懲罰的表達式如下

其中，和表示在第k-1和k個狀態選擇的動作得到的狀態對應的路徑標量速度；

當程序選擇的動作指向的下一個狀態違反約束條件，即違反步驟S312不等式組或當前狀態的動作范圍內沒有動作可選擇時，則獲得懲罰，否則，當程序選擇的動作指向的下一個狀態不違反約束條件時，則得到獎勵；

S35設定強化學習的動作選擇范圍，所述動作范圍選擇設定步驟具體如下：

S35l根據式計算當前狀態的最大標量加速度以及最小標量加速度

S352根據勻加速運動方程求得下一狀態允許的最大/最小標量速度，標量速度范圍內Q值大于0的網格點即為可供選擇的動作范圍；

S4使用強化學習模塊學習最優的策略軌跡，具體包括以下步驟：

S41初始化Q值表，將Q值表上的值都設置為0，設定最大學習次數并初始化學習次數為1；

S42當學習次數＜最大學習次數時，轉到步驟S44，否則轉到步驟S49；

S43令k＝1，初始化獎勵值R_k+1＝0，從起始狀態(0，0)開始學習過程，計算下一狀態動作范圍，并使用貪婪策略從動作范圍中選取出動作A_k；

S44當k≤N并且R_k+1≥0時，轉到步驟S45；

S45執行動作A_k，獲得獎勵或懲罰R_k+1和狀態S_k+1，并計算下一狀態動作范圍；

S46如果獎勵或懲罰R_k+1＜0或下一狀態動作范圍為空，則轉到步驟S47，否則轉到步驟S48；

S47令Q(s_k+1，A_k+1)＝0，并根據步驟S32的狀態值函數更新動作Q值，同時為了加速懲罰，對這段探索過程中的所有動作都添加一個懲罰項進行懲罰，具體做法為對于從1逐漸增大到k的整數j，有Q(S_j，A_j)＝Q(S_j，A_j)+ρ^k-jR_k+1，其中ρ為懲罰因子，其取值范圍為0＜ρ＜1，之后令學習次數+1并轉到步驟S42；

S48在狀態S_k+1計算動作范圍，并通過貪婪策略從動作范圍中選取動作A_k+1，之后根據步驟S32的狀態值函數更新動作Q值，令S_k←S_k+1，A_k←A_k+1，k←k+1，并轉到步驟S45；

S49當學習次數到達最大學習次數時，令貪婪因子ε＝0重新進行學習過程，即在從狀態(0，0)開始學習時，不會再進行探索，而會直接選取當前動作范圍內具有最大路徑標量速度的動作，從而獲得在最大學習次數下所能獲得的最優策略軌跡，同時將訓練后的Q值表保存到存儲模塊中，在下一次訓練時調用訓練過的Q值表，節省學習時間；

S5運行策略軌跡獲得反饋的關節力矩：求出到達每個路徑離散點k所需要的時間t(k)：

由勻加速運動方程，規劃完成時間最優軌跡后路徑離散點的加速度為

其中s(k)表示第k個離散點所對應的路徑位移，表示第k個離散點所對應的路徑速度，表示第k個離散點所對應的路徑加速度；

由速度加速度方程，可求出到達每個路徑離散點k所需要的時間t(k)：

求得到達每個路徑離散點的時間之后，便可求得每個控制周期的路徑標量位移；若要求第n個控制周期的標量位移，到達該控制點的時間即為nT，其中T為控制周期；假設該控制時間與到達路徑離散點k所需要的時間t(k)最接近，則這兩點的時間間隔為：

Δt＝nT-t(k)

根據勻加速運動方程，該控制點的路徑位移即為：

從而可以求得每個控制周期的路徑標量位移，再代入函數q(s)即可求得每個控制周期的關節角度，最后將關節角度轉化為關節脈沖指令傳輸到機器人控制器運行；

S6將反饋的關節力矩輸入到強化學習模塊從而對強化學習環境進行修正，具體做法為：在獲得測量力矩之后，如果某一狀態點對應的測量力矩超過給定力矩約束的限制，即將相平面上的該狀態點設為不可行狀態，當在該狀態的前一狀態通過貪婪策略剛好選擇的動作指向該不可行狀態時，將之視為違反約束條件的情況并直接對相應動作的Q值進行懲罰。

2.根據權利要求1所述的基于強化學習的機器人時間最優軌跡規劃方法的控制器，其特征在于，包括路徑參數化模塊、路徑離散化模塊、強化學習模塊和存儲模塊；

所述路徑參數化模塊，用于將機器人關節參數轉化為關于末端路徑的標量參數；

所述路徑離散化模塊，用于將機器人末端的連續任務路徑離散化為若干個離散點；

所述強化學習模塊，用于構建機器人時間最優軌跡規劃的強化學習環境以及在強化學習環境中學習最優策略軌跡；

所述存儲模塊，用于存儲強化學習的學習數據。