工作空间六自由度位姿动态测量设备及方法

1.一种工作空间六自由度位姿动态测量方法，其特征是，包括如下步骤：
步骤1.组装六自由度位姿传感器，在传感器组装完成后，依靠包括影像仪、测量臂的高精度辅助工具标定传感器表面WMPS接收器光学中心在传感器坐标系UVW下精确坐标；
六自由度位姿传感器的传感器壳外形为长方体空腔，长方体截面为正方形，长方体每个侧面及棱边上分为上下两排交错安装8个WMPS接收器，其中最上排4个WMPS接收器与壳体外壁保持一定夹角，从而保证传感器四周及顶端均有3个或3个以上WMPS接收器可以接收测量空间中发射器发来的光脉冲，同时壳体顶端设计有激光跟踪仪靶球坐以方便现场比对；
传感器壳体下部安装有转接法兰，法兰四周设计有定位通孔，法兰中央设计有定位销孔，同通过转接法兰可将传感器安装在待测物体表面；
传感器壳体内部安装有WMPS接收器信号采集处理电路，WMPS信号采集处理电路核心元件为大规模FPGA芯片，FPGA芯片外部配有FLASH芯片、DDR芯片及Zigbee无线通讯芯片，FPGA芯片内部搭建有SOPC嵌入式处理系统电路，系统内集成有32位RSIC处理器内核、总线及内存控制器、闪存控制器、SPI串行接口外设，WMPS信号采集处理电路通过处理器总线上的8个数据采集通道完成，每通道负责采集一个外部WMPS接收器信号，依靠通道内配置的专用FIFO实现不同通道信号的同时触发、同时采集，数据采集时处理器通过总线上的同步采集控制模块控制所有通道同时采集WMPS接收器电脉冲，当收到WMPS接收器电脉冲时，触发模块锁存FPGA内部计时器时间值并将其写入该通道专用FIFO，当采集结束，处理器从各通道FIFO中依次读出所缓存的时间信息，并结合预先存储的发射器转速计算发射器转台转角；
步骤2.布置好发射站，待发射站运行平稳后建立WMPS系统坐标系；将六自由度位姿传感器固定在待测物体表面，打开传感器电源，以覆盖测量空间的WMPS系统坐标系作为全局测量坐标系；
步骤3.打开上位机测量软件，通过Zigbee无线通讯网络向传感器发送测量命令，待传感器返回WMPS接收器角度信息后解算接收器在WMPS系统坐标系下坐标即：当WMPS接收器接收到两台以上发射站发出的光脉冲后，可通过角度交会测量方法测得WMPS接收器光学中心在WMPS系统坐标系XYZ下坐标；
步骤4.解算得到传感器表面三个或三个以上WMPS接收器坐标之后，以可测WMPS接收器几何中心为中心建立过渡坐标系 X′Y′Z′并得到过渡坐标系到WMPS系统坐标系的旋转矩阵RX′Y′Z′-XYZ及平移矢量TX′Y′Z′-XYZ，下标X′Y′Z′-XYZ表示二者为过渡坐标系X′Y′Z′到WMPS系统坐标系XYZ的坐标变换关系，同时通过预先标定的WMPS接收器在传感器坐标系UVW下精确坐标也可得到该过渡坐标系到传感器坐标系的旋转矩阵RX′Y′Z′-UVW及平移矢量TX′Y′Z′-UVW，下标X′Y′Z′-UVW表示此二者为过渡坐标系X′Y′Z′到传感器坐标系UVW坐标变换关系；此时，传感器坐标系UVW到WMPS坐标系XYZ的旋转矩阵RUVW-XYZ可通过 式计算，传感器坐标系原点在WMPS坐标系下的坐标
OXYZ为：
步骤5.当传感器被固定在运动物体表面待测关键点时，二者可视为同一刚体，则运动物体关键点坐标为OXYZ，假定测得的传感器坐标系UVW到WMPS坐标系XYZ的旋转矩阵RUVW-XYZ为：
(1)中rij为矩阵RuvW-XYZ中元素，得到刚体此时的RPY角度，RPY是pitch-yaw-roll的缩写，α、β、θ为：
T表示转置。
2.如权利要求1所述方法，其特征是，所述传感器坐标系定义为：以转接法兰底面中心为坐标系原点，以过原点垂直于转接法兰底面指向跟踪仪靶球坐的射线为坐标系Z轴，由原点指向第一个WMPS接收器在法兰底面投影点的射线为坐标系X轴，传感器Y轴遵循右手定则。
3.如权利要求1所述方法，其特征是，所述WMPS系统测量坐标系定义为：当没有特殊要求时，以测量系统内编号为1的发射站坐标系为测量坐标系。
4.一种工作空间六自由度位姿动态测量设备，其特征是，包括：
发射站用于向外发射带有角度信息的光信号；
六自由度传感器，内部设置有WMPS接收器用于接收发射站发出的光信号，六自由度传感器根据WMPS接收器收到光信号后的输出计算出发射器转台转过的角度信息，六自由度传感器还设置有Zigbee无线通讯接口，用于接收来自上位机开始测量的命令，并将角度信息输送到上位计算机，六自由度位姿传感器结构具体为：
传感器壳外形为长方体空腔，长方体截面为正方形，长方体每个侧面及棱边上分为上下两排交错安装8个WMPS接收器，其中最上排4个WMPS接收器与壳体外壁保持一定夹角，从而保证传感器四周及顶端均有3个或3个以上WMPS接收器可以接收测量空间中发射器发来的光脉冲，同时壳体顶端设计有激光跟踪仪靶球坐以方便现场比对；
传感器壳体下部安装有转接法兰，法兰四周设计有定位通孔，法兰中央设计有定位销孔，同通过转接法兰可将传感器安装在待测物体表面；
传感器壳体内部安装有WMPS接收器信号采集处理电路，WMPS信号采集处理电路核心元件为大规模FPGA芯片，FPGA芯片外部配有FLASH芯片、DDR芯片及Zigbee无线通讯芯片，FPGA芯片内部搭建有SOPC嵌入式处理系统电路，系统内集成有32位RSIC处理器内核、总线及内存控制器、闪存控制器、SPI串行接口外设，WMPS信号采集处理电路通过处理器总线上的8个数据采集通道完成，每通道负责采集一个外部WMPS接收器信号，依靠通道内配置的专用FIFO实现不同通道信号的同时触发、同时采集，数据采集时处理器通过总线上的同步采集控制模块控制所有通道同时采集WMPS接收器电脉冲，当收到WMPS接收器电脉冲时，触发模块锁存FPGA内部计时器时间值并将其写入该通道专用FIFO，当采集结束，处理器从各通道FIFO中依次读出所缓存的时间信息，并结合预先存储的发射器转速计算发射器转台转角；
影像仪、测量臂，用于标定六自由度传感器上的WMPS接收器光学中心在六自由度传感器坐标系UVW下精确坐标；
上位计算机，用于：
1)以覆盖测量空间的WMPS系统坐标系作为全局测量坐标系；接收角度信息，当WMPS接收器接收到两台以上发射站发出的光脉冲后，通过角度交会测量方法测得WMPS接收器光学中心在WMPS系统坐标系XYZ下坐标；
2)解算得到传感器表面三个或三个以上WMPS接收器坐标后，以可测WMPS接收器几何中心为中心建立过渡坐标系X′Y′Z′并得到过渡坐标系到WMPS系统坐标系的旋转矩阵RX’Y’Z’-XYZ及平移矢量TX′Y′Z′-XYZ，同时通过预先标定的WMPS接收器在传感器坐标系UVW下精确坐标也得到该过渡坐标系到传感器坐标系的旋转矩阵RX′Y′Z′-UVW及平移矢量TX′Y′Z′-UVW；
3)此时，传感器坐标系UVW到WMPS坐标系XYZ的旋转矩阵RUVW-XYZ通过下式计算：
六自由度传感器坐标系原点OXYZ在WMPS坐标系XYZ下的坐标可通过下式计算：
4)当传感器被固定在运动物体表面待测关键点时，运动物体表面关键点坐标即OXYZ，假定测得的传感器坐标系UVW到WMPS坐标系XYZ的旋转矩阵RUVW-XYZ为：
(4)式中rij为计算得出的矩阵RUVW-XYZ中元素，得到刚体此时的RPY角度，RPY是pitch-yaw-roll的缩写，角度α、β、θ为：

