[发明专利]基于关节臂机器人的避碰轨迹规划方法和装置

摘要

本发明公开了一种基于关节臂机器人的避碰轨迹规划方法和装置，根据作业任务要求在规定的时间之内生成候补避碰轨迹序列，考虑路径长度，关节转动总量，以及作业用时这三方面因素，从中选择最佳的避碰轨迹，从而实现自主避碰路径规划，避免在作业过程之中的人工干预。

主权项

1.基于关节臂机器人的避碰轨迹规划方法，其特征在于，包括：步骤1，从任务规划器所发送的规划任务列表中选择一项任务作为当前的规划任务Task，并获取所述规划任务Task的起始位姿Pstart和终止位姿Pend，以及所述规划任务Task对应的被加工对象和关节臂的参数，所述起始位姿Pstart为所述规划任务Task的配置空间C的起始位坐标向量，所述终止位姿Pend为所述规划任务Task的所述配置空间C的终止位坐标向量，所述配置空间C的维度等于所述关节臂的关节数D；步骤2，在给定的预设时间T之内，将随机产生的不同规划路径进行碰撞检测，从而产生获得M个候补避碰轨迹序列{Qi}(i＝1 to M)，所述候补避碰轨迹序列{Qi}(i＝1 to M)之中的每一项Qi为从所述起始位姿Pstart至所述终止位姿Pend的N个路径点所构成的向量[P1，P2，…，Pj－1，Pj，Pj+1，…，PN‑1，PN]，所述P1为所述起始位姿Pstart，所述PN为所述终止位姿Pend，所述下标j为构成所述候补避碰轨迹Qi的所述路径点的序列编号，所述下标j为从1至所述N的正整数，所述下标i为所述候补避碰轨迹Qi的序列编号，即所述下标i为从1至所述M的正整数；步骤3，利用评价函数f(Qi)＝A*clength(Qi)+B*crotation(Qi)+C*cduration(Qi)对所述候补避碰轨迹序列{Qi}(i＝1 to M)之中的每一项Qi进行评价计算之后，将所述f(Qi)的最小值fmin所对应的所述候补避碰轨迹Qi作为所述最优路径Qoptimal，其中：所述clength(Qi)为所述候补避碰轨迹Qi所对应的轨迹长度，所述clength(Qi)的计算公式为：clength(Qi)＝Σ(j＝2 to N)|Pj‑Pj‑1|，所述|Pj‑Pj‑1|为所述关节臂从所述路径点Pj‑1至所述路径点Pj的移动距离；所述crotation(Qi)为所述候补避碰轨迹Qi所对应的所有关节的行程，所述crotation(Qi)的计算公式为：Σ(k＝1 to D)Σ(j＝2 to N)|θPkj–θPk(j‑1)|，所述下标k为所述关节臂的序列编号，即所述下标k为从1至所述关节数D的正整数，所述|θPkj–θPk(j‑1)|为所述关节臂的所述第k个关节从所述路径点Pj‑1至所述路径点Pj的旋转角度，所述θPkj为所述关节臂的第k个关节在所述路径点Pj的角度值；所述cduration(Qi)为所述候补避碰轨迹Qi所对应的持续时间，所述cduration(Qi)的计算公式为：cduration(Qi)＝Tend‑Tstart，所述Tstart为所述关节臂在所述起始位姿Pstart的时刻，所述Tend为所述关节臂在所述终止位姿Pend的时刻；所述A为所述候补避碰轨迹Qi所对应的轨迹长度的加权系数；所述B为候补避碰轨迹Qi所对应的所有关节的行程的加权系数；所述C为候补避碰轨迹Qi所对应的持续时间的加权系数；所述加权系数A，所述加权系数B和所述加权系数C满足：A+B+C＝1。