基于双延迟深度确定性策略梯度算法的机械臂抗干扰控制

Anti-Interference Control of Manipulator Based on Twin Delayed Deep Deterministic Policy Gradient

摘要: 针对机械臂运动中和未知环境接触以及建模误差带来的干扰，提出由导纳控制和基于双延迟深度确定性策略梯度算法(Twin Delayed Deep Deterministic Policy Gradient, TD3)的PID控制结合的控制策略。通过导纳控制，将机械臂和环境的接触力矩转化为关节角度以修正期望轨迹，抵消环境带来的扰动。并针对接触力矩测量成本高，提出使用基于卡尔曼滤波的广义动量观测器进行接触力矩的估计。针对内环控制，提出基于TD3的PID位置控制器，TD3是深度确定性策略梯度算法(Deep Deterministic Policy Gradient, DDPG)的改进算法，智能体在不断交互和学习的过程中能够自动调整PID参数，避免了经典PID控制在非线性控制问题中表现较差的缺点。搭建机械臂仿真平台，仿真实验结果表明，所设计的控制策略使机械臂具有良好的抗干扰效果。

Abstract: In order to solve the interference caused by unknown environment contact and modeling error, a control strategy combining admittance control and PID control based on Twin Delayed Deep Deterministic Policy Gradient (TD3) is proposed. The admittance control strategy is used to convert the contact torque between the manipulator and the environment into the joint Angle to correct the expected trajectory and offset the disturbance caused by the environment. In view of the high cost of measuring the contact torque, a generalized momentum observer based on Kalman filter is proposed to estimate the contact torque. For inner loop control, a PID position controller based on TD3 is proposed. TD3 is an improved algorithm based on Deep Deterministic Policy Gradient (DDPG). Agents can automatically adjust PID parameters in the process of continuous interaction and learning. The disadvantages of poor performance of classical PID control in nonlinear control problems are avoided. The simulation platform of the manipulator is built, and the simulation results show that the designed control strategy makes the manipulator have good anti-interference effect.

1. 引言

控制是系统施加输入信号，使控制对象按照预定计划运行。控制系统中，不可避免地会遇到模型参数扰动、非建模动力学、系统状态耦合、未知外部扰动等影响，降低系统的控制精度，削弱鲁棒性，甚至导致系统不稳定 [1] 。这对控制系统的设计和应用提出了很大的挑战。因此，文献 [2] 指出自动控制问题的本质是干扰抑制问题。因此，在抗干扰控制中，应最大限度地消除各种不确定因素对系统控制精度的影响，使控制对象能够快速准确地进行跟踪。抗干扰控制方法一直以来受到学界广泛关注。

随着机械臂产业的不断发展，相较于传统的结构化环境和机械臂任务，例如粉刷、去除毛刺以及碰撞检测等工作，这些任务需要更加细致地思考与环境的接触情况。针对未知外部扰动，机械臂的接触力矩，一般通过力矩传感器或双编码器进行测量，但是，集成额外的传感设备以获得所需的力/扭矩信息通常在技术上困难且昂贵 [3] [4] 。无传感器估计接触力矩的方法，通常通过动力学模型进行估计。文献 [5] [6] 使用广义动量法，去除了动力学模型中的加速度项的误差干扰，直接观测广义动量，但使用广义动量的接触力矩估计会受到参数辨识误差等因素的影响。文献 [7] [8] 引入扰动观测器，通过卡尔曼滤波算法估计接触力矩可以提高估计的准确性。

目前，机械臂关节的PID (proportion integration differentiation)控制广泛应用于机械臂的实际生产中，虽然这种控制器不依赖于系统的具体动力学方程，但它们在实践中被证明是有效的 [9] 。在此基础上，学者专家使用不同方法对控制器参数进行调整，不同的干扰环境下，控制器的最优参数也不同，自适应参数可以提高控制器的鲁棒性。Sharma等人使用FOPID (Fractional order PID)控制器，利用分数阶积分器和导数算子代替整数阶，控制机械臂机械手 [10] 。Bingül和Oguzhan提出了机械臂的FOPID控制器，并进一步使用PSO (粒子群优化)和GA (遗传算法)来整定PID参数 [11] 。文献 [12] [13] 引入模糊控制，提出了一种模糊PID控制技术，使得PID控制器不需要精确的PID参数。

