【《环保自动植树机器人控制系统设计》19000字】

[23]求解工作空间。其主要思想是根据每个关节的活动范围，在其相应的活动范围内随机取值，这些随机值的末端便构成了工作空间。4.2.2工作空间仿真基于上面的6自由度机械臂的D-H模型，利用matlabroboticstoolbox建立机器人的连杆模型，程序参考附录一（2）。具体过程如下所示：求解所建立机械臂的运动学正解，得到机械臂末端的坐标矢量。这里可以直接应用之前运动学分析中的成果。在MATLABR2019b的环境中，利用Rand()函数产生随机数为关节i转动范围最小值，为转动范围最大值。设置N=2000，得到2000个随机取得的工作点，也就是机械臂末端在基坐标系中的2000个随机位姿。模型以及结果如下所示：(a)整体侧视图(b)X方向视图(c)Y方向视图(d)Z方向视图图4.3蒙特卡洛法工作空间分析由以上的仿真结果可以得到，机械臂工作空间在X、Y、Z方向各范围内，点数均密集分布，漏失的空洞较少，其工作空间结构紧凑，验证了基于蒙特卡洛方法的植树机器人工作空间分析的正确性，为后续运动学分析提供了参考依据。4.3关节空间轨迹规划研究机械臂运动路径点（节点）可以用工具坐标系｛T｝相对于工作坐标系｛S｝的位姿来表示。为了求得在关节空间形成所求轨迹，首先用运动学反解将路径点转换成关节矢量角度值，然后对每个关节拟合一个光滑函数，使之从起始点开始，依次通过所有路径点，最后到达目标点。对于每一段路径，各个关节运动时间均相同，这样保证所有关节同时到达路径点和终止点，从而得到工具坐标系｛T｝应有的位置和姿态。尽管每个关节在同一段路径中的运动时间相同，但各个关节函数之间却是相互独立的。关节空间法是以关节角度的函数描述机器人轨迹的，关节空间法不必在直角坐标系中描述两个路径点之间的路径形状，计算简单、容易。此外，由于关节空间与直角坐标空间之间并不是连续的对应关系，因而不会发生机构的奇异性问题。在关节空间中进行轨迹规划，需要给定机器人在起始点和终止点手臂的位形。对关节进行插值时，应满足一系列的约束条件，例如在进行种植作业时，执行末端在初始点、夹住树苗运动过程、下降进行栽种等节点上的位姿、速度和加速度的要求；与此相应的各个关节位移、速度、加速度在整个时间间隔内连续性要求；其极值必须在各个关节变量的容许范围之内等。在满足所要求的约束条件下，可以选取不同类型的关节插值函数，生成不同的轨迹。下面着重讨论关节轨迹的插值方法。关节轨迹插值计算的方法较多，现简述如下。4.3.1三次多项式插值在机械臂的运动过程中，因为已知相对于起始点的关节角度，且可以通过运动学反解得到终止点的关节角。因此，运动轨迹的描述，可用包含起始点和终止点关节角度的一个平滑插值函数来表示，在时刻的值为起始关节角度，在终端时刻的值是终止关节角度。显然，有许多平滑函数可作为关节插值函数，如图4.4所示。图4.4单个关节的不同轨迹曲线为了实现单个关节的平稳运动，轨迹函数至少需要满足四个约束条件，其中两个约束条件是起始点和终止点对应的关节角度： (4.1)为了满足关节运动速度的连续性要求，另外还有两个约束，即在起始点和终止点的关节速度要求。在当前情况下，规定： （4.2）上述四个边界约束条件（4.1）和式（4.2）唯一地确定了一个三次多项式： （4.3）运动轨迹上的关节速度和加速度则为： （4.4）将（4.3）（4.4）代入相应的约束条件，得到和系数，，，有关的四个方程：

