基于控制障碍函数的重载机器人安全控制方法及控制器

本发明属于运动变量调控，具体涉及一种基于控制障碍函数的重载机器人安全控制方法及控制器。

背景技术：

1、重载机器人的整机质量和运动惯量较大，制动距离较长。机器人运动或负载位置发生变化时，负载质心、运动响应和稳定余量也会改变，执行器的可用输出还可能受到负载、温度及供能条件的影响。在狭窄区域、坡道或存在动态障碍物的环境中，障碍物距离、支撑条件和环境变化进一步增加了安全控制的难度。

2、现有重载机器人通常按照给定的轨迹或速度指令运行，并采用固定限幅限制执行器输出。固定限幅适用于工况变化较小的场合，但难以反映负载、制动能力和执行器状态参数的实时变化。避碰、稳定和关节限位等要求分别处理时，还可能出现一项控制修正减小另一项安全余量的情况。

3、控制障碍函数能够将距离、稳定余量和关节运动范围等安全要求转化为控制约束，并与任务控制指令一同纳入二次规划求解。应用于重载机器人时，还需要根据负载参数、环境信息和执行器状态参数调整控制信号范围及其相邻周期变化量，并对输入数据的时效性、求解状态和执行器故障进行检查。

4、因此，有必要提出一种能够在同一控制周期内处理控制信号范围、多项安全约束和异常输出的安全控制方法，以便在任务控制过程中及时修正控制信号，降低重载机器人发生碰撞、失稳或执行器超限的风险。

5、现有技术中，中国专利申请号为cn202411320397.2公开了一种基于控制障碍函数的无人机吊运系统触觉遥操作控制方法及系统，通过简化建立uav二阶积分动力学模型，根据uav、有效载荷以及悬绳和障碍物环境之间的交互，利用控制障碍函数将避障问题转化为二次规划(qp)优化问题，实现了无人机吊运系统在非结构环境中的有效避障；此外，还提出了触觉遥操作力反馈模型，使其满足输出无源性约束要求。操作员可以直观地感知障碍物的存在，并将无人机吊运系统引向其他合适的方向，使无人机吊运遥操作控制系统能够同时满足安全性和稳定性要求。是对无人机如何感知障碍物及避障的控制系统的改进。而如何基于控制障碍函数降低重载机器人发生碰撞、失稳或执行器超限的风险是目前亟待解决的问题。

技术实现思路

1、针对相关技术中的问题，本发明提供一种基于控制障碍函数的重载机器人安全控制方法及控制器，在不改变任务控制器基本控制目标的情况下，使控制信号满足执行器输出范围、承载能力和关节运动范围要求，并避免机器人与障碍物发生碰撞；当出现二次规划无可行解、输入数据超时或执行器异常中的任一情况时，输出预先设定的安全控制指令，必要时切断驱动使能。

2、为解决上述技术问题，本发明是通过以下技术方案实现的：

3、包括以下步骤：

4、获取机器人状态、负载参数、环境信息和执行器状态参数，对所获取的机器人状态、负载参数、环境信息和执行器状态参数进行更新，并为更新后的机器人状态、负载参数、环境信息和执行器状态参数分别设置时间戳和版本号，形成机器人实时信息；根据所述机器人实时信息确定控制信号的幅值范围及相邻控制周期的变化范围，建立控制信号约束集；同时，根据所述机器人实时信息构建包括避碰约束函数和稳定约束函数在内的安全约束函数；

5、所述安全约束函数，根据其相对于控制信号的相对阶进行逐阶递推，直至得到包含控制信号的末阶辅助函数，并根据所述末阶辅助函数建立相应的控制障碍函数约束；

6、将所述控制信号约束集和控制障碍函数约束作为约束条件进行二次规划求解，得到安全控制信号；

7、当满足以下任一条件时，输出预先设定的安全控制指令：

8、机器人实时信息发生超时或缺失；

9、二次规划求解前后的信息版本号不一致；

10、执行器异常；

11、二次规划无可行解；

12、否则，输出所述二次规划求解得到的安全控制信号。

13、所述机器人实时信息超时，是指任一项进行安全控制所必需的机器人实时信息，其时间戳与当前控制时刻之间的时间差超过对应的预设有效期限。

