二自由度機(jī)器人的位置控制

1、實(shí)驗(yàn) 二自由度機(jī)器人的位置控制一、實(shí)驗(yàn)?zāi)康?. 運(yùn)用Matlab語(yǔ)言、Simulink及Robot工具箱，搭建二自由度機(jī)器人的幾何模型、動(dòng)力學(xué)模型，2. 構(gòu)建控制器的模型，通過調(diào)整控制器參數(shù)，對(duì)二自由度機(jī)器人的位姿進(jìn)行控制，并達(dá)到較好控制效果。二、工具軟件1.Matlab軟件2.Simulink動(dòng)態(tài)仿真環(huán)境3.robot工具箱模型可以和實(shí)際中一樣，有自己的質(zhì)量、質(zhì)心、長(zhǎng)度以及轉(zhuǎn)動(dòng)慣量等，但需要注意的是它所描述的模型是理想的模型，即質(zhì)量均勻。這個(gè)工具箱還支持Simulink的功能，因此，可以根據(jù)需要建立流程圖，這樣就可以使仿真比較明了。把robot 工具箱拷貝到MATLAB/toolbox文件夾

2、后，打開matalb軟件，點(diǎn)擊file-set path，在打開的對(duì)話框中選add with subfolders，選中添加MATLAB/toolbox/robot，保存。這是在matlab命令窗口鍵入roblocks就會(huì)彈出robot 工具箱中的模塊（如下圖）。三、實(shí)驗(yàn)原理在本次仿真實(shí)驗(yàn)中，主要任務(wù)是實(shí)現(xiàn)對(duì)二自由度機(jī)器人的控制，那么首先就要?jiǎng)?chuàng)建二自由度機(jī)器人對(duì)象，二自由度機(jī)器人坐標(biāo)配置仿真參數(shù)如下表1： 表1 二連桿參數(shù)配置意義符號(hào)值單位桿1長(zhǎng)l10.45m桿2長(zhǎng)l20.55m桿1重心lc10.091m桿2重心lc20.105m桿1重量m123.90kg桿2重量m24.44kg桿1慣量I11

3、.27kg. m2桿2慣量I20.24kg. m2重力加速度G9.8m /sec21.運(yùn)動(dòng)學(xué)模型 構(gòu)建二連桿的運(yùn)動(dòng)學(xué)模型，搭建twolink模型在MATLAB命令窗口下用函數(shù)drivebot(WJB)即可觀察到該二連桿的動(dòng)態(tài)位姿圖。%文件名命名為自己名字的首字母_twolink%構(gòu)造連桿一L1=link(0 0.45 0 0 0,'standard') ;L1.m=23.9 ;L1.r=0.091 0 0 ;L1.I=0 0 0 0 0 0 ;L1.Jm=0 ;L1.G=1 ;%構(gòu)造連桿二L2=link(0 0.55 0 0 0,'standard') ;L2.

