機器人的力控制

1、LOGO 機器人的力控制 關節(jié)空間的力控制 力控制 柔順控制 主動阻抗控制 力和位置混合控制 笛卡爾空間的力控制 機器人的力控制，著重研究如何控制機器人的各個 關節(jié)使其末端表現出一定的力和力矩特性，是利用機器 人進行自動加工（如裝配等）的基礎。 一 二 三 主要內容 四 一、 剛度與柔順 為了達到期望的機器人末端位置和姿態(tài)，機器人所能夠表 現的力或力矩的能力。 關節(jié)的機械形變關節(jié)的機械形變 連桿的撓性（連桿的撓性（flexibility） 為了達到期望的關節(jié)位置，該關節(jié)所能夠表現的力或力矩 的能力。 關節(jié)的剛度關節(jié)的剛度 一、 剛度與柔順 指機器人的末端能夠對外力的變化作出相應的響應，表現 為

2、低剛度。 主動柔順（主動柔順（active compliance） 被動柔順（被動柔順（passive compliance） 是指不需要對機器人進行專門的控制即具有的柔順能力。 特點：特點： 柔順能力由機械裝置提供，只能用于特定的任務；響應速度 快，成本低。 是指通過對機器人進行專門的控制獲得的柔順能力。 通常，主動柔順通過控制機器人各關節(jié)的剛度，使機器人末端表現出 所需要的柔順性。 一、 剛度與柔順 主動柔順具有阻抗控制阻抗控制、力位混合控制力位混合控制和動態(tài)混合控制動態(tài)混合控制等類型。 在柔順坐標空間將任務分解為某些自由度的位置控制和另一些 自由度的力控制，然后將計算結果在關節(jié)空間合并為

3、統(tǒng)一的關 節(jié)力矩。 u 力位混合控制力位混合控制 u 阻抗控制阻抗控制 通過力與位置之間的動態(tài)關系實現柔順控制。阻抗控制的靜 態(tài)，即力和位置的關系，用剛性矩陣描述。阻抗控制的動態(tài)， 即力和速度的關系，用粘滯阻尼矩陣描述。 u 動態(tài)混合控制動態(tài)混合控制 分別組成位置控制回路和力控制回路，通過控制律的綜合實 現柔順控制。 柔順控制 笛為靜力矩。 關節(jié)空間的力或力矩與機器人末端的力或力矩具有 直接聯系。通常，靜力和靜力矩可以用6維矢量表示。 二、工業(yè)機器人的笛卡爾空間靜力與關節(jié)空間靜力的轉換 T xyzxyz Ffffmmm 關節(jié)為廣義力矢量， 控制 F xyz mmm 其中， xyz fff 為靜

4、力， 所謂靜力變換，是指機器人在靜止狀態(tài)下的力或力所謂靜力變換，是指機器人在靜止狀態(tài)下的力或力 矩的變換。矩的變換。 （1） 設基坐標系下廣義力 的虛擬位移為 ，如式（2）所示。 （2） 則廣義力 所做的虛功記為 ，見式（3）。 （3） 在坐標系 下 ，機器人所做的虛功 為 （4） 其中， 是機器人在坐標系 下的廣義力， 是機器人在坐標 系 下的虛擬位移。 F D T xyzxyz Dddd FW T WF D C C F CCT C WFD C F C CD C 由第二章式（2-197）可知，基坐標系下的虛擬位移 和坐標系 下的虛擬位移 之間存在如下關系。 D C C CD ()()() (

5、)()() ()()() 000 000 000 C xyzxyz x x C xyzxyz y y C xyzxyz zC z C xyz x x C xyz y y C xyz z z nnnpnpnpnd d ooop op op od d aaapapapad d DHD nnn ooo aaa （5） 1 () CT FHF H 機器人在基坐標系和坐標系 下所做的虛功相等。由式（3）、 （4）、（5）整理可得 其中，矩陣 為不同坐標系下微分變換的等價變換矩陣，見式（5）。 （6） 機器人在關節(jié)空間的虛功，可以表示為式（7） T qq WF dq （7） 其中， 是機器人在關節(jié)空間所做

