機(jī)器人入門知識(shí)(課件)第10章力控與順應(yīng)控制

第十章機(jī)器人的力控和順應(yīng)控制

ChapterⅩForceControlandCompliancefor

RobotManipulators10.1引言10.2通用機(jī)器人控制器和控制結(jié)構(gòu)10.3通用機(jī)器人的動(dòng)力學(xué)10.4阻抗控制10.5主動(dòng)剛度控制10.6位置和力的混合控制2/4/20231智能與控制工程研究所10.1引言（Introduction）工業(yè)機(jī)器人的控制可大致分為三種形式位置控制（PositionControl）力控（ForceControl）順應(yīng)控制（Compliance）2/4/20232智能與控制工程研究所10.1.1位置控制（PositionControl）

位置控制是在預(yù)先指定的坐標(biāo)系上,對(duì)機(jī)器人末端執(zhí)行器（endeffector）的位置和姿態(tài)（方向）的控制。如圖10-1所示，末端執(zhí)行器的位置和姿態(tài)是在三維空間描述的，包括三個(gè)平移分量和三個(gè)旋轉(zhuǎn)分量，它們分別表示末端執(zhí)行器坐標(biāo)在參考坐標(biāo)中的空間位置和方向（姿態(tài)）。因此，必須給它指定一個(gè)參考坐標(biāo)，原則上這個(gè)參考坐標(biāo)可以任意設(shè)置，但為了規(guī)范化和簡(jiǎn)化計(jì)算，通常以endeffectorXXYYZZ圖10-1機(jī)器人操作手O機(jī)器人的基坐標(biāo)作為參考坐標(biāo)。機(jī)器人的基坐標(biāo)的設(shè)置也不盡相同，如日本的Movemaster－Ex系列機(jī)器人，它們的基坐標(biāo)都設(shè)置在腰關(guān)節(jié)上，而美國(guó)的Stanford機(jī)器人和Unimation公司出產(chǎn)的PUM系列機(jī)器人則是以肩關(guān)節(jié)坐標(biāo)作為機(jī)器人的基坐標(biāo)的。2/4/20233智能與控制工程研究所機(jī)器人的位置控制主要有直角坐標(biāo)和關(guān)節(jié)坐標(biāo)兩種控制方式。直角坐標(biāo)位置控制：是對(duì)機(jī)器人末端執(zhí)行器坐標(biāo)在參考坐標(biāo)中的位置和姿態(tài)的控制。通常其空間位置主要由腰關(guān)節(jié)、肩關(guān)節(jié)和肘關(guān)節(jié)確定，而姿態(tài)（方向）由腕關(guān)節(jié)的兩個(gè)或三個(gè)自由度確定。通過(guò)解逆運(yùn)動(dòng)方程，求出對(duì)應(yīng)直角坐標(biāo)位姿的各關(guān)節(jié)位移量，然后驅(qū)動(dòng)伺服結(jié)構(gòu)使末端執(zhí)行器到達(dá)指定的目標(biāo)位置和姿態(tài)。關(guān)節(jié)坐標(biāo)位置控制：直接輸入關(guān)節(jié)位移給定值，控制伺服機(jī)構(gòu)。從70年代初開(kāi)始，專家們提出了各種各樣的位置控制方法和相應(yīng)的控制算法，其中比較有代表性的有：（1）解運(yùn)動(dòng)位置的控制RMPC

（ResolvedMotionPositionControl）

1972年由RichardP.Paul提出機(jī)器人關(guān)節(jié)坐標(biāo)路徑和直角坐標(biāo)路徑兩種軌跡控制方法，其代表作為：RPPaul.Modeling,TrajectoryCalculationandServingofaComputerControlledArm.StanfordArtificialIntelligenceLab.,StanfordUniversity,Stanford,CA.A.I.Memo177,Sept.1972RPPaul.ManipulatorCartesianPathControl.IEEETrans.OnSys.Man,Cybernetics,Vol.SMC-9,Nov.1979,PP.702-7112/4/20234智能與控制工程研究所（2）解運(yùn)動(dòng)速度的控制RMRC

