《机器人建模与控制 》 实验报告汇 项目1-10 机器人实验平台及基本操作 - 单关节机械臂协作控制实验

浙江大学工程师学院实验报告课程名： 工业机人系控制姓 名：专 业： 控制工程学 号：指导教： 吴俊2017年 月27日机器人实验平台及基本操作浙江大学工程师学院实验报告实验项目名称： 机器人实验平台及基本操作 组学生姓名： 实验地点： 工业机器人实验中心 实验日期：2017 年月24日一、 认识机器人平台实验用的机器人有6 个关节从基座开始第1个关节称为轴1第2个关节称为轴2、…、最后一个关节称为末端执行器 。机器人关机后再次开启电源至少需要等2 分钟。防护装置停止状态。使能器电机开启状态；第二档按下去，机器人将处于防护装置停止状态。二、 机器人平台手动操纵1-3”的动作模式，前后摇动操纵杆可以控制轴2轴1/逆时钟方向旋转操纵杆可以控制轴3的运动。4-6”的动作模式，前后摇动操纵杆可以控制轴5轴4/逆时钟方向旋转操纵杆可以控制轴6的运动。2机器人实验平台及基本操作xyz轴正x轴y轴运动，顺/逆时钟旋转操纵杆可以控制机器人末端沿着z轴运动。三、 RAPD程序初步RAPD1个初始位姿出发，拿起工件，移至其它位置，放下工件，机器人手爪返回初始位姿。程序代码：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程序编辑器进行编程实现机器人拿放工件动作。xxxxxxxx@zu.eu.c201712月8日24：004组号：暂未被分配

沁户丿驴本科实验报告姓名：学院：系：专业：

控制科学与工程学院控制科学与工程系自动化（控制）学号：指导教师： 吴俊2023年 3 月 20 日泊J 塌擘实验报告

专业：姓名：学号：日期：2023年3月20日地点： 线上仿真 课程名称：机器人建模与控制指导老师：吴俊成绩： 实验名称：机械臂正、逆运动学求解实验类型：设计验证性同组学生： 无、实验目的和要求（必填）、CopperliaSim的软件环境理解并应用机械臂正使用仿真环境进行验证为实践环境萸定基础＿、 实验内容和原理（必填）D-H参数以关节O厂1为原点，轴i-1为z轴（正向自选），以与下一个关节形成的几何连杆为x轴（正方向为指向下一级连杆的方向），以右手定则确定Y轴，对每一个关节建立联体坐标系。当然，该方法建立的坐标系是不唯一的。臂的动学求解在改进D书法建立的联体坐标系中，可知前后柜邻的两级关节坐标系转换公式为cos0, —sin01 。 a一1式为i-1r —n,sa,1 s8,sa,1 —sna,1 —ma1d,1I 'sna-1 s8n1-1a,1 sa,-1，1机械臂的逆运 动学求解逆运动学的求解过程是已知笛卡尔空间中末端执行器的位姿信息，反求关节空间中的各个关节的可变参数信息的过程。该过程的求解方法有解析法和数值法两种，可能存在多组解。封闭解法是指基于解析形式的算法。将运动学的封闲解法分为两类：代数解法和几何解法。数值解法具有迭代性质，所以比封阳解法的求解速度慢得多。在多重解的情况下，数值解法不能求出全部的解。解析解的存在性6自由度橾作臂都是可解的6自由有解析解。这种存在解析解（封闲解的操作臂具有如下特性存在几个正交0或士90°6个旋转关节的操作臂存在封闲解的充分条件是相邻的三个关节轴线相交于一点。2对于X'Y'万欧拉角，其转换为旋转矩阵得公式如下，2Rx'Y'Z'(a,fJ,Y)=

C2C3C1S3+C3S1S2S1的一C1C3S2

的S3C钰1C1的S3

