機器人力控制

1、LOGO機器人的力控制關(guān)節(jié)空間的力控制力控制柔順控制主動阻抗控制力和位置混合控制笛卡爾空間的力控制 機器人的力控制，著重研究如何控制機器人的各個關(guān)節(jié)使其末端表現(xiàn)出一定的力和力矩特性，是利用機器人進行自動加工（如裝配等）的基礎(chǔ)。一二三 主要內(nèi)容一、 剛度與柔順 為了達到期望的機器人末端位置和姿態(tài)，機器人所能夠表現(xiàn)的力或力矩的能力。關(guān)節(jié)的機械形變關(guān)節(jié)的機械形變連桿的撓性（連桿的撓性（flexibility） 為了達到期望的關(guān)節(jié)位置，該關(guān)節(jié)所能夠表現(xiàn)的力或力矩的能力。關(guān)節(jié)的剛度關(guān)節(jié)的剛度一、 剛度與柔順 指機器人的末端能夠?qū)ν饬Φ淖兓鞒鱿鄳捻憫?，表現(xiàn)為低剛度。主動柔順（主動柔順（active c

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

3、與位置之間的動態(tài)關(guān)系實現(xiàn)柔順控制。阻抗控制的靜態(tài)，即力和位置的關(guān)系，用剛性矩陣描述。阻抗控制的動態(tài)，即力和速度的關(guān)系，用粘滯阻尼矩陣描述。u 動態(tài)混合控制動態(tài)混合控制分別組成位置控制回路和力控制回路，通過控制律的綜合實現(xiàn)柔順控制。柔順控制笛為靜力矩。 關(guān)節(jié)空間的力或力矩與機器人末端的力或力矩具有直接聯(lián)系。通常，靜力和靜力矩可以用6維矢量表示。二、工業(yè)機器人的笛卡爾空間靜力與關(guān)節(jié)空間靜力的轉(zhuǎn)換TxyzxyzFfffmmm關(guān)節(jié)為廣義力矢量，控制Fxyzmmm其中，xyzfff為靜力， 所謂靜力變換，是指機器人在靜止狀態(tài)下的力或力所謂靜力變換，是指機器人在靜止狀態(tài)下的力或力矩的變換。矩的變換。（1）

4、 設基坐標系下廣義力 的虛擬位移為 ，如式（2）所示。 （2）則廣義力 所做的虛功記為 ，見式（3）。 （3） 在坐標系 下 ，機器人所做的虛功 為 （4）其中， 是機器人在坐標系 下的廣義力， 是機器人在坐標系 下的虛擬位移。FDTxyzxyzDdddFWTWF D CCFCCT CWFDCF CCD C 由第二章式（2-197）可知，基坐標系下的虛擬位移 和坐標系 下的虛擬位移 之間存在如下關(guān)系。D C CCD()()()()()()()()()000000000CxyzxyzxxCxyzxyzyyCxyzxyzzCzCxyzxxCxyzyyCxyzzznnnpnpnpnddooop op

5、 op oddaaapapapaddDHDnnnoooaaa （5）1()CTFHFH 機器人在基坐標系和坐標系 下所做的虛功相等。由式（3）、（4）、（5）整理可得 其中，矩陣 為不同坐標系下微分變換的等價變換矩陣，見式（5）。（6） 機器人在關(guān)節(jié)空間的虛功，可以表示為式（7） TqqWF dq（7）其中， 是機器人在關(guān)節(jié)空間所做的虛功；qW12TqnFfff是關(guān)節(jié)空間的虛擬位移。是機器人關(guān)節(jié)空間的等效靜力或靜力矩；12Tndqdqdqdq 由第二章式（2-207）知，笛卡爾空間與關(guān)節(jié)空間的虛擬位移之間存在如下關(guān)系( )DJ q dq其中，為機器人的雅可比矩陣。( )J q（8） 考慮到機器