（4.5）求解上述方程组，得 （4.6）此解只适用于关节起始和终止速度为0的情况，在种植过程中，机械臂在起始点（树苗框）和目标点的速度均为0，这种方法适用。4.3.2五次多项式插值如果对于运动轨迹有更为严格、细致的标准要求，则需要更多的约束条件，那么三次多项式就不能满足需要，必须用更高阶的多项式对运动轨迹的路径段进行插值。比如，对某段路径的起始点和终止点都规定了关节的位置、速度和加速度，则要用一个五次多项式进行插值，即 （4.7）此时多项式的系数必须受到以下约束： （4.8）解得这个方程组为： （4.9）对于起始点和目标点来说，它们对应的六个关节角速度和关节角加速度为零，即，，，，得到特解形式如下所示： （4.10）4.3.3三次与五次多项式插值仿真详细程序见论文末尾附录一（3）。(a)位置(b)速度(c)加速度图4.5三次多项式插值实验图详细程序见论文末尾附录一（4）。(a)位置(b)速度(c)加速度图4.6五次多项式插值实验图由以上三次和五次多项式插值仿真实验，我们可以看到，五次多项式插值的结果曲线更加平滑，没有生硬、突然的转折。进一步地我们可以得到一个结论，五次多项式插值法效果更好，因为其保证了关节角、关节角速度和角加速度的连贯性。在工程层面上，最直接的好处就是使得机械臂关节处的电机更加稳定运行，电机寿命也更长，机械臂效果更好，更利于种植作业。相较于三次多项式插值法，五次多项式插值法需要求解一个六元一次方程组，比前者多采用了关节角加速度的束缚条件。因此五次多项式插值法计算量相应也要更大一些，这是无法避免的。4.4笛卡尔空间轨迹规划研究4.4.1空间直线插补算法空间直线插补算法是根据始末A、B两点位姿，并求A、B两点间直线上各插补点的位姿。假设直线始末两点的坐标值分别为和。在进行直线插补时，机械臂的姿态随给定的步长从初始姿态均匀向末端姿态变化。具体通过以下步骤进行定步长插补：设定沿直线运动的速度和插补周期；计算两点空间间的距离：；计算插补总时间；计算插补次数。当余数为零时，；当余数不为零时，；其中为取整函数。插补次数能够反映了直线的精度，通常来说插补次数越多，机械臂的轨迹越接近直线。计算插补增量：（4.11）计算插补点的坐标值：（4.12）求出齐次变换矩阵，用齐次变换矩阵乘以新坐标下点的坐标，得到空间点相对于基坐标系下的坐标值。再通过运动学反解，得到各个点的关节角。4.4.2空间直线插补仿真实验直线插补时，指定起止坐标与速度。确定要插直线的周期增量，分解到xyz轴。详细代码参考附录一（5）。图4.7空间直线插补仿真实验图仿真结果可以看出通过一些列小的中间连续点，插补构成整体的直线轨迹。插补次数越多，其轨迹越接近直线。4.4.3空间圆弧插补算法空间圆弧是指三维空间中任一平面内的圆弧。因而要实现空间圆弧插补算法，就要先把空间圆弧的转化为平面圆弧。空间圆弧插补总体过程可以分为三步：第一步，化空间圆弧为平面圆弧；第二步，用平面圆弧插补算法，求出平面圆弧插补点的坐标；第三步，将这些点的坐标转化为空间坐标系下的坐标值。空间中三个点，，不共线，那么它们共同决定了一个空间圆弧。为了把空间圆弧转化为平面圆弧，首先建立以为坐标的平面圆弧，新坐标系的原点设为A点。将作为新坐标系的轴正方向，方向作为轴正方向，由右手法则确定轴正方向。坐标示意图如图4.8示。图4.8空间圆弧转平面圆弧示意图圆弧位于平面内，此时一个空间圆弧问题就转化为了平面圆弧问题。如下图所示。图4.9平面圆弧插补对于基础坐标系的空间圆弧三点，和，在平面中可以表示为，，，其中，，。其插补算法步骤如下：1．若B点是圆弧上的一点，则先要确定圆心坐标。设圆心相对与基础坐标系的坐标为，由三点的坐标以及辅助点Q，P，其中线段AB垂直于OQ，BC垂直于OP。如图4.10,可以列出下列方程组。（4.13）其中为基础坐标系的圆心,,,使用高斯列主元消元法即可求得三元方程组，从而求出圆心相对与基础坐标系的坐标为，为了插补计算简便，将坐标系的原点转移到圆弧的圆心上面。图4.10平面圆弧插补算法2．求圆弧的半径和总的圆心角C、A间圆弧角度为（4.14）其中R为圆弧半径，3．设定步数N，计算角位移，再求出各插补点的坐标，从而进行插补（4.15）中间点的坐标为（4.16）中间点的坐标为（4.17）4．求解两个坐标系的变换矩阵，令M表示由坐标系向基础坐标系的变换矩阵，M由，，矢量在基础坐标系中的方向余弦和在基础坐标系下的坐标构成。（4.18）（4.19）（4.20）（4.21）将，，分别化为单位向量：，，，变化矩阵M可以表示为（4.22）利用上面介绍的平面圆弧插补算法，求出插补点在坐标系中的坐标值。再将计算得到的插值点坐标变换到基础坐标系中（4.23）为空间中R点相对于基础坐标系的坐标值，为空间中R点相对于的坐标值。

