[发明专利]工业机械臂路径规划方法

权利要求书

1.一种工业机械臂路径规划方法，其特征在于，包括：

根据机器人标准D-H参数建立机械臂数学模型，建立三维障碍物环境模型根据所述机械臂数学模型和三维障碍物环境模型，并采用RRT算法在障碍物环境中找到一条所述机器人的机械臂从起始位姿到目标位姿的避障路径；

找到所述避障路径中的关键任务点并求解关节角，使用五次多项式插值规划相邻的关键任务点之间的关节运动规律，根据所述关节运动规律，输出对所述避障路径优化后的平滑路径以及关节角的运动规律图。

2.如权利要求1所述的工业机械臂路径规划方法，其特征在于，根据机器人标准D-H参数建立机械臂数学模型，建立三维障碍物环境模型根据所述机械臂数学模型和三维障碍物环境模型，并采用RRT算法在障碍物环境中找到一条所述机器人的机械臂从起始位姿到目标位姿的避障路径，包括以下步骤：

步骤1：按照机器人标准D-H参数建立机械臂数学模型；

步骤2：建立三维障碍物环境模型，其中，障碍物采用球形包络描述为A(B0,r),其中B0(x0,y0,z0)为球心在基坐标系中的坐标，r为球的半径；

步骤3：对随机树Tinit进行扩展，随机树将Tinit将Qinit作为起点，将Qgoal作为目标点；并将Qinit作为本次扩展的父节点；

步骤4：从当前定义的父节点Qinit开始扩展，在地图空间C中随机生成探索点Qrand，然后在当前随机树中找到距离Qrand最近的点Qnear；

步骤5：在沿着Qnear和Qrand的方向上，找到距离Qnear为deltaq的Qnew，其中，deltaq≤ρ；若没有遇到障碍物，则Qnew添加到树中，转至步骤6；若遇到障碍物，则舍弃Qnew，转回步骤4；

步骤6：更新随机树Tinit，将Qnew作为随机树Tinit下一次扩展的父节点；

步骤7：综合随机树Tinit规划好的路径点，得到优化序列避障路径Q1(Qinit),Q2,Q3,...,Qn(Qgoal)。

3.如权利要求1所述的工业机械臂路径规划方法，其特征在于，步骤S2，找到所述避障路径中的关键任务点并求解关节角，使用五次多项式插值规划相邻的关键任务点之间的关节运动规律，根据所述关节运动规律，输出对所述避障路径优化后的平滑路径以及所述机械臂的运动规律图，包括：

步骤8：建立五次多项式插值函数，包括：角位移、角速度、角加速度的函数表达式分别为：

约束条件如下，相对于三次多项式插值，增加了对起止点角速度的约束，并设t₀为0：

求解得：

其中，t为时间，a为多项式系数，θ为关节角向量，t₀和t_f为关节插值的起始和终止时间点；

步骤9：根据步骤7中所得优化序列避障路径Q1(Qinit),Q2,Q3,...,Qn(Qgoal)，顺次取得优化序列避障路径中关键任务节点对应的关节坐标向量；

步骤10：对优化序列避障路径中的关节坐标向量求逆，生成整个轨迹中采样点关节角运动规律图。