S2-c2匀C1C2通过计算可以得知，旋转矩阵转换为欧拉角得公式如下，�=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组关节角参数带入正运动学解，计算机械臂末端Tip点的空间位置，663，，＇亢一6亢一32,663，，＇亢一6亢一32,＇60（（（（（@122，12，全一工1，12， －'工12．4将以上5组关节角分别输入122，12，全一工1，12， －'工12．46ZU-I解析解）6．器进行相关计算），计算对应的．@(-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. 将所 求关节角为参仿真程从仿真中得到机械臂末端执行的空间位置姿，与第6步给定的位置姿态行。五、 实验记录和处理机械臂的改所示表5-1ZJU-I型机械臂的改进DH参数表序号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关节角的正运动学解计算机械臂末端以'Y'欧拉角表示需要记录计算过程。Matlab,创建函数,依次将多角的齐次变换矩阵相乘得到锻后的总Matlab计算反三角函数X'Y'Z'欧拉角。表5-2ZJU-I型机械臂的正运动学计算序号关节角参数位置欧拉角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，了 飞当关节角为，了 飞l47l4745374541223666。-0.5000汀。-0.50000 0一冗冗亢）齐次变换矩阵为。l45379502461024066为阵矩换变次齐0。l45379502461024066为阵矩换变次齐00635820700508350302850o，0539o398勹, 6060430,66o0＇－_－－＿，T7冗石-，冗石一寸／角节关当4．62290120o62290120o－05为阵矩换变次齐,36883848502000700冗节8500'O0冗节,'－冗了l冗节=冗节,T7冗节，冗节寸／角节关当5．汀詈

0.7702I183。80I183。807061?4009。0.6356

0.5928l6l6195712051215660。-0.6830664 仿真程 序验证3中的计算结果，将仿真得到的末端位姿与第3步得到的计算结果进行比对；表 5-3 Z型机械臂的正运动学仿真序号关节角参数位置欧拉角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述，关于6角的运解析解表，由表达式的正负中4个解（角度约束的）。8955。4此4个不同9的逆运动学解，得到4组不同的的关节角。将数据带入所得解析解的公式中，可以计算得到如下数据，表型机臂的逆运动计算组别要求的末端位姿逆运动学解析解计算的关节角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将所求关节角作为参数输入仿真程序，从仿真中得到机械臂末端执行器的空。表5-5 型机臂的逆运动学真组别要求的末端位姿由逆运动学关节角计算的末端位姿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.../\、

实验结果与分析（必填）1.正运动学求解由上表5-2和表5-3可得，理论推导和仿真得出的末端坐标相差不超过0.2mm,欧拉角相差不超过0.01rad。在考虑计算数值误差的前提下，可认为理论计算及其推导公式是正确的。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。。但是，结合附录表7-1，并4对于。11七、 附录（）机械臂参数表7-1ZJU-I型桌面机械臂每—轴的角度约束关节—关节二关节三关节四关节五关节六最小关节值-200°-90°-120°-150°-150°-180°最大关节值200°90°120°150°150°180°表7-2ZJU -I型桌面机械臂每—轴的速度、加速约束关节—关节二关节三关节四关节五关节六关节速度100°/s100°/s100°/s100°/s100°/s100°/s关节加速度500°/2500°/ s2500°/ s2500°/ s2500°/ s2500°/ s2图7-1机械臂几何参数示意图12（一二）。（）正运动学程序。正运动学计算器函数%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运动计算器调用%mycalculate.mclc,clearall％调用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强化训练与实践实验报告实验名称：机械臂正逆运动学实验姓名与学号：指导老师：刘山所在学院：竺可桢学院年级与专业：2019级 机器人工程一、实验原理（具体实现见头文件bot）DH（）：i∝厂71𝑎厂71𝑑𝜃offset1000.4𝜃1029°00𝜃290°300.50𝜃3049°00.89𝜃4059°00𝜃5-90°69°00𝜃607000.50089°0000同时将各关节变量的限制储存在数组中：DH0880𝑇=0𝑇∙1𝑇∙2𝑇∙3𝑇∙4𝑇∙5𝑇∙6𝑇∙7𝑇8

0𝑇：8 1 2 3 4 5 6 7 8（3）根据

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值。𝜃中。二、仿真试验（具体实现见iktest.cpp）对三组关节角进行正运动学求解，并将关节角发布到gazebo仿真得到以下结果：（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]对三组位置和姿态进行逆运动学结算后将得到的六祖关节角发布到gazebo得到以下结果：（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]三、实物实验（具体实现见robotet.cpp）（1）正运动学第一组测量结果：【0.148，0.192，0.205】绝对误差：【0.002，0.008，0.0043】（2）正运动学第二组测量结果：【0.302，0.092，0.189】绝对误差：【0.002，0.008，0.0117】（3）正运动学第三组测量结果：【0.305，-0.103，0.275】绝对误差：【0.005，0.003，0.005】（4）逆运动学第一组测量结果：【0.195，0.196，0.215】绝对误差：【0.005，0.004，0.0143】（5）逆运动学第二组测量结果：【0.155，0.196，0.201】绝对误差：【0.005，0.004，0.0003】（6）逆运动学第三组测量结果：【0.305，0，0.117】绝对误差：【0.005，0，0.005】组号：暂未被分配

