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

技术特征：

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

步骤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＜vrobot_endm/s进入工作空间时，控制机械臂执行步骤3的斥力场向量函数。当人以速度vH＞vrobot_endm/s进入工作空间时，采用Pivot算法对斥力场向量函数进行优化，具体步骤如下：

步骤5-1：定义其中a是的单位向量，r是Vrep的单位向量；

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

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

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

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

步骤5-3：当β＝0°，无法直接构建步骤4-2坐标系，则假设和Vrep存在一定误差β，构建新的a向量，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，更新斥力场向量函数。

2.根据权利要求1所述的基于改进人工势场法的人机安全避障路径规划方法，其特征在于：步骤5所述的斥力场向量函数构造方法中，在障碍物速度过大，使用传统人工势场法无法避开时，通过引入Pivot算法，更新斥力场向量函数，进而合成新的势力场进行机械臂避障。

3.根据权利要求1所述的基于改进人工势场法的人机安全避障路径规划方法，其特征在于：步骤6所述的避免机械臂陷入局部最小值问题中，通过引入时间因子，对斥力场向量函数进行加权计算，在机械臂陷入局部最小值时，增大斥力，让机械臂快速脱离。