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

说明书

本发明公开了一种融合动态系统与模型预测控制的机械臂实时避障方法，该方法结合动态系统和模型预测控制，能够处理动态环境中的实时避障问题。该方法可根据移动障碍物的位置信息实时规划出避障轨迹，并通过使用模型预测控制方法进行机械臂控制，考虑多项线性或非线性约束条件。本发明提供一种融合动态系统和模型预测控制的机械臂实时避障方法，提高了机械臂末端跟踪的精度，有效地提升了机械臂实时避障的能力。

技术领域

本发明属于机械臂实时避障技术领域，尤其是一种融合动态系统与模型预测控制的机械臂实时避障方法。

背景技术

近年来，由于人力成本逐渐增长等原因，机器人产业高速发展，整体规模持续增长，机器人代替劳动者从事繁重枯燥的工作，提升生产效率，改善生产环境。随着人机协作场景的广泛应用及生产需求的复杂化，机器人对动态环境的适应能力也变得更加重要。基于动态系统的机器人避障方法作为机器人学的一个研究重点，已经在人机协作、工业生产以及医疗康复等领域发挥了重要的作用。

然而，基于动态系统的普通机器人避障方法无法保证高维机械臂各连杆的防碰性，其生成的避障轨迹仅仅针对点状的移动机器人，若将该方法迁移到多自由度的机械臂上，必然会涉及到高维空间下的避障问题。模型预测控制是一种有效的优化方法，可同时对多个指定目标进行优化，并且能够满足多项线性或非线性约束，能够确保机械臂末端跟踪所规划出的避障轨迹的同时保持各连杆与移动障碍物保持一定的动态距离，防止发生碰撞。在目前以动态系统来对机械臂进行实时避障的方法中，还未能有效解决以采用模型预测控制的多约束满足性质来提升高维机械臂实时避障能力的问题。

发明内容

在基于动态系统的机器人实时避障中，为了解决由于机械臂多自由度而产生的高维避障问题，本发明提供一种融合动态系统与模型预测控制的机械臂实时避障方法，利用动态系统方法进行实时避障轨迹的规划，将机械臂各连杆与障碍物间的动态距离作为控制的约束条件，在机械臂末端跟踪规划轨迹的同时保持动态距离约束的满足，有效地提高了机械臂实时避障的有效性。

本发明解决其技术问题所采用的技术方案是：

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

步骤1)建立一个原始的动态系统DS模型作为机械臂末端在无障碍物环境下期望的运动轨迹方程，并给出相应的末端位置序列；所述的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上关节速度控制量的最优解计算关节速度最优控制量公式如下：