機(jī)械臂運(yùn)動控制實(shí)驗(yàn)一、實(shí)物搭建利用搭建圖紙搭好底座部分之后，搭建相應(yīng)的臂。搭建時(shí)確保相鄰關(guān)節(jié)中心與連桿在一條直線上，方便分析與計(jì)算。最終搭建圖1所示。圖1二、建模分析利用D-H方程對機(jī)器臂進(jìn)行分析，首先對于機(jī)器臂每個(gè)關(guān)節(jié)建立坐標(biāo)系如圖2所示。圖2D-H參數(shù)表#da0-11d10901-220d202-H30d30將D-H參數(shù)代入相應(yīng)的A矩陣，可得到機(jī)器人的正運(yùn)動學(xué)方程如下：A1=cos(1)sin(1)00 0010 sin(1)-cos(1)00 00d11, A2=cos(2)sin(2)00 -sin(2)cos(2)00 0010 d2cos(2)d2sin(2)01,A3=cos(3)sin(3)00 -sin(3)cos(3)00 0010 d3cos(3)d3sin(3)01.則根據(jù)機(jī)器人的正運(yùn)動學(xué)方程為：TH=A1*A2*A3。即TH=cos1sin100 0010 sin1-cos100 00d11*cos2sin200 sin2cos200 0010 d2cos2d2sin201*cos3sin300 sin3cos300 0010 d3cos3d3sin301=cos2+3*cos1cos2+3*sin1sin2+30 sin2+3*cos1sin2+3*sin1cos2+30 sin1-cos100 cos1*(d3cos2+3+d2cos2)sin1*(d3cos2+3+d2cos2)d1+d3sin2+3+d2sin2)1 設(shè)機(jī)器臂末端最終期望的位置及姿態(tài)為：pnoa=nx ny nz 0ox oy oz 0ax ay az 0noa1逆運(yùn)動學(xué)求解過程中，注意到該機(jī)器人只有三個(gè)自由度，因而它的最終姿態(tài)是無法控制的，故只能利用矩陣最后一列的分量來進(jìn)行運(yùn)算求解。在等式A1*A2*A3=pnoa兩邊同時(shí)左乘A1-1,使1從2和3中解耦得：A2*A3=A1-1*pnoa,即cos2+3sin2+300 sin2+3cos2+300 0010 d3*cos2+3+d2*cos2d3*sin2+3+d2*sin201=nxcos1+ny sin(1) nz nxsin1-ny cos(1) 0oxcos1+oy sin(1) oz oxsin1-oy cos(1) 0axcos1+ay sin(1) az axsin1-ay cos(1) 0ncos1+o sin(1) a-d1nsin1-o cos(1) 1由方程兩邊矩陣分量（3，4）相等得1=atan(on)由方程兩邊矩陣分量（1，4）和（2，4）得2d22+2d32+2d2d3cos3=(ncos1+o sin(1)2+(a-d1)2即3=-arccos(a-d1)2+(ncos1+o sin(1)2-d22-d322d2d3)在等式A2*A3=A1-1*pnoa兩邊同時(shí)左乘A2-1，即A3=A2-1*A1-1*pnoa.利用方程等式兩邊矩陣分量（1，4）和（2，4）得a-d1*sin2+n cos1+o sin1cos2=d3cos3+d2,a-d1*cos2-n cos1+o sin1sin2=d3sin3聯(lián)立方程組得2=arccos(n cos1+o sin1d3cos3+d2+a-d1d3 sin(3)(a-d1)2+(n cos1+o sin1)2)綜上可得，機(jī)器臂的逆運(yùn)動學(xué)方程為1=atan(on)3=-arccos(a-d1)2+(ncos1+o sin(1)2-d22-d322d2d3)2=arccos(n cos1+o sin1d3cos3+d2+a-d1d3 sin(3)(a-d1)2+(n cos1+o sin1)2)三、編程求解及實(shí)現(xiàn)在實(shí)驗(yàn)中關(guān)于距離的測量利用了樂高機(jī)器人中的15個(gè)孔的長桿，以一個(gè)孔為單位長度，則可獲得相應(yīng)的距離值d1=11.5,d2=20,d3=21.5.以期望位置n=20,o=15,a=0為例，運(yùn)行MATLAB程序：d1=11.5;d2=20;d3=21.5;n=20;o=15;a=0;/以坐標(biāo)（20，15，0）為例th1=atan(o/n)th3=-acos(a-d1)2+(n*cos(th1)+o*sin(th1)2-d22-d32)/(2*d2*d3)th2=acos(n*cos(th1)+o*sin(th1)*(d3*cos(th3)+d2)+(a-d1)*d3*sin(th3)/(a-d1)2+(n*cos(th1)+o*sin(th1)2)根據(jù)求出的各角度值，為了能夠在樂高機(jī)械臂上實(shí)現(xiàn)位置的移動，由于前兩個(gè)關(guān)節(jié)由齒輪、渦輪傳動，因而需要求出各關(guān)節(jié)相應(yīng)的等效傳動比。根據(jù)相應(yīng)的齒輪參數(shù)，得到關(guān)節(jié)一的傳動比為1：72，關(guān)節(jié)二的傳動比為1：168。在實(shí)際運(yùn)行中發(fā)現(xiàn)當(dāng)關(guān)節(jié)一轉(zhuǎn)動過程中，由于關(guān)節(jié)二的齒輪固連在關(guān)節(jié)一上，因而發(fā)生相對運(yùn)動，根據(jù)相應(yīng)的齒數(shù)比計(jì)算發(fā)現(xiàn)，轉(zhuǎn)動過程有2=13*1。利用如下步驟即可將所求角度值轉(zhuǎn)變?yōu)樗欧姍C(jī)應(yīng)轉(zhuǎn)動的角度值。th2=th2-1/3*th1;th1=168*th1*180/pi;th2=72*th2*180/pi;th3=th3*180/pi; 根據(jù)計(jì)算得到的伺服電機(jī)A、B、C應(yīng)該轉(zhuǎn)過的角度th3、th2、th1。利用bricxcc編程如下，驅(qū)動電機(jī)轉(zhuǎn)動相應(yīng)的角度即可。注意在運(yùn)行過程中需保持桿d2和d3保持平行，桿d1與d2垂直。如圖3所示。圖3task main() RotateMotor(OUT_C, 50, 6194); RotateMotor(OUT_B, 50, 997); RotateMotor(OUT_A, 50, 97);當(dāng)運(yùn)行到期望的坐標(biāo)點(diǎn)時(shí)，如果繼續(xù)選擇另一個(gè)點(diǎn)，則可以重新利用MATLAB命令計(jì)算出新的轉(zhuǎn)動角度，與之前轉(zhuǎn)動角度相減，則可得到從當(dāng)前狀態(tài)移動到新的預(yù)期位置各個(gè)關(guān)節(jié)所需轉(zhuǎn)動的角度。某些情況下，雖然求得各個(gè)關(guān)節(jié)應(yīng)當(dāng)轉(zhuǎn)動的角度，但由于實(shí)際地面的約束，會使實(shí)際轉(zhuǎn)動過程中無法達(dá)到相應(yīng)的位置。對于這種情況，首先進(jìn)行判斷出現(xiàn)的條件，再進(jìn)行具體的處理。如可轉(zhuǎn)動第二個(gè)關(guān)節(jié)時(shí)多轉(zhuǎn)動一定的角度，然后再轉(zhuǎn)動第三個(gè)關(guān)節(jié)之后再反向轉(zhuǎn)動相應(yīng)的角度保證達(dá)到期望的目標(biāo)狀態(tài)。四、誤差分析實(shí)驗(yàn)過程中發(fā)現(xiàn)，每次運(yùn)行NXT程序使機(jī)械手到達(dá)期望位置時(shí)，都會存在一定的偏差。誤差的范圍約在一個(gè)單位長度范圍內(nèi)。期望坐標(biāo)點(diǎn)第一次實(shí)驗(yàn)第二次實(shí)驗(yàn)第三次實(shí)驗(yàn)（20，15）（21，15.5）（20，16）（20.5，15.5）（10，10）（10.5，9.5）（11.5，10.5）（11，9.5）（7，16）（7，15）（7，16）（7.5，16）分析造成誤差的主要原