CN114952849A - 一种基于强化学习与动力学前馈融合的机器人轨迹跟踪控制器设计方法 - Google Patents

Abstract

本发明属于机器人控制技术领域，公开了一种基于强化学习与动力学前馈融合的机器人轨迹跟踪控制器设计方法.该方法可在被控系统中存在动力学不确定性、未建模非线性因素、参数摄动和未知外部干扰等条件下，能够自主准确跟踪参考轨迹，并自适应减小轨迹误差以实现提高轨迹跟踪精度。本发明运用了动力学前馈控制和人工蜂群算法对动力学参数进行辨识补偿作为控制方法的基础，通过二者的融合，加之针对未建模的非线性因素、外部干扰等偏差的强化学习TD3补偿算法，实现了控制方法和控制器的设计。该控制方法能对不同结构参数机械臂的动力学不确定性所产生的轨迹误差进行自适应反馈调整和补偿，有效提高轨迹跟踪精度和性能。

Description

一种基于强化学习与动力学前馈融合的机器人轨迹跟踪控制 器设计方法

技术领域

本发明属于机器人控制技术领域，涉及一种提高机械臂轨迹跟踪精度的控制方法，尤其涉及一种基于强化学习与动力学前馈融合的机器人轨迹跟踪控制器设计方法。

背景技术

机械臂广泛部署于智能制造的各个环节，特别是在协作搬运、激光焊接，抛光打磨以及高精度的加工等领域。由于机械臂刚性、电机力矩有限、电机响应速度有限，使得加装负载运动后产生偏差，并且安装后的真机动力学参数与名义值有偏差，导致输出力矩值不足以执行相应的轨迹。这些因素使得机械臂无法满足高精度任务的需求。传统工厂中的采用部分轨迹降低机械臂运行速度，如包裹的搬运与摆放。任务过程中设置多个停止点位来进行误差修正，如智能工厂中工件的夹取与安装，使得机械臂部分运行轨迹能满足任务需求。但低速运行以及运行中设置暂停点位使得机械臂的工作效率大打折扣，不能完全发挥机械臂的优势。因此为保证机械臂运行轨迹逼近于规划轨迹，需要对机器人轨迹误差补偿算法进行研究。

目前常用的轨迹跟踪误差补偿算法，存在以下的不足：采用PID控制线性组合误差收敛较慢、抗干扰能力差，即使参数经过精心调节，仍难以精准整定且轨迹精度易受到外部扰动影响。采用动力学前馈控制无法应对非模型偏差。采用强化学习进行力矩环对机械臂控制，学习周期较长，训练过程不可控，存在损坏机械臂的情况，无法应对未知的复杂环境，结果不镇定。

发明内容

本发明的目的在于针对机械臂在运动过程中由于模型参数偏差、非线性扰动等造成轨迹跟踪精度不足的情况，提供一种基于强化学习与动力学前馈融合的机器人轨迹跟踪控制器设计方法，该方法可得到一种强化学习TD3与动力学前馈结合的最优控制器来确保轨迹跟踪和误差收敛的精度，同时也提高同步控制性能。

为实现上述目的，本发明提供了如下的技术方案：

根据本发明的一种具体实例，一种基于强化学习与动力学前馈融合的机器人轨迹跟踪控制器设计方法，所述控制方法包含以下几个步骤：

A.动力学参数辨识模型构建

构建机械臂动力学参数辨识模型，通过设计的激励轨迹，采集机械臂运动信息与已知的动力学模型方程相结合，计算出对应的偏差；

B.基于人工蜂群算法动力学参数辨识

针对机械臂动力学模型参数的辨识，求解过程通过人工蜂群算法循环迭代更新，找到满足的“蜜源”，即找到满足精度的解，最小化偏差e，从而得到机械臂的真实动力学参数；

C.设计动力学前馈控制器

通过建立动态的惯量模型，给出系统内部控制所需的控制量，减小目标关节角度与实际关节角度的偏差，减小位置误差；

