一种基于尾部风险约束的机械臂安全控制方法及系统

本发明涉及机器人控制，尤其涉及一种基于尾部风险约束的机械臂安全控制方法及系统。

背景技术：

1、机械臂广泛应用于工业装配、搬运、焊接、打磨、分拣以及协作作业等场景。在实际生产过程中，机械臂不仅需要完成目标到达或轨迹跟踪任务，还必须保证执行过程中的平稳性、安全性和机构寿命。然而，传统机械臂控制方法大多以目标误差最小或任务完成率最高为主要优化目标，对控制过程中的速度波动、加速度冲击及长期磨损缺乏统一建模与约束。

2、近年来，深度强化学习在机械臂控制中取得了较好效果。基于深度强化学习的机械臂控制方法能够直接从状态输入到动作输出建立映射关系，具有较强的非线性表达能力和自适应能力。但现有多数强化学习控制方法通常仅通过奖励函数驱动策略优化，在提高任务成功率方面效果较好，而在控制过程中容易产生较大的速度尖峰和加速度尖峰，导致机械臂执行动作过于激进，进而加剧机构磨损和安全风险。

3、为了改善上述问题，部分研究开始在强化学习中引入安全约束或代价惩罚机制。然而，已有约束方法通常以平均代价为主要调节依据，只能反映整体风险水平，难以有效表征和抑制控制过程中少量但幅值很高的尾部风险动作。此类尾部高风险动作虽然在统计上出现频率较低，但更容易引起机械冲击、执行超限、设备振动及零部件疲劳，对工业现场的稳定运行影响更为显著。

技术实现思路

1、为了解决上述技术问题，本发明的目的是提供一种基于尾部风险约束的机械臂安全控制方法及系统，能够降低机械臂动作突变和控制抖动，提高机械臂运行稳定性。

2、本发明所采用的第一技术方案是：一种基于尾部风险约束的机械臂安全控制方法，包括以下步骤：

3、采集机械臂的当前状态向量数据并进行数据预处理与动作生成处理，得到机械臂末端在三维笛卡尔空间中的位移增量动作；

4、根据机械臂末端在三维笛卡尔空间中的位移增量动作控制机械臂执行安全代价计算，并进行奖励和代价评估，得到机械臂当前状态动作对的累计任务收益估计值与机械臂当前状态动作对的累计安全代价估计值；

5、筛选机械臂执行当前动作的成功轨迹进行尾部风险统计，并对拉格朗日乘子进行动态更新，输出更新后的拉格朗日乘子；

6、结合机械臂当前状态动作对的累计任务收益估计值、机械臂当前状态动作对的累计安全代价估计值与更新后的拉格朗日乘子，进行策略优化，输出机械臂的安全控制动作。

7、进一步，所述采集机械臂的当前状态向量数据并进行数据预处理与动作生成处理，得到机械臂末端在三维笛卡尔空间中的位移增量动作这一步骤，其具体包括：

8、获取机械臂的当前状态向量数据并进行数据预处理，得到预处理后的机械臂当前状态向量，所述机械臂的当前状态向量数据包括机械臂当前末端位置、关节位置、关节速度、目标位置以及当前位置与目标位置之间的相对误差；

9、基于策略网络，对预处理后的机械臂当前状态向量进行融合编码和非线性映射处理，提取反映机械臂当前控制状态的特征信息；

10、根据机械臂当前控制状态的特征信息确定机械臂末端在三维笛卡尔空间中的位移增量动作。

11、进一步，所述根据机械臂末端在三维笛卡尔空间中的位移增量动作控制机械臂执行安全代价计算，并进行奖励和代价评估，得到机械臂当前状态动作对的累计任务收益估计值与机械臂当前状态动作对的累计安全代价估计值这一步骤，其具体包括：

12、根据机械臂末端在三维笛卡尔空间中的位移增量动作控制机械臂执行安全代价计算，得到机械臂执行动作的总安全代价；

13、基于机械臂执行动作的总安全代价，对机械臂执行当前动作进行奖励和代价评估，得到机械臂当前状态动作对的累计任务收益估计值与机械臂当前状态动作对的累计安全代价估计值。

14、进一步，所述根据机械臂末端在三维笛卡尔空间中的位移增量动作控制机械臂执行安全代价计算，得到机械臂执行动作的总安全代价这一步骤，其具体包括：

15、根据机械臂末端在三维笛卡尔空间中的位移增量动作控制机械臂执行当前动作，采集机械臂的当前动作向量，所述机械臂的当前动作向量包括机械臂各关节在各时刻的关节速度与机械臂各关节在各时刻的位移变化量；

16、基于机械臂各关节各时刻的关节速度，计算当前时刻与前一时刻的关节速度差值，得到机械臂各关节的加速度；

17、对机械臂各关节在各时刻的位移变化量进行累积计算，得到机械臂各关节磨损量；

18、根据机械臂各关节在各时刻的关节速度平方项的均值计算机械臂关节速度代价，根据机械臂各关节的加速度平方项的均值计算机械臂关节加速度代价，根据机械臂各关节磨损量与预设磨损预算的占比计算机械臂关节磨损代价；

19、对机械臂关节速度代价、机械臂关节加速度代价与机械臂关节磨损代价进行求和，得到机械臂执行动作的总安全代价。

20、进一步，所述基于机械臂执行动作的总安全代价，对机械臂执行当前动作进行奖励和代价评估，得到机械臂当前状态动作对的累计任务收益估计值与机械臂当前状态动作对的累计安全代价估计值这一步骤，其具体包括：

21、将预处理后的机械臂当前状态向量与机械臂的当前动作向量进行拼接，得到机械臂当前状态动作联合向量；

