一种机械臂试探感知的避障方法与流程

本发明属于工业机器人路径规划领域，具体是一种机械臂试探感知的避障方法，该避障方法进行实时碰撞检测，对障碍物进行有效的避障规划，实现在非视觉情况下的避障反应。

背景技术：

随着机械臂逐渐扩大在各个领域的应用，面对复杂任务要求和未知作业环境，如何保证机械臂本体和环境中物体的安全性是研究的关键问题之一。当机械臂末端在任务轨迹上遇到障碍物时，传统的碰撞检测算法大多基于外部传感器，不仅增加了机械臂成本和控制系统的复杂性，而且在恶劣环境下(在浓雾、黑暗、水下等能见度低或红外、声波传播受限的恶劣环境下)会受到不同程度的干扰，甚至导致任务无法完成。

目前机械臂避碰策略主要包含二个阶段：碰撞检测阶段和安全机制阶段。在未知环境下，实时的碰撞检测及安全机制至关重要。现有碰撞检测研究主要有两种方法：主动避碰和被动避碰。主动避碰是指机械臂在非接触障碍物情况下提前感知障碍物进行避碰运动，该类避碰一般通过相机、雷达、激光测距等外部设备融合避碰算法。但是单一的主动避碰在浓雾、黑暗、水下等能见度低或红外、声波传播受限的恶劣环境下，分别会受到不同程度的影响从而不能满足避碰要求，所以需要被动避碰来解决上述问题。被动避碰是指机械臂在接触到障碍物后的碰撞检测以及采取相应安全反应策略。在该类避碰中，借助力矩传感器、智能皮肤等外部传感器检测外力，会增大系统复杂程度与成本。

目前碰后安全机制研究，主要有立即制动策略，切换控制模式，以及脱离碰撞区域策略三种。立即制动策略是指机械臂控制系统检测到碰撞信号后，机械臂执行紧急停止的策略。如果人机协作过程中机械臂与人发生挤压似的碰撞，虽然机械臂及时停止但是人处于挤压状态，存在安全隐患问题。切换控制模式是指碰撞发生时由位置控制模式切换为力控模式，从而进入零力模式，该方法虽然能够及时停止而且其柔性特性不会对人造成后续危害，但是存在容易造成系统的不稳定的问题。脱离碰撞区域策略是指碰撞后机械臂改变原来的运动轨迹，离开碰撞区域。这些研究的共同点是检测到碰撞后，机械臂所执行的任务会终止，待操作人员离开并确保安全后对机械臂重规划使其继续工作，不利于高效的自动化生产。

技术实现要素：

针对现有技术的不足，本发明提供一种机械臂试探感知的避障方法。该方法先实时获取机械臂当前是否发生碰撞，然后根据碰撞位置建立相应的虚拟碰撞空间，执行试探感知避障。

本发明解决所述技术问题的技术方案是，设计一种机械臂试探感知的避障方法，具体包括下述步骤：

步骤1：设定机械臂初始关节角度、目标位置和期望轨迹；

步骤2：建立机械臂的运动学模型；

步骤3：实时检测碰撞：实时检测机械臂是否发生碰撞，如果机械臂没有发生碰撞，将执行主任务(轨迹跟踪)，转到步骤4；如果机械臂发生碰撞，将执行从任务(避障运动)，转到步骤5；

步骤4：如果机械臂没有发生碰撞，机械臂将一直进行末端轨迹的跟踪，不考虑躲避障碍物，跟踪精度特别高，到达目标点后停止运动；

步骤5：如果机械臂发生碰撞，将控制机械臂原路径返回运动到机械臂之前任意tk时刻状态，从而迅速脱离碰撞区域，不会继续发生碰撞，再开始执行试探感知的避障策略，进入步骤6；

步骤6：建立虚拟障碍物空间。用虚拟障碍物空间位置信息代替真实障碍物位置信息，分三种情况：

1)当障碍物出现在末端期望轨迹附近即末端关节检测到碰撞，采用球体作为虚拟障碍物空间来表征真实障碍物信息，并将虚拟障碍物空间的圆心d设在碰撞瞬时机械臂末端的位置，然后以半径为r0在圆心d构建一个球体，这个球体就是虚拟障碍物空间；