（ResolvedMotionRateControl）1969年由D.E.Whitney提出。代表作是：DEWhitney.ResolvedMotionRateControlofManipulatorsandHuman,ProsthesesIEEETrans.onMan-Mach.system.Vol.MMS-10,No.2,June1969,pp.47-53（3）解運(yùn)動(dòng)加速度的控制RMAC

(ResolvedMotionAccelerationControl)1980年由美籍華人科學(xué)家陸?zhàn)B生（J.Y.S.Luh）提出。其代表作為：JYSLuh,MWWalker,andRPPaul.ResolvedAccelerationcontrolofMechanicalManipulators.IEEETrans.onAuto.Control,Vol.AC25,No.3,June1980,pp468-4742/4/20235智能與控制工程研究所（4）解運(yùn)動(dòng)力的控制RMFC

(ResolvedMotionForceControl)1982年由吳清華（WuC.H.）和R.P.Paul提出。其代表作為：CHWuandRPPaul.ResolvedMotionForceControlofRobotManipulators.IEEETrans.onSys.ManandCybernetics,Vol.SMC－12，No.3,May/June,1982解運(yùn)動(dòng)位置的控制RMPC，解運(yùn)動(dòng)速度的控制RMRC，解運(yùn)動(dòng)加速度的控制RMAC和解運(yùn)動(dòng)力的控制RMFC這四種控制方法是機(jī)器人運(yùn)動(dòng)控制的經(jīng)典之作?，F(xiàn)有的通用工業(yè)機(jī)器人一般只具有位置（姿態(tài)，速度）控制能力。如美國(guó)的UnimationPUMA系列機(jī)器人，CINCINNAT1－T3系列機(jī)器人，Stanford機(jī)器人等，它們的重復(fù)定位精度均達(dá)到或接近±0.1mm。日本三菱公司的Movemaster－EX機(jī)器人為±0.3mm，高精度的Adapt機(jī)器人和Delta機(jī)器人的重復(fù)定位精度達(dá)到或接近±0.01mm。所有這些都具有關(guān)節(jié)位置和直角坐標(biāo)位置的控制，且具有專用的機(jī)器人語(yǔ)言（如VAL－Ⅱ）或通用的高級(jí)語(yǔ)言（如BASIC）編程和示教再現(xiàn)能力。2/4/20236智能與控制工程研究所10.1.2力控(Forcecontrol)力控是對(duì)機(jī)器人末端執(zhí)行器輸出力或關(guān)節(jié)力矩的控制。較早提出機(jī)器人力控的是Groome，他在1972年將力反饋控制用在方向舵的駕駛系統(tǒng)中。參見(jiàn)下文：RCTGroome.ForceFeedbackSteeringofteleoperatorSystem.Master’sThesis,MassachusettsInstituteofTechnology(MIT),Aug.19721974年，Jilani將力傳感器安裝在一臺(tái)單軸液壓機(jī)械手上進(jìn)行力反饋控制實(shí)驗(yàn)。參見(jiàn)下文：MAJilani.ForceFeedbackHydraulicServoforAdvancedAutomationMachine.Master’sThesis,MIT,Dept.ofMechanicalEngineering,19742/4/20237智能與控制工程研究所真正將力控用于多關(guān)節(jié)機(jī)器人上的是Whitney，他在1977年將力傳感器用在多關(guān)節(jié)機(jī)器人上，并用解運(yùn)動(dòng)速度的方法（RMRC）推導(dǎo)出力反饋控制的向量表達(dá)式。而R.P.Paul（1972）和Silver（1973）則分別用選擇自由關(guān)節(jié)（freejoints）的方法實(shí)現(xiàn)對(duì)機(jī)器人力的開(kāi)環(huán)控制。見(jiàn)下文：見(jiàn)RMPC列舉的文（1）DSilver.ThelittleRobotSystem.AIM-73,Cambridge,MIT,ArtificialIntelligenceLab.,19731976年R.P.Paul和B.Shimano進(jìn)一步完善上述方法，采用腕力傳感器實(shí)現(xiàn)對(duì)機(jī)器人力的閉環(huán)控制。見(jiàn)下文：RPPaulandBShimano.ComplianceandControl.Proc.JointAutomaticcontrol,Conf.SamFrancisco,IEEE,pp694-699,19762/4/20238智能與控制工程研究所10.1.3順應(yīng)控制(ComplianceControl)順應(yīng)控制又叫依從控制或柔順控制，它是在機(jī)器人的操作手受到外部環(huán)境約束的情況下，對(duì)機(jī)器人末端執(zhí)行器的位置和力的雙重控制。順應(yīng)控制對(duì)機(jī)器人在復(fù)雜環(huán)境中完成任務(wù)是很重要的，例如裝配，鑄件打毛刺，旋轉(zhuǎn)曲柄，開(kāi)關(guān)帶鉸鏈的門或盒蓋，擰螺釘?shù)?。順?yīng)控制可分為兩種方式:被動(dòng)式(PassiveCompliance)主動(dòng)式(ActiveCompliance)2/4/20239智能與控制工程研究所被動(dòng)柔順(PassiveCompliance)被動(dòng)式順應(yīng)控制是設(shè)計(jì)一種柔性機(jī)械裝置，并把它安裝在機(jī)械手的腕部，用來(lái)提高機(jī)械手順應(yīng)外部環(huán)境的能力，通常稱之為柔順手腕（ComplianceWrist）。這種裝置的結(jié)構(gòu)有很多種類型，比較成熟的典型結(jié)構(gòu)是由美國(guó)麻省的TheCharlesStarkDraperLab.的D.E.Whitney領(lǐng)導(dǎo)的一個(gè)小組研制的一種稱之為RCC（RemoteCenterCompliance）的無(wú)源機(jī)械裝置，它是一種由鉸鏈連桿和彈簧等彈性材料組成的具有良好消振能力和一定柔順的無(wú)源機(jī)械裝置。該裝置有一個(gè)特殊的運(yùn)動(dòng)學(xué)特性，即在它的中心桿上有一個(gè)特殊的點(diǎn)，稱為柔順中心（ComplianceCenter），如圖10-2所示。若對(duì)柔順中心施加力，則使中心桿產(chǎn)生平移運(yùn)動(dòng)，若把力矩施加到該點(diǎn)上，則產(chǎn)生對(duì)該點(diǎn)的旋轉(zhuǎn)運(yùn)動(dòng)，該點(diǎn)（柔順中心）往往被選作為工作坐標(biāo)的原點(diǎn)。像RCC這樣的被動(dòng)式柔順手腕，由于不需要信息處理，而只靠自身的機(jī)構(gòu)調(diào)整，所以具有快速響應(yīng)的能力，而且結(jié)構(gòu)簡(jiǎn)單，價(jià)格低廉。但它只能在諸如插軸入孔這樣一些專用場(chǎng)合使用，且柔順中心的調(diào)整也比較困難，不能適應(yīng)桿件長(zhǎng)度的變化，柔順度固定，無(wú)法適應(yīng)不同作業(yè)任務(wù)要求，這些都是由于其機(jī)械結(jié)構(gòu)和彈性材料決定的，因此其通用性較差。后來(lái)也有人設(shè)計(jì)一種柔順中心和柔性度可變的RCC裝置，稱為VRCC(VariableRCC)，但結(jié)構(gòu)復(fù)雜，重量大，且可調(diào)范圍有限。2/4/202310智能與控制工程研究所柔順中心旋轉(zhuǎn)部件平移部件O(a)RCCδxF(b)平移δθM(c)旋轉(zhuǎn)圖10-2RCC工作原理···2/4/202311智能與控制工程研究所主動(dòng)剛度控制(ActiveStiffnessControl)剛度控制是阻抗控制的一個(gè)特例，它是對(duì)機(jī)器人操作手靜態(tài)力和位置的雙重控制?？刂频哪康氖钦{(diào)整機(jī)器人操作手與外部環(huán)境接觸時(shí)的伺服剛度，以滿足機(jī)器人順應(yīng)外部環(huán)境的能力。其代表作是：JKSalisbury.ActiveStiffnessControlofaManipulatorinCartesianCoordinates.IEEEConf.ofDecisionandControl.Nov.1980.pp.95-106.Dept.ofComputerScience,StanfordUniversity.位置/力混和控制（HybridPosition/ForceControl）位置/力混和控制是由RaibertandCraig在1981年提出的它的思想是分別將機(jī)器人的力控和位置控制在控制器的兩個(gè)不同通道上實(shí)現(xiàn)，這就是著名的R－C控制器。其代表作是：MHRaibertandJJCraig.HybridPosition/ForcecontrolofManipulators.Trans,ofASME,JournalofDSMC,Vol.102,June1981.pp.126-1332/4/202312智能與控制工程研究所順應(yīng)控制（Compliancecontrol）有關(guān)順應(yīng)控制的理論和方法，是由Mason在1981年提出的。內(nèi)容包括對(duì)外部環(huán)境的描述，自然約束和人為約束條件，力控與位置控制等。其代表作是：MTMason.ComplianceandForcecontrolforComputerControlledManipulators.IEEETrans.OnSMC,Vol.SMC-11,No.6,June.1981.pp.418-432RPPaulandBShimano.ComplianceandControl.AmericanAutomaticControlCouncil,proc.ofthe1976JointAutomaticControlConference,1976.pp.694-6992/4/202313智能與控制工程研究所10.2通用機(jī)器人控制器和控制結(jié)構(gòu)

（TheStructureofGeneralRobot）圖10-3通用機(jī)器人控制結(jié)構(gòu)解逆運(yùn)動(dòng)程Xd

→θd關(guān)節(jié)位控制PID光電碼盤(pán)機(jī)器人操作手XdθdiθbiθeiX＋－···2/4/202314智能與控制工程研究所由圖10-3可知，通用機(jī)器人是一個(gè)半閉環(huán)控制機(jī)構(gòu)，即關(guān)節(jié)坐標(biāo)采用閉環(huán)控制方式，由光電碼盤(pán)提供各關(guān)節(jié)角位移實(shí)際值的反饋信號(hào)θbi。直角坐標(biāo)采用開(kāi)環(huán)控制方式，由直角坐標(biāo)期望值Xd解逆運(yùn)動(dòng)方程，獲得各關(guān)節(jié)位移的期望值θdi，作為各關(guān)節(jié)控制器的參考輸入，它與光電碼盤(pán)檢測(cè)的關(guān)節(jié)角位移θbi比較后獲得關(guān)節(jié)角位移的偏差θei，由偏差控制機(jī)器人操作手各關(guān)節(jié)伺服機(jī)構(gòu)（通常采用PID方式），使機(jī)械手末端執(zhí)行器到達(dá)預(yù)定的位置和姿態(tài)。直角坐標(biāo)位置采用開(kāi)環(huán)控制的主要原因是目前尚無(wú)有效準(zhǔn)確獲取（檢測(cè)）末端執(zhí)行器位置和姿態(tài)的手段。但由于目前采用計(jì)算機(jī)求解逆運(yùn)動(dòng)方程的方法比較成熟，所以控制精度還是很高的。如美國(guó)UnimationPUMA系列機(jī)器人CINCINNATI-T3系列機(jī)器人和Stanford機(jī)器人，其直角坐標(biāo)位置重復(fù)定位精度達(dá)到±0.1mm。日本三菱公司的RM－101和Movemaster－EX機(jī)器人重復(fù)定位精度為±0.3mm，而坐標(biāo)型高精度機(jī)器人Delta和Adapt機(jī)器人重復(fù)定位精度甚至達(dá)到±0.01mm。（注意：重復(fù)定位精度不是軌跡控制精度，后者精度要低得多）。應(yīng)該指出的是目前通用工業(yè)機(jī)器人位置控制是基于運(yùn)動(dòng)學(xué)的控制而非動(dòng)力學(xué)控制。只適用于運(yùn)動(dòng)速度和加速度較小的應(yīng)用場(chǎng)所。對(duì)于快速運(yùn)動(dòng)，負(fù)載變化大和要求力控的機(jī)器人還必須考慮其動(dòng)力學(xué)行為。2/4/202315智能與控制工程研究所10.3通用機(jī)器人的動(dòng)力學(xué)（DynamicsofGeneralRobots）正動(dòng)力學(xué)計(jì)算由關(guān)節(jié)力矩τ計(jì)算關(guān)節(jié)加速度，即τ→通用機(jī)器人的動(dòng)力學(xué)模型為H(q)+C(q,)+G(q)+JT(q)Fe=τ（10.1）式中： H(q)——慣量矩 q——關(guān)節(jié)位置 C(q,)——向心力和哥氏力的力矩 G(q)——重力矩 Fe——外力和外力矩(含摩擦力和阻尼作用) JT(q)——Jacobian陣

τ——關(guān)節(jié)驅(qū)動(dòng)力矩q..q..q..q.q.q.2/4/202316智能與控制工程研究所解析法：求（10.1）式的解析解，計(jì)算精確，旦計(jì)算量大，通用性差。數(shù)值法：將（10.1）式改寫(xiě)成 H(q)=τ－b (10.2)式中： b＝C(q,)+G(q)+JT(q)Fe ——偏移力矩步驟是：（ⅰ）計(jì)算b：由于b與無(wú)關(guān)，只與q和有關(guān)，可假設(shè)＝0，由逆動(dòng)力學(xué)計(jì)算得到關(guān)節(jié)力矩b；（ⅱ）計(jì)算H（q）只與有關(guān)，與q和無(wú)關(guān)，可設(shè)＝G(q)=Fe=0,令＝τj＝[00…010…0]T，用逆動(dòng)力學(xué)計(jì)算得到關(guān)節(jié)力矩τ應(yīng)為H(q)的第j列，由于H(q)為對(duì)稱陣，因而只要計(jì)算上三角陣即可。因而需進(jìn)行N次逆動(dòng)力學(xué)計(jì)算，計(jì)算量大?？勺鋈缦潞?jiǎn)化：由于＝τj，則第j個(gè)關(guān)節(jié)后所有N-j+1個(gè)連桿可合成為單個(gè)剛體，稱為第j個(gè)桿到機(jī)械手末端的組合件（合成桿），由力學(xué)基本原理可計(jì)算出組合件質(zhì)量Mj，質(zhì)心Cj，慣量矩Ej，所受合力Fj及合力矩Nj，這樣可作為一個(gè)臂參加逆動(dòng)力學(xué)遞推計(jì)算，大大減小計(jì)算量。q..q.q.q.q.q.q..q..q..q..q..2/4/202317智能與控制工程研究所逆動(dòng)力學(xué)計(jì)算根據(jù)機(jī)械手狀態(tài)q，，計(jì)算關(guān)節(jié)驅(qū)動(dòng)力矩τ，方法如下：（1）拉格朗日法* （2）牛頓歐拉法（遞推算法）（3）Kane法* （4）阿貝爾方程法（5）高斯最大約束原理 （6）廣義達(dá)朗貝爾原理