6、的虛功； q W 12 T qn Ffff 是關節(jié)空間的虛擬位移。 是機器人關節(jié)空間的等效靜力或靜力矩； 12 T n dqdqdqdq 由第二章式（2-207）知，笛卡爾空間與關節(jié)空間的虛擬位移之 間存在如下關系 ( )DJ q dq 其中，為機器人的雅可比矩陣。( )J q （8） 考慮到機器人在笛卡爾空間與關節(jié)空間的虛功是等價的，由 式（3）、（7）和（8）可得 注：注：式（9）給出了機器人末端在笛卡爾空間的廣義靜力與關節(jié)空 間的靜力之間的等效關系，即笛卡爾空間與關節(jié)空間的靜力變換。 （9） ( )T q FJ qF 利用主動剛性控制，可以使特定方向的剛度降低或加強。圖1 為主動剛性控制

7、框圖。 圖中，是末端笛卡爾坐標系的剛性對稱矩陣，可以人為設定。 圖1. 主動剛性控制框圖 p K 注：注：該方案通過對關節(jié)位置的控制，使機器人末端表現出一定的 剛度。 對于關節(jié)空間的位置偏差 注注：當 時，關節(jié)空間的控制力或力矩為0。 當 時，關節(jié)空間具有一定的控制力或力矩，從而使 機器人末端表現出希望的剛度。 上述主動剛性控制的控制律為 機器人末端的位姿偏差。末端位姿偏差經過剛性對稱矩陣 ， d qq ，利用雅可比矩陣 將其轉換為 J 轉換為末端廣義力，再通過力變換轉換為關節(jié)空間的力或力矩。 p K () T pd J K J qq（20） 0 d qq 0 d qq 三、 阻抗控制主動柔順

8、 阻抗控制主動柔順，是指通過力與位置之間的動態(tài)關系 實現的柔順控制。 位置型阻抗控制位置型阻抗控制 力反饋型阻抗控制力反饋型阻抗控制 柔順型阻抗控制柔順型阻抗控制 三、 阻抗控制主動柔順 將利用力傳感器測量到的力信號引入位置控制系統(tǒng)，可以 構成力反饋型阻抗控制。圖2所示是一種力反饋型阻抗控制的 框圖。 圖2. 力反饋型阻抗控制 在不考慮力反饋通道時，圖2所示系統(tǒng)是一個基于雅可比矩陣的 增量式控制系統(tǒng)。它由位置控制位置控制和速度控制速度控制兩部分構成。 位置控制部分位置控制部分 力反饋引入位置控制和速度控制后，機器人末端表現出一定的柔順 性，其剛度降低，并具有粘滯阻尼特性。 速度控制部分速度控制

9、部分 以期望的位置 作為給定，位置反饋由關節(jié)位置利用運動學方程 計算獲得。 d x 以期望的速度 作為給定，速度反饋由關節(jié)速度利用雅可比矩陣 計算獲得。 d x 位置控制部分位置控制部分 由圖2可知，其輸出 為 其中： 為期望位置； d x 1 q 1 1 ( ) pdfp qK JxT qK F （21） 為機器人的運動學方程，即基坐標系到末端坐標系的變換矩陣； 是關節(jié)位置矢量； T 是機器人末端的廣義力； 是雅可比矩陣； 是位置控制部分的力與位置變換系數； 是位置控制系數。 q F J fp K p K 由第二章式（2-207）可知 u 該位置控制是建立在微分運動基礎上的。該位置控制是建立

10、在微分運動基礎上的。 u 該該位置控制為積分控制。位置控制為積分控制。 u 力反饋的引入降低了機器人末端的剛度。力反饋的引入降低了機器人末端的剛度。 采用增量輸出，使得該位置控制具有積分作用。 11 ( ) d dqJDJxT q 結合式（21）和式（22），可得出如下結論： 當機器人的當前位置與期望位置存在較大的偏差時，該位置 控制中的笛卡爾位置偏差與關節(jié)位置偏差的轉換將不準確。為 了避免系統(tǒng)振蕩，位置控制系數 不應選擇過大。 p K 當末端受到外力或力矩時，力反饋的引入使得位置可以存 在一定的偏差，從而使末端表現出柔順性。 越大，末端剛 度越低。 fp K （22） 速度控制部分速度控制部