2)当障碍物出现在远离期望轨迹附近即非末端关节检测到碰撞，若碰撞传感器确定了发生碰撞的关节，假定虚拟障碍物空间的圆心d在碰撞连杆长度的1/2中心处，然后以虚拟障碍物空间半径r0在圆心d构建一个球体，这个球体就是虚拟障碍物空间。利用d-h连杆变换矩阵可以确定虚拟障碍物空间的圆心d的空间坐标位置，如图2是当障碍物出现在远离期望轨迹附近时虚拟障碍物空间的圆心d的位置确定示例；

3)当障碍物出现在远离期望轨迹附近即非末端关节检测到碰撞，且不能确定发生碰撞的关节位置，此时撤离后停机处理，人为清除障碍物后继续工作；

步骤7：根据虚拟障碍物空间的位置信息，重规划路径，机械臂恢复运动，当机械臂与虚拟障碍物空间的最小距离d0小于某一距离阈值dm时，即||d0||≤dm，机械臂执行从任务(避障运动)，控制机械臂自运动进行避障，从而迅速脱离碰撞区域；当障碍物与机械臂的最短距离大于距离阈值时，即||d0||＞dm时，只进行末端轨迹的跟踪，不考虑躲避障碍物，跟踪精度特别高；

步骤8：如果在设定的距离阈值dm内第二次发生碰撞，说明实际障碍物较大超出初始虚拟障碍物区域，先迅速撤离再扩大虚拟障碍物空间的半径为r1＝μ×r0，重复步骤7；如果仍然相撞，同理重复，步骤7，继续扩大虚拟障碍物空间的半径ri＝μ×ri-1，这样进行多次试探性避障，最终到达目标点；但是当障碍物特别大的特殊情况，机械臂可能与扩大多次后的虚拟障碍物空间的距离d0＜0，超出避障极限，无法靠机械臂避障，此时需要撤离后停机处理，人为清除障碍物后再工作；其中μ为膨胀系数，μ＞1具体取值根据实验而定。

上述步骤3中对于实时碰撞检测可以借助于碰撞传感器，如力矩传感器、加速度传感器、电子皮肤等实现实时碰撞检测，也可以采用建立外力观测器模型方式进行碰撞检测。

上述步骤3采用外力观测器模型方式进行碰撞检测，首先要建立机械臂的基于电流的外力观测器模型，实时采集机械臂各关节电机的期望角度qd、期望角速度和期望角加速度(通常可从机械臂关节电机驱动器中直接读出)的信息，将采集到的电机端期望角度qd、期望角速度和期望角加速度的信息输入到外力观测器模型中，能得到监测值r来实时检测是否碰撞，监测值r为电机输出扭矩与电机驱动所需的扭矩的估计值及干扰项之差，并设定动态阈值rthres防止误检测；当无碰撞时，监测值趋近于0，转到步骤4；当机械臂与外界障碍物碰撞时，监测值会迅速以指数形式上升到||r||＞rthres(rthres＞0)，转到步骤5。

与现有技术相比，本发明有益效果在于：

1)本发明借助人对障碍物碰撞的自然反应机制，即人类受到外物刺激后，会由神经系统感知疼痛并将刺激信号传递到肌肉组织以迅速躲避外物，来解决协同机械臂的避障问题。本发明提供一种试探感知的避障策略，使机械臂具有人类躲避刺激的反应动作，即建立虚拟障碍物，对其进行多次试探性避障规划，并保证机械臂末端可以沿给定的期望轨迹运动，具有可靠性高和良好的实时动态避障特性。

2)现有机械臂碰撞后直接停止，不具备避障功能，本申请在机械臂确定碰撞后进行试探性尝试，能够继续执行避障任务，而不会立即停止，显著提高工作效率。