4、m=4.44 ;L2.r=0.105 0 0 ;L2.I=0 0 0 0 0 0 ;L2.Jm=0 ;L2.G=1 ;%（機(jī)器人的名字請(qǐng)用自己名字的首字母如）WJB=robot(L) ;WJB.name='WJB_twolink' ; %設(shè)定二連桿名字qz=0 0 ;qr=0 pi/2 ;2.二連桿動(dòng)力學(xué)部分實(shí)現(xiàn)機(jī)器人內(nèi)部動(dòng)力學(xué)構(gòu)建，根據(jù)拉格朗日法建立機(jī)器人動(dòng)力學(xué)模型（見書上P55）即下式 ： 仍然用matlab下M函數(shù)來實(shí)現(xiàn)：%文件名命名為自己名字的首字母_dl%二連桿動(dòng)力學(xué)部分function qdd=WJB_dl(u) %自己名字的首字母q=u(1:2); qd=u(3:

5、4); tau=u(5:6);g=9.8;m1=23.9 ; m2=4.44 ;l1=0.45 ; l2=0.55 ;lc1=0.091 ;lc2=0.105 ;I1=1.27 ; I2=0.24 ;M11=m1*lc12+m2*(l12+lc22+2*l1*lc2*cos(q(2)+I1+I2 ;M12=m2*(lc22+l1*lc2*cos(q(2)+I2 ;M21=m2*(lc22+l1*lc2*cos(q(2)+I2 ;M22=m2*lc22+I2 ;M=M11 M12 ;M21 M22 ;C11=-(m2*l1*lc2*sin(q(2)*qd(2) ;C12=-m2*l1*lc2*s

6、in(q(2)*(qd(1)+qd(2) ;C21=m2*l1*lc2*sin(q(2)*qd(1);C22=0 ;C=C11 C12 ;C21 C22 ;G1=(m1*lc1+m2*l1)*g*sin(q(1)+m2*lc2*g*sin(q(1)+q(2) ;G2=m2*lc2*g*sin(q(1)+q(2) ;G=G1 ;G2 ;qdd=inv(M)*(tau-G-C*qd)最后，還需將機(jī)器人動(dòng)力學(xué)和幾何學(xué)聯(lián)系在一起。通過機(jī)器人學(xué)工具箱中的robot模塊實(shí)現(xiàn)。3控制器設(shè)計(jì)（任選一二）（1）簡(jiǎn)單PD控制率，結(jié)構(gòu)圖如下，此種方法沒有加任何補(bǔ)償，存在較大穩(wěn)態(tài)誤差，但是控制算法非常簡(jiǎn)單。（2）PD

7、加重力補(bǔ)償 帶有重力補(bǔ)償?shù)腜D控制可設(shè)計(jì)成 t=Kp(q期望值-q)-Kd*qd+G(q)重力項(xiàng) 3. PD加前饋補(bǔ)償控制加了一個(gè)逆動(dòng)力學(xué)模塊 t=Kp(q期望值-q)+Kd*(q期望值一階導(dǎo)-q一階導(dǎo))+M（q）*q二階導(dǎo)+C*q一階導(dǎo)+G（q）四、實(shí)驗(yàn)步驟1運(yùn)動(dòng)學(xué)模型 在matlab菜單file下新建一個(gè)M-file，將機(jī)器人運(yùn)動(dòng)學(xué)模型添加進(jìn)去（注意更改自己的機(jī)器人命名,自己名字的首字母縮寫_twolink），并將此M-file命名后保存在work文件夾下，備用。2在matlab命令窗口調(diào)用函數(shù)drivebot（機(jī)器人名字自己名字首字母的縮寫，不加twolink），出現(xiàn)機(jī)器人的動(dòng)態(tài)位姿圖，

8、調(diào)節(jié)q1、q2可直觀的看出二自由度機(jī)器人的位姿在改變。3動(dòng)力學(xué)模型 在matlab菜單file下再新建一個(gè)M-file，將機(jī)器人動(dòng)力學(xué)學(xué)模型添加進(jìn)去，并將此M-file命名后（自己名字首字母_mdl）保存在work文件夾下，備用。4，將機(jī)器人運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型聯(lián)系起來在matlab命令窗口輸入命令roblocks調(diào)出robot工具箱，再輸入simulink調(diào)出SImulink動(dòng)態(tài)仿真環(huán)境。5、在Matlab菜單file下新建一個(gè)model，將robot工具箱中的robot模塊拖拽到model文件里，雙擊編輯機(jī)器人屬性，將robot object改為機(jī)器人的名字（自己名字首字母的縮寫）（即運(yùn)

9、動(dòng)學(xué)構(gòu)建的機(jī)器人對(duì)象）。再選中robot模塊，右鍵菜單找到look under mask，點(diǎn)開，可以找到機(jī)器人內(nèi)部動(dòng)力學(xué)模型，將其中的S-Function替換成Simulink下面的MatlabFunction，雙擊此MatlabFunction彈出對(duì)話框，將其中的函數(shù)改為動(dòng)力學(xué)模型文件名。6.添加控制器 根據(jù)控制器設(shè)計(jì)的方案，在Simulink下找出構(gòu)成控制系統(tǒng)的其他模塊，其中綜合點(diǎn)及matrix gain 在math operations里；示波器scope和終止端terminator在輸出池sinks里；常量constant在輸入模塊sources里； 將各個(gè)模塊拖拽到model文件里，可以通過鼠標(biāo)拖住連線。7、動(dòng)態(tài)仿真 雙擊綜合點(diǎn)，將其屬性改成有一個(gè)減號(hào)，形成負(fù)反饋；常量constant給定你期望位姿（注意是二自由度機(jī)器人，需輸入2*！的矩陣），初步給定KP、KD參數(shù)（2*2的矩陣）。在model文件菜單欄下面，點(diǎn)擊一個(gè)箭頭（start simulation）或者在菜單欄點(diǎn)擊Simulation ，在下拉菜單中選擇start simulation，即可開始仿真，此時(shí)雙擊打開scope即可得到響應(yīng)曲