11、分 由圖2可知，其輸出 為 其中： 為期望速度； d x 2 q 1 2vdfv qK JxJqK F （23） 是關節(jié)速度矢量； 是速度控制部分的力與位置變換系數； 是速度控制系數。 q fv K v K 一般地，雅可比矩陣 是關節(jié)位置矢量的函數。在關節(jié)位置矢量 的小鄰域內，可以認為 是常量。不考慮 的時變性，對式（2-207 ）求一階導數，得到式（24）。 J JJ 比較式（23）和式（24）可知，速度控制也是以微分運動為基礎 的，而且是以 在關節(jié)位置矢量的小鄰域內是常量為前提的。因此 ，速度控制的周期不應過長，以避免式（24）不成立，導致速度 估計不準確。另外，力反饋的引入增加了機器人末

12、端的速度控制的 粘滯阻尼。當末端受到外力或力矩時，力反饋的引入使得速度可以 存在一定的偏差，從而使末端表現出柔順性。 越大，末端的粘 滯阻尼越大。 fv K （24） 11 () dd dqJxxJxJq 位置控制部分的輸出 和速度控制部分的輸出 相加，作為 機器人的關節(jié)控制增量 ，用于控制機器人的運動。因此，圖2 所示的力反饋型阻抗控制，其本質上是以位置控制為基礎的。 值得注意的是，對于上述力反饋型阻抗控制，機器人末端的剛 度在一個控制周期內是不受控制的，即機器人末端在一個控制周期 內并不具有柔順性。 1 q 2 q q 位置型阻抗控制位置型阻抗控制，是指機器人末端沒有受到外力作用時，通過

13、位置與速度的協(xié)調而產生柔順性的控制方法。位置型阻抗控制， 根據位置偏差和速度偏差產生笛卡爾空間的廣義控制力，轉換 為關節(jié)空間的力或力矩后，控制機器人的運動。 原 理 圖3. 位置型阻抗控制 框圖 假設機器人的動力方程如下 位置型阻抗控制的控制律為 ( )HqCqg q 其中， 為慣量矩陣， 為阻尼矩陣， 為重力項， 為關節(jié) 空間的力或力矩矢量。 HC( )g q （25） ( ) () T pddd g qJKxxKxx （26） 其中， 為重力補償項， 為剛度系數矩陣， 為阻尼系數 矩陣， 為機器人的期望位置， 為機器人的期望速度， 為機器 人的當前位置， 為機器人的當前速度， 為機器人的力

14、矩矢量。 ( )gq p K d K d x d x x x 將式（26）代入式（25）中，得到位置型阻抗控制的動力學方程 （27） （28） ( )( )() T pddd HqCqg qg qJKxxKxx 如果重力補償項 能夠完全補償重力項 ，則動力學方程 由式（27）轉變?yōu)槭剑?8） ( )g q( )g q () T pddd HqCqJKxxKxx 由式（28）可知，當機器人的當前位置到達期望位置，當前速度 達到期望速度時， ， ，式（28）成為式 （29）。 0 d xx0 d xx 0HqCq 0J （29） 此時，機器人各關節(jié)不再提供除重力補償以外的力或力矩，機器 人處于無激

15、勵的平衡狀態(tài)。另外，當機器人處于奇異位置時， 。 此時，機器人也處于無激勵的平衡狀態(tài)，但位置和速度均可能存 在誤差。 為驗證系統(tǒng)的穩(wěn)定性，建立式（30）所示的正定Lyapunov函數。 （30） （31） 穩(wěn)定性分析 1 2 TT xpx Ve K eq Hq 其中， 。xd exx 對式（30）求導數，并將式（28）代入，得 TTTT xpxpxDx Ve K eq JK eK eq Cq 考慮 為常數的情況。此時，有下式成立 d x TTTT x exq J 0 TT xDx Ve K eq Cq （32） 將式（32）代入式（31）中，得 （33） （34） 對式（34）求導數，并將式（

16、29）代入，得 因此，當 時， 是漸進穩(wěn)定的，但不能保證 。其物理意 義是，當機器人處于奇異狀態(tài)時，雖然機器人末端在位置和速度上 都可能存在誤差，但因計算出的關節(jié)力或力矩為0，機器人中止運 動。 由于 且 ，根據Lyapunov穩(wěn)定性定理，系統(tǒng)是穩(wěn)定的。 上述結論是在 的前提下獲得的。當 時，由式（31）可 知， 不能保證小于等于0。 0V 0V 0J 0J V 對于 時的情況，可以建立式（34）所示的正定Lyapunov函數0J 1 2 T Vq Hq 0 T Vq Cq 0J q 0 x e （35） 柔順型阻抗控制，柔順型阻抗控制，是指機器人末端收到環(huán)境的外力作用時，通過 位置與外力的協(xié)