3)本申请在进行碰撞检测时可以助于碰撞传感器，如力矩传感器、加速度传感器等实时碰撞检测，并能发现碰撞发生位置，实时碰撞检测，对障碍物进行有效的避障规划策略，实现在非视觉情况下的避障反应，提高试探碰撞检测的适用范围。考虑到这些传感器检测到碰撞时，无法有效获得障碍物的位置和形状信息，本文以盲人触觉摸索环境为启发设计了一种机械臂试探感知的避障策略实现避障任务。

4)本申请中进行碰撞检测时借助外力观测器建模方式，可以在不依靠外部传感器下，机械臂仍能够继续执行避障任务，效率比用外力观测器的直接停止的情况要高，本申请在检测到碰撞后，根据碰撞情形会继续工作，完成达到目标点的工作；与有外部传感器比，成本更低，在对于不能使用视觉传感器的情况下，本申请仍能很好地完成避障，能继续工作。同时由于不使用外部传感器，减少了机器人的成本和控制系统的复杂性。

5)本申请中借助外力观测器建模方式能解决外部传感器失效情况下的避障任务，如外部传感器在浓雾、黑暗、水下等能见度低或红外、声波传播受限的恶劣环境下，分别会受到不同程度的影响从而不能满足避碰要求。解决了现有的避碰规划中依赖外部传感器，效率低和不能实时避碰等缺点。

附图说明

图1为本发明的ur5的d-h坐标系；

图2为本发明的当障碍物出现在远离期望轨迹附近时虚拟障碍物空间的圆心d的位置确定示例；

图3为本发明的方法流程图；

图4为本发明的机械臂避障过程的路径序列；其中，图4(a)机械臂的初始位置，并且开始执行主任务，图4(b)机械臂撞上六维力传感器并检测到意外碰撞停止运动，图4(c)控制机械臂原路径返回运动到机械臂之前tk时刻状态，并开始执行从任务，图4(d)机械臂执行从任务，图4(e)机械臂执行从任务结束，继续执行主任务，图4(f)机械臂到达目标位置；

图5为本发明的外力观测器的监测值r；

图6为本发明的碰撞发生时六维力传感器检测到的碰撞力大小。

具体实施方式

下面给出本发明的具体实施例。具体实施例仅用于进一步详细说明本发明，不限制本申请权利要求的保护范围。

本发明提供一种机械臂试探感知的避障方法，具体包括下述步骤：

步骤1：输入初始关节角度、目标位置、期望轨迹；

步骤2：建立机械臂的运动学模型；

步骤3：由碰撞传感器实时检测机械臂是否发生碰撞，如果机械臂没有发生碰撞，将执行主任务(轨迹跟踪)，转到步骤4；如果机械臂发生碰撞，将执行从任务(避障运动)，转到步骤5。

步骤4：如果机械臂没有发生碰撞，机械臂将一直进行末端轨迹的跟踪，不考虑躲避障碍物，跟踪精度特别高，到达目标点后停止运动。

步骤5：如果机械臂发生碰撞，将控制机械臂原路径返回运动到机械臂之前tk时刻状态，从而迅速脱离碰撞区域，不会继续发生碰撞，再开始执行试探感知的避障策略。

步骤6：建立虚拟障碍物空间。用虚拟障碍物空间位置信息代替真实障碍物位置信息。分三种情况：1)当障碍物出现在末端期望轨迹附近即末端关节检测到碰撞，采用球体作为虚拟障碍物空间来表征真实障碍物信息，并将碰撞中心点d设在碰撞瞬时机械臂末端的位置，然后以半径为r0在碰撞中心点d构建一个球体，这个球体就是虚拟障碍物空间。2)当障碍物出现在远离期望轨迹附近即非末端关节检测到碰撞，若碰撞传感器确定了发生碰撞的关节，假定虚拟障碍物空间的圆心d在碰撞连杆长度的1/2中心处，然后以虚拟障碍物空间半径r0在圆心d构建一个球体，这个球体就是虚拟障碍物空间。利用d-h连杆变换矩阵可以确定虚拟障碍物空间的圆心d的空间坐标位置，如图2是当障碍物出现在远离期望轨迹附近时虚拟障碍物空间的圆心d的位置确定示例。3)当障碍物出现在远离期望轨迹附近即非末端关节检测到碰撞，且不能确定发生碰撞的关节位置，此时撤离后停机处理，人为清除障碍物后继续工作。