14、本发明通过获取带有时间戳和版本号的机器人实时信息包括机器人状态、负载参数、环境信息及执行器状态参数建立控制信号约束集，在控制信号约束集和控制障碍函数约束下根据二次规划求解输出安全控制信号，克服了固定限幅在复杂工况下适应性不足的问题，实现基于实时状态的安全边界动态更新，提升重载机器人安全工况的适应性，使得重载机器人在承受巨大负载和外部干扰时，仍能保证运动精准、平稳、安全。

15、优选地，机器人状态包括位置、速度、关节运动状态中的至少一种；负载参数包括负载质量和负载质心位置；环境信息包括坡度、附着条件以及机器人与障碍物之间的相对关系；执行器状态参数包括温度和当前可用输出。

16、优选地，控制信号约束集为：

17、

18、其中，表示控制信号约束集；表示负载参数、环境信息和执行器状态参数；表示当前控制周期待求的控制信号；和分别表示当前工况下控制信号的下限和上限；表示上一控制周期已经输出的控制信号；和分别表示相邻控制周期允许的控制信号变化量下限和上限。

19、式（1）中：

20、

21、其中，表示当前控制周期待求的控制信号；表示负载参数、环境信息和执行器状态参数；和分别表示根据当前负载参数、环境信息和执行器状态参数确定的控制信号下限和上限。

22、用于限制控制信号的大小，防止控制指令超过执行器在当前负载和工作状态下的实际输出能力。控制信号能够沿两个方向作用时，分别修正两个方向的允许输出；控制信号只能沿一个方向作用时，不可输出方向的限值取零，修正后的控制信号范围不得超过执行器的额定输出范围。

23、式（2）中：

24、

25、其中，表示当前控制周期待求的控制信号；表示上一控制周期已经输出的控制信号；和分别表示相邻控制周期允许的控制信号变化量下限和上限。

26、用于限制控制信号在相邻周期之间的变化速度，避免驱动力、制动力或关节力矩发生突变。

27、优选地，根据机器人类型和作业要求分别确定是否选用承载能力约束函数和关节运动范围约束函数，并将避碰约束函数、稳定约束函数以及选用的承载能力约束函数、关节运动范围约束函数组成安全约束函数集合。所述安全约束函数集合如下式：

28、

29、其中，表示安全约束函数集合；表示避碰约束函数；表示选用的稳定约束函数集合；表示可选安全约束函数集合，由根据机器人类型和作业要求选用的承载能力约束函数、关节运动范围约束函数构成；两者均不选用时为空集；表示根据负载质心投影距离建立的稳定约束函数；表示根据恢复力矩和倾覆力矩建立的稳定约束函数；表示第个关节的承载能力约束函数；和分别表示第个关节的位置上限约束函数和位置下限约束函数；表示第个安全约束函数；表示机器人状态；表示负载参数、环境信息和执行器状态参数；表示安全约束编号，表示关节编号。所述约束函数均以机器人状态以及负载、环境和执行器状态参数的参数为变量，函数值≥0表示满足相应安全要求。

30、优选地，避碰约束函数为机器人与障碍物之间的距离减去安全距离：

31、

32、其中，表示避碰约束函数；表示机器人与障碍物之间的实际距离；表示机器人与障碍物之间需要保持的安全距离。

33、所述安全距离为：

34、

35、其中，表示安全距离；表示预留的基础安全距离；表示机器人与障碍物之间的非负相对接近速度；表示数据采集、状态估计、控制计算、通信传输和执行器响应所需的总时延；表示机器人在当前工况下的可用制动减速度；表示障碍物朝机器人方向运动时可能达到的最大加速度；表示测距误差的安全系数；表示距离测量误差的标准差或误差估计值。 所述当前可用制动减速度为正值。

36、优选地，稳定约束函数采用下述式（7）和式（8）中的至少一种；选用承载能力约束函数时采用式（9）；选用关节运动范围约束函数时采用式（10）、式（11）和式（12）：

37、

38、其中，式（7）中的表示质心投影稳定约束函数，表示负载质心投影到支撑区域边界的最小距离，表示预留的稳定距离；

39、式（8）中的表示力矩稳定约束函数，表示恢复力矩，表示倾覆力矩，表示预留的稳定力矩；

40、式（9）中的表示第个关节的承载能力约束函数，表示第个关节的允许力矩，表示第个关节的当前力矩；上标2表示相应力矩的平方；

41、式（10）中的表示第个关节的位置上限约束函数，表示第个关节的位置上限，表示第个关节的当前位置，表示第个关节的停止距离；

42、式（11）中的表示第个关节的位置下限约束函数，表示第个关节的位置下限，表示第个关节的当前位置，表示第个关节的停止距离；

