买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：西北工业大学;西北工业大学太仓长三角研究院

摘要：本发明公开了一种基于数字仿真与OBB包围盒的活动线缆碰撞检测方法，首先建立一根活动线缆力学模型；接下来求解一根活动线缆位姿；然后预检测两根活动线缆是否可能发生碰撞；之后建立两根活动线缆的包围盒模型；最后判断两根活动线缆的包围盒是否发生相交。本发明采用Kirchhoff弹性细杆理论建立活动线缆的力学模型，可以较好的体现线缆的弯曲、扭转特性。

主权项：1.一种基于数字仿真与OBB包围盒的活动线缆碰撞检测方法，其特征在于，包括如下步骤：步骤1：建立一根活动线缆力学模型；活动线缆力学模型建立在Kirchhoff弹性细杆理论的基础上；活动线缆力学模型由一组微分方程组成，用于描述在不同载荷和约束条件下，细杆的弹性和塑性响应；该步骤用到的变量定义如下：PO：活动线缆固定端端点；Pl：活动线缆活动端端点；L：活动线缆长度；P-xyz：以线缆微元段截面中心为原点，微元段副法向、切向和法向为轴向的Frenet坐标系；O-ξηζ：以O为原点建立笛卡尔三维惯性坐标系，为全局坐标系；Fi：线缆微元段截面受到邻近截面作用内力主矢在i轴方向的分力；s：线缆微元段截面上的弧坐标，即从PO开始的弧长距离；T：线缆微元段截面切线方向的单位向量；ωi：线缆微元段截面在i轴方向的弯扭度；ki：线缆微元段截面绕i轴的抗弯刚度抗扭刚度；E：活动线缆的杨氏模量；G：活动线缆的剪切模量；Ji：线缆微元段截面对i轴的惯性矩；dp：线缆微元段截面直径；q1,q2,q3,q4：线缆微元段截面的欧拉四元数；步骤1-1：建立全局坐标系O-ξηζ，输入待建模活动线缆的长度L、固定端端点坐标PO、活动端端点坐标Pl、杨氏模量E、剪切模量G、截面直径dp；步骤1-2：将活动线缆按长度平均划分为10个微元段；步骤1-3：根据公式代入活动线缆的E、G、dp计算出活动线缆微元段的抗弯刚度抗扭刚度ki；步骤1-4：用欧拉四元数q1,q2,q3,q4表示弯扭度ωi；弯扭度和欧拉四元数的代数关系为： 其中步骤1-5：建立关于线缆微元段的Kirchhoff受力平衡方程组： 该方程组由Frenet坐标系下三个轴向上的力的平衡与力矩平衡构成；平衡方程组和方程构成了本质上为7个未知数Fx、Fy、Fz、q1、q2、q3、q4和7个方程的微分方程组；步骤1-6：按照步骤1-3～步骤1-5的方式建立10个线缆微元段的Kirchhoff受力平衡方程组；步骤1-7：增加线缆的几何约束；由于活动线缆的长度和活动点坐标为已知量，需要增加4个几何约束方程，包括： 至此，单根活动线缆的70个未知量，74个方程的超定微分方程组建立完毕，该方程组即为活动线缆力学模型；步骤2：求解一根活动线缆位姿；求解活动线缆位姿即求解步骤1建立的超定方程组，再通过解得的已知量计算活动线缆每个离散点的全局坐标；该步骤用到的变量定义如下：Cn：第n阶导的权系数矩阵；N：离散点的数量；si：活动线缆上第i个的点的弧坐标；步骤2-1：将方程组的微分项离散为权系数矩阵第n阶导的元素与函数值的乘积的和；n阶导数表示为其中权系数矩阵的计算由离散点的选择方式决定，使用Chebyshev方法选取离散点，即在离散化处理后，方程组中的微分项都被替换成了由离散点构成的一阶多项式的形式；步骤2-2：将求解超定代数方程组的问题转化为优化问题；设超定代数方程组中第j个方程的表达式为fjx＝0，则代数方程组为fx＝f1x,f2x,…,ftxT＝0，fx是方程组的残差形式；优化问题的目标函数定义为： 步骤2-3：运用Levenberg-Marquard法求解非线性优化问题；输入包括向量函数fx＝f1x,f2x,…,ftxT，解向量的初值x0，以及收敛条件εx,εg，输出包括超定代数方程组的解向量；步骤2-4：利用方程组的解计算活动线缆离散点的全局坐标；活动线缆离散点坐标与欧拉四元数有关，计算公式为： 活动线缆上每个离散点的全局坐标已经解出，在活动平面的一个转动角度下，不考虑碰撞情景的单根线缆位姿仿真已完成；步骤3：预检测两根活动线缆是否可能发生碰撞；判断n根活动线缆是否发生碰撞，等价于判断n根活动线缆中所有的线缆对中，是否存在发生碰撞的线缆对，两根线缆组成一个线缆对，既n根线缆中存在Cn2个不同的线缆对；若不存在发生碰撞的线缆对，则判定n根活动线缆未发生碰撞；若存在任意发生碰撞的线缆对，则判定n根活动线缆发生碰撞；该步骤用到的变量定义如下：l1：活动线缆1的长度；l2：活动线缆2的长度；dm：活动线缆1活动端坐标与活动线缆2活动端坐标的距离；df：活动线缆1固定端坐标与活动线缆2固定端坐标的距离；步骤3-1：已知活动线缆1和活动线缆2的活动端和固定端坐标，利用两点间距离公式求出dm和df；步骤3-2：若满足l1+l2dm+df时，判断活动线缆1和活动线缆2之间不会发生碰撞，不需要进行步骤4和步骤5；若不满足上式，则活动线缆1和活动线缆2之间可能发生碰撞，需要进行步骤4和步骤5进一步进行检测；步骤4：建立两根活动线缆的包围盒模型；该步骤用到的变量定义如下：ξi：活动线缆第i个离散点在全局坐标系O-ξηζ下的ξ轴坐标值ηi：活动线缆第i个离散点在全局坐标系O-ξηζ下的η轴坐标值ζi：活动线缆第i个离散点在全局坐标系O-ξηζ下的ζ轴坐标值Ω1：一根活动线缆的离散点集合被分割平面划分后部分点形成的点集Ω2：一根活动线缆的离散点集合被分割平面划分后另一部分点形成的点集μξ：一个点集中所有点ξ轴坐标的平均值μη：一个点集中所有点η轴坐标的平均值μζ：一个点集中所有点ζ轴坐标的平均值A：根据一个点集中的坐标值和平均值得到的3行3列协方差矩阵BOX：记录OBB包围盒所有信息的数组包括9个元素B：包围盒中心点ξbox：OBB包围盒中心点的ξ轴坐标ηbox：OBB包围盒中心点的η轴坐标ζbox：OBB包围盒中心点的ζ轴坐标R：从全局坐标系O-ξηζ到OBB包围盒坐标系B-123的基变换矩阵r1：从全局坐标系O-ξηζ到OBB包围盒坐标系B-123的基变换矩阵第一行3维行向量r2：从全局坐标系O-ξηζ到OBB包围盒坐标系B-123的基变换矩阵第二行3维行向量r3：从全局坐标系O-ξηζ到OBB包围盒坐标系B-123的基变换矩阵第三行3维行向量he1：OBB包围盒在协方差矩阵特征向量1方向的半长轴he2：OBB包围盒在协方差矩阵特征向量2方向的半长轴he3：OBB包围盒在协方差矩阵特征向量3方向的半长轴步骤4-1：活动平面在-30°～30°范围内转动时，对活动线缆1在此过程中由步骤2得到的所有离散点坐标的η轴坐标进行排序，找到包含最小η轴坐标的离散点；步骤4-2：过该点做平行于ξOζ平面的分割平面，将线缆运动过程中所有离散点分为Ω1和Ω2两个点集；步骤4-3：求出Ω1中所有点在ξ轴、η轴、ζ轴的平均值μξ、μη、μζ、，其中n为Ω1中点的个数，μη、μζ的求法同μξ，包围盒的中心点坐标即为μξ、μη、μζ；步骤4-4：计算Ω1中所有点的坐标形成的协方差矩阵A，其中covξ,η＝E[ξi-μξηi-μη],i∈Ω1，E表示取平均值；步骤4-5：计算A的3个特征向量并单位正交化，得到以点Bμξ,μη,μζ为原点，A的3个特征向量为方向的OBB包围盒坐标系B-123；步骤4-6：计算从全局坐标系O-ξηζ到OBB包围盒坐标系B-123的基变换矩阵R，取第一行为r1，第二行为r2，第三行为r3；步骤4-7：利用R求出Ω1中所有点在OBB包围盒坐标系B-123中的坐标值，并利用排序找到三个轴向上各自绝对值最大的的三个正数，将其定义为包围盒的半长轴he1、he2和he3；步骤4-8：建立BOX数组，BOX＝{μξ,μη,μζ,r1,r2,r3,he1,he2,he3}，BOX可以表示Ω1生成的包围盒中的所有特征；步骤4-9：对Ω2点集重复步骤4-3～步骤4-8；步骤4-9：对活动线缆2重复步骤4-1～步骤4-9；步骤5：判断两根活动线缆的包围盒是否发生相交；使用分离轴定理SAT确定活动线缆1和活动线缆2的包围盒是否相交；该步骤用到的变量定义如下：S：包围盒1和包围盒2可能存在某一条的分离轴：分离轴共有15种可能性，分别为平行于包围盒1和包围盒2某一面法向量的3×2共6根轴，以及平行于包围盒1某一边和包围盒2某一边公垂线的3×3共9根轴；B1：包围盒1中心点即包围盒1坐标系原点；B2：包围盒2中心点即包围盒2坐标系原点；Bs1：包围盒1中心点在S上的投影点；Bs1：包围盒1中心点在S上的投影点；ds12：B1、B2连线在S上的投影线段的长度；rs1：包围盒1在S上的投影半径的长度；rs2：包围盒2在S上的投影半径的长度；Ωs1：计算rs1需要的点集；Ωs2：计算rs2需要的点集；步骤5-1：根据B1、B2坐标值计算直线B1B2的方程；步骤5-2：根据包围盒坐标系的主轴方向向量与半长轴长度，计算包围盒1、包围盒2每个面的平面方程；步骤5-3：根据步骤5-2得到的12个平面方程和B1B2的直线方程，计算B1B2与包围盒1、包围盒2各自的一个交面，包围盒1上交面的4个顶点形成点集Ωs1，包围盒2上交面的4个顶点形成点集Ωs2；步骤5-4：从15条分离轴中选出一条未计算的分离轴S；根据两个包围盒坐标系某一主轴的方向向量，或两个包围盒坐标系各自某一主轴方向向量叉乘后的方向向量，确定S的方向向量；步骤5-5：过B1、B2做两个垂直于S的平面，这两个平面与S的交点为B1、B2在S上的投影Bs1、Bs2，利用两点间距离公式计算Bs1、Bs2的距离ds12；步骤5-6：将Ωs1中的4个点以步骤5-5的方式投影至S上，其中满足Bs1到投影点的方向为Bs1Bs2且距离Bs1最远的一点为包围盒1在S上的投影点，该投影点到Bs1的距离为rs1；步骤5-7：将Ωs2中的4个点以步骤5-5的方式投影至S上，其中满足Bs2到投影点的方向为Bs2Bs1且距离Bs2最远的一点为包围盒2在S上的投影点，该投影点到Bs2的距离为rs2；步骤5-8：若满足ds12rs1+rs2，判断包围盒1与包围盒2不相交；否则返回步骤5-4根据下一根可能的分离轴计算两包围盒是否相交；如果对于15条可能存在的分离轴，ds12rs1+rs2都不成立，则判断包围盒1与包围盒2相交；此时计算线段B1B2的中点坐标，记该点位置为碰撞发生位置；步骤5-9：若活动线缆1的两个包围盒与活动线缆2的两个包围盒都不相交，则判断活动线缆1和活动线缆2未发生碰撞，输出线缆未发生碰撞；若四次包围盒相交检测中有一次包围盒相交出现，则判断活动线缆1和活动线缆2发生碰撞，输出发生碰撞的线缆以及所有碰撞位置。

全文数据：

权利要求：

百度查询： 西北工业大学 西北工业大学太仓长三角研究院 一种基于数字仿真与OBB包围盒的活动线缆碰撞检测方法