機器人單關節力矩控制

時間 2019-11-06

標籤機器人關節力矩控制简体版

原文原文鏈接

　　對於自由運動機器人來講，控制的目的是要控制機器人末端的位置和姿態（統一簡稱爲位置），即所謂的位置控制問題。指望機器人末端達到的位置稱爲指望位置或指望軌跡，指望軌跡能夠在機器人任務空間中給出，也能夠經過逆運動學轉化爲機器人關節空間中的指望軌跡。指望軌跡一般有兩種形式：一種是一個固定位置（setpoint），另外一種是一條隨時間連續變化的軌跡（trajectory）。php

　　對於運動受限的機器人來講，其控制問題要複雜得多。因爲機器人與環境接觸，這時不只要控制機器人末端位置，還要控制末端做用於環境的力。也就是說，不只要使機器人末端達到指望值，還要使其做用於環境的力達到指望值。更普遍意義下的運動受限機器人還包括機器人協同工做的狀況，這時控制還應包括各機器人間運動的協調，負荷的分配以及所共同夾持的負載所受內力的控制等複雜問題。html

　　以下圖所示，考慮單個電機與連桿的模型，其中$\tau$爲電機施加的力矩，$\theta$是連桿與水平線的夾角：git

　　則系統的動力學方程爲：$$\tau=M\ddot{\theta}+mgr\cos\theta$$github

　　$M$是連桿繞轉軸的轉動慣量，$m$是連桿的質量，$r$是轉軸到連桿質心的距離，$g$是重力加速度。實際狀況中轉軸處會存在摩擦力，這裏簡單假設摩擦力爲粘滯阻力，即$\tau_{fric}=b\dot{\theta}$，將阻力項加入上面的動力學方程中，模型變爲：$$\tau=M\ddot{\theta}+mgr\cos\theta+b\dot{\theta}$$ide

　　可將其改寫爲：$$\tau=M\ddot{\theta}+h(\theta,\dot{\theta})$$函數

　　爲進行仿真，設參數$M=0.5kg\cdot m^2$，$m=1kg$，$r=0.1m$，$b=0.1N\cdot m\cdot s/rad$，當連桿在平面上運動時$g=0$，當連桿垂直水平面運動時$g=9.81m/s^2$oop

PID控制

　　PID控制器時是最經常使用的反饋控制器。$$\boxed{\tau=K_p\theta_e+K_i\int\theta_e(t)dt+K_d\dot{\theta_e}}$$學習

　　其中比例係數$K_p$的做用就像一個虛擬彈簧，試圖減少位置偏差$\theta_e=\theta_d-\theta$；微分系數$K_d$的做用像虛擬阻尼器，試圖減少速度偏差$\dot{\theta_e}=\dot{\theta_d}-\dot{\theta}$；積分系數能夠減少穩態偏差。spa

PD控制器與偏差的二階方程

　　忽略積分項，即$K_i=0$，這就是PD控制。咱們假設機器人在水平面上運動（g=0），將PD控制規則帶入動力學方程中可獲得$M\ddot{\theta}+b\dot{\theta}=K_p(\theta_d-\theta)+K_d(\dot{\theta_d}-\dot{\theta})$，若是給定的控制量$\theta_d$恆定，則$\dot{\theta_d}=\ddot{\theta_d}=0$，因而$\theta_e=\theta_d-\theta$，$\dot{\theta_e}=-\dot{\theta}$，$\ddot{\theta_e}=-\ddot{\theta}$。動力學方程可改寫爲偏差$\theta_e$的二階微分方程：$$M\ddot{\theta_e}+(b+K_d)\dot{\theta_e}+K_p\theta_e=0$$.net

　　將其改寫爲標準二階形式：$$\ddot{\theta_e}+\frac{b+K_d}{M}\dot{\theta_e}+\frac{K_p}{M}\theta_e=0\rightarrow\ddot{\theta_e}+2\xi\omega_n\dot{\theta_e}+\omega_n^2\theta_e=0$$

　　阻尼比$\xi$和阻尼天然頻率$\omega_n$爲：$\xi=\frac{b+K_d}{2\sqrt{K_pM}}$、$\omega_n=\sqrt{\frac{K_p}{M}}$

PID控制器與偏差的三階方程

　　考慮連桿在垂直水平面的平面上轉動（g>0），用上面的PD控制器，偏差動力學方程可寫爲：$$M\ddot{\theta_e}+(b+K_d)\dot{\theta_e}+K_p\theta_e=mgr\cos\theta$$

　　這意味着穩態時$\theta$知足$K_p\theta_e=mgr\cos\theta$，即當$\theta_d\neq\pm\pi/2$時最終偏差$\theta_e$不爲零，由於機器人須要提供一個非零的力矩使連桿保持在$\theta_d\neq\pm\pi/2$這個位置。