6、人在笛卡爾空間與關(guān)節(jié)空間的虛功是等價的，由式（3）、（7）和（8）可得注：注：式（9）給出了機器人末端在笛卡爾空間的廣義靜力與關(guān)節(jié)空間的靜力之間的等效關(guān)系，即笛卡爾空間與關(guān)節(jié)空間的靜力變換。（9）( )TqFJ qF 利用主動剛性控制，可以使特定方向的剛度降低或加強。圖1為主動剛性控制框圖。圖中，是末端笛卡爾坐標系的剛性對稱矩陣，可以人為設定。圖1. 主動剛性控制框圖pK注：注：該方案通過對關(guān)節(jié)位置的控制，使機器人末端表現(xiàn)出一定的 剛度。對于關(guān)節(jié)空間的位置偏差注注：當 時，關(guān)節(jié)空間的控制力或力矩為0。 當 時，關(guān)節(jié)空間具有一定的控制力或力矩，從而使機器人末端表現(xiàn)出希望的剛度。上述主動剛性控制的

7、控制律為機器人末端的位姿偏差。末端位姿偏差經(jīng)過剛性對稱矩陣 ，dqq，利用雅可比矩陣 將其轉(zhuǎn)換為J轉(zhuǎn)換為末端廣義力，再通過力變換轉(zhuǎn)換為關(guān)節(jié)空間的力或力矩。pK()TpdJ K J qq（20）三、 阻抗控制主動柔順 阻抗控制主動柔順，是指通過力與位置之間的動態(tài)關(guān)系實現(xiàn)的柔順控制。位置型阻抗控制位置型阻抗控制力反饋型阻抗控制力反饋型阻抗控制柔順型阻抗控制柔順型阻抗控制三、 阻抗控制主動柔順 將利用力傳感器測量到的力信號引入位置控制系統(tǒng)，可以構(gòu)成力反饋型阻抗控制。圖2所示是一種力反饋型阻抗控制的框圖。圖2. 力反饋型阻抗控制在不考慮力反饋通道時，圖2所示系統(tǒng)是一個基于雅可比矩陣的增量式控制系統(tǒng)。它

8、由位置控制位置控制和速度控制速度控制兩部分構(gòu)成。位置控制部分位置控制部分力反饋引入位置控制和速度控制后，機器人末端表現(xiàn)出一定的柔順性，其剛度降低，并具有粘滯阻尼特性。速度控制部分速度控制部分以期望的位置 作為給定，位置反饋由關(guān)節(jié)位置利用運動學方程計算獲得。dx以期望的速度 作為給定，速度反饋由關(guān)節(jié)速度利用雅可比矩陣計算獲得。dx 位置控制部分位置控制部分由圖2可知，其輸出 為其中： 為期望位置；dx1q11( )pdfpqK JxT qK F（21）為機器人的運動學方程，即基坐標系到末端坐標系的變換矩陣；是關(guān)節(jié)位置矢量；T是機器人末端的廣義力；是雅可比矩陣；是位置控制部分的力與位置變換系數(shù)；是

9、位置控制系數(shù)。qFJfpKpK由第二章式（2-207）可知 u 該位置控制是建立在微分運動基礎(chǔ)上的。該位置控制是建立在微分運動基礎(chǔ)上的。u 該位置控制為積分控制。該位置控制為積分控制。u 力反饋的引入降低了機器人末端的剛度。力反饋的引入降低了機器人末端的剛度。采用增量輸出，使得該位置控制具有積分作用。11( )ddqJDJxT q結(jié)合式（21）和式（22），可得出如下結(jié)論： 當機器人的當前位置與期望位置存在較大的偏差時，該位置控制中的笛卡爾位置偏差與關(guān)節(jié)位置偏差的轉(zhuǎn)換將不準確。為了避免系統(tǒng)振蕩，位置控制系數(shù) 不應選擇過大。pK 當末端受到外力或力矩時，力反饋的引入使得位置可以存在一定的偏差，從

