[发明专利]一种基于时空相关跟踪策略的碰撞实时检测方法

权利要求书

1.一种基于时空相关跟踪策略的碰撞实时检测方法，其特征在于，该方法包括以下步骤：

步骤一、确定装配物体与基准物体发生干涉的区域：利用计算机建立装配物体M与基准物体N之间的装配关系，确定装配物体M与基准物体N发生干涉的区域的起始位置a和最终位置b；

以基准物体N的局部坐标系为基准坐标系，在基准坐标系中确定起始位置a的坐标为(a_x,a_y,a_z)，起始位置a的方位角为(a_α,a_β,a_γ)，a_α为起始位置a的坐标点和基准坐标系原点之间的连线与基准坐标系x轴之间的夹角，a_β为起始位置a的坐标点和基准坐标系原点之间的连线与基准坐标系y轴之间的夹角，a_γ为起始位置a的坐标点和基准坐标系原点之间的连线与基准坐标系z轴之间的夹角，则起始位置a的位姿向量最终位置b的坐标为(b_x,b_y,b_z)，最终位置b的方位角为(b_α,b_β,b_γ)，b_α为最终位置b的坐标点和基准坐标系原点之间的连线与基准坐标系x轴之间的夹角，b_β为最终位置b的坐标点和基准坐标系原点之间的连线与基准坐标系y轴之间的夹角，b_γ为最终位置b的坐标点和基准坐标系原点之间的连线与基准坐标系z轴之间的夹角，则最终位置b的位姿向量

基准物体N的坐标为(n_x,n_y,n_z)，方位角为(n_α,n_β,n_γ)，n_α为基准物体N的坐标点和基准坐标系原点之间的连线与基准坐标系x轴之间的夹角，n_β为基准物体N的坐标点和基准坐标系原点之间的连线与基准坐标系y轴之间的夹角，n_γ为基准物体N的坐标点和基准坐标系原点之间的连线与基准坐标系z轴之间的夹角，则基准物体N的位姿向量装配物体M在基准坐标系中的坐标为(c_x,c_y,c_z)，方位角为(c_α,c_β,c_γ)，c_α为装配物体M的坐标点和基准坐标系原点之间的连线与基准坐标系x轴之间的夹角，c_β为装配物体M的坐标点和基准坐标系原点之间的连线与基准坐标系y轴之间的夹角，c_γ为装配物体M的坐标点和基准坐标系原点之间的连线与基准坐标系z轴之间的夹角，则装配物体M的位姿向量

步骤二、根据公式计算装配物体M在装配过程中的非约束项变化向量其中，为中间向量且为取反运算，为装配物体M在装配过程中的运动约束向量且为装配物体M在最终位置b处的位置约束向量且·|·为去卷积运算；

步骤三、根据公式计算装配物体M在装配过程中的装配方向向量且其中，为位姿变化向量的单位化向量且*为卷积运算符合；

步骤四、根据公式计算装配物体M在装配过程中相对于的约束项变化向量其中，为装配物体M在装配过程中的装配方向向量的单位化向量；

步骤五、判断装配物体M的位姿向量和起始位置a的位姿向量的约束项的接近程度：根据公式计算装配物体M的位姿向量和起始位置a的位姿向量的向量差其中，根据公式计算装配物体M的位姿向量和起始位置a的位姿向量的约束项的接近向量其中，为装配物体M在起始位置a处的位置约束向量且即

根据公式计算装配物体M的位姿向量和起始位置a的位姿向量的约束项的接近向量的均值f₁，当f₁的值小于预设的第一均值阈值时，装配物体M的位置就是装配干涉的初始位置，执行步骤六；否则，装配物体M停止移动；

步骤六、确定装配物体M的装配运动量：根据公式计算装配物体M在装配过程中的非约束项单位变化向量其中，为装配物体M的运动步长向量且即

根据公式计算装配物体M在装配过程中的约束项单位变化向量其中，

在方向上投影长度为j且则为装配物体M在装配过程中的约束项实际变化量，则装配物体M的装配运动量为

步骤七、更新装配物体M的位姿向量：利用装配物体M的装配运动量对装配物体M的位姿向量进行更新，获得装配物体M的更新位姿向量

步骤八、判断在方向上投影长度是否非负：当步骤六中的j≥0时，执行步骤九；当步骤六中的j<0时，循环步骤二，其中，每次循环中步骤五中的装配物体M的位姿向量均采用装配物体M的更新位姿向量

步骤九、判断装配物体M的位姿向量和最终位置b的位姿向量的约束项的接近程度：根据公式计算装配物体M的位姿向量和最终位置b的位姿向量的向量差其中，根据公式计算装配物体M的位姿向量和最终位置b的位姿向量的约束项的接近向量即

根据公式计算装配物体M的位姿向量和最终位置b的位姿向量的约束项的接近向量的均值f₂，当f₂的值小于预设的第二均值阈值时，则干涉剔除，装配过程结束；否则，循环步骤二。