本科实验报告姓名：学院：控制科学与工程学院系：控制科学与工程系专业：自动化（控制）学号：指导教师：吴俊2023年 3 月 29 日实验报告

专业）姓名学号日期：2023年3月29日地点线仿真 课程称机人模与制 指导： 吴俊 成： 实验称仿实二械臂到轨规实类型究计性 组学：无一、 实验目和要必填）CopperliaSim理解并应用机械臂轨迹规划知识，在各个关节轴的位置、速度、加速度的限制下，规划处满足要求的轨迹并使用仿真环境验证；为实践环境奠定基础。二、 实验内和原必填）机械臂的轨迹规划方法机械臂的轨迹规划是指在满足特定约束条件下，为机械臂从初始位置到目标位置生成一条连续、平滑且可行的路径。在实际应用中，轨迹规划需要考虑机械臂的关节和末端执行器的运动范围、速度和加速度限制以及避免碰撞等因素。从规划的空间上来分，机械别的轨迹规划方法有关节空间的轨迹规划、笛卡尔空间的轨迹规划。关节空间的规划方法：计算量小，没有奇异位型。但是对于末端复杂的轨迹不易转换；笛卡尔空间的规划方法：计算量很大，而且可能会有无逆解的现象，从而产生奇异位型。但是由于笛卡尔空间对于复杂轨迹的规划较为直观，转换方便。从规划空间内，轨迹的生成方式来分，可以分为插值法、优化法、人工势场法、采样法、动力学建模法等。插值法：插值法是一种简单的轨迹规划方法，包括线性插值、多项式插值和样条插值等。这类方法通过在关节空间或笛卡尔空间生成一组离散的点，然后用合适的函数连接这些点，形成一条平滑的轨迹。优化法：优化方法通过定义一个代价函数来描述轨迹的优劣，并尝试找到使代价函数最小化的轨迹。代价函数可以包括路径长度、能量消耗、执行时间等因素。常用的优化方法包括梯度下降法、模拟退火法和遗传算法等。人工势场法：人工势场法将机械臂所在的环境视为一个势场，目标位置产生吸引力，而障碍物产生排斥力。通过计算各种力的合成，生成一条使机械臂避免障碍物并朝向目标位置移动的轨迹。2采样法：基于采样的方法通过随机或有规律地采样机械臂的配置空间，然后将可行的采样点连接起来，形成一棵搜索树。常用的基于采样的算法包括概率路线图（PRM）、快速扩展随机树（RRT）和其变体。动力学建模法：轨迹生成算法通过对机械臂的动力学和运动学进行建模，直接计算满足约束条件的轨迹。这类方法通常需要较高的数学和计算能力，但可以生成精确的轨迹。例如，最小二乘法、最大速度轨迹算法等。机械臂的关节空间轨迹规划方法（初始角度移动到目标（目标角度插值法：这是一种简单的关节空间轨迹规划方法，它在初始关节角度和目标关节角度之间生成一组中间关节角度，然后通过合适的函数（如线性插值、多项式插值或样条插值）连接这些点以生成平滑的轨迹。最小二乘法：这种方法通过对机械臂的动力学和运动学进行建模，计算一条使关节速度和加速度的平方误差最小的轨迹。这可以通过求解一个最小二乘优化问题来实现。最优控制法：最优控制法将轨迹规划问题转化为一个最优控制问题，例如求解一条使能量消耗最小或运动时间最短的轨迹。这可以通过动态规划或者模型预测控制（MPC）等方法实现。基于采样的方法：基于采样的方法，如概率路线图（PRM）和快速扩展随机树（RRT）等，可以用于关节空间轨迹规划。这些方法通过随机或有规律地采样关节空间，然后将可行的采样点连接起来，形成一棵搜索树，从而找到一条从初始关节配置到目标关节配置的路径。多项式插值和样条插值的区别多项式插值方法是通过一个多项式函数来拟合给定的一组离散数据点。给定n+1nphenomenon），多项式曲线在数据点附近出现振荡，导致拟合效果不佳。（如二次或三次多项式（也称为节点样条满足一定的光滑性条件，333-4-3两点之间的五次样条插值假设t=𝑡0（00000t=𝑡1时位于末位置的关节角为（𝜃1𝜃2𝜃3𝜃4𝜃5𝜃6,）.假设每个关节的初始速度和初06（初始5对任一关节角，令其关节角轨迹的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假设关节角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