步骤7：根据虚拟障碍物空间的位置信息，重规划路径，机械臂恢复运动。当机械臂与虚拟障碍物空间的最小距离小于某一距离阈值时，即||d0||≤dm，机械臂执行从任务(避障运动)，控制机械臂自运动进行避障，从而迅速脱离碰撞区域。当障碍物与机械臂的最短距离大于距离阈值时，即||d0||＞dm时，只进行末端轨迹的跟踪，不考虑躲避障碍物，跟踪精度特别高；

步骤8：如果在设定的距离阈值dm内第二次发生碰撞，说明实际障碍物较大超出初始虚拟障碍物区域，先迅速撤离再扩大虚拟障碍物空间的半径为r1＝μ×r0，重复步骤7。如果仍然相撞，同理重复，步骤7，继续扩大虚拟障碍物空间的半径ri＝μ×ri-1，这样进行多次试探性避障，最终到达目标点。但是当障碍物特别大的特殊情况，机械臂可能与扩大多次后的虚拟障碍物空间的距离d0＜0，超出避障极限，无法靠机械臂避障，此时需要撤离后停机处理，人为清除障碍物后再工作。其中μ为膨胀系数，μ＞1具体取值根据实验而定。

实施例

下面以具体实施例来验证本发明，该具体实施例选ur机械臂作为对象，但实施对象不限于此。

步骤1：输入初始关节角度、目标位置、期望轨迹；

步骤2：建立机械臂的运动学模型，本文以ur5机器人为主要研究对象。ur5是一款具有6个旋转关节的串联机器人，为清晰地表明该机器人各连杆之间的位姿关系，可采用d-h坐标变换法进行建模，机器人d-h坐标系如图1所示。图1中，关节的编号从0开始计数，即关节0就是基座。xi、yi、zi分别是关节i在空间中的坐标系。

工业机器人的连杆参数分为连杆的几何参数和连杆的关系参数，主要包括连杆长度ai，连杆扭角αi，连杆距离di和连杆转角θi。得到连杆的变化通式为：

其中，c＝cos，s＝sin，t是变换矩阵；连杆长度ai表示沿着xi轴从xi轴与zi-1轴的交点到oi的距离；连杆距离di表示沿着zi-1轴从oi-1到xi轴与zi-1轴的交点的距离；连杆扭角αi表示绕xi轴从zi-1轴到zi的角度；连杆转角θi表示绕zi-1轴从xi-1到xi的角度。

根据上述表达，机器人末端所在坐标系相对于基坐标系可以表示为：

式中⁰rn为旋转矩阵，⁰pn为位置矢量。px、py、pz是位置矢量的三个分量，r11等是旋转矩阵的6个分量之一。

步骤3：由碰撞传感器实时检测机械臂是否发生碰撞，如果机械臂没有发生碰撞，将执行主任务(轨迹跟踪)，转到步骤4；如果机械臂发生碰撞，将执行从任务(避障运动)，转到步骤5。

步骤4：如果机械臂没有发生碰撞，机械臂将一直进行末端轨迹的跟踪，不考虑躲避障碍物，跟踪精度特别高，到达目标点后停止运动。轨迹跟踪公式为：

其中xd为末端期望角度，为末端期望角速度，x为末端实际位置，k是一个正定矩阵，本实验k＝{130；013}一个矩阵，k的正确选取可以保证末端的跟踪精度特别高。j∈rm×n表示机械臂的雅可比矩阵，符号“+”表示对矩阵求广义逆。

步骤5：如果机械臂发生碰撞，控制机械臂原路径返回运动到机械臂之前tk时刻状态，从而迅速脱离碰撞区域，不会继续发生碰撞，公式如下：

其中代表tk时刻下的关节速度控制指令，qr(tc)代表碰撞时刻关节位置。