D.对未建模的误差采用强化学习TD3进一步补偿。

进一步的，步骤A具体为：

采用牛顿-欧拉迭代构建机械臂动力学参数辨识模型：

公式(1)中，q是机械臂关节角度，

为关节角速度，

为关节角加速度，τ是各个关节轴驱动力矩，D(q)是对称正定的n×n质量矩阵，

是科里奥利和离心力向量，维度为n×1，G(q)是包含重力矩的向量；F_cv为摩擦力项，具体展开为：

公式(2)中F_cv为摩擦力，f_c是库伦摩擦力矩，f_v是粘性摩擦系数，sign为符号函数；通过公式(1)对串联机械臂进行递推，得到机械臂动力学模型；然后，通过设计的激励轨迹，采集到机械臂的各个关节力矩、关节角度、角速度、角加速度信息和已知的动力学模型方程相结合，计算出对应的偏差:

公式(3)中e代表理论值和真实值的偏差；τ_t为通过公式(1)计算得出的关节力矩，F_t为传感器采集到的各个关节力矩信息，m代表关节轴数，n代表采样数量；w_i为第i关节轴力矩残差的权重。

进一步的，步骤B具体为：

针对机械臂动力学模型参数的辨识，求解过程可以通过人工蜂群算法循环迭代更新，找到满足的“蜜源”，即找到满足精度的解；要求解的m个动力学参数，如下：

X＝[x₁,x₂,…,x_m] (4)

其中，x代表动力学模型中待辨识的参数，第n次迭代求解得到的第i个蜜源表达为：

对第i个蜜源在搜索约束的范围内，进行随机初始化：

X_i＝X_min+rand(0,1)(X_max-X_min) (6)

公式(6)中rand(0,1)表示生成范围为[0 1]的随机数，X_max为辨识参数上限， X_min为辨识参数下限，X_i为第i次搜索初始化值；然后，蜜蜂更新蜜源的位置信息：

v_ij＝x_ij+δ(x_ij-x_kj) (7)

式中，v代表更新的位置，j∈{1,2,3...,m-1,m}代表更新的是第几个参数，i和 k代表第几次更新，但i≠k，δ∈[-1,1]，δ(x_ij-x_kj)表示扰动大小；当新的蜜源V_i带入公式(3)的求解小于X_i时，其中V_i＝[v_i1,v_i2,…,v_im]，采用贪心的方法用V_i替代X_i；

所有的引领蜂根据公式(7)更新后，跟随蜂按照引领蜂提供的蜜源信息，进行更新，经过迭代，如果蜜源X_i达到阈值但没找到更好的蜜源，则X_i将会被放弃，并将引领蜂转换为侦查蜂。最终，判断求得的解是否满足终止条件，或者是否达到了迭代的上限。

通过人工蜂群算法对参数进行辨识，最小化偏差e，从而得到机械臂的真实动力学参数。

进一步的，步骤C具体为：

采集关节角度、角速度和加速度，通过公式(1)和B中计算出的真机模型参数，计算得到动力学力矩τ_d，然后采用双环PID控制，其中内环的控制律为：

其中

为目标关节角速度，

为机械臂真实关节角速度，

为两者偏差值，

为比例控制，T_I为积分控制参数，

为微分控制参数；

外环的控制律为：

e(t)＝q_d-q (11)

其中q_d为目标关节角度，q为机械臂真实关节角度，e为两者偏差值，K_P为比例控制，T_I为积分控制参数，T_D为微分控制参数；

前馈控制的控制律为：

τ＝τ_PID+τ_d (12)

其中τ为动力学前馈输出力矩，τ_PID为双环PID力矩，τ_d为动力学参数辨识后计算出的动力学力矩。

进一步的，步骤D具体为：

构建机械臂输入输出模型：

x_t+1＝f(x_t,τ_t) (13)

式中f函数代表机械臂非线性动力系统，t时刻输入为关节力矩τ_t，