4.4.4空间圆弧插补仿真实验详细程序代码见附录一（6）。图4.11空间圆弧插补图仿真结果可以看出通过一些列小的中间连续点，构成整体的圆弧轨迹，所插入点越多，其轨迹就越平滑。圆弧插补是复杂动作的重要组成部分。4.5基于MATLAB机械臂轨迹规划仿真研究 针对实际中需要的使用的运动轨迹，简化各类动作，设计直线与圆弧形轨迹规划，为后续植树机械臂一些列动作奠定基础。

4.5.1直线轨迹研究详细程序代码见附录一（7）。图4.12直线轨迹示意图机械臂沿直线路径运动，这种基本的运动路径是各动作中最常见的，是复杂动作的基础。在机械臂进行种植时，最后一阶段将采用直线轨迹，将树苗插入土内。4.5.2圆弧形轨迹研究详细程序代码见附录一（8）。(a)整体侧视图(b)Y方向视图(c)X方向侧视图(d)Z方向视图图4.13圆弧轨迹示意图仿真实验中，机械臂的圆弧动作平滑、美观，圆弧形运动在机械臂取树苗和栽种树苗的过程中是必不可少的环节，在从树苗存放处到夹取树苗以后，将做圆弧形运动至栽种点上方。4.5.3种植过程仿真机械臂的路径规划图如图14所示。机器人的机械臂初始位置为A点（树苗存放处），然后控制执行末端以圆弧形轨迹伸至D点，再通过直线形轨迹运动至P点，执行种植。栽种完成以后，机械臂按原路径返回至初始位置。机械臂由A点运动经由B点、C点、D点及其逆过程都采用PTP（点到点）圆弧插补的方式运动，由D点到P点的运动过程采用PTP直线插补的方式运动。图4.14机械臂路径规划示意图通过调用teach()函数，可以实时得到机械臂位姿，图4.15所示为机械臂初始位置，图4.16至图4.18分别为其他中间位置状态，图4.19为机械臂目标位置。(a)机械臂位于初始位置A (b)机械臂位于中间位置B(c)机械臂位于中间位置C(d)机械臂位于中间位置D(e)机械臂位于目标位置P图4.14机械臂进行植树作业轨迹目标位置A为取树苗位置，也是初始位置。B、C为中间过程位置，D为圆弧轨迹终点位置，P为目标栽种位置。4.6本章小结本章对于机械臂轨迹规划的基本知识做了介绍。阐述了机械臂模型建立的基本原则、基本方法，进一步地，对于所建立的机械臂的工作空间进行了仿真模拟，对于轨迹规划进行了三次、五次多项式插值法，比较了二者的优劣。然后针对建立的机械臂模型，采用MATLAB进行插补仿真实验，以及轨迹规划实例，模拟了机械臂的运动过程，绘制出机械臂的作业轨迹，直观地验证了该轨迹规划的可行性及有效性。最后，对于移动平台上的机械臂植树过程进行路径规划。