之间，任意时刻的关节角期望值为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 多点之间的变次数样条插值（4-3-4）原问题可以转换为对如下公式中4n+2个多项式函数参数的求解：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则，可以求解得多项式样条的系数如下，

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()函数拟合样条的起点到其终点。三、 主要仪设备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型机械臂的逆运动学计算组别要求的末端位姿逆运动学解析解计算的关节角(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)图自带的逆运动学求解器验证起点计算结果(degree)图自带的逆运动学求解器验证终点计算结果选取起点的逆运动学解（关节角参数,rad）为(1.04691599,1.05168993,-0.53145444,0.00294117,0.52390861,0.69854057)选取终点的逆运动学解（关节角参数,rad）为(-0.7356519,1.10250941,1.05320703,-0.87536185,0.07485627,-0.04280543)五、 实验数记录理得到控制结果如下所示，7图机械臂6个关节的角度图机械臂6个关节的旋转速度图机械臂6个关节的旋转加速度8图机械臂到达起始位置图机械臂到达终点位置六、 实验结与分必填）2.1s0.69854057)的起始点到达关节角参数为（0.320,-0.250,0.160,3.000,0.264,-0.841）的终点，且满足机械臂的角度、速度、加速度的限制，故轨迹规划推导与程序编写正确。9附录(一) 机械臂数表7-1ZJU-I型桌面机械臂每一轴的角度约束关节一关节二关节三关节四关节五关节六最小关节值−200°−90°−120°−150°−150°−180°最大关节值200°90°120°150°150°180°表7-2ZJU-I型桌面机械臂每一轴的速度、加速度约束关节一关节二关节三关节四关节五关节六关节速度100°/𝑠𝑠100°/𝑠𝑠100°/𝑠𝑠100°/𝑠𝑠100°/𝑠𝑠100°/𝑠𝑠关节加速度500°/𝑠𝑠2500°/𝑠𝑠2500°/𝑠𝑠2500°/𝑠𝑠2500°/𝑠𝑠2500°/𝑠𝑠210实验三定点转动实验姓名 学号 2020年6月1日实验原理与主要公式实验原理运用雅可比矩阵，在笛卡尔空间中根据末端速度计算各关节角速度。主要公式1）雅可比2）逆运动学求解当最后三轴相交时，连杆坐标系{4}、{5}、{6}的原点均位于这个交点上，这点的基坐标如下：利用 得到：仿真实验步骤、成果与结果分析仿真实验步骤1）编写程序计算实时关节角度对应的旋转矩阵，用雅可比矩阵计算各关节的瞬时角速度，并随着角速度变化实时更新关节角度及对应的旋转矩阵；2）probot_gazeboCmakelist3）进入速度控制器，运行编写的.cpp文件，通过速度控制使机械臂绕定点转动，在仿真软件的左侧栏读取机械臂的末端位置，与初始末端位置进行比较。仿真实验成果表1-仿真末端位置xyz理论值0.228900.454仿真值0.228877-0.0001130.454217误差0.0000230.0001130.000217表2-仿真末端姿态rxryrz初始值1.57070.00000.0000终点值-0.852857-1.545931-2.275600仿真实验结果分析xyz0.1mm40°，因此可以认为该实验在误差允许范围内达成定点转动的实验目的。实物实验步骤、成果与结果分析实物实验步骤1）编写程序计算实时关节角度对应的旋转矩阵，用雅可比矩阵计算各关节的瞬时角速度，并随着角速度变化实时更新关节角度及对应的旋转矩阵；2）在probot_gazebo的Cmakelist文件中登记节点及所用到的库；3）message；4）进入速度控制器，运行编写的.cpp文件，通过速度控制使机械臂绕定点转动，在仿真软件的左侧栏读取机械臂的末端位置，与初始末端位置进行比较；5）断电并手动恢复机械臂至初始位姿。实物实验成果表3-实际实验末端位置xyz理论值0.228900.454仿真值0.2340.0000.450误差0.00510.0000.004仿真实验结果分析xyz1mm40°，因此可以认为该实验在误差允许范围内达成定点转动的实验目的。实验二轨迹生成实验一、实验原理1、机械臂轨迹的五次多项式插值方法2、通过速度控制使机械臂按预期的轨迹运动到指定位置二、主要公式1、机械臂轨迹的五次多项式插值方法给定一系列的位置信息(q0,q1,……,qn)，根据逆运动学求得每个中间点对应的关节角度(θ0,θ1,……,θn)，考虑到六轴机械臂的特点，给定一个机械臂末端位姿对应有多个关节角度解，故以给定的起点位置对应的关节角度为(0,0,0,0,0,0)，通过选取下一个中间点若干个关节角解中与上一个已确定的解的欧氏距离最小的解，继而确定所有中间点对应的唯一关节角度。考虑2个数据点之间插值的情况，五次多项式插值方法的数学表达式为：taatat2at3at4at5f 0 1 2 3 4 5其中，a0,,为待确定的参数。这里我们一共需要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二次规划解决轨迹规划由于给定4个关节点，利用上述五次多项式插值方法，可得到3条样条曲线f(t)，有t)5

