本發(fā)明屬于計(jì)算機(jī)仿真，具體涉及一種活動線纜碰撞檢測方法。

背景技術(shù)：

1、在機(jī)載衛(wèi)星系統(tǒng)工作的過程中，天線陣面由跟蹤伺服轉(zhuǎn)臺帶動，不停做往返旋轉(zhuǎn)運(yùn)動，伺服機(jī)構(gòu)上的多根線纜也在跟隨運(yùn)動，這些線纜被稱為活動線纜。由于活動線纜會影響伺服機(jī)構(gòu)的理想工作狀態(tài)，在線纜裝配前對活動線纜的運(yùn)動形態(tài)進(jìn)行研究尤為重要。

2、目前關(guān)于活動線纜的力學(xué)仿真研究中很少提及線纜間碰撞的情景。然而，假如活動線纜間存在相互碰撞現(xiàn)象，碰撞線纜的位姿和其對伺服機(jī)構(gòu)產(chǎn)生的干擾力矩，都會較按未考慮碰撞時的仿真結(jié)果出現(xiàn)較大的變化。因此，在仿真過程中進(jìn)行碰撞檢測非常重要。

3、文獻(xiàn)(金望韜,劉檢華,劉佳順,等.光滑平面約束下的活動線纜物理特性建模與運(yùn)動仿真技術(shù)[j].機(jī)械工程學(xué)報(bào),2016,52(3):118-127.)利用kirchhoff彈性細(xì)桿理論對活動線纜進(jìn)行力學(xué)建模，提出一套微分方程組表達(dá)線纜微元的力學(xué)特性。但是該方法未考慮活動平面轉(zhuǎn)動后線纜的位姿變化，同時忽略了多根線纜發(fā)生碰撞的實(shí)際工況，在工程應(yīng)用中存在一定局限性。

4、文獻(xiàn)(王發(fā)麟,郭宇,廖文和,黃少華.基于距離場和掃掠剪除算法的線纜碰撞檢測技術(shù)[j].計(jì)算機(jī)工程與應(yīng)用,2017,53(10):27-33.)利用包圍球?qū)哟谓Y(jié)構(gòu)、距離場和掃掠剪除算法的方法進(jìn)行碰撞檢測，提出一種基于三維距離場映射的碰撞檢測方法。但是該方法所研究的線纜形狀與本發(fā)明檢測的線纜有所差別，同時該方法未考慮運(yùn)動過程中的碰撞檢測，僅對某一時刻運(yùn)動線纜的位姿狀態(tài)進(jìn)行碰撞檢測，所以該方法并不適用于本發(fā)明面向的工程應(yīng)用場景。

5、目前活動線纜碰撞檢測的相關(guān)方法存在以下局限性：

6、1.對活動線纜進(jìn)行位姿仿真時沒有考慮其運(yùn)動特性，難以在活動線纜隨平面轉(zhuǎn)動的每一時刻給出對應(yīng)的線纜位姿，從而更難考慮活動線纜在運(yùn)動過程中是否發(fā)生碰撞；

7、2.對不同線纜相互碰撞的檢測方法主要為計(jì)算多個包圍球或aabb包圍盒間是否發(fā)生相交。這些方法存在時間復(fù)雜度高的局限性，同時對于原本不可能碰撞的線纜，以及特殊形態(tài)的線纜(如懸鏈線)存在較多的冗余計(jì)算。

技術(shù)實(shí)現(xiàn)思路

1、為了克服現(xiàn)有技術(shù)的不足，本發(fā)明提供了一種基于數(shù)字仿真與obb包圍盒的活動線纜碰撞檢測方法，首先建立一根活動線纜力學(xué)模型；接下來求解一根活動線纜位姿；然后預(yù)檢測兩根活動線纜是否可能發(fā)生碰撞；之后建立兩根活動線纜的包圍盒模型；最后判斷兩根活動線纜的包圍盒是否發(fā)生相交。本發(fā)明采用kirchhoff彈性細(xì)桿理論建立活動線纜的力學(xué)模型，可以較好的體現(xiàn)線纜的彎曲、扭轉(zhuǎn)特性。

2、本發(fā)明解決其技術(shù)問題所采用的技術(shù)方案如下：

3、步驟1：建立一根活動線纜力學(xué)模型；