5.结论与展望5.1结论本课题主要以植树机器人为应用背景，构建了以四轮小车为载体的移动平台，实现了户外地形的树苗种植。植树机器人以机械臂为核心执行端，从车载树苗框内夹取树苗，进行栽种作业。目前无论是在工业界还是学术界，对于将机械臂与小型移动平台结合作业的设计还不多见，本课题有着很好的研究意义。文本主要内容包括机械臂的建模与工作空间分析、机械臂正逆运动学的快速求解、机械臂在自由空间中的轨迹规划方法以及机械臂的避障运动规划方法。首先，总结了机械臂逆运动学以及运动规划的研究现状；接着描述了机械臂的数学模型以及运动时涉及到的轨迹规划方法。本文完成的主要工作及研究成果如下：1.通过对机器人关键部件的选型，自主设计并搭建具有灵活性、协作性的四轮式小车硬件平台。并对该硬件平台利用数学方法进行运动学分析，建立了数学模型，并求解各轮速度对机器人移动平台的速度贡献，确保了在理论设计上的可行性。2.机械臂是进行种植作业的核心部件，首先采用标准D-H建模方法建立机械臂模型。分析了机械臂的正运动学，从逐次解耦的方向推导逆运动学的闭合解析解，再用仿真验证推导过程是否正确。最后通过蒙特卡洛法对机械臂的工作空间进行仿真，为后续运动规划打下基础。再对比实现机械臂关节空间轨迹规划有两种方法：三次多项式插值法和五次多项式插值法，对两种插值法进行比较分析，为够保证角加速度的连贯性，以及使电机平稳运行，选择了五次多项式插值法。然后在笛卡尔空间中规划机械臂的轨迹，利用空间直线和空间圆弧插值算法，实现机械臂在规定路径上运动。最后，通过MATLAB仿真实验，模拟了机械臂的完整种植动作，验证了计划轨迹的可行性。5.2展望未来自动植树机器人平台将会为林业、环境等相关领域做出巨大贡献。由于疫情原因，本文的室外实验较少，后期研究可在本文研究的基础上补充更多的室外场景实验。本文依然存在很多不足之处，将来可以在以下这些问题上做出进一步研究：1.种植机械臂的研究方向有避障功能研究、冗余空间研究、姿态多样性等，是目前研究的热点所在。通过提高机械臂的仿生水平，提高执行效率，进而缩短机械臂到达目标点的时间，从而提高自动植树机器人的作业速度和种植成功率，是接下来研究中要解决的一个关键问题。由于Arduino开发板价格较为低廉，其性能稍有欠缺，例如：机械臂在种植过程中可能遇到障碍物，对于障碍物的自动规避是一个研究重点，但这些问题都需要使用更为复杂的算法进行处理，增加了开发难度，可以在后续的基础上使用性能更好、稳定性更强的开发板，比如STM32F407开发板。2.野外地形复杂，既有相对平坦的地区，也有存在许多岩石和树杈等障碍。在这样的情况，轮式和履带式机器人移动缓慢，甚至无法前进，它们不断与地面产生摩擦，对机器人零部件造成很大的损伤，而多足移动平台能够适应这样的地形条件，加上近年来仿生学研究的不断深入，结合自然界动物独特的结构优势，仿生多足机器人的研究将成为机器人移动平台研究的重点之一。3.机器人的作业内容不断增加，在本项目中，施肥、浇水等等动作可以同步进行，此时多机械手的协同作业就显得愈加重要。结合森林各项作业内容，今后可以在多机械手协同作业方面开展深入研究。5.3社会影响力分析（1）从经济角度考量。目前机械臂的研究已经越来越广泛，成品的机械臂在市场上也随处可见。其成本已经大大下降，因而应用于植树作业上也不失为一种较好的选择。相较于传统的飞机播撒树种等方式，由于其间距统一，利于其成长为树木后对于阳光、养分的吸收，故成活率更高一些。（2）从人力资源角度考量。避免了劳动力进行低技术的重复性劳作，可以实现恶劣环境的全天候作业。（3）从环境角度考量。自动植树机器人对于环境的改善有很大作用，所播树种，栽种的树苗，对于全球变暖，水土流失，沙尘暴等都有较好的治理作用。