17、調而產生柔順性的控制方法。柔順型阻抗控制， 根據環(huán)境外力、位置偏差和速度偏差產生笛卡爾空間的廣義控制 力，轉換為關節(jié)空間的力或力矩后，控制機器人的運動。柔順型 阻抗控制與位置型阻抗控制相比，只是在笛卡爾空間的廣義控制 力中增加了環(huán)境力。 原 理 圖4. 柔順型阻抗控制 框圖 當機器人的末端接觸彈性目標時，目標會由于彈性變形而產生彈力， 作用于機器人的末端。在彈性目標被機器人末端擠壓時，機器人末 端位置與彈性目標原表面位置的偏差即為變形量。顯然，當機器人 末端尚未到達彈性目標時，雖然機器人末端位置與彈性目標表面位 置之間存在偏差，但彈性目標的變形量為零。為了便于對目標的變 形量進行描述，定義一個

18、正定函數，如式（36）所示。 在式（26）基礎上，將彈力引入機器人的阻抗控制，得到柔順型阻 抗控制的控制律 （37） （36） ,0 ( ) 0,0 xx P x x ( ) ()() T pdddfe g qJKxxKxxK P xx 其中， 為環(huán)境力系數矩陣， 為彈性目標表面原位置。 f K e x （38） 將式（37）代入式（25）中，如果重力補償項 能夠完全補 償重力項 ，則動力學方程轉變?yōu)槭剑?8） ( )g q ( )g q 由式（38）可知，當機器人的當前位置到達期望位置，當前速度 達到期望速度，彈性目標無形變時， ， ， ，式（38）成為式（29）。 0 d xx0 d xx

19、 0J 此時，機器人各關節(jié)不再提供除重力補償以外的力或力矩，機器 人處于無激勵的平衡狀態(tài)。另外，當機器人處于奇異位置時， 。 此時，機器人也處于無激勵的平衡狀態(tài)，但位置和速度均可能存 在誤差，彈性目標也可能存在變形。 ()() T pdddfe HqCqJKxxKxxK P xx 0 e xx 為驗證系統(tǒng)的穩(wěn)定性，建立式（39）所示的正定Lyapunov函數。 （39） （40） 穩(wěn)定性分析 1 2 TTT xpxxefxe Ve K eq Hqe K e 其中， ， 。xd exx 對式（39）求導數，并將式（38）代入，得 () TTTTT xpxpxDxfxexefxe Ve K eq

20、JK eK eK P eq Cqe K e 考慮 和 為常數的情況。此時，除式（32）成立外，還有下式 成立 d x TTTT xe exq J V （41） 將式（32）和式（41）代入式（40）中，得式（33）所示的 的表達式。 xee exx e x 由于 且 ，根據Lyapunov穩(wěn)定性定理，系統(tǒng)是穩(wěn)定的。0V 0V （42） 表面上看，式（42）在兩種情況下成立，一種情況為 ，另一種情況為 。當 時，機 器人停止運動， 。此時，式（38）變成式（43） 考察 時的情況，由式（39）可知0V （35） 0 TTTTTTTT pxfxepxfxe Vq J K eq Hqq J K eq

21、J K eHqJ K e 0 TT pxfxe J K eHqJ K e 0q 0 x T pdfe HqJKxxK P xx 0q 由式（43）可知，當 時， 也同 樣成立。由此可見，當 且 時， ， 。此時 ，系統(tǒng)也處于無激勵的平衡狀態(tài)。這說明，在環(huán)境剛度與機械手阻 力間的組合作用下，系統(tǒng)能夠達到平衡狀態(tài)。換言之，在位置與外 力的協(xié)調作用下，機器人末端表現出柔順性。 越大，末端的柔順 性越大。 0q 0 TT pxfxe J K eHqJ K e 0q pxfxe K eK e 0q 0V f K 四、 力和位置混合控制 力位混合柔順控制，是指分別組成位置控制回路和力控制 回路，通過控制律

