基于改进人工势场法的人机安全避障路径规划方法与流程

本发明涉及工业环境下机械臂路径规划领域，尤其涉及一种基于改进的人工势场法的人机安全动态避障路径规划方法。

背景技术：

在传统的工业领域中，机械臂通常被用在静态环境下完成重复的喷涂、装配、焊接以及运输等任务，这些任务的基本操作是抓取对象。当操作人员进入工作环境参与机械臂任务或者机械臂操作环境成为动态时，机械臂需要实时检测变化，然后调整运动路径，保证人机安全并完成抓取任务。

人工势场法是一种典型的局部路径规划方法，其基本思想是在机器人的工作环境中构造一个人工势场，将不希望进入工作环境的障碍物定义为斥力场，操作目标定义为引力场，使势场中的机器人受到目标和障碍物的相互影响，完成避障和抓取任务。然而，人工势场法通常存在局部极小值问题，并且在多连杆结构的机械臂上应用困难。刘山等人通过直接在笛卡尔空间上构造吸引速度和排斥速度(刘山；谢龙.一种基于改进人工势场法的多自由度机械臂动态避障路径规划方法[P].中国专利：CN108326849A，2018-07-27)，避免了笛卡尔空间障碍物到机械臂关节空间的映射，使人工势场法能够适用于多自由度机械臂。李玉齐等人通过最小二叉堆排序提高A^*搜索最小估计代价的效率(李玉齐；林森阳；鲍海峰；王玉林；王博；肖洒.基于优化A^*的人工势场机械臂三维避障路径规划方法[P].中国专利：CN108274465A,2018-07-13),用A^*避免陷入局部极小值的问题并避免机械臂抖动。但以上两种方法，并未考虑机械臂和障碍物的移动速度关系，当障碍物移动速度过快时，机械臂可能无法及时躲避，在人机安全避障中存在一定局限性。

技术实现要素：

本发明克服现有技术的上述缺点，提出一种基于改进人工势场法的人机安全避障路径规划方法，根据障碍物与机械臂的速度关系，控制机械臂执行不同避障方式，保证了人机安全。

本发明针对不同的障碍物与机械臂的速度关系，分别建立斥力场向量函数，并利用Pivot算法改进斥力场向量函数。首先，本发明通过Kinetic相机，获取障碍物，目标点以及机械臂的位姿关系。其次，本发明通过在机械臂末端执行器构造边界球，判断障碍物是否进入边界球并执行避障任务，定义引力场向量函数，再根据障碍物与机械臂的速度关系定义斥力场向量函数，主要考虑以下情况：1)障碍物快速接近机械臂，当速度vH＞vrobot_endm/s，系统规划的新路径不能保证人体的安全，机械臂根据人的运动方向，通过Pivot算法优化斥力场向量函数；2)障碍物缓慢接近机械臂，当速度vH＜vrobot_endm/s，使用传统的斥力场向量函数。最后对引力以及斥力进行矢量合成，进行轨迹规划，生成避免碰撞的新路径，当机械臂陷入局部最小值时，引入时间因子，对机械臂产生一定扰动，快速脱离。如果人突然加速，应该对第一种情况做出反应。

基于改进人工势场法的人机安全避障路径规划方法，具体步骤如下：

步骤1:：基于Kinetic深度相机，将人物点云信息和3D机器人模型放在统一坐标系下，获取人-机械臂末端执行器位置关系D(E,O)，其中E表示机械臂末端，O表示障碍物。

步骤2：在末端执行器处定义边界球，构建无碰撞空间Ccollision_free，半径为R。

步骤3：获取目标-机械臂末端执行器位置关系D(E,T)以及速度关系V(E,T)，其中E表示机械臂末端，T表示目标点。

步骤3-1：计算基于目标位置的引力场向量函数：

其中dE-T表示目标与末端的距离误差，K1,D1为控制参数。

步骤3-2：计算基于目标速度的引力场向量函数：

其中vE-T表示目标与末端的速度误差，K2,D2为控制参数。

步骤3-3：合成引力场向量函数：

Vsum＝αVtarget+βVvel (6)