附录一（1）运动学反解验证clearall;closeall;%输入已知信息%K=input('按ai-1、αi-1、θi、di为列向量输入矩阵K；')K=[00pi/20;170-pi/2pi/20;560000;155-pi/20365;0pi/200;0-pi/200];n=6;Trans=eye(4);fori=1：nt=[cos(K(i,3))-sin(K(i,3))0K(i,1);sin(K(i,3))*cos(K(i,2))cos(K(i,3))*cos(K(i,2))-sin(K(i,2))-K(i,4)*sin(K(i,2));sin(K(i,3))*sin(K(i,2))cos(K(i,3))*sin(K(i,2))cos(K(i,2))K(i,4)*cos(K(i,2));0001];eval(strcat('T',num2str(i),'=t'))Trans=Trans*t;endTrans%运动反解p=input('输入要求的θi中的i的具体数值：');symsb1b2b3b4b5b6;ifp==2b1=input('θ1值：');elseifp==3b1=input('θ1值：');b2=input('θ2值：');elseifp==4b1=input('θ1值：');b2=input('θ2值：');b3=input('θ3值：');elseifp==5b1=input('θ1值：');b2=input('θ2值：');b3=input('θ3值：');b4=input('θ4值：');elseifp==6b1=input('θ1值：');b2=input('θ2值：');b3=input('θ3值：');b4=input('θ4值：');b5=input('θ5值：');end%W：θ角未知，ai、αi、di均已知的信息矩阵W=[00b10;170-pi/2b20;5600b30;155-pi/2b4365;0pi/2b50;0-pi/2b60];fori=1：pk=[cos(W(i,3))-sin(W(i,3))0W(i,1);sin(W(i,3))*cos(W(i,2))cos(W(i,3))*cos(W(i,2))-sin(W(i,2))-W(i,4)*sin(W(i,2));sin(W(i,3))*sin(W(i,2))cos(W(i,3))*sin(W(i,2))cos(W(i,2))W(i,4)*cos(W(i,2));0001];Trans=inv(k)*Trans;endRight=eye(4);fori=p+1：nt=[cos(K(i,3))-sin(K(i,3))0K(i,1);sin(K(i,3))*cos(K(i,2))cos(K(i,3))*cos(K(i,2))-sin(K(i,2))-K(i,4)*sin(K(i,2));sin(K(i,3))*sin(K(i,2))cos(K(i,3))*sin(K(i,2))cos(K(i,2))K(i,4)*cos(K(i,2));0001];Right=Right*t;endS=Right-Transdouble(solve(S(1,1))*180/pi)double(solve(S(1,2))*180/pi)double(solve(S(1,3))*180/pi)double(solve(S(1,4))*180/pi)double(solve(S(2,1))*180/pi)double(solve(S(2,2))*180/pi)double(solve(S(2,3))*180/pi)double(solve(S(2,4))*180/pi)

（2）机械臂工作空间仿真程序

