[发明专利]一种机械臂试探感知的避障方法

权利要求书

1.一种机械臂试探感知的避障方法，该方法包括下述步骤：

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

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

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

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

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

步骤6：建立虚拟障碍物空间：

用虚拟障碍物空间位置信息代替真实障碍物位置信息，分三种情况：

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

2)当障碍物出现在远离期望轨迹附近即非末端关节检测到碰撞，若能确定发生碰撞的关节位置，假定虚拟障碍物空间的圆心D在碰撞连杆长度的1/2中心处，然后以虚拟障碍物空间半径R₀在圆心D构建一个球体，这个球体就是虚拟障碍物空间；

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

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

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

2.根据权利要求1所述的避障方法，其特征在于，所述步骤3中对于实时碰撞检测的方式为借助于碰撞传感器或采用建立外力观测器模型方式进行碰撞检测。

3.根据权利要求2所述的避障方法，其特征在于，所述碰撞传感器为力矩传感器、加速度传感器或电子皮肤。

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