x_t为t时刻机械臂状态，x_t+1为t+1时刻机械臂状态，智能体输出为关节力矩值。

TD3智能体输出值，通过力矩的形式补偿到各个关节：

式中t为第t时刻，τ_t为t时刻的力矩，h(Δθ,t)为原始的机械臂控制算法，Δθ为目标角度和真实角度差值，θ_r为真实角度值，

为真实角速度。原始的机械臂控制算法中各轴相互立，各轴误差不对其他的轴输出力矩产生影响。a为智能体补偿的力矩值，

TD3的观测状态为：

式中θ_r是t时刻关节角的真实值，

为关节的真实角速度，θ_n为t时刻关节的目标值θ_r,

观测值包含当前时刻各个目标关节的角度。针对非线性轨迹误差系统f(x_t,τ_t)，TD3可以不基于数学模型学习补偿参数值，以达到最优控制的目标。

结合公式(12)和公式(14)得，机械臂控制系统力矩输出为：

τ＝τ_PID+τ_d+a (16)

本发明的有益效果是：

本发明的方法运用动力学前馈控制和人工蜂群算法对动力学参数进行辨识补偿，通过二者的融合，加之针对未建模的非线性因素、外部干扰等偏差的强化学习TD3补偿算法，实现了控制方法和控制器的设计。运用人工蜂群算法与动力学前馈相融合，人工蜂群算法具有离线辨识动力学参数能力，能够不断迭代获得趋近于真机的动力学模型参数。动力学前馈采用辨识后的动力学参数能够计算出各轴所需力矩大小。采用强化学习TD3算法能够在很短时间内补偿不确定性、参数摄动和干扰，将被控系统轨迹跟踪误差收敛到期望值(域)附近，有效提高了轨迹跟踪精度和性能。该控制方法能对不同结构参数机械臂的动力学不确定性所产生的轨迹误差进行自适应反馈调整和补偿，同时也提高了同步控制性能。

附图说明

附图1是本发明的控制方法的应用示意图

附图2是本发明的控制效果示意图。

附图3是本发明的人工蜂群算法动力学参数辨识流程示意图。

具体实施方式

下面结合附图和具体实施步骤对本发明的技术方案做进一步详细的说明。

一种基于强化学习与动力学前馈融合的机器人轨迹跟踪控制器设计方法，所述控制方法包含以下几个步骤：

A.动力学参数辨识模型构建。采用牛顿-欧拉迭代构建机械臂动力学参数辨识模型：

公式(1)中，q是机械臂关节角度，

为关节角速度，

为关节角加速度，τ是各个关节轴驱动力矩，D(q)是对称正定的n×n质量矩阵，

是科里奥利和离心力向量，维度为n×1，G(q)是包含重力矩的向量；F_cv为摩擦力项，具体展开为：

公式(2)中F_cv为摩擦力，f_c是库伦摩擦力矩，f_v是粘性摩擦系数，sign为符号函数；通过公式(1)对串联机械臂进行递推，得到机械臂动力学模型；然后，通过设计的激励轨迹，采集到机械臂的各个关节力矩、关节角度、角速度、角加速度信息和已知的动力学模型方程相结合，计算出对应的偏差:

公式(3)中e代表理论值和真实值的偏差；τ_t为通过公式(1)计算得出的关节力矩，F_t为传感器采集到的各个关节力矩信息，m代表关节轴数，n代表采样数量；w_i为第i关节轴力矩残差的权重。

B.基于人工蜂群算法动力学参数辨识。针对机械臂动力学模型参数的辨识，求解过程可以通过人工蜂群算法循环迭代更新，找到满足的“蜜源”，即找到满足精度的解；要求解的m个动力学参数，如下：

X＝[x₁,x₂,…,x_m] (20)

其中，x代表动力学模型中待辨识的参数，第n次迭代求解得到的第i个蜜源表达为：

对第i个蜜源在搜索约束的范围内，进行随机初始化：

