[發明專利]一種基于關節能量均衡分配的關節機器人軌跡規劃方法

有效

• 申請號: 201710481048.2
• 申請日: 2017-06-22
• 公開（公告）號: CN107199569B
• 公開（公告）日: 2020-01-21
• 發明（設計）人: 葉伯生;周向東;陶婕妤;宋寶;熊碩;唐小琦;王昌杰;郭藝璇;李浩
• 申請（專利權）人: 華中科技大學
• 主分類號: B25J9/16
• 分類號: B25J9/16;B25J17/02
• 代理公司: 42201 華中科技大學專利中心
• 代理人: 周磊;曹葆青

1.一種基于關節能量均衡分配的關節機器人軌跡規劃方法，其特征在于，包括以下步驟：

1)根據D-H參數法建立n關節機器人連桿模型；

步驟1)具體步驟如下：

根據D-H參數法建立關節機器人的連桿坐標系模型，列出參數表，定義參數表中的關節角度θ＝[θ₁,θ₂,...,θ_n]，分別求解機器人關節i相對于關節i-1的變換矩陣：

其中，2≤i≤n，α_i-1表示關節i的軸線相對關節i-1軸線轉動的角度，d_i表示a_i-1和a_i之間的距離；

2)建立機器人動力學模型，獲得機器人關節力矩關系式；

步驟2)具體過程如下：

利用牛頓-歐拉法，首先向外遞推計算各連桿的速度和加速度，從而獲得各連桿的慣性力和力矩，然后向內遞推計算各連桿相互作用的力和力矩，以及計算各關節的驅動力和驅動力矩，最終得到機器人關節力矩τ、角度、角速度及角加速度的關系式：

其中，表示慣性力矩，表示離心力矩與哥氏力矩之和，G(θ)表示重力力矩，機器人關節力矩τ是上述力矩的合成；

3)辨識機器人在不同位姿的動力學參數；

步驟3)具體過程如下：

根據理論辨識法對機器人不同位置及姿態下各個連桿的慣性張量進行辨識，慣性張量表示如下所示：

其中，I_xx＝∫∫∫_V(y²+z²)ρdυ、I_yy＝∫∫∫_V(x²+z²)ρdυ、I_zz＝∫∫∫_V(x²+y²)ρdυ表示機器人各連桿繞各自連桿坐標軸x、y、z的質量慣性矩，I_xy＝∫∫∫_Vxyρdυ、I_yz＝∫∫∫_Vyzρdυ、I_zx＝∫∫∫_Vzxρdυ表示慣性積；

4)輸入機器人起始關節角度和終止關節角度，得到各個關節的力矩曲線，對機器人各個關節力矩曲線進行擬合，利用曲率最大法挑選特征點，獲得各關節對應的特征點數，記錄最大特征點數及獲得最大特征點數的關節；

步驟4)具體過程如下：

4.1)輸入機器人起始關節角度及目標關節角度，對起始位置到目標位置的路徑進行初始規劃，使機器人各關節同時啟動，同時到達，并以S型加減速運行，使機器人運行平穩且運行時間最優，辨識在對應位姿下機器人的慣性特性后，帶入步驟2)中機器人關節力矩的關系式中，從而得到各關節對應的力矩曲線；

4.2)利用曲率最大點的方法確定需要插入的中間離散點，中間離散點的曲率計算如下：

并且

其中，|κ_ir|表示機器人關節i上第r個離散點p_ir所對應的曲率，β_ir機器人關節i上r-1、r、r+1離散點相連而得到的夾角，||p_ir+1-p_ir-1||表示r-1和r+1離散點連線的模長，p_ir-1、p_ir和p_ir+1分別表示離散點坐標(x_ir-1,τ_ir-1)、(x_ir,τ_ir)和(x_ir+1,τ_ir+1)，x_ir-1、x_ir和x_ir+1表示第r-1、r和r+1個離散點，τ_ir-1、τ_ir和τ_ir+1表示第r-1、r和r+1個離散點的力矩；

如果κ_ir＞κ_ir-1、κ_ir＞κ_ir+1和κ_ir＞a·κ_iavg這三個條件同時滿足，則在各關節中挑選滿足上述條件的中間離散點作為插值點，遍歷各關節，得到各個關節的插值點數N_i，從而獲得特征插值點數N_e＝max(N₁,N₂,...,N_n)，并記錄特征插值點數所對應關節e；

5)建立評價函數作為能量均衡分配的判斷標準，利用粒子群算法尋求評價函數最優解，則獲得機器人運行軌跡；

步驟5)具體過程如下：

5.1)針對機器人能量均衡的目標建立評價函數f(p):

其中，P_i＝T_i·ω_i，P_e＝[P_e1,P_e2,...,P_en]表示各關節電機的額定功率，c_i對應關節i的功率權重；

5.2)對于關節e，以記錄的N_e個特征點為已知關節角度x_ie，規劃關節角速度分別由粒子群算法計算對應θ_e下，其他五個關節最優角度，具體步驟如下：

5.2.1)初始化粒子規模L，每個粒子在空間中位置表示為粒子速度表示為其中l∈[1,L]，表示第l個粒子，j∈[1,N_e]表示離散點j，k表示當前的迭代次數；

5.2.2)計算第l個粒子在k+1次迭代后的速度和位置通過第l個粒子在經過k次迭代后的速度和位置及第k次迭代后該粒子自己搜索的歷史最優值和全部粒子搜索的歷史最優值得到，計算公式如下所示：

其中，ω是慣性系數，是粒子保持原有速度的系數，c₁是粒子跟蹤自己歷史最優值的權重系數，c₂是粒子跟蹤群體最優值的權重系數，ξ,η∈[0,1]，代表隨機數，r代表約束因子；

5.2.3)搜索得到關節e角度為θ_ej時，對應一組機器人關節角，縮小機器人角度范圍后，再對θ_ej+1利用粒子群算法得到一組機器人關節角，直到N_e個特征點都得到一組最優關節值。

2.根據權利要求1所述的一種基于關節能量均衡分配的關節機器人軌跡規劃方法，其特征在于，n＝5或n＝6。