步骤6：建立虚拟障碍物空间。用虚拟障碍物空间位置信息代替真实障碍物位置信息。分三种情况：1)当障碍物出现在末端期望轨迹附近即末端关节检测到碰撞，采用球体作为虚拟障碍物空间来表征真实障碍物信息，并将碰撞中心点d设在碰撞瞬时机械臂末端的位置，然后以半径为r0在碰撞中心点d构建一个球体，这个球体就是虚拟障碍物空间。2)当障碍物出现在远离期望轨迹附近即非末端关节检测到碰撞，若碰撞传感器确定了发生碰撞的关节，假定虚拟障碍物空间的圆心d在碰撞连杆长度的1/2中心处，然后以虚拟障碍物空间半径r0在圆心d构建一个球体，这个球体就是虚拟障碍物空间。利用d-h连杆变换矩阵可以确定虚拟障碍物空间的圆心d的空间坐标位置，如图2是当障碍物出现在远离期望轨迹附近时虚拟障碍物空间的圆心d的位置确定示例。3)当障碍物出现在远离期望轨迹附近即非末端关节检测到碰撞，且不能确定发生碰撞的关节位置，此时撤离后停机处理，人为清除障碍物后继续工作。

步骤7：根据虚拟障碍物空间的位置信息，重规划路径，机械臂恢复运动。

执行避障，其公式为：

其中

式中，表示关节速度向量，j∈rm×n表示机械臂的雅可比矩阵，j0∈rm×n表示最靠近障碍的标志点运动的雅可比矩阵，表示标志点运动的雅可比矩阵，符号“+”表示对矩阵求广义逆。为标志点的避障速度值。α为任务转化变量，切换主从任务。s为速度转化变量，其中s0＜1，机械臂进入距离阈值内进行降速运动，减少对未知障碍物的二次伤害。s和α都随着||d0||的变化而变化。vm为标志点能产生的最大避障速度值。xd为末端期望角度，x为末端实际位置，k是一个正定矩阵，k的正确选取可以保证末端的跟踪精度特别高。d0为标志点与障碍物的距离，dm为给定的距离阈值。n0为单位向量。

当机械臂与虚拟障碍物空间的最小距离小于某一距离阈值时，即||d0||≤dm，机械臂执行从任务(避障运动)，控制机械臂自运动进行避障，从而迅速脱离碰撞区域，避障任务公式为：

当障碍物与机械臂的最短距离大于距离阈值时，即||d0||＞dm时，只进行主任务(末端轨迹的跟踪)，不考虑躲避障碍物，跟踪精度特别高，轨迹跟踪公式为：

步骤8：如果在设定的距离阈值dm内第二次发生碰撞，说明实际障碍物较大超出初始虚拟障碍物区域，先迅速撤离再扩大虚拟障碍物空间的半径为r1＝μ×r0，重复步骤7。如果仍然相撞，继续扩大虚拟障碍物空间的半径ri＝μ×ri-1，同理重复，步骤7，这样进行多次试探性避障，最终到达目标点。但是当障碍物特别大的特殊情况，机械臂可能与扩大多次后的虚拟障碍物空间的距离d0＜0，超出避障极限，无法靠机械臂避障，此时需要撤离后停机处理，人为清除障碍物后再工作。其中μ为膨胀系数，μ＞1具体取值根据实验而定。其整个动态避障流程图如图3所示。

实施例2

本实施例研究对象及过程同实施例1，不同之处在于，本申请采用外观测器建模方式进行碰撞检测，

建立机械臂的基于电流的外力观测器模型。机械臂的关节输出扭矩由电机扭矩τm、外力矩τext和干扰项ρ三部分组成：

τall＝τm+τext+ρ(7)

其中干扰项ρ包括摩擦力矩τf和柔性元件承受扭矩τfle，关节柔性主要来自于谐波减速器，它使关节刚度降低，具备了柔性特征。

已知期望轨迹，根据机器人各关节的期望角度、期望角速度和期望角加速度计算动态系统的估计扭矩。τm表示电机驱动所需的扭矩，其估计值表示为：