（3）三次多项式插值算法clear;clc;q_array=[0,50,150,100,0];%指定起止位置t_array=[0,2,4,8,10];%指定起止时间v_array=[0,10,20,-15,0];%指定起止速度t=[t_array(1)];q=[q_array(1)];v=[v_array(1)];a=[0];%初始状态fori=1:1:length(q_array)-1;%每一段规划的时间a0=q_array(i);a1=v_array(i);a2=(3/(t_array(i+1)-t_array(i))^2)*(q_array(i+1)-q_array(i))-(1/(t_array(i+1)-t_array(i)))*(2*v_array(i)+v_array(i+1));a3=(2/(t_array(i+1)-t_array(i))^3)*(q_array(i)-q_array(i+1))+(1/(t_array(i+1)-t_array(i))^2)*(v_array(i)+v_array(i+1));%计算三次多项式系数ti=t_array(i)+0.001:0.001:t_array(i+1);qi=a0+a1*(ti-t_array(i))+a2*(ti-t_array(i)).^2+a3*(ti-t_array(i)).^3;vi=a1+2*a2*(ti-t_array(i))+3*a3*(ti-t_array(i)).^2;ai=2*a2+6*a3*(ti-t_array(i));t=[t,ti];q=[q,qi];v=[v,vi];a=[a,ai];endsubplot(3,1,1),plot(t,q,'r'),xlabel('t'),ylabel('position');gridon;subplot(3,1,2),plot(t,v,'b'),xlabel('t'),ylabel('velocity');gridon;subplot(3,1,3),plot(t,a,'g'),xlabel('t'),ylabel('accelerate');gridon;

（4）五次多项式插值算法clear;clc;q_array=[0,50,150,100,40];%指定起止位置t_array=[0,3,6,12,14];%指定起止时间v_array=[0,10,20,-15,0];%指定起止速度a_array=[0,20,30,-20,0];%指定起止加速度t=[t_array(1)];q=[q_array(1)];v=[v_array(1)];a=[a_array(1)];%初始状态fori=1：1：length(q_array)-1;%每一段规划的时间T=t_array(i+1)-t_array(i)a0=q_array(i);a1=v_array(i);a2=a_array(i)/2;a3=(20*q_array(i+1)-20*q_array(i)-(8*v_array(i+1)+12*v_array(i))*T-(3*a_array(i)-a_array(i+1))*T^2)/(2*T^3);a4=(30*q_array(i)-30*q_array(i+1)+(14*v_array(i+1)+16*v_array(i))*T+(3*a_array(i)-2*a_array(i+1))*T^2)/(2*T^4);a5=(12*q_array(i+1)-12*q_array(i)-(6*v_array(i+1)+6*v_array(i))*T-(a_array(i)-a_array(i+1))*T^2)/(2*T^5);%计算五次多项式系数ti=t_array(i)：0.001：t_array(i+1);qi=a0+a1*(ti-t_array(i))+a2*(ti-t_array(i)).^2+a3*(ti-t_array(i)).^3+a4*(ti-t_array(i)).^4+a5*(ti-t_array(i)).^5;vi=a1+2*a2*(ti-t_array(i))+3*a3*(ti-t_array(i)).^2+4*a4*(ti-t_array(i)).^3+5*a5*(ti-t_array(i)).^4;ai=2*a2+6*a3*(ti-t_array(i))+12*a4*(ti-t_array(i)).^2+20*a5*(ti-t_array(i)).^3;t=[t,ti(2：end)];q=[q,qi(2：end)];v=[v,vi(2：end)];a=[a,ai(2：end)];endsubplot(3,1,1),plot(t,q,'r'),xlabel('t'),ylabel('position');holdon;plot(t_array,q_array,'o','color','g'),gridon;subplot(3,1,2),plot(t,v,'b'),xlabel('t'),ylabel('velocity');holdon;plot(t_array,v_array,'*','color','y'),gridon;subplot(3,1,3),plot(t,a,'g'),xlabel('t'),ylabel('accelerate');holdon;plot(t_array,a_array,'^','color','r'),gridon;