其中α，β为两种吸引力速度的合成权系数，Vamax为引力场向量函数能产生的机械臂末端执行器最大线速度。

步骤4：定义斥力场向量函数：

其中Vrmax为斥力场向量函数能产生的机械臂末端执行器最大线速度，ρ为障碍物与无碰撞空间中心距离。

步骤5：判断障碍物与机械臂的速度关系，当人缓慢接近机械臂，以速度vH＜vH_dangerm/s进入工作空间时，控制机械臂执行步骤3的斥力场向量函数。当人以速度vH＞vrobot_endm/s进入工作空间时，采用Pivot算法对斥力场向量函数进行优化，具体步骤如下：

步骤5-1：定义β＝arccos(a^Tr),其中a是的单位向量，r是Vrep的单位向量。

步骤5-2：构建坐标系(a,v,n)，n为垂直a-r平面的单位向量，v为垂直n-a的单位向量。

n＝a×r,

步骤5-3：当β＞90°，障碍物远离机械臂，斥力场向量函数保持不变，执行步骤3。

步骤5-4：当0°＜β＜90°，障碍物靠近机械臂，通过Pivot算法调整斥力场向量函数，新的斥力场向量函数表示为：

Vrpviot＝||Vrep||(cosγa+sinγv) (10)

步骤5-3：当β＝0°，无法直接构建步骤4-2坐标系，则假设和Vrep存在一定误差β，构建新的a向量，见附图5，r向量表示为(rx,ry,rz)，定义：

η＝α+λ (12)

若r向量不在x-y平面且不在z轴，则新的a向量为：

若r向量在x-y平面，则新的a向量为：

a＝[cos(λ+β),sin(λ+β),0]^T (16)

若r向量在z轴上，则新的a向量为：

a＝[0,sinβ,cosβ]^T (17)

步骤6：对引力场向量函数以及斥力场向量函数进行矢量合成，通过机械臂逆运动学计算，控制机械臂避障。

其中ut为时间因子，η为机械臂停留的时间，β为恢复扰动时间，若机械臂停留η时间，则陷入局部极小值，增大斥力场向量，保证机械臂继续避障。在T时间后，检测机械臂是否远离障碍物并继续向目标点靠近，若远离障碍物，则避障成功，否则返回步骤5，更新斥力场向量函数。

本发明的优点：本发明设计的基于改进人工势场法的人机安全避障路劲规划方法，一方面是通过kinetic深度相机获取人与机械臂点云信息，相比于直接通过单目或双目相机识别人物和机械臂，识别精度以及鲁棒性更高；另一方面，针对不同障碍物与人的速度关系，采取了多种保障安全的方式，构建不同的斥力场向量函数，提高了在工业环境下的人机安全性，此外通过引入时间因子，对斥力场向量函数加权计算，解决了局部最小值问题。

附图说明

图1是本发明的方法流程图

图2是本发明的人机位姿关系图

图3是本发明的机械臂末端执行器边界球图

图4是本发明的引力场说明图

图5是本发明的假设的坐标系图

具体实施方式

以下结合附图对本发明实例做进一步详述：

本发明的一种基于改进人工势场法的人机安全避障路劲规划方法，具体过程如下：

在对机械臂进行避障控制前，需要对Kinetic相机进行内参标定，使得获取点云信息能够达到更好的精度，控制对象为Uuniversal Robot公司生产的UR5六自由度关节机器人。相机与计算机通过USB连接传输数据，机械臂通过局域网与计算机相连接。

步骤1：基于Kinetic深度相机，将人物点云信息和3D机器人模型放在统一坐标系下，如图2所示。定义：

其中表示障碍物对于机械臂基坐标的相对位姿，根据机械臂的正运动学，可以求得机械臂末端执行器相对于机械臂基坐标的相对位姿定义障碍物与末端执行器在基坐标系下的位置关系：

步骤2：在机械臂末端执行器处人为设定一个边界半球，构建无碰撞空间Ccollision_free，如图3所示，半径为0.5m，当人或障碍物的距离小于0.5m时，系统开始计算斥力场向量并控制机械臂避障。