*最常用，計(jì)算量最小控制系統(tǒng)仿真（七種不同類型的控制器）（1）獨(dú)立關(guān)節(jié)的PID控制 （2）分解運(yùn)動(dòng)速度控制（3）分解運(yùn)動(dòng)加速度控制 （4）計(jì)算力矩控制（5）變結(jié)構(gòu)控制 （6）自適應(yīng)控制（7）順應(yīng)控制q.2/4/202318智能與控制工程研究所參考文獻(xiàn)[1]黃心漢，PUMA560機(jī)器人及其運(yùn)動(dòng)學(xué)方程，電氣自動(dòng)化，1986（5）：16－21[2]黃心漢，機(jī)器人的主動(dòng)順應(yīng)控制，華中學(xué)院學(xué)報(bào)，1987（4）：147－154[3]RichardP.Paul,MaRongandHongZhang.TheDynamicsofthePUMAManipulator.ResearchReportofSchoolofElectricalEngineering,PurdueUniversity,WestLafayette,Indiana49707TR-EE84-19[4]韓朔眺等，機(jī)器人PUMA560的動(dòng)力學(xué)方程，機(jī)器人,1987（4）：23－27[5]賀日盢，機(jī)器人PUMA560逆運(yùn)動(dòng)方程的解析法，西安公路學(xué)院自動(dòng)化系2/4/202319智能與控制工程研究所10.4阻抗控制(ImpedanceControl)