　　設$K_p=2.205N\cdot m/rad$，$K_d=2N\cdot m\cdot s/rad$，連桿初始角度$\theta(0)=-\pi/2$，初始角速度$\dot{\theta}(0)=0$，指望角度$\theta_d=0$，指望角速度$\dot{\theta_d}=0$。根據所設置的參數在Mathematica中求解微分方程，結果以下：

　　穩態偏差可求解方程$K_p(\theta_d-\theta)=mgr\cos\theta$求得，約爲-0.408弧度，即穩態時與指望值相差23.4°

　　能夠在VREP中仿真該過程，新建一個長度爲0.2m的圓柱體，質量設爲1kg，在圓柱體一段添加旋轉關節，圓柱體X、Y軸的轉動慣量設爲0.49(根據轉動慣量的平行軸定理，繞關節轉動的轉動慣量爲0.5)。關節設爲力矩模式，打開關節動力學屬性對話框，設置PID參數和目標位置（90°）：

　　選擇ODE物理引擎進行仿真，打開圓柱體動力學屬性對話框，點擊編輯材料（Edit material），在Angular damping中輸入0.1（Angular damping：an angular movement damping value, that adds angular drag, and that can increase stability）。給關節添加阻力可參考Set internal friction of joints和Material properties。

　　添加Graph記錄關節轉角進行仿真，能夠看出存在穩態偏差：

　　爲了消除穩態偏差，設$K_i>0$，即便用PID控制器。積分控制的做用是消除穩態偏差，由於系統只要存在偏差，積分做用就不斷地積累，輸出控制量，直到誤差爲零，積分做用纔會中止。但積分做用太強會使系統超調加大，甚至使系統出現振盪。加入積分項後關於偏差的微分方程爲：$M\ddot{\theta_e}+(b+K_d)\dot{\theta_e}+K_p\theta_e+K_i\int\theta_e(t)dt=\tau_{dist}$，其中$\tau_{dist}$是重力項$mgr\cos\theta$。對該方程兩邊求導，能夠獲得偏差的三階微分方程：$$M\theta^{(3)}_e+(b+K_d)\ddot{\theta_e}+K_p\dot{\theta_e}+K_i\theta_e=\dot{\tau_{dist}}$$

　　若是$\tau_{dist}$爲常量（好比穩態時），方程右邊$\dot{\tau_{dist}}$項爲零。使用拉普拉斯變換將時域中的微分方程轉換爲複數域中的代數方程：$$s^3+\frac{b+K_d}{M}s^2+\frac{K_p}{M}s+\frac{K_i}{M}=0$$

　　若是該特徵方程根的實部全爲負，則該系統穩定，且$\theta_e$趨於零。根據系統特徵方程的勞斯判據，爲了保證方程全部根的實部都爲負，PID控制器的係數必須知足下列條件：$$\begin{align*}K_d&>-b\\K_p&>0\\\frac{(b+K_d)K_p}{M}>K_i&>0\end{align*}$$

　　所以$K_i$的值必須保證落在上下界限以內。一般先選擇合適的$K_p$、$K_d$值加快暫態過程，而後調整$K_i$使其能大到消除穩態偏差，又不至於過大而影響穩定性。畫出根軌跡圖（根軌跡是指系統開環傳遞函數中某個參數從零變到無窮時，閉環特徵根s在平面上移動的軌跡），能夠看出當$K_i=(b+K_d)K_p/M$時兩個根將到達虛軸，$K_i$繼續變大根會進入複平面的右半部分，此時系統將變得不穩定。

　　PID控制的僞代碼以下：

time = 0                          // dt = servo cycle time
eint = 0                          // error integral
qprev = senseAngle                // initial joint angle q

loop
    [qd,qdotd] = trajectory(time) // from trajectory generator
   
    q = senseAngle                // sense actual joint angle
    qdot = (q - qprev)/dt         // simple velocity calculation
    qprev = q
   
    e = qd - q
    edot = qdotd - qdot
    eint = eint + e*dt
   
    tau = Kp*e + Kd*edot + Ki*eint
    commandTorque(tau)
   
    time = time + dt
end loop

　　使用以前創建好的VREP模型，設置積分系數$K_i=1$後再進行仿真：

　　能夠看出添加積分項後穩態偏差消除（可是產生了超調量）：

　　實際上在許多機器人控制器中設$K_i=0$，即不使用積分項。由於相比穩態偏差，穩定性纔是最重要的考慮因素。

前饋控制（Feedforward Control）

