欢迎访问《控制理论与应用》期刊网站！

不确定扰动下的打磨机器人动态轨迹规划

doi: 10.7641/CTA.2025.40452

邹涛，凌振舫，项超群，张建辉

广州大学机械与电气工程学院, 广东广州 510006

基金项目: 广州市室校联合实验室项目(2023A03J0120)资助

详细信息

作者简介

邹涛教授,主要研究方向为工业过程建模与仿真、模型预测控制、先进过程控制和实时优化技术,E-mail: tzou@gzhu.edu.cn;

凌振舫硕士研究生,目前研究方向为机器视觉、机器人轨迹规划等,E-mail: lingzhenfang104@gmail.com;

项超群副教授,硕士生导师,主要研究方向为仿生机器人感知、建模及驱控、助力及康复机器人等,E-mail: cqxiang86@163.com,

张建辉教授,博士生导师,研究方向主要包括压电流体驱动技术、超声电机技术、光声成像技术,E-mail: zhangjh@nuaa.edu.cn.

通信作者

凌振舫，E-mail: 1244275527@qq.com;Tel.:+8618106547231

Dynamic trajectory planning of polishing robot based on fuzzy compensation model predictive control

ZOU Tao ， LING Zhen-fang ， XIANG Chao-qun ， ZHANG Jian-hui

School of Mechanical and Electrical Engineering, Guangzhou University, Guangzhou Guangdong 510006 , China

Funds: Supported by the Guangzhou City University-Laboratory Joint Laboratory Funded Project (2023A03J0120)

摘要

针对存在关节约束、摩擦干扰及末端外力扰动等复杂工况下的打磨机器人轨迹规划问题, 本文提出一种融合力反馈的双层结构预测控制轨迹规划方法, 旨在实现动态轨迹规划并提升系统干扰能力. 首先, 上层采用模型预测控制在理想条件下生成全局最优轨迹; 下层通过模糊补偿的预测控制根据轨迹偏差及其变化率实时修正预测控制输出, 同时结合力传感器反馈数据构建动态约束优化模型. 最后, 通过仿真实验验证了该方法通过分层优化与实时补偿的协同机制, 可以在复杂工况下实现稳定、平滑的轨迹输出.

关键词

打磨机器人 / 动态轨迹规划 / 模糊补偿控制 / 力反馈 / 双层结构预测控制

Abstract

To address the trajectory planning challenges of polishing robots under complex working conditions involving joint constraints, frictional disturbances, and external end-effector forces, a dual-layer predictive control method integrated with force feedback is proposed to achieve dynamic trajectory planning and enhance system robustness. The hierarchical framework consists of two layers: the upper layer generates globally optimal trajectories using model predictive control (MPC) under ideal conditions, while the lower layer dynamically adjusts the control output through fuzzy-compensated predictive control. Simultaneously, force sensor feedback is integrated to construct a dynamic constraint optimization model, which adjusts pressure thresholds based on task requirements to prevent overshoot and ensure safe interaction. Simulation results demonstrate that the proposed method achieves stable and smooth trajectory tracking in complex environments through the synergistic mechanism of hierarchical optimization and real-time compensation, with significant improvements in both precision and energy efficiency.

Keywords

polishing robot / dynamic trajectory planning / fuzzy compensation control / force feedback / dual-layer predictive control

1 引言 2 打磨机器人建模 2.1 理想状态下打磨机器人的动力学方程 2.2 打磨任务中的机器人动力学方程 3 打磨机器人轨迹规划 3.1 打磨机器人末端靠近工件过程中的末端移动轨迹规划 3.2 打磨机器人打磨过程中的轨迹规划及优化 3.3 力矩信息与位置信息的转换 4 仿真实验与分析 5 结论

1 引言

现代机械加工制造过程中，产品表面的打磨抛光是其中不可或缺的一个环节，打磨抛光不仅可以提升产品的外观美观度，还可以去除材料表面的毛刺、划痕、飞边等缺陷，提高表面光滑度、减少材料表面的微小裂纹和划痕，增加产品耐腐蚀性和耐磨性、增强产品的性能，使之达到所需的精确度和功能性以及去除应力集中等. 但是一般产品的打磨工作环境恶劣，存在严重的粉尘污染和高温环境等问题. 因此，引入机器人进行自动化打磨成为了一种出色的解决办法 ^[1-2]，打磨机器人不仅可以保持高效率作业还可以保证作业中操作的精确性，尤其是在材料处理和表面加工等环节，通过精确的运动控制，打磨机器人在确保可重复性的同时可以保证零件打磨结果更加精确规整，具备高精度的特点. 近年来，随着制造业的发展，机器人打磨抛光系统已经广泛应用于自由曲面工件的表面加工，如汽车车身、风力发电机叶片和航空发动机叶片等复杂曲面零件 ^[3]，在对这些自由曲面工件进行抛光加工时都需要对打磨机器人进行合理的打磨轨迹规划以及高精度的轨迹跟踪控制，以避免工件上的过抛光或欠抛光现象，从而大大提高表面质量 ^[4] .

