1.3 机器人自主移动与作业技术

1.3.1 机器人自主移动技术

复杂多变的环境和任务要求智能机器人具有更好的地形适应能力和自主移动能力，而足式机器人比轮式机器人更能适应各种复杂、崎岖的地形。作为典型的多足步行机器人，六足机器人具有丰富的运动形式、冗余的肢体结构以及良好的灵活度和稳定性，能够广泛适应各种崎岖地形，因此适用于对自主性和可靠性要求比较高的任务。

图1-49所示为哈尔滨工业大学研制的小型六足机器人HITCR，它具有体积小、重量轻的特点，便于其他大型移动设备携带，且可穿越更狭小的环境。该机器人共有24个自由度，机器人躯干上安装了小型双目视觉系统，腿部配备力觉传感器，可实现非结构化环境下自主、稳定的全地形步行。该机器人的腿部关节采用模块化设计，为了减小关节的体积和重量，提出了基于大功率直流无刷电机驱动、谐波减速及同步带传动的关节设计方案。电机选用具有大功率体积比、机体小且为扁平设计的盘式电机，在满足关节输出转矩和转速要求的基础上，结合定制的谐波减速器，有效减小了关节的横向尺寸。

HITCR机器人的控制系统采用模块化、多级分布式的组织方式来提高其执行效率，整个控制系统分为运动规划层、腿间协调层用于、单腿控制层和电机驱动层。运动规划层基于嵌入式操作系统，实现人机交互及机器人行进路径的规划。腿间协调层由基于ARM和DSP的嵌入式主控制器实现，主要完成与腿间协调相关的算法。基于DSP的单腿核心控制器用来执行单腿控制层任务，该层采用模块化设计，每个单腿系统都具有一个单独的控制器，主要实现单腿的独立任务，并将腿部感知的接触及腿部姿态信息传输到上层的腿间协调层。电机驱动层由EPOS2驱动模块构成，用于完成底层的电机精确控制。

1.3.1.1 自适应足端轨迹规划

六足机器人具有串、并联复合拓扑的结构特点，当六足机器人处于支撑相时，机器人的腿与躯干形成了典型的并联多环机构，其正运动学问题归结为在各支撑腿足端位置、关节角已知的情况下如何确定机器人躯干的位姿，反之逆运动学问题则是在各支撑腿足端位置、躯干位姿已知的情况下对腿部各关节转角进行准确求解。六足机器人HITCR的躯干具有3个移动、3个转动共6个空间自由度，而六条腿共有18个主动自由度，整个机器人共计24个自由度，因此属于具有冗余自由度的空间并联多环机构。根据稳定性分析，该机器人稳定步行的必要条件是在运动的任意时刻至少有三条不相邻的腿同时处于支撑状态。六足机器人的支撑腿坐标系如图1-50所示。

足端轨迹规划重点关注六足机器人步行时机器人各足端的运动轨迹生成问题，其目标是既要满足机器人在复杂地形下运动稳定性、运动快速性等需求，同时考虑机器人自身的机构、运动学和动力学约束，从而保证机器人有序、稳定、快速的运动。针对复杂崎岖的地形，六足机器人要实现自主移动，首先要具有机器人足端轨迹对地形变化的适应性，即能够生成自主跨越障碍并稳定落足的连续轨迹。

为便于机器人各腿足端轨迹的协调规划，以基关节轴线的中心（如O₆₀）为坐标原点建立轨迹规划的参考坐标系，该坐标系的各轴方向与躯干坐标系平行，Z′_m0与躯干垂直指向相反，X′_m0与机器人的前行速度V方向相同。足端轨迹模型如图1-51所示。

足端轨迹的摆动相和支撑相统一在参考坐标系中规划。原点O′_m4为初始位姿下第m腿足端点在相应参考坐标系的X′_m0O′_m0Z′_m0平面的投影。轨迹Track_ A（曲线QATBP）为摆动相轨迹在平面X′_m0O′_m0Z′_m0的投影，该轨迹分为引导层ATB段和调整层CQ及BP段。引导层轨迹主要实现摆动功能，具有良好的形状和动态特性，而将其进一步区分为上下引导层则是用于机器人自主判断和切换位姿控制策略。调整层轨迹主要用于适应变化地面，可分别针对不同地形通过对应的轨迹调整解决机器人足端的起落问题。

