[发明专利]基于改进人工势场法的人机安全避障路径规划方法

权利要求书

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

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

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

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

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

其中d_E-T表示目标与末端的距离误差，K₁,D₁为控制参数；

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

其中v_E-T表示目标与末端的速度误差，K₂,D₂为控制参数；

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

V_sum＝αV_target+βV_vel (6)

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

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

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

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

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

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

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

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

V_rpviot＝||V_rep||(cosγa+sinγv) (10)

步骤5-5：当θ＝0°，无法直接构建步骤5-2坐标系，则假设和V_rep存在一定误差θ，构建新的μ向量，r向量表示为(r_x,r_y,r_z)，定义：

η＝μ+λ (12)

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

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

μ＝[cos(λ+θ),sin(λ+θ),0]^T (16)

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

μ＝[0,sinθ,cosθ]^T (17)

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

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

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

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