22、将机械臂当前状态动作联合向量输入至收益评估网络，评估机械臂执行当前动作后对目标到达任务的贡献程度值，输出机械臂当前状态动作对的累计任务收益估计值；

23、将机械臂当前状态动作联合向量输入至代价评估网络，基于机械臂执行动作的总安全代价评估机械臂执行当前动作后产生的安全风险水平，输出机械臂当前状态动作对的累计安全代价估计值。

24、进一步，所述筛选机械臂执行当前动作的成功轨迹进行尾部风险统计，并对拉格朗日乘子进行动态更新，输出更新后的拉格朗日乘子这一步骤，其具体包括：

25、筛选机械臂执行当前动作的成功轨迹并结合机械臂执行动作的总安全代价进行尾部风险统计，得到机械臂的尾部风险指标；

26、基于机械臂的尾部风险指标对拉格朗日乘子进行动态更新，输出更新后的拉格朗日乘子。

27、进一步，所述筛选机械臂执行当前动作的成功轨迹并结合机械臂执行动作的总安全代价进行尾部风险统计，得到机械臂的尾部风险指标这一步骤，其具体包括：

28、筛选机械臂执行当前动作的成功轨迹，所述成功轨迹表示在策略网络输出动作的驱动下完成目标到达任务，且回合终止时末端执行器与目标位置之间的距离小于预设成功阈值的轨迹；

29、提取机械臂执行当前动作的成功轨迹对应的机械臂执行动作的总安全代价，构建机械臂的活动成本序列；

30、基于机械臂的活动成本序列，提取均值活动成本和高分位活动成本，所述均值活动成本用于表征成功轨迹在有效运动阶段内的平均安全风险水平，所述高分位活动成本用于表征活动成本序列中尾部高风险尖刺水平；

31、对均值活动成本和高分位活动成本进行加权求和，得到机械臂的尾部风险指标。

32、进一步，所述基于机械臂的尾部风险指标对拉格朗日乘子进行动态更新，输出更新后的拉格朗日乘子这一步骤，其具体包括：

33、将机械臂的尾部风险指标与预设安全阈值进行比较，得到尾部风险偏差；

34、根据尾部风险偏差对拉格朗日乘子进行动态更新，若尾部风险指标高于预设安全阈值时，增大拉格朗日乘子以增强安全代价约束强度，若尾部风险指标低于预设安全阈值时，减小拉格朗日乘子以降低对任务收益优化的抑制程度，输出更新后的拉格朗日乘子。

35、进一步，所述结合机械臂当前状态动作对的累计任务收益估计值、机械臂当前状态动作对的累计安全代价估计值与更新后的拉格朗日乘子，进行策略优化，输出机械臂的安全控制动作这一步骤，其具体包括：

36、根据机械臂当前状态动作对的累计任务收益估计值确定任务奖励项，根据机械臂当前状态动作对的累计安全代价估计值以及更新后的拉格朗日乘子确定安全代价项；

37、获取机械臂当前动作幅值及相邻时刻动作变化量确定动作平滑项；

38、结合任务奖励项、安全代价项以及动作平滑项构建策略优化目标函数，并对策略网络进行参数更新；

39、将更新后的策略网络部署于机械臂控制端，根据在实际运行过程中输入实时状态信息，输出机械臂的安全控制动作。

40、本发明所采用的第二技术方案是：一种基于尾部风险约束的机械臂安全控制系统，包括：

41、第一模块，用于采集机械臂的当前状态向量数据并进行数据预处理与动作生成处理，得到机械臂末端在三维笛卡尔空间中的位移增量动作；

42、第二模块，用于根据机械臂末端在三维笛卡尔空间中的位移增量动作控制机械臂执行安全代价计算，并进行奖励和代价评估，得到机械臂当前状态动作对的累计任务收益估计值与机械臂当前状态动作对的累计安全代价估计值；

43、第三模块，用于筛选机械臂执行当前动作的成功轨迹进行尾部风险统计，并对拉格朗日乘子进行动态更新，输出更新后的拉格朗日乘子；

44、第四模块，用于结合机械臂当前状态动作对的累计任务收益估计值、机械臂当前状态动作对的累计安全代价估计值与更新后的拉格朗日乘子，进行策略优化，输出机械臂的安全控制动作。

45、本发明方法及系统的有益效果是：本发明通过采集机械臂的当前状态向量数据并进行数据预处理与动作生成处理，得到机械臂末端在三维笛卡尔空间中的位移增量动作；进而根据机械臂末端在三维笛卡尔空间中的位移增量动作控制机械臂执行安全代价计算，并进行奖励和代价评估，得到机械臂当前状态动作对的累计任务收益估计值与机械臂当前状态动作对的累计安全代价估计值，通过构建关节速度代价、关节加速度代价和关节磨损代价组成的多源安全代价模型，能够更全面地反映机械臂控制过程中的实际风险；进一步筛选机械臂执行当前动作的成功轨迹进行尾部风险统计，并对拉格朗日乘子进行动态更新，输出更新后的拉格朗日乘子，引入多源安全代价建模与尾部风险感知拉格朗日调节机制，提高机械臂控制过程中的安全性和平滑性；最后结合机械臂当前状态动作对的累计任务收益估计值、机械臂当前状态动作对的累计安全代价估计值与更新后的拉格朗日乘子，进行策略优化，输出机械臂的安全控制动作，在策略优化目标函数中加入动作平滑惩罚项，以降低动作突变和控制抖动，提高机械臂运行稳定性。能够在保证目标到达性能的同时，有效降低速度和加速度尖刺、减少超限率并延缓机构磨损，适用于工业机械臂、协作机械臂及关节型机器人系统的安全运动控制。