10、而使末端表現(xiàn)出柔順性。 越大，末端剛度越低。fpK（22）速度控制部分速度控制部分由圖2可知，其輸出 為其中： 為期望速度；dx 2q12vdfvqK JxJqK F（23）是關(guān)節(jié)速度矢量；是速度控制部分的力與位置變換系數(shù)；是速度控制系數(shù)。q fvKvK 一般地，雅可比矩陣 是關(guān)節(jié)位置矢量的函數(shù)。在關(guān)節(jié)位置矢量的小鄰域內(nèi)，可以認為 是常量。不考慮 的時變性，對式（2-207）求一階導數(shù)，得到式（24）。JJJ比較式（23）和式（24）可知，速度控制也是以微分運動為基礎(chǔ)的，而且是以 在關(guān)節(jié)位置矢量的小鄰域內(nèi)是常量為前提的。因此，速度控制的周期不應過長，以避免式（24）不成立，導致速度估計不準確。

11、另外，力反饋的引入增加了機器人末端的速度控制的粘滯阻尼。當末端受到外力或力矩時，力反饋的引入使得速度可以存在一定的偏差，從而使末端表現(xiàn)出柔順性。 越大，末端的粘滯阻尼越大。fvK（24）11()dddqJxxJxJq 位置控制部分的輸出 和速度控制部分的輸出 相加，作為機器人的關(guān)節(jié)控制增量 ，用于控制機器人的運動。因此，圖2所示的力反饋型阻抗控制，其本質(zhì)上是以位置控制為基礎(chǔ)的。 值得注意的是，對于上述力反饋型阻抗控制，機器人末端的剛度在一個控制周期內(nèi)是不受控制的，即機器人末端在一個控制周期內(nèi)并不具有柔順性。位置型阻抗控制位置型阻抗控制，是指機器人末端沒有受到外力作用時，通過位置與速度的協(xié)調(diào)而產(chǎn)

12、生柔順性的控制方法。位置型阻抗控制，根據(jù)位置偏差和速度偏差產(chǎn)生笛卡爾空間的廣義控制力，轉(zhuǎn)換為關(guān)節(jié)空間的力或力矩后，控制機器人的運動。原 理圖3. 位置型阻抗控制框圖假設機器人的動力方程如下位置型阻抗控制的控制律為 ( )HqCqg q其中， 為慣量矩陣， 為阻尼矩陣， 為重力項， 為關(guān)節(jié)空間的力或力矩矢量。HC( )g q（25）( )()Tpdddg qJKxxKxx（26）其中， 為重力補償項， 為剛度系數(shù)矩陣， 為阻尼系數(shù)矩陣， 為機器人的期望位置， 為機器人的期望速度， 為機器人的當前位置， 為機器人的當前速度， 為機器人的力矩矢量。 ()gqpKdKdxdx xx 將式（26）代入式

13、（25）中，得到位置型阻抗控制的動力學方程（27）（28）( )( )()TpdddHqCqg qg qJKxxKxx如果重力補償項 能夠完全補償重力項 ，則動力學方程由式（27）轉(zhuǎn)變?yōu)槭剑?8） ()g q( )g q()TpdddHqCqJKxxKxx由式（28）可知，當機器人的當前位置到達期望位置，當前速度達到期望速度時， ， ，式（28）成為式（29）。0dxx0dxx0HqCq0J （29）此時，機器人各關(guān)節(jié)不再提供除重力補償以外的力或力矩，機器人處于無激勵的平衡狀態(tài)。另外，當機器人處于奇異位置時， 。此時，機器人也處于無激勵的平衡狀態(tài)，但位置和速度均可能存在誤差。為驗證系統(tǒng)的穩(wěn)定性

14、，建立式（30）所示的正定Lyapunov函數(shù)。（30）（31）穩(wěn)定性分析12TTxpxVe K eq Hq其中， 。xdexx對式（30）求導數(shù)，并將式（28）代入，得TTTTxpxpxDxVe K eq JK eK eq Cq考慮 為常數(shù)的情況。此時，有下式成立dxTTTTxexq J 0TTxDxVe K eq Cq （32）將式（32）代入式（31）中，得（33）（34）對式（34）求導數(shù)，并將式（29）代入，得 因此，當 時， 是漸進穩(wěn)定的，但不能保證 。其物理意義是，當機器人處于奇異狀態(tài)時，雖然機器人末端在位置和速度上都可能存在誤差，但因計算出的關(guān)節(jié)力或力矩為0，機器人中止運動。由