X_i＝X_min+rand(0,1)(X_max-X_min) (22)

公式(6)中rand(0,1)表示生成范围为[0 1]的随机数，X_max为辨识参数上限， X_min为辨识参数下限，X_i为第i次搜索初始化值；然后，蜜蜂更新蜜源的位置信息：

v_ij＝x_ij+δ(x_ij-x_kj) (23)

式中，v代表更新的位置，j∈{1,2,3...,m-1,m}代表更新的是第几个参数，i和 k代表第几次更新，但i≠k，δ∈[-1,1]，δ(x_ij-x_kj)表示扰动大小；当新的蜜源V_i带入公式(3)的求解小于X_i时，其中V_i＝[v_i1,v_i2,…,v_im]，采用贪心的方法用V_i替代X_i；

所有的引领蜂根据公式(7)更新后，跟随蜂按照引领蜂提供的蜜源信息，进行更新，经过迭代，如果蜜源X_i达到阈值但没找到更好的蜜源，则X_i将会被放弃，并将引领蜂转换为侦查蜂。最终，判断求得的解是否满足终止条件，或者是否达到了迭代的上限。人工蜂群算法动力学参数辨识流程如图3所示。

通过人工蜂群算法对参数进行辨识，最小化偏差e，从而得到机械臂的真实动力学参数。

C.设计动力学前馈控制器。动力学前馈控制可以加快关节误差的收敛速度，改进机械臂动态响应特性，进而提高轨迹跟踪精度，实现高速高精度的运动控制。前馈控制的原理是通过建立动态的惯量模型，给出系统内部控制所需的控制量，进而减小目标关节角度与实际关节角度的偏差，减小位置误差，提高轨迹跟踪的精度。

针对动力学前馈控制，采集关节角度、角速度和加速度，通过公式(1)和B 中计算出的真机模型参数，计算得到动力学力矩τ_d，然后采用双环PID控制，其中内环的控制律为：

其中

为目标关节角速度，

为机械臂真实关节角速度，

为两者偏差值，

为比例控制，T_I为积分控制参数，

为微分控制参数；

外环的控制律为：

e(t)＝q_d-q (27)

其中q_d为目标关节角度，q为机械臂真实关节角度，e为两者偏差值，K_P为比例控制，T_I为积分控制参数，T_D为微分控制参数；

前馈控制的控制律为：

τ＝τ_PID+τ_d (28)

其中τ为动力学前馈输出力矩，τ_PID为双环PID力矩，τ_d为动力学参数辨识后计算出的动力学力矩。

D.对未建模的误差采用强化学习TD3进一步补偿。

构建机械臂输入输出模型：

x_t+1＝f(x_t,τ_t) (29)

式中f函数代表机械臂非线性动力系统，t时刻输入为关节力矩τ_t，

x_t为t时刻机械臂状态，x_t+1为t+1时刻机械臂状态，智能体输出为关节力矩值。

TD3智能体输出值，通过力矩的形式补偿到各个关节：

式中t为第t时刻，τ_t为t时刻的力矩，h(Δθ,t)为原始的机械臂控制算法，Δθ为目标角度和真实角度差值，θ_r为真实角度值，

为真实角速度。原始的机械臂控制算法中各轴相互立，各轴误差不对其他的轴输出力矩产生影响。a为智能体补偿的力矩值，

TD3的观测状态为：

式中θ_r是t时刻关节角的真实值，

为关节的真实角速度，θ_n为t时刻关节的目标值θ_r,

观测值包含当前时刻各个目标关节的角度。针对非线性轨迹误差系统f(x_t,τ_t)，TD3可以不基于数学模型学习补偿参数值，以达到最优控制的目标。

结合公式(12)和公式(14)得，机械臂控制系统力矩输出为：

τ＝τ_PID+τ_d+a (32)

以六自由度机械臂的轨迹跟踪控制器设计为例，参照图1，选取人工蜂群算法补偿后的动力学前馈控制与双环PID作为主控制系统、采用强化学习TD3 算法对非模型偏差进行补偿，作为辅助控制系统。

