CN111791234A - 一种狭窄空间内多机器人空间工作位置防撞控制算法 - Google Patents

Abstract

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

Description

一种狭窄空间内多机器人空间工作位置防撞控制算法

【技术领域】

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

【背景技术】

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

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

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

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

【发明内容】

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

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

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)；

7)比较第一运动参数矩阵与第二运动参数矩阵，若两者不相等，则两个机器人均执行下一运动；若两者相等，则说明可能发生碰撞，重复步骤5) -6)。

进一步的，所述步骤22)包括，在每个连杆上定义一个固连坐标系，固连在连杆i上的固连坐标系为坐标系{i}，坐标系{i}的原点位于关节轴i-1和i 的公垂线与关节i轴线的交点上；坐标系{i}的z轴和关节i的轴线重合，坐标系{i}的x轴在关节轴i和关节轴i+1的公垂线上，方向从i指向i+1，坐标系{i} 的y轴由右手定则确定。

进一步的，所述步骤23)包括，

231)绕X_i-1轴旋转α_i-1角，使坐标系i过渡到坐标系{R}，Z_i-1转到Z_R，并与Z_i方向一致；

232)坐标系{R}沿X_i-1轴平移a_i-1距离，把坐标系移动到关节i上，使坐标系{R}过度到{Q}；

233)坐标系{Q}绕Z_Q轴转动θ_i角，使坐标系{Q}变换到坐标系{P}；

234)坐标系{P}沿Z_i轴平移d_i距离，使坐标系{P}过渡到和坐标系{i}重合，将坐标系{i}中定义的矢量变换成坐标系{i-1}中的描述。

与现有技术相比，本发明一种狭窄空间内多机器人空间工作位置防撞控制算法的有益效果在于：通过定义不同机器人中各个关节点、连杆坐标，建立连杆坐标系，然后建立两个机器人之间的连杆坐标系切换的数学模型，实现两个机器人运动参数矩阵在同一坐标系的描述，为后续两个机器人运动参数矩阵的比较提供了前提基础；然后根据机器人实际位置变化关系实时计算相互关系，以其中一个机器人为主机器人，按照其规划的运动轨迹执行，按照设定的频率实时获取两个机器人下一时间点的运动参数矩阵，通过比较两个运动参数矩阵，判断是否存在碰撞风险。通过实践的检验，本方案的算法能够稳定可靠的保证多机器人在狭小空间内高速不间断的工作而不发生碰撞；且计算精度高、效率高。

【附图说明】

图1为本发明实施例的逻辑判断控制流程图；

图2为本发明实施例中参考系的示意图；

图3为本发明实施例单位向量在参考系中的示意图；

图4为本发明实施例中坐标系A与参考系的示意图；

图5为本发明实施例中机器人关机与连杆坐标系的示意图。

【具体实施方式】

实施例：

本实施例一种狭窄空间内多机器人空间工作位置防撞控制算法，其包括以下步骤：

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

机器人的主要结构是由连杆、关节和末端执行器组成的开式链机构。以 i_A为基础建立坐标系A，其中3个坐标轴分别为i_A、j_A、k_A；坐标系A中任意一点

的坐标对于参考系i₀的坐标变换表达如下：

其中，参考系i₀的三个坐标轴对应的三个单位向量分别为i₀、j₀、k₀；

如图2所示，参考系i₀中P点的几何解析式为：

如图3所示，单位向量方向的数学解析式模型为：

其中，向量i_A是单位向量，与参考系轴x₀的单位向量i₀的夹角为α；轴y₀与单位向量j₀的夹角为β；轴z₀与单位向量k₀的夹角为γ。

如图4所示，参考系i₀中，以任意点为坐标系的转换数学模型为：

从而，i_A坐标系中任意一点的坐标相对于参考系i₀的坐标变换表达如公式1)所示。

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

21)连杆连接的矩阵描述：建立用六组(a_i，α_i，d_i，θ_i)的矩阵来描述两个机器人运动的18个固定参数。

如图5所示，机器人是由一系列连接在一起的连杆和可以在某一二维坐标平面内做有限转动的关节构成的(单一自由度)。

从机器人固定的基座开始为连杆进行编号，可以命名固定基座为连杆0，第一个可转动的连杆为连杆1，以此类推，机器人末端连杆为连杆i。本实施例的应用主要针对6轴机器人，为了确定机器人末端执行器及各连杆在三维空间中的位置和姿态，计算时i不大于6。

运动连杆的状态，可以用2个参数来表示，即连杆的物理长度a和连杆的转向角度。

相邻两个连杆的连接方式可以由2个参数描述，即连杆的偏距d和关节角α。其中偏距用来描述沿相邻两相邻连杆公共轴线方向的距离，关节角用来描述两相邻连杆绕公共轴线旋转的夹角。

