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

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

Man-Machine Safety Obstacle Avoidance Path Planning Method Based on Improved Artificial Potential Field Method

【技术实现步骤摘要】
基于改进人工势场法的人机安全避障路径规划方法
本专利技术涉及工业环境下机械臂路径规划领域，尤其涉及一种基于改进的人工势场法的人机安全动态避障路径规划方法。
技术介绍
在传统的工业领域中，机械臂通常被用在静态环境下完成重复的喷涂、装配、焊接以及运输等任务，这些任务的基本操作是抓取对象。当操作人员进入工作环境参与机械臂任务或者机械臂操作环境成为动态时，机械臂需要实时检测变化，然后调整运动路径，保证人机安全并完成抓取任务。人工势场法是一种典型的局部路径规划方法，其基本思想是在机器人的工作环境中构造一个人工势场，将不希望进入工作环境的障碍物定义为斥力场，操作目标定义为引力场，使势场中的机器人受到目标和障碍物的相互影响，完成避障和抓取任务。然而，人工势场法通常存在局部极小值问题，并且在多连杆结构的机械臂上应用困难。刘山等人通过直接在笛卡尔空间上构造吸引速度和排斥速度(刘山；谢龙.一种基于改进人工势场法的多自由度机械臂动态避障路径规划方法[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(aTr),其中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相机进行内参标定，使得获取点云信息能够达到更好的精度，控制对象为UuniversalRobot公司生产的UR5六自由度关节机器人。相机与计算机通过USB连接传输数据，机械臂通过局域网与计算机相连接。步骤1：基于Kinetic深度相机，将人物点云信息和3D机器人模型放在统一坐标系下，如图2所示。定义：其中表示障碍物对于机械臂基坐标的相对位姿，根据机械臂的正运动学，可以求得机械臂末端执行器相对于机械臂基坐标的相对位姿定义障碍物与末端执行器在基坐标系下的位置关系：步骤2：在机械臂末端执行器处人为设定一个边界半球，构建无碰撞空间Cc本文档来自技高网...

【技术保护点】
1.基于改进人工势场法的人机安全避障路径规划方法，具体步骤如下：步骤1:：基于Kinetic深度相机，将人物点云信息和3D机器人模型放在统一坐标系下，获取人‑机械臂末端执行器位置关系D(E,O)，其中E表示机械臂末端，O表示障碍物；步骤2：在末端执行器处定义边界球，构建无碰撞空间Ccollision_free，半径为R；步骤3：获取目标‑机械臂末端执行器位置关系D(E,T)以及速度关系V(E,T)，其中E表示机械臂末端，T表示目标点；步骤3‑1：计算基于目标位置的引力场向量函数：

【技术特征摘要】
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°，障碍物远离机械臂...

【专利技术属性】
技术研发人员：欧林林，吴加鑫，禹鑫燚，金燕芳，来磊，
申请(专利权)人：浙江工业大学，
类型：发明
国别省市：浙江,33