阻抗控制的概念是N.Hogan在1985年提出的*，他利用Norton等效網(wǎng)絡(luò)概念，把外部環(huán)境等效為導(dǎo)納，而將機(jī)器人操作手等效為阻抗，這樣機(jī)器人的力控制問(wèn)題便變?yōu)樽杩拐{(diào)節(jié)問(wèn)題。阻抗由慣量－彈簧－阻尼三項(xiàng)組成，期望力為：

Fd

=K△X+B△+M△(10.3)式中：△X＝Xd－X,Xd為名義位置，X為實(shí)際位置。它們的差△X為位置誤差，K、B、M為彈性、阻尼和慣量系數(shù)矩陣，一旦K、B和M被確定，則可得到笛卡兒坐標(biāo)的期望動(dòng)態(tài)響應(yīng)。利用式（10.3）計(jì)算關(guān)節(jié)力矩，無(wú)需求運(yùn)動(dòng)學(xué)逆解，而只需計(jì)算正運(yùn)動(dòng)學(xué)方程和Jacobian矩陣的逆J－1。NHogan.ImpedanceControl:AnApproachtoManipulationPartⅠ-Theory,PartⅡ-Implementation,PartⅢ-Application.ASMETrans.ofDSMC,Vol.107(1),19852/4/202320智能與控制工程研究所圖10-4阻抗控制結(jié)構(gòu)圖J-1KPARMXdX＋＋＋－－－－J-1JTKvKf1KE力傳感器FsF＋－－Kf2XE＋．Xd．X2/4/202321智能與控制工程研究所圖10-4中，當(dāng)阻尼反饋矩陣Kf2＝0時(shí)，稱為剛度控制。

剛度控制是用剛度矩陣Kp來(lái)描述機(jī)器人末端作用力與位置誤差的關(guān)系，即 F(t)=Kp

△X(10.4)式中Kp通常為對(duì)角陣，即Kp＝diag[Kp1Kp2…Kp6]。剛度控制的輸入為末端執(zhí)行器在直角坐標(biāo)中的名義位置，力約束則隱含在剛度矩陣Kp中，調(diào)整Kp中對(duì)角線元素值，就可改變機(jī)器人的順應(yīng)特性。

阻尼控制則是用阻尼矩陣Kv來(lái)描述機(jī)器人末端作用力與運(yùn)動(dòng)速度的關(guān)系，即 F(t)=Kv△（10.5）式中Kv是六維的阻尼系數(shù)矩陣，阻尼控制由此得名。通過(guò)調(diào)整Kv中元素值，可改變機(jī)器人對(duì)運(yùn)動(dòng)速度的阻尼作用。2/4/202322智能與控制工程研究所

阻抗控制本質(zhì)上還是位置控制，因?yàn)槠漭斎肓繛槟┒藞?zhí)行器的位置期望值Xd（對(duì)剛度控制而言）和速度的期望值（對(duì)阻抗控制而言）。但由于增加了力反饋控制環(huán)，使其位置偏差△X和速度偏差△與末端執(zhí)行器與外部環(huán)境的接觸力的大小有關(guān)，從而實(shí)現(xiàn)力的閉環(huán)控制。這里力－位置和力－速度變換是通過(guò)剛度反饋矩陣Kf1和阻尼反饋矩陣Kf2來(lái)實(shí)現(xiàn)的。這樣系統(tǒng)的閉環(huán)剛度可求出*當(dāng)Kf2＝0時(shí)Kcp＝(I＋KpKf1)－1Kp（10.6） Kf1＝Kcp－1－Kp－1（10.7）當(dāng)Kf1＝0時(shí)Kcv＝(I＋KvKf2)－1Kv（10.8） Kf2＝Kcv－1－Kv－1（10.9）黃心漢，機(jī)器人的主動(dòng)順應(yīng)控制，華中工學(xué)院學(xué)報(bào)，1987-15(4):147-1542/4/202323智能與控制工程研究所10.5主動(dòng)剛度控制（ActiveStiffnessControl）10.5.1廣義直角坐標(biāo)剛度與關(guān)節(jié)坐標(biāo)剛度GeneralizedCartesianCoordinateStiffnessandJointFranceStiffness將線性彈簧的虎克定理f＝k