15、于 且 ，根據(jù)Lyapunov穩(wěn)定性定理，系統(tǒng)是穩(wěn)定的。上述結(jié)論是在 的前提下獲得的。當 時，由式（31）可知， 不能保證小于等于0。0V 0V 0J 0J V對于 時的情況，可以建立式（34）所示的正定Lyapunov函數(shù)0J 12TVq Hq0TVq Cq 0J q0 xe （35）柔順型阻抗控制，柔順型阻抗控制，是指機器人末端收到環(huán)境的外力作用時，通過位置與外力的協(xié)調(diào)而產(chǎn)生柔順性的控制方法。柔順型阻抗控制，根據(jù)環(huán)境外力、位置偏差和速度偏差產(chǎn)生笛卡爾空間的廣義控制力，轉(zhuǎn)換為關(guān)節(jié)空間的力或力矩后，控制機器人的運動。柔順型阻抗控制與位置型阻抗控制相比，只是在笛卡爾空間的廣義控制力中增加了環(huán)境力

16、。原 理圖4. 柔順型阻抗控制框圖當機器人的末端接觸彈性目標時，目標會由于彈性變形而產(chǎn)生彈力，作用于機器人的末端。在彈性目標被機器人末端擠壓時，機器人末端位置與彈性目標原表面位置的偏差即為變形量。顯然，當機器人末端尚未到達彈性目標時，雖然機器人末端位置與彈性目標表面位置之間存在偏差，但彈性目標的變形量為零。為了便于對目標的變形量進行描述，定義一個正定函數(shù)，如式（36）所示。在式（26）基礎(chǔ)上，將彈力引入機器人的阻抗控制，得到柔順型阻抗控制的控制律（37）（36）,0( )0,0 xxP xx( )()()Tpdddfeg qJKxxKxxK P xx其中， 為環(huán)境力系數(shù)矩陣， 為彈性目標表面原

17、位置。fK（38）將式（37）代入式（25）中，如果重力補償項 能夠完全補償重力項 ，則動力學方程轉(zhuǎn)變?yōu)槭剑?8） ()g q( )g q由式（38）可知，當機器人的當前位置到達期望位置，當前速度達到期望速度，彈性目標無形變時， ， ， ，式（38）成為式（29）。0dxx0dxx0J 此時，機器人各關(guān)節(jié)不再提供除重力補償以外的力或力矩，機器人處于無激勵的平衡狀態(tài)。另外，當機器人處于奇異位置時， 。此時，機器人也處于無激勵的平衡狀態(tài)，但位置和速度均可能存在誤差，彈性目標也可能存在變形。()()TpdddfeHqCqJKxxKxxK P 為驗證系統(tǒng)的穩(wěn)定性，建立式（39）所示的正定Lyapuno

18、v函數(shù)。（39）（40）穩(wěn)定性分析12TTTxpxxefxeVe K eq Hqe K e其中， ， 。xdexx對式（39）求導數(shù)，并將式（38）代入，得()TTTTTxpxpxDxfxexefxeVe K eq JK eK eK P eq Cqe K e考慮 和 為常數(shù)的情況。此時，除式（32）成立外，還有下式成立dxTTTTxeexq JV（41）將式（32）和式（41）代入式（40）中，得式（33）所示的的表達式。xeeexxex由于 且 ，根據(jù)Lyapunov穩(wěn)定性定理，系統(tǒng)是穩(wěn)定的。0V 0V （42） 表面上看，式（42）在兩種情況下成立，一種情況為 ，另一種情況為 。當 時，機