（5）空间直线插补算法clear;clc;p0=[1,2,3];pf=[2,4,5];%指定起止位置v=0.1;%指定速度x=[p0(1)];y=[p0(2)];z=[p0(3)];L=((pf(1)-p0(1))^2+(pf(2)-p0(2))^2+(pf(3)-p0(3))^2)^0.5;%直线长度N=L/v;%插补次数dx=(pf(1)-p0(1))/N;%每个周期各轴增量dy=(pf(2)-p0(2))/N;dz=(pf(3)-p0(3))/N;fort=1:1:N%插补x(t+1)=x(t)+dx;y(t+1)=y(t)+dy;z(t+1)=z(t)+dz;endplot3(x,y,z,'r'),xlabel('x'),ylabel('y'),zlabel('z'),holdon,plot3(x,y,z,'o','color','g'),gridon;

（6）空间圆弧插补算法point_x1=[12;3;4;1];%起始点point_x2=[13;5;6;1];%终点point_x3=[11;3;5;1];%路径点x1=point_x1(1);x2=point_x2(1);x3=point_x3(1);y1=point_x1(2);y2=point_x2(2);y3=point_x3(2);z1=point_x1(3);z2=point_x2(3);z3=point_x3(3);A=[(x1-x2),(y1-y2),(z1-z2);(x2-x3),(y2-y3),(z2-z3);(z2-z1)*(y3-y2)-(y2-y1)*(z3-z2),(x2-x1)*(z3-z2)-(z2-z1)*(x3-x2),(y2-y1)*(x3-x2)-(x2-x1)*(y3-y2)];%三元一次方程系数矩阵B=[1/2*(x1^2-x2^2+y1^2-y2^2+z1^2-z2^2);1/2*(x2^2-x3^2+y2^2-y3^2+z2^2-z3^2);x1*((z2-z1)*(y3-y2)-(y2-y1)*(z3-z2))+y1*((x2-x1)*(z3-z2)-(z2-z1)*(x3-x2))+z1*((y2-y1)*(x3-x2)-(x2-x1)*(y3-y2))];%三元一次方程解矩阵O=inv(A)*B%圆心坐标矩阵point_circle(1)=O(1);point_circle(2)=O(2);point_circle(3)=O(3);R=sqrt((point_x1(1)-point_circle(1))^2+(point_x1(2)-point_circle(2))^2+(point_x1(3)-point_circle(3))^2);%圆半径L=sqrt((point_x1(1)-point_x2(1))^2+(point_x1(2)-point_x2(2))^2+(point_x1(3)-point_x2(3))^2);%圆弧两点距离theta=2*asin(L/(2*R));%圆弧跨度角n=[(point_x1(1)-point_circle(1))/R;(point_x1(2)-point_circle(2))/R;(point_x1(3)-point_circle(3))/R];%齐次变换矩阵n向量x1tox2=[point_x2(1)-point_x1(1);point_x2(2)-point_x1(2);point_x2(3)-point_x1(3)];a1=cross(n,x1tox2);a_1=a1(1)/sqrt(a1(1)^2+a1(2)^2+a1(3)^2);a_2=a1(2)/sqrt(a1(1)^2+a1(2)^2+a1(3)^2);a_3=a1(3)/sqrt(a1(1)^2+a1(2)^2+a1(3)^2);a=[a_1;a_2;a_3];%齐次变换距阵a向量o=cross(a,n);%齐次变换距阵o向量T=[n(1),o(1),a(1),point_circle(1);n(2),o(2),a(2),point_circle(2);n(3),o(3),a(3),point_circle(3);0,0,0,1];%齐次变换矩阵fork=(0：30)x(k+1)=R*cos(k*theta/30);y(k+1)=R*sin(k*theta/30);X_base=T*[x(k+1);y(k+1);0;1];X1(k+1)=X_base(1);Y1(k+1)=X_base(2);Z1(k+1)=X_base(3);endplot3(X1,Y1,Z1,'rp')%三维绘图holdonplot3(point_circle(1),point_circle(2),point_circle(3),'rp')xlabel('X轴'),ylabel('Y轴'),zlabel('Z轴')title('空间圆弧插补图')legend('插补圆弧')