强化学习是机器学习的一种，它与动态规划和最优控制理论有着密切的关系。通过智能体与环境的互相作用找到最佳策略是强化学习的基础 [14] 。目前，深度强化学习已应用于机械臂 [15] 、无人机 [16] [17] 、能源 [18] [19] 、交通运输 [20] 等控制领域。基于马尔可夫决策过程的深度强化学习为实现机械臂的智能控制提供了有效途径。在此基础上，提出了一种基于深度强化学习的无模型机械臂控制方案，实现了机械臂的高精度和高鲁棒性控制。本文针对PID控制中参数调整问题，提出TD3-PID算法，将TD3算法应用于机械臂PID控制中，提高PID控制的精确性和鲁棒性。并利用PID控制器引导TD3算法的探索过程，解决了TD3算法在不使用任何先验知识的情况下探索最优策略较慢的问题。

针对机械臂与环境的接触设计的控制器需要考虑减小接触时环境对于机械臂的干扰，实现对于机械臂位置和输出力矩的高精度控制。文献 [21] 提出的导纳控制通过调整导纳参数改变机械臂末端位置和接触力之间的动态关系，现已广泛应用于机械臂与环境的接触问题 [22] [23] 。本文建立双环导纳策略，将接触力矩转换为修正角度以修正期望轨迹，抵消未知环境对于机械臂的干扰。

本文将基于扰动卡尔曼滤波的外力矩估计方法应用于机械臂的接触力矩估计。设计了基于TD3的PID控制器，可以避免机械臂的参数识别误差以及克服经典PID控制在复杂非线性控制系统表现不佳的缺点，并与经典PID控制和传统FUZZY-PID控制进行对比，结果表明，TD3-PID控制参数的调节更加精确和高效，能实现更稳定、更准确的跟踪结果。并且本文针对机械臂运行时的接触力矩干扰设计了双环导纳策略，通过接触力矩对期望轨迹进行修改，抵消未知环境带来的扰动。最后，搭建双杆机械臂仿真平台，验证所提方法的有效性。

2. 基于TD3的自适应抗干扰控制器设计

2.1. 导纳控制策略

机械臂关节的通用动力学模型：

$M\left(\theta \right)\ddot{\theta }+C\left(\theta ,\stackrel{\dot{}}{\theta }\right)\stackrel{\dot{}}{\theta }+G\left(\theta \right)=\tau -{\tau }_e$ (1)

式中， $\tau$ 是力矩； $\theta$ 是关节位置； $\stackrel{\dot{}}{\theta }$ 是关节速度； $\ddot{\theta }$ 是关节加速度； $M\left(\theta \right)$ 是惯性项； $C\left(\theta ,\stackrel{\dot{}}{\theta }\right)$ 是离心力科氏力项； $G\left(\theta \right)$ 是重力项； ${\tau }_e$ 是机械臂和环境的接触力矩。

考虑机械臂与环境的接触，导纳控制可以根据接触力矩 ${\tau }_e$ 调整期望轨迹，使得机械臂的实际轨迹贴近期望轨迹，降低接触力矩的影响。导纳控制结构如下图1所示。

Figure 1. Admittance strategy structure chart

图1. 导纳策略结构图

导纳控制通常描述力与角度之间的关系，接触力矩与机械臂的导纳模型为

$M{\ddot{\theta }}_m+C{\stackrel{\dot{}}{\theta }}_m+K{\theta }_m={\tau }_e$ (2)