ati,0tt(t)

1(t)

i05

1iati,t

1tt2 i02i 1 2 5 i3t)i03it,t2tt3轨迹需满足以下条件（1）期望的中间点关节角度；（2）相邻轨迹在中间点速度连续；（3）相邻轨迹在中间点加速度连续；第j条样条曲线在t=tj-1时刻，函数值为第j-1个中间点对应的关节角，在t=tj时刻，函数值为第j个中间点对应的关节角，即j(Tj1)j1,j=1,2j(Tj)j第j个中间点的相邻轨迹fj(t)和fj+1(t)的速度和加速度连续，即(k)(t

)(k)(t

)，k=1,2,j=1,2j j jjJerk即加加速度尽量小，以避免加速度和速度的突变。加速度与速度限制关节1关节2关节3关节4关节5关节6v_max(rad/s)0.94250.83780.94251.15191.15191.1519a_max(rad/s2)0.36650.27490.36650.45820.45820.4582因此，定义每个样条段优化目标cost代价函数设定为所有轨迹曲线的jerk的L2范数，即3 tj

3tti1

ij1

(t) dt2结合二次规划（QP）进行路径规划，QP公式如下min12

xTHxfTxLBxUBxAxbHkernelequalityequalityboundarymatrix，Ainequalitymatrix，binequalityboundarymatrixcost函数转化为条件如下（1）代价函数ai0 ai0a ai1 i1tt t2 t3 t4 t5ai23t0 0 0 6 24t 60t2ai2i a i ai3 i3ai4 ai4 i5 i53 tj

