[發明專利]六自由度機械臂動力學模型辨識方法

在審

• 申請號: 201710265921.4
• 申請日: 2017-04-21
• 公開（公告）號: CN107498562A
• 公開（公告）日: 2017-12-22
• 發明（設計）人: 禹鑫燚;詹益安;歐林林;王正安;洪學勁峰
• 申請（專利權）人: 浙江工業大學
• 主分類號: B25J9/18
• 分類號: B25J9/18
• 代理公司: 杭州天正專利事務所有限公司33201
• 代理人: 王兵,黃美娟

1.六自由度機械臂動力學模型辨識方法，，具體步驟如下：

步驟1：建立機器人機械臂的動力學模型方程；首先，對于一個6自由度機械臂，通過牛頓-歐拉方法可以推導得出其動力學模型：其中，τ＝(τ₁,τ₂,...,τ_n)為機械臂的驅動力矩向量，n表示機械臂關節自由度個數，τ_i表示第i個關節的驅動力矩，q，是三組n×1維向量，分別表示關節位置，關節速度和關節加速度，D(q)是一個與位形相關的n×n的矩陣，被稱為慣性矩陣，對于任何機械臂來講，它的慣性矩陣是對稱且正定的，為科氏力及離心力項，g(q)為重力項，只取決于機器人的位姿，是靜態量；然后，根據改進的牛頓-歐拉動力學模型，其動力學模型可以被改寫成其中Φ是一個n×10n的觀測矩陣，只跟機械臂關節運動數據有關，p是機械臂慣性參數向量，p＝[p₁,p₂,...,p_n]^T，p_i是連桿i的慣性參數向量：p_i＝[I_xxi,I_xyi,I_xzi,I_yyi,I_yzi,I_zzi,mr_xi,mr_yi,mr_zi,m_i]^T，其中I_xxi,I_xyi,I_xzi,I_yyi,I_yzi,I_zzi為連桿i慣性矩陣I_ci的6個參數，mr_xi,mr_yi,mr_zi表示連桿i的一階質量矩，m_i是連桿i的質量；p_i的前9個量均包含在D(q)和項內，m_i包含在g(q)中；

步驟2：確定所需辨識的動力學參數；本說明確定辨識參數采用的方法是線性摩擦模型：其中τ_f為摩擦力矩，f_c為庫倫摩擦系數，f_v為粘性摩擦系數，為符號函數，滿足考慮摩擦的動力學參數可寫成向量的形式：p_dyn＝[p_dyn1,p_dyn2,...,p_dynn]^T，其中p_dyni＝[I_xxi,I_xyi,I_xzi,I_yyi,I_yzi,I_zzi,mr_xi,mr_yi,mr_zi,m_i,f_ci],i＝1,...,n，p_dyn就是動力學參數辨識的辨識對象，每個關節均包含10個機械臂慣性參數及2個摩擦參數；根據修改后的牛頓-歐拉參數，改進的機械臂的動力學方程可以改寫成動力學參數p_dyn的線性函數：其中Φ_dyn是一個n×12n的觀測矩陣，從而可以確定所辨識的動力學參數；

步驟3：對比其它動力學辨識算法，提出了基于改進的PSO算法的動力學模型辨識方法；其實現的詳細過程如下：首先，隨機初始化h個粒子群體，并根據所需辨識的械臂動力學模型參數個數設置粒子的維數，再在動力學參數取值范圍內隨機初始化粒子的起始位置和速度，初始化的各個粒子構成機械臂動力學參數的初始候選解集g_best以及p_jbest，其中，g_Best表示整個粒子群找到的全局最優值，p_jBest表示每個粒子本身所找到的最優解；其次，粒子群更新自身的速度和位置；接著，根據式得到變異的粒子群y(t)，其中，x_jk，y_jk表示第j個粒子的第k維元素，r_k是粒子第k維元素取值范圍內的隨機數，rand()是(0,1]之間的隨機數，R∈(0，1]是變異概率，再在變異的粒子群中選取有利的變異，對粒子群x(t)進行變異操作，并且根據適應度函數對當前粒子群中每個個體與p_jbest進行適應度比較，更新p_jbest，；之后，對各個粒子的p_jbest進行適應度比較，更新g_best；然后，判斷是否滿足迭代停止條件，即是否達到設定的迭代次數T_max，如果沒達到就重新更新自身的速度和位置；最后，全局極值對應的粒子為種群的最優解，即機械臂動力學參數的辨識值；

步驟4：設計基于改進的PSO算法動力學模型辨識方法的激勵軌跡；本說明對于激勵軌跡設計采用的是一種周期性的軌跡；激勵軌跡是有限項傅立葉級數，產生周期性的響應，機械臂的關節軌跡的表達式為：其中，t是時間，q_i,0是關節位置的偏移量，a_i,k和b_i,k是正弦和余弦函數的振幅，ω_f是傅立葉級數的基頻，這個傅立葉級數指定一個以T_f＝2π/ω_f為周期的周期函數，N是傅立葉級數的諧波項目數，每個傅立葉級數包含2N+1個參數，這些軌跡參數可以通過反復實驗來選擇；

步驟5：在UR5機械臂上進行參數辨識實驗；首先，本說明先激勵UR5機械臂的前三個關節，將后三個關節鎖死；接著，根據激勵軌跡，可以得到工作空間內的機械臂末端運動軌跡的3維圖，每次實驗時重復跟蹤軌跡，等機械臂瞬態效應消失后采集300組關節數據；關節的測量力矩是通過采集關節電機電流數據獲取的，關節力矩與關節電流的關系如下：τ＝KI，其中I是關節的電機電流，K是電流力矩系數；觀察觀測矩陣，其中有一些列中的所有元素恒等于0，表明這些列所對應的慣性參數對機械臂的動力學模型沒有影響，可以消去這些慣性參數；然后，本說明隨機初始化在位置邊界里全部粒子的位置，并按照適應度函數計算每個粒子的適應度，最后計算出個體最優值和全局最優值；如果實驗過程中迭代結果的前后值的差少于規定精度很多次，就認為整個實驗過程已趨于穩定；本說明中使用改進的粒子群算法辨識出前三個關節的動力學參數，然后基于實驗中辨識動力學模型的預測力矩和測量的實際力矩的比較，結果顯示雖然預測誤差在機械臂速度反轉時略大，但是預測力矩與實際測量力矩值的趨勢基本一致，從而證明PSO算法具有較強的參數尋優能力；為了證明PSO算法辨識模型的精確度，給出一個測量力矩和預測力矩之間的相關系數η來衡量辨識模型的精確度，這個相關系數η由歸一化互協方差函數定義：其中τ_i是預測力矩，τ_mi是測量力矩，η越接近1，辨識模型就越精確，當η越接近0時，辨識模型的精確度就越低；最后，為了驗證省略4、5、6連桿慣性參數對1、2、3關節力矩辨識的影響，驅動所有關節跟蹤激勵軌跡，其中前3個關節使用之前的激勵軌跡，后3個關節使用任意軌跡，對比前3個關節的測量力矩和預測力矩可以看出，預測力矩和測量力矩比較吻合,從而表明辨識所得的模型具有滿意的精度；

步驟6：在UR5機械臂上進行模型驗證實驗；首先，為了驗證本說明方法得到的動力學辨識模型的有效性，設計了另外一組機械臂前3個關節的驗證軌跡；然后，根據式用辨識所得的參數對驗證軌跡產生的力矩進行預測，驗證實驗表明得到的辨識模型能夠準確地預測關節力矩數據；此外，根據歸一化互協方差函數，驗證測試表明所提出的辨識方法是可靠的。