《機器人建模與控制 》 實驗報告匯 項目1-10 機器人實驗平臺及基本操作 - 單關(guān)節(jié)機械臂協(xié)作控制實驗

浙江大學工程師學院實驗報告課程名： 工業(yè)機人系控制姓 名：專 業(yè)： 控制工程學 號：指導教： 吳俊2017年 月27日機器人實驗平臺及基本操作浙江大學工程師學院實驗報告實驗項目名稱： 機器人實驗平臺及基本操作 組學生姓名： 實驗地點： 工業(yè)機器人實驗中心 實驗日期：2017 年月24日一、 認識機器人平臺實驗用的機器人有6 個關(guān)節(jié)從基座開始第1個關(guān)節(jié)稱為軸1第2個關(guān)節(jié)稱為軸2、…、最后一個關(guān)節(jié)稱為末端執(zhí)行器 。機器人關(guān)機后再次開啟電源至少需要等2 分鐘。防護裝置停止狀態(tài)。使能器電機開啟狀態(tài)；第二檔按下去，機器人將處于防護裝置停止狀態(tài)。二、 機器人平臺手動操縱1-3”的動作模式，前后搖動操縱桿可以控制軸2軸1/逆時鐘方向旋轉(zhuǎn)操縱桿可以控制軸3的運動。4-6”的動作模式，前后搖動操縱桿可以控制軸5軸4/逆時鐘方向旋轉(zhuǎn)操縱桿可以控制軸6的運動。2機器人實驗平臺及基本操作xyz軸正x軸y軸運動，順/逆時鐘旋轉(zhuǎn)操縱桿可以控制機器人末端沿著z軸運動。三、 RAPD程序初步RAPD1個初始位姿出發(fā)，拿起工件，移至其它位置，放下工件，機器人手爪返回初始位姿。程序代碼：MODULEMoule3CONSTrobtargetp19:=[[347.2,39.70,90.18][0.2687,-0.7355,0.29628-0.6643],[0,0-3,],[9E+9,9E09,E+099E09,9E09,9+0]];CONSTrobtargetp20:=[[349.6,31.00,59.45][0.2687,-0.7361,0.2962,0.63641,[0,0,3,1,[9E+099E+09,9E09,E+09,9E09,9E+0]];CONSTrobtargetp21:=[[349.9,35.23,99.01][0.2081,-0.6142,0.28558-0.6682],[0,0-3,],[9E+9,9E09,E+099E09,9E09,9+0]];CONSTrobtargetp22:=[[248.5,40.08,99.06][0.2076,-0.6058,0.28517-0.6696],[0,0-3,],[9E+9,9E09,E+099E09,9E09,9+0]];CONSTrobtargetp23:=[[249.3,41.16,64.10][0.2059,-0.6264,0.28619-0.6679],[0,0-3,],[9E+9,9E09,E+099E09,9E09,9+0]];CONSTrobtargetp24:=[[249.2,37.57,82.01][0.2014-0.6315,-0.89708,0.6464,[0,0,3,1,[9E+099E+09,9E09,E+09,9E09,9E+0]];CONSTrobtargetp25:=[[249.2,37.57,82.01][0.2014-0.6315,-0.89708,0.6464,[0,0,3,1,[9E+099E+09,9E09,E+09,9E09,9E+0]];CONSTrobtargetp26:=[[249.2,37.57,82.01][0.2014-0.6315,-0.89708,0.6464,[0,0,3,1,[9E+099E+09,9E09,E+09,9E09,9E+0]];POCoutie3()MoeLp0,v00,z50,tGip;MoeLp90,200,z0,trip;3機器人實驗平臺及基本操作MoeLp00,80,z50,tGrip;estdoGrpperpen;StdoGiperCose;MoeLp10,80,z50,tGrip;MoeLp20,200,z0,trip;MoeLp30,100,z0,trip;estdoGrpperClos;StdoGiperOn;MoeLp40,100,z0,trip;MoeLp50,100,z0,trip;MoeJp10,v00,z0,tGrip;ENDPROCENDMODULE機器人拿放工件動作完整視頻見附件四、實驗心得RAPD程序編輯器進行編程實現(xiàn)機器人拿放工件動作。xxxxxxxx@zu.eu.c201712月8日24：004組號：暫未被分配

沁戶丿驢本科實驗報告姓名：學院：系：專業(yè)：

控制科學與工程學院控制科學與工程系自動化（控制）學號：指導教師： 吳俊2023年 3 月 20 日泊J 塌擘實驗報告

專業(yè)：姓名：學號：日期：2023年3月20日地點： 線上仿真 課程名稱：機器人建模與控制指導老師：吳俊成績： 實驗名稱：機械臂正、逆運動學求解實驗類型：設計驗證性同組學生： 無、實驗目的和要求（必填）、CopperliaSim的軟件環(huán)境理解并應用機械臂正使用仿真環(huán)境進行驗證為實踐環(huán)境萸定基礎＿、 實驗內(nèi)容和原理（必填）D-H參數(shù)以關(guān)節(jié)O廠1為原點，軸i-1為z軸（正向自選），以與下一個關(guān)節(jié)形成的幾何連桿為x軸（正方向為指向下一級連桿的方向），以右手定則確定Y軸，對每一個關(guān)節(jié)建立聯(lián)體坐標系。當然，該方法建立的坐標系是不唯一的。臂的動學求解在改進D書法建立的聯(lián)體坐標系中，可知前后柜鄰的兩級關(guān)節(jié)坐標系轉(zhuǎn)換公式為cos0, —sin01 。 a一1式為i-1r —n,sa,1 s8,sa,1 —sna,1 —ma1d,1I 'sna-1 s8n1-1a,1 sa,-1，1機械臂的逆運 動學求解逆運動學的求解過程是已知笛卡爾空間中末端執(zhí)行器的位姿信息，反求關(guān)節(jié)空間中的各個關(guān)節(jié)的可變參數(shù)信息的過程。該過程的求解方法有解析法和數(shù)值法兩種，可能存在多組解。封閉解法是指基于解析形式的算法。將運動學的封閑解法分為兩類：代數(shù)解法和幾何解法。數(shù)值解法具有迭代性質(zhì)，所以比封陽解法的求解速度慢得多。在多重解的情況下，數(shù)值解法不能求出全部的解。解析解的存在性6自由度橾作臂都是可解的6自由有解析解。這種存在解析解（封閑解的操作臂具有如下特性存在幾個正交0或士90°6個旋轉(zhuǎn)關(guān)節(jié)的操作臂存在封閑解的充分條件是相鄰的三個關(guān)節(jié)軸線相交于一點。2對于X'Y'萬歐拉角，其轉(zhuǎn)換為旋轉(zhuǎn)矩陣得公式如下，2Rx'Y'Z'(a,fJ,Y)=

C2C3C1S3+C3S1S2S1的一C1C3S2

的S3C鈺1C1的S3

S2-c2勻C1C2通過計算可以得知，旋轉(zhuǎn)矩陣轉(zhuǎn)換為歐拉角得公式如下，?=arcsin(r13),也即，a=arctan2(-r23,r33)?=arcsin(r13),也即，y=arctan(-r12,r11)

sin(a)主要儀器設備ZJU-I型桌面機械臂． ．lZJU-I型桌面機械臂． ．l23．四、 操作方法與實驗步驟．．l2寫出ZJU-I．l2．．3寫出ZJU-I型機械臂的正運動學解（不用給出最終的齊次變換矩．．3將以下5組關(guān)節(jié)角參數(shù)帶入正運動學解，計算機械臂末端Tip點的空間位置，663，，＇亢一6亢一32,663，，＇亢一6亢一32,＇60（（（（（@122，12，全一工1，12， －'工12．4將以上5組關(guān)節(jié)角分別輸入122，12，全一工1，12， －'工12．46ZU-I解析解）6．器進行相關(guān)計算），計算對應的．@(-0.066,0.339,0.444,-2.618,-0.524,-3.141)(0.3,0.25,0.26,-2.64,0.59,-2.35)(0.42,0,0.36,3.14,1,-1.57)(0.32,-0.25,0.16,3,0.265,-0.84)37. 將所 求關(guān)節(jié)角為參仿真程從仿真中得到機械臂末端執(zhí)行的空間位置姿，與第6步給定的位置姿態(tài)行。五、 實驗記錄和處理機械臂的改所示表5-1ZJU-I型機械臂的改進DH參數(shù)表序號a序號a仁1a仁1di0i1。。0.2312-90°。-0.05402-90°3。0.185。034。0.170.07704+90°590°。0.077590°690°。。0670.0855機械臂的正運動學解 公式l ，58001l ，00011．0070000100-5080＝T3_0l ，l ，7。74。05．0 01o0100l ，58001l ，00011．0070000100-5080＝T3_0l ，l ，7。74。05．0 01o0100c0oo220＿s0c0－—－＿』=T_5T2,rJrJ7。3 10200 100010001050o4l4110-［0SCS＝T4＝T18001—|』＝T6。。011—|—|J58001—|』＝T6。。011—|—|J55。0081}7oo0010010045 1000c0-＿T7＿，7。10c00其中，

切= 對對扛T

r11 r12 r13 xr23yz22r23yz22r230r12r130r0 1r11=c-C6(cc1ssc234r12=s江234)+c1c234,r13=C1C5C234-扣r21=c6(c心-sr22=和S234-S6(C1C5-江r23=丙r31=S5C234+C薩拉z34,r32=C滬234-S薩貸234,r33=-c貸234,x=0.185c1s2-0.0855s1ss-0.023s1+0.077c1s0.l7c1c23+0.0855c1csc234,y=0.023c1+0.185s1s2+0.0855c1ss+0.077s僅234+0.17s1s23+0.0855css芷z=0.17c23+0.185c2+0.077c234-0.0855c貸234+023.3關(guān)節(jié)角的正運動學解計算機械臂末端以'Y'歐拉角表示需要記錄計算過程。Matlab,創(chuàng)建函數(shù),依次將多角的齊次變換矩陣相乘得到鍛后的總Matlab計算反三角函數(shù)X'Y'Z'歐拉角。表5-2ZJU-I型機械臂的正運動學計算序號關(guān)節(jié)角參數(shù)位置歐拉角Xyz<Xx傷Yz1冗冗'0'冗)石 了0.09040.16430.6075-1.824-0.058-2.6932亢冗亢 冗）＇了'0.24540.25370.3474-2.159-0.448-1.3283,了0冗 亢亢勹Z了了'-0.09700.24550.4603-2.094-1.047-2.6184（冗，冗＇冗0冗z冗)了',百＇-0.27140.20880.4603-0.2280.1302.6335（）'TI'TI'TI'TI'0.22560.10710.5519-2.5830.634-1.867冗5石＇，石''了'5

,齊次變換矩陣為I435I4351O16066汀＄＄盓 嘉0 0 0，了 飛當關(guān)節(jié)角為，了 飛l47l4745374541223666。-0.5000汀。-0.50000 0一冗冗亢）齊次變換矩陣為。l45379502461024066為陣矩換變次齊0。l45379502461024066為陣矩換變次齊00635820700508350302850o，0539o398勹, 6060430,66o0＇－_－－＿，T7冗石-，冗石一寸／角節(jié)關(guān)當4．62290120o62290120o－05為陣矩換變次齊,36883848502000700冗節(jié)8500'O0冗節(jié),'－冗了l冗節(jié)=冗節(jié),T7冗節(jié)，冗節(jié)寸／角節(jié)關(guān)當5．汀詈

0.7702I183。80I183。807061?4009。0.6356

0.5928l6l6195712051215660。-0.6830664 仿真程 序驗證3中的計算結(jié)果，將仿真得到的末端位姿與第3步得到的計算結(jié)果進行比對；表 5-3 Z型機械臂的正運動學仿真序號關(guān)節(jié)角參數(shù)位置歐拉角XyzaxyYz1亢'0'冗)石 了0.09040.16440.6075-1.825-0.045-2.6952冗冗冗0冗), '＇0.24540.25390.3473-2.163-0.442-1.3363（,2", 冗 了勹2"-'-0.09720.24550.4602-2.089-1.037-2.6154（亢，亢＇冗 冗）＇右＇-0.271 60.20860.4729-0.2200.1382.6285（亢亢亢亢亢）'TZ'TZ'TZ'TZ'0.22560.10730.5519-2.5830.641-1.869ZJU-I型桌面機械臂的逆運動學解析解（可選）lyz1m5飛lyz1m5飛0gg|120rrr＝T7飛0訂（）將燈與cs飛0u'rn，cossin01r11u'rn，cossin01r11r12r13x-1 s1。。r21r22r23y _ 2＇rr2/ yuu'rn。。。。。 。。。1 -0.23 r31r32r33 z 。。。。。 。1 1 0 0 1其中，r11 S6SC6S5Cz34r12 z34S5沈Cz34r13=r21=Tzz=-C5S6Tz3=S5r31 S6+C6S5Sz34r32 S5S6S234T33 - C5Sz34x=0.17s23+0.185c2+0.077s234+0.0855c5c234= 0.0855s5+0.023z=0.17c23+0.185c2+0.077c234-0.0855css234令等式兩邊的矩陣2,3),(2,4)相等，可得，7- s1r13+c1r23=5s7-s1x+c1y=0.0855s5+0.023將下式減去0.0855倍的上式，可得，-1(x- 0.0855 r13)+1(y-0.085 5r23)=0.023由上式可得，1=Atan2(y-0.0855r23,x-0.0855r13)-tan2(0.023,士J(x-0.0855r1)1:.+(y-0.0855r23)1:.- 0.023)同理，令等式兩邊的矩陣2,1),2,2)-s1r12+c1r22 s6 ＝由上式可得，

-s1r11+c1r21 c6仇= Atan2(s1r12-c1rc1r-s1r同理，令等式兩邊的矩陣2,1),2,3)除，可得忒-s1r13+ss由上式可得，

-s1r11干r21 ＝C505=tan2(c6(-1r13+1r23,-1+1r1)，令等式兩邊的矩陣1,33,3)-r33．r13

-r33"c1r13+s1r23

"Cz34設，P=c1x+s1y-(0.077s4+0.0855c5c4)=017s3+0.185s2Q=z-0.23-(077c4-0.0855c5s4)=017c3+0.185c2由上式可得，03=Atan2(士 -z,C3)(0.185+0.17c3)P-0.17s3Q (0.185+0.17c3)Q+0.17s3?先=Atan2(0.172+0.1852+乙*0.17*0.11::l5c3'0.172+0.1852+乙*0.17*0.11::l5c304=an2(r3,r3)-2 -038述，關(guān)于6角的運解析解表，由表達式的正負中4個解（角度約束的）。8955。4此4個不同9的逆運動學解，得到4組不同的的關(guān)節(jié)角。將數(shù)據(jù)帶入所得解析解的公式中，可以計算得到如下數(shù)據(jù)，表型機臂的逆運動計算組別要求的末端位姿逆運動學解析解計算的關(guān)節(jié)角1234561.1---1.04690.54320.5314-0.55150.52390.69851.2---1.04691.0516-0.53140.00290.52390.69851.3----1.9328-1.05080.51260.0532-2.75940.78541.4----1.9328-0.5604-0.51260.5879-2.75940.78542.1(----1.57150.46050.6608-0.07450.52360.00132.2(----1.57151.0923-0.66080.61530.52360.00132.3(-0.066,0.339,0.444,-2.618,-0.524,-3.141)-1.418-1.11720.6811-0.5429-2.69850.14692.4(-----1.418-0.466-0.68110.1681-2.69850.14693.1(0.3,0.25,0.26,-2.64,0.59,-2.35)0.63810.78991.3432-1.3169-0.01040.01013.2(0.3,0.25,0.26,-2.64,0.59,-2.36)0.63812.0661-0.34320.0934-0.01040.01013.3(0.3,0.25,0.26,-2.64,0.59,-2.37)-2.3648-2.06581.3456-0.10193.03630.11153.4(0.3,0.25,0.26,-2.64,0.59,-2.38)-2.3648-0.7875-1.34561.31093.03630.11154.1(0.42,0,0.36,3.14,1,-1.57)-0.06590.82730.8245-1.08010.0546-0.03624.2(0.42,0,0.36,3.14,1,-1.58)-0.06591.6149-0.8245-0.21860.0546-0.03624.3(0.42,0,0.36,3.14,1,-1.59)-3.0752-1.61490.82450.21853.08490.03534.4(0.42,0,0.36,3.14,1,-1.60)-3.0752-0.8273-0.82451.083.08490.03535.1(0.32,-0.25,0.16,3,0.265,-0.84)-0.73571.10251.0532-0.87540.0748-0.01285.2(0.32,-0.25,0.16,3,0.265,-0.85)-0.73572.1065-1.05320.22690.0748-0.01285.3(0.32,-0.25,0.16,3,0.265,-0.86)2.5266-2.10721.0513-0.2177-3.10170.10295.4(0.32,-0.25,0.16,3,0.265,-0.87)2.5266-1.1049-1.05130.8826-3.10170.1029將所求關(guān)節(jié)角作為參數(shù)輸入仿真程序，從仿真中得到機械臂末端執(zhí)行器的空。表5-5 型機臂的逆運動學真組別要求的末端位姿由逆運動學關(guān)節(jié)角計算的末端位姿1.10.1170.3340.499-2.019-o.058-2.190.11690.33410.4989-2.02-o.05-2.191.20.1170.3340.499-2.019-o.058-2.190.11690.33410.4990-2.02-o.05-2.191.30.1170.3340.499-2.019-o.058-2.190.11690.33410.4991-2.02-o.07-2.191.40.1170.3340.499-2.019-o.058-2.190.11680.33410.4991-2.02-o.07-2.192.1-o.0660.3390.444-2.618-o.524-3.141-o.06620.33900.4439-2.60-o.51-3.132.2-o.0660.3390.444-2.618-o.524-3.141-o.06620.33900.4440-2.61-o.52-3.132.3-o.0660.3390.444-2.618-o.524-3.141-o.06610.33910.4440-2.62-o.55-3.132.4-o.0660.3390.444-2.618-o.524-3.141-o.06620.33910.4441-2.62-o.54-0.250.26-2.640.59-2.350.29980.25020.2600-2.640.60-2.353.20.30.250.26-2.640.59-2.360.29990.25020.2600-2.640.60-2.353.30.30.250.26-2.640.59-2.370.29990.25020.2601-2.640.60-2.363.40.30.250.26-2.640.59-2.380.29990.25030.2601-2.640.60-2.364.10.42。0.363.141一1.570.42000.00030.35993.141.00一1.574.20.42。0.363.141一1.580.42000.00020.36003.141.00一1.574.30.42。0.363.141一1.590.42010.00020.36013.141.00一1.574.40.42。0.363.141一1.60.42010.00030.36013.141.00一1.575.10.32-o.250.1630.265-o.840.3201-o.24980.16002.990.27-o.845.20.32-o.250.1630.265-o.850.3201-o.24980.16002.990.27-o.845.30.32-o.250.1630.265-o.860.3202-o.24990.16013.000.27-o.845.40.32-o.250.1630.265-o.870.3203-o.24990.16013.000.27-o.83...i.../\、

實驗結(jié)果與分析（必填）1.正運動學求解由上表5-2和表5-3可得，理論推導和仿真得出的末端坐標相差不超過0.2mm,歐拉角相差不超過0.01rad。在考慮計算數(shù)值誤差的前提下，可認為理論計算及其推導公式是正確的。2.逆運動學求解表6-1 ZJU-I型機械臂運動學仿真絕對誤差對照表組別絕對誤差1.1-0.00010.0001-0.0001-0.00100.00800.00001.2-0.00010.00010.0000-0.00100.00800.00001.3-0.00010.00010.0001-0.0010-0.01200.00001.4-0.00020.00010.0001-0.0010-0.01200.00002.1-0.00020.0000-0.00010.01800.01400.OllO2.2-0.00020.00000.00000.00800.00400.OllO2.3-0.00010.00010.0000-0.0020-0.02600.OllO2.4-0.00020.00010.0001-0.0020-0.01600.OllO3.1-0.00020.00020.00000.00000.01000.00003.2-0.00010.00020.00000.00000.01000.01003.3-0.00010.00020.00010.00000.01000.01003.4-0.00010.00030.00010.00000.01000.02004.10.00000.0003-0.00010.00000.00000.00004.20.00000.00020.00000.00000.00000.01004.30.00010.00020.00010.00000.00000.02004.40.00010.00030.00010.00000.00000.03005.10.00010.00020.0000-0.01000.00500.00005.20.00010.00020.0000-0.01000.00500.0100103 I0002I0001I0001I0000I0050I02004 0.0003 000 0.0050 0.0400由上表5-4和表5-5可得，理論推導和仿真得出的末端坐標相差不超過3mm,歐拉角相差不超過0.04ra。。但是，結(jié)合附錄表7-1，并4對于。11七、 附錄（）機械臂參數(shù)表7-1ZJU-I型桌面機械臂每—軸的角度約束關(guān)節(jié)—關(guān)節(jié)二關(guān)節(jié)三關(guān)節(jié)四關(guān)節(jié)五關(guān)節(jié)六最小關(guān)節(jié)值-200°-90°-120°-150°-150°-180°最大關(guān)節(jié)值200°90°120°150°150°180°表7-2ZJU -I型桌面機械臂每—軸的速度、加速約束關(guān)節(jié)—關(guān)節(jié)二關(guān)節(jié)三關(guān)節(jié)四關(guān)節(jié)五關(guān)節(jié)六關(guān)節(jié)速度100°/s100°/s100°/s100°/s100°/s100°/s關(guān)節(jié)加速度500°/2500°/ s2500°/ s2500°/ s2500°/ s2500°/ s2圖7-1機械臂幾何參數(shù)示意圖12（一二）。（）正運動學程序。正運動學計算器函數(shù)%calculateFunction.mfunction[Ta,b,c,f,e,d]=calculateFunct1on(al,a2,a3,a4,as, a6)%ZJU-I型桌面機械臂正運動學求解器%Tt07,a,b,c,d,e,fX'Y'Z歐拉拍t01=[cos(a1)-sin(al)00;sin(a1)cos(al)00;0010.23;0001];t12=[sin(a2)cos(a2)00;001-0.054;cos(a2)-sin(a2)00;0001];t23=[cos(a3)-sin(a3)00.185;sin(a3)cos(a3)00;0010;0001];t34=[-sin(a4)-cos(a4)00.17;cos(a4)-sin(a4)00;0010.077;0001];t45=[-sin(a5)-cos(aS)00;00-1-0.077;cos(a5)-sin(aS)00;0001];t56=[cos(a6)-sin(a6)00;00-10;sin(a6)cos(a6)00;0001];t67=[1000;0100;0 010.0855;0001];t17 =t12*t23*t34*t45*t56*t67;t07 =t01*t12*t23*t34*t45*t56*t67;T=t07;％獲取位置a= T(l,4);b=T(2,4);c=T(3,4);％獲取歐拉拍R=T(l:3,1:3)[d,e,f]2ale(R,");d:-d;e=-e;f=-f;13endMatlab運動計算器調(diào)用%mycalculate.mclc,clearall％調(diào)用ZJU-I型桌面機械臂正運動學求解器%[T,a,b,c,d,e,f]=calculateFunction(pi/6,pi/6,pi/3,0,pi/3,pi/6);%[T,a,b,c,d,e,f]=calculateFunction(pi/6,0,pi/6,0,pi/3,0);%[T,a,b,c,d,e,f]=calculateFunction(pi/2,0,pi/2,-pi/3,pi/3,pi/6);%[T,a,b,c,d,e,f]=calculateFunction(-pi/6,-pi/6,-pi/3,0,pi/12,pi/2);[T,a,b,c,d,e,f]=calculateFunction(pi/12,pi/12,pi/12,pi/12,pi/12,pi/12);%Tt07,ab,c代表位置，defX'Y'Z的歐拉拍T,a,b,c,d,e,f14I強化訓練與實踐實驗報告實驗名稱：機械臂正逆運動學實驗姓名與學號：指導老師：劉山所在學院：竺可楨學院年級與專業(yè)：2019級 機器人工程一、實驗原理（具體實現(xiàn)見頭文件bot）DH（）：i∝廠71??廠71????offset1000.4??1029°00??290°300.50??3049°00.89??4059°00??5-90°69°00??607000.50089°0000同時將各關(guān)節(jié)變量的限制儲存在數(shù)組中：DH0880??=0???1???2???3???4???5???6???7??8

0??：8 1 2 3 4 5 6 7 8（3）根據(jù)

0??的（4，1）（4，2）（4，3）XYZ歐88（eslt表示0??os）88IEERIEER56的d0.4和6??35-6d7由于ao01的dIER32和3同??30-1dz0.4ws（如下圖）進行計算。doublenewposdoublenewpos[6]={position[0],position[1],position[2]-d[0],position[3]+pi/2,position[4],position[5]};IER解法需要的坐標系461）XYZ060??。62坐標系6和7O6={0,,005}766（4）O4O6O70????6_7。6IERf1f23①記r=2+y2+z2=f12+f22+f32+a12+22+2d2?f3,將上圖中1,2,3的表達式代????3=(???a22?42/(2?a2?4,3值。②由z=sn2?sn∝1?f1+sθ2?sn∝1?f2+s∝1?f3+2?s∝1，u=anθ2(z?4c32?2(s3+a2u+z+4c3=,??值。2 2x=c1g1?s1g,y=s1g1+c1g1值： at2(g1*6)-2*O(),g1*60)+2*O()) 636④由3??=0??70??36636??4??5??6???4??6 ???4??5??6???4??6 ??4??563??=7 ??4??6 ???4??6 ???476??4??5??6+??4??6 ???4??5??6+??4??6 ??4??54at2(_(2,),R_(,)21和2）6由元素（1，1）和（1，3）得到??5值。??中。二、仿真試驗（具體實現(xiàn)見iktest.cpp）對三組關(guān)節(jié)角進行正運動學求解，并將關(guān)節(jié)角發(fā)布到gazebo仿真得到以下結(jié)果：（1）第一組：[0.927,-0.687,-0.396,0,1.083,0.927]第二組：[0.322,-0.855,-0.021,0,0.877,0.322]第三組：[-0.322,-0.636,-0.011,0,0.647,-0.322]對三組位置和姿態(tài)進行逆運動學結(jié)算后將得到的六祖關(guān)節(jié)角發(fā)布到gazebo得到以下結(jié)果：（1）第一組：[0.2,0.2,0.2007,1.57,-1.57,0]（2）第二組：[0.15,0.2,0.2007,0,0,0]（3）第三組：[0.3,0,0.122,1.57,0,0]三、實物實驗（具體實現(xiàn)見robotet.cpp）（1）正運動學第一組測量結(jié)果：【0.148，0.192，0.205】絕對誤差：【0.002，0.008，0.0043】（2）正運動學第二組測量結(jié)果：【0.302，0.092，0.189】絕對誤差：【0.002，0.008，0.0117】（3）正運動學第三組測量結(jié)果：【0.305，-0.103，0.275】絕對誤差：【0.005，0.003，0.005】（4）逆運動學第一組測量結(jié)果：【0.195，0.196，0.215】絕對誤差：【0.005，0.004，0.0143】（5）逆運動學第二組測量結(jié)果：【0.155，0.196，0.201】絕對誤差：【0.005，0.004，0.0003】（6）逆運動學第三組測量結(jié)果：【0.305，0，0.117】絕對誤差：【0.005，0，0.005】組號：暫未被分配

本科實驗報告姓名：學院：控制科學與工程學院系：控制科學與工程系專業(yè)：自動化（控制）學號：指導教師：吳俊2023年 3 月 29 日實驗報告

專業(yè)）姓名學號日期：2023年3月29日地點線仿真 課程稱機人模與制 指導： 吳俊 成： 實驗稱仿實二械臂到軌規(guī)實類型究計性 組學：無一、 實驗目和要必填）CopperliaSim理解并應用機械臂軌跡規(guī)劃知識，在各個關(guān)節(jié)軸的位置、速度、加速度的限制下，規(guī)劃處滿足要求的軌跡并使用仿真環(huán)境驗證；為實踐環(huán)境奠定基礎。二、 實驗內(nèi)和原必填）機械臂的軌跡規(guī)劃方法機械臂的軌跡規(guī)劃是指在滿足特定約束條件下，為機械臂從初始位置到目標位置生成一條連續(xù)、平滑且可行的路徑。在實際應用中，軌跡規(guī)劃需要考慮機械臂的關(guān)節(jié)和末端執(zhí)行器的運動范圍、速度和加速度限制以及避免碰撞等因素。從規(guī)劃的空間上來分，機械別的軌跡規(guī)劃方法有關(guān)節(jié)空間的軌跡規(guī)劃、笛卡爾空間的軌跡規(guī)劃。關(guān)節(jié)空間的規(guī)劃方法：計算量小，沒有奇異位型。但是對于末端復雜的軌跡不易轉(zhuǎn)換；笛卡爾空間的規(guī)劃方法：計算量很大，而且可能會有無逆解的現(xiàn)象，從而產(chǎn)生奇異位型。但是由于笛卡爾空間對于復雜軌跡的規(guī)劃較為直觀，轉(zhuǎn)換方便。從規(guī)劃空間內(nèi)，軌跡的生成方式來分，可以分為插值法、優(yōu)化法、人工勢場法、采樣法、動力學建模法等。插值法：插值法是一種簡單的軌跡規(guī)劃方法，包括線性插值、多項式插值和樣條插值等。這類方法通過在關(guān)節(jié)空間或笛卡爾空間生成一組離散的點，然后用合適的函數(shù)連接這些點，形成一條平滑的軌跡。優(yōu)化法：優(yōu)化方法通過定義一個代價函數(shù)來描述軌跡的優(yōu)劣，并嘗試找到使代價函數(shù)最小化的軌跡。代價函數(shù)可以包括路徑長度、能量消耗、執(zhí)行時間等因素。常用的優(yōu)化方法包括梯度下降法、模擬退火法和遺傳算法等。人工勢場法：人工勢場法將機械臂所在的環(huán)境視為一個勢場，目標位置產(chǎn)生吸引力，而障礙物產(chǎn)生排斥力。通過計算各種力的合成，生成一條使機械臂避免障礙物并朝向目標位置移動的軌跡。2采樣法：基于采樣的方法通過隨機或有規(guī)律地采樣機械臂的配置空間，然后將可行的采樣點連接起來，形成一棵搜索樹。常用的基于采樣的算法包括概率路線圖（PRM）、快速擴展隨機樹（RRT）和其變體。動力學建模法：軌跡生成算法通過對機械臂的動力學和運動學進行建模，直接計算滿足約束條件的軌跡。這類方法通常需要較高的數(shù)學和計算能力，但可以生成精確的軌跡。例如，最小二乘法、最大速度軌跡算法等。機械臂的關(guān)節(jié)空間軌跡規(guī)劃方法（初始角度移動到目標（目標角度插值法：這是一種簡單的關(guān)節(jié)空間軌跡規(guī)劃方法，它在初始關(guān)節(jié)角度和目標關(guān)節(jié)角度之間生成一組中間關(guān)節(jié)角度，然后通過合適的函數(shù)（如線性插值、多項式插值或樣條插值）連接這些點以生成平滑的軌跡。最小二乘法：這種方法通過對機械臂的動力學和運動學進行建模，計算一條使關(guān)節(jié)速度和加速度的平方誤差最小的軌跡。這可以通過求解一個最小二乘優(yōu)化問題來實現(xiàn)。最優(yōu)控制法：最優(yōu)控制法將軌跡規(guī)劃問題轉(zhuǎn)化為一個最優(yōu)控制問題，例如求解一條使能量消耗最小或運動時間最短的軌跡。這可以通過動態(tài)規(guī)劃或者模型預測控制（MPC）等方法實現(xiàn)?；诓蓸拥姆椒ǎ夯诓蓸拥姆椒ǎ绺怕事肪€圖（PRM）和快速擴展隨機樹（RRT）等，可以用于關(guān)節(jié)空間軌跡規(guī)劃。這些方法通過隨機或有規(guī)律地采樣關(guān)節(jié)空間，然后將可行的采樣點連接起來，形成一棵搜索樹，從而找到一條從初始關(guān)節(jié)配置到目標關(guān)節(jié)配置的路徑。多項式插值和樣條插值的區(qū)別多項式插值方法是通過一個多項式函數(shù)來擬合給定的一組離散數(shù)據(jù)點。給定n+1nphenomenon），多項式曲線在數(shù)據(jù)點附近出現(xiàn)振蕩，導致擬合效果不佳。（如二次或三次多項式（也稱為節(jié)點樣條滿足一定的光滑性條件，333-4-3兩點之間的五次樣條插值假設t=??0（00000t=??1時位于末位置的關(guān)節(jié)角為（??1??2??3??4??5??6,）.假設每個關(guān)節(jié)的初始速度和初06（初始5對任一關(guān)節(jié)角，令其關(guān)節(jié)角軌跡的5次多項式如下：0 1 2 3 4 5p(t)aatat2at3at4at50 1 2 3 4 5對上述公式進行一階和二階求導，可得，1 2 3 4 5v(t)a2at3at24at35at41 2 3 4 52 3 4 5a(t)2a2 3 4 5帶入邊界條件，可得，p(0)a00

6at12at220at3v(0)a0 1a(0)2a0 2p(t)aatat2at3at4at50 1 0 11 21 31 41 5111 21314151v(t)a2at3at24at3511 21314151 a(t)2a6at12at220at30 1 2 31 41 51假設關(guān)節(jié)角i的信息記錄為X0 i0 0 0 0初位置末位置初T速度、末速度、初加速度，末加速度，假設邊界條件信息帶入，可得，0 0 0 0 0 1a5t5

t4 t3

t2t

1a1 1 1 1 1

4X0

T0 0 0 0

0 0 0 0 1 0a3Mai 4 4t322t

1 0a 1 1 1 1

20 0 0 2 0 020t312t26t 2 0 0a5

1 1 1aM1X

0因此，在時間t0~t1

之間，任意時刻的關(guān)節(jié)角期望值為40 0 0 0 0 110t5

t4 t3

t2t

1 1 1 1 1

p(t)t5t4

t3t2t

1a=t5t4

t3t2t

0 0 0 0 1 0 0i 4

4t3

3t22t

1 0 0 1 1 1

0 0 0 2 0 0 020t312t26t 2 0 0 01 1 1 多點之間的變次數(shù)樣條插值（4-3-4）原問題可以轉(zhuǎn)換為對如下公式中4n+2個多項式函數(shù)參數(shù)的求解：SxSix,i0,1,n1y

bxxcxx2dxx3exx4,

i0,n1Sx

i i i i i i i i ii ybxxcxx2dxx3,

other i i i i i i i其中，令

kiyi1yi,i0,1,,n1hixi1xi,i0,1,,n1則，可以求解得多項式樣條的系數(shù)如下，

0 3k 6k 1

10c010c01000 0c0h2hh00c20h1h20

3k

h03k 1 0 1 1

21

cn2 0 0

hn32hn32hn2

hn2 3kn23kn3 c 0

0 h 2h

h

h h n1

n2

n2

n1

`n2 n33k 6k n1n2

i0

3h2

k0,h4

n1 n2i0bkici1hi2cihi,

i2,n2

d

0 0ci1ci,

i1,2,n2ih 3 3 i 3hi2k c h

i32k c3hn1n1n1,h

in1

n1n1,

in1hn1 0 03h20 0

3k0,h4

i0

n1

hn1k cn1n1,

in1h4

3h2n1

n15CopperliasimsysCall_init()sysCall_actuation()sysCall_actuation()每次trajectory_server()trajectory_server()函數(shù)擬合樣條的起點到其終點。三、 主要儀設備ZJU-ICoppeliaSimPython四、 操作方與實驟0，0，2起點位姿為：（笛卡爾空間位姿表示）（0.117，0.334，0.499，-2.019，-0.058，-2.190）終點位姿為：（笛卡爾空間位姿表示）（0.32，-0.25，0.16，3，0.265，-0.84）表ZJU-I型機械臂的逆運動學計算組別要求的末端位姿逆運動學解析解計算的關(guān)節(jié)角(rad)123456起點(0.117,0.334,0.499,-2.019,-0.058,-2.190)1.04690.54320.5314-0.55150.52390.6985起點(0.117,0.334,0.499,-2.019,-0.058,-2.190)1.04691.0516-0.53140.00290.52390.6985起點(0.117,0.334,0.499,-2.019,-0.058,-2.190)-1.9328-1.05080.51260.0532-2.75940.7854起點(0.117,0.334,0.499,-2.019,-0.058,-2.190)-1.9328-0.5604-0.51260.5879-2.75940.7854終點(0.32,-0.25,0.16,3,0.265,-0.84)-0.73571.10251.0532-0.87540.0748-0.0128終點(0.32,-0.25,0.16,3,0.265,-0.85)-0.73572.1065-1.05320.22690.0748-0.01286終點(0.32,-0.25,0.16,3,0.265,-0.86)2.5266-2.10721.0513-0.2177-3.10170.1029終點(0.32,-0.25,0.16,3,0.265,-0.87)2.5266-1.1049-1.05130.8826-3.10170.1029使用自帶的逆運動學求解器驗證(degree)圖自帶的逆運動學求解器驗證起點計算結(jié)果(degree)圖自帶的逆運動學求解器驗證終點計算結(jié)果選取起點的逆運動學解（關(guān)節(jié)角參數(shù),rad）為(1.04691599,1.05168993,-0.53145444,0.00294117,0.52390861,0.69854057)選取終點的逆運動學解（關(guān)節(jié)角參數(shù),rad）為(-0.7356519,1.10250941,1.05320703,-0.87536185,0.07485627,-0.04280543)五、 實驗數(shù)記錄理得到控制結(jié)果如下所示，7圖機械臂6個關(guān)節(jié)的角度圖機械臂6個關(guān)節(jié)的旋轉(zhuǎn)速度圖機械臂6個關(guān)節(jié)的旋轉(zhuǎn)加速度8圖機械臂到達起始位置圖機械臂到達終點位置六、 實驗結(jié)與分必填）2.1s0.69854057)的起始點到達關(guān)節(jié)角參數(shù)為（0.320,-0.250,0.160,3.000,0.264,-0.841）的終點，且滿足機械臂的角度、速度、加速度的限制，故軌跡規(guī)劃推導與程序編寫正確。9附錄(一) 機械臂數(shù)表7-1ZJU-I型桌面機械臂每一軸的角度約束關(guān)節(jié)一關(guān)節(jié)二關(guān)節(jié)三關(guān)節(jié)四關(guān)節(jié)五關(guān)節(jié)六最小關(guān)節(jié)值?200°?90°?120°?150°?150°?180°最大關(guān)節(jié)值200°90°120°150°150°180°表7-2ZJU-I型桌面機械臂每一軸的速度、加速度約束關(guān)節(jié)一關(guān)節(jié)二關(guān)節(jié)三關(guān)節(jié)四關(guān)節(jié)五關(guān)節(jié)六關(guān)節(jié)速度100°/????100°/????100°/????100°/????100°/????100°/????關(guān)節(jié)加速度500°/????2500°/????2500°/????2500°/????2500°/????2500°/????210實驗三定點轉(zhuǎn)動實驗姓名 學號 2020年6月1日實驗原理與主要公式實驗原理運用雅可比矩陣，在笛卡爾空間中根據(jù)末端速度計算各關(guān)節(jié)角速度。主要公式1）雅可比2）逆運動學求解當最后三軸相交時，連桿坐標系{4}、{5}、{6}的原點均位于這個交點上，這點的基坐標如下：利用 得到：仿真實驗步驟、成果與結(jié)果分析仿真實驗步驟1）編寫程序計算實時關(guān)節(jié)角度對應的旋轉(zhuǎn)矩陣，用雅可比矩陣計算各關(guān)節(jié)的瞬時角速度，并隨著角速度變化實時更新關(guān)節(jié)角度及對應的旋轉(zhuǎn)矩陣；2）probot_gazeboCmakelist3）進入速度控制器，運行編寫的.cpp文件，通過速度控制使機械臂繞定點轉(zhuǎn)動，在仿真軟件的左側(cè)欄讀取機械臂的末端位置，與初始末端位置進行比較。仿真實驗成果表1-仿真末端位置xyz理論值0.228900.454仿真值0.228877-0.0001130.454217誤差0.0000230.0001130.000217表2-仿真末端姿態(tài)rxryrz初始值1.57070.00000.0000終點值-0.852857-1.545931-2.275600仿真實驗結(jié)果分析xyz0.1mm40°，因此可以認為該實驗在誤差允許范圍內(nèi)達成定點轉(zhuǎn)動的實驗目的。實物實驗步驟、成果與結(jié)果分析實物實驗步驟1）編寫程序計算實時關(guān)節(jié)角度對應的旋轉(zhuǎn)矩陣，用雅可比矩陣計算各關(guān)節(jié)的瞬時角速度，并隨著角速度變化實時更新關(guān)節(jié)角度及對應的旋轉(zhuǎn)矩陣；2）在probot_gazebo的Cmakelist文件中登記節(jié)點及所用到的庫；3）message；4）進入速度控制器，運行編寫的.cpp文件，通過速度控制使機械臂繞定點轉(zhuǎn)動，在仿真軟件的左側(cè)欄讀取機械臂的末端位置，與初始末端位置進行比較；5）斷電并手動恢復機械臂至初始位姿。實物實驗成果表3-實際實驗末端位置xyz理論值0.228900.454仿真值0.2340.0000.450誤差0.00510.0000.004仿真實驗結(jié)果分析xyz1mm40°，因此可以認為該實驗在誤差允許范圍內(nèi)達成定點轉(zhuǎn)動的實驗目的。實驗二軌跡生成實驗一、實驗原理1、機械臂軌跡的五次多項式插值方法2、通過速度控制使機械臂按預期的軌跡運動到指定位置二、主要公式1、機械臂軌跡的五次多項式插值方法給定一系列的位置信息(q0,q1,……,qn)，根據(jù)逆運動學求得每個中間點對應的關(guān)節(jié)角度(θ0,θ1,……,θn)，考慮到六軸機械臂的特點，給定一個機械臂末端位姿對應有多個關(guān)節(jié)角度解，故以給定的起點位置對應的關(guān)節(jié)角度為(0,0,0,0,0,0)，通過選取下一個中間點若干個關(guān)節(jié)角解中與上一個已確定的解的歐氏距離最小的解，繼而確定所有中間點對應的唯一關(guān)節(jié)角度。考慮2個數(shù)據(jù)點之間插值的情況，五次多項式插值方法的數(shù)學表達式為：taatat2at3at4at5f 0 1 2 3 4 5其中，a0,,為待確定的參數(shù)。這里我們一共需要6個約束條件，即起點和終點的位置、速度和加速度信息。即給定如下條件：（t0=00（t=aatat2at3at4at5 f f

0 1f

2f 3f

4f 5f（t0=01（t=a2atat24at3at4 f f 1 2f 3f

4f 5f（t0=022（t=2a

6at112at220at3求解得

f f

2 3f 4f 5fa0=0a=1 02a=022

(812)t

)t2a= f

0 f 0 f

0 f f30 0

2t3f(1416)tf

(32)t2a4=

f f 0 ff2t4f

0 f f22)t)t

f

120

f

160)tf

f f5

f2t5fn2二次規(guī)劃解決軌跡規(guī)劃由于給定4個關(guān)節(jié)點，利用上述五次多項式插值方法，可得到3條樣條曲線f(t)，有t)5

ati,0tt(t)

1(t)

i05

1iati,t

1tt2 i02i 1 2 5 i3t)i03it,t2tt3軌跡需滿足以下條件（1）期望的中間點關(guān)節(jié)角度；（2）相鄰軌跡在中間點速度連續(xù)；（3）相鄰軌跡在中間點加速度連續(xù)；第j條樣條曲線在t=tj-1時刻，函數(shù)值為第j-1個中間點對應的關(guān)節(jié)角，在t=tj時刻，函數(shù)值為第j個中間點對應的關(guān)節(jié)角，即j(Tj1)j1,j=1,2j(Tj)j第j個中間點的相鄰軌跡fj(t)和fj+1(t)的速度和加速度連續(xù)，即(k)(t

)(k)(t

)，k=1,2,j=1,2j j jjJerk即加加速度盡量小，以避免加速度和速度的突變。加速度與速度限制關(guān)節(jié)1關(guān)節(jié)2關(guān)節(jié)3關(guān)節(jié)4關(guān)節(jié)5關(guān)節(jié)6v_max(rad/s)0.94250.83780.94251.15191.15191.1519a_max(rad/s2)0.36650.27490.36650.45820.45820.4582因此，定義每個樣條段優(yōu)化目標cost代價函數(shù)設定為所有軌跡曲線的jerk的L2范數(shù)，即3 tj

3tti1

ij1

(t) dt2結(jié)合二次規(guī)劃（QP）進行路徑規(guī)劃，QP公式如下min12

xTHxfTxLBxUBxAxbHkernelequalityequalityboundarymatrix，Ainequalitymatrix，binequalityboundarymatrixcost函數(shù)轉(zhuǎn)化為條件如下（1）代價函數(shù)ai0 ai0a ai1 i1tt t2 t3 t4 t5ai23t0 0 0 6 24t 60t2ai2i a i ai3 i3ai4 ai4 i5 i53 tj

3根據(jù)代價函數(shù)tti1

j1

(t)

dt，研究第i條樣條曲線的加加速度Jerk2代價有

0

ai00 a i1tj3t2dta a a a a a

tj0

0 0 0 6

60t2ai2t i i0

i1 i2

i3 i4

i5 t

j1

j16

ai324t 60t20 0 0 0 0 0

ai4 i5

ai00 0 0 0 0 0 a i1a a a a a a

tj0 0 0 0 0 0 ai2ati0 i1 i2 i3 i4 i5at

j10 0 0 36

2dt

i30 0 0

576t2

1440t3

ai4 a a a a a a

0000000000ai000000ai100000ai20036t72t2120t3a0072t2192t3360t4ai4 00120t3360t4720t5i50

0 0 036t2 144t3 360t4 i5i0 i1 i2

i3 i4

i5 0000

i3（2）約束條件同理，將上述起始約束轉(zhuǎn)換為QP約束等式Aeqxbeq，約束條件有期望的中間點關(guān)節(jié)角角度1,2,3,4a a a a a a

1t

1110 11 12 13 14 15

a20

a21

a22

a23

a24

a25t22a a a a a a

t3

30 31 32 33 34 35

3a40

a41

a42

a43

a44

a45

t4t5

4相鄰軌跡在中間點位置、速度、加速度連續(xù)iii1i速度連續(xù)：(1)t(1)ti i i1 i加速度連續(xù)：(2)t(2)ti i i1 iQPAxb，約束條件有ai0ai1關(guān)節(jié)速度約束t01 2 4t3 4ai2v,t

tti a

ii1 ii3ai4 i5ai0ai1關(guān)節(jié)加速度約束2t0 0 2 6t 12t2 20t3ai2a,t

tti a

ii1 ii3二、仿真實驗1、仿真實驗步驟

i=1,2,3,4,5,6

ai4 i5空間路徑點位姿開始軌跡規(guī)劃時間結(jié)束？YesNo結(jié)束軌跡規(guī)劃是否到達中間點YesNo誤差計算，輸出Subscriber訂閱當前關(guān)節(jié)角度、當前關(guān)節(jié)角速度從多解中確定距離上個路徑點角度最近的一個解帶入五次多項式獲得各個關(guān)節(jié)速度Subscriber訂閱當前時間publish發(fā)布速度五次多項式軌跡規(guī)劃方程計算第0個路徑點對應的關(guān)節(jié)角(0,0,0,0,0,0)計算第i個路徑點對應的關(guān)節(jié)角代碼功能實現(xiàn)方框圖仿真實驗代碼實現(xiàn)邏輯如上方框圖所示。ROSGazebo2、仿真實驗成果與結(jié)果分析仿真過程中到達各個中間點機械臂位置如下圖所示。經(jīng)過第一點 經(jīng)過第二點 經(jīng)過第三點 經(jīng)過第四點根據(jù)訂閱的機械臂末端位姿得到的軌跡規(guī)劃過程中依次到達各個中間點時的末端位置與速度，與理論值進行比較分析，如下表所示。xyxyz理論值0.300-0.1000.122仿真值0.290698-0.103570.133734絕對誤差0.0093020.003570.011734相對誤差3.10%3.57%9.62%（單位:m）由上表絕對誤差分析和視頻中機械臂實物運行過程可知，在誤差允許范圍內(nèi)，機械臂安全平穩(wěn)運行，從仿真視頻以及打印輸出的實時數(shù)據(jù)分析可得，仿真模型能夠到達過程中各個中間點，完成軌跡規(guī)劃過程。三、實物實驗1、實物實驗步驟運行機械臂軌跡規(guī)劃程序，觀察機械臂運行狀態(tài)。完成軌跡規(guī)劃過程后，測量機械臂末端位置，與理論值進行比較分析。2、實物實驗成果與結(jié)果分析視頻文件見軌跡規(guī)劃實物視頻.mp4實物實驗中過程中到達各個中間點機械臂位置如下圖所示。 實物機械臂到達第一個點時位姿 實物機械臂到達第二個點時位姿實物機械臂到達第三個點時位姿 實物機械臂到達第四個點時位姿xyxyz理論值0.300-0.1000.122測量值0.318-0.1110.107絕對誤差0.0180.0110.015相對誤差6.00%11.00%12.29%（單位:m）由于機械臂末端姿態(tài)難以測量，通過目測，大致符合理論設定值角度[1.57,1.57,0],如下圖所示。實物機械臂到達第四個點時位姿機與機械臂通訊存在偏差以及機械臂運動引入的系統(tǒng)誤差因素。實驗報告課程名稱：I強化訓練與實踐實驗題目：機械臂力矩控制指導老師：劉山學生姓名與學號：一、 力控原理解析1、力矩計算原理本次力控實驗采用牛頓歐拉法迭代計算力矩，其理論迭代式如下：2、五次多項式規(guī)劃原理將自定義的插值條件代入上述三個公式，可以得到一個六元方程組，如下所示：六個等式的右側(cè)的位置、速度、加速度已知，可以求解出a0到a5六個系數(shù)的表達式如下：將這六個系數(shù)代回到公式：即可得到可用于速度控制的五次插值多項式。3、力矩控制機械臂原理：.052s0.30.10.3]處二、 力控代碼實現(xiàn)1、五次多項式規(guī)劃代碼：MatrixXdplanner(doubleplanning_time,MatrixXdinit_final_position,MatrixXdinit_final_velocity,MatrixXdinit_final_acceleration){doubleinit_position,final_position,init_velocity,final_velocity,init_acceleration,final_acceleration;MatrixXdPoly_coef(6,6);for(inti=0;i<6;i++){init_position=init_final_position(i,0);final_position=init_final_position(i,1);//賦值初末位置init_velocity=init_final_velocity(i,0);final_velocity=init_final_velocity(i,1);//賦值初末速度init_acceleration=init_final_acceleration(i,0);final_acceleration=init_final_acceleration(i,1);//賦值初末加速度Poly_coef(i,0)=init_position;Poly_coef(i,1)=init_velocity;Poly_coef(i,2)=0.5*init_acceleration;Poly_coef(i,3)=(20*final_position-20*init_position-(8*final_velocity+12*init_velocity)*planning_time-(3*init_acceleration-final_acceleration)*pow(planning_time,2))/(2*pow(planning_time,3));Poly_coef(i,4)=(30*init_position-30*final_position+(14*final_velocity+16*init_velocity)*planning_time+(3*init_acceleration-2*final_acceleration)*pow(planning_time,2))/(2*pow(planning_time,4));Poly_coef(i,5)=(12*final_position-12*init_position-(6*final_velocity+6*init_velocity)*planning_time-(init_acceleration-final_acceleration)*pow(planning_time,2))/(2*pow(planning_time,5));}returnPoly_coef;}2、牛頓歐拉法迭代for(inti=0;i<6;i++){R=CaL_T(updated_state(i,0),i).block(0,0,3,3);P=CaL_T(updated_state(i,0),i).block(0,3,3,1);w[i+1]=R*w[i]+updated_state(i,1)*Z;Vector3dVector1=R*w[i];dw[i+1]=R*dw[i]+Vector1.cross(updated_state(i,1)*Z)+updated_state(i,2)*Z;a[i+1]=R*(dw[i].cross(P)+w[i].cross(w[i].cross(P))+a[i]);ac[i]=dw[i+1].cross(Pc[i])+w[i+1].cross(w[i+1].cross(Pc[i]))+a[i+1];}

F[i+1]=m[i]*ac[i];Vector3dVector2=Ic[i]*w[i+1];N[i+1]=Ic[i]*dw[i+1]+w[i+1].cross(Vector2);for(inti=6;i>0;i--){R=CaL_T(updated_state(i,0),i).block(0,0,3,3);P=CaL_T(updated_state(i,0),i).block(0,3,3,1);f[i]=R.inver