3根据代价函数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）约束条件同理，将上述起始约束转换为QP约束等式Aeqxbeq，约束条件有期望的中间点关节角角度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相邻轨迹在中间点位置、速度、加速度连续iii1i速度连续：(1)t(1)ti i i1 i加速度连续：(2)t(2)ti i i1 iQPAxb，约束条件有ai0ai1关节速度约束t01 2 4t3 4ai2v,t

tti a

ii1 ii3ai4 i5ai0ai1关节加速度约束2t0 0 2 6t 12t2 20t3ai2a,t

tti a

ii1 ii3二、仿真实验1、仿真实验步骤

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

ai4 i5空间路径点位姿开始轨迹规划时间结束？YesNo结束轨迹规划是否到达中间点YesNo误差计算，输出Subscriber订阅当前关节角度、当前关节角速度从多解中确定距离上个路径点角度最近的一个解带入五次多项式获得各个关节速度Subscriber订阅当前时间publish发布速度五次多项式轨迹规划方程计算第0个路径点对应的关节角(0,0,0,0,0,0)计算第i个路径点对应的关节角代码功能实现方框图仿真实验代码实现逻辑如上方框图所示。ROSGazebo2、仿真实验成果与结果分析仿真过程中到达各个中间点机械臂位置如下图所示。经过第一点 经过第二点 经过第三点 经过第四点根据订阅的机械臂末端位姿得到的轨迹规划过程中依次到达各个中间点时的末端位置与速度，与理论值进行比较分析，如下表所示。xyxyz理论值0.300-0.1000.122仿真值0.290698-0.103570.133734绝对误差0.0093020.003570.011734相对误差3.10%3.57%9.62%（单位:m）由上表绝对误差分析和视频中机械臂实物运行过程可知，在误差允许范围内，机械臂安全平稳运行，从仿真视频以及打印输出的实时数据分析可得，仿真模型能够到达过程中各个中间点，完成轨迹规划过程。三、实物实验1、实物实验步骤运行机械臂轨迹规划程序，观察机械臂运行状态。完成轨迹规划过程后，测量机械臂末端位置，与理论值进行比较分析。2、实物实验成果与结果分析视频文件见轨迹规划实物视频.mp4实物实验中过程中到达各个中间点机械臂位置如下图所示。 实物机械臂到达第一个点时位姿 实物机械臂到达第二个点时位姿实物机械臂到达第三个点时位姿 实物机械臂到达第四个点时位姿xyxyz理论值0.300-0.1000.122测量值0.318-0.1110.107绝对误差0.0180.0110.015相对误差6.00%11.00%12.29%（单位:m）由于机械臂末端姿态难以测量，通过目测，大致符合理论设定值角度[1.57,1.57,0],如下图所示。实物机械臂到达第四个点时位姿机与机械臂通讯存在偏差以及机械臂运动引入的系统误差因素。实验报告课程名称：I强化训练与实践实验题目：机械臂力矩控制指导老师：刘山学生姓名与学号：一、 力控原理解析1、力矩计算原理本次力控实验采用牛顿欧拉法迭代计算力矩，其理论迭代式如下：2、五次多项式规划原理将自定义的插值条件代入上述三个公式，可以得到一个六元方程组，如下所示：六个等式的右侧的位置、速度、加速度已知，可以求解出a0到a5六个系数的表达式如下：将这六个系数代回到公式：即可得到可用于速度控制的五次插值多项式。3、力矩控制机械臂原理：.052s0.30.10.3]处二、 力控代码实现1、五次多项式规划代码：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