[发明专利]一种基于三维坐标定位的门座起重机位姿确定方法

权利要求书

1.一种基于三维坐标定位的门座起重机位姿确定方法，其特征在于该方法在门座起重机的本体上选取至少两个基准点，然后采用三维定位技术对两基准点进行三维坐标的测量，将测得的三维坐标利用三角函数计算得到两基准点相对于起重机旋转中心与机房下平面交点C的三维坐标以及臂架的回转角α_臂架和臂架与水平夹角β_臂架的起重机位姿参数，由起重机位姿参数确定门座起重机杆件系统的空间位置，所述起重机旋转中心与机房下平面交点C的X_C坐标由下述公式(1)计算得到：

X_C＝X_B+L_BC×cos(α_BC)＝X_B+√(X_BC²+Y_BC²)×cos(degrees(α_臂架+degrees(arctan(Y_BC/X_BC))))(1)

其中：X_B、X_C为B基准点和交点C的X坐标；X_BC、Y_BC为B基准点相对交点C的X和Y坐标；L_BC为B基准点至交点C的距离；

所述起重机旋转中心与机房下平面交点C的Y_C坐标由下述公式(2)计算得到：

Y_C＝Y_B+L_BC×sin(α_BC)＝√(X_BC²+Y_BC²)×sin(degrees(α_臂架+degrees(arctan(Y_BC/X_BC)))) (2)

其中：Y_B、Y_C为B基准点和交点C的Y坐标；α_BC为B基准点至交点C的回转角；

所述起重机旋转中心与机房下平面交点C的Z_C坐标由下述公式(3)计算得到：

Z_C＝Z_B-Z_BC(3)

其中：Z_C、Z_B为交点C和B基准点的Z坐标；Z_BC为B基准点相对交点C的Z坐标；

所述臂架的回转角α_臂架由下述公式(4)计算得到：

α_臂架＝degrees(arctan((Y_A-Y_B)/(X_A-X_B))-arcsin(|Y_AC-Y_BC|/√((Y_A-Y_B)²+(X_A-X_B)²)) (4)

其中：X_A、Y_A、X_B、Y_B为A、B两基准点的X和Y坐标；Y_AC、Y_BC为A、B两基准点相对交点C的Y坐标；

所述臂架与水平夹角β_臂架由下式(5)计算得到：

臂架与水平夹角β_臂架＝degrees(arctan((Z_A-Z_D)/(√((X_A-X_C)²+(Y_A-Y_C)²-Y_AE²)-L_CD)+arccos((L_DE²+((√((X_A-X_C)²+(Y_A-Y_C)²-Y_AE²))-L_CD)/cos(arctan((Z_A-Z_D)/(√((X_A-X_C)²+(Y_A-Y_C)²-Y_AE²)-L_CD)))²-(X_AE²+Y_AE²))/(2×L_DE×(√((X_A-X_C)²+(Y_A-Y_C)²-Y_AE²))-L_CD)/cos(arctan((Z_A-Z_D)/(√((X_A-X_C)²+(Y_A-Y_C)²-Y_AE²)-L_CD))))(5)

其中：Z_A、Z_D为A基准点和臂架下铰点的中点D的Z坐标；X_A、X_C为A基准点和交点C的X坐标；Y_A、Y_C为A基准点和交点C的Y坐标；L_DE为臂架下铰点的中点D至臂架上部与象鼻梁中部铰点E的距离；L_CD为交点C至臂架下铰点的中点D的距离；X_AE、Y_AE为A基准点相对臂架上部与象鼻梁中部铰点E的X和Y坐标。