一種基于三維坐標(biāo)定位的門座起重機(jī)位姿確定方法
【專利摘要】本發(fā)明公開了一種基于三維坐標(biāo)定位的門座起重機(jī)位姿確定方法，其特點(diǎn)是該方法在門座起重機(jī)的本土上選取至少兩個(gè)基準(zhǔn)點(diǎn)，然后采用三維定位技術(shù)對(duì)兩基準(zhǔn)點(diǎn)進(jìn)行三維坐標(biāo)的測(cè)量，將測(cè)得的三維坐標(biāo)利用三角函數(shù)計(jì)算得到兩基準(zhǔn)點(diǎn)相對(duì)臂架的旋轉(zhuǎn)中心點(diǎn)XC、YC和ZC坐標(biāo)以及臂架的回轉(zhuǎn)角α臂架和臂架與水平夾角β臂架的起重機(jī)位姿參數(shù)，由起重機(jī)位姿參數(shù)確定門座起重機(jī)桿件系統(tǒng)的空間位置。本發(fā)明與現(xiàn)有技術(shù)相比具有方法簡(jiǎn)單易操作，成本低，可靠度高，較好的解決了船廠起重機(jī)的防撞問題，尤其適用于起重機(jī)的安全監(jiān)控和安全生產(chǎn)的保駕護(hù)航。
【專利說明】
一種基于三維坐標(biāo)定位的門座起重機(jī)位姿確定方法
技術(shù)領(lǐng)域
[0001]本發(fā)明涉及起重機(jī)安全監(jiān)控及防撞技術(shù)領(lǐng)域，具體地說是一種基于三維坐標(biāo)定位的門座起重機(jī)位姿確定方法。
【背景技術(shù)】
[0002]門座起重機(jī)多用于船廠、碼頭等場(chǎng)合，是大中型鋼結(jié)構(gòu)分段、大型設(shè)備等的重要吊裝工具。門座起重機(jī)的桿件系統(tǒng)相對(duì)較為復(fù)雜，通常由門座部分、轉(zhuǎn)柱部分和臂架系統(tǒng)組成，臂架系統(tǒng)又包括主臂架、象鼻梁、大拉桿、對(duì)重拉桿等。在門座起重機(jī)的運(yùn)行、回轉(zhuǎn)及變幅過程中，各桿件的位置為非常復(fù)雜的空間曲線，門座起重機(jī)與廠區(qū)其它設(shè)備或建筑物之間的相對(duì)位置關(guān)系難以確定，所以，在實(shí)際操作過程中，為了生產(chǎn)安全的需要，要隨時(shí)掌握門座起重機(jī)桿件系統(tǒng)的空間位置，即門座起重機(jī)位姿，以免與其它設(shè)備或建筑物發(fā)生碰撞而導(dǎo)致嚴(yán)重事故。
[0003]門座起重機(jī)桿件系統(tǒng)的空間位置可用旋轉(zhuǎn)中心坐標(biāo)、回轉(zhuǎn)角度和俯仰角度來唯一確定門座起重機(jī)的位姿，目前起重機(jī)自身的控制系統(tǒng)中，無法得到這三個(gè)參數(shù)的信息。目前公布的專利及期刊文獻(xiàn)中，也僅提及測(cè)量起重機(jī)某些測(cè)點(diǎn)的技術(shù)，尚沒有涉及確定門座起重機(jī)位姿信息的技術(shù)內(nèi)容。

【發(fā)明內(nèi)容】

