在學習機器人動力學相關內容時看到MATLAB論壇上一個有意思的仿真項目Impedance Control for a 2-Link Robot Arm - User-interactive，一個用MATLAB實現的平面二連桿機械臂阻抗控制仿真。用戶可以點擊並拖拽鼠標來實時改變機械臂的目標位置，在控制力矩作用下機械臂會跟隨目標點運動。按空格鍵可以切換控制模式，此時拖拽鼠標用來給末端施加一個擾動力，由於阻抗控制的作用末端表現得像彈簧阻尼器一樣，擾動力消失后末端回復到目標位置。
讓我們來關注一下實現的細節。如下圖所示，連桿1和連桿2在XY平面上通過旋轉關節串聯構成一個二自由度的機械臂（忽略關節摩擦等因素），重力加速度$g$沿着Y軸負方向（向量形式可寫為$\mathbf{g}=[0,-g,0]^T$）。機械臂關節$a$固定在坐標原點，末端為$e$，F為作用在末端的力。$c_1$與$c_2$分別為兩個連桿的質心，桿長分別為$l_1$、$l_2$，桿的質量分別為$m_1$、$m_2$，質心到關節的距離分別為$d_1$、$d_2$。$\theta_1$為與Y軸正方向的夾角($\theta_1=\theta_2=0$時機器人保持豎直狀態)，$\theta_2$為連桿2與連桿1之間的夾角，是一個相對角度。
定義上述變量后根據理論力學等知識開始一系列計算。首先計算連桿上各點的位置坐標：
連桿1與連桿2在坐標系中的方向向量與角度$\theta_1$、$\theta_2$相關，其中連桿1的方向向量為$\mathbf{er_1}=\begin{bmatrix}-sin\theta_1\\ \cos\theta_1\\0\end{bmatrix}$，連桿2的方向向量為$\mathbf{er_2}=\begin{bmatrix}-sin(\theta_1+\theta_2)\\ \cos(\theta_1+\theta_2)\\0\end{bmatrix}$
$\mathbf{r_{ac_1}}=d_1 \cdot \mathbf{er_1}\\ \mathbf{r_{bc_2}}=d_2 \cdot\mathbf{er_2}\\ \mathbf{r_{be}}=l_2 \cdot\mathbf{er_2}\\ \mathbf{r_{ab}}=l_1 \cdot\mathbf{er_1}\\ \mathbf{r_{ac_2}}=\mathbf{r_{ab}}+\mathbf{r_{bc_2}}\\ \boxed{\mathbf{r_{ae}}=\mathbf{r_{ab}}+\mathbf{r_{be}}}$
然后計算各點速度：連桿1上線速度的方向向量為$\mathbf{ev_1}=\begin{bmatrix}-cos\theta_1\\ -\sin\theta_1\\0\end{bmatrix}$，連桿2上線速度的方向向量為$\mathbf{ev_2}=\begin{bmatrix}-cos(\theta_1+\theta_2)\\ -\sin(\theta_1+\theta_2)\\0\end{bmatrix}$
$\mathbf{v_{c_1}}=d_1 \cdot \dot{\theta_1} \cdot\mathbf{ev_1}\\ \mathbf{v_b}=l_1 \cdot \dot{\theta_1}\cdot \mathbf{ev_1}\\ \mathbf{v_{c_2}}=\mathbf{v_b} + d_2 \cdot (\dot{\theta_1}+\dot{\theta_2})\cdot \mathbf{ev_2}\\ \boxed{\mathbf{v_e}=\mathbf{v_b} + l_2 \cdot (\dot{\theta_1}+\dot{\theta_2})\cdot \mathbf{ev_2}}$
接着對速度求導計算各點加速度：
$\begin{align*}&\mathbf{a_{c_1}}=\dot{\mathbf{v_{c_1}}} = d_1 \cdot \ddot{\theta_1} \cdot\mathbf{ev_1} - d_1 \cdot \dot{\theta_1}^2 \cdot \mathbf{er_1}\\&\mathbf{a_b}=\dot{\mathbf{v_b}} = l_1 \cdot \ddot{\theta_1} \cdot\mathbf{ev_1} - l_1 \cdot \dot{\theta_1}^2 \cdot \mathbf{er_1}\\&\mathbf{a_{c_2}}=\dot{\mathbf{v_{c_2}}} = d_2 \cdot (\ddot{\theta_1}+\ddot{\theta_2}) \cdot\mathbf{ev_2} - d_2 \cdot (\dot{\theta_1}+\dot{\theta_2})^2 \cdot \mathbf{er_2} + \mathbf{a_b}\end{align*}$
根據受力情況可計算出關節處的力矩，對於關節$b$來說出除了電機力矩$\mathbf{\tau_2}$外，桿件2自身重量以及末端上的作用力$\mathbf{F}$都會對$b$點產生力矩，因此$b$點的合力矩為：
$\mathbf{M_b}=(\mathbf{r_{bc_2}}\times m_2 \mathbf{g}) + (\mathbf{r_{be}}\times \mathbf{F}) +\mathbf{\tau_2}$
根據動量矩定理，桿2對$b$點的動量矩變化率$\boxed{\frac{d\mathbf{L_2}}{dt} =\mathbf{M_b} }$，$\frac{d\mathbf{L_2}}{dt} =I_2(\mathbf{\ddot{\theta_1}}+\mathbf{\ddot{\theta_2}})+ \mathbf{r_{bc_2}} \times m_2 \mathbf{a_{c_2}}$，其中$I_2$為桿2繞其質心$c_2$的轉動慣量，$I_2=\frac{1}{12}m_2{l_2}^2$。根據動量矩定理可得到角加速度$\ddot{\theta_2}$與力矩$\mathbf{M_b}$的關系式。
對於關節$a$，外力的合力矩為：
$\begin{align*}\mathbf{M_a}&=(\mathbf{r_{ac_2}}\times m_2 \mathbf{g}) +(\mathbf{r_{ac_1}}\times m_1 \mathbf{g}) + (\mathbf{r_{ae}}\times \mathbf{F}) +\mathbf{\tau_1}\\ \frac{d\mathbf{L_1}}{dt}&=I_2(\mathbf{\ddot{\theta_1}}+\mathbf{\ddot{\theta_2}})+I_1\mathbf{\ddot{\theta_1}}+ (\mathbf{r_{ac_2}} \times m_2 \mathbf{a_{c_2}}) + (\mathbf{r_{ac_1}} \times m_1 \mathbf{a_{c_1}})\end{align*}$
其中$I_1$為桿1繞其質心$c_1$的轉動慣量，$I_1=\frac{1}{12}m_1{l_1}^2$。根據動量矩定理$\boxed{\frac{d\mathbf{L_1}}{dt} =\mathbf{M_a} }$可得到角加速度$\ddot{\theta_1}$與力矩$\mathbf{M_a}$的關系式。
通常可將根據動量矩定理或牛頓-歐拉法推導出的等式寫為如下形式：$$\boxed{\tau = M(\theta)\ddot{\theta}+C(\theta,\dot{\theta})+G(\theta)}$$
如果機械臂自由度為$n$，$M(\theta)$為$n\times n$階正定對稱矩陣，$M(\theta)\ddot{\theta}$代表慣性力項。$M(\theta)$中的主對角線元素表示各連桿本身的有效慣量，代表給定關節上的力矩與產生的角加速度之間的關系，非對角線元素表示連桿之間的耦合慣量，即是某連桿的加速運動對另一關節產生的耦合作用力矩的度量 ；$C(\theta,\dot{\theta})$為$n\times 1$階向心力和科氏力項；$G(\theta)$為$n\times 1$階的重力項，與機器人的形位$\theta$有關。
在純位置控制下施加在機械臂末端的外力並不會影響末端的運動，因為這種情況可以認為機械臂是完全剛性的。而如果要實現主動柔順控制，即使機械臂表現出一定的柔性就需要考慮其與環境之間的相互作用。這時關節驅動力矩可寫為：$$\boxed{\tau = M(\theta)\ddot{\theta}+C(\theta,\dot{\theta})+G(\theta)+J^T(\theta)F_{tip}}$$
上面等式中，$F_{tip}$為機械臂末端與外界環境之間的交互力，$J$為機械臂的雅可比矩陣，用於將關節空間速度映射到操作空間：$v=J\dot{\theta}$，雅可比矩陣的轉置也可將操作空間中的力映射到關節空間中：$\tau=J^TF$。對於本例，機器人雅可比矩陣可用MATLAB中的函數jacobian來計算，$J=jacobian(\mathbf{r_{ae}}, [\theta_1,\theta_2])$。$J^T(\theta)F_{tip}$代表作用於機器人關節上的外界環境力矩，等式左邊的$\tau$為機器人關節驅動力矩，計算出該力矩后就可以輸入給機器人驅動系統，實現期望的運動。
機械臂與環境交互產生的力矩可寫為$\tau_{ext}=J^T(\theta)F_{tip}=\underline{J^T(\theta)[M(\ddot{x_d}-\ddot{x})+D(\dot{x_d}-\dot{x})+K(x_d-x)]}$，$x_d$和$x$分別代表目標位置和實際位置，$\dot{x_d}$和$\dot{x}$分別代表目標速度和實際速度。矩陣$K$和$D$代表與環境交互的剛度和阻尼。機械臂末端的加速度$\ddot{x}$一般難以測量，如果直接對速度進行微分又會產生大量噪聲。通常對於協作型機械臂，自身重量設計的較輕，因此可以忽略其慣性，即令$M=0$。
下面開始對動力學系統進行迭代計算，按照仿真步長對求解出的角加速度進行積分，更新機器人的關節位置和速度，並在圖形界面中動態顯示。
改變剛度和阻尼系數后（Kp=100, Kd=10），明顯可以看出機器人剛性變大，在末端施加力只產生很小的形變，而且控制末端運動到目標位置時更迅速。
參考：
Impedance Control for a 2-Link Robot Arm
Numerical Integration in Games Development