当机器人在平坦地形移动时，摆动相轨迹仅包含引导层，起始于A点位置，终止于B点位置。当机器人在崎岖地形移动时，则摆动相轨迹可在曲线TBP段上任意一点终止并快速转换到支撑相轨迹，若终点高于水平参考位置则终止于引导层，低于水平参考位置则终止于调整层。轨迹Track_B（曲线AT′B′）为摆动腿提前进入支撑相的轨迹，该轨迹主要是为了满足多足协调控制的需要。轨迹Track_C（线段BA）为摆动相轨迹在平面X′_m0O′_m0Z′_m0的投影，主要用于提供无位姿调整时机器人支撑相轨迹。而当六足机器人处于重度崎岖地形条件下，在移动的同时还需位姿调整，则可采用轨迹Track_D（线段B″JKC″）实现位姿控制。

崎岖地形下，机器人足端轨迹地面自适应原理如图1-52所示。其中，虚线表示崎岖变化的地形，而带有箭头的粗实线则表示机器人在摆动相和支撑相时的足端轨迹。

如图1-52a所示，四步轨迹分别对应不同的落地情况：以地面垂直高度来衡量，第一步轨迹的终点高于起点，第二步轨迹的终点低于起点，第三步足端终点略高于起点且跨越了凹陷的地形，第四步终点与起点等高但跨越了凸起的地形。在四种不同情况下，足端轨迹的摆动相都可以根据地形的变化，自主动态调整以适应未知地形。当无位姿调整时，机器人躯干的高度在每一步运动中都保持不变，支撑相的投影为直线。如图1-52b所示，当有躯干高度调整时，支撑相轨迹的投影为折线，以上一步支撑相轨迹结束的位置B_i-1作为该步摆动相轨迹的起始点A_i，该位置以腿部坐标系为基准，规划的足端轨迹相对腿部坐标系静止。基于以上策略规划的足端轨迹，在轻度崎岖地形条件下，在足端轨迹适应变化地形的同时可以保证躯干在整个步行过程中以步行初始水平面为基准保持位姿不变；在重度崎岖地形条件下，机器人移动不是以固定的初始水平面为基准，而是以不断变化的躯干平面自身为基准，并且躯干可以通过位姿的调整来灵活适应不断变化的地形。

1.3.1.2 基于Walknet局部规则的自由步态规划

Walknet步行控制器给出了腿间相互作用的六条局部规则，可据此来协调六足机器人各腿的摆动和支撑运动，该控制策略直接、有效地解决了腿间运动的协调问题。为实现腿间相序在外界干扰下的自主调整，我们基于Walknet控制策略提出了三条腿间局部规则，并结合腿部的传感器信息来实现自由步态。

自由步态是一种非模式化的、非重复性的步行方式，该步态对于地形变化具有较好的动态调节能力，可通过实时变换步行模式以适应地形变化或步行任务要求。自由步态的步行模式可由步态参数δ_gp进行统一描述

式中，T是步行周期；t_swing是摆动相持续时间，t_stance是支撑相持续时间。

当机器人在平坦地形下运动时，δ_gp为某一固定值，此时为固化步态模式，可以根据步行任务或环境状态变化动态改变δ_gp值以切换步态模式。例如，当δ_gp=1/2时，机器人采取三足步态；当δ_gp=1/3时，机器人为四足步态；当δ_gp=1/6时，机器人为五足步态。因此针对崎岖地形的自由步态，可通过腿间的激励和抑制关系（该激励来自于机器人的多模态感知系统）实时改变δ_gp值来保证机器人稳定步行和地形通过，步态模式则为介于三足与六足之间的波动步态（1/6＜δ_gp＜1/2）。

根据步行稳定性和连续性的需要，提出了以下腿间局部规则：

1）规则1：摆动腿，通过在一定范围内推迟相邻腿的后极限位置来抑制其同时前摆。

2）规则2：支撑腿，在到达后极限位置时强制其所有相邻摆动腿提前进入支撑相，并且在确认其相邻腿都处于支撑相时开始前摆。

3）规则3：由摆动相进入支撑相的腿，通过在一定范围内提前相邻腿的后极限位置来激励其相继前摆。

规则1和规则2约束了任意时刻至少有三条腿处于支撑相，满足了稳定步行的必要条件；规则3则保证了步态的连续性和高效性。规则执行的优先级为：规则2＞规则1＞规则3。