4、活動線纜力學(xué)模型建立在kirchhoff彈性細(xì)桿理論的基礎(chǔ)上；活動線纜力學(xué)模型由一組微分方程組成，用于描述在不同載荷和約束條件下，細(xì)桿的彈性和塑性響應(yīng)；

5、該步驟用到的變量定義如下：

6、po：活動線纜固定端端點(diǎn)；

7、pl：活動線纜活動端端點(diǎn)；

8、l：活動線纜長度；

9、p-xyz：以線纜微元段截面中心為原點(diǎn)，微元段副法向、切向和法向?yàn)檩S向的frenet坐標(biāo)系；

10、o-ξηζ：以o為原點(diǎn)建立笛卡爾三維慣性坐標(biāo)系，為全局坐標(biāo)系；

11、fi：線纜微元段截面受到鄰近截面作用內(nèi)力主矢在i軸方向的分力；

12、s：線纜微元段截面上的弧坐標(biāo)，即從po開始的弧長距離；

13、t：線纜微元段截面切線方向的單位向量；

14、ωi：線纜微元段截面在i軸方向的彎扭度；

15、ki：線纜微元段截面繞i軸的抗彎剛度/抗扭剛度；

16、e：活動線纜的楊氏模量；

17、g：活動線纜的剪切模量；

18、ji：線纜微元段截面對i軸的慣性矩；

19、dp：線纜微元段截面直徑；

20、q1,q2,q3,q4：線纜微元段截面的歐拉四元數(shù)；

21、步驟1-1：建立全局坐標(biāo)系o-ξηζ，輸入待建模活動線纜的長度l、固定端端點(diǎn)坐標(biāo)po、活動端端點(diǎn)坐標(biāo)pl、楊氏模量e、剪切模量g、截面直徑dp；

22、步驟1-2：將活動線纜按長度平均劃分為10個微元段；

23、步驟1-3：根據(jù)公式代入活動線纜的e、g、dp計(jì)算出活動線纜微元段的抗彎剛度/抗扭剛度ki；

24、步驟1-4：用歐拉四元數(shù)q1,q2,q3,q4表示彎扭度ωi；彎扭度和歐拉四元數(shù)的代數(shù)關(guān)系為：

25、

26、其中

27、步驟1-5：建立關(guān)于線纜微元段的kirchhoff受力平衡方程組：

28、

29、該方程組由frenet坐標(biāo)系下三個軸向上的力的平衡與力矩平衡構(gòu)成；平衡方程組和方程構(gòu)成了本質(zhì)上為7個未知數(shù)fx、fy、fz、q1、q2、q3、q4和7個方程的微分方程組；

30、步驟1-6：按照步驟1-3～步驟1-5的方式建立10個線纜微元段的kirchhoff受力平衡方程組；

31、步驟1-7：增加線纜的幾何約束；

32、由于活動線纜的長度和活動點(diǎn)坐標(biāo)為已知量，需要增加4個幾何約束方程，包括：

33、

34、

35、至此，單根活動線纜的70個未知量，74個方程的超定微分方程組建立完畢，該方程組即為活動線纜力學(xué)模型；

36、步驟2：求解一根活動線纜位姿；

37、求解活動線纜位姿即求解步驟1建立的超定方程組，再通過解得的已知量計(jì)算活動線纜每個離散點(diǎn)的全局坐標(biāo)；

38、該步驟用到的變量定義如下：

39、cn：第n階導(dǎo)的權(quán)系數(shù)矩陣；

40、n：離散點(diǎn)的數(shù)量；

41、si：活動線纜上第i個的點(diǎn)的弧坐標(biāo)；

42、步驟2-1：將方程組的微分項(xiàng)離散為權(quán)系數(shù)矩陣第n階導(dǎo)的元素與函數(shù)值的乘積的和；

43、n階導(dǎo)數(shù)表示為其中

44、權(quán)系數(shù)矩陣的計(jì)算由離散點(diǎn)的選擇方式?jīng)Q定，使用chebyshev方法選取離散點(diǎn)，即在離散化處理后，方程組中的微分項(xiàng)都被替換成了由離散點(diǎn)構(gòu)成的一階多項(xiàng)式的形式；

45、步驟2-2：將求解超定代數(shù)方程組的問題轉(zhuǎn)化為優(yōu)化問題；