[0004]本發(fā)明的目的是針對(duì)現(xiàn)有技術(shù)的不足而設(shè)計(jì)的一種基于三維坐標(biāo)定位的門座起重機(jī)位姿確定方法，采用先測(cè)得若干個(gè)基準(zhǔn)點(diǎn)的位置坐標(biāo)，然后由若干個(gè)位置坐標(biāo)換算得到起重機(jī)的姿態(tài)參數(shù)，對(duì)起重機(jī)防撞具有很強(qiáng)的實(shí)用性，方法簡(jiǎn)單易操作，成本低，可靠度高，尤其適用于起重機(jī)的安全監(jiān)控和安全生產(chǎn)的保駕護(hù)航。
[0005]本發(fā)明的目的是這樣實(shí)現(xiàn)的:一種基于三維坐標(biāo)定位的門座起重機(jī)位姿確定方法，其特點(diǎn)是該方法在門座起重機(jī)的本體上選取至少兩個(gè)基準(zhǔn)點(diǎn)，然后采用三維定位技術(shù)對(duì)兩基準(zhǔn)點(diǎn)進(jìn)行三維坐標(biāo)的測(cè)量，將測(cè)得的位置三維坐標(biāo)利用三角函數(shù)計(jì)算得到兩基準(zhǔn)點(diǎn)相對(duì)于起重機(jī)旋轉(zhuǎn)中心與機(jī)房下平面交點(diǎn)C的三維坐標(biāo)以及臂架的回轉(zhuǎn)角(??和臂架與水平夾角的起重機(jī)位姿參數(shù)，由起重機(jī)位姿參數(shù)確定門座起重機(jī)桿件系統(tǒng)的空間位置，所述起重機(jī)旋轉(zhuǎn)中心與機(jī)房下平面交點(diǎn)C的Xe坐標(biāo)由下述公式(I)計(jì)算得到:
[0006]Xc = Xb+Lbc X cos (aBC) =Xb+/" (Xbc2+Ybc2) X cos (degrees (a_+degrees( arc tan(Ybc/Xbc) )))(I)
[0007]
[0008]其中:XB、Xc為B基準(zhǔn)點(diǎn)和交點(diǎn)C的X坐標(biāo);XBC、YBC為B基準(zhǔn)點(diǎn)相對(duì)交點(diǎn)C的X和Y坐標(biāo)；Lbc為B基準(zhǔn)點(diǎn)至交點(diǎn)C的距離。
[0009]所述起重機(jī)旋轉(zhuǎn)中心與機(jī)房下平面交點(diǎn)C的Yc坐標(biāo)由下述公式(2)計(jì)算得到:
[0010]Yc = Yb+Lbc X sin(aBc) =/" (Xbc2+Ybc2 )Xsin(degrees(a離+degrees(arctan(Ybc/Xbc)))) (2)
[0011]
[0012]其中:YB、Yc為B基準(zhǔn)點(diǎn)和交點(diǎn)C的Y坐標(biāo);aBC為B基準(zhǔn)點(diǎn)至交點(diǎn)C的回轉(zhuǎn)角。
[0013]所述起重機(jī)旋轉(zhuǎn)中心與機(jī)房下平面交點(diǎn)C的Zc坐標(biāo)由下述公式(3)計(jì)算得到:
[0014]Zc = Zb-Zbc(3)
[0015]其中:ZC、ZB為交點(diǎn)C和B基準(zhǔn)點(diǎn)的Z坐標(biāo);ZbAB基準(zhǔn)點(diǎn)相對(duì)交點(diǎn)C的Z坐標(biāo)。
[0016]所述臂架的回轉(zhuǎn)角aigg由下述公式(4)計(jì)算得到:
[0017]a|gg=degrees(arctan( (Ya-Yb)/(Xa~Xb) )-arcsin( Yac-Ybc I //"( (Ya-Yb)2+(Xa~Xb)2)) (4)
[0018]其中:XA、YA、XB、Yb為A、B兩基準(zhǔn)點(diǎn)的X和Y坐標(biāo);Yac、Ybc為A、B兩基準(zhǔn)點(diǎn)相對(duì)交點(diǎn)C的Y坐標(biāo)。
[0019]所述臂架與水平夾角由下式(5)計(jì)算得到:
[0020]臂架與水平夾角β離=degrees(arctan((Za-Zd)/(/" ((Xa-Xc)2+(Ya-Yc)2~Yae2)-LcD)+arccos( (Lde2+( d (( Xa-Xc )2+ ( Ya-Yc ) 2_Yae2 ) )_Lcd)/ cos(arctan( ( Za-Zd ) / d (( Xa-Xc )2+ (Ya-Yc)2-Yae2)-Lcd)))2_(Xae2+Yae2) )/(2 X Lde X (^ ((Xa-Xc)2+ (Ya-Yc)2_Yae2))-Lcd)/cos(arctan( (Za-Zd)/(丨((Xa-Xc )2+( Ya-Yc ) 2-Yae2 ) -Lcd ))))(5)
[0021]其中:Za、Zd為A基準(zhǔn)點(diǎn)和臂架下鉸點(diǎn)的中點(diǎn)D的Z坐標(biāo);XA、Xc為A基準(zhǔn)點(diǎn)和交點(diǎn)C的X坐標(biāo);Ya、Yc為A基準(zhǔn)點(diǎn)和交點(diǎn)C的Y坐標(biāo);Lde為臂架下鉸點(diǎn)的中點(diǎn)D至臂架上部與象鼻梁中部鉸點(diǎn)E的距離;Ud為交點(diǎn)C至臂架下鉸點(diǎn)的中點(diǎn)D的距離;XAE、YAE為A基準(zhǔn)點(diǎn)相對(duì)臂架上部鉸點(diǎn)與象鼻梁中部鉸點(diǎn)E的X和Y坐標(biāo)。
[0022]所述門座起重機(jī)的本體上選取至少兩個(gè)基準(zhǔn)點(diǎn)，其一基準(zhǔn)點(diǎn)設(shè)置在象鼻梁頭部主鉤滑輪組中心附近，另一基準(zhǔn)點(diǎn)設(shè)置在轉(zhuǎn)盤尾部中點(diǎn)附近。
[0023]所述三維定位技術(shù)為GPS、glonass或北斗衛(wèi)星定位技術(shù)。
[0024]本發(fā)明與現(xiàn)有技術(shù)相比具有方法簡(jiǎn)單易操作，成本低，可靠度高，較好的解決了船廠起重機(jī)的防撞問題，尤其適用于起重機(jī)的安全監(jiān)控和安全生產(chǎn)的保駕護(hù)航。
【附圖說明】
[0025]圖1為測(cè)量點(diǎn)A的布置及俯仰角示意圖；
[0026I圖2為測(cè)量點(diǎn)B的布置示意圖；
[0027]圖3為臂架的回轉(zhuǎn)角示意圖。
【具體實(shí)施方式】
[0028]本發(fā)明在門座起重機(jī)的本體上選取A、B兩點(diǎn)為測(cè)量基準(zhǔn)點(diǎn)，然后采用三維定位裝置測(cè)得A、B兩基準(zhǔn)點(diǎn)的三維坐標(biāo)，由該兩基準(zhǔn)點(diǎn)的三維坐標(biāo)利用三角函數(shù)計(jì)算得到門座起重機(jī)的位姿參數(shù)，由起重機(jī)位姿參數(shù)確定門座起重機(jī)桿件系統(tǒng)的空間位置。本發(fā)明關(guān)鍵在于基準(zhǔn)點(diǎn)的選取與起重機(jī)位姿計(jì)算方法的推導(dǎo)，基準(zhǔn)點(diǎn)數(shù)量不宜太多，否則會(huì)增加定位裝置的采購成本，也不宜太少，否則沒有足夠的坐標(biāo)信息來推算起重機(jī)的姿態(tài)。
[0029]實(shí)施例1
[0030]參閱附圖1，本發(fā)明選定的基準(zhǔn)點(diǎn)有2個(gè)，一個(gè)基準(zhǔn)點(diǎn)布置在象鼻梁頭部主鉤滑輪組中心A’附近的A點(diǎn)。
[0031]參閱附圖2，另一個(gè)基準(zhǔn)點(diǎn)布置在轉(zhuǎn)盤尾部中點(diǎn)B’附近的B點(diǎn)。在起重機(jī)變幅平面內(nèi)，A’點(diǎn)為距回轉(zhuǎn)中心前部最遠(yuǎn)點(diǎn)，B’點(diǎn)為距回轉(zhuǎn)中心后部最遠(yuǎn)點(diǎn)，將測(cè)點(diǎn)布置在這兩個(gè)位置上能最大程度的減小測(cè)量誤差，但是在實(shí)際安裝定位裝置時(shí)，由于安裝空間的限制，并不能安裝在理論點(diǎn)上，只能安裝在其附近的某一位置，即分別為A點(diǎn)和B點(diǎn)。通過三維坐標(biāo)定位技術(shù)測(cè)量得到A點(diǎn)和B點(diǎn)的坐標(biāo)，分別為A(Xa，Ya，Za)和B(Xb，Yb，Zb)。
[0032]參閱附圖3，根據(jù)起重機(jī)的實(shí)際結(jié)構(gòu)，得到A點(diǎn)相對(duì)起重機(jī)旋轉(zhuǎn)中心與機(jī)房下平面交點(diǎn)C的坐標(biāo)(Xac，Yac，Zac )，B點(diǎn)相對(duì)起重機(jī)旋轉(zhuǎn)中心與機(jī)房下平面交點(diǎn)C的坐標(biāo)(Xbc，Ybc，ZBC);A點(diǎn)相對(duì)臂架上部與象鼻梁中部鉸點(diǎn)E坐標(biāo)(XAE，YAE，ZAE);起重機(jī)旋轉(zhuǎn)中心與機(jī)房下平面交點(diǎn)C至臂架下鉸點(diǎn)的中點(diǎn)D的距離Lcd;臂架下鉸點(diǎn)的中點(diǎn)D的坐標(biāo)Zd;臂架下鉸點(diǎn)的中點(diǎn)D至臂架上部與象鼻梁中部鉸點(diǎn)E的距離Lde。通過推導(dǎo)，得到臂架旋轉(zhuǎn)角度α戀§為:
[0033]α離=degrees(arctan( (Ya-Yb)/(Xa-Xb) )-arcsin( Yac-Ybc I //"( (Ya-Yb)2+(Xa~Xb
)2))式⑷
[0034]
[0035]起重機(jī)旋轉(zhuǎn)中心與機(jī)房下平面交點(diǎn)C的Xe為:
[0036]Xe = Xb+Lbc Xcos(aBC)= Xb+^ (Xbc2+Ybc2 ) X cos (degrees(a|gg+degrees(arctan(Ybc/Xbc))))式(I)
[0037]其中:Lbc= I(Xbc2+Ybc2) ;aBc = degrees(a離+arctan(YBc/XBC)) ο
[0038]起重機(jī)旋轉(zhuǎn)中心與機(jī)房下平面交點(diǎn)C的Yc為:
[0039]Yc = Yb+Lbc X sin(aBc) =/"(Xbc2+Ybc2) X sin(degrees(a離+degrees(arctan(YBc/Xbc))))
[0040]式(2)
[0041]起重機(jī)旋轉(zhuǎn)中心與機(jī)房下平面交點(diǎn)C的坐標(biāo)Zc為:
[0042]Zc = Zb-Zbc式(3)
[0043]臂架與水平夾角(俯仰角)β酸為:
[0044]{3|gg=degrees(arctan( (Za-Zd)/(/" ( (Xa-Xc)2+(Ya-Yc)2-Yae2)-Lcd) +
[0045]arccos( (Lde2+( ( (Xa-Xc)2+(Ya~Yc)2-Yae2) )~Lcd)/cos
[0046](arctan( (Za-Zd)/(((( Xa-Xc )2+( Ya-Yc ) 2-Yae2 ) -Lcd )) )2_(Xae2+Yae2) )/(2 X
[0047 ]LdeX (^(( Xa-Xc )2+( Ya-Yc ) 2_Yae2 )) -Lcd ) /co s (arc tan (( Za-ZdXa-Xc )2+ ( Ya-
Yc)2
[0048]-Yae2)-Lcd))))
[0049]式(5)
[0050]其中:lac= /~ ( (Xa_Xc)2+(Ya_Yc)2) ；La'c = /" (Lac2-Yae2) ； Lao= (La'c-Lcd)/cos(arctan( (Za-Zd)/(La'c—Lcd) )) ；La'e = /"(Xae2+Yae2)。
[0051]以上實(shí)施例只是對(duì)本發(fā)明做進(jìn)一步說明，并非用以限制本發(fā)明專利，凡為本發(fā)明等效實(shí)施，均應(yīng)包含于本發(fā)明專利的權(quán)利要求范圍之內(nèi)。
【主權(quán)項(xiàng)】
1.一種基于三維坐標(biāo)定位的門座起重機(jī)位姿確定方法，其特征在于該方法在門座起重機(jī)的本體上選取至少兩個(gè)基準(zhǔn)點(diǎn)，然后采用三維定位技術(shù)對(duì)兩基準(zhǔn)點(diǎn)進(jìn)行三維坐標(biāo)的測(cè)量，將測(cè)得的三維坐標(biāo)利用三角函數(shù)計(jì)算得到兩基準(zhǔn)點(diǎn)相對(duì)于起重機(jī)旋轉(zhuǎn)中心與機(jī)房下平面交點(diǎn)C的三維坐標(biāo)以及臂架的回轉(zhuǎn)角α戀5和臂架與水平夾角β戀5的起重機(jī)位姿參數(shù)，由起重機(jī)位姿參數(shù)確定門座起重機(jī)桿件系統(tǒng)的空間位置，所述起重機(jī)旋轉(zhuǎn)中心與機(jī)房下平面交點(diǎn)C的Xe坐標(biāo)由下述公式⑴計(jì)算得到: Xc = Xb+Lbc X cos(aBc) =Xb+/" (Xbc2+Ybc2) X cos(degrees(a離+degrees(arctan(Ybc/Xbc)))) (I) 其中:XB、Xc為B基準(zhǔn)點(diǎn)和交點(diǎn)C的X坐標(biāo);Xbc、YbAB基準(zhǔn)點(diǎn)相對(duì)交點(diǎn)C的X和Y坐標(biāo);Lbc為B基準(zhǔn)點(diǎn)至交點(diǎn)C的距離； 所述起重機(jī)旋轉(zhuǎn)中心與機(jī)房下平面交點(diǎn)C的Yc坐標(biāo)由下述公式(2)計(jì)算得到: Yc=Yb+LbcXsin(aBc) =/"(Xbc2+Ybc2) Xsin(degrees(a離fdegrees(arctan(YBc/XBC)))) (2) 其中:Yb、Yc為B基準(zhǔn)點(diǎn)和交點(diǎn)C的Y坐標(biāo);aBC*B基準(zhǔn)點(diǎn)至交點(diǎn)C的回轉(zhuǎn)角； 所述起重機(jī)旋轉(zhuǎn)中心與機(jī)房下平面交點(diǎn)C的Zc坐標(biāo)由下述公式(3)計(jì)算得到: Zc = Zb-Zbc(3) 其中:Zc、Zb為交點(diǎn)C和B基準(zhǔn)點(diǎn)的Z坐標(biāo);ZbcSB基準(zhǔn)點(diǎn)相對(duì)交點(diǎn)C的Z坐標(biāo)； 所述臂架的回轉(zhuǎn)角aigg由下述公式(4)計(jì)算得到: α?β= degrees(arctan((Ya-Yb)/(Xa-Xb))-arcsin( Yac-YbcI/((Ya_Yb)2+(Xa_Xb)2)) (4) 其中:Xa、Ya、Xb、Yb為A、B兩基準(zhǔn)點(diǎn)的X和Y坐標(biāo);YAC、YBC為A、B兩基準(zhǔn)點(diǎn)相對(duì)交點(diǎn)C的Y坐標(biāo)； 所述臂架與水平夾角由下式(5)計(jì)算得到: 臂架與水平夾角β離=degrees(arctan( (Za-Zd)/(/" ((Xa-Xc)2+(Ya-Yc)2~Yae2)-Lcd) +arccos((LDE2+( (^( (Xa-Xc )2+(Ya-Yc)2-Yae2 ))-Lcd )/cos(arctan( (Za-Zd)/(^( (Xa-Xc )2+( Ya-Yc)2-Yae2)~Lcd) ) )2-(Xae2+Yae2) )/(2 X Lde X (/" ((Xa~Xc)2+(Ya~Yc)2~Yae2) )~Lcd)/cos (arctan((Za-Zd ) / (丨((Xa-Xc )2+( Ya-Yc ) 2_Yae2 ) -Lcd )))) (5) 其中:ZA、ZD為A基準(zhǔn)點(diǎn)和臂架下鉸點(diǎn)的中點(diǎn)D的Z坐標(biāo);Xa、Xc為A基準(zhǔn)點(diǎn)和交點(diǎn)C的X坐標(biāo)；Ya、Yc為A基準(zhǔn)點(diǎn)和交點(diǎn)C的Y坐標(biāo);Lde為臂架下鉸點(diǎn)的中點(diǎn)D至臂架上部與象鼻梁中部鉸點(diǎn)E的距離;Ud為交點(diǎn)C至臂架下鉸點(diǎn)的中點(diǎn)D的距離;XAE、YAE為A基準(zhǔn)點(diǎn)相對(duì)臂架上部與象鼻梁中部鉸點(diǎn)E的X和Y坐標(biāo)。2.根據(jù)權(quán)利要求1所述基于三維坐標(biāo)定位的門座起重機(jī)位姿確定方法，其特征在于所述門座起重機(jī)的本體上選取至少兩個(gè)基準(zhǔn)點(diǎn)，其一基準(zhǔn)點(diǎn)設(shè)置在象鼻梁頭部主鉤滑輪組中心附近，另一基準(zhǔn)點(diǎn)設(shè)置在在轉(zhuǎn)盤尾部中點(diǎn)附近。3.根據(jù)權(quán)利要求1所述基于三維坐標(biāo)定位的門座起重機(jī)位姿確定方法，其特征在于所述三維定位技術(shù)為GPS、glonass或北斗衛(wèi)星定位技術(shù)。
【文檔編號(hào)】B66C15/04GK105836627SQ201610246533
【公開日】2016年8月10日
【申請(qǐng)日】2016年4月20日
【發(fā)明人】李元春, 李永鳳, 任宇剛, 董元躍
【申請(qǐng)人】中船第九設(shè)計(jì)研究院工程有限公司