dx推廣到直角坐標(biāo)中六維矩陣的形式有f＝kδx（10.10）式中δx＝[dxdydzδxδyδz]T稱為位置偏差向量，其中前三個(gè)分量是位置偏差平移分量，后三個(gè)分量是旋轉(zhuǎn)分量；

f=[fxfyfzmxmymz

]T是六維力向量；k＝6×6維剛度矩陣，矩陣元素kij

(i,j=1,2,3…6)表示位置偏差向量與力向量之間的關(guān)系，如果將k選定為6×6的對(duì)角陣，即k＝diag[k11k22…k66]，即表明力向量與位置偏差向量是去耦的，這時(shí)它們之間的各個(gè)分量之間具有一一對(duì)應(yīng)的線性關(guān)系。2/4/202324智能與控制工程研究所對(duì)角剛度矩陣所依附的直角坐標(biāo)原點(diǎn)稱為剛度中心（StiffnessCenter），顯然剛度中心具有這樣的性質(zhì)，即如果在這一點(diǎn)施加力，只會(huì)引起沿力方向上的平移運(yùn)動(dòng)，如果對(duì)通過(guò)該點(diǎn)的坐標(biāo)軸施加力矩，只會(huì)產(chǎn)生繞該軸的旋轉(zhuǎn)運(yùn)動(dòng)。這與被動(dòng)柔順手腕RCC的柔順中心的運(yùn)動(dòng)特性一致。但由于剛度矩陣所依附的坐標(biāo)可以任意設(shè)置，故剛度中心位置也可任意改變，這是被動(dòng)柔順手腕無(wú)法實(shí)現(xiàn)的。式f＝kδx是在直角坐標(biāo)中描述六維力向量與位置偏差向量的關(guān)系式，因而稱k為廣義直角坐標(biāo)剛度矩陣。運(yùn)用Jacobian陣J作微分變換，則有