图1-53所示为局部规则在腿间的执行关系。规则1和规则2作用于任意的相邻腿之间；规则3作用于腿L₃和腿R₃之间，由腿L₃对腿L₂、腿L₂对腿L₁、腿R₃对腿R₂、腿R₂对腿R₁单向作用；并且规定腿的摆动优先级为：L₃＞R₃，L₂＞R₂，L₁＞R₁，L₃＞L₂，L₂＞L₁，R₃＞R₂，R₂＞R₁，由此实现腿部稳定的依次摆动。

机器人步态控制原理如图1-54所示，整个机器人步态控制系统由主控制器和腿部控制器组成，两种控制器之间通过CAN总线进行高速通信。位于躯干的机器人主控制器作为控制核心，主要实现基于局部规则的多足运动协调功能。各腿部控制器采集关节角位置传感器，计算腿部相位信息并实时发送到主控制器；当腿部控制器感知到处于支撑相的腿到达后极限位置时，则向主控制发送执行规则2请求并等待响应；当处于摆动相的腿部通过力传感器感知到与地面接触而进入支撑相后，则向主控制器发送执行规则3请求并等待响应。主控制器的协调模块根据各腿部的请求及相位信息，按照腿间局部规则及优先级关系判定各腿下一时刻的状态，向各腿部控制器发送动作指令，各腿部控制器按照接收到的主控制器指令执行摆动或支撑运动。

1.3.1.3 位姿自主控制

机器人位姿自主控制的意义在于根据地形的变化自主选择合理的位姿控制策略以保证机器人的全地形移动能力。针对不同地形特点，我们分别采取不同的位姿控制策略：针对轻度崎岖地形，提出基于虚拟悬架动力学模型的位姿保持策略；而针对重度崎岖地形，则采用基于主支撑三角形的位姿调整策略。

（1）轻度崎岖地形位姿保持策略

凭借其冗余并联构型和丰富的运动模式，六足机器人具有较高的稳定裕度和地形适应能力。轻度崎岖地形的干扰通常不会导致机器人运动失稳，但会对机器人位姿产生影响，从而进一步影响运动的平顺性及快速性。因此，我们设计了基于虚拟弹簧动力学模型的双环积分滑膜控制器，以初始位姿为基准，以保持机器人躯干水平和高度不变为控制目标，保证运动的平稳 性。虚 拟 悬 架 动 力 学 模型（Virtual Suspension Dynamic Model，VSDM）如图1-55所示，在机器人躯干的竖直方向、俯仰方向及滚转方向上分别设置1个一维虚拟弹簧阻尼器，则机器人躯干的位姿控制可转化成弹簧阻尼器的控制，通过对弹簧阻尼器的参数调节实现对竖直方向波动以及俯仰角、滚转角扰动的主动抑制。

假定地面和机器人本体均为刚性，该动力学模型的数学表达式如下

式中，K_z、K_p、K_r分别是竖直、俯仰和滚转方向的虚拟弹簧刚度系数；C_z、C_p、C_r分别是竖直、俯仰和滚转方向的虚拟阻尼系数；I_p、I_r分别是俯仰和滚转方向的转动惯量；z、θ_p、θ_r分别是躯干高度、俯仰角和滚转角；M_g是机器人重量；分别是俯仰角速度和俯仰角加速度；分别是滚转方向角速度和角加速度；分别是躯干高度方向速度和加速度。

当机器人处于平坦地形缓慢运动状态时，位姿控制可以由式（1-83）模型决定的线性时不变系统来实现。但机器人在崎岖地形步行过程中，存在由于自身状态改变以及外界环境对机体作用带来的干扰，该情况下的模型表示如下

式中，u_z、u_p、u_r分别是竖直、俯仰和滚转方向的控制量；d_z、d_p、d_r分别是竖直、俯仰和滚转方向的干扰量；ΔM_g、ΔI_p、ΔI_r分别是竖直、俯仰和滚转方向惯量的改变量。

滑膜控制器凭借其特有的滑膜运动，能够屏蔽模型参数及外界扰动的影响，从而使控制系统具有良好的鲁棒性。为了通过VSDM对机器人进行位姿保持，我们设计了具有双环结构的滑膜控制器，通过对位置和速度误差的跟踪保证位姿控制精度。如图1-56所示，该结构外环为位置环、内环为速度环，外环输出的速度指令作为内环的外部指令。

