目前用於噴漆、搬運、點焊等操作的工業機器人只具有簡單的軌跡控制。軌跡控制適用於機器人的末端執行器在空間沿某一規定的路徑運動,在運動過程中末端執行器不與任何外界物體接觸。對於執行擦玻璃、轉動曲柄、擰螺絲、研磨、打毛刺、裝配零件等作業的機器人,其末端執行器與環境之間存在力的作用,且環境中的各種因素不確定,此時僅使用軌跡控制就不能滿足要求。執行這些任務時必須讓機器人末端執行器沿着預定的軌跡運動,同時提供必要的力使它能克服環境中的阻力或符合工作環境的要求。
以擦玻璃為例,如果機器人手爪抓着一塊很大很軟的海綿,並且知道玻璃的精確位置,那么通過控制手爪相對於玻璃的位置可以完成擦玻璃作業;但如果作業是用刮刀刮去玻璃表面上的油漆,而且玻璃表面空間位置不准確,或者手爪的位置誤差比較大,由於存在沿垂直玻璃表面的誤差,作業執行的結果不是刮刀接觸不到玻璃就是刮刀把玻璃打碎。因此,根據玻璃位置來控制擦玻璃機器人是行不通的。比較好的方法是控制工具與玻璃之間的接觸力,這樣即便是工作環境(如玻璃)位置不准確時也能保持工具與玻璃正確接觸。相應地,機器人不但要有軌跡控制的功能,而且要有力控制的功能。
下面是一個機器人開門的例子。如圖所示,門把手的運動軌跡受到自然約束(機器人手爪與環境接觸時,環境的幾何特性構成對作業的約束),被限制在以轉軸為圓心半徑為R的圓弧上。傳統的軌跡控制下為了保證機器人不把門或門把手扯掉,需要機器人沿着准確的軌跡運動,否則徑向上很小的誤差都會產生很大的力。
如果只考慮靜態,力和位置關系可用剛性矩陣來描述。如果考慮力和速度之間的關系,可用粘滯阻力矩陣來描述。通常對於需要進行位置控制的自由度,則要求在該方向上有很大的剛性,即表現出很硬的特性;對於需要力控制的自由度,則要求在該方向上有較小的剛性,即表現出較軟的特性。
對上面的問題,可以在徑向賦予機器人較小的剛性(較大的柔性)。添加虛擬彈簧,將其剛度系數$k_x$設的很小,則當實際軌跡與期望軌跡不一致時彈簧產生的回復力也很小,不至於將門破壞掉。機器人末端與環境間的剛度矩陣可以定義為:$$\mathbf{K}=\begin{pmatrix}k_x & 0\\ 0 & k_y\end{pmatrix};k_x\ll 1,k_y\gg 1$$
通過雅可比矩陣可以將機器人末端剛度轉換到關節空間中。設雅可比矩陣$\pmb{J}$將關節空間的速度$\dot{\pmb{q}}$映射到操作空間$\dot{\pmb{p}}$。末端剛度矩陣為$\pmb{K}_p$,$\Delta\pmb{p}$為末端位移變化量,$\pmb{F}$為末端位移變化引起的力。則有:$\pmb{F}=\pmb{K}_p\Delta\pmb{p}$
關節空間剛度矩陣$\mathbf{K}_q$可以根據雅可比矩陣計算出:$$\mathbf{K}_q=\mathbf{J}^T\mathbf{K}_p\mathbf{J}$$
簡單推理過程為:$\pmb{\tau}=\pmb{J}^T\pmb{F}=\pmb{J}^T\pmb{K}_p\Delta\pmb{p}=\pmb{J}^T\pmb{K}_p\pmb{J}\Delta\pmb{q}$,根據$\pmb{\tau}=\pmb{K}_q\Delta\pmb{q}$,即可以推導出上面關系式。
柔順性分為主動柔順性和被動柔順性兩類。機器人憑借一些輔助的柔順機構,使其在與環境接觸時能夠對外部作用力產生自然順從,稱為被動柔順性。所謂被動柔順機構,即利用一些可以使機器人在與環境作用時能夠吸收或儲存能量的機構器件如彈簧、阻尼等構成的機構。機器人利用力的反饋信息采用一定的控制策略去主動控制作用力,稱為主動柔順性。
機器人的阻抗控制(Impedance Control)或導納控制(Admittance Control)是通過調節機器人末端位置與力之間的動態特性來實現柔順性。機器人與外界環境之間的交互可以由質量-彈簧-阻尼系統來描述,如下圖所示,系統動力學方程為:$$m\ddot{x}+b\dot{x}+kx=f$$
其中$x$為位移,$m$為質量,$b$為阻尼,$k$為剛度,$f$為作用力
對上面的動力學方程進行拉普拉斯變換,可以得到:$$(ms^2+bs+k)X(s)=F(s)$$
阻抗定義為$Z(s)=F(s)/X(s)$,導納定義為阻抗的倒數:$Y(s)=Z^{-1}(s)=X(s)/F(s)$.由於$\Delta X=Y\Delta F$,如果導納很小,那么力的變化$\Delta F$將產生很小的位移變化$\Delta X$。
阻抗控制是一種建立在力與速度或位置之間的動態關系上的控制方法。通過建立機器人運動和外部作用力之間的動態關系,對機器人運動進行調節,從而實現機器人與環境的動態調整。$$M\ddot{x}+B\dot{x}+Kx=f_{ext}$$
對上面的方程有兩種解讀角度:
- 阻抗控制:當機器人末端產生位置偏差量$X$時,控制關節力矩使得其末端產生阻抗力$f_{ext}$
- 導納控制:當機器人通過力/力矩傳感器感知到外部作用力$f_{ext}$時,控制末端位移或速度
低阻尼參數可以提高機器人對操作者交互力的跟隨能力,使得操作者可以通過較小的作用力就能使機器人跟隨其做柔順移動。但較低的阻尼參數卻限制了拖動至目標點時交互的穩定性,機器人的位置會出現過沖
通過調整 K,D 參數,可以改變實驗特性,增加 K 時,單位作用力上產生的位移就變小,即需要用更大的力才能拉動關節;反之,減少 K 時,單位作用力上產生的位移就變大,用很小的力就能使關節產生較大的位移。增加 D 時,相當於增加系統的阻尼特性,可以進一步的提高系統運動時的平穩性,但如果 D 很大,可能出現系統響應很慢的情況,不能很好的跟隨所施加的外力信息;另一方面,減少 D 時,可以適當的提高系統的快速性,但是過小的D 會引起系統的不穩定現象。
參考:
Robot Manipulator Control Theory and Practice
Impedance and Interaction Control - Summer School on Impedance