[发明专利]障碍环境下的工业机器人动力学性能最优轨迹规划方法

权利要求书

1.障碍环境下的工业机器人动力学性能最优轨迹规划方法，其特征在于，包括如下步骤：

S1、通过输入层输入环境地图和机器人本体信息，输出层输出轨迹的时空信息，以驱动机器人完成给定的任务；

S2、通过C空间高效的RRT-Connect算法，获得无碰撞路径，然后对路径进行修剪与光滑操作；

S3、通过动力学优化器，将第二阶段规划好的路径优化成动力学可执行轨迹；

S31、假设机器人的轨迹Q＝(q₁，q₂，…，q_k，T)，其中，q_k为路径结点，T为轨迹时间，则定义目标函数C(Q)：

式中，第一项为关节力矩的平方和，表示轨迹能量消耗；第二项为关节跃度的平方和，表示轨迹的平滑性；第三项表示轨迹里程时间，三项的加权总和作为目标函数，其中，τ为关节扭矩矢量，可以通过机器人逆动力学公式计算得到；为关节跃度矢量，T为轨迹时间变量，通过参数化轨迹直接得到和T的值，K_e、K_j、K_t为各个目标函数的权重因子；

S32、根据目标函数(1)式，得到以下优化问题：

min C(Q) (2)

式(2)为目标函数，式(3)为约束函数，依次为关节速度、关节加速度和关节扭矩约束；

S33、采用SQP算法解算上述约束函数，即得最优轨迹。

2.如权利要求1所述的障碍环境下的工业机器人动力学性能最优轨迹规划方法，其特征在于，所述环境地图通过激光雷达扫描技术快速获取。

3.如权利要求1所述的障碍环境下的工业机器人动力学性能最优轨迹规划方法，其特征在于，所述步骤S2通过以下步骤完成路径的修剪操作：

对于采样节点序列路径Q＝(q₁，q₂，…，q_k)，如果q_i-1和q_i+1之间能够直接连通，通过删除位姿点坐标q_i剪短路径；

对于相隔位姿点之间的位姿，通过线性插值计算中间位姿点，然后采用碰撞检测函数，对每个中间位姿点进行检测，最终保证所有的中间位姿点属于无障碍构型空间C_free。

4.如权利要求1所述的障碍环境下的工业机器人动力学性能最优轨迹规划方法，其特征在于，所述步骤S2中的路径光滑操作采用B样条拟合位姿点序列，如果检测出拟合后的轨迹发生碰撞，则还需返回路径采样，继续采样操作，如果检测光滑后的路径没有碰撞，则表示光滑操作成功，可以直接进入动力学规划。