式中，M为机械臂的惯性系数、C为机械臂的阻尼系数、K为机械臂的刚度系数； ${\theta }_m$ ， ${\stackrel{\dot{}}{\theta }}_m$ ， ${\ddot{\theta }}_m$ 分别为因接触力矩产生的干扰位置、干扰速度、干扰加速度。

导纳控制修正后的修正位置 ${\theta }_k$ 如下

${\theta }_k={\theta }_d-{\theta }_m$ (3)

式中， ${\theta }_d$ 为期望位置， ${\theta }_k$ 为修正位置。

建立导纳控制策略，外环用于消除接触力的影响，修改期望轨迹；内环为基于深度强化学习的PID控制位置环，对修改后的轨迹进行跟踪控制。

2.2. 基于扰动卡尔曼滤波的机械臂接触力矩估计方法

2.2.1. 基于广义动量的机械臂动力学模型

现有的接触力估计方法通常依赖于关节加速度 $\ddot{\theta }$ 和惯性矩阵M的反演。由于通常只能测量关节角度和速度，因此需要对数值进行微分来获得关节加速度，这个过程会大大放大测量噪声。通过广义动量法可以解决这个问题。

将机械臂的接触力矩作为系统外的扰动，通过扰动观测器原理，接触力矩可以为

${\stackrel{\dot{}}{\tau }}_e=A_e{\tau }_e+{\omega }_e$ (4)

式中： ${\tau }_e$ 为接触力矩向量， ${\tau }_e\in {ℝ}^n$ ； $A_e$ 为表示外力矩变化趋势的对角矩阵， $A_e\in {ℝ}^{n\times n}$ ；接触力矩模型误差 ${\omega }_e$ 为高斯噪声， ${\omega }_e\text{~}\mathcal{N}\left(0,Q_{c,e}\right)$ ， $Q_{c,e}$ 为其协方差对角矩阵， $Q_{c,e}\in {ℝ}^{n\times n}$ 。

引入机械臂的广义动量为

