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

权利要求书

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}中的描述。