---
tags: 學習心得 , 機器人
---

# [學習心得]SCARA二軸機械手臂系統參數鑑別

## 安裝SYMORO

* 詳見\Scara\Step0 l Installing SYMORO\32 bit\安裝說明.docx

### SYMORO介紹

* SYMORO是一套用來計算機械手臂動態模型的開源軟體，在Python 2.7開發開源軟體，可在Windows或Linux上執行。只要輸入參數，就可以幫你計算出機械手臂的物理模型。
* 但目前軟體好像沒有人在繼續維護，所以對應python的套件版本都停留在很久以前，要安裝的話要把對應的套件版本都安裝好，詳細可見安裝說明。
> 參考文獻：W. Khalil, A. Vijayalingam, B. Khomutenko, I. Mukhanov, P. Lemoine and G. Ecorchard, "OpenSYMORO: An open-source software package for symbolic modelling of robots," 2014 IEEE/ASME International Conference on Advanced Intelligent Mechatronics, 2014, pp. 1206-1211
### 關於Python 3版本
* 到SYMORO的GitHub上，去Pull request看可以找到Python 3的版本，如此就可以使用Anaconda建環境，但是有些bug可能要改一下python程式。
> GitHub: https://github.com/symoro/symoro

## 建立SYMORO模型

* 過程詳見\Scara\Step1 l SYMORO Model Constructing\SYMORO動態模型建立說明文件V3.docx


### **IDM**, Inverse Dynamic Model
常見的機械手臂的動態方程式為

其中 *τ* 為機械手臂力矩，為一nx1個的向量，*H*為機械手臂的慣量矩陣，*GAM*為機械手臂中剩餘力項的矩陣，包含了科氏力+向心力*C*、重力*G*和摩擦力*23*等，而一般SCARA前兩軸為水平放置，因此無重力項。
我們可以將上式右邊軸空間變數*θ*、$\dot{θ}$、$\ddot{θ}$相同的項合併，得到一個簡化參數的IDM如下：


其中 *φ*^T^ 為回歸矩陣，即是與軸空間變數有關的輸入矩陣，Θ為簡化後的動態模型參數，也就是本偏要鑑別的參數。如此便可利用RLS等方法，對IDM模型做系統參數鑑別。

### **DDM**, Direct Dynamic Model

做完系統參數鑑別後，若能得到H和GAM矩陣，便可藉由目前手臂的軸位置、軸關節速度和輸入力矩，去推估機械手臂的加速度，如下式


藉此我們可以對機械手臂進行更精準地控制(如：前饋)，或是做重力補償、順應性等控制。

## 產生激發軌跡

為了激發出機械臂動態，本方法利用傅立葉級數產生一組軌跡，如下


透過設計參數a~l~、b~l~、q~0~來決定軌跡動態，w~f~為基本頻率，為運作週期t~f~的倒數，可自行決定。
同時，將軌跡微分可得軸速度、軸加速度如下：


這些位置、速度及加速度命令，須符合馬達的軸空間限制、額定轉矩及額定扭矩，也可能會加入卡式空間限制。另外為了得到最佳的激發軌跡，軌跡方程式條件數(cond)越低越好，才能避免病態系統出現。

此為一個最佳化問題如下：


可以利用matlab內建的fmincon來找出最佳的軌跡。(每次跑出來的最佳軌跡都不會一樣，但只要條件數低就行。)

> 參考文獻：J. Swevers, C. Ganseman, J. De Schutter, H. Van Brussel, EXPERIMENTAL ROBOT IDENTIFICATION USING OPTIMISED PERIODIC TRAJECTORIES, Mechanical Systems and Signal Processing, Volume 10, Issue 5, 1996, Pages 561-577

## 激發軌跡模擬

設計出軌跡後，需先模擬出實際甩動機械臂的動態，以確保不會超出空間限制(無論是軸空間還是卡式空間)。另外也要注意機械臂的某一軸是否有維持同一方向的情況，或是有太細微的甩動，會無法激發出機械臂的慣量。

