[发明专利]一种狭窄空间内多机器人空间工作位置防撞控制算法

说明书

本发明揭示了一种狭窄空间内多机器人空间工作位置防撞控制算法，通过定义不同机器人中各个关节点、连杆坐标，建立连杆坐标系，然后建立两个机器人之间的连杆坐标系切换的数学模型，实现两个机器人运动参数矩阵在同一坐标系的描述，为后续两个机器人运动参数矩阵的比较提供了前提基础；然后根据机器人实际位置变化关系实时计算相互关系，以其中一个机器人为主机器人，按照其规划的运动轨迹执行，按照设定的频率实时获取两个机器人下一时间点的运动参数矩阵，通过比较两个运动参数矩阵，判断是否存在碰撞风险。本发明能够解决狭窄空间范围内双机器人协同作业时的防撞问题，且同时保障机器人能够高效的运作。

【技术领域】

本发明属于机器人运动控制算法技术领域，特别是涉及一种狭窄空间内多机器人空间工作位置防撞控制算法。

【背景技术】

单纯研究机器人相对于固定参考系的运动作为函数进行分析研究，此类研究并不考虑机器人运动所受的力和力矩，把机器人空间位移解析的表示为时间或者空间的函数，特别研究机器人各关节变量与固定参考系原点，机器人末端执行器与固定参考系原点之间的位置与姿态的关系。

进行这种研究的目的是对多关节机器人在运动过程中各关节节点空间坐标的变换轨迹与时间之间的关系进行函数化，为多台机器人在狭窄空间运动管理建立数学模型。

通用工业机器人多机小范围协作工作时，会产生互相干涉碰撞的问题，行业内通常遇到此类工况的做法是设置安全区域，当一台机器人工作时，另一台机器人是绝对不能进入此区域的，这种做法的后果是系统效率低，不能满足用户的节拍要求。

因此，有必要提供一种新的狭窄空间内多机器人空间工作位置防撞控制算法来解决上述问题。

【发明内容】

本发明的主要目的在于提供一种狭窄空间内多机器人空间工作位置防撞控制算法，能够解决狭窄空间范围内双机器人协同作业时的防撞问题，且同时保障机器人能够高效的运作。

本发明通过如下技术方案实现上述目的：一种狭窄空间内多机器人空间工作位置防撞控制算法，其包括以下步骤：

1)对各个机器人建立动作时各关节节点及末端执行器的相对坐标解析式；

2)建立机器人连杆连接的数学模型：

21)连杆连接的矩阵描述：对每个机器人用参数(a_i，α_i，d_i，θ_i)建立六组矩阵来描述该机器人运动的所有运动参数；其中a_i表示连杆i两端关节轴的公垂线长度，即连杆i的长度；α_i为关节轴i与关节轴i+1之间的夹角； d_i表示公垂线a_i-1与关节轴i的交点到公垂线a_i与关节轴i的交点的有向距离；关节角θ_i表示公垂线a_i-1的延长线与公垂线a_i之间绕关节轴i旋转所形成的夹角；

22)建立机器人连杆坐标系的数学模型：

23)建立相邻连杆坐标系转换的数学模型，获得各机器人运动参数矩阵计算模型：

3)在各个机器人中设定其中一个作为主机器人，其按照设定的运动轨迹正常运行；

4)根据主机器人的运动轨迹通过公式4)计算得到主机器人下一设定时间点的第一运动参数矩阵；

5)根据另一机器人设定的运动轨迹通过公式4)计算得到下一设定时间点该机器人的第二运动参数矩阵；

6)判断两个机器人的运动参数是否均在各自的安全范围内，若是，则继续步骤6)；若不是，则重复步骤4)-5)；