躯干高度、俯仰角和滚转角具有相同的数学模型，因此可以采用相同的滑膜控制器分别进行控制，对VSDM模型进行转换可以得到

式中，u是力补偿量，u=[u_z，u_p，u_r]^T；d′是干扰量，d′=[d′_z，d′_p，d′_r]^T，包括外部环境的干扰量和状态改变产生的内部干扰量。

设外环位置控制指令为x_d，x为位置实际输出值，位置误差为e_s=x_d-x。定义外环滑膜函数为

式中，S_O是外环滑膜函数；k₁是外环滑膜函数积分环节系数。

当k₁＞0，则有

设计外环控制律如下

式中，ρ₁是符号函数系数，ρ₁＞0。

内环速度控制指令为，控制误差。定义内环滑膜函数为

式中，S_I是内环滑膜函数；k₂是内环滑膜函数积分环节系数；是实际速度输出值。

当k₂＞0时，则有

设计内环控制律如下，其中符号函数系数μ＞0，ρ₂＞||d′||。

（2）重度崎岖地形位姿调整策略

对于重度崎岖地形，六足机器人需要采用位姿调整策略，对躯干的高度和倾角进行调整。六足机器人各支撑足端点在其躯干坐标系中构成的多边形，称为构型多边形（Configu-ration Polygon，CP）。受崎岖地形影响，构型多边形通常为空间多边形，比较典型的是三足、四足和五足支撑时的构型多边形，如图1-57所示。可以看出，在四足和五足支撑情况下并非所有的足端点都位于同一平面。

根据机器人步态稳定条件，任意时刻都要保证至少有三条不相邻的腿处于支撑阶段，定义连接三条不相邻腿的足端点所构成的三角形为主支撑三角形（Master Polygon，MP），则步行过程中任何构型多边形总存在唯一的主支撑三角形。六足机器人的CP与地形共同决定了其位姿，但由于CP作为空间多边形不适合作为基准面来衡量躯干的姿态，而MP近似表征了构型多边形的方位特征，因此采用MP作为位姿调整的基准面，其选取规则如下：

1）如果腿L₂、R₁、R₃为支撑相，则选择这三条腿足端点组成的支撑三角形为MP。

2）如果腿R₂、L₁、L₃为支撑相，则选择这三条腿足端点组成的支撑三角形为MP。

3）如果六条腿都为支撑相，则选择腿L₂、R₁、R₃足端点组成的支撑三角形为MP。

其中，L₁为机器人躯干左侧的第1条腿（靠近机器人头部），R₁为躯干右侧的第1条腿（靠近机器人头部），依此类推。

确定了MP之后，以主支撑腿中的后腿与中腿的腿部坐标系坐标原点连线为基准轴线，对躯干的姿态进行调整，并且以躯干平面与MP平行为控制目标。如图1-58所示，以六条腿同时支撑状态为例，腿R₁触发重度崎岖标志位，并且判定为MP。此时基准轴线为O₂₀O₄₀，位姿控制目标为与LMP平行的平面ΔO₂₀O₄₀O′₆₀，由此可得到由躯干的高度以及俯仰角和滚转角组成的控制量。位姿调整过程中，除确定基准轴线的两条腿外，其余腿部坐标系原点由位姿调整运动而产生的轨迹如O₁₀O′₁₀、O₆₀O′₆₀、O₅₀O′₅₀、O₃₀O′₃₀所示。如图1-59所示，足端轨迹规划基于腿部坐标系进行，由于处于支撑相的足端相对地面静止，根据运动的相对性，足端轨迹是与腿部坐标原点运动轨迹形状相同的曲线。由于躯干的高度、俯仰角及偏转角之间具有严格的对应关系，因此，只需要以其中的一个因素作为控制量，采用基于VSDM的双环积分滑膜控制器进行控制即可实现机器人的位姿调整，而无须对上述三个量分别进行控制。

1.3.2 机器人自主作业技术

1.3.2.1 基于动作基元轨迹的模仿学习方法研究

