[发明专利]一种融合动态系统与模型预测控制的机械臂实时避障方法

权利要求书

1.一种融合动态系统与模型预测控制的机械臂实时避障方法，其特征在于：包括如下步骤：

步骤1)建立一个原始的动态系统DS模型作为机械臂末端在无障碍物环境下期望的运动轨迹方程，并给出相应的末端位置序列；其中ξ为三维空间的位置信息，为三维空间的速度信息；

步骤2)通过视觉传感器测得k时刻机械臂工作空间内最近球形障碍物的半径r_o和质心位置并计算其势函数并进行坐标系转换得到其梯度矩阵对原始DS进行调制，得到即机械臂末端在障碍物环境下的避障轨迹方程，并得到相应的末端位置序列其中k为离散时间序列，I表示单位矩阵；

步骤3)根据机械臂的运动学模型，计算机械臂各关节与障碍物的安全域指标中的最小值ESF^k以及机械臂的末端误差e^k，计算公式如下：

其中，d为机械臂关节个数，i＝1,2,…,d表示机械臂关节的标号，为k时刻i关节的三维空间位置，为k时刻i关节的三维空间速度，为k时刻障碍物质心的三维空间速度，由前后时刻质心的位移量除以时间间隔得到，为k时刻调制DS所规划出的机械臂末端下一个预到达路径点的位置信息，为k时刻机械臂末端的实际位置信息；

步骤4)设置关节域阈值将约束条件和其他约束条件以及末端误差e^k代入MPC求解器进行求解，使末端误差最小化的同时保持关节域约束，得到当前预测域NΔt上关节速度控制量的最优解计算关节速度最优控制量公式如下：

其中，e_k+i|k为末端误差基于k时刻在未来第k+i时刻的情况，ESF_k+i|k为机械臂各关节的最小安全域基于k时刻在未来第k+i的情况；预测域NΔt中，N为预测步长，Δt为预测模型的采样周期，其取值是依据实际控制需要所确定的；

步骤5)通过关节控制器将所得到的关节速度用于机械臂的控制，并按照设定的控制率等待到下一时刻；

重复执行步骤2)-5)，对各时刻下的机械臂进行避障规划与控制，直到机械臂的末端到达目标位置。

2.如权利要求1所述的一种融合动态系统与模型预测控制的机械臂实时避障方法，其特征在于：所述步骤2)中，所述障碍物的质心位置为在视觉传感器的相机坐标系下x，y，z坐标；障碍物质心的位置信息分别为k时刻障碍物质心的x，y，z坐标信息。

3.如权利要求1所述的一种融合动态系统与模型预测控制的机械臂实时避障方法，其特征在于：所述步骤2)中，所述视觉传感器是提供障碍物3D位置信息输出的RGB-D传感器。

4.如权利要求1所述的一种融合动态系统与模型预测控制的机械臂实时避障方法，其特征在于：所述步骤4)中，所述的关节域阈值为用户指定，其取值表示机械臂上点的和移动障碍物之间的动态距离。

5.如权利要求1所述的一种融合动态系统与模型预测控制的机械臂实时避障方法，其特征在于：所述步骤4)中，所述其他约束条件包括关节角度极限的约束、关节速度变化率的约束以及视觉传感器的遮挡约束。