## 產生數據和數據處理

模擬沒問題後便可上機台實驗，控制方法使用簡單如PD-like控制器即可


產生每個取樣時間的力矩命令以及軸位置數據檔，用以參數鑑別，因為鑑別軌跡是週期性的，可以跑很多次，在之後處理資料時將多次的數據做平均，以得到較佳的數據。

因為只有軸位置命令，需先將軸位置經過Butterworth濾波器做零相位濾波後(可用matlab的butterworth和filtfilt函數，頻寬根據經驗法則是10倍的系統動態頻率)，再利用中央差分法得到軸速度和軸加速度，最後將軸空間資訊代入到先前的回歸矩陣W。

此時因為W矩陣中含有sin和cos會使得W出現更高頻，且力矩數據也尚未做濾波，為了將兩者頻寬一致，我們設定n~d~個數據做一次平行抽取濾波(可用matlab的decimate)，除了可以濾掉兩者的高頻雜訊(8階Chebyshev濾波器)，也能減少之後做鑑別的參數資料量，因為太接近的數據不需要這麼多。

## 參數鑑別

以下介紹三個離線(off-line)鑑別IDM簡化參數的方法：

### **IDIM-LS**, Inverse Dynamic Identification Method - Least Squares


IDIM-LS是常見的鑑別方法，利用前面提到簡化的IDM模型 *τ=φ^T^Θ* ，可以將數據處理後的軸空間資訊和力矩輸出填入*τ*和*φ*中，再利用最小平方法(least-square)就可求得簡化的參數*Θ*。另外可以再利用加權最小平方法(weighted least-square)，將權重設為原本LS和真實輸出誤差的倒數(即誤差越大權重越小)，求得更準確的參數值。

參考文獻：M. Gautier, W. Khalil, and P. P. Restrepo, “Identification of the dynamic parameters of a closed-loop robot,” in Proc. IEEE Int. Conf. Robot. Autom., 1995, pp. 3045–3050.

### **DIDIM**, Direct and Inverse Dynamic Identification


DIDIM為輸入誤差的參數鑑別方法，其概念是利用相同的控制律（模型系統的增益會調整），去觀察真實系統和模型系統的系統輸入去持續修正模型參數，但需要有一開始較接近真實值的參數，可利用IDIM-LS或是機構本身的CAD檔先取得接近真實值的參數，再用DIDIM繼續鑑別。

>參考文獻：M. Gautier, A. Janot and P. Vandanjon, "A New Closed-Loop Output Error Method for Parameter Identification of Robot Dynamics," in IEEE Transactions on Control Systems Technology, vol. 21, no. 2, pp. 428-444, March 2013

### **IDIM-IV**, Inverse Dynamic Identification Method – Instrument Variable

>.

## 驗證模型

設計一個新軌跡，利用CTC(computed torque control)或是阻抗控制(impedance control)等方法，進行循跡控制。

## 分離模型中各項

利用SYMORO得到之模型僅有慣量H以及剩餘力項GAM兩個矩陣，利用鑑別出的基本參數可以帶入得到慣量矩陣的真實值，而GAM則要利用一些方法去分離出摩擦力F、重力G、科氏力與向心力C。


>1. 摩擦力
摩擦力為最容易分離的，直接從GAM找出黏滯摩擦力f~v~和庫倫摩擦力f~c~項即可。
>2. 重力
重力可以透過將$\dot{θ}$設為0，如此一來剩餘的力項即是重力項。
>3. 向心力
如果僅僅將剩餘力項扣掉摩擦力和重力，只能得到*C(θ,*$\dot{θ}$)此項，但若要得到可以分離出角速度*$\dot{θ}$矩陣C，則必須利用反對稱矩陣 求解，找到符合下式的C矩陣
>
>

由於向心力矩陣會出現多組符合的解，有一個特殊解的方法如下所示：


如此解出來的C便將角速度項分離出來。