式中：qd、分别为机器人各关节的期望角度、期望角速度和期望角加速度；m(q)为惯性矩阵，且为对称正定矩阵；项包含哥氏力和离心力，g(q)为重力矩。

根据直流无刷电机的电气特性，电流与转矩关系是成比例的，所以实际电流能估计关节的转矩，则电机输出扭矩可以估计为：

外力距τext∈rⁿ(rⁿ实矩阵，τext∈rⁿ，表示τext是正定矩阵。)是由外力fext引起，它们的关系式为：

τext＝jg^t(q)×fext(10)

其中，kr：扭矩常数。ir：电机实际电流值。jg：几何雅克比。

由(3)、(4)、(5)和(6)可设计一个虚拟传感器来识别是否发生碰撞，定义一个n×1监测值r(监测值r用来检测是否发生碰撞，表示电机输出扭矩与电机驱动所需的扭矩的估计值及干扰项之差。)为：

监测值r为无量纲值。在低速轻载情况下，干扰项ρ数值较小，对外力测的影响较小，近似为常系数。并且后续的碰撞控制策略只要求检测出是否碰撞，不需要对碰撞力矩进行精确计算，所以这里可以忽略干扰项ρ不影响碰撞识别。

实时采集机械臂各关节电机的期望角度qd、期望角速度和期望角加速度(通常可从机械臂关节电机驱动器中直接读出)的信息；

将采集到的电机端期望角度qd、期望角速度和期望角加速度的信息输入到外力观测器模型中，能得到监测值r来实时检测是否碰撞并设定动态阈值防止误检测。当无碰撞时，监测值趋近于0，转到步骤4；当机械臂与外界障碍物碰撞时，监测值会迅速以指数形式上升到||r||＞||rthres||，转到步骤5。

采用六轴机械臂平台对本发明机械臂安全碰撞策略进行验证。六轴机械臂平台的机械臂有六个转动关节，参见图1，从底座到终端依次为关节一到六，关节一与基座链接，关节二直接与关节一相连，关节三通过杆长为425mm的连杆与关节二链接，再通过杆长为392mm的连杆与关节四相连，关节四与关节五直接连接，关节五与关节六直接相连。关节一、二的质量均为3.4kg，关节三的质量为2.65kg，关节四、五、六质量均为1.57kg。其中将底座第一、第五与末端关节位置固定，仅利用第二、三和四关节的数据进行碰撞检测，以连杆一、连杆二和连杆三来表示，作为简化三连杆机械臂来进行验证试验。

本实施例采用的三自由度平面冗余度机械臂的避障策略过程为：机械臂开始关节角度为(0°，-145°，-65°，-75°，0°，0°)，终止关节角度为(0°，-81.2°，-154.8°，-131.89°，0°，0°)。末端期望轨迹是起始点为(767.89,90)，终点为(141.5,90)的直线，其无障碍的运行时间为10s，机器人末端做匀速直线运动。而此刻人为手持六维力传感器出现在该路径上。实验结果的屏幕截图如图4所示，整个运动过程按照从左到右，从上到下的顺序。图4(a)机械臂的初始位置，并且开始执行主任务，图4(b)机械臂撞上六维力传感器并检测到意外碰撞停止运动，图4(c)控制机械臂原路径返回运动到机械臂之前tk时刻状态，并开始执行从任务，图4(d)机械臂执行从任务，图4(e)机械臂执行从任务结束，继续执行主任务，图4(f)机械臂到达目标位置。实际运行时间为24s，在第4s时撞上障碍物。其中图5为外力观测器的监测值，图6为碰撞发生时六维力传感器检测到的碰撞力大小。

人为手持六维力传感器在大约7s的时候撞上机械臂，三个监测值同时有反应说明，碰撞位置在第三个杆件上，三个关节上的监测值分别为7.84，13.25，3.2，都达到安全阈值。当机械臂没有发生碰撞，三个关节上的监测值几乎为零。这里六维力传感器用来查看碰撞力大小，从而来调整安全阈值大小。

实验结果表明本发明设计的基于外力观测器的机械臂安全碰撞策略是可行有效的。

本发明未述及之处适用于现有技术。