基于牛顿-欧拉动力学系统，采用人工蜂群算法进行动力学参数辨识：

其中，针对牛顿-欧拉法建立的动力学模型，要对模型参数进行辨识；在本案例中，六自由度机械臂，辨识项为：

各关节质量：

[m₁ m₂ m₃ m₄ m₅ m₆] (33)

各个关节的质心位置：

惯性张量：

其中第i轴的质心惯性张量Ic_i，表示为：

各个关节的摩擦系数。其中包含粘滞摩擦和库伦摩擦：

f_v＝[f_v1 f_v2 f_v3 f_v4 f_v5 f_v6] (37)

f_C＝[f_C1 f_C2 f_C3 f_C4 f_C5 f_C6] (38)

通过辨识出以上参数，采用公式计算出前馈输出力矩。

双环PID系统与TD3控制系统：

每个轴分别部署双环PID系统，分别进行参数的调整和控制。使得各轴角度跟踪误差能够收敛。然后对机械臂添加TD3强化学习系统进行训练，TD3算法对力矩环进行控制，通过力矩补偿加速误差收敛，提高轨迹跟踪精度。其中智能体观测值为各轴真实角度、角速度、目标角度、目标角速度。评价函数为各轴角度偏差(负值)乘以权重值。目标为使得评价函数计算值趋近于零。

图2证明了该控制算法能有效对目标轨迹进行跟踪。这说明了本方法能够有效解决机械臂轨迹跟踪精度不足的情况，控制稳定性、鲁棒性、快速性和实时性都比较突出。

Claims (5)

1.一种基于强化学习与动力学前馈融合的机器人轨迹跟踪控制器设计方法，其特征在于，主要包含以下几个步骤：

A.动力学参数辨识模型构建

构建机械臂动力学参数辨识模型，通过设计的激励轨迹，采集机械臂运动信息与已知的动力学模型方程相结合，计算出对应的偏差；

B.基于人工蜂群算法动力学参数辨识

针对机械臂动力学模型参数的辨识，求解过程通过人工蜂群算法循环迭代更新，找到满足的“蜜源”，即找到满足精度的解，最小化偏差e，从而得到机械臂的真实动力学参数；

C.设计动力学前馈控制器

通过建立动态的惯量模型，给出系统内部控制所需的控制量，减小目标关节角度与实际关节角度的偏差，减小位置误差；

D.对未建模的误差采用强化学习TD3进一步补偿。

2.如权利要求1所述的一种基于强化学习与动力学前馈融合的机器人轨迹跟踪控制器设计方法，其特征在于，步骤A具体为：

采用牛顿-欧拉迭代构建机械臂动力学参数辨识模型：

公式(1)中，q是机械臂关节角度，

为关节角速度，

为关节角加速度，τ是各个关节轴驱动力矩，D(q)是对称正定的n×n质量矩阵，

是科里奥利和离心力向量，维度为n×1，G(q)是包含重力矩的向量；F_cv为摩擦力项，具体展开为：

公式(2)中F_cv为摩擦力，f_c是库伦摩擦力矩，f_v是粘性摩擦系数，sign为符号函数；通过公式(1)对串联机械臂进行递推，得到机械臂动力学模型；然后，通过设计的激励轨迹，采集到机械臂的各个关节力矩、关节角度、角速度、角加速度信息和已知的动力学模型方程相结合，计算出对应的偏差:

公式(3)中e代表理论值和真实值的偏差；τ_t为通过公式(1)计算得出的关节力矩，F_t为传感器采集到的各个关节力矩信息，m代表关节轴数，n代表采样数量；w_i为第i关节轴力矩残差的权重。

3.如权利要求2所述的一种基于强化学习与动力学前馈融合的机器人轨迹跟踪控制器设计方法，其特征在于，步骤B具体为：