$\{\begin{array}{l}p=M\left(\theta \right)\stackrel{\dot{}}{\theta }\hfill \\ \stackrel{\dot{}}{p}=\stackrel{\dot{}}{M}\left(\theta \right)\stackrel{\dot{}}{\theta }+M\left(\theta \right)\ddot{\theta }\hfill \end{array}$ (5)

因式 $\stackrel{\dot{}}{M}\left(\theta \right)-2C\left(\theta ,\stackrel{\dot{}}{\theta }\right)$ 的反对称性，进而得到，

$\stackrel{\dot{}}{M}\left(\theta \right)=C\left(\theta ,\stackrel{\dot{}}{\theta }\right)+C{\left(\theta ,\stackrel{\dot{}}{\theta }\right)}^{\text{T}}$ (6)

结合式(4)~(6)，式(1)可表示为

$\stackrel{\dot{}}{p}={\tau }_u+{\tau }_e+{\omega }_p$ (7)

${\tau }_u=\tau +C{\left(\theta ,\stackrel{\dot{}}{\theta }\right)}^{\text{T}}\stackrel{\dot{}}{\theta }-G\left(\theta \right)$ (8)

式中： ${\omega }_p$ 为参数辨识误差， ${\omega }_p\text{~}\mathcal{N}\left(0,Q_{c,p}\right)$ ； $Q_{c,p}$ 为其协方差对角矩阵， $Q_{c,p}\in {ℝ}^{n\times n}$ 。

${\tau }_u=\tau +C{\left(\theta ,\stackrel{\dot{}}{\theta }\right)}^{\text{T}}\stackrel{\dot{}}{\theta }-G\left(\theta \right)$ (9)

通过式(4)~(5)，将接触力矩和动量设为状态变量，并将动量设为状态空间的输出，式(1)的状态空间表达式为：

$\{\begin{array}{l}\stackrel{\dot{}}{x}=Ax+Bu+\omega \hfill \\ y=Cx+v\hfill \end{array}$ (10)

式中：状态向量 $x={\left[p^{\mathrm{T}},{\tau }_e^{\mathrm{T}}\right]}^{\text{T}}$ ；输入 $u={\tau }_u$ ；系统状态转移矩阵A、输入矩阵B与输出矩阵C分别为

$A=\left[\begin{array}{cc}{0}_{n\times n}& I_n\\ 0_{n\times n}& A_e\end{array}\right],B=\left[\begin{array}{c}{I}_n\\ 0_{n\times n}\end{array}\right],C=\left[\begin{array}{cc}{I}_n& 0_{n\times n}\end{array}\right]$

式中， $I_n$ 为单位矩阵， $I_n\in {ℝ}^{n\times n}$ 。系统过程噪声 $\omega$ ， $\omega ={\left[{\omega }_p^{\mathrm{T}}{\omega }_e^{\mathrm{T}}\right]}^{\text{T}}$ ，即

$\omega \text{~}\mathcal{N}\left(0,Q_c\right)=\mathcal{N}\left(0,\left[\begin{array}{cc}{Q}_{c,p}& 0_{n\times n}\\ 0_{n\times n}& Q_{c,e}\end{array}\right]\right)$

$\theta$ 和 $\stackrel{\dot{}}{\theta }$ 是可测的，所以动量可以作为一个测量值，测量噪声 $v\text{~}\mathcal{N}\left(0,R_c\right)$ 。

关节角度 $\theta$ 可以物理测量，关节速度 $\stackrel{\dot{}}{\theta }$ 由关节角度进行数值微分计算得到，可以假设关节角度的测量噪声和关节速度的噪声相比可以忽略不计。协方差矩阵 $R_c$ 可以为

$R_c=M\left(\theta \right)Q_{\stackrel{\dot{}}{\theta }}M{\left(\theta \right)}^{\text{T}}$ (11)

式中， $Q_{\stackrel{\dot{}}{\theta }}$ 是一个对角矩阵，包含了各个关节中速度的估计方差， $Q_{\stackrel{\dot{}}{\theta }}\in {ℝ}^{n\times n}$ 。

2.2.2. 基于广义动量的机械臂动力学模型

通过卡尔曼滤波算法对估计器进行优化，需要先将系统离散化处理，对式(10)进行离散化得到

$\{\begin{array}{l}{x}_k=A_k{x}_{k-1}+B_k{u}_k+{\omega }_k\hfill \\ y_k=C_k{x}_k+v_k\hfill \end{array}$ (12)

离散系统矩阵为：

$\left[\begin{array}{cc}{A}_k& B_k\\ 0_{n\times \left(n+n\right)}& I_n\end{array}\right]=\exp \left(\left[\begin{array}{cc}A& B\\ 0_{n\times \left(n+n\right)}& 0_{n\times n}\end{array}\right]T_s\right)$ (13)

$C_k=C,R_k=\frac{1}{T_s}{R}_c$ (14)

$L=\left[\begin{array}{cc}A& Q_c\\ 0_{\left(n+n\right)\times \left(n+n\right)}& -A^{\text{T}}\end{array}\right]$ (15)

$\left[\begin{array}{cc}{M}_{11}^k& M_{12}^k\\ 0_{\left(n+n\right)\times \left(n+n\right)}& M_{22}^k\end{array}\right]=\exp \left(L\cdot T_s\right)$ (16)

$Q_k=M_{12}^k\left(M_{11}^k\right)$ (17)

式中， $T_s$ 为采样周期， $C_k$ 为输出矩阵， $v_k$ 为测量噪声， $R_k$ 为其协方差矩阵， ${\omega }_k$ 为过程噪声， $Q_k$ 为其协方差矩阵。综上所述，基于扰动卡尔曼滤波的接触力矩估计器可以归纳为以下步骤。

续表

2.4. 基于TD3的参数自适应PID控制器

本文所提出的控制器，采用导纳控制和基于TD3的PID控制器，实时PID参数，以应对非线性变化和不确定性因素对系统性能的影响。TD3算法是一种深度强化学习方法，根据DDPG算法发展而来。DDPG是通过神经网络技术进行逼近的方法，它可以处理具有连续动作空间的问题，并利用经验回放和双网络解决了演员–评论家难以收敛的问题。DDPG虽然可以解决很多问题并取得不错效果，但其计算效果经常取决于众多的超参数 [24] [25] 。

本文所提出的TD3-PID控制原理如下：设计奖励函数，智能体给机械臂PID控制器PID参数，进而计算出电机力矩，与环境交互获得新的状态值，更新优化神经网络，由智能体再次输出PID参数。TD3算法训练框图如图2所示。

Figure 2. TD3-PID algorithm training block diagram

图2. TD3-PID算法训练框图

在TD3算法中，奖励函数评估代理行动，指导其在环境中决策以最大化长期累计回报。为机械臂轨迹跟踪任务设计奖励函数，考虑轨迹误差、关节角度和上一步回报值。奖励函数给予正负奖励以指导代理优化学习，轨迹误差增大时罚奖励为负，超出合理关节角度奖励也为负。引入上一步回报值以增强周期内步与上一步关联性，更好地反映当前状态和动作对未来长期累计回报的影响。本文的奖励函数为：

$r=-0.1e_1^2-0.1e_2^2-0.01K_{t-1}-0.01X_{t-1}+b$

$e_1$ 、 ${\stackrel{\dot{}}{e}}_1$ 、 $e_2$ 、 ${\stackrel{\dot{}}{e}}_2$ 分别为关节轨迹误差速打赏度，加速度， $K_{t-1},X_{t-1}$ 分别为上一时间步机械臂关节的回报值， $b$ 为偏置。

3. 仿真结果及分析

3.1. 外力矩估计仿真

搭建双杆机械臂仿真平台，对所提方法进行仿真验证。为了优化接触力矩观测器估计性能，通过大量仿真，将协方差矩阵设定为对角线上的常数值，以提高扰动卡尔曼滤波器的效能，协方差矩阵参数如表1所示。这种调整方法参考文献 [26] 中提供的具体指导。

Table 1. Parameters of covariance matrix in Kalman filtering

表1. 卡尔曼滤波中协方差矩阵参数

为了验证所提方法的适用性，采用该方法对机械臂和环境的接触力矩进行仿真，其中模型包含刚体动力学、电机惯性、关节摩擦和测量噪声。在仿真中，机械臂以位置控制方式操作。在机械臂运动过程中，模拟了在双杆机械臂两个关节上分别施加100 Nm的阶跃信号接触力矩，如图3所示；在双杆机械臂两个关节上分别施加振幅为100 Nm的正弦信号接触力矩，如图4所示。接触力矩是黑色曲线，标准广义动量观测器估计出的接触力矩是蓝色曲线，基于卡尔曼滤波的广义动量观测器估计的接触力矩是红色曲线，基于卡尔曼滤波的广义动量观测器实现了更高的估计精度。

(a) 关节1(b) 关节2

Figure 3. Overstep signal joint contact torque estimation diagram

图3. 越阶信号关节接触力矩估计图

(a) 关节1(b) 关节2

Figure 4. Sine function joint contact torque estimation diagram

图4. 正弦函数关节接触力矩估计图

3.2. TD3-PID控制器仿真

在机械臂环境中搭建TD3-PID位置控制器，实时调整比例、积分和微分参数，并且搭建经典PID位置控制器和FUZZY-PID位置控制器作为对照组。本文的TD3算法参数设置如下表2所示。

Table 2. RL agent module parameter settings

表2. RL agent模块参数设置

Figure 5. Return values obtained by TD3 algorithm intelligences per cycle

图5. TD3算法训练过程每个周期获得的回报值

TD3算法训练过程中的训练周期数和每个周期获得的回报值如图5所示，从该图可以看到，在TD3智能体的训练过程中，回报值最终稳定并逼近理论最高回报值600，说明了本文所搭建的TD3-PID控制器的稳定性。图6是回报值最高的一个周期内控制器PID参数的变化效果，由于机械臂动力学模型的非线性，智能体通过调整出时变的参数以提高控制器轨迹跟踪性能。

(a) 关节1(b) 关节2

Figure 6. Parameter variation diagram of the TD3-PID controller

图6. TD3-PID控制器的参数变化图

为了验证所提出算法TD3-PID的精度，将其与调整好参数的经典PID算法，以及传统FUZZY-PID算法进行对比。图7和图8显示了使用TD3-PID控制算法，经典PID算法和FUZZY-PID算法的位置环，机械臂关节的轨迹跟踪和速度跟踪情况。图中红色线条是TD3-PID算法轨迹跟踪曲线；蓝色线条是PID算法轨迹跟踪曲线；深蓝色线条是FUZZY-PID算法轨迹跟踪曲线；黑色线条是期望轨迹。

根据仿真结果显示，利用PID算法、FUZZY-PID算法和TD3-PID算法进行机械臂关节的轨迹跟踪时，位置稳态误差都能够在短时间内收敛并趋近于零。但是，使用TD3-PID算法相比于经典PID算法和FUZZY-PID算法，位置跟踪初期跟踪效果更好，跟踪曲线更为平滑；速度跟踪稳态误差较小，可以避免在初始阶段PID算法和FUZZY-PID算法都会出现的剧烈抖动现象。

(a) 关节1(b) 关节2

Figure 7. Mechanical arm joint trajectory tracking effect

图7. 机械臂关节轨迹跟踪效果图

(a) 关节1(b) 关节2

Figure 8. Mechanical arm speed tracking position effect

图8. 机械臂速度跟踪位置效果图

图9表示在PID算法、FUZZY-PID算法和TD3-PID算法作用下的机械臂的电机力矩输入情况。从中可以看出，TD3-PID算法的电机力矩相比经典PID算法和FUZZY-PID算法有更小的抖动幅度和抖动频率。较小的力矩抖动使得电机的控制更加精确，有助于延长电机使用寿命，提升整个系统的能量效率。

综上所述，通过将TD3算法引入PID控制，进行自适应调节，可显著提升机械臂轨迹跟踪性能。

在机械臂关节1和关节2分别施加图4所示的正弦接触力矩。根据所提出控制器，内环位置环使用TD3-PID控制器，外环由基于卡尔曼滤波的广义动量观测器得到接触力矩的估计值，使用导纳算法进行期望轨迹的修正，仿真得到的导纳策略机械臂关节位置曲线如图10的红色线条所示。蓝色线条为PID导纳控制器，内环位置环为经典PID控制；深蓝色线条为FUZZY-PID导纳控制器，内环位置环为FUZZY-PID控制。从图中可以看出对于有较大接触力矩的关节1，PID导纳控制器轨迹跟踪的效果较差，FUZZY-PID导纳控制器轨迹跟踪效果略有改善，TD3-PID导纳控制器的轨迹追踪效果有较大幅度提升。

(a) 关节1(b) 关节2

Figure 9. Torque diagram of joint motor

图9. 关节电机力矩图

(a) 关节1(b) 关节2

Figure 10. Joint trajectory tracking diagram of admittance strategy robot arm

图10. 导纳策略机械臂关节轨迹跟踪图

4. 结论

本文综合运用导纳控制和基于TD3的PID控制策略，针对机械臂运动中的未知环境和建模误差干扰问题提出了有效解决方案。通过导纳控制抵消干扰并利用基于卡尔曼滤波的广义动量观测器估计接触力矩，结合TD3-PID控制器在位置环进行自适应调整参数，仿真结果表明所提出的控制策略显著优于传统方法经典PID控制和FUZZY-PID控制，能有效应对系统不确定性和非线性挑战以及未知环境干扰，能够提高控制精度并展示更高的稳定性。未来可进一步优化算法、验证实验并将其应用于实际机械臂系统中。