（7）机械臂直线运动MATLAB仿真实验代码clc;clear;%建立机器人模型%thetadaalphaoffsetL1=Link([00.40.025pi/20]);%定义连杆的D-H参数L2=Link([pi/200.5600]);L3=Link([000.035pi/20]);L4=Link([00.5150pi/20]);L5=Link([pi00pi/20]);L6=Link([00.08000]);robot=SerialLink([L1L2L3L4L5L6],'name','zhixian');%连接连杆，机器人取名zhixianT1=transl(0.5,0,0);%根据给定起始点，得到起始点位姿T2=transl(0,0.5,0.5);%根据给定终止点，得到终止点位姿T=ctraj(T1,T2,50);Tj=transl(T);plot3(Tj(:,1),Tj(:,2),Tj(:,3));%输出末端轨迹gridon;q=robot.ikine(T)holdonrobot.plot(q);%动画演示

（8）圆弧形轨迹%%创建机械臂L1=Link([012.40pi/20-pi/2]);L2=Link([000-pi/2]);L3=Link([015.430pi/2]);L4=Link([000-pi/200]);L5=Link([015.9250pi/2]);L6=Link([000-pi/2]);L7=Link([015.0000pi/2]);Rbt=SerialLink([L1L2L3L4L5L6L7],'name','zhixian');%%规划路径q0=[0000000];qsq1=[0.460880.3769901.3101.44510];qsq2=[.816810.5654901.068101.25660];qsq3=[2.360.6911500.84801.44510];qsq4=[2.660.3769901.3101.44510];qsq5=[pi/20.6283101.570800.942490];qsq6=[00.6283101.570800.942490];t=0：.04：2;sqtraj1=jtraj(q0,qsq1,t);sqtraj2=jtraj(qsq1,qsq2,t);sqtraj3=jtraj(qsq2,qsq3,t);sqtraj4=jtraj(qsq3,qsq4,t);sqtraj5=jtraj(qsq4,qsq1,t);sqtraj6=jtraj(qsq1,q0,t);sqtraj7=jtraj(qsq6,q0,t);holdonatj=zeros(4,4);%%绘制动画view(-35,40)xlim([-40,40])ylim([-40,40])zlim([0,60])fori=1：1：51atj=Rbt.fkine(sqtraj1(i,：));JTA(i,：)=transl(atj);jta=JTA;plot3(jta(i,1),jta(i,2),jta(i,3),'r.')Rbt.plot(sqtraj1(i,：))plot3(JTA(i,1),JTA(i,2),JTA(i,3),'b')endfori=1：1：51atj2=Rbt.fkine(sqtraj2(i,：));JTA2(i,：)=transl(atj2);jta2=JTA2;plot3(jta2(i,1),jta2(i,2),jta2(i,3),'r.')Rbt.plot(sqtraj2(i,：))plot3(JTA2(i,1),JTA2(i,2),JTA2(i,3),'b')endfori=1：1：51atj3=Rbt.fkine(sqtraj3(i,：));JTA3(i,：)=transl(atj3);jta3=JTA3;plot3(jta3(i,1),jta3(i,2),jta3(i,3),'r.')Rbt.plot(sqtraj3(i,：))plot3(JTA3(i,1),JTA3(i,2),JTA3(i,3),'b')endfori=1：1：51atj4=Rbt.fkine(sqtraj4(i,：));JTA4(i,：)=transl(atj4);jta4=JTA4;plot3(jta4(i,1),jta4(i,2),jta4(i,3),'r.