1.一种融合动态系统与模型预测控制的机械臂实时避障方法,其特征在于:包括如下步骤:
步骤1)建立一个原始的动态系统DS模型 作为机械臂末端在无障碍物环境下期望的运动轨迹方程,并给出相应的末端位置序列;其中ξ为三维空间的位置信息,为三维空间的速度信息;
步骤2)通过视觉传感器测得k时刻机械臂工作空间内最近球形障碍物的半径ro和质心位置 并计算其势函数 并进行坐标系转换 得到其梯度矩阵 对原始DS进行调制,得到 即机械臂末端在障碍物环境下的避障轨迹方程,并得到相应的末端位置序列 其中k为离散时间序列,I表示单位矩阵;
步骤3)根据机械臂的运动学模型,计算机械臂各关节与障碍物的安全域指标中的最小k k
值ESF以及机械臂的末端误差e,计算公式如下:其中,d为机械臂关节个数,i=1,2,…,d表示机械臂关节的标号, 为k时刻i关节的三维空间位置, 为k时刻i关节的三维空间速度, 为k时刻障碍物质心的三维空间速度,由前后时刻质心的位移量除以时间间隔得到, 为k时刻调制DS所规划出的机械臂末端下一个预到达路径点的位置信息, 为k时刻机械臂末端的实际位置信息;
k
步骤4)设置关节域阈值 将该约束 和其他约束条件以及末端误差e代入MPC求解器进行求解,使末端误差最小化的同时保持关节域约束,得到当前预测域NΔt上关节速度控制量的最优解 计算关节速度最优控制量 公式如下:其中,ek+i|k为末端误差基于k时刻在未来第k+i时刻的情况,ESFk+i|k为机械臂各关节的最小安全域基于k时刻在未来第k+i的情况;
步骤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)中,所述其他约束条件包括关节角度极限的约束、关节速度变化率的约束以及视觉传感器的遮挡约束。
6.如权利要求1所述的一种融合动态系统与模型预测控制的机械臂实时避障方法,其特征在于:所述步骤4)中,所述的预测域NΔt中,N为预测步长,Δt为预测模型的采样周期,其取值是依据实际控制需要所确定的。