46、設(shè)超定代數(shù)方程組中第j個方程的表達(dá)式為fj(x)＝0，則代數(shù)方程組為f(x)＝(f1(x),f2(x),…,ft(x))t＝0，f(x)是方程組的殘差形式；

47、優(yōu)化問題的目標(biāo)函數(shù)定義為：

48、

49、步驟2-3：運(yùn)用levenberg-marquard法求解非線性優(yōu)化問題；

50、輸入包括向量函數(shù)f(x)＝(f1(x),f2(x),…,ft(x))t，解向量的初值x0，以及收斂條件εx,εg，輸出包括超定代數(shù)方程組的解向量；

51、步驟2-4：利用方程組的解計(jì)算活動線纜離散點(diǎn)的全局坐標(biāo)；

52、活動線纜離散點(diǎn)坐標(biāo)與歐拉四元數(shù)有關(guān)，計(jì)算公式為：

53、

54、活動線纜上每個離散點(diǎn)的全局坐標(biāo)已經(jīng)解出，在活動平面的一個轉(zhuǎn)動角度下，不考慮碰撞情景的單根線纜位姿仿真已完成；

55、步驟3：預(yù)檢測兩根活動線纜是否可能發(fā)生碰撞；

56、判斷n根活動線纜是否發(fā)生碰撞，等價于判斷n根活動線纜中所有的線纜對中，是否存在發(fā)生碰撞的線纜對，兩根線纜組成一個線纜對，既n根線纜中存在個不同的線纜對；若不存在發(fā)生碰撞的線纜對，則判定n根活動線纜未發(fā)生碰撞；若存在任意發(fā)生碰撞的線纜對，則判定n根活動線纜發(fā)生碰撞；

57、該步驟用到的變量定義如下：

58、l1：活動線纜1的長度；

59、l2：活動線纜2的長度；

60、dm：活動線纜1活動端坐標(biāo)與活動線纜2活動端坐標(biāo)的距離；

61、df：活動線纜1固定端坐標(biāo)與活動線纜2固定端坐標(biāo)的距離；

62、步驟3-1：已知活動線纜1和活動線纜2的活動端和固定端坐標(biāo)，利用兩點(diǎn)間距離公式求出dm和df；

63、步驟3-2：若滿足l1+l2<dm+df時，判斷活動線纜1和活動線纜2之間不會發(fā)生碰撞，不需要進(jìn)行步驟4和步驟5；若不滿足上式，則活動線纜1和活動線纜2之間可能發(fā)生碰撞，需要進(jìn)行步驟4和步驟5進(jìn)一步進(jìn)行檢測；

64、步驟4：建立兩根活動線纜的包圍盒模型；

65、該步驟用到的變量定義如下：

66、ξi：活動線纜第i個離散點(diǎn)在全局坐標(biāo)系o-ξηζ下的ξ軸坐標(biāo)值

67、ηi：活動線纜第i個離散點(diǎn)在全局坐標(biāo)系o-ξηζ下的η軸坐標(biāo)值

68、ζi：活動線纜第i個離散點(diǎn)在全局坐標(biāo)系o-ξηζ下的ζ軸坐標(biāo)值

69、ω1：一根活動線纜的離散點(diǎn)集合被分割平面劃分后部分點(diǎn)形成的點(diǎn)集

70、ω2：一根活動線纜的離散點(diǎn)集合被分割平面劃分后另一部分點(diǎn)形成的點(diǎn)集

71、μξ：一個點(diǎn)集中所有點(diǎn)ξ軸坐標(biāo)的平均值

72、μη：一個點(diǎn)集中所有點(diǎn)η軸坐標(biāo)的平均值

73、μζ：一個點(diǎn)集中所有點(diǎn)ζ軸坐標(biāo)的平均值

74、a：根據(jù)一個點(diǎn)集中的坐標(biāo)值和平均值得到的3行3列協(xié)方差矩陣

75、box：記錄obb包圍盒所有信息的數(shù)組(包括9個元素)

76、b：包圍盒中心點(diǎn)

77、ξbox：obb包圍盒中心點(diǎn)的ξ軸坐標(biāo)

78、ηbox：obb包圍盒中心點(diǎn)的η軸坐標(biāo)

79、ζbox：obb包圍盒中心點(diǎn)的ζ軸坐標(biāo)

