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

摘要

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

主权项

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_H＜v_{robot_end}m/s进入工作空间时，控制机械臂执行步骤3的斥力场向量函数。当人以速度v_H＞v_{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‑3：当β＝0°，无法直接构建步骤4‑2坐标系，则假设和V_rep存在一定误差β，构建新的a向量，r向量表示为(r_x,r_y,r_z)，定义：η＝α+λ            (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：对引力场向量函数以及斥力场向量函数进行矢量合成，通过机械臂逆运动学计算，控制机械臂避障。其中u_t为时间因子，η为机械臂停留的时间，β为恢复扰动时间，若机械臂停留η时间，则陷入局部极小值，增大斥力场向量，保证机械臂继续避障。在T时间后，检测机械臂是否远离障碍物并继续向目标点靠近，若远离障碍物，则避障成功，否则返回步骤5，更新斥力场向量函数。