[发明专利]工作空间六自由度位姿动态测量设备及方法

权利要求书

1.一种工作空间六自由度位姿动态测量方法，其特征是，包括如下步骤：

步骤1.组装六自由度位姿传感器，在传感器组装完成后，依靠包括影像仪、测量臂等高精度辅助工具标定传感器表面接收器光学中心在传感器坐标系UVW下精确坐标；

步骤2.布置好发射站，待发射站运行平稳后建立WMPS系统测量坐标系；将六自由度位姿传感器固定在待测物体表面，打开传感器电源，以覆盖测量空间的WMPS系统坐标系作为全局测量坐标系；

步骤3.打开上位机测量软件，通过Zigbee无线通讯网络向传感器发送测量命令，待传感器返回接收器角度信息后解算接收器在测量坐标系下坐标，当接收器接收到两台以上发射站发出的光脉冲后，可通过角度交会测量方法测得接收器光学中心在WMPS系统坐标系XYZ下坐标；

步骤4.解算得到传感器表面三个或三个以上接收器坐标之后，以可测接收器几何中心为中心建立过渡坐标系X′Y′Z′并得到过渡坐标系到WMPS系统坐标系的旋转矩阵R_{X′Y′Z′-XYZ}及平移矢量T_{X′Y′Z′-XYZ}，下标X′Y′Z′-XYZ表示二者为过渡坐标系X′Y′Z′到wMPS系统坐标系XYZ的坐标变换关系，同时通过预先标定的接收器在传感器坐标系UVW下精确坐标也可得到该过渡坐标系到传感器坐标系的旋转矩阵R_{X′Y′Z′-UVW}及平移矢量T_{X′Y′Z′-UVW}，下标X′Y′Z′-UVW表示此二者为过渡坐标系X′Y′Z′到传感器坐标系UVW坐标变换关系；此时，传感器坐标系UVW到WMPS坐标系XYZ的旋转矩阵R_UVW-XYZ可通过式计算，传感器坐标系原点在WMPS坐标系下的坐标O_XYZ为：

OXYZ=TX′Y′Z′-XYZ-RX′Y′Z′-XYZ×RX′Y′Z′-UVWT×TX′Y′Z′-UVW;]]>

步骤5.当传感器被固定在运动物体表面待测关键点时，二者可视为同一刚体，则运动物体关键点坐标为O_XYZ，假定测得的传感器坐标系UVW到WMPS坐标系XYZ的旋转矩阵R_UVW-XYZ为：

RUVW-XYZ=r11r12r13r21r22r23r31r32r33;---(1)]]>

(1)中rij为矩阵R_UVW-XYZ中元素，得到刚体此时的RPY角度，RPY是pitch-yaw-roll的缩写，α、β、θ为：

α=tan-1(r21/r11);β=tan-1(-r31/r312+r332);γ=tan-1(r32/r33);---(2)]]>

T表示转置。