δx＝Jδθ（10.11）式中δθ＝θd－θ，為指令關(guān)節(jié)角位移與實(shí)際關(guān)節(jié)角位移的差值。設(shè)靜力和動(dòng)態(tài)力均被補(bǔ)償，則滿足式（10.11）作用力f所需的關(guān)節(jié)力矩為： τ＝JTf （10.12）2/4/202325智能與控制工程研究所f＝kδx （10.13）δx＝Jδθ（10.14）τ＝JT

f （10.15）由式（10.13）～（10.15）可得：

τ＝JTkJδθ（10.16）令kθ＝JTkJ，則有

τ＝kθδθ（10.17）我們將kθ稱為關(guān)節(jié)剛度矩陣（JointStiffnessMatrix），它表示關(guān)節(jié)位移偏差與關(guān)節(jié)力矩之間的關(guān)系。如果直角坐標(biāo)剛度矩陣k是對(duì)角陣，由kθ＝JTkJ可知，關(guān)節(jié)剛度矩陣kθ是非對(duì)角的對(duì)稱陣。這意味著有關(guān)關(guān)節(jié)的位置誤差會(huì)影響其它關(guān)節(jié)的指令力矩，即關(guān)節(jié)剛度是耦合的。正是基于這個(gè)原因，采用直角坐標(biāo)剛度控制比較方便。2/4/202326智能與控制工程研究所10.5.2主動(dòng)剛度控制結(jié)構(gòu)