工业机器人的轨迹规划是指在给定条件下，规划一条符合机器人运动学和动力学约束的从初始状态到目标状态的路径，并对该路径进行时间参数化，可分为笛卡尔空间和关节空间两种 ^[5] . 笛卡尔空间轨迹规划直观，精度高，但求逆解次数多，计算量太大且可以规划的轨迹大都比较简单. 而关节空间轨迹规划计算量小，适用场景多，因而在实际应用中，关节空间轨迹规划应用较多 ^[6] . 随着现代制造业对打磨机器人的要求越来越高，不仅需要对打磨机器人进行轨迹规划，还需要根据任务要求等因素对打磨机器人的运动轨迹进行优化 ^[7] . 针对上述问题，研究人员针对时间最优、能量最优以及混合最优等方面展开了研究 ^[8-11] . 在曲面加工领域，有研究人员 ^[12] 根据机器人动力学方程，以速度约束下的能量最优为目标，建立机器人系统的能量特性模型，并通过优化算法，有效地降低了机器人的能量损耗. Lin ^[13] 基于三次样条插值法，以最小加速度和关节速度约束为优化目标，通过粒子群和K-means聚类的混合算法优化，有效地减少了机器人运行过程中关节受到的冲击.

在实际加工作业中，打磨机器人往往会受到许多不确定性因素的影响，因此在有了优化的轨迹之后还需要控制打磨机器人关节的运动，对轨迹进行跟踪. 近几十年来，随着模型预测控制（model predictive control，MPC）在经典工业过程控制中的出色表现，成为了解决上述机器人打磨问题的一种合适而有效的方法 ^[14-18]，它可以根据系统的动力学模型，通过预测状态变量的来生成最优控制序列，约束打磨机器人动力系统，在复杂情况下提供最优控制策略. 但当其应用于真实工业环境中的机器人系统时，仍然不可避免的会受到建模不准确以及内部摩擦和外部干扰等不确定性因素的影响 ^[19-22]，并且在工作时机器人面临的外部条件也是不断变化的，因此需要在打磨过程中对机器人进行动态轨迹规划. 目前，大多数机器人的动态轨迹规划的研究集中在避障领域 ^[23-25]，利用传感器等感知系统的信息帮助机器人规划出一条安全的轨迹.

针对上述问题，本文提出了一种基于双层结构预测控制的打磨机器人动态轨迹规划方法，该方法可以在对打磨机器人进行轨迹规划的同时根据传感器返回的实时信息和相关约束对打磨轨迹进行优化，其中具有模糊补偿的预测控制框图如图1所示. 整体结构框图如图2所示.

图1控制框图

Fig.1Lower-level control block diagram

2 打磨机器人建模

2.1 理想状态下打磨机器人的动力学方程

理想情况下，机械臂的动力学方程为

M (q) \ddot{q} + C (q, \dot{q}) \ddot{q} + G (q) = τ

(1)

其中:

q ， \dot{q} ， \ddot{q}

分别为关节i 的角位移向量、角速度矢量和角加速度向量; M（q）为质量矩阵; C（q，

\dot{q}

）为离心力和科氏力向量; G（q）为重力向量.

2.2 打磨任务中的机器人动力学方程

在实际打磨任务中，要考虑机械臂系统存在的摩擦以及执行打磨任务时打磨工件对机械臂末端的作用力等因素的影响，在这种情况下，打磨机器人的动力学方程可以表示为

M (q) \ddot{q} + C (q, \dot{q}) \dot{q} + G (q) + F_{r} (\dot{q}) + J^{T} F = τ,

(2)

其中:

F_{r} （ \dot{q} ）

为关节的摩擦力矩; J^T为打磨机器人的力雅可比矩阵，它是速度雅可比矩阵的转置; F为打磨机器人末端所受到的打磨工件对机械臂末端的作用力，其在机器人执行打磨任务时可以由六维力传感器测量得到.

以最常用的库伦–粘滞摩擦模型来表示打磨机器人关节的摩擦力矩，则打磨机器人的动力学方程变为

M (q) \ddot{q} + C (q, \dot{q}) \dot{q} + G (q) +

F_{c} s g n \dot{q} + F_{v} (\dot{q}) + J^{T} F = τ

(3)

其中: F_c为每个关节库仑摩擦系数组成的矩阵，F_v为由每个关节粘性摩擦系数组成的对角矩阵. 由库伦–粘滞摩擦模型可知，这里的F_c和F_v均为对角矩阵.

图2整体结构框图

Fig.2Structure diagram

3 打磨机器人轨迹规划

本文针对的打磨机器人进行打磨任务的轨迹可以分为末端靠近待打磨工件和对待打磨工件进行打磨两个部分，本文将分别对这两部分进行打磨机器人运动轨迹的规划.

3.1 打磨机器人末端靠近工件过程中的末端移动轨迹规划

如图3所示，设工件上的打磨轨迹起始点为I，点H 在点I的外部且与I在同一条表面法线上，则打磨机器人末端靠近工件过程中末端的移动轨迹可以分为两个阶段：第1阶段为打磨机器人调整末端姿态并快速靠近点H的过程，第2阶段为打磨工具沿工件的表面法线方向贴合打磨轨迹起始点I的过程.

在这两个阶段中，打磨机器人末端进行点到点的运动，可以在关节空间中对打磨机器人的运动轨迹进行规划，点H和点I所对应的关节空间的各关节旋转角度可以通过机器人逆运动学解算得出.