近年来，随着人工智能、传感器技术的快速发展，机器人的智能程度不断提高，机器人自主作业的需求不断增加。在自主作业中，机器人需要具备学习新任务的能力，能够根据不同任务需求和环境进行智能决策并自主规划任务。传统的机器人任务编程方法严重依赖于编程人员的专业知识，需要提前考虑好所有可能发生的情况，一旦任务发生改变就要重新进行编程，因此机器人作业任务的通用性和扩展性较差。而最近提出的模仿学习方法则提供了一个自然直观的快速传授机器人新技能的机制。机器人通过感知人类演示，提取相关信息并编码最优策略，实现不同作业任务的泛化，并在作业过程中通过训练不断优化机器人执行任务的效果，从而实现机器人自主作业。

机器人成功完成自主作业任务的首要环节是如何实现任务需要的机器人末端轨迹，一个任务的整体轨迹一般由不同的动作基元组成，例如典型抓取放置任务包括抓、放、返回等动作基元。人类针对每个动作基元进行示教，机器人记录轨迹后，通过轨迹滤波消除抖动，采用动态时间规整（Dynamical Time Warping，DTW）算法保证多次示教轨迹的时间一致性和均匀性，采用高斯混合模型与高斯混合回归（Gaussian Mixture Model and Gaussian Mixture Regression，GMM/GMR）算法建模轨迹整体分布并提取最优示教轨迹，并采用动态运动基元（Dynamic Movement Primitives，DMPs）方法对最优示教轨迹进行建模并学习得到轨迹的形状参数，该形状参数可以用于多个相似作业任务下起始位置和终止位置的泛化。

DMPs方法将末端轨迹的各个自由度解耦成独立的离散运动DMP，一个DMP可以用不依赖于时间t的标准系统和转换系统描述

标准系统通过不显示依赖于时间t的阶段变量x表示离散运动的演化过程，转换系统则利用非线性强迫力f将线性弹簧阻尼系统转换为具有弱非线性的弹簧阻尼系统，非线性力f可以由K个基函数的权重进行线性加权组合得到，表示为

其中，基函数权重w_i可利用局部加权回归（Locally Weighted Regression，LWR）方法学习后作为离散运动的形状参数。当初始位置y₀或者目标位置G发生改变时，DMPs可以基于学习到的形状参数，利用转换系统逐步积分得到每个时刻下的期望位置。

若不改变轨迹初始位置和终止位置，机器人可以复现人类示教时的任务，图1-60所示为机器人经过DMPs学习后复现人类示教抓取放置任务的末端轨迹，图中整体轨迹拟合效果较好，即使轨迹中出现一定的偏差（z坐标值后半部分），机器人学习得到的轨迹仍然能够保留人类示教时候的轨迹形状和趋势，可以满足机器人复现人类示教任务的基本要求。

采用DMPs作为学习模型的主要优势体现在基于其形状参数的不变特性所呈现的良好轨迹泛化效果。图1-61所示为在保证原始演示轨迹相对运动趋势方向不变的前提下，初始位置和终止位置发生改变后，用DMPs模型泛化后得到的任务执行末端轨迹。图中三维空间位置随时间分布趋势与复现任务的轨迹较为相似，体现了DMPs模型的良好泛化性能。

1.3.2.2 基于图像关键帧提取的任务表达方法研究

机器人自主作业技术不仅需要轨迹上的复现和泛化，还需要建立作业任务表达模型。机器人可以基于任务表达模型进行自主决策，从而完成更高智能的任务。任务表达模型一般可以用一个六元数组表示

式中，η是作业任务所包含的动作总数；W是作业任务场景下的整体观测值；P是目标操作物体的观测值；F是获得观测值的区域大小；O是最后获得的目标物体执行任务序列，包含各个动作的关键位置信息；A是最后需要调用的DMPs运动基元来完成整个作业任务。

通常采用人类对机器人进行拖动示教的方式为任务提供观测值，但该示教方法对操作者和机器人的能力要求较高且操作烦琐。为此，本节提出了基于图像关键帧提取的任务表达方法，机器人通过视觉传感器观察人类动作获取任务信息。针对典型的抓取放置任务，在任务表达模型中动作主要分为两类：一类是抓取，一类是放置。表达过程中，会从抓取动作中捕获动作目标物体，从放置过程中确定上一步所提取目标物体在本次抓取放置中最终的位姿状态。主要采用图像处理的方法去解决上述问题，假设状态S_i-pre所采集的图像设为P_i-pre，状态S_i-pro所采集的图像设为P_i-pro，第i组抓取放置动作的目标物体图像设为P_i-obj，被放置的位置P_i-final。每次演示时，经帧间差分、形态学处理以及确定边界框的大小进行依次提取，提取流程如图1-62所示，详细提取过程如下。