19、器人停止運動， 。此時，式（38）變成式（43）考察 時的情況，由式（39）可知0V （35）0TTTTTTTTpxfxepxfxeVq J K eq Hqq J K eqJ K eHqJ K e 0TTpxfxeJ K eHqJ K e0q 0 x TpdfeHqJKxxK P xx0q 由式（43）可知，當 時， 也同樣成立。由此可見，當 且 時， ， 。此時，系統(tǒng)也處于無激勵的平衡狀態(tài)。這說明，在環(huán)境剛度與機械手阻力間的組合作用下，系統(tǒng)能夠達到平衡狀態(tài)。換言之，在位置與外力的協(xié)調(diào)作用下，機器人末端表現(xiàn)出柔順性。 越大，末端的柔順性越大。0q 0TTpxfxeJ K eHqJ K e0q

20、pxfxeK eK e0q 0V fK四、 力和位置混合控制 力位混合柔順控制，是指分別組成位置控制回路和力控制回路，通過控制律的綜合實現(xiàn)的柔順控制。改進的改進的R-C力和位置混合控制力和位置混合控制 R-C力和位置混合控制力和位置混合控制 圖5所示的控制方案是由Raibert和Craig于1981年提出的，稱為R-C力和位置混合控制。該控制方案由兩大部分組成，分別為位置位置/速度控制部分速度控制部分和力控制部分力控制部分。圖5. R-C力位混合控制位置位置/速度控制部分速度控制部分由位置和速度兩個通道構(gòu)成。位置通道位置通道以末端期望的笛卡爾空間位置 作為給定，位置反饋由關(guān)節(jié)位置利用運動學方程

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

22、末端坐標系的變換矩陣；T是位置/速度控制部分產(chǎn)生的關(guān)節(jié)空間力或力矩；是雅可比矩陣；是位置通道的積分系數(shù)；是位置和速度通道的選擇矩陣。是關(guān)節(jié)位置矢量；qpJppKpC11( )ppppipdpdpdKKs J CxT qKJ CxJq是位置通道的比例系數(shù)；piK是速度通道的比例系數(shù)；pdK是關(guān)節(jié)位置矢量；q 為期望速度；dx 力控制部分力控制部分由PI和力前饋兩個通道構(gòu)成。PI通道以機器人末端期望的笛卡爾空間廣義力 作為給定，力反饋由力傳感器測量獲得。利用雅可比矩陣，將笛卡爾空間的力偏差轉(zhuǎn)換為關(guān)節(jié)空間的力偏差，經(jīng)過PI運算后作為關(guān)節(jié)控制力或力矩的一部分。力前饋通道直接利用雅可比矩陣將 轉(zhuǎn)換到關(guān)節(jié)

23、空間，作為關(guān)節(jié)控制力或力矩的一部分。力前饋通道的作用是加快系統(tǒng)對期望力 的響應速度。 為力控制部分各個分量的選擇矩陣，用于對各個分量的作用大小進行選擇。fCdFdFdF力控制部分產(chǎn)生的關(guān)節(jié)空間力或力矩，見式（45）其中： 機器人關(guān)節(jié)空間的力或力矩是位置/速度控制部分和力控制部分產(chǎn)生的力或力矩之和。pf（45）為期望的機器人末端在笛卡爾空間的廣義力；dF為機器人末端當前的廣義力；F是力控制部分產(chǎn)生的關(guān)節(jié)空間力或力矩；是力通道的積分系數(shù)；是力控制部分的選擇矩陣。ffpKfCTTffpfifdfbfdKKs J CFK FJ C F是力通道的比例系數(shù)；ftK為測量得到的廣義力；fbKF（46） 圖5所示的力和位置混合控制方案，未考慮機械手動態(tài)耦合影響，在工作空間的某些奇異位置上出現(xiàn)不穩(wěn)定。圖6為改進的R-C力和位置混合控制方案。圖6. 改進的R-C力位混合控制其改進主要體現(xiàn)在以下幾個方面：（1）考慮機械手的動態(tài)影響，并對機械手所受的重力、哥氏力和向心力進行補償。如圖6中的 ，以及位置/速度/加速度控制部分增加的慣量矩陣 。（2）考慮力控制系統(tǒng)的欠阻尼特性，在力控制回路中加入阻尼反饋，以消弱振蕩因素。如圖6中的 通道，其信號取自機器人的當前速度 。（3）引入加速度前饋，以滿足作業(yè)任務對加速度的要求，也可使速度