　　PID控制不考慮機器人的動力學特性，只按照誤差進行負反饋控制，即只有在獲得偏差信號後才能輸出控制量。另外一種控制策略是根據機器人動力學模型預先產生控制力/力矩。動力學模型爲：$$\tau=\widetilde{M}(\theta)\ddot{\theta}+\widetilde{h}(\theta,\dot{\theta})$$

　　根據軌跡生成器輸出的指望位置$\theta_d$、速度$\dot{\theta_d}$、加速度$\ddot{\theta_d}$，前饋力矩爲：$$\tau(t)=\widetilde{M}(\theta_d(t))\ddot{\theta_d}(t)+\widetilde{h}(\theta_d(t),\dot{\theta_d}(t))$$

　　前饋控制僞代碼以下：

time = 0                                           // dt = servo cycle time
loop
    [qd,qdotd,qdotdotd] = trajectory(time)         // trajectory generator
    tau = Mtilde(qd)*qdotdotd + htilde(qd,qdotd)   // calculate dynamics
    commandTorque(tau)
    time = time + dt
end loop

　　若是動力學模型徹底準確，那麼用前饋控制便可直接計算力矩。但實際上模型老是存在各類偏差，所以前饋控制通常老是與反饋控制結合起來使用。好比下面這個例子，若是$\widetilde{r}=0.08m$，即動力學公式中轉軸到質心距離這個參數爲0.08m（實際上這個值爲0.1m）。由於存在測量和製造偏差，準確的動力學模型很可貴到。考慮2個任務：任務1連桿運動軌跡爲$\theta_d(t)=-\frac{\pi}{2}-\frac{\pi}{4}\cos(t)$，運動時間爲$0\leqslant t \leqslant \pi$；任務2連桿運動軌跡爲$\theta_d(t)=\frac{\pi}{2}-\frac{\pi}{4}\cos(t)$，運動時間爲$0\leqslant t \leqslant \pi$

　　能夠看出模型存在偏差的狀況下，根據動力學方程計算力矩而獲得的結果與指望值之間存在誤差。對於任務2重力會阻礙指望的運動，致使產生較大的跟蹤偏差。

　　對任務2，在MATLAB/Simulink中進行仿真也能夠獲得一樣的結果：

　　其中arm dynamics子模塊根據輸入的力矩計算並輸出角度或角速度：

　　指望角度軌跡（藍線）與根據不許確的動力學模型計算的實際軌跡（橙線）以下圖所示：

前饋-反饋控制（feedforward and feedback control）

　　前饋控制是一種預測控制，經過對系統當前工做狀態的瞭解，預測出下一階段系統的運行情況。前饋的缺點是在使用時須要對系統有精確的瞭解，只有瞭解了系統模型纔能有針對性的給出預測補償。但在實際工程中並非全部的對象都是可獲得精確模型的，並且不少控制對象在運行的同時自身的結構也在發生變化。因此僅用前饋並不能達到良好的控制品質。這時就須要加入反饋，反饋的特色是根據誤差來決定控制輸入，無論對象的模型如何，只要有誤差就根據誤差進行糾正，能夠有效的消除穩態偏差。

　　前饋-反饋綜合控制結合兩者的優勢，能夠提升系統響應速度。從前饋控制角度看，因爲增長了反饋控制，下降了對前饋控制模型精度的要求；從反饋控制角度看，前饋控制做用對主要干擾及時進行粗調，大大減小反饋控制的負擔。

　　結合前饋與反饋，實際角加速度$\ddot{\theta}$可寫爲：$$\ddot{\theta}=\ddot{\theta_d}+K_p\theta_e+K_d\dot{\theta_e}+K_i\int\theta_e(t)dt$$

　　將其帶入動力學預測方程中可獲得前饋-反饋控制器：$$\boxed{\tau=\widetilde{M}(\theta)(\ddot{\theta_d}+K_p\theta_e+K_i\int\theta_e(t)dt+K_d\dot{\theta_e}) +\widetilde{h}(\theta,\dot{\theta})}$$

　　前饋-反饋控制器的結構以下圖所示，一般$K_i$設爲零，即便用PD+前饋控制：

　　前饋-反饋控制的僞代碼以下：

time = 0                                   // dt = cycle time
eint = 0                                   // error integral
qprev = senseAngle                         // initial joint angle q
loop
    [qd,qdotd,qdotdotd] = trajectory(time) // from trajectory generator
    
    q = senseAngle                         // sense actual joint angle
    qdot = (q - qprev)/dt                  // simple velocity calculation
    qprev = q
    
    e = qd - q
    edot = qdotd - qdot
    eint = eint + e*dt
   
    tau = Mtilde(q)*(qdotdotd+Kp*e+Kd*edot+Ki*eint) + htilde(q,qdot)
    commandTorque(tau)
    
    time = time + dt
end loop