(TheStructureofActiveStiffnessControl)圖10-5是J.K.Salisbury*提出的主動(dòng)剛度控制的結(jié)構(gòu)圖。JKSalisbury.ActiveStiffnessControlofaManipulatorinCartesianCoordinates.Proc.of19thIEEEConf.onDec.andcontr.1980,pp.95-106圖10-5剛度控制結(jié)構(gòu)圖CKθJT+++－K(s+a)/(s+b)Kf/s+－ARMG++++Kω

D+－++V0sign()+2/4/202327智能與控制工程研究所圖10-5中，θ－關(guān)節(jié)角度，－關(guān)節(jié)角速度，fs－由腕力傳感器提供的末端執(zhí)行器與外部環(huán)境接觸力向量。運(yùn)用Jacobian陣可將fs轉(zhuǎn)換成關(guān)節(jié)平衡力矩τs，即

τs=JTfs （10.18）系統(tǒng)外環(huán)為位置環(huán)，由偏差δθ計(jì)算修正力矩Kθδθ，然后疊加一偏置力矩τb，取這兩項(xiàng)之和作為關(guān)節(jié)的指令力矩τc，即τc＝Kθδθ＋τb（10.19）式中，τb＝JTfb，是外加力矩，它由任務(wù)確定fb，再經(jīng)J陣轉(zhuǎn)換為τb。如果外加力fb＝0（τb＝0），則稱為零力控制。對(duì)于剛度控制，將τc直接加到關(guān)節(jié)伺服電機(jī)，用力開(kāi)環(huán)控制便可實(shí)現(xiàn)。該系統(tǒng)為提高系統(tǒng)對(duì)力信號(hào)的響應(yīng)性能，加入了力反饋伺服環(huán)（內(nèi)環(huán)），采用腕力傳感器檢測(cè)實(shí)際作用力fs，用Jacobian矩陣JT變換為關(guān)節(jié)力矩τs，與指令力矩τc比較后獲得關(guān)節(jié)力矩誤差δτc＝τc－τs，使校正網(wǎng)絡(luò)C獲得修正力矩信號(hào)，從而提高機(jī)器人對(duì)外力作用的響應(yīng)性能，使末端執(zhí)行器輸出力更接近期望值。在機(jī)械手與環(huán)境接觸前，末端執(zhí)行器（手爪）與工件的重力可作為偏移量，在計(jì)算實(shí)際作用力時(shí)可將該偏移量減去，從而消除手爪和工件重力的影響。2/4/202328智能與控制工程研究所