在第1阶段，由于打磨机器人末端靠近待打磨工件时末端不会受到作用力，因此式（3）中J ^TF = 0，则在这个过程中，式（3）可以简化为

M (q) \ddot{q} + C (q, \dot{q}) \dot{q} + G (q) +

F_{c} s g n \dot{q} + F_{v} (\dot{q}) = τ

(4)

图3机器人末端靠近工件示意图

Fig.3Diagram of the robot end-effector approaching the workpiece

对于打磨机器人的动力学方程，设状态变量x =

{[\begin{matrix} x_{1}^{T} x_{2}^{T} \end{matrix}]}^{T} = {[\begin{matrix} q^{T} {\dot{q}}^{T} \end{matrix}]}^{T}

，控制变量u = τ，输出变量y = q，则打磨机器人系统的状态方程可以表示为

\begin{matrix} \dot{x} = {[{\dot{q}}^{T} {\ddot{q}}^{T}]}^{T} = \\ [\begin{matrix} \dot{q} \\ M (q)^{- 1} [u - C (q, \dot{q}) \dot{q} - G (q) - F_{c} s g n \dot{q} - F_{v} (\dot{q})] \end{matrix}] . \end{matrix}

(5)

打磨机器人在工作过程中除了会受到关节摩擦的影响以外，上式中的

C （ q ， \dot{q} ）

和G（q）也不一定与打磨过程中真实的离心力，科氏力向量和重力向量相等，因此打磨机器人系统作为强耦合和高度非线性的系统，在一系列不确定性因素的影响下，只采用模型预测控制算法对其进行运动轨迹控制效果可能会不理想，尤其在打磨机器人末端已经很靠近打磨工件时，不精确的末端运动轨迹很可能会导致打磨工件与末端发生碰撞. 因此，本文通过构建模糊补偿器来消除不确定项对打磨机器人运动轨迹的影响，将得到的补偿控制量与模型预测控制相结合，实现打磨机器人在靠近打磨工件过程中的精确控制，即，令

ξ = ξ_{1} + ξ_{2},

(6)

其中: ξ为控制器总控制量; ξ₁为模型预测控制的控制量; ξ₂为模糊补偿的控制量.

在理想情况下系统的状态空间方程可以表示为

\{\begin{matrix} \dot{x} = A x + B ξ_{1}, \\ y = C x . \end{matrix}

(7)

假设采样周期为T，则在t = k时刻对打磨机器人系统状态空间方程用前向欧拉法进行离散化，即

\frac{x (k + 1) - x (k)}{T} = A x (k) + B ξ_{1} (k) .

(8)

则式（8）可转化为

\{\begin{array}{l} x (k + 1) = (I + T A) x (k) + T B ξ_{1} (k), \\ y = C x . \end{array}

(9)

令

A^{'} = (I + T A),

(10)

B^{'} = T B .

(11)

则在k时刻，输入一组U_k，向后预测N步，根据当前的x（k）可得

\begin{matrix} x (k + 1 ∣ k) = A^{'} x (k ∣ k) + B^{'} ξ_{1} (k ∣ k), \\ ⋮ \\ x (k + N ∣ k) = A^{'} x (k + N - 1 ∣ k) + \\ B^{'} ξ_{1} (k + N - 1 ∣ k) . \end{matrix}

(12)

则

\begin{matrix} [\begin{matrix} x (k ∣ k) \\ x (k + 1 ∣ k) \\ x (k + 2 ∣ k) \\ ⋮ \\ x (k + N ∣ k) \end{matrix}] = \\ [\begin{matrix} I \\ A^{'} \\ {(A^{'})}^{2} \\ ⋮ \\ {(A^{'})}^{N} \end{matrix}] x (k) + [\begin{matrix} 0 & 0 & \dots & 0 \\ B^{'} & 0 & \dots & 0 \\ A^{'} B^{'} & B^{'} & \dots & 0 \\ ⋮ & ⋮ & ⋮ \\ {(A^{'})}^{N - 1} B^{'} & {(A^{'})}^{N - 2} B^{'} & \dots & B^{'} \end{matrix}] \times \end{matrix}

[\begin{matrix} ξ_{1} (k ∣ k) \\ ξ_{1} (k + 1 ∣ k) \\ ξ_{1} (k + 2 ∣ k) \\ ⋮ \\ ξ_{1} (k + N - 1 ∣ k) \end{matrix}] .

(13)

定义k时刻的当前状态与目标状态的差值为

e (k) = x (k) - x_{t} (k),

(14)

其中x_t（k）为打磨机器人最终的目标状态，则同理可得

\begin{matrix} [\begin{matrix} e (k ∣ k) \\ e (k + 1 ∣ k) \\ e (k + 2 ∣ k) \\ ⋮ \\ e (k + N ∣ k) \end{matrix}] = \\ [\begin{matrix} 1 \\ A^{'} \\ {(A^{'})}^{2} \\ ⋮ \\ {(A^{'})}^{N} \end{matrix}] x (k) + [\begin{matrix} 0 & 0 & \dots & 0 \\ B^{'} & 0 & \dots & 0 \\ A^{'} B^{'} & B^{'} & \dots & 0 \\ ⋮ & ⋮ & ⋮ \\ {(A^{'})}^{N - 1} B^{'} {(A^{'})}^{N - 2} B^{'} & \dots & B^{'} \end{matrix}] \times \\ [\begin{matrix} ξ_{1} (k ∣ k) \\ ξ_{1} (k + 1 ∣ k) \\ ξ_{1} (k + 2 ∣ k) \\ ⋮ \\ ξ_{1} (k + N - 1 ∣ k) \end{matrix}] - [\begin{matrix} x_{t} (k ∣ k) \\ x_{t} (k + 1 ∣ k) \\ x_{t} (k + 2 ∣ k) \\ ⋮ \\ x_{t} (k + N - 1 ∣ k) \end{matrix}] . \end{matrix}