首先，将P_i-final和P_i-pre灰度化后差分并取绝对值，提取目标区域，得到的灰度图取合适的阈值将目标区域图像二值化，以便后续处理。

然后，由于场景光线不稳定以及图像采集设备误差，将会导致目标区域图像中仍然存在一些较小高亮区域干扰。在形态学运算中，膨胀和腐蚀是其中的基础方法。在本步中，先采用腐蚀运算以消除目标区域二值图像中不需要的小空洞与连线，腐蚀截止到二值图像中仅剩一个连通域。记录腐蚀运算次数，对此时的二值图像进行膨胀运算。严格来说，腐蚀与膨胀运算并不是逆运算，因此为保证目标区域物体边缘信息不丢失，膨胀的运算次数将比腐蚀运算要多。

最后，依据此时仅有一个连通域的二值函数图像，对连通域内所有高亮点的坐标取均值，近似得到目标物体中心在XOY平面的像素坐标，依据连通域的大小选取合适的边界框，利用物体中心的像素坐标和边界框大小，最终在图像P_i-pre上截取感兴趣区域（ROI）得到只包含目标物体的图像P_i-obj。

对于包含多个动作的作业任务，基于上述的图像处理方法将得到的目标物体整体图像序列P存储到任务模型中，最后在任务复现阶段基于目标物体图像序列完成作业任务的复现。

1.3.2.3 基于图像匹配的作业任务复现方法研究

基于上述建立的任务表达模型式（1-95），在任务模型中存储目标物体图像序列P后，在任务复现阶段，机器人需要根据实际作业任务场景W_actual，通过P和W_actual的匹配过程获得目标物体序列O，从而完成自主作业任务复现过程。

我们主要采用加速鲁棒性特征提取（Speeded-Up Robust Features，SURF）算法以进行图像特征提取与匹配，SURF的描述符描述了特征点邻域内的强度分布，类似于SIFT及其变体所提取的梯度信息。同时建立在x和y方向上的一阶Haar小波响应的分布，利用积分图像来获得速度。这不仅减少了特征计算和匹配的时间，而且可以同时提高稳健性。

在基于SURF算法进行图像匹配后，采用随机抽样一致性（Random Sample Consensus，RANSAC）算法剔除SURF匹配出错的特征。经典RANSAC算法具有良好的鲁棒性，并且可以降低噪声的干扰。RANSAC算法在图像匹配时的基本执行流程如下：

1）设P为特征匹配得到的特征点匹配对集合，在其中随机选取n对特征点匹配对，作为一个样本集N进行初始化运算，得到相应的匹配转换矩阵T；将样本集外的特征点匹配对分别用T代入误差度量函数。

2）设定一个阈值k，遍历样本集外的特征点匹配对求误差，将未达到阈值的匹配对联合样本集N组成匹配一致集N^*，计算其在P中所占的比例S。

3）重复以上步骤，选取比例S最大的变换矩阵记为T_S-max。

4）集合P所有特征匹配对，对一致集比例S最大的变换矩阵T_S-max计算误差，将超过阈值的元素剔除，再用剩余的特征点匹配对求取变换矩阵，该矩阵即为最终的变换矩阵T_final。

在机器人作业任务复现阶段，主要采用基于SURF和RANSAC的算法完成新场景下目标物体与其目标位置的定位获得目标物体执行任务序列O。该部分算法的具体图像分析流程如图1-63所示。首先记录新场景图片下的观测值W_actual，这里是相机视角下截取的彩色图像，将彩色图像转换为灰度图像后，基于SURF算法与任务表达模型中的目标物体图像序列P匹配获得匹配后的特征点对；然后基于RANSAC算法排除异常特征点对获得转换矩阵，并基于计算得到的转换矩阵利用任务表达模型中的观测区域大小F和目标物体执行任务序列O，得到新场景下的框选位置，确定目标物体位姿和放置位置，并调用相应的DMPs动作基元生成泛化轨迹，控制机械臂执行各个动作完成整体作业任务。