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所述的避免机械臂陷入局部最小值问题中,通过引入时间因子,对斥力场向量函数进行加权计算,在机械臂陷入局部最小值时,增大斥力,让机械臂快速脱离。