43、式（12）中的表示第个关节的停止距离，表示第个关节朝向相应限位方向的速度分量，表示第个关节当前可用的减速度；下标表示关节编号，下标c表示朝向相应限位的速度分量。

44、优选地，对于相对阶为的安全约束函数，令：

45、

46、其中，表示第个安全约束的初始辅助函数；表示第个安全约束函数；表示安全约束编号。

47、

48、其中，表示第个安全约束函数对应的第阶辅助函数；表示上一阶辅助函数沿机器人系统状态轨迹的时间导数；表示第个安全约束函数在第阶递推中采用的扩展k类函数；表示递推阶数；表示第个安全约束函数相对于控制信号的相对阶。递推至第阶时，所得辅助函数中包含待求解的控制信号，并据此建立相应的控制障碍函数约束。

49、

50、其中，表示第个安全约束函数在第阶递推中得到的辅助函数；表示递推至第阶时得到的末阶辅助函数，该辅助函数中包含控制信号，并通过不等式形成第项控制障碍函数约束；表示第个安全约束函数相对于控制信号的相对阶；表示递推阶数；表示机器人状态；表示待求解的控制信号；表示第项安全约束对应的非负误差补偿量；表示安全约束编号。

51、优选地，二次规划求解方程如下：

52、

53、其中，式（16）中的表示待求控制信号，表示任务控制器给出的标准控制指令，表示本周期控制信号相对上一周期输出的变化量，表示控制信号偏差项的正定权重矩阵，表示控制信号变化量项的正半定权重矩阵；表示任务跟踪约束的松弛变量，表示松弛变量二次项的正权重系数，上标表示矩阵或向量的转置；

54、式（17）中的和分别表示控制障碍函数约束矩阵和约束向量；和分别表示任务跟踪约束矩阵和约束向量；表示松弛变量分配矩阵；表示控制信号约束集；表示非负松弛变量。

55、其中，目标函数包括控制信号与标准控制指令的偏差、控制信号变化量及任务跟踪约束的松弛变量；控制信号偏差项采用正定权重矩阵，控制信号变化量项采用正半定权重矩阵，松弛变量二次项的权重为正值；控制障碍函数约束作为硬约束，任务跟踪约束作为软约束。

56、本发明还公开了一种基于控制障碍函数的重载机器人安全控制方法的控制器，包括处理器、存储器、通信接口和安全输出接口；

57、处理器用于执行计算机程序；

58、存储器用于存储计算机程序；

59、通信接口用于接收机器人状态、负载参数、环境信息和执行器状态参数，并发送安全控制信号或预先设定的安全控制指令；

60、安全输出接口用于输出安全停机信号，并控制安全继电器切断驱动使能。

61、本发明具有以下有益效果：

62、1.本发明通过获取带有时间戳和版本号的机器人实时信息包括机器人状态、负载参数、环境信息及执行器状态参数建立控制信号约束集，在控制信号约束集和控制障碍函数约束下根据二次规划求解输出安全控制信号，克服了固定限幅在复杂工况下适应性不足的问题，实现基于实时状态的安全边界动态更新，提升重载机器人安全工况的适应性，使得重载机器人在承受巨大负载和外部干扰时，仍能保证运动精准、平稳、安全；

63、2.本发明中所有的机器人状态、负载参数、环境信息和执行器状态参数均带有时间戳和版本号，处理器在每个控制周期开始时先判断各项信息是否仍在规定的有效时间内，并记录本周期采用的版本号，从根本上消除了因参数数据陈旧、数据错位或数据丢失导致的安全控制器使用错误信息计算，使得在复杂环境下重载机器人能更好地适应各种复杂作业情况，为重载机器人安全运行提供精确的数据保障；

64、3.采用了承载能力约束函数和关节运动范围约束函数，承载能力约束采用力矩平方比较方式，使得当前关节力矩的平方不超过允许力矩的平方，避免了正负方向判断的复杂性，且保留了更平滑的数学特性，有利于二次规划的数值稳定性，并且约束函数还考虑了关节运动的动力学特性，实现了“提前刹车”的主动安全保护，减小了关节力矩过载的风险，避免了机械结构损坏；

65、4.避碰、稳定和关节承载能力等作为约束函数，使得多约束条件下，系统始终运行在由安全约束交集组成的“安全集”内，从而彻底避免了“一项修正抵消另一项”的作业错误，避免了一项控制修正减小另一项安全余量的情况。