1 引言（Introduction）

机器人在工业应用中有大量接触作业需求，如打磨^[1]、擦拭^[2]和柔性材料铺放^[3]等。传统接触任务一般采用离线规划法，首先基于接触任务模型或实验测试获取合适的运动轨迹和接触力，并将轨迹根据运动时间进行离散化处理，然后采用如力/位混合、PID（比例―积分―微分）等控制算法实现接触力跟踪。由于接触任务建模较为困难，实验测试需要大量数据，因而此类基于时间的离线规划方法周期较长，且抗扰动性弱^[4]。当技术人员需要在线对接触任务完成质量检查^[4]或缺陷修复时，需要将机器人停机，然后重新规划运动轨迹，恢复作业。这种模式的工作效率较低，且难以适应多样化的操作场景。因此，接触任务的运动规划与控制，不仅需要满足规划周期短和力跟踪控制精度高的要求，还要具有较强的抗扰动能力，即能够在受到人为扰动后，自主恢复接触作业。

示教学习为机器人运动规划提供了另一种方法，即通过示教的方式将人工作业经验迁移到机器人上，可有效缩短机器人运动规划周期，被广泛地应用于喷涂、点胶、搬运等自由空间任务，以及打磨、装配、焊接等接触任务。示教方法包括动觉示教^[5]、视觉示教^[6]和遥操作示教^[7]等，其中动觉示教的方式能直接对人工执行接触任务的轨迹与力数据进行采集^[1]。在获得示教数据后，需要从轨迹数据中提取规律，构建运动模型，指导机器人的运动规划。基于示教数据建模的运动轨迹方法中应用较为广泛的是基于时间序列的概率模型^[8]，其将示教轨迹看作随机过程，并基于最大似然估计求解选定的统计模型参数，获得时间到轨迹的映射关系，常用建模方法有高斯混合回归^[9]、高斯过程回归^[10]、支持向量回归^[11]、局部加权回归^[12]和隐马尔可夫模型^[13]等。由于这些方法生成的轨迹精确度与时间相关，因此对任务中扰动较为敏感。

为使机器人在运动中具有抗扰动性，需要将示教轨迹描述为动态系统（DS）模型，并基于示教轨迹求解DS参数，实现针对任务的运动规划。文[14]提出运动动态基元（DMP），该模型由基于示教轨迹数据的非线性前馈项和弹簧阻尼的线性反馈组成，可使运动收敛到终点。为提高DMP对高维示教轨迹的泛化能力，在其基础上，提出了轨迹概率运动表达的概率运动基元（ProMP）^[15]和核运动基元（KMP）^[16]。此类DS用于时间轨迹序列的规划，在扰动较小时，不会影响吸引子的动态特性，但无法抵抗较大扰动，例如人手阻挡机器人运动^[17]，在扰动结束后难以依赖动态系统的运动规划直接恢复任务。

为进一步提高机器人运动抗扰动性，需构建时间无关的DS模型。文[18]提出DS稳定估计器（SEDS），该模型基于高斯混合模型将示教数据编码成1阶自洽系统，并建立系统全局渐近稳定的充分条件，但对复杂示教轨迹的学习能力较差。为解决该问题，可先基于示教数据构建灵活的李雅普诺夫函数，然后基于李雅普诺夫稳定性求解DS。SEDS-II模型^[19]采用回归算法将示教轨迹建模为DS，同时保证稳定性。$ \tau $-SEDS算法^[20]将示教数据映射到隐空间，并在隐空间中采用SEDS算法学习全局渐近稳定DS，再逆映射获得原空间DS。基于能量的动态系统稳定器（ESDS）^[21]在李雅普诺夫函数基础上叠加虚拟能量，生成高精度的非自洽DS。为解决SEDS的两步学习耗时较长、需调整参数较多的问题，文[22]提出基于快速微分同胚映射的动态系统，建立示教数据与某已知全局渐近稳定系统之间的微分同胚映射，并基于迭代方法快速建立映射关系，被用在自由空间的抓取任务中^[23]。

上述动态系统运动规划方法可以直接用于自由空间的运动轨迹规划，但应用到接触任务时，需添加力控制调节项。文[24]采用DMP建立位置动态系，将接触力表示为类似运动DMP模型的形式，并采用力/位混合控制将力误差转换为位置补偿量。文[25]也在DMP基础上，添加基于导纳的力控制调节项，并将其与无源性系统和迭代学习控制相结合，实现对变化接触表面的在线适应。这种方法将时间相关的DS模型与力控制结合，实现了对接触任务的运动规划与控制，但难以满足人将机器人拖离接触面的动态交互需求。文[26]将DMP和ProMP相结合、采用改进的VMP（via-point movement primitive）模型获得参考速度源，并添加力控制、姿态控制和力矩控制速度源，通对控制器刚度值和速度源的调节来实现机器人受到扰动时的自适应控制和扰动结束后的作业恢复^[2]。为使机器人在受到扰动后能够根据动态系统运动规划恢复接触作业，需要基于示教轨迹建立时间无关的DS模型。文[4]采用时间无关的动态系统对接触任务进行运动规划，并通过径向基函数编码的力修正项进行阻抗力控制，进一步结合能量箱的方式保证系统的无源性，但整个设计过程复杂。

为实现接触任务轨迹的快速规划与力控制，并且使机器人在作业中具有对外界扰动的适应性，本文提出微分同胚映射动态系统运动规划与力控制相结合的接触任务运动规划与控制方法。为了建立时间无关的运动DS模型，首先通过导纳控制器设计接触任务动觉示教，并采用微分同胚映射建立位置DS模型。其次，将接触任务划分为3种状态，分别基于所建立DS模型设计各状态力调节项和姿态控制项，将位置DS模型应用于接触任务的全过程，使其具有较强的环境适应性和抗扰动性。

2 人工示教（Human demonstration）

选用碳纤维织物复合材料铺放任务作为实验场景进行验证，为此搭建示教平台，并通过动觉示教的方式采集人工作业过程中的位置和力数据。

2.1 平台搭建

