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

摘要

本发明公开了一种基于时空相关跟踪策略的碰撞实时检测方法，包括步骤：一、确定装配物体与基准物体发生干涉的区域；二、计算装配物体M在装配过程中的非约束项变化向量；三、计算装配物体M在装配过程中的装配方向向量；四、计算装配物体M在装配过程中相对于的约束项变化向量；五、判断装配物体M的位姿向量和起始位置a的位姿向量的约束项的接近程度；六、确定装配物体M的装配运动量；七、更新装配物体M的位姿向量；八、判断在方向上投影长度是否非负；九、判断装配物体M的位姿向量和最终位置b的位姿向量的约束项的接近程度。本发明通过跟踪上一时间点活动对象位置，进行下一时刻碰撞位置实时检测和干涉剔除，实时性好，效率高。

主权项

1.一种基于时空相关跟踪策略的碰撞实时检测方法，其特征在于，该方法包括以下步骤：步骤一、确定装配物体与基准物体发生干涉的区域：利用计算机建立装配物体M与基准物体N之间的装配关系，确定装配物体M与基准物体N发生干涉的区域的起始位置a和最终位置b；以基准物体N的局部坐标系为基准坐标系，在基准坐标系中确定起始位置a的坐标为(a_x,a_y,a_z)，起始位置a的方位角为(a_α,a_β,a_γ)，a_α为a_x和基准坐标系原点之间的连线与基准坐标系x轴之间的夹角，a_β为a_y和基准坐标系原点之间的连线与基准坐标系y轴之间的夹角，a_γ为a_z和基准坐标系原点之间的连线与基准坐标系z轴之间的夹角，则起始位置a的位姿向量最终位置b的坐标为(b_x,b_y,b_z)，最终位置b的方位角为(b_α,b_β,b_γ)，b_α为b_x和基准坐标系原点之间的连线与基准坐标系x轴之间的夹角，b_β为b_y和基准坐标系原点之间的连线与基准坐标系y轴之间的夹角，b_γ为b_z和基准坐标系原点之间的连线与基准坐标系z轴之间的夹角，则最终位置b的位姿向量基准物体N的坐标为(n_x,n_y,n_z)，方位角为(n_α,n_β,n_γ)，n_α为n_x和基准坐标系原点之间的连线与基准坐标系x轴之间的夹角，n_β为n_y和基准坐标系原点之间的连线与基准坐标系y轴之间的夹角，n_γ为n_z和基准坐标系原点之间的连线与基准坐标系z轴之间的夹角，则基准物体N的位姿向量装配物体M在基准坐标系中的坐标为(c_x,c_y,c_z)，方位角为(c_α,c_β,c_γ)，c_α为c_x和基准坐标系原点之间的连线与基准坐标系x轴之间的夹角，c_β为c_y和基准坐标系原点之间的连线与基准坐标系y轴之间的夹角，c_γ为c_z和基准坐标系原点之间的连线与基准坐标系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₂的值小于预设的第二均值阈值时，则干涉剔除，装配过程结束；否则，循环步骤二。