工作空间六自由度位姿动态测量设备及方法\n技术领域\n[0001] 本发明属于工业现场大尺寸三维坐标测量方法，特别涉及一种基于工作空间测量定位系统的工作空间六自由度位姿动态测量设备及方法。\n背景技术\n[0002] 刚体六自由度用于描述其空间位置与姿态，即刚体沿X、Y、Z三个坐标方向的位移及自身滚动(roll)、俯仰(pitch)，偏航(yaw)三个角度。随着多轴运动控制技术在机器人研究、大型工件装配、柔性制造等领域的广泛应用，如何及时测量物体的六自由度参数为运动控制系统提供有效反馈已成为工业及科研领域共同关注的焦点。由于传统方法无法同时得到互相关联的六个自由度参数，现有六自由度测量方案主要采用激光跟踪仪或摄影测量技术，世界范围内只有Leica、V-STARS等少数厂商提供相关测量产品。但由于受到测量原理限制，这些方案在使用中各自存在严重问题：跟踪仪测量通视要求较高，测量过程中仪器与待测物间不允许有其他物体遮挡光路，而且跟踪仪为单站测量设备，无法实现多物体的同时跟踪；摄影测量方法依赖复杂的图像处理手段，难以实现实时测量，目前的光学制造水平也限制了摄影测量的精度和范围。\n发明内容\n[0003] 为克服现有技术的不足，以工作空间测量定位系统(WMPS：Workspace Measurement Positioning System)为基础提供一种测量精度高、范围大，不受临时遮挡的影响，更适于工业现场使用的自由度位姿测量方法，本发明采取的技术方案是，工作空间六自由度位姿动态测量设备及方法，主要包括下列步骤：\n[0004] 步骤1.组装六自由度位姿传感器，在传感器组装完成后，依靠包括影像仪、测量臂等高精度辅助工具标定传感器表面接收器光学中心在传感器坐标系UVW下精确坐标；\n[0005] 步骤2.布置好发射站，待发射站运行平稳后建立WMPS系统测量坐标系；将六自由度位姿传感器固定在待测物体表面，打开传感器电源，以覆盖测量空间的WMPS系统坐标系作为全局测量坐标系；\n[0006] 步骤3.打开上位机测量软件，通过Zigbee无线通讯网络向传感器发送测量命令，待传感器返回接收器角度信息后解算接收器在测量坐标系下坐标，当接收器接收到两台以上发射站发出的光脉冲后，可通过角度交会测量方法测得接收器光学中心在WMPS系统坐标系XYZ下坐标；\n[0007] 步骤4.解算得到传感器表面三个或三个以上接收器坐标之后，以可测接收器几何中心为中心建立过渡坐标系X′Y′Z′并得到过渡坐标系到WMPS系统坐标系的旋转矩阵RX′Y′Z′-XYZ及平移矢量TX′Y′Z′-XYZ，下标X′Y′Z′-XYZ表示二者为过渡坐标系X′Y′Z′到WMPS系统坐标系XYZ的坐标变换关系，同时通过预先标定的接收器在传感器坐标系UVW下精确坐标也可得到该过渡坐标系到传感器坐标系的旋转矩阵RX′Y′Z′-UVW及平移矢量TX′Y′Z′-UVW，下标X′Y′Z′-UVW表示此二者为过渡坐标系X′Y′Z′到传感器坐标系UVW坐标变换关系；此时，传感器坐标系UVW到WMPS坐标系XYZ的旋转矩阵RUVW-XYZ可通过 式计算，传感器坐标系原点在WMPS坐标系下的坐标\nOXYZ为：\n[0008] \n[0009] 步骤5.当传感器被固定在运动物体表面待测关键点时，二者可视为同一刚体，则运动物体关键点坐标为OXYZ，假定测得的传感器坐标系UVW到WMPS坐标系XYZ的旋转矩阵RUVW-XYZ为：\n[0010] \n[0011] (1)中rij为矩阵RUVW-XYZ中元素，得到刚体此时的RPY(pitch-yaw-roll)角度α、β、θ为：\n[0012] \n[0013] T表示转置。\n[0014] 所述传感器坐标系定义为：以转接法兰底面中心为坐标系原点，以过原点垂直于转接法兰底面指向跟踪仪靶球坐的射线为坐标系Z轴，由原点指向接收器1在法兰底面投影点的射线为坐标系X轴，传感器Y轴遵循右手定则。\n[0015] 所述WMPS系统测量坐标系定义为：当没有特殊要求时，以测量系统内编号为1的发射站坐标系为测量坐标系。\n[0016] 工作空间六自由度位姿动态测量设备，包括：\n[0017] 发射站用于向外发射带有角度信息的光信号；\n[0018] 六自由度传感器，内部设置有WMPS接收器用于接收发射站发出的光信号，六自由度传感器根据接收器收到光信号后的输出计算出发射器转台转过的角度信息，六自由度传感器还设置有Zigbee无线通讯接口，用于接收来自上位机开始测量的命令，并将角度信息输送到上位计算机；\n[0019] 影像仪、测量臂，用于标定六自由度传感器上的接收器光学中心在六自由度传感器坐标系UVW下精确坐标；\n[0020] 上位计算机，用于：\n[0021] 1)以覆盖测量空间的WMPS系统坐标系作为全局测量坐标系；接收角度信息，当接收器接收到两台以上发射站发出的光脉冲后，通过角度交会测量方法测得接收器光学中心在WMPS系统坐标系XYZ下坐标；\n[0022] 2)解算得到传感器表面三个或三个以上接收器坐标后，以可测接收器几何中心为中心建立过渡坐标系X′Y′Z′并得到过渡坐标系到WMPS系统坐标系的旋转矩阵RX′Y′Z′-XYZ及平移矢量TX′Y′Z′-XYZ，同时通过预先标定的接收器在传感器坐标系UVW下精确坐标也得到该过渡坐标系到传感器坐标系的旋转矩阵RX′Y′Z′-UVW及平移矢量TX′Y′Z′-UVW；\n[0023] 3)此时，传感器坐标系UVW到WMPS坐标系XYZ的旋转矩阵RUVW-XYZ通过下式计算：\n[0024] \n[0025] 六自由度传感器坐标系原点OXYZ在WMPS坐标系XYZ下的坐标可通过下式计算：\n[0026] \n[0027] 4)当传感器被固定在运动物体表面待测关键点时，运动物体表面关键点坐标即OXYZ，假定测得的传感器坐标系UVW到WMPS坐标系XYZ的旋转矩阵RUVW-XYZ为：\n[0028] \n[0029] (4)式 中 rij为 计 算 得 出 的 矩 阵RUVW-XYZ中 元 素，则 刚 体 此 时 的RPY(pitch-yaw-roll)角度α、β、θ为：\n[0030] \n[0031] 六自由度传感器构成为：\n[0032] 传感器壳外形为长方体空腔，长方体截面为正方形，长方体每个侧面及棱边上分为上下两排交错安装8个WMPS接收器，其中最上排4个WMPS接收器与壳体外壁保持一定夹角，从而保证传感器四周及顶端均有3个或3个以上接收器可以接收测量空间中发射器发来的光脉冲，同时壳体顶端设计有激光跟踪仪靶球坐以方便现场比对；\n[0033] 传感器壳体下部安装有转接法兰，法兰四周设计有定位通孔，法兰中央设计有定位销孔，同通过转接法兰可将传感器安装在待测物体表面；\n[0034] 传感器壳体内部安装有WMPS接收器信号采集处理电路，WMPS信号采集处理电路核心元件为大规模FPGA芯片，FPGA芯片外部配有FLASH芯片、DDR芯片及Zigbee无线通讯芯片，FPGA芯片内部搭建有SOPC嵌入式处理系统电路，系统内集成有32位RSIC处理器内核、总线及内存控制器、闪存控制器、SPI串行接口外设，WMPS信号采集处理电路通过处理器总线上的8个数据采集通道完成，每通道负责采集一个外部接收器信号，依靠通道内配置的专用FIFO实现不同通道信号的同时触发、同时采集，数据采集时处理器通过总线上的同步采集控制模块控制所有通道同时采集接收器电脉冲，当收到接收器电脉冲时，触发模块锁存FPGA内部计时器时间值并将其写入该通道专用FIFO，当采集结束，处理器从各通道FIFO中依次读出所缓存的时间信息，并结合预先存储的发射器转速计算发射器转台转角。\n[0035] 本发明具有如下技术效果：\n[0036] 本发明采用工作空间测量定位系统，并将多个WMPS系统接收器及此信号处理器集成在一个位姿传感器内，通过同步触发技术同时测量所有接收器坐标，以位姿传感器实现待测物六自由度参数的动态跟踪测量，因而本发明具有测量精度高、范围大，不受临时遮挡的影响，更适于工业现场使用的特点。\n附图说明\n[0037] 图1采用WMPS系统实现六自由度位姿测量。图中，21-24表示4个激光发射站，25为工业用自动导航小车，26为6自由度位姿传感器，27为工业机器人，101、201分别表示激光平面。\n[0038] 图26自由度位姿感器外形。图中，1-8表示8个接收器，9表示壳体，10表示转接法兰，11表示跟踪仪靶球座。\n[0039] 图3信号采集处理系统结构框图。\n具体实施方式\n[0040] 针对现有六自由度测量系统存在的问题，本发明提出一种采用室内测量定位系统的六自由度位姿动态测量方法。本发明所涉及的方法是基于论文《扫描平面激光空间定位系统测量网络的构建》中所述的工作空间测量定位系统(WMPS：Workspace Measurement Positioning System)。该系统为多站分布式室内定位系统，针对大型制造业测量需求特点可实现大尺度空间坐标的网络化高精度自动测量。WMPS系统组成如图1所示，采用基于光电扫描的空间角度自动测量方法对单个光电接收器(简称接收器)进行定位，发射站在工作时不负责解算接收器坐标，而是通过向外发射带有角度信息的光信号，为测量空间内的光电接收器提供定位服务。系统内每个接收器收到发射站光信号后自动计算自身在各个发射站坐标系的下的角度信息，并结合已知的发射站方位信息使用角度交会方法计算自身三维坐标。\n[0041] 本发明的目的在于克服现有技术不足，充分利用WMPS系统网络化并行测量的特点，提出一种采用室内测量定位系统的六自由度位姿动态测量方法，将多个WMPS系统接收器及此信号处理器集成在一个位姿传感器内，通过同步触发技术同时测量所有接收器坐标，以位姿传感器实现待测物六自由度参数的动态跟踪测量。\n[0042] 本发明技术方案是这样实现的：\n[0043] 1、所述六自由度位姿传感器如图2所示，传感器整体为呈柱形，由壳体，转接法兰，WMPS接收器等主要部件组成，传感器结构具有以下特点：\n[0044] 1)传感器壳外形为长方体空腔，长方体截面为正方形，长方体每个侧面及棱边上分为上下两排交错安装8个WMPS接收器。其中最上排4个WMPS接收器与壳体外壁保持一定夹角，从而保证传感器四周及顶端均有3个或3个以上接收器可以接收测量空间中发射器发来的光脉冲。同时壳体顶端设计有激光跟踪仪靶球坐以方便现场比对。\n[0045] 2)传感器壳体下部安装有转接法兰，法兰四周设计有定位通孔，法兰中央设计有定位销孔。同通过转接法兰可将传感器安装在待测物体表面，如附图1所示。\n[0046] 3)传感器壳体内部安装有WMPS系统接收器信号采集处理电路，可并行处理壳体表面接收器信号，信号采集处理电路通过导线与壳体表面接收器连接。\n[0047] 4)传感器坐标系定义为：以转接法兰底面中心为坐标系原点，以过原点垂直于转接法兰底面指向跟踪仪靶球坐的射线为坐标系Z轴，由原点指向接收器1在法兰底面投影点的射线为坐标系X轴，传感器Y轴遵循右手定则。\n[0048] 2、所述信号采集处理电路系统框图如图3所示，\n[0049] 1)所述六自由度传感器工作时，其表面接收器接收到测量区域内发射器发出的光信号并将其转换为电脉冲信号。信号采集处理电路对接收器产生的电脉冲信号间隔进行测量并得到发射站旋转角度信息。\n[0050] 2)所述信号采集处理电路核心元件为大规模FPGA芯片。FPGA芯片外部配有FLASH芯片、DDR芯片及Zigbee无线通讯芯片。FPGA芯片内部搭建有SOPC嵌入式处理系统电路，系统内集成有32位RSIC处理器内核、总线及内存控制器、闪存控制器、SPI串行接口等外设。\n[0051] 3)传感器数据采集通过处理器总线上的8个数据采集通道完成，每通道负责采集一个外部接收器信号，依靠通道内配置的专用FIFO实现不同通道信号的同时触发、同时采集。数据采集时处理器通过总线上的同步采集控制模块控制所有通道同时采集接收器电脉冲。当收到接收器电脉冲时，触发模块锁存FPGA内部计时器时间值并将其写入该通道专用FIFO。当采集结束，处理器从各通道FIFO中依次读出所缓存的时间信息，并结合预先存储的发射器转速计算发射器转台转角。\n[0052] 4)上位机通过Zigbee无线通讯接口向信号采集处理电路发送开始测量命令，当数据采集处理完毕，信号采集处理电路通过Zigbee接口将数据发送回上位机进行解算，即解算坐标在上位机进行。\n[0053] 3、采用所述传感器实现六自由度位姿测量原理：\n[0054] 1)依靠影像仪、测量臂等高精度辅助工具标定传感器表面接收器光学中心在传感器坐标系UVW下精确坐标。以覆盖测量空间的WMPS系统坐标系作为全局测量坐标系。当接收器接收到两台以上发射站发出的光脉冲后，可通过角度交会测量方法测得接收器光学中心在WMPS系统坐标系XYZ下坐标。角度交会测量方法是已经公开的一种坐标测量方法，在论文《Optical measurement techniques for mobile and large-scale dimensional metrology》中有详细分类及解释。\n[0055] 2)解算得到传感器表面三个或三个以上接收器坐标后，可以可测接收器几何中心为中心建立过渡坐标系X′Y′Z′并得到过渡坐标系到WMPS系统坐标系的旋转矩阵RX′Y′Z′-XYZ及平移矢量TX′Y′Z′-XYZ。同时通过预先标定的接收器在传感器坐标系UVW下精确坐标也可得到该过渡坐标系到传感器坐标系的旋转矩阵RX′Y′Z′-UVW及平移矢量TX′Y′Z′-UVW[0056] 3)此时，传感器坐标系UVW到WMPS坐标系XYZ的旋转矩阵RUVW-XYZ可通过下式计算：\n[0057] \n[0058] 传感器坐标系原点在WMPS坐标系下的坐标为：\n[0059] \n[0060] 4)当传感器被固定在运动物体表面待测关键点时，二者可视为同一刚体。则运动物体关键点坐标为OXYZ，假定测得的传感器坐标系UVW到WMPS坐标系XYZ的旋转矩阵RUVW-XYZ为：\n[0061] \n[0062] (8)式 中 rij为 计 算 得 出 的 矩 阵RUVW-XYZ中 元 素，则 刚 体 此 时 的RPY(pitch-yaw-roll)角度α、β、θ为：\n[0063] \n[0064] RPY角度是一种用于描述刚体姿态的方法，可作为三个基本旋转的序列复合生成刚体局部坐标系到全局坐标系的旋转矩阵，局部坐标系关于全局坐标系的x，y和z轴的旋转角度分别叫做roll，pitch和yaw旋转。\n[0065] 下面结合附图和具体实施方式进一步详细说明本发明。\n[0066] 所述六自由度位姿测量方法采用以下方式实现：\n[0067] 1、依据附图3组装六自由度位姿传感器，在传感器组装完成后，依靠影像仪、测量臂等高精度辅助工具标定传感器表面接收器光学中心在传感器坐标系UVW下精确坐标。\n[0068] 2、测量时布置好发射站，待发射站运行平稳后建立WMPS系统测量坐标系，并将所述六自由度位资传感器固定在待测物体表面，打开传感器电源。以覆盖测量空间的WMPS系统坐标系作为全局测量坐标系。打开上位机测量软件，通过Zigbee无线通讯网络向传感器发送测量命令。待传感器返回接收器角度信息后解算接收器在测量坐标系下坐标。当接收器接收到两台以上发射站发出的光脉冲后，可通过角度交会测量方法测得接收器光学中心在WMPS系统坐标系XYZ下坐标。\n[0069] 3、解算得到传感器表面三个或三个以上接收器坐标之后，可以可测接收器几何中心为中心建立过渡坐标系X’Y’Z’并得到过渡坐标系到WMPS系统坐标系的旋转矩阵RX′Y′Z′-XYZ平移矢量RX′Y′Z′-XYZ。同时通过预先标定的接收器在传感器坐标系UVW下精确坐标也可得到该过渡坐标系到传感器坐标系的旋转矩阵RX′Y′Z′-UVW及平移矢量TX′Y′Z′-UVW此时，传感器坐标系UVW到WMPS坐标系XYZ的旋转矩阵RUVW-XYZ可通过(1)式计算。传感器坐标系原点在WMPS坐标系下的坐标为：\n[0070] \n[0071] 4、当传感器被固定在运动物体表面待测关键点时，二者可视为同一刚体。则运动物体关键点坐标为OXYZ，假定测得的传感器坐标系UVW到WMPS坐标系XYZ的旋转矩阵RUVW-XYZ为：\n[0072] \n[0073] 刚体此时的RPY(pitch-yaw-roll)角度α、β、θ为：\n[0074] \n[0075] T在矩阵中表示转置，这里的T表示矩阵 为矩阵RX′Y′Z′-UVW的转置矩阵。