(15)

如果只考虑打磨机器人每个关节的驱动力矩所作的功，则打磨机器人各个关节的功率可以用关节力矩的平方表示如下:

P_{i} = τ_{i}^{2}

(16)

则打磨机器人的总能耗为

E = \sum_{i = 1}^{N} P_{i} .

(17)

令代价函数为

\begin{matrix} J = \sum_{i = 1}^{N - 1} {‖q_{i} e (k + i ∣ k)‖}_{Q}^{2} + \\ \sum_{i = 1}^{N - 1} {‖Δ ξ_{1} (k + i ∣ k)‖}_{I}^{2} + \\ \sum_{i = 0}^{N} {‖ξ_{1} (k + i ∣ k)‖}_{R}^{2} + \\ ‖ e (k + N ∣ k) ‖_{F}^{2} + ρ ε, \end{matrix}

(18)

其中: ∆u为控制量的变化值; Q和R分别表示状态差值权重和控制输入权重; ρε为松弛因子. ∆u越小，表示输出的轨迹越平滑，可以保证打磨机器人各个关节的速度和加速度不会发生突变，对于打磨机器人的关节做功来说，也可以防止其各个关节快速的加速和减速，减少不必要的运动，减少打磨机器人工作过程中消耗的能量.

在实际任务中，打磨机器人系统会受到约束，现定义系统的状态约束和输入约束为