80、r：從全局坐標(biāo)系o-ξηζ到obb包圍盒坐標(biāo)系b-123的基變換矩陣

81、r1：從全局坐標(biāo)系o-ξηζ到obb包圍盒坐標(biāo)系b-123的基變換矩陣第一行(3維行向量)

82、r2：從全局坐標(biāo)系o-ξηζ到obb包圍盒坐標(biāo)系b-123的基變換矩陣第二行(3維行向量)

83、r3：從全局坐標(biāo)系o-ξηζ到obb包圍盒坐標(biāo)系b-123的基變換矩陣第三行(3維行向量)

84、he1：obb包圍盒在協(xié)方差矩陣特征向量1方向的半長軸

85、he2：obb包圍盒在協(xié)方差矩陣特征向量2方向的半長軸

86、he3：obb包圍盒在協(xié)方差矩陣特征向量3方向的半長軸

87、步驟4-1：活動平面在-30°～30°范圍內(nèi)轉(zhuǎn)動時，對活動線纜1在此過程中由步驟2得到的所有離散點(diǎn)坐標(biāo)的η軸坐標(biāo)進(jìn)行排序，找到包含最小η軸坐標(biāo)的離散點(diǎn)；

88、步驟4-2：過該點(diǎn)做平行于ξoζ平面的分割平面，將線纜運(yùn)動過程中所有離散點(diǎn)分為ω1和ω2兩個點(diǎn)集；

89、步驟4-3：求出ω1中所有點(diǎn)在ξ軸、η軸、ζ軸的平均值μξ、μη、μζ、，其中n為ω1中點(diǎn)的個數(shù)，μη、μζ的求法同μξ，包圍盒的中心點(diǎn)坐標(biāo)即為μξ、μη、μζ；

90、步驟4-4：計(jì)算ω1中所有點(diǎn)的坐標(biāo)形成的協(xié)方差矩陣a，其中cov(ξ,η)＝e[(ξi-μξ)(ηi-μη)],i∈ω1，e表示取平均值；

91、步驟4-5：計(jì)算a的3個特征向量并單位正交化，得到以點(diǎn)b(μξ,μη,μζ)為原點(diǎn)，a的3個特征向量為方向的obb包圍盒坐標(biāo)系b-123；

92、步驟4-6：計(jì)算從全局坐標(biāo)系o-ξηζ到obb包圍盒坐標(biāo)系b-123的基變換矩陣r，取第一行為r1，第二行為r2，第三行為r3；

93、步驟4-7：利用r求出ω1中所有點(diǎn)在obb包圍盒坐標(biāo)系b-123中的坐標(biāo)值，并利用排序找到三個軸向上各自絕對值最大的的三個正數(shù)，將其定義為包圍盒的半長軸he1、he2和he3；

94、步驟4-8：建立box數(shù)組，box＝{μξ,μη,μζ,r1,r2,r3,he1,he2,he3}，box可以表示ω1生成的包圍盒中的所有特征；

95、步驟4-9：對ω2點(diǎn)集重復(fù)步驟4-3～步驟4-8；

96、步驟4-9：對活動線纜2重復(fù)步驟4-1～步驟4-9。

97、步驟5：判斷兩根活動線纜的包圍盒是否發(fā)生相交；

98、使用分離軸定理sat確定活動線纜1和活動線纜2的包圍盒是否相交；

99、該步驟用到的變量定義如下：

100、s：包圍盒1和包圍盒2可能存在某一條的分離軸：分離軸共有15種可能性，分別為平行于包圍盒1和包圍盒2某一面法向量的3×2共6根軸，以及平行于包圍盒1某一邊和包圍盒2某一邊公垂線的3×3共9根軸；

101、b1：包圍盒1中心點(diǎn)即包圍盒1坐標(biāo)系原點(diǎn)；

102、b2：包圍盒2中心點(diǎn)即包圍盒2坐標(biāo)系原點(diǎn)；

103、bs1：包圍盒1中心點(diǎn)在s上的投影點(diǎn)；

104、bs1：包圍盒1中心點(diǎn)在s上的投影點(diǎn)；

105、ds12：b1、b2連線在s上的投影線段的長度；

106、rs1：包圍盒1在s上的投影半徑的長度；

107、rs2：包圍盒2在s上的投影半徑的長度；