步骤3：定义引力场向量函数，引力场向量函数由2部分合成，如图4，首先同步骤1，获取目标物体与末端执行器在基坐标系下的位置关系D(E,T)以及速度关系V(E,T)。

首先计算基于目标位置的引力场向量函数：

其中dE-T表示目标与末端的距离误差，K1,D1为控制参数，分别取0.7,0.3，该速度能够保证机械臂末端向目标靠近。

再计算基于目标速度的引力场向量函数：

其中vE-T表示目标与末端的速度误差，K2,D2为控制参数，分别取0.7,0.3，该速度能够保证机械臂末端跟踪上动态目标。

最终合成引力场向量函数：

Vsum＝αVtarget+βVvel (6)

其中α，β为两种吸引力速度的合成权系数，Vamax为引力场向量函数能产生的机械臂末端执行器最大线速度。

步骤4：定义斥力场向量函数：

其中Vrmax为斥力场向量函数能产生的机械臂末端执行器最大线速度，α为正的系数，取5。

步骤5:当人或障碍物进入无碰撞空间后，判断速度vH＜vH-danger＝0.2m/s时，直接调用步骤3的斥力场向量，控制机械臂避障，判断速度vH＞vH-danger＝0.2m/s，若直接调用步骤3的斥力场向量，机械臂在排斥力的作用下，障碍物与机械臂将同向运动，两者仍然会有相撞的可能，需要优化斥力场向量函数，主要考虑3种情况。

首先定义斥力场向量Vrep与其随位置变化的向量的夹角β：

β＝arccos(a^Tr)，

构建坐标系(a,v,n)，其中n为垂直a-r平面的单位向量，v为垂直n-a的单位向量。当β≥90°时，表示障碍物远离末端执行器，机械臂可以执行步骤3的斥力场向量，沿斥力场向量原方向安全运动并且不发生碰撞。

Vrpviot＝Vrep (23)

当β＜90°时，表示障碍物靠近障碍物，为了有效地避开动态障碍物，需要构建新的斥力场向量函数，将新的斥力场向量投射在a-v平面上，新的函数表示为：

Vrpviot＝||Vrep||(cosγa+sinγv) (10)

其中γ表示Vrpviot相对于边界球球心的角度，表示斥力场向量允许变化的最大角度，c为正的常数，取5。

当β＝0°，斥力场向量平行于变化方向，无法构建a-r平面，则无法改变斥力场向量方向，为此需要假设存在一个很小的β，如图5所示。定义图中圆锥体底面的中点为(0,0,rz)，则r可以表示为(rx,ry,rz)，首先计算：

η＝α+λ (12)

其中α表示向量a和r在圆锥体地面的投影夹角，λ表示r在锥底的投影角度，若r不在x-y平面上，并且不在z轴上，则a向量可以表示为：

若r在x-y平面上，则a向量可以表示为：

a＝[cos(λ+β),sin(λ+β),0]^T (16)

若r在z轴上，则a向量可以表示为：

a＝[0,sinβ,cosβ]^T (17)

步骤6：对引力场向量函数和斥力场向量函数进行矢量合成，通过机械臂逆运动学计算，控制机械臂避障并完成任务。

若机械臂在空间中某点位置停留一定时间，则陷入局部最小值问题，通过引入时间因子ut对斥力场向量函数进行加权计算，快速脱离局部极小值。之后每0.01S采样位姿信息，检测机械臂末端是否远离障碍物，若远离障碍物并向靠近目标点，则避障成功，否则返回步骤5，更新斥力场向量函数。

步骤5所述的斥力场向量函数构造方法中，在障碍物速度过大，使用传统人工势场法无法避开时，通过引入Pivot算法，更新斥力场向量函数，进而合成新的势力场进行机械臂避障。

步骤5所述的避免机械臂陷入局部最小值问题中，通过引入时间因子，对斥力场向量函数进行加权计算，在机械臂陷入局部最小值时，增大斥力，让机械臂快速脱离。

本说明书实施例所述的内容仅仅是对发明构思实现形式的例举，本发明的保护范围不应当被视为仅限于实施例所陈述的具体形式，本发明的保护范围也及于本领域技术人员根据本发明构思所能够想到的等同技术手段。