在力反饋回路中，加入超前－滯后網(wǎng)絡(luò)，以提高系統(tǒng)的穩(wěn)定性，與超前－滯后網(wǎng)絡(luò)并聯(lián)一積分環(huán)節(jié)Kf

/s，以減小力的穩(wěn)態(tài)誤差，提高系統(tǒng)精度,積分環(huán)節(jié)前加入一帶死區(qū)的限幅器，死區(qū)可減小極限環(huán)，限幅器則對(duì)大誤差信號(hào)減小積分器的有效增益。速度反饋回路，提供阻尼力矩KωD，其中Kω是阻尼系數(shù)矩陣。D為關(guān)節(jié)瞬時(shí)慣量，加入關(guān)節(jié)瞬時(shí)慣量D的目的是為了保持慣量變化時(shí)系統(tǒng)的穩(wěn)定性。關(guān)節(jié)阻尼系數(shù)矩陣為 Kω＝JTKv

J (見(jiàn)10.5.1節(jié)式（10.16）) （10.20）考慮重力負(fù)載G和摩擦力矩Vosign（），這樣加到關(guān)節(jié)i的控制力矩為τi

=τci＋Ciδτci+KωiDiδ+Vosign（）+Gi（10.21）2/4/202329智能與控制工程研究所10.6位置和力的混合控制

（HybridPosition/ForceControlforRobotManipulators）10.6.1C曲面（C-surface）在環(huán)境約束情況下，對(duì)機(jī)器人進(jìn)行位置和力的混合控制，通常要先建立一個(gè)控制曲面，即所謂C曲面*，在C曲面的切線方向進(jìn)行位置控制而沿C曲面的法線方向進(jìn)行力控。MTMason.ComplianceandForceControlforComputerControlledManipulators.IEEETrans.onSMC-11,1981(6):418-432C曲面定義為*:C=(q|f2≤F(q)≤f1)(10.22)JPMerlet.C-surfaceAppliedtotheDesignofHybridPosition/ForceController.IEEEConf.ofRobotics&Automation,1987(2)式（10.22）中，當(dāng)f1→0,f2→0時(shí)（即零力作用）的C集叫做C曲面，它的一面為非接觸面，另一面為約束面。如圖10-6所示。Nq0圖10-6C曲面2/4/202330智能與控制工程研究所

在靜態(tài)情況下，C曲面可看作一幾何問(wèn)題。如果只有接觸力，且機(jī)器人定位精度已知，在結(jié)構(gòu)化環(huán)境下，我們就能確定機(jī)器人以什么樣的組合形態(tài)會(huì)導(dǎo)致它與環(huán)境相接觸。定義N（q0）為C曲面q0點(diǎn)的法線方向，則在接觸點(diǎn)q0處沿N軸方向的運(yùn)動(dòng)會(huì)導(dǎo)致作用力增大，反之作用力減少。因此沿法線N的運(yùn)動(dòng)可控制作用力的大小。以混合控制的觀點(diǎn)，找到了C曲面的法線N，即給出了力控制的方向。而位置控制則不能越過(guò)C曲面