108、ωs1：計(jì)算rs1需要的點(diǎn)集；

109、ωs2：計(jì)算rs2需要的點(diǎn)集；

110、步驟5-1：根據(jù)b1、b2坐標(biāo)值計(jì)算直線b1b2的方程；

111、步驟5-2：根據(jù)包圍盒坐標(biāo)系的主軸方向向量與半長軸長度，計(jì)算包圍盒1、包圍盒2每個面的平面方程；

112、步驟5-3：根據(jù)步驟5-2得到的12個平面方程和b1b2的直線方程，計(jì)算b1b2與包圍盒1、包圍盒2各自的一個交面，包圍盒1上交面的4個頂點(diǎn)形成點(diǎn)集ωs1，包圍盒2上交面的4個頂點(diǎn)形成點(diǎn)集ωs2；

113、步驟5-4：從15條分離軸中選出一條未計(jì)算的分離軸s。根據(jù)兩個包圍盒坐標(biāo)系某一主軸的方向向量，或兩個包圍盒坐標(biāo)系各自某一主軸方向向量叉乘后的方向向量，確定s的方向向量；

114、步驟5-5：過b1、b2做兩個垂直于s的平面，這兩個平面與s的交點(diǎn)為b1、b2在s上的投影bs1、bs2，利用兩點(diǎn)間距離公式計(jì)算bs1、bs2的距離ds12；

115、步驟5-6：將ωs1中的4個點(diǎn)以步驟5-5的方式投影至s上，其中滿足bs1到投影點(diǎn)的方向?yàn)閎s1bs2且距離bs1最遠(yuǎn)的一點(diǎn)為包圍盒1在s上的投影點(diǎn)，該投影點(diǎn)到bs1的距離為rs1；

116、步驟5-7：將ωs2中的4個點(diǎn)以步驟5-5的方式投影至s上，其中滿足bs2到投影點(diǎn)的方向?yàn)閎s2bs1且距離bs2最遠(yuǎn)的一點(diǎn)為包圍盒2在s上的投影點(diǎn)，該投影點(diǎn)到bs2的距離為rs2；

117、步驟5-8：若滿足ds12>rs1+rs2，判斷包圍盒1與包圍盒2不相交；否則返回步驟5-4根據(jù)下一根可能的分離軸計(jì)算兩包圍盒是否相交；如果對于15條可能存在的分離軸，ds12>rs1+rs2都不成立，則判斷包圍盒1與包圍盒2相交；此時計(jì)算線段b1b2的中點(diǎn)坐標(biāo)，記該點(diǎn)位置為碰撞發(fā)生位置；

118、步驟5-9：若活動線纜1的兩個包圍盒與活動線纜2的兩個包圍盒都不相交，則判斷活動線纜1和活動線纜2未發(fā)生碰撞，輸出線纜未發(fā)生碰撞；若四次包圍盒相交檢測中有一次包圍盒相交出現(xiàn)，則判斷活動線纜1和活動線纜2發(fā)生碰撞，輸出發(fā)生碰撞的線纜以及所有碰撞位置。

119、優(yōu)選地，所述兩個包圍盒坐標(biāo)系某一主軸的方向向量等價于面的法向量，共6種；所述兩個包圍盒坐標(biāo)系各自某一主軸方向向量叉乘后的方向向量等價于兩條邊的公垂線，共9種。

120、本發(fā)明的有益效果如下：

121、1、本發(fā)明采用kirchhoff彈性細(xì)桿理論建立活動線纜的力學(xué)模型，可以較好的體現(xiàn)線纜的彎曲、扭轉(zhuǎn)特性；

122、2、本發(fā)明選用非線性最小二乘法levenberg-marquardt算法進(jìn)行線纜模型的求解，該方案具有較高的建模精度和較快求解速度的優(yōu)點(diǎn)，解決了彈性細(xì)桿模型難以求解的問題；

123、3、本發(fā)明在使用包圍盒進(jìn)行精確碰撞器檢測前先進(jìn)行預(yù)檢測，排除對不相交線纜間的冗余計(jì)算，加快計(jì)算執(zhí)行效率；

124、4、本發(fā)明采用了兩包圍盒(支持?jǐn)U展為多包圍盒)包含一根活動線纜的包圍盒生成方式，綜合了計(jì)算速度和計(jì)算精度的雙重要求。