如图5所示，连杆i和连杆i-1为相邻的两个连杆，关节轴i-1和关节轴i之间的夹角为α_i-1，公垂线长度为a_i-1(即连杆i-1的长度)。同理，a_i表示连杆 i两端关节轴的公垂线长度，即连杆i的长度。连杆偏距d_i表示公垂线a_i-1与关节轴i的交点到公垂线a_i与关节轴i的交点的有向距离。当关节i是移动关节时，连杆偏距d_i是一个变量。关节角θ_i表示公垂线a_i-1的延长线与公垂线a_i之间绕关节轴i旋转所形成的夹角。当关节i是转动关节时，关节角θ_i是一个变量。

机器人是由一系列连接在一起的连杆和转动关节构成，每个连杆可以用4个运动寻参数来描述，其中2个参数用于描述连杆本身，另外2个用于描述相邻连杆之间的关系。

一般来说，对于转动关节，关节角是变量，其余三个连杆参数固定不变；对于移动关节，连杆偏距是变量，其余三个连杆参数不变。

综上所述，对于一个6轴机器人，我们可以建立用6组(a_i，α_i，d_i，θ_i) 的矩阵来描述其18个固定参数。

22)建立机器人连杆坐标系的数学模型：为了描述出连杆与相邻连杆之间的相对位置关系，需要在每个连杆上定义一个固连坐标系；每一个杆件的坐标系z轴和原点固连在该杆件的前一个轴线上。

如图5所示，固连在连杆i上的固连坐标系成为坐标系{i}，坐标系{i}的原点位于关节轴i-1和i的公垂线与关节i轴线的交点上。坐标系{i}的z轴和关节i的轴线重合，坐标系{i}的x轴在关节轴i和关节轴i+1的公垂线上，方向从 i指向i+1，坐标系{i}的y轴由右手定则确定。

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

相邻的两个连杆坐标系的变换需要建立3个中间转换坐标系{P}、{Q}、 {R}，通过以下四个步骤实现：

231)绕X_i-1轴旋转α_i-1角，使坐标系i过渡到坐标系{R}，Z_i-1转到Z_R，并与Z_i方向一致；

232)坐标系{R}沿X_i-1轴平移a_i-1距离，把坐标系移动到关节i上，使坐标系{R}过度到{Q}；

233)坐标系{Q}绕Z_Q轴转动θ_i角，使坐标系{Q}变换到坐标系{P}；

234)坐标系{P}沿Z_i轴平移d_i距离，使坐标系{P}过渡到和坐标系{i}重合。

通过以上步骤可以将坐标系{i}中定义的矢量变换成坐标系{i-1}中的描述。

其变换的数学模型表示为：

即：

其矩阵表达为：

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

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

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

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

7)比较第一运动参数矩阵与第二运动参数矩阵，若两者不相等，则两个机器人均执行下一运动；若两者相等，则说明可能发生碰撞，重复步骤5) -6)，如图1所示。

本实施例一种狭窄空间内多机器人空间工作位置防撞控制算法，通过定义不同机器人中各个关节点、连杆坐标，建立连杆坐标系，然后建立两个机器人之间的连杆坐标系切换的数学模型，实现两个机器人运动参数矩阵在同一坐标系的描述，为后续两个机器人运动参数矩阵的比较提供了前提基础；然后根据机器人实际位置变化关系实时计算相互关系，以其中一个机器人为主机器人，按照其规划的运动轨迹执行，按照设定的频率实时获取两个机器人下一时间点的运动参数矩阵，通过比较两个运动参数矩阵，判断是否存在碰撞风险。通过实践的检验，本方案的算法能够稳定可靠的保证多机器人在狭小空间内高速不间断的工作而不发生碰撞；且计算精度高、效率高。

以上所述的仅是本发明的一些实施方式。对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (3)

1.一种狭窄空间内多机器人空间工作位置防撞控制算法，其特征在于：其包括以下步骤：

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)；

7)比较第一运动参数矩阵与第二运动参数矩阵，若两者不相等，则两个机器人均执行下一运动；若两者相等，则说明可能发生碰撞，重复步骤5)-6)。

2.如权利要求1所述的狭窄空间内多机器人空间工作位置防撞控制算法，其特征在于：所述步骤22)包括，在每个连杆上定义一个固连坐标系，固连在连杆i上的固连坐标系为坐标系{i}，坐标系{i}的原点位于关节轴i-1和i的公垂线与关节i轴线的交点上；坐标系{i}的z轴和关节i的轴线重合，坐标系{i}的x轴在关节轴i和关节轴i+1的公垂线上，方向从i指向i+1，坐标系{i}的y轴由右手定则确定。

3.如权利要求1所述的狭窄空间内多机器人空间工作位置防撞控制算法，其特征在于：所述步骤23)包括，

231)绕X_i-1轴旋转α_i-1角，使坐标系i过渡到坐标系{R}，Z_i-1转到Z_R，并与Z_i方向一致；

232)坐标系{R}沿X_i-1轴平移a_i-1距离，把坐标系移动到关节i上，使坐标系{R}过度到{Q}；

233)坐标系{Q}绕Z_Q轴转动θ_i角，使坐标系{Q}变换到坐标系{P}；

234)坐标系{P}沿Z_i轴平移d_i距离，使坐标系{P}过渡到和坐标系{i}重合，将坐标系{i}中定义的矢量变换成坐标系{i-1}中的描述。

Images