22、的綜合實現的柔順控制。 改進的改進的R-C力和位置混合控制力和位置混合控制 R-C力和位置混合控制力和位置混合控制 圖5所示的控制方案是由Raibert和Craig于1981年提出的，稱為 R-C力和位置混合控制。該控制方案由兩大部分組成，分別為位置位置 /速度控制部分速度控制部分和力控制部分力控制部分。 圖5. R-C力位混合控制 位置位置/速度控制部分速度控制部分 由位置和速度兩個通道構成。 位置通道位置通道以末端期望的笛卡爾空間位置 作為給定，位置反饋由關 節(jié)位置利用運動學方程計算獲得。利用雅可比矩陣，將笛卡爾空間 的位姿偏差轉換為關節(jié)空間的位置偏差，經過PI運算后作為關節(jié)控 制力或力矩

23、的一部分。 速度通道速度通道以末端期望的笛卡爾空間速度 作為給定，速度反饋由關 節(jié)速度利用雅可比矩陣計算獲得。同樣地，速度通道利用雅可比矩 陣，將笛卡爾空間的速度偏差轉換為關節(jié)空間的速度偏差。然后， 經過比例運算，其結果作為關節(jié)控制力或力矩的一部分。 為位置/ 速度控制部分各個分量的選擇矩陣，用于對各個分量的作用大小進 行選擇，表現在機器人末端為各個分量的柔順性不同。 d x d x p C 位置/速度控制部分產生的關節(jié)空間力或力矩，見式（44） 其中： 為期望位置； d x （44） 為機器人的運動學方程，即基坐標系到末端坐標系的變換矩陣；T 是位置/速度控制部分產生的關節(jié)空間力或力矩； 是

24、雅可比矩陣； 是位置通道的積分系數； 是位置和速度通道的選擇矩陣。 是關節(jié)位置矢量； q p J pp K p C 11 ( ) ppppipdpdpd KKs J CxT qKJ CxJq 是位置通道的比例系數； pi K 是速度通道的比例系數； pd K 是關節(jié)位置矢量； q 為期望速度；d x 力控制部分力控制部分 由PI和力前饋兩個通道構成。 PI通道以機器人末端期望的笛卡爾空間廣義力 作為給定，力反饋 由力傳感器測量獲得。利用雅可比矩陣，將笛卡爾空間的力偏差轉 換為關節(jié)空間的力偏差，經過PI運算后作為關節(jié)控制力或力矩的一 部分。 力前饋通道直接利用雅可比矩陣將 轉換到關節(jié)空間，作為關

25、節(jié)控 制力或力矩的一部分。力前饋通道的作用是加快系統(tǒng)對期望力 的 響應速度。 為力控制部分各個分量的選擇矩陣，用于對各個分量 的作用大小進行選擇。 f C d F d F d F 力控制部分產生的關節(jié)空間力或力矩，見式（45） 其中： 機器人關節(jié)空間的力或力矩是位置/速度控制部分和力控制部分 產生的力或力矩之和。 pf （45） 為期望的機器人末端在笛卡爾空間的廣義力； d F 為機器人末端當前的廣義力；F 是力控制部分產生的關節(jié)空間力或力矩； 是力通道的積分系數； 是力控制部分的選擇矩陣。 f fp K f C TT ffpfifdfbfd KKs J CFK FJ C F 是力通道的比例系

26、數； ft K 為測量得到的廣義力； fb KF （46） 圖5所示的力和位置混合控制方案，未考慮機械手動態(tài)耦合影 響，在工作空間的某些奇異位置上出現不穩(wěn)定。圖6為改進的R-C 力和位置混合控制方案。 圖6. 改進的R-C力位混合控制 其改進主要體現在以下幾個方面： （1）考慮機械手的動態(tài)影響，并對機械手所受的重力、哥氏力和向 心力進行補償。如圖6中的 ，以及位置/速度/加速度控 制部分增加的慣量矩陣 。 （2）考慮力控制系統(tǒng)的欠阻尼特性，在力控制回路中加入阻尼反饋， 以消弱振蕩因素。如圖6中的 通道，其信號取自機器人的當 前速度 。 （3）引入加速度前饋，以滿足作業(yè)任務對加速度的要求，也可使