\{\begin{matrix} τ_{m i n} < τ < τ_{m a x}, \\ θ_{m i n} < q < θ_{m a x}, \\ {\dot{θ}}_{m i n} < \dot{q} < {\dot{θ}}_{m a x} . \end{matrix}

其中: θ_min，θ_max，

{\dot{θ}}_{m i n}

，

{\dot{θ}}_{m a x}

分别表示关节机械臂的最小允许角位移、最大允许角位移、最小角速度、最大角速度.

上式可转化为

\{\begin{matrix} x_{m i n} ⩽ x ⩽ x_{m a x} \\ ξ_{m i n} < ξ_{1} < ξ_{m a x} \end{matrix}

则上述问题可转化为

(19)

通过求解式（19）便可得到最优控制输入序列

U_{k}^{*}

，取

U_{k}^{*}

中的第1个元素

ξ_{1}^{*} （ k ）

对系统进行后续控制.

考虑到打磨机器人在实际工作过程中会受到一系列不确定性因素影响，这些不确定性因素可能会限制单一的模型预测控制算法在轨迹控制方面的效果，需要将其他控制方法与模型预测控制结合进行控制. 因此，本文将采用模糊控制理论来设计一个模糊补偿器，旨在对机械臂系统中存在的不确定性进行有效的补偿，以优化打磨机器人运行的轨迹，提高打磨机器人在复杂环境下的操作精度与可靠性.

打磨机器人系统为非线性系统，在进行轨迹优化控制时需要将其简化为线性系统，本文使用逆动力学方法，将系统的控制输入变量与状态变量的非线性关系改为一个新的控制变量与系统状态变量的线性关系，以此来实现反馈线性化，则控制律可以转化为

τ = M (q) ξ + \bar{C} (q, \dot{q}) \dot{q} + \bar{G} (q) .

(20)

在打磨机器人工作过程中，离心力和科氏力向量与重力向量不一定与真实的

C （ q ， \dot{q} ）

和G（q）相等，因此，上式中

\bar{C} （ q ， \dot{q} ）

和

\bar{G} （ q ）

分别是

C （ q ， \dot{q} ）

和G（q）的估计值.

对打磨机器人系统进行反馈线性化，可以得到

\begin{matrix} \ddot{q} = ξ + M (q)^{- 1} ((\bar{C} (q, \dot{q}) - C (q, \dot{q})) \dot{q} + \\ (\bar{G} (q) - G (q)) - F_{c} s g n \dot{q} - F_{v} (\dot{q})) . \end{matrix}

(21)

则打磨机器人运动中所有不确定的项为

\begin{matrix} Φ = M (q)^{- 1} ((\bar{C} (q, \dot{q}) - C (q, \dot{q})) \dot{q} + \\ (\bar{G} (q) - G (q)) - F_{c} s g n \dot{q} - F_{v} (\dot{q})) . \end{matrix}

(22)

本文采用模糊补偿器对式（22）中打磨机器人系统存在的不确定的项进行处理，选择双输入单输出Mamdani模糊推理机，对实际运行的轨迹和模型预测控制输出的轨迹之间的轨迹偏差和偏差的变化速率进行检测，作为模糊推理机的输入变量并模糊化，在解模糊化处理之后，得到模糊补偿器输出的控制量ξ2，以对打磨机器人的运行轨迹进行精确控制. 针对模糊推理机输出的解模糊化处理，本文采用重心（center of gravity，COG）法，它可以将模糊逻辑系统的模糊输出转化为一个精确的控制量，输出表达式为

ξ_{2} = \frac{\sum_{i = 1}^{n} c_{i} μ_{A_{i}} (x) μ_{B_{i}} (y)}{\sum_{i = 1}^{n} μ_{A_{i}} (x) μ_{B_{i}} (y)} .

(23)

式（23）计算了所有模糊集的隶属度加权平均的中心位置，可以精确地量化补偿器输出，在输入信号变化比较微小时也可以保证输出发生相应的变化，且输出控制也更平滑.

令模糊补偿器输入的轨迹偏差和偏差的变化速率分别为

d = q - q_{m p c}

(24)

\dot{d} = \dot{q} - {\dot{q}}_{m p c}

(25)

其中: q_mpc为理想情况下模型预测控制输出的关节角度;

{\dot{q}}_{m p c}

为理想情况下模型预测控制输出的关节角速度.

令模糊补偿器输入的轨迹偏差的论域为 X，偏差变化速率的论域为Y，输出控制量的论域为Z. X，Y，Z 的模糊集均为[正大正中正小零负小负中负大] = [PB PM PS ZE NS NM NB]，则对应的隶属函数如图4–6所示，模糊规则控制表如表1所示.

在k时刻，检测到该时刻的x，通过式（19）求解出最优控制输入序列

U_{k}^{*}

，然后取

U_{k}^{*}

中的第1个元素

ξ_{1}^{*} （ k ）

计算得到此时的q_mpc和

{\dot{q}}_{m p c}

，则根据轨迹偏差和偏差变化率的定义可以计算出k时刻相应的d和

\dot{d}

，输入进模糊控制器后可以得到控制补偿量ξ₂，将ξ₂与模型预测控制器的输出

ξ_{1}^{*} （ k ）

结合，计算得到打磨机器人各个关节的控制力矩并进行控制，使得打磨机器人系统可以根据性能需求和环境条件动态调整轨迹，提高机械臂的控制精度和适应能力.

图4轨迹偏差d的隶属函数

Fig.4Membership function of d

图5轨迹偏差变化率

\dot{d}

的隶属函数

Fig.5Membership function of

\dot{d}

图6模糊补偿量ξ₂的隶属函数

Fig.6Membership function of ξ₂

表1模糊控制规则表

Table1Fuzzy control rules

在第2阶段，打磨机器人末端的打磨工具从H点沿工件的表面法线方向移动到I点的过程中，末端打磨工具会逐渐对打磨工件产生压力，打磨机器人末端也会受到相应的反作用力，因此J^TF会从0开始逐渐增大. 在这个过程中，过大的打磨压力则可能会导致任务开始后打磨工具无法转动，甚至会导致夹持装置过载损坏. 因此在打磨工具贴合打磨工件的过程中，末端打磨工具受到的压力需要保持在一个适当的值.

设定打磨机器人在执行打磨任务时对工件施加的期望压力范围为[0，F_max]，此时的代价函数更新为

\begin{matrix} J = \sum_{i = 1}^{N - 1} {‖q_{i} e (k + i ∣ k)‖}_{Q}^{2} + \\ \sum_{i = 1}^{N - 1} {‖Δ ξ_{1} (k + i ∣ k)‖}_{I}^{2} + \\ \sum_{i = 0}^{N} {‖r_{i} ξ_{1} (k + i ∣ k)‖}_{R}^{2} + \\ ‖ e (k + N ∣ k) ‖_{F}^{2} + \\ λ_{e} ({(F (k) - F_{r e f})}^{2} + λ_{p} P (k)) + ρ ε, \end{matrix}

(26)

其中: λ_e是调节实际压力与期望压力之间偏差项的权重; F（k）为k时刻六维力传感器返回的压力值; F_ref为打磨压力的期望值; λ_p为惩罚项的权重; P（k）为惩罚函数，在打磨工具对工件施加的压力过大时进行补偿.

惩罚函数P（k）由下式给出:

P (k) = \{\begin{matrix} F (k) - F_{m a x}, F (k) > F_{m a x}, \\ 0, F (k) < F_{m a x} . \end{matrix}

(27)

3.2 打磨机器人打磨过程中的轨迹规划及优化

在对工件进行打磨时，一般先利用工业相机扫描得到的点云数据来提取打磨路径，然后再根据打磨路径对其进行轨迹规划，通过这些点云数据可以解算出打磨路径上的点所对应的表面法向量. 有了准确的点云数据和法向量，便能够为打磨机器人末端执行器在打磨路径上的每一点计算出必要的位姿，这里的位姿包括末端执行器的位置和姿态两部分，其中姿态的确定依赖于表面法向量的指向. 得到打磨机器人末端的目标位姿后，就可以通过机器人逆运动学解算出这些点对应的关节空间中各个关节的旋转角度，进而根据这些数据来制定整个关节空间的轨迹规划，即规划出一系列关节角度随时间变化的曲线.

假设关节i的旋转角度点为 [q₁ q₂ q₃ · · · q_n]，则设定其每个旋转角度所对应的角速度为

[\begin{matrix} {\dot{q}}_{1} {\dot{q}}_{2} {\dot{q}}_{3} \dots \end{matrix} {\dot{q}}_{n}]

，其中

[\begin{matrix} {\dot{q}}_{1} {\dot{q}}_{2} {\dot{q}}_{3} \dots \end{matrix} {\dot{q}}_{n}]

可以由下式确定:

{\dot{q}}_{j} = \{\begin{array}{ll} 0, j = 0, \\ \frac{1}{2} (\frac{q_{j + 1} - q_{j}}{Δ t} + \frac{q_{j} - q_{j - 1}}{Δ t}), 0 < j < n, \\ 0, j = n . \end{array}

(28)

将每个路径点所对应的旋转角度以及角速度进行配对，可以得到打磨机器人运动轨迹中每个点对应的目标状态.

除此之外，在机器人执行打磨任务的过程中，过小的打磨压力会导致打磨效果较差，过大的打磨压力不但会导致过打磨问题，还会加重打磨机器人末端以及各个关节的载荷，会使打磨机器人能量消耗过大，甚至可能导致打磨机器人某部位的损坏，因此打磨工件受到的压力需要保持在一个适当的值，设定打磨机器人在执行打磨任务时对工件施加的期望压力范围为 [F_min，F_max]，此时的惩罚函数P（k）更新为

P (k) = \{\begin{array}{l} F (k) - F_{max}, F (k) > F_{max}, \\ |F (k) - (\frac{1}{4} (F_{min} + F_{max}) + F_{ref})|, \\ F_{min} < F (k) < F_{max}, \\ F_{min} - F (k), F (k) < F_{min} . \end{array}

(29)

此时可通过求解下式得到ξ₁，即

(30)

3.3 力矩信息与位置信息的转换

工业机器人一般使用位置控制或速度控制的接口，而不是力矩层面的接口，因此，在这种情况下如果要求最终输出给被控对象的控制量为位置信息而不是力矩信息，那么就可以利用当前时刻的关节位置和速度结合当前时刻的控制输入，通过机器人动力学方程计算出当前时刻的关节的加速度，之后利用求得的关节加速度来更新下一时刻的关节速度，即

\dot{q} (t + Δ t) = \dot{q} (t) + \ddot{q} (t) Δ t,

(31)

之后根据更新的关节速度更新下一时刻关节位置，即

q (t + Δ t) = q (t) + \dot{q} (t + Δ t) Δ t

(32)

并且在初始时刻，q（t）= 0且

\dot{q}

（t）= 0.

这样，控制器输出的控制量就转换为机器人的关节空间轨迹.

4 仿真实验与分析

本文在仿真软件MATLAB中搭建仿真实验平台，对打磨机器人进行建模，用3个关节对本文提出的方法进行验证及分析，并将其他关节固定在零位.

在实验测试中，为了使模拟场景更加逼真，需要模拟摩擦以及末端扰动，本文采用库伦粘滞摩擦模型，其表达式为

F_{r} (\dot{q}) = 0.2 \dot{q} + 0.2 s g n \dot{q} .

(33)

对于末端力，其表达式为

F = s i n t + η

(34)

除此之外，各关节的约束如表2所示.

表2每个关节的约束

Table2Constraints for each joint

对于点与点之间，在仅使用MPC进行轨迹规划时，系统初始状态为q_start = [0 0 0]^T，参考位置为q_ref =

{[\frac{π}{4} 1 \frac{π}{2}]}^{T}

，采样周期为T = 0.001 s，而MPC控制器的预测层数N = 10.3个关节的关节空间运动轨迹如图7–9所示.

图7关节1的运动轨迹

Fig.7Motion trajectory of joint 1

图8关节2的运动轨迹

Fig.8Motion trajectory of joint 2

图9关节3的运动轨迹

Fig.9Motion trajectory of joint 3

由图7–10分析可知，在理想情况下，仅使用单纯的模型预测控制器可以在符合相关约束的情况下规划出关节运动的平滑轨迹，可以快速的到达目标点.

图10关节3的角速度

Fig.10Angular velocity of joint 3

当机器人受到内部摩擦以及外部扰动影响，利用MPC对规划的轨迹进行跟踪时，机械臂关节的关节空间运动轨迹则如由图11–13所示.

由图11–13分析可知，当在机器人运动过程中加入摩擦和外部干扰等不确定项时，仅用模型预测控制器无法实现较高精度的目标轨迹跟踪，这些不确定性强烈影响了打磨机器人的轨迹跟踪效果，关节3的跟踪误差甚至达到了1.5 rad以上，并且违反了机器人的约束条件.

图11关节1的运动轨迹

Fig.11Motion trajectory of joint 1

图12关节2的运动轨迹

Fig.12Motion trajectory of joint 2

图13关节3的运动轨迹

Fig.13Motion trajectory of joint 3

当在机器人运动过程中加入摩擦和外部干扰等不确定项，并且使用力反馈模型预测控制器以及模糊补偿器进行末端轨迹跟踪时，关节3的轨迹跟踪误差如图14所示.

从图14可以看出，使用力反馈模型预测控制器以及模糊补偿器进行末端轨迹跟踪时，关节的跟踪误差基本稳定在零值，最大跟踪误差减小为0.032 4 rad，不会违反机器人的约束条件.

图14关节3的轨迹跟踪误差

Fig.14Trajectory tracking error of joint 3

实验结果说明，在理想情况下对打磨机器人进行动态轨迹规划并利用力反馈模型预测控制器以及模糊补偿器对该轨迹进行跟踪的方法能够有效改善打磨机器人在打磨任务中存在的摩擦以及受到的末端力干扰等不确定项的影响，有效提高了打磨的精度以及机器人的稳定性.

5 结论

为了实现打磨机器人在不确定性因素干扰的情况下实现动态打磨轨迹规划、末端打磨力恒定并且提高打磨轨迹精度，本文提出了一种融入力反馈的模糊预测控制方法对打磨机器人进行轨迹规划，通过在理性情况下利用MPC控制器对打磨机器人进行关节空间的动态轨迹规划，并在引入干扰后利用融入力反馈的模糊预测控制器对该轨迹进行动态跟踪，同时利用末端力惩罚项来保证末端打磨力恒定，侧面保证打磨精度，解决了打磨机器人的动态轨迹规划问题.

实验结果表明，本文提出的方法可以在符合相关约束的情况下规划出关节运动的平滑轨迹并保证机器人可以到达目标点，并且关节的跟踪误差基本稳定在零值，最大跟踪误差减小为0.032 4 rad，显著提升了打磨的精度和机器人的稳定性.

图1控制框图

Fig.1Lower-level control block diagram

下载: 全尺寸图片

图2整体结构框图

Fig.2Structure diagram

下载: 全尺寸图片

图3机器人末端靠近工件示意图

Fig.3Diagram of the robot end-effector approaching the workpiece

下载: 全尺寸图片

图4轨迹偏差d的隶属函数

Fig.4Membership function of d

下载: 全尺寸图片

图5轨迹偏差变化率

\dot{d}

的隶属函数

Fig.5Membership function of

\dot{d}

下载: 全尺寸图片

图6模糊补偿量ξ₂的隶属函数

Fig.6Membership function of ξ₂

下载: 全尺寸图片

图7关节1的运动轨迹

Fig.7Motion trajectory of joint 1

下载: 全尺寸图片

图8关节2的运动轨迹

Fig.8Motion trajectory of joint 2

下载: 全尺寸图片

图9关节3的运动轨迹

Fig.9Motion trajectory of joint 3

下载: 全尺寸图片

图10关节3的角速度

Fig.10Angular velocity of joint 3

下载: 全尺寸图片

图11关节1的运动轨迹

Fig.11Motion trajectory of joint 1

下载: 全尺寸图片

图12关节2的运动轨迹

Fig.12Motion trajectory of joint 2

下载: 全尺寸图片

图13关节3的运动轨迹

Fig.13Motion trajectory of joint 3

下载: 全尺寸图片

图14关节3的轨迹跟踪误差

Fig.14Trajectory tracking error of joint 3

下载: 全尺寸图片

表1模糊控制规则表

Table1Fuzzy control rules

下载: 全尺寸图片

表2每个关节的约束

Table2Constraints for each joint

下载: 全尺寸图片

图1控制框图

Fig.1Lower-level control block diagram

图2整体结构框图

Fig.2Structure diagram

图3机器人末端靠近工件示意图

Fig.3Diagram of the robot end-effector approaching the workpiece

图4轨迹偏差d的隶属函数

Fig.4Membership function of d

图5轨迹偏差变化率

\dot{d}

的隶属函数

Fig.5Membership function of

\dot{d}

图6模糊补偿量ξ₂的隶属函数

Fig.6Membership function of ξ₂

图7关节1的运动轨迹

Fig.7Motion trajectory of joint 1

图8关节2的运动轨迹

Fig.8Motion trajectory of joint 2

图9关节3的运动轨迹

Fig.9Motion trajectory of joint 3

图10关节3的角速度

Fig.10Angular velocity of joint 3

图11关节1的运动轨迹

Fig.11Motion trajectory of joint 1

图12关节2的运动轨迹

Fig.12Motion trajectory of joint 2

图13关节3的运动轨迹

Fig.13Motion trajectory of joint 3

图14关节3的轨迹跟踪误差

Fig.14Trajectory tracking error of joint 3

表1模糊控制规则表

Table1Fuzzy control rules

表2每个关节的约束

Table2Constraints for each joint

图1控制框图

Fig.1Lower-level control block diagram

图2整体结构框图

Fig.2Structure diagram

图3机器人末端靠近工件示意图

Fig.3Diagram of the robot end-effector approaching the workpiece

图4轨迹偏差d的隶属函数

Fig.4Membership function of d

图5轨迹偏差变化率

\dot{d}

的隶属函数

Fig.5Membership function of

\dot{d}

图6模糊补偿量ξ₂的隶属函数

Fig.6Membership function of ξ₂

图7关节1的运动轨迹

Fig.7Motion trajectory of joint 1

图8关节2的运动轨迹

Fig.8Motion trajectory of joint 2

图9关节3的运动轨迹

Fig.9Motion trajectory of joint 3

图10关节3的角速度

Fig.10Angular velocity of joint 3

图11关节1的运动轨迹

Fig.11Motion trajectory of joint 1

图12关节2的运动轨迹

Fig.12Motion trajectory of joint 2

图13关节3的运动轨迹

Fig.13Motion trajectory of joint 3

图14关节3的轨迹跟踪误差

Fig.14Trajectory tracking error of joint 3

表1模糊控制规则表

Table1Fuzzy control rules

表2每个关节的约束

Table2Constraints for each joint

ZHU D, FENG X, XU X,et al. Robotic grinding of complex components: A step towards efficient and intelligent machining-challenges,solutions,and applications. Robotics and Computer-Integrated Manufacturing,2020,65:101908.

MÁRQUEZ J J, PÉREZ J M, RIOS J,et al. Process modeling for robotic polishing. Journal of Materials Processing Technology,2005,159(1):69-82.

LI W L, XIE H, ZHANG G,et al.3-D shape matching of a blade surface in robotic grinding applications. IEEE/ASME Transactions on Mechatronics,2016,21(5):2294-2306.

MUBANG X, DING Y, YANG G. A model-based trajectory planning method for robotic polishing of complex surfaces. IEEE Transactions on Automation Science and Engineering,2021,19(4):2890-2903.

GASPARETTO A, BOSCARIOL P, LANZUTTI A,et al. Trajectory planning in robotics. Mathematics in Computer Science,2012,6(3):269-279.

LONG Zhang, LI Xiantao, SHUAI Tao,et al. Review of research state of trajectory planning for industrial robots. Mechanical Science and Technology for Aerospace Engineering,2021,40(6):853-862.(龙樟, 李显涛, 帅涛, 等. 工业机器人轨迹规划研究现状综述. 机械科学与技术,2021,40(6):853-862.)

KIM J, CROFT E A. Online near time-optimal trajectory planning for industrial robots. Robotics and Computer-Integrated Manufacturing,2019,58(3):158-171.

HUANG J, HU P, WU K,et al. Optimal time-jerk trajectory planning for industrial robots. Mechanism and Machine Theory,2018,121(2):530-544.

JI C, KONG M, LI R. Time-energy optimal trajectory planning for variable stiffness actuated robot. IEEE Access,2019,7(11):14366-14377.

WU G, ZHAO W, ZHANG X. Optimum time-energy-jerk trajectory planning for serial robotic manipulators by reparameterized quintic NURBS curves. Proceedings of the Institution of Mechanical Engineers, Part C: Journal of Mechanical Engineering Science,2021,235(19):4382-4393.

ZHOU, Wei. Research on Trajectory Planning of Robotic Manipulators based on Particle Swarm Optimization. Nanjing: Nanjing University of Posts and Telecommunications,2023.(周伟. 基于粒子群算法的机械臂轨迹规划研究. 南京: 南京邮电大学,2023.)

ZHOU J, CAO H J, JIANG P,et al. Energy-saving trajectory planning for robotic high-speed milling of sculptured surfaces. IEEE Transactions on Automation Science and Engineering,2022,19(3):2278-2294.

LIN H I. A fast and unified method to find a minimum-jerk robot joint trajectory using particle swarm optimization. Journal of Intelligent & Robotic Systems,2014,75(3):379-392.

MORARI M, LEE J H. Model predictive control: Past,present and future. Computers & Chemical Engineering,1999,23(4):667-682.

MAYNE D Q, RAWLINGS J B, RAO C V,et al. Constrained model predictive control: Stability and optimality. Automatica,2000,36(6):789-814.

RAWLINGS J B, MAYNE D Q. Model Predictive Control: Theory and Design. Madison, WI, USA: Nob Hill,2009.

MAYNE D Q. Model predictive control: Recent developments and future promise. Automatica,2014,50(12):2967-2986.

XU Zhichao. Research on motion planning and control of visionbased spraying robots. Beijing: Beijing University of Civil Engineering and Architecture,2024.(徐智超. 基于视觉的喷涂机器人运动规划与控制研究. 北京: 北京建筑大学,2024.)

DUCHAINE V, BOUCHARD S, GOSSELIN C M. Computationally efficient predictive robot control. IEEE/ASME Transactions on Mechatronics,2007,12(5):570-578.

BARRETO J C L S, SCOLARI A G C, CARLOS E T D,et al. Design and implementation of model-predictive control with friction compensation on an omnidirectional mobile robot. IEEE/ASME Transactions on Mechatronics,2014,19(2):467-476.

YANG J, ZHENG W X, LI S,et al. Design of a prediction-accuracyenhanced continuous-time MPC for disturbed systems via a disturbance observer. IEEE Transactions on Industrial Electronics,2015,62(9):5807-5816.

KAYACAN E, RAMON H, SAEYS W,et al. Robusttrajectory tracking error model-based predictive control for unmanned ground vehicles, IEEE/ASME Transactions on Mechatronics,2016 21(2):806-814

ZHANG, Li. Research on intelligent motion planning and obstacle avoidance methods for industrial robotic arms. Guangzhou: South China University of Technology,2018.(张立. 工业机械臂的智能运动规划与避障方法研究. 广州: 华南理工大学,2018.)

KAYACAN E, RAMON H, SAEYS W. Robust trajectory tracking error model-based predictive control for unmanned ground vehicles. IEEE/ASME Transactions on Mechatronics,2016,21(2):806-814.

YANG Xi. Real-time dynamic trajectory replanning of robotic manipulators based on visual perception. Harbin: Harbin Institute of Technology,2022.(杨熹. 基于视觉感知的机械臂实时动态轨迹重规划. 哈尔滨: 哈尔滨工业大学,2022.)