针对机械臂动力学模型参数的辨识，求解过程可以通过人工蜂群算法循环迭代更新，找到满足的“蜜源”，即找到满足精度的解；要求解的m个动力学参数，如下：

X＝[x₁,x₂,…,x_m] (4)

其中，x代表动力学模型中待辨识的参数，第n次迭代求解得到的第i个蜜源表达为：

对第i个蜜源在搜索约束的范围内，进行随机初始化：

X_i＝X_min+rand(0,1)(X_max-X_min) (6)

公式(6)中rand(0,1)表示生成范围为[01]的随机数，X_max为辨识参数上限，X_min为辨识参数下限，X_i为第i次搜索初始化值；然后，蜜蜂更新蜜源的位置信息：

v_ij＝x_ij+δ(x_ij-x_kj) (7)

式中，v代表更新的位置，j∈{1,2,3...,m-1,m}代表更新的是第几个参数，i和k代表第几次更新，但i≠k，δ∈[-1,1]，δ(x_ij-x_kj)表示扰动大小；当新的蜜源V_i带入公式(3)的求解小于X_i时，其中V_i＝[v_i1,v_i2,…,v_im]，采用贪心的方法用V_i替代X_i；

所有的引领蜂根据公式(7)更新后，跟随蜂按照引领蜂提供的蜜源信息，进行更新，经过迭代，如果蜜源X_i达到阈值但没找到更好的蜜源，则X_i将会被放弃，并将引领蜂转换为侦查蜂。最终，判断求得的解是否满足终止条件，或者是否达到了迭代的上限。

通过人工蜂群算法对参数进行辨识，最小化偏差e，从而得到机械臂的真实动力学参数。

4.如权利要求3所述的一种基于强化学习与动力学前馈融合的机器人轨迹跟踪控制器设计方法，其特征在于，步骤C具体为：

采集关节角度、角速度和加速度，通过公式(1)和B中计算出的真机模型参数，计算得到动力学力矩τ_d，然后采用双环PID控制，其中内环的控制律为：

其中

为目标关节角速度，

为机械臂真实关节角速度，

为两者偏差值，

为比例控制，T_I为积分控制参数，

为微分控制参数；

外环的控制律为：

e(t)＝q_d-q (11)

其中q_d为目标关节角度，q为机械臂真实关节角度，e为两者偏差值，K_P为比例控制，T_I为积分控制参数，T_D为微分控制参数；

前馈控制的控制律为：

τ＝τ_PID+τ_d (12)

其中τ为动力学前馈输出力矩，τ_PID为双环PID力矩，τ_d为动力学参数辨识后计算出的动力学力矩。

5.如权利要求4所述的一种基于强化学习与动力学前馈融合的机器人轨迹跟踪控制器设计方法，其特征在于，步骤D具体为：

构建机械臂输入输出模型：

x_t+1＝f(x_t,τ_t) (13)

式中f函数代表机械臂非线性动力系统，t时刻输入为关节力矩τ_t，

x_t为t时刻机械臂状态，x_t+1为t+1时刻机械臂状态，智能体输出为关节力矩值。

TD3智能体输出值，通过力矩的形式补偿到各个关节：

式中t为第t时刻，τ_t为t时刻的力矩，h(Δθ,t)为原始的机械臂控制算法，Δθ为目标角度和真实角度差值，θ_r为真实角度值，

为真实角速度。原始的机械臂控制算法中各轴相互立，各轴误差不对其他的轴输出力矩产生影响。a为智能体补偿的力矩值，

TD3的观测状态为：

式中θ_r是t时刻关节角的真实值，

为关节的真实角速度，θ_n为t时刻关节的目标值θ_r,

观测值包含当前时刻各个目标关节的角度。针对非线性轨迹误差系统f(x_t,τ_t)，TD3可以不基于数学模型学习补偿参数值，以达到最优控制的目标。

结合公式(12)和公式(14)得，机械臂控制系统力矩输出为：

τ＝τ_PID+τ_d+a (16)。

Images