[发明专利]一种超声检测中机械手防碰撞方法

权利要求书

1.一种超声检测中机械手防碰撞方法，其特征在于，包括以下步骤：

步骤一，建立用于超声检测的机械手各个关节的连杆坐标系，并建立相邻2个连杆坐标系之间的变换矩阵；

步骤二，根据相邻2个连杆坐标系之间的变换矩阵与机械手自身的连杆参数和关节变量，建立超声检测机械手的运动学方程；

步骤三，将检测系统中的设备分为静止体和碰撞体两类，并确定静止体的符号距离场，在碰撞体运动过程中，本方法采用的离散碰撞检测以时间步长Δt为间隔，对碰撞体模型与符号距离场进行碰撞检测；碰撞体上的点根据机械手运动学方程求解，将碰撞体上的点变换到符号距离场坐标系下的坐标点，通过搜索符号距离场确定变换后的坐标点与静止体的最短欧氏距离，并与安全阈值比较，根据比较结果确定是否执行相应的碰撞策略；

所述的步骤三中，在计算碰撞体运动过程中，以机械手碰撞体上的点在当前时刻以相应速率运动，并结合当前时刻的符号距离场值D(p)，计算下一时刻的近似符号距离场值，令机械手最大运动速率为v_max，取d(t_i)'＝d(t_i-1)-γ×Δt×v_max，其中γ为估计系数，当比较值δ＝d(t_i)'-σ满足δ＞0，其中σ为安全阈值，则机械手碰撞体上的点不与静止体发生碰撞，然后令近似距离场值d(t_i)'作为t_i时刻的距离场值d(t_i)，即d(t_i)＝d(t_i)'，继续进行再下一时刻的计算；否则，根据离散距离场检测进一步检测下一时刻准确的距离场值。

2.根据权利要求1所述的一种超声检测中机械手防碰撞方法，其特征在于，所述的步骤一中，相邻的两个连杆坐标系{i-1}和{i}之间的变换矩阵为：

其中，i＝1,2,...,6，l_i、α_i、d_i和θ_i分别为连杆坐标系{i}中的连杆长度、连杆扭角、连杆偏距和关节变量角，Rot(Z_i-1,θ_i)表示绕连杆坐标系{i-1}中的Z轴旋转θ_i角度的旋转矩阵，Rot(X_i,α_i)表示绕连杆坐标系{i}中的X轴旋转α_i角度的旋转矩阵，Trans(0,0,d_i)表示沿着X轴运动0距离，沿Y轴运动0距离，沿Z轴运动d_i距离的平移矩阵，Trans(l_i,0,0)表示沿着X轴运动l_i距离，沿Y轴运动0距离，沿Z轴运动0距离的平移矩阵。

3.根据权利要求2所述的一种超声检测中机械手防碰撞方法，其特征在于，所述的步骤二中，机械手的运动学方程为：

其中，

其中θ_j为连杆坐标系{j}中的关节变量角，j＝0,1,2,...,6。

4.根据权利要求1所述的一种超声检测中机械手防碰撞方法，其特征在于，所述的步骤三中，静止体的符号距离场通过下式表达：

D:

其中，p表示三维空间中的点，q表示静止体上的点，S表示闭合曲面即静止体的表面，表示三维空间，表示一维标量，sgn(p)表示当p在闭合曲面内部时取-1，在闭合曲面外面时取1，min{|p-q|}表示求出点p到闭合曲面s的最短距离。