示教时传感器和执行器等各部件安装及受力示意如图 1所示，机械臂末端关节$ J_{\text{end}} $中内部6维力/力矩传感器为$ S_{0} $，$ J_{\text{end}} $与外部6维力/力矩传感器$ S_{1} $通过连接件$ C_{0} $连接，并将执行器$ T_{0} $安装在$ S_{1} $末端，分别对2个6维力/力矩传感器进行零点标定。在图 1中，$ S_{0} $检测数据$ \mathit{\boldsymbol{F}}_{0} = $ $ (\mathit{\boldsymbol{f}}_{0}, \mathit{\boldsymbol{\tau}}_{0})^{\rm T} $，$ S_{1} $检测数据$ \mathit{\boldsymbol{F}}_{1} =(\mathit{\boldsymbol{f}}_{1}, \mathit{\boldsymbol{\tau}}_{1})^{\rm T} $为接触面对执行器$ T_{0} $的反力，人手施加在连接件$ C_{0} $处的力与力矩数据$ \mathit{\boldsymbol{F}}_{2} = (\mathit{\boldsymbol{f}}_{2}, \mathit{\boldsymbol{\tau}}_{2})^{\rm T} $。

2.2 数据采集

针对机械臂末端安装的部件和模具，分别建立如图 2(b)所示坐标系：模具$ M_{0} $坐标系$ \{M\} $，其与机械臂的基坐标系$ \{B\} $坐标轴平行，到$ \{B\} $的变换关系为$ _{M}^{\, B} \mathit{\boldsymbol{T}} $；传感器$ S_{0} $坐标系为$ \{S_{0}'\} $，其与机械臂末端坐标系$ \{J\} $的坐标轴平行，到$ \{J\} $的变换关系为$ _{S_{0}'}^{\; J} \mathit{\boldsymbol{T}} $；传感器$ S_{1} $坐标系为$ \{S_{1}'\} $，其与$ \{J\} $的坐标轴平行，到$ \{J\} $的变换关系为$ _{S_{1}'}^{\; J} \mathit{\boldsymbol{T}} $；执行器$ T_{0} $末端坐标系$ \{E\} $，其与$ \{J\} $的坐标轴平行，到$ \{J\} $的变换关系为$ _{E}^{J} \mathit{\boldsymbol{T}} $。可通过机械臂运动学获得$ \{J\} $到$ \{B\} $的变换关系$ _{J}^{B} \mathit{\boldsymbol{T}} $，进而获得$ \{S_{0}'\} $、$ \{S_{1}'\} $、$ \{E\} $到$ \{B\} $变换关系分别为

$ \begin{align} \begin{cases} {}_{S_{0}'}^{\; B} \mathit{\boldsymbol{T}} ={}_{J}^{B} \mathit{\boldsymbol{T}} {}_{S_{0}'}^{\; J} \mathit{\boldsymbol{T}} \\ {}_{S_{1}'}^{\; B} \mathit{\boldsymbol{T}} ={}_{J}^{B} \mathit{\boldsymbol{T}} {}_{S_{1}'}^{\; J} \mathit{\boldsymbol{T}} \\ {}_{E}^{B} \mathit{\boldsymbol{T}} ={}_{J}^{B} \mathit{\boldsymbol{T}} {}_{E}^{J} \mathit{\boldsymbol{T}} \end{cases} \end{align} $

(1)

$ \mathit{\boldsymbol{F}}_{0} $在$ \{B\} $中表示为$ \mathit{\boldsymbol{F}}_{0}' =(\mathit{\boldsymbol{f}}_{0}', \mathit{\boldsymbol{\tau}}_{0}')^{\rm T} $：

$ \begin{align} \begin{cases} \mathit{\boldsymbol{f}}_{0}' ={}_{S_{0}'}^{\; B} \mathit{\boldsymbol{M}} \mathit{\boldsymbol{f}}_{0} \\ \mathit{\boldsymbol{\tau}}_{0}' =\mathit{\boldsymbol{P}}_{t} {}_{S_{0}'}^{\; B} \mathit{\boldsymbol{M}} \mathit{\boldsymbol{f}}_{0} +{}_{S_{0}'}^{\; B} \mathit{\boldsymbol{M}} \mathit{\boldsymbol{\tau}}_{0} \end{cases} \end{align} $

(2)

其中，$ {}_{S_{0}'}^{\; B} \mathit{\boldsymbol{M}} $为变换关系$ {}_{S_{0}'}^{\; B} \mathit{\boldsymbol{T}} $中的旋转矩阵，$ \mathit{\boldsymbol{P}}_{t} $为$ {}_{S_{0}'}^{\; B} \mathit{\boldsymbol{T}} $中变换向量$ {}_{S_{0}'}^{\; B} \mathit{\boldsymbol{P}} $的斜对称矩阵。

设计动觉示教时机械臂末端导纳力控制律为

$ \begin{align} \mathit{\boldsymbol{M}}\ddot{\mathit{\boldsymbol{X}}}+\mathit{\boldsymbol{B}}\dot{\mathit{\boldsymbol{X}}}=\mathit{\boldsymbol{F}}_{0}' \end{align} $

(3)

其中，$ \mathit{\boldsymbol{M}} $为$ 6\times 6 $惯性系数矩阵，$ \mathit{\boldsymbol{B}} $为$ 6\times 6 $阻尼系数矩阵，$ \ddot{{\mathit{\boldsymbol{X}}}} $、$ \dot{{\mathit{\boldsymbol{X}}}} $分别为期望加速度、期望速度。

第$ t $控制周期，期望位姿、期望速度和加速度控制输出量分别为

$ \begin{align} \begin{cases} \mathit{\boldsymbol{X}}(t)=\dot{\mathit{\boldsymbol{X}}}(t)T+\mathit{\boldsymbol{X}}(t-1) \\ \mathit{\boldsymbol{\dot{X}}}(t)=\mathit{\boldsymbol{\ddot{X}}}(t)T+\mathit{\boldsymbol{\dot{X}}}(t-1) \\ \mathit{\boldsymbol{\ddot{X}}}(t)= \left[ \mathit{\boldsymbol{F}}_{0}' (t)-\mathit{\boldsymbol{B\dot{X}}}(t-1)\right]/\mathit{\boldsymbol{M}} \end{cases} \end{align} $

(4)

其中，$ \mathit{\boldsymbol{X}}(t-1) $、$ \mathit{\boldsymbol{\dot{X}}}(t-1) $和$ \mathit{\boldsymbol{\ddot{X}}}(t-1) $分别表示第$ t-1 $控制周期的期望位姿、速度和加速度，$ \mathit{\boldsymbol{F}}_{0}' (t) $为第$ t $控制周期$ \mathit{\boldsymbol{F}}_{0}' $值，$ T $为机械运动控制周期。

在人工示教过程中，如图 2(a)所示，人拖动机械臂末端进行铺放作业，并尽量使机械臂末端$ z $向沿着模具表面法向，同时记录传感器$ S_{1} $的力/力矩$ \mathit{\boldsymbol{F}}_{1} =(\mathit{\boldsymbol{f}}_{1}, \mathit{\boldsymbol{\tau}}_{1})^{\rm T} $和$ \{E\} $相对于$ \{B\} $的运动轨迹$ \mathit{\boldsymbol{\xi}}_{0} =(\mathit{\boldsymbol{x}}_{0}, \mathit{\boldsymbol{q}}_{0})^{\rm T} $，其中$ \mathit{\boldsymbol{x}}_{0} $为示教轨迹的位置。基于模具表面的三角面片信息，获取示教轨迹位置$ \mathit{\boldsymbol{x}}_{0} $的各离散点对应的三角面片的单位法向量$ \mathit{\boldsymbol{N}}_{z} =(n_{x}, n_{y}, n_{z})^{\rm T} $，并计算$ \mathit{\boldsymbol{f}}_{1} $在单位法向量$ \mathit{\boldsymbol{N}}_z $的投影，将投影值作为执行实际任务时机械臂末端的$ z $向期望力：

$ \begin{align} f_{\rm n} =\mathit{\boldsymbol{f}}_{1} \cdot \mathit{\boldsymbol{N}}_z \end{align} $

(5)

在示教轨迹的离散位置点分别建立坐标系作为机械臂末端运动的期望姿态。首先搜索曲面上离$ \mathit{\boldsymbol{\xi}}_{0} $最近的三角面片，将其单位法向$ \mathit{\boldsymbol{N}}_{z} $作为$ z $向，$ \mathit{\boldsymbol{\xi}}_{0} $轨迹运动方向$ \mathit{\boldsymbol{v}}_{q} $作为$ y $向，通过$ \mathit{\boldsymbol{N}}_{z} $与$ \mathit{\boldsymbol{v}}_{q} $叉乘，获得垂直于$ z $向的$ x $向：

$ \begin{align} \mathit{\boldsymbol{N}}_{x} =\mathit{\boldsymbol{N}}_{z} \times \mathit{\boldsymbol{v}}_{q} \end{align} $

(6)

由于$ z $向和$ y $向可能不垂直，因此将$ \mathit{\boldsymbol{N}}_{x} $与$ \mathit{\boldsymbol{N}}_{z} $叉乘，获得垂直于$ z $向的$ y $向：

$ \begin{align} \mathit{\boldsymbol{N}}_{y} =\mathit{\boldsymbol{N}}_{z} \times \mathit{\boldsymbol{N}}_{x} \end{align} $

(7)

通过式(6)(7)可获得示教轨迹各点的期望姿态，从而将执行器的$ z $方向调整至始终与曲面垂直。本文采用四元数的方式描述姿态，将示教轨迹的期望姿态描述为$ \mathit{\boldsymbol{q}}_{0}' $。通过上述方法，获得人工完成碳纤维织物复合材料铺放的期望位置$ \mathit{\boldsymbol{x}}_{0} $、期望姿态$ \mathit{\boldsymbol{q}}_{0}' $和执行器的$ z $向期望力$ f_{\rm n} $。

3 运动动态系统建立（Establishment of the motion dynamical system）

基于微分同胚映射的动态系统是一种时不变系统，表达式中不显式地含有时间。在获得示教轨迹的位置$ \mathit{\boldsymbol{x}}_{0} $后，连续非线性的DS将运动轨迹描述为非线性微分方程：

$ \begin{align} \mathit{\boldsymbol{\dot{x}}}={\mathit{\boldsymbol{f}}}(\mathit{\boldsymbol{x}}) \end{align} $

(8)

将期望的非零平衡点$ \mathit{\boldsymbol{x}}^{*} $，经平移变为零平衡点，并构建变量$ \mathit{\boldsymbol{x}}'=\mathit{\boldsymbol{x}}-\mathit{\boldsymbol{x}}^{*} $，得到新的平衡方程：

$ \begin{align} \mathit{\boldsymbol{\dot{x}}}' =\mathit{\boldsymbol{f}} \left(\mathit{\boldsymbol{x}}'+\mathit{\boldsymbol{x}}^{*}\right) \end{align} $

(9)

空间中的任意起点都将向着动态系统平衡点运动，为实现机器人末端运动与示教运动趋势一致，结合式(9)，将示教轨迹的终点设为零平衡点，即将示教轨迹点的位置坐标减去终点的位置坐标。采用文[22]的微分同胚映射动态系统建立方法，在隐空间$ A $中，将实际空间$ B $中示教轨迹$ {\mathit{\boldsymbol{b}}} $的起点与终点相连，建立简单轨迹$ {\mathit{\boldsymbol{a}}} $。建立隐空间$ A $中简单轨迹$ \mathit{\boldsymbol{a}} = (\mathit{\boldsymbol{x}}_{{\mathit{\boldsymbol{a}}}, i}) $和实际空间$ B $中示教轨迹$ {\mathit{\boldsymbol{b}}}=(\mathit{\boldsymbol{x}}_{{\mathit{\boldsymbol{b}}}, i}) $之间的映射，$ i = 0, \cdots, N $。其映射关系为

$ \begin{align} \mathit{\boldsymbol{\phi}} (\mathit{\boldsymbol{a}})=\mathit{\boldsymbol{a}}+k_{\bm\phi} (\mathit{\boldsymbol{a}})\mathit{\boldsymbol{v}} \end{align} $

(10)

其中，$ k_{\bm\phi} ({\mathit{\boldsymbol{a}}})={\rm e}^{-\rho^{2} \| \mathit{\boldsymbol{a}}-\mathit{\boldsymbol{c}} \|^{2}} $为径向基函数，$ \mathit{\boldsymbol{c}} $为$ k_{\bm\phi} (\mathit{\boldsymbol{a}}) $的中心位置，$ \rho > 0 $为$ k_{\bm\phi} (\mathit{\boldsymbol{a}}) $的宽度，$ \mathit{\boldsymbol{v}} $为平移的变换方向。

通过迭代的方法，可以获取超参数$ \rho $，迭代过程中的映射误差函数为

$ \begin{align} F_{\rm p}( \mathit{\boldsymbol{\varPhi}}_{\mathit{\boldsymbol{x}}} ({\mathit{\boldsymbol{a}}} ), {\mathit{\boldsymbol{b}}} )=\frac{1}{N}\sum _{i=1}^N \left\| \mathit{\boldsymbol{\varPhi}}_{{\mathit{\boldsymbol{x}}}} ( \mathit{\boldsymbol{x}}_{{\mathit{\boldsymbol{a}}}, i} )-\mathit{\boldsymbol{x}}_{{\mathit{\boldsymbol{b}}}, i}\right\| \end{align} $

(11)

其中，$ \mathit{\boldsymbol{\varPhi}}_{{\mathit{\boldsymbol{x}}}} (\mathit{\boldsymbol{a}})=\phi_{N_1}\circ\cdots\circ\phi_{1}({\mathit{\boldsymbol{a}}}) $为映射函数$ \mathit{\boldsymbol{\phi}} (\mathit{\boldsymbol{a}}) $经过$ N_1 $次迭代获得的最终映射函数，$ \circ $代表函数复合运算。

建立隐空间$ A $中轨迹$ {\mathit{\boldsymbol{a}}}'=\mathit{\boldsymbol{x}}_A^i-\mathit{\boldsymbol{x}}_A^N $（$ i =0, \cdots, N $）的动态系统：

$ \begin{align} {\dot{{\mathit{\boldsymbol{x}}}}_A}=\mathit{\boldsymbol{f}}_{1} (\mathit{\boldsymbol{x}}_A)=\gamma (\mathit{\boldsymbol{x}}_A)\mathit{\boldsymbol{Px}}_A \end{align} $

(12)

其中，$ \gamma (\mathit{\boldsymbol{x}}_A)= \begin{cases} \dfrac{\| {\mathit{\boldsymbol{x}}}_0\|} {N\Delta t\| \mathit{\boldsymbol{x}}_A\|}, & \| {\mathit{\boldsymbol{x}}}_A\| \geqslant \dfrac{\| {\mathit{\boldsymbol{x}}}_0\|} {N} \\ \dfrac{1}{\Delta t}, & \text{其他} \end{cases} $，$ {\mathit{\boldsymbol{x}}}_0 $是运动起点位置，$ {\mathit{\boldsymbol{x}}}_A $为机器人实际运动时在$ A $空间中的位置，$ \mathit{\boldsymbol{P}} $为对称负定矩阵。

通过隐空间$ A $中轨迹$ {\mathit{\boldsymbol{a}}} $和实际空间$ B $中轨迹$ {\mathit{\boldsymbol{b}}} $的位置映射函数$ \mathit{\boldsymbol{\varPhi}}_{{\mathit{\boldsymbol{x}}}} ({\mathit{\boldsymbol{a}}}) $，得到实际空间$ B $中的示教轨迹$ {\mathit{\boldsymbol{b}}} $的动态系统：

$ \begin{align} \dot{{\mathit{\boldsymbol{x}}}}_{B} =\mathit{\boldsymbol{f}}_{2} (\mathit{\boldsymbol{x}}_B)=\gamma \left(\mathit{\boldsymbol{\varPhi}}^{-1}(\mathit{\boldsymbol{x}}_B)\right)\mathit{\boldsymbol{J}}_{\rm h1}\left(\mathit{\boldsymbol{\varPhi}}^{-1}(\mathit{\boldsymbol{x}}_B)\right)\mathit{\boldsymbol{P}}\mathit{\boldsymbol{\varPhi}}^{-1}(\mathit{\boldsymbol{x}}_B) \end{align} $

(13)

其中，$ {\mathit{\boldsymbol{x}}}_B $为机器人实际运动时在$ B $空间中的位置，$ \mathit{\boldsymbol{J}}_{\rm h1}(\mathit{\boldsymbol{x}})=\dfrac{\partial \mathit{\boldsymbol{\varPhi}} (\mathit{\boldsymbol{x}})}{\partial \mathit{\boldsymbol{x}}} $。

基于第2节中采集的人工示教的位置数据$ \mathit{\boldsymbol{x}}_{0} $和上述动态系统的建立方法，建立图 3(a)中隐空间$ A $中简单直线轨迹$ {\mathit{\boldsymbol{a}}} $和图 3(b)中实际空间$ B $中示教轨迹$ {\mathit{\boldsymbol{b}}} $的映射关系，并建立简单轨迹$ {\mathit{\boldsymbol{a}}} $的动态系统，进而获得示教轨迹$ {\mathit{\boldsymbol{b}}} $的动态系统。在实际空间$ B $中任意设置一个测试点，作为运动起点，从图 3(b)可以看出，该点在运动中不断地向示教轨迹$ {\mathit{\boldsymbol{b}}} $靠近，最终收敛到示教轨迹终点，并如图 3(a)所示，该点在隐空间$ A $中也是朝向简单轨迹$ {\mathit{\boldsymbol{a}}} $逐渐靠近，并收敛到简单轨迹$ {\mathit{\boldsymbol{a}}} $的终点。

4 接触任务运动控制（Motion control for contact task）

如图 4所示，将机器人接触任务的执行过程划分为接触作业（图 4(a)）、人工扰动（图 4(b)）和自由空间运动（图 4(c)）3个状态。当机器人在接触空间中作业时，需要末端姿态为示教轨迹的对应期望姿态$ \mathit{\boldsymbol{q}}_{0}' $，并且末端$ z $向接触力为期望力$ f_{\rm n} $。当人工干扰机器人作业时，人手将机器人的末端拖离接触面，此时机器人末端需要跟随人手运动。在人工扰动消除后，机器人需要从被释放的位置重新开始接触作业。

4.1 接触作业

为实现机械臂末端与模具的接触力控制和末端姿态控制，对第3节建立的动态系统添加导纳力控制调节项，并基于姿态反馈设计PI（比例－积分）控制器，其控制框架如图 5所示。结合动态系统的运动速度$ \mathit{\boldsymbol{v}}_{B} =(v_{Bx}, v_{By}, v_{Bz})^{\rm T} $和机械臂末端$ z $向导纳控制器输出速度调节量$ \Delta v $，获得机械臂末端的速度控制量$ \mathit{\boldsymbol{v}}_{\rm c} $，并乘以控制周期$ T $，获得位置调节量$ \Delta \mathit{\boldsymbol{x}}_{\rm c} $，进而与当前实际位置$ \mathit{\boldsymbol{x}}_{\rm e} $叠加，获得位置控制量$ \mathit{\boldsymbol{x}}_{\rm c} $，同时基于PI控制对机械臂末端的姿态$ \mathit{\boldsymbol{q}}_{\rm e} $进行反馈调节，获得机械臂末端的姿态控制量$ \mathit{\boldsymbol{q}}_{\rm c} $。将$ \mathit{\boldsymbol{x}}_{\rm c} $和$ \mathit{\boldsymbol{q}}_{\rm c} $输入到机器人内部控制器实现对机器人接触作业的运动控制。

在接触作业中，机械臂末端姿态的$ z $向始终沿着接触面法向，且在$ z $向施加期望接触力，因此只需考虑机械臂末端$ z $向的单自由度导纳控制，其控制律为

$ \begin{align} m\ddot{e}^{2}+b\dot{e}^{2}+ke=\Delta f \end{align} $

(14)

其中，$ e=x_{{\rm e}z} -x_{{\rm c}z} $为机械臂末端$ z $向实际位置$ x_{{\rm e}z} $和控制输入位置$ x_{{\rm c}z} $之间的误差，$ \dot{e} $和$ \ddot{e} $分别为速度和加速度误差；$ \Delta f=f_{\rm e} -f_{\rm d} $为机械臂末端$ z $向实际力$ f_{\rm e} $与期望力$ f_{\rm d} $的差值，$ f_{\rm d} =f_{\rm n} $；$ m $为导纳控制器的惯性系数；$ b $为阻尼系数；$ k $为刚度系数。

若将刚度系数$ k $设置为0，则既可以在接触作业中进行力跟踪控制^[27]，还可以在人工扰动情况下控制机器人跟随人的意图运动^[2]。结合所建立的动态系统的输出速度$ \mathit{\boldsymbol{v}}_{B} $，获得$ \{B\} $系下速度控制量$ \mathit{\boldsymbol{v}}_{\rm c} $和位置控制量$ \mathit{\boldsymbol{x}}_{\rm c} $分别为

$ \begin{align} \begin{cases} \mathit{\boldsymbol{v}}_{\rm c} =\mathit{\boldsymbol{v}}_{B} +{}_{E}^{B} \mathit{\boldsymbol{M}} (\mathit{\boldsymbol{S}}_{Z} \Delta v) \\ \mathit{\boldsymbol{x}}_{\rm c} =\Delta\mathit{\boldsymbol{x}}_{\rm c}+\mathit{\boldsymbol{x}}_{\rm e} \end{cases} \end{align} $

(15)

其中，$ \mathit{\boldsymbol{S}}_{Z} =(0, 0, 1)^{\rm T} $；$ \Delta v $为导纳力控制产生的机械臂末端$ z $向速度调节量；$ \Delta\mathit{\boldsymbol{x}}_{\rm c}={\mathit{\boldsymbol{v}}}_{\rm c}T $；$ {}_{E}^{B} \mathit{\boldsymbol{M}} $为$ \{E\} $系相对于$ \{B\} $系的旋转变换矩阵，$ {}_{E}^{B} \mathit{\boldsymbol{M}}={}_{J}^{B} \mathit{\boldsymbol{M}}{}_{E}^{J} \mathit{\boldsymbol{M}} $。

对机械臂采用离散控制形式，在第$ t $控制周期，对机械臂末端与模具接触的导纳控制产生的速度调节量$ \Delta v(k) $和期望加速度$ \ddot{x}_{{\rm c}z} (t) $分别为

$ \begin{align} \begin{cases} \Delta v(t)=\ddot{x}_{{\rm c}z} (t)\cdot T \\ \ddot{x}_{{\rm c}z} (t)=\ddot{x}_{{\rm e}z} (t)+\dfrac{1}{m} \Big[ \Delta f(t)-b\big( \dot{x}_{{\rm c}z} (t-1)-\dot{x}_{{\rm e}z} (t) \big) \Big] \end{cases} \end{align} $

(16)

其中，$ \dot{x}_{{\rm e}z} (t) $和$ \ddot{x}_{{\rm e}z} (t) $分别为第$ t $控制周期实际运动的速度和加速度，$ \Delta f(t) $为第$ t $控制周期的力误差，$ \dot{x}_{{\rm c}z} (t-1) $为第$ t-1 $控制周期执行器的$ z $向速度。

第$ t $控制周期，坐标系$ \{B\} $下机械臂末端位置控制量输出为

$ \begin{align} \mathit{\boldsymbol{x}}_{\rm c} (t)=\Delta\mathit{\boldsymbol{x}}_{\rm c}(t)+\mathit{\boldsymbol{x}}_{\rm e} (t) \end{align} $

(17)

其中，$ \Delta\mathit{\boldsymbol{x}}_{\rm c}(t)={\mathit{\boldsymbol{v}}}_{\rm c}(t)T $，$ \mathit{\boldsymbol{v}}_{\rm c} (t)=\mathit{\boldsymbol{v}}_{B} (t)+{}_{E}^{B} \mathit{\boldsymbol{M}} (\mathit{\boldsymbol{S}}_{z} \Delta v(t)) $，$ \mathit{\boldsymbol{x}}_{\rm e} (t) $为第$ t $控制周期时机械臂末端的实际位置。

当机器人在接触空间作业时，搜索距离当前接触点最近的示教轨迹点的期望姿态$ \mathit{\boldsymbol{q}}_{0}' $作为机器人实际运动的期望姿态$ \mathit{\boldsymbol{q}}_{\rm n} $，且当前机械臂末端的实际姿态为$ \mathit{\boldsymbol{q}}_{\rm e} $。利用四元数与RPY（横滚、俯仰、偏航）角之间的转换关系，获得实际角度$ \mathit{\boldsymbol{\theta}}_{\rm c} $与期望角度$ \mathit{\boldsymbol{\theta}}_{\rm d} $相对于坐标系$ \{B\} $的角误差$ \Delta \mathit{\boldsymbol{\theta}} =(\Delta \alpha, \Delta \beta, \Delta \gamma)^{\rm T} $，通过$ {\rm PI} $控制器计算获得角度调节量$ \Delta \mathit{\boldsymbol{\theta}}_{\rm c} $与角度控制量$ \mathit{\boldsymbol{\theta}}_{\rm c} $，并将$ \mathit{\boldsymbol{\theta}}_{\rm c} $转换为四元数姿态，得到机械臂末端控制姿态$ \mathit{\boldsymbol{q}}_{\rm c} $。

第$ t $控制周期，经PI控制器计算，获得角度调节量输出和角度控制量输出：

$ \begin{align} \begin{cases} \Delta \mathit{\boldsymbol{\theta}}_{\rm c} (t)=\mathit{\boldsymbol{K}}_{\rm P} \Delta \mathit{\boldsymbol{\theta}} (t)+\mathit{\boldsymbol{K}}_{\rm I} \sum\limits_{i=0}^{t} \Delta \mathit{\boldsymbol{\theta}} (i) \\ \mathit{\boldsymbol{\theta}}_{\rm c} (t)=\mathit{\boldsymbol{\theta}}_{\rm e} (t)+\Delta \mathit{\boldsymbol{\theta}}_{\rm c} (t) \end{cases} \end{align} $

(18)

其中，$ \Delta \mathit{\boldsymbol{\theta}} (t) $和$ \Delta \mathit{\boldsymbol{\theta}}(i) $为第$ t $控制周期和第$ i $控制周期的姿态角度误差，$ \mathit{\boldsymbol{\theta}}_{\rm e} (t) $为第$ t $控制周期的实际姿态角度，$ \mathit{\boldsymbol{K}}_{\rm P} =\left(k_{{\rm p}\alpha}, k_{{\rm p}\beta}, k_{{\rm p}\gamma}\right)^{\rm T} $为比例系数，$ \mathit{\boldsymbol{K}}_{\rm I} = $ $ \left(k_{{\rm i}\alpha}, k_{{\rm i}\beta}, k_{{\rm i}\gamma}\right)^{\rm T} $为积分系数。

4.2 人工扰动

在接触任务正常执行过程中，通过检测机械臂末端受到的外界扰动力，可以判断机械臂作业是否受到人工扰动。设定机械臂末端除$ z $向之外其他方向的力阈值，当大于阈值时，即认为发生人工扰动。在人工扰动条件下完成接触作业时，期望机械臂末端能够跟随人手运动，且在交互结束后能够恢复接触作业，为此设计将动态系统和导纳控制器相结合的运动控制框架。如图 6所示，将动态系统的速度$ \mathit{\boldsymbol{v}}_{B} $乘以方向选择矩阵$ \mathit{\boldsymbol{S}}_{T} $后，与导纳示教控制器输出的包含位姿的速度调节量$ \Delta \mathit{\boldsymbol{\dot{\xi}}} $叠加，获得机械臂末端包含位姿的速度控制量$ \mathit{\boldsymbol{\dot{\xi}}}_{\rm c} $，将其乘以控制周期$ T $获得末端位姿控制量$ \mathit{\boldsymbol{\xi}}_{\rm c} $。将$ \mathit{\boldsymbol{\xi}}_{\rm c} $输入到机器人内部控制器，实现人工扰动条件下的机器人运动控制，即机器人末端跟随人手运动。

相比于第2节基于导纳控制器的示教，当机器人自主执行任务时，将执行器$ T_{0} $直接安装在机械臂末端关节$ J_{\text{end}} $法兰处，机械臂的末端即为执行器$ T_{0} $的末端，因此当人手拖动机械臂末端运动时，传感器$ S_{0} $的检测数据$ \mathit{\boldsymbol{F}}_{0} =(\mathit{\boldsymbol{f}}_{0}, \mathit{\boldsymbol{\tau}}_{0}) ^{\rm T} $即为人手作用力与力矩$ \mathit{\boldsymbol{F}}_{2} =(\mathit{\boldsymbol{f}}_{2} {\rm, }\mathit{\boldsymbol{\tau}}_{2}) ^{\rm T} $，其在坐标系$ \{B\} $中表示为$ \mathit{\boldsymbol{F}}_{0}' =(\mathit{\boldsymbol{f}}_{0}', \mathit{\boldsymbol{\tau}}_{0}') ^{\rm T} $，即为当人施加扰动时导纳控制器的期望力与力矩。设计人施加扰动时的导纳控制律为

$ \begin{align} \mathit{\boldsymbol{M}}'\mathit{\boldsymbol{\ddot{\xi}}}+\mathit{\boldsymbol{B}}'\mathit{\boldsymbol{\dot{\xi}}}=\mathit{\boldsymbol{F}}_{0}' \end{align} $

(19)

其中，$ \mathit{\boldsymbol{M}}' $为$ 6\times 6 $惯性系数矩阵，$ \mathit{\boldsymbol{B}}' $为$ 6\times 6 $阻尼系数矩阵，$ \mathit{\boldsymbol{\ddot{\xi}}} $和$ \mathit{\boldsymbol{\dot{\xi}}} $分别为期望加速度和期望速度。

采用式(4)的计算方法，获得交互力$ \mathit{\boldsymbol{F}}_{0}' $产生的速度调节量$ \Delta \mathit{\boldsymbol{\dot{\xi}}} $，并与动态系统产生的速度$ \mathit{\boldsymbol{v}}_{B} $叠加，获得速度的控制量：

$ \begin{align} \Delta \mathit{\boldsymbol{\dot{\xi}}}_{\rm c} =\Delta \mathit{\boldsymbol{\dot{\xi}}}+\mathit{\boldsymbol{V}}_{B} =\Delta \mathit{\boldsymbol{\dot{\xi}}}+\mathit{\boldsymbol{S}}_{T} \mathit{\boldsymbol{v}}_{B} \end{align} $

(20)

其中，$ \mathit{\boldsymbol{S}}_{T} =\left[ \mathit{\boldsymbol{I}}_{3\times 3}, \; {\mathit{\boldsymbol{0}}}_{3\times 3} \right]^{\rm T} $。

存在人工扰动时，机械臂的运动控制量为

$ \begin{align} \mathit{\boldsymbol{\xi}}_{\rm c} =\Delta \mathit{\boldsymbol{\xi}}_{\rm c} +\mathit{\boldsymbol{\xi}}_{\rm e} \end{align} $

(21)

其中，$ \Delta \mathit{\boldsymbol{\xi}}_{\rm c} =\Delta \mathit{\boldsymbol{\dot{\xi}}}_{\rm c} T $，$ \mathit{\boldsymbol{\xi}}_{\rm e} $为机器人实际运动的位姿反馈。

在人工扰动消失后，机械臂末端从自由空间向接触空间运动，采用4.1节的接触空间作业控制方式，在导纳力控制器和动态系统的共同作用下，使机械臂快速到达接触空间并恢复接触作业。

4.3 自由空间运动

在机械臂进行接触作业之前，首先其末端从自由空间向接触空间运动，需要对此进行规划与控制。基于第3节建立的动态系统，将示教轨迹起点设置为动态系统的终点，可以实现机械臂从自由空间任意起点$ \xi_{a} $到接触空间轨迹起点的运动规划。如图 7所示，接触面一般为曲面，这导致动态系统规划的运动在未到达示教轨迹起点$ \xi_{b} $前已经在点$ \xi_{a}' $发生接触，并且从接触点$ \xi_{a}' $开始，机器人末端姿态的$ z $向须为曲面的法向。当从接触点$ \xi_{a}' $运动到$ \xi_{b} $时，单一的动态系统的运动规划会使得接触力过大，影响机械臂的正常工作。

为保证机械臂末端在到达示教轨迹起点前可以正常运动，在动态系统基础上添加$ z $向导纳力控制器产生的速度调节量$ \Delta v $，导纳控制律与式(14)相同，并将$ z $向期望力设置为第2节获得的示教轨迹$ \mathit{\boldsymbol{\xi}}_{0} $的第1个起点的$ z $向期望力$ f_{{\rm n}1} $，其运动速度控制量$ \mathit{\boldsymbol{v}}_{\rm c}' $和位置控制量$ \mathit{\boldsymbol{x}}_{\rm c}' $分别为

$ \begin{align} \begin{cases} \mathit{\boldsymbol{v}}_{\rm c}' =\mathit{\boldsymbol{v}}_{B} +{}_{E}^{B} \mathit{\boldsymbol{M}} (\mathit{\boldsymbol{S}}_{Z} \Delta v) \\ \mathit{\boldsymbol{x}}_{\rm c}' =\mathit{\boldsymbol{v}}_{\rm c}' T+\mathit{\boldsymbol{x}}_{\rm e} \end{cases} \end{align} $

(22)

其中，$ \Delta v $计算方法与式(16)相同。

上述的运动规划保证了在机械臂未到达示教轨迹起点就与曲面提前接触时，机械臂仍可正常工作。此外，还需考虑运动中的姿态规划。首先对自由空间运动进行姿态规划，将终点$ \xi_{b} $的姿态$ \mathit{\boldsymbol{q}}_{b} $作为终点姿态，通过四元数球面插值的方式，获得运动轨迹点的姿态为

$ \begin{align} \mathit{\boldsymbol{q}}_{b}' =\mathit{\boldsymbol{q}}_{a} \otimes (\mathit{\boldsymbol{q}}_{a}^{-1} \otimes \mathit{\boldsymbol{q}}_{b})^{t_{1}} \end{align} $

(23)

其中，$ 0\leqslant t_{1} \leqslant 1 $；$ \otimes $为四元数乘法，若四元数$ \mathit{\boldsymbol{p}}=(p_{w}, \mathit{\boldsymbol{p}}_{v}^{\rm T})^{\rm T} $，四元数$ \mathit{\boldsymbol{q}}=(q_{w}, \mathit{\boldsymbol{q}}_{v}^{\rm T})^{\rm T} $，则其乘法为$ \mathit{\boldsymbol{p}}\otimes \mathit{\boldsymbol{q}}=(p_{w} q_{w} -\mathit{\boldsymbol{p}}_{v} \mathit{\boldsymbol{q}}_{v}, p_{w} \mathit{\boldsymbol{q}}_{v}^{\rm T}+q_{w} \mathit{\boldsymbol{p}}_{v}^{\rm T}+\mathit{\boldsymbol{p}}_{v}^{\rm T}\times \mathit{\boldsymbol{q}}_{v}^{\rm T}) $。

连接自由空间轨迹起点$ \xi_{a} $位置$ \mathit{\boldsymbol{x}}_{a} =(x_{a}, y_{a}, z_{a})^{\rm T} $和终点$ \xi_{b} $的位置$ \mathit{\boldsymbol{x}}_{b} =(x_{b}, y_{b}, z_{b})^{\rm T} $获得直线段$ L $，在第$ t $控制周期，将机械臂末端控制位置$ \mathit{\boldsymbol{x}}_{c}' (t) $投影到直线$ L $，投影点的位置为$ \mathit{\boldsymbol{x}}_{c}'' (t)=(x_{i}, y_{i}, z_{i})^{\rm T} $，由点$ \mathit{\boldsymbol{x}}_{a} $、$ \mathit{\boldsymbol{x}}_{b} $和$ \mathit{\boldsymbol{x}}_{c}'' (t) $共线可得：

$ \begin{align} \frac{x_{i} -x_{a}} {x_{b} -x_{a}} =\frac{y_{i} -y_{a}} {y_{b} -y_{a}} =\frac{z_{i} -z_{a}} {z_{b} -z_{a}} =\rho \end{align} $

(24)

则可获得投影点的各坐标值为

$ \begin{align} \begin{cases} x_{i} =\rho (x_{b} -x_{a})+x_{a} \\ y_{i} =\rho (y_{b} -y_{a})+y_{a} \\ z_{i} =\rho (z_{b} -z_{a})+z_{a} \end{cases} \end{align} $

(25)

投影点位置$ \mathit{\boldsymbol{x}}_{c}'' (t) $与起点位置$ \mathit{\boldsymbol{x}}_{a} $之间的距离与直线段$ L $的长度之比为

$ \begin{align} \beta =\frac{\left\| \mathit{\boldsymbol{x}}_{c}'' (t)-\mathit{\boldsymbol{x}}_{a} \right\|}{\| \mathit{\boldsymbol{x}}_{b} -\mathit{\boldsymbol{x}}_{a} \|} \end{align} $

(26)

将$ \beta $作为四元数球面插值的指数$ t_{1} $，即可获得自由空间运动轨迹上各点的对应姿态。

当机械臂末端到达接触空间但未到达示教轨迹起点时，期望姿态为$ \mathit{\boldsymbol{q}}_{\rm d}' $。首先，将接触点所在模具表面的三角面片法向$ \mathit{\boldsymbol{n}}_{z} $作为机械臂末端的$ z $向，当前运动方向$ \mathit{\boldsymbol{v}}_{q}' $设置为$ y $方向，采用式(6)的计算方法，获得$ x $方向$ \mathit{\boldsymbol{n}}_{x} $，再采用式(7)的计算方法，获得新的$ y $方向$ \mathit{\boldsymbol{n}}_{y} $，并将获得的期望姿态转换成四元数$ \mathit{\boldsymbol{q}}_{\rm n}' $。通过传感器$ S_{0} $实时检测末端力，在检测到末端与模具表面接触后，将姿态$ \mathit{\boldsymbol{q}}_{\rm n}' $作为期望姿态$ \mathit{\boldsymbol{q}}_{\rm d}' $，并采用4.1节的PI姿态控制法对提前接触点$ \xi_{a}' $和示教起点$ \xi_{b} $之间的轨迹姿态进行控制。

5 实验与分析（Experiment and analysis）

基于第2节搭建的示教平台和第4节提出的接触任务不同状态的机器人运动控制方法，针对碳纤维织物铺放任务分别进行无人工扰动作业和有人工扰动作业实验，验证本文方法的有效性。

针对无人工扰动条件下的接触任务作业，从自由空间中选择10个不同的起点开始运动。机器人先到达示教轨迹起点，然后自主进行接触任务作业。10次实验中，实际轨迹与示教轨迹对应部分相比，在示教轨迹起点处轨迹误差最大为0.004 m，在示教轨迹终点处为0.007 m，示教轨迹起点和示教轨迹终点之间部分的轨迹误差在0.008 m以内，姿态误差在1.01$ ^{\circ} $以内。对动态系统添加力控调制项后，不同起点处的实际轨迹位置重复性较好，并且在PI控制下姿态偏差较小，可以获得较好的控制效果。相比于只进行动态系统运动规划的情形，机械臂末端$ z $向的力控制对位置控制产生调节量，同时考虑到接触任务中模具$ M_{0} $为自由曲面，变化的表面形状对动态系统向示教轨迹的贴合运动也产生了一定阻碍，导致实际轨迹与示教轨迹之间存在一定的偏差。

随机选择其中3个不同起点的运动轨迹，如图 8(a)所示，机械臂末端从自由空间开始向示教轨迹起点运动，且在到达示教轨迹起点附近后，不同起点运动轨迹接近重合，向示教轨迹终点运动，重复性较好。其中3个不同起点轨迹对应示教轨迹部分的姿态误差如图 8(b)所示，在PI控制下姿态最大误差为0.98$ ^{\circ} $，姿态控制效果较好。

在人工扰动条件下做了10次接触任务作业实验。每次实验选择相同的运动起点，当机械臂末端运动到随机位置时，施加人工扰动，将机械臂末端拖离接触面，机械臂末端随人手运动；扰动结束后，释放机械臂末端，让其自动恢复接触作业。在施加人工扰动前，机械臂末端处于接触作业状态，且与无人扰动的接触作业状态相同。消除扰动并恢复接触作业后，机械臂轨迹与示教轨迹之间的位置、姿态最大误差与无人扰动时的结果一致，其中第1次施加人工扰动时机械臂末端的全部运动轨迹$ R_{1} $如图 9(a)所示，局部运动轨迹如图 9(b)所示；在$ r_{1} $点施加人工扰动，机械臂末端与模具$ M_{0} $脱离接触，在$ r_{2} $点消除人工扰动，$ r_{1} $与$ r_{2} $点之间为机械臂末端在自由空间中的运动轨迹，消除扰动后机械臂末端逐渐与模具恢复接触，并且其轨迹逐渐与无人扰动时的$ H_{1} $轨迹接近重合。

为对比本文方法与基于时间的离线运动规划与控制的力跟踪效果，采用4.1节的导纳力控制方法，并将图 5中的$ \mathit{\boldsymbol{v}}_{B} $变为离线控制速度，进行离线的力跟踪实验。在调整控制器参数后进行10次重复实验，得到力的均方根误差，如表 1所示，其范围为0.73~0.80 N，平均值为0.77 N。采用本文方法，在无人工扰动条件下和有人工扰动条件下分别进行10次实验，恢复接触作业后的力与对应期望力的均方根误差分别如表 2和表 3所示。在无人工扰动时力的均方根误差范围为0.81~0.93 N，平均值为0.86 N，而在有人工扰动时恢复接触作业后的力均方根误差范围为0.85~0.96 N，平均值为0.91 N。本文方法相比于基于时间序列的离线运动规划与控制法，在无人扰动时力的均方根误差平均值增大0.09 N，原因为动态系统在线规划轨迹与示教轨迹之间存在偏差，且曲面的变化对力的跟踪也有一定影响，使得力的偏差较大。在有人扰动时相比于无人扰动时力的均方根误差增大0.05 N，二者结果相近。

在10次离线实验中，力跟踪效果最佳实验的实际力如图 10(a)所示，力误差值变化如图 10(b)所示，误差最大绝对值为2.43 N，98.5%的力误差分布范围为$ - $2.0~1.7 N。其中，图 8的$ H_{1} $轨迹所对应示教轨迹部分的力变化如图 11(a)所示，力误差值变化如图 11(b)所示，误差最大绝对值为2.59 N，98.7%的误差值分布范围为$ - $2.2~2.0 N，力均方根误差为0.86 N。相比于离线实验的最佳结果，$ H_{1} $轨迹的力控制误差绝对值与力误差值变化范围与其接近，但均方根误差较大，且在终点附近，由于动态系统不断向终点收敛，因此力误差较大。

恢复接触后$ R_{1} $轨迹对应示教轨迹部分的力变化如图 12(a)所示，力误差值变化如图 12(b)所示。刚恢复接触时接触力较小，而接触轨迹中间部分的期望力较大，使得在刚恢复接触位置处力误差绝对值最大，为4.1 N。恢复接触后，随着运动，力的误差值逐渐减小，98.4%的力误差值分布范围为$ - $2.4~2.2 N，均方根误差为0.90 N，与$ H_{1} $轨迹的力跟踪结果相近。在消除人工扰动后，本文方法自动恢复接触作业，且仍可获得与无人扰动时相近的力控效果。同时，消除人工扰动后，机械臂末端刚恢复接触时力误差较大，因此与无人工扰动的情况相比，有人工扰动时力均方根误差的平均值较大。

6 结论（Conclusion）

针对存在人工扰动条件下的机器人接触作业，提出了基于微分同胚映射动态系统的运动规划和控制方法，并分别设计基于动态系统的力控制调节项和姿态控制项，使机器人在接触任务执行过程中具有较强的抗扰性。具体贡献如下：

(1) 采用微分同胚映射动态系统对人工示教轨迹位置建立动态系统，实现了接触任务的运动快速规划，并考虑接触任务中接触作业、人工干扰下运动和自由空间运动等不同的需求，分别设计基于运动动态系统的位置控制调节项和姿态控制项，实现了基于动态系统的接触任务全过程运动规划与控制。

(2) 搭建碳纤维织物复合材料铺放实验平台，在无人工扰动时，机械臂末端从自由空间中任意起点开始运动，10次实验的力均方根误差为0.86 N，与离线方法的力跟踪实验结果相近，充分说明了该方法可以用于接触任务的运动规划与控制，丰富了接触任务的运动规划与控制方法。

(3) 当有人工扰动时，消除扰动、恢复接触作业后与无人工扰动条件下的力均方根误差结果相近，且轨迹与无人工扰动时的轨迹接近重合，验证了该方法具有较强的抗扰动性，可以用于有人工干预作业需求的接触任务执行场景。

研究过程中，未考虑曲面形状变化对基于动态系统在线运动规划的力控制的影响，后续工作拟对曲面形状变化对力控制的影响进行研究，提高本文方法的力跟踪效果。