CN103398681B - 一种六自由度立体定位装置和方法 - Google Patents

Abstract

本发明涉及一种六自由度立体定位装置和方法，特别是涉及一种通过无线测距的六自由度立体定位装置和方法。本发明的一种六自由度立体定位装置包含三个子装置，RFID读写控制器、六自由度立体定位发送装置和六自由度立体定位接收装置。本发明的定位方法是将所述接收装置设定在外部参考点上，将所述发射装置设定在被测物体上，所述RFID读写控制器、所述接收装置和所述发送装置执行预定控制指令，所述RFID读写控制器利用数值方法计算得出被测物体的六自由度立体定位信息。本发明提供的六自由度立体定位装置和方法，无需精密瞄准设备，无需设定大量辅助标记物，测量过程较为简单，应用场景更为广阔。

Description

一种六自由度立体定位装置和方法

技术领域

本发明涉及一种六自由度立体定位装置和方法，特别是涉及一种通过无线测距的六自由度立体定位装置和方法。

背景技术

随着工业生产方式的变化和行业需求的发展，出现了各种定位需求，其中包括对刚体的六个自由度的测量需求。现存在几种接触式的六自由度测量方法，但在有些场景下，接触式测量会给被测刚体的运动带来一定阻碍，并且接触式测量会带来额外的测量误差。

现有的无线立体定位标签如《一种简单精确的射频定位***和方法》专利公开号WO2013007089所述的***只能够定位其标签所在的立***置，不能测量器标签所指立体方位角，还有一些定位装置是利用陀螺仪进行角度定位，但陀螺仪难以避免累积误差。

此外相关论文如《六自由度测试***》孙长库等，仪器仪表学报，Vol.19No.4，《同时测量物体六自由度的方法》匡萃方等，激光技术，Vol.29No.5，《六自由度运动的非接触式测量***的研究》施奇等，实验技术与管理，Vol.26No.8公开了一些测量物体六自由度的方法，但这些***测量过程较为繁琐，需要设定大量瞄准设备和辅助标记物，待测刚体距离较远时解析度较低，应用场景受限。

发明内容

本发明的目的是提供一种六自由度立体定位装置和方法，特别是提供一种通过无线测距的六自由度立体定位装置和方法，利用无线测距技术测量出足够多的距离数据，通过数值计算方法计算出六自由度立体定位信息。

本发明的一种六自由度立体定位装置，包含三个子装置，RFID读写控制器、六自由度立体定位发送装置和六自由度立体定位接收装置；

所述RFID读写控制器，包括RFID读写模块、处理控制单元和人机交互界面；

所述六自由度立体定位发送装置，包括无线电测距发射器、转向与角度测量构件、处理单元、RFID标签和基板；所述六自由度立体定位发送装置，利用其转向与角度测量构件的旋转，使其无线电测距发射器设置于三个不共线的发射参考点；

所述六自由度立体定位接收装置，包括无线电测距接收器、转向与角度测量构件、处理单元、RFID标签和基板；所述六自由度立体定位接收装置，利用其转向与角度测量构件的旋转，使其无线电测距接收器设置于三个不共线的接收参考点。

作为优选的技术方案：

所述RFID读写控制器中，所述RFID读写模块为一块可嵌入于移动式设备的中长距离超高频射频标签读写器模块，完成中长距离超高频射频标签读写功能；

所述处理控制单元根据使用者的指示，通过所述RFID读写模块，按一定的顺序发出控制指令并接收测量结果，利用数值方法计算出六自由度立体定位信息；

所述人机交互界面，用于接收使用者的指示并输出被测物体的六自由度立体定位测量结果。

所述六自由度立体定位发送装置中，所述转向与角度测量构件有两个端点，一端在基板上作为转向中心点，所述转向与角度测量构件能以转轴中心点为原点，遵从控制指令，在平行于基板的平面上转动到指定角度，另一端用于固定所述无线电测距发射器，所述无线电测距发射器与所述转向与角度测量构件的转向中心点之间的距离恒定已知为r₁；

所述六自由度立体定位接收装置中，所述转向与角度测量构件有两个端点，一端在基板上作为转向中心点，所述转向与角度测量构件能以转轴中心点为原点，遵从控制指令，在平行于基板的平面上转动到指定角度，另一端用于固定所述无线电测距接收器，所述无线电测距接收器与所述转向与角度测量构件的转向中心点之间的距离恒定已知为r₂；

所述处理单元执行控制指令并反馈控制指令执行结果；

所述RFID标签是一块可嵌入于移动式设备的中长距离超高频射频标签，用于接收RFID读写控制器的控制指令，将控制指令转交给所述处理单元，并将控制指令执行结果反馈至RFID读写控制器；

所述基板用于承载所述转向与角度测量构件、处理单元和RFID标签；

所述无线电测距发射器通过无线电发射无线测距参考信号；

所述无线电测距接收器通过接收无线电测距发射器发射的无线测距参考信号，测量无线电测距接收器与无线电测距发射器之间的直线距离。

本发明还提供了一种六自由度立体定位方法，将所述接收装置设定在外部参考点上，将所述发射装置设定在被测物体上，所述RFID读写控制器、所述接收装置和所述发送装置执行预定控制指令，所述RFID读写控制器利用数值方法计算得出被测物体的六自由度立体定位信息；

作为优选的技术方案：

所述接收装置设定方法是：以外部参考点为原点建立三维右手直角坐标系，且规定外部参考点所在切面为xoy平面；将所述接收装置设置在外部参考点所在切面上，使得所述接收装置的转向与角度测量构件的转轴中心点与外部参考点重合，使得所述三维右手直角坐标系的x轴正方向为所述接收装置的转向与角度测量构件的角度0方向，规定逆时针方向为角度增加方向；

所述发射装置设定方法是：以被测物体的待测点为原点，在被测物体的待测点所在切面上建立二维直角坐标系；将所述发送装置设置在被测物体的待测点所在切面上，使得所述发送装置的转向与角度测量构件的转轴中心点与被测物体上的待测点重合，使得所述二维右手直角坐标系的x轴正方向为所述发送装置的转向与角度测量构件的角度0方向，规定逆时针方向为角度增加方向；

所述预定控制指令的流程如下：

发送装置将转向与角度测量构件至旋转至角度0，使得无线电测距发射器处于发射参考点1；

接收装置将转向与角度测量构件至旋转至角度0，使得无线电测距接收器处于接收参考点1；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L1；

接收装置将转向与角度测量构件至旋转至角度B1，使得无线电测距接收器处于接收参考点2；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L2；

发送装置将转向与角度测量构件至旋转至角度A1，使得无线电测距发射器处于发射参考点2；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L3；

接收装置将转向与角度测量构件至旋转至角度B2，使得无线电测距接收器处于接收参考点3；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L4；

发送装置将转向与角度测量构件至旋转至角度A2，使得无线电测距发射器处于发射参考点3；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L5；

接收装置将转向与角度测量构件至旋转至角度0，使得无线电测距接收器处于接收参考点1；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L6；

在整个过程中，需要保证所述六自由度立体定位接收装置的无线电测距接收器始终处于所述六自由度立体定位发送装置的无线电测距发射器的视距可见范围内。

计算被测物体的六自由度立体定位信息的数值方法如下：

设所述接收参考点1、接收参考点2和接收参考点3的三维直角坐标系坐标分别为(x₁，y₁，z₁)、(x₂，y₂，z₂)和(x₃，y₃，z₃)，满足方程

(x₁，y₁，z₁)＝(r₂，0，0)

(x₂，y₂，z₂)＝(r₂cos(B1)，r₂sin(B1)，0)

(x₃，y₃，z₃)＝(r₂cos(B2)，r₂sin(B2)，0)

利用数值方法，求解出接收参考点1、接收参考点2和接收参考点3的三维直角坐标系坐标；

设所述被测物体的待测点的六自由度立体定位信息为(x，y，z，α，β，γ)，设所述发射参考点1、发射参考点2和发射参考点3的三维直角坐标系坐标为(x′₁，y′₁，z′₁)、(x′₂，y′₂，z′₂)和(x′₃，y′₃，z′₃)，满足方程组

$\left\{\begin{array}{c}{[x_1^{\′},y_1^{\′},z_1^{\′}]}^T=R{[r_1,\mathrm{0,0}]}^T+{[x,y,z]}^T\\ {[x_2^{\′},y_2^{\′},z_2^{\′}]}^T=R{[r_1\cos \left(A1\right),r_1\sin \left(A1\right),0]}^T+{[x,y,z]}^T\\ {[x_3^{\′},y_3^{\′},z_3^{\′}]}^T=R{[r_1\cos \left(A2\right),r_1\sin \left(A2\right),0]}^T+{[x,y,z,]}^T\\ {\left|\right|{[x_1^{\′},y_1^{\′},z_1^{\′}]}^T-{[x_1,y_1,z_1]}^T\left|\right|}_2=L1\\ {\left|\right|{[x_1^{\′},y_1^{\′},z_1^{\′}]}^T-{[x_2,y_2,z_2]}^T\left|\right|}_2=L2\\ {\left|\right|{[x_2^{\′},y_2^{\′},z_2^{\′}]}^T-{[x_2,y_2,z_2]}^T\left|\right|}_2=L3\\ {\left|\right|{[x_2^{\′},y_2^{\′},z_2^{\′}]}^T-{[x_3,y_3,z_3]}^T\left|\right|}_2=L4\\ {\left|\right|{[x_3^{\′},y_3^{\′},z_3^{\′}]}^T-{[x_3,y_3,z_3]}^T\left|\right|}_2=L5\\ {\left|\right|{[x_3^{\′},y_3^{\′},z_3^{\′}]}^T-{[x_1,y_1,z_1]}^T\left|\right|}_2=L6\end{array}\right.$

其中，[]^T表示向量的转置，||||₂表示向量的欧几里得范数，R为旋转矩阵，且有

R＝R_x(γ)R_y(β)R_z(α)

$R_x\left(\mathrm{\γ}\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\γ}& -\sin \mathrm{\γ}\\ 0& \sin \mathrm{\γ}& \cos \mathrm{\γ}\end{array}\right],R_y\left(\mathrm{\β}\right)=\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& \sin \mathrm{\β}\\ 0& 1& 0\\ -\sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right],R_z\left(\mathrm{\α}\right)=\left[\begin{array}{ccc}\cos \mathrm{\α}& -\sin \mathrm{\α}& 0\\ \sin \mathrm{\α}& \cos \mathrm{\α}& 0\\ 0& 0& 1\end{array}\right]$

利用数值方法，求解所述方程组，取视距范围内实数解，得到被测物体的待测点的六自由度立体定位信息。

有益效果

本发明的一种六自由度立体定位装置和方法，六自由度立体定位装置包含RFID读写控制器、六自由度立体定位发送装置和六自由度立体定位接收装置三个子装置。在实际应用中，可以将六自由度立体定位发送装置和六自由度立体定位接收装置分别放置于被测物体和参考物体上，六自由度立体定位发送装置和六自由度立体定位接收装置依次执行由RFID读写控制器发出的特定指令序列，利用无线测距技术测量出足够多的距离数据，通过数值计算方法计算出六自由度立体定位信息。与传统的六自由度立体定位装置和方法相比，本发明提供的六自由度立体定位装置和方法，无需精密瞄准设备，无需设定大量辅助标记物，测量过程较为简单，应用场景更为广阔。

附图说明

图1是六自由度立体定位装置示意图，

图2是RFID读写控制器示意图

图3是六自由度立体定位发送装置示意图

图4是六自由度立体定位接收装置示意图

其中1是RFID读写控制器，2是六自由度立体定位发送装置，3是六自由度立体定位接收装置。

其中11是RFID读写模块，12是处理控制单元，13是人机交互界面。

其中21是无线电测距发射器，22是转向与角度测量构件，23是处理单元，24是RFID标签，25是基板。

其中31是包括无线电测距接收器，32是转向与角度测量构件，33是处理单元，34是RFID标签，35是基板。

具体实施方式

下面结合具体实施方式，进一步阐述本发明。应理解，这些实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本申请所附权利要求书所限定的范围。

如图1所示，六自由度立体定位装置包括三个子装置，RFID读写控制器1、六自由度立体定位发送装置2、六自由度立体定位接收装置3。

如图2所示，RFID读写控制器1，包括RFID读写模块11、处理控制单元12、人机交互界面13；所述RFID读写模块11为一块可嵌入于移动式设备的中长距离超高频射频标签读写器模块，完成中长距离超高频射频标签读写功能；所述处理控制单元12根据使用者的指示，通过所述RFID读写模块，按一定的顺序发出控制指令并接收测量结果，利用数值方法计算出六自由度立体定位信息；所述人机交互界面13，用于接收使用者的指示并输出被测物体的六自由度立体定位测量结果。

如图3所示，六自由度立体定位发送装置2(后简称发送装置)，包括无线电测距发射器21、转向与角度测量构件22、处理单元23、RFID标签24、基板25；无线电测距发射器21通过无线电发射无线测距参考信号；转向与角度测量构件22有两个端点，一端在基板25上作为转向中心点，所述转向与角度测量构件能以转轴中心点为原点，遵从控制指令，在平行于基板25的平面上转动到指定角度，另一端用于固定所述无线电测距发射器，所述无线电测距发射器与所述转向与角度测量构件的转向中心点之间的距离恒定已知为r₁；处理单元23执行控制指令并反馈控制指令执行结果；RFID标签24是一块可嵌入于移动式设备的中长距离超高频射频标签，用于接收RFID读写控制器1的控制指令，将控制指令转交给所述处理单元23，并将控制指令执行结果反馈至RFID读写控制器1；基板25用于承载所述转向与角度测量构件22、处理单元23和RFID标签24。

如图4所示，六自由度立体定位接收装置3(后简称接收装置)，包括无线电测距接收器31、转向与角度测量构件32、处理单元33、RFID标签34、基板35；无线电测距接收器31通过接收无线电测距发射器21发射的无线测距参考信号，测量无线电测距接收器21与无线电测距发射器32之间的直线距离；转向与角度测量构件32有两个端点，一端在基板上作为转向中心点，转向与角度测量构件32能以转轴中心点为原点，遵从控制指令，在平行于基板35的平面上转动到指定角度，另一端用于固定无线电测距接收器31，无线电测距接收器31与转向与角度测量构件32的转向中心点之间的距离恒定已知为r₂；处理单元33执行控制指令并反馈控制指令执行结果；RFID标签34是一块可嵌入于移动式设备的中长距离超高频射频标签，用于接收RFID读写控制器1的控制指令，将控制指令转交给所述处理单元33，并将控制指令执行结果反馈至RFID读写控制器1；基板35用于承载所述转向与角度测量构件32、处理单元33和RFID标签34。

本发明还提供了一种六自由度立体定位方法，将所述接收装置设定在外部参考点上，将所述发射装置设定在被测物体上，所述RFID读写控制器、所述接收装置和所述发送装置执行预定控制指令，所述RFID读写控制器利用数值方法计算得出被测物体的六自由度立体定位信息；

其中，所述接收装置设定方法是：以外部参考点为原点建立三维右手直角坐标系，且规定外部参考点所在切面为xoy平面；将所述接收装置设置在外部参考点所在切面上，使得所述接收装置的转向与角度测量构件的转轴中心点与外部参考点重合，使得所述三维右手直角坐标系的x轴正方向为所述接收装置的转向与角度测量构件的角度0方向，规定逆时针方向为角度增加方向；

所述发射装置设定方法是：以被测物体的待测点为原点，在被测物体的待测点所在切面上建立二维直角坐标系；将所述发送装置设置在被测物体的待测点所在切面上，使得所述发送装置的转向与角度测量构件的转轴中心点与被测物体上的待测点重合，使得所述二维右手直角坐标系的x轴正方向为所述发送装置的转向与角度测量构件的角度0方向，规定逆时针方向为角度增加方向；

所述预定控制指令的流程如下：

发送装置将转向与角度测量构件至旋转至角度0，使得无线电测距发射器处于发射参考点1；

接收装置将转向与角度测量构件至旋转至角度0，使得无线电测距接收器处于接收参考点1；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L1；

接收装置将转向与角度测量构件至旋转至角度B1，使得无线电测距接收器处于接收参考点2；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L2；

发送装置将转向与角度测量构件至旋转至角度A1，使得无线电测距发射器处于发射参考点2；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L3；

接收装置将转向与角度测量构件至旋转至角度B2，使得无线电测距接收器处于接收参考点3；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L4；

发送装置将转向与角度测量构件至旋转至角度A2，使得无线电测距发射器处于发射参考点3；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L5；

接收装置将转向与角度测量构件至旋转至角度0，使得无线电测距接收器处于接收参考点1；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L6；

在整个过程中，需要保证所述六自由度立体定位接收装置的无线电测距接收器始终处于所述六自由度立体定位发送装置的无线电测距发射器的视距可见范围内。

计算被测物体的六自由度立体定位信息的数值方法如下：

设所述接收参考点1、接收参考点2和接收参考点3的三维直角坐标系坐标分别为

(x₁，y₁，z₁)、(x₂，y₂，z₂)和(x₃，y₃，z₃)，满足方程

(x₁，y₁，z₁)＝(r₂，0，0)

(x₂，y₂，z₂)＝(r₂cos(B1)，r₂sin(B1)，0)

(x₃，y₃，z₃)＝(r₂cos(B2)，r₂sin(B2)，0)

利用数值方法，求解出接收参考点1、接收参考点2和接收参考点3的三维直角坐标系坐标；

设所述被测物体的待测点的六自由度立体定位信息为(x，y，z，α，β，γ)，设所述发射参考点1、发射参考点2和发射参考点3的三维直角坐标系坐标为(x′₁，y′₁，z′₁)、(x′₂，y′₂，z′₂)和(x′₃，y′₃，z′₃)，满足方程组

$\left\{\begin{array}{c}{[x_1^{\′},y_1^{\′},z_1^{\′}]}^T=R{[r_1,\mathrm{0,0}]}^T+{[x,y,z]}^T\\ {[x_2^{\′},y_2^{\′},z_2^{\′}]}^T=R{[r_1\cos \left(A1\right),r_1\sin \left(A1\right),0]}^T+{[x,y,z]}^T\\ {[x_3^{\′},y_3^{\′},z_3^{\′}]}^T=R{[r_1\cos \left(A2\right),r_1\sin \left(A2\right),0]}^T+{[x,y,z,]}^T\\ {\left|\right|{[x_1^{\′},y_1^{\′},z_1^{\′}]}^T-{[x_1,y_1,z_1]}^T\left|\right|}_2=L1\\ {\left|\right|{[x_1^{\′},y_1^{\′},z_1^{\′}]}^T-{[x_2,y_2,z_2]}^T\left|\right|}_2=L2\\ {\left|\right|{[x_2^{\′},y_2^{\′},z_2^{\′}]}^T-{[x_2,y_2,z_2]}^T\left|\right|}_2=L3\\ {\left|\right|{[x_2^{\′},y_2^{\′},z_2^{\′}]}^T-{[x_3,y_3,z_3]}^T\left|\right|}_2=L4\\ {\left|\right|{[x_3^{\′},y_3^{\′},z_3^{\′}]}^T-{[x_3,y_3,z_3]}^T\left|\right|}_2=L5\\ {\left|\right|{[x_3^{\′},y_3^{\′},z_3^{\′}]}^T-{[x_1,y_1,z_1]}^T\left|\right|}_2=L6\end{array}\right.$

其中，[]^T表示向量的转置，||||₂表示向量的欧几里得范数，R为旋转矩阵且有

R＝R_x(γ)R_y(β)R_z(α)

$R_x\left(\mathrm{\γ}\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\γ}& -\sin \mathrm{\γ}\\ 0& \sin \mathrm{\γ}& \cos \mathrm{\γ}\end{array}\right],R_y\left(\mathrm{\β}\right)=\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& \sin \mathrm{\β}\\ 0& 1& 0\\ -\sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right],R_z\left(\mathrm{\α}\right)=\left[\begin{array}{ccc}\cos \mathrm{\α}& -\sin \mathrm{\α}& 0\\ \sin \mathrm{\α}& \cos \mathrm{\α}& 0\\ 0& 0& 1\end{array}\right]$

利用数值方法，求解所述方程组，取视距范围内实数解，得到被测物体的待测点的六自由度立体定位信息。

Claims (4)

1.一种六自由度立体定位装置，其特征是：包含三个子装置，RFID读写控制器、六自由度立体定位发送装置和六自由度立体定位接收装置；

所述RFID读写控制器，包括RFID读写模块、处理控制单元和人机交互界面；

所述六自由度立体定位发送装置，包括无线电测距发射器、转向与角度测量构件、处理单元、RFID标签和基板；所述六自由度立体定位发送装置，利用其转向与角度测量构件的旋转，使其无线电测距发射器设置于三个不共线的发射参考点；

所述六自由度立体定位接收装置，包括无线电测距接收器、转向与角度测量构件、处理单元、RFID标签和基板；所述六自由度立体定位接收装置，利用其转向与角度测量构件的旋转，使其无线电测距接收器设置于三个不共线的接收参考点。

2.根据权利要求1所述的一种六自由度立体定位装置，其特征在于，所述RFID读写控制器中，所述RFID读写模块为一块可嵌入于移动式设备的中长距离超高频射频标签读写器模块，完成中长距离超高频射频标签读写功能；所述处理控制单元根据使用者的指示，通过所述RFID读写模块，按一定的顺序发出控制指令并接收测量结果，利用数值方法计算出六自由度立体定位信息；所述人机交互界面，用于接收使用者的指示并输出被测物体的六自由度立体定位测量结果。

3.根据权利要求1所述的一种六自由度立体定位装置，其特征在于，所述六自由度立体定位发送装置中，所述转向与角度测量构件有两个端点，一端在基板上作为转向中心点，所述转向与角度测量构件能以转向中心点为原点，遵从控制指令，在平行于基板的平面上转动到指定角度，另一端用于固定所述无线电测距发射器，所述无线电测距发射器与所述转向与角度测量构件的转向中心点之间的距离恒定已知为r₁；

所述六自由度立体定位接收装置中，所述转向与角度测量构件有两个端点，一端在基板上作为转向中心点，所述转向与角度测量构件能以转向中心点为原点，遵从控制指令，在平行于基板的平面上转动到指定角度，另一端用于固定所述无线电测距接收器，所述无线电测距接收器与所述转向与角度测量构件的转向中心点之间的距离恒定已知为r₂；

所述六自由度立体定位发送装置和六自由度立体定位接收装置中的处理单元执行控制指令并反馈控制指令执行结果；

所述RFID标签是一块可嵌入于移动式设备的中长距离超高频射频标签，用于接收RFID读写控制器的控制指令，将控制指令转交给所述六自由度立体定位发送装置和六自由度立体定位接收装置中的处理单元，并将控制指令执行结果反馈至RFID读写控制器；

所述六自由度立体定位发送装置中的基板用于承载所述六自由度立体定位发送装置中的转向与角度测量构件、处理单元和RFID标签；

所述六自由度立体定位接收装置中的基板用于承载所述六自由度立体定位接收装置中的转向与角度测量构件、处理单元和RFID标签；

所述无线电测距发射器通过无线电发射无线测距参考信号；

所述无线电测距接收器通过接收无线电测距发射器发射的无线测距参考信号，测量无线电测距接收器与无线电测距发射器之间的直线距离。

4.如权利要求3所述的一种六自由度立体定位装置的定位方法，其特征是：将所述接收装置设定在外部参考点上，将所述发射装置设定在被测物体上，所述RFID读写控制器、所述接收装置和所述发送装置执行预定控制指令，所述RFID读写控制器利用数值方法计算得出被测物体的六自由度立体定位信息；具体为：

以外部参考点为原点建立三维右手直角坐标系，且规定外部参考点所在切面为xoy平面；将所述接收装置设置在外部参考点所在切面上，使得所述接收装置的转向与角度测量构件的转轴中心点与外部参考点重合，使得所述三维右手直角坐标系的x轴正方向为所述接收装置的转向与角度测量构件的角度0方向，规定逆时针方向为角度增加方向；

以被测物体的待测点为原点，在被测物体的待测点所在切面上建立二维直角坐标系；将所述发送装置设置在被测物体的待测点所在切面上，使得所述发送装置的转向与角度测量构件的转轴中心点与被测物体上的待测点重合，使得所述二维右手直角坐标系的x轴正方向为所述发送装置的转向与角度测量构件的角度0方向，规定逆时针方向为角度增加方向；

所述预定控制指令的流程如下：

发送装置将转向与角度测量构件至旋转至角度0，使得无线电测距发射器处于发射参考点1；

接收装置将转向与角度测量构件至旋转至角度0，使得无线电测距接收器处于接收参考点1；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L1；

接收装置将转向与角度测量构件至旋转至角度B1，使得无线电测距接收器处于接收参考点2；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L2；

发送装置将转向与角度测量构件至旋转至角度A1，使得无线电测距发射器处于发射参考点2；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L3；

接收装置将转向与角度测量构件至旋转至角度B2，使得无线电测距接收器处于接收参考点3；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L4；

发送装置将转向与角度测量构件至旋转至角度A2，使得无线电测距发射器处于发射参考点3；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L5；

接收装置将转向与角度测量构件至旋转至角度0，使得无线电测距接收器处于接收参考点1；

RFID读写控制器测量接收装置无线电测距接收器与发送装置无线电测距发射器之间的直线距离L6；

在整个过程中，需要保证所述六自由度立体定位接收装置的无线电测距接收器始终处于所述六自由度立体定位发送装置的无线电测距发射器的视距可见范围内；

所述RFID读写控制器计算被测物体的六自由度立体定位信息的数值方法如下：

设所述接收参考点1、接收参考点2和接收参考点3的三维直角坐标系坐标分别为(x₁，y₁，z₁)、(x₂，y₂，z₂)和(x₃，y₃，z₃)，满足方程

(x₁，y₁，z₁)＝(r₂，0，0)

(x₂，y₂，z₂)＝(r₂cos(B1)，r₂sin(B1)，0)

(x₃，y₃，z₃)＝(r₂cos(B2)，r₂sin(B2)，0)

利用数值方法，求解出接收参考点1、接收参考点2和接收参考点3的三维直角坐标系坐标；

设所述被测物体的待测点的六自由度立体定位信息为(x，y，z，α，β，γ)，设所述发射参考点1、发射参考点2和发射参考点3的三维直角坐标系坐标为(x′₁，y′₁，z′₁)、(x′₂，y′₂，z′₂)和(x′₃，y′₃，z′₃)，满足方程组

$\left\{\begin{array}{c}{\[x_1^{\′},y_1^{\′},z_1^{\′}\]}^T=R{\[r_1,0,0\]}^T+{\[x,y,z\]}^T\\ {\[x_2^{\′},y_2^{\′},z_2^{\′}\]}^T=R{\[r_1\cos \left(A1\right),r_1\sin \left(A1\right),0\]}^T+{\[x,y,z\]}^T\\ {\[x_3^{\′},y_3^{\′},z_3^{\′}\]}^T=R{\[r_1\cos \left(A2\right),r_1\sin \left(A2\right),0\]}^T+{\[x,y,z\]}^T\\ \left|\right|{\[x_1^{\′},y_1^{\′},z_1^{\′}\]}^T-{\[x_1,y_1,z_1\]}^T|{|}_2=L1\\ \left|\right|{\[x_1^{\′},y_1^{\′},z_1^{\′}\]}^T-{\[x_2,y_2,z_2\]}^T|{|}_2=L2\\ \left|\right|{\[x_2^{\′},y_2^{\′},z_2^{\′}\]}^T-{\[x_2,y_2,z_2\]}^T|{|}_2=L3\\ \left|\right|{\[x_2^{\′},y_2^{\′},z_2^{\′}\]}^T-{\[x_3,y_3,z_3\]}^T|{|}_2=L4\\ \left|\right|{\[x_3^{\′},y_3^{\′},z_3^{\′}\]}^T-{\[x_3,y_3,z_3\]}^T|{|}_2=L5\\ \left|\right|{\[x_3^{\′},y_3^{\′},z_3^{\′}\]}^T-{\[x_1,y_1,z_1\]}^T|{|}_2=L6\end{array}\right.$

其中，[]^T表示向量的转置，||||₂表示向量的欧几里得范数，R为旋转矩阵，且有

R＝R_x(γ)R_y(β)R_z(α)

$\begin{array}{ccc}{R}_x\left(\mathrm{\γ}\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\γ}& -\sin \mathrm{\γ}\\ 0& \sin \mathrm{\γ}& \cos \mathrm{\γ}\end{array}\right]& R_y\left(\mathrm{\β}\right)=\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& \sin \mathrm{\β}\\ 0& 1& 0\\ -\sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right]& R_z\left(\mathrm{\α}\right)=\left[\begin{array}{ccc}\cos \mathrm{\α}& -\sin \mathrm{\α}& 0\\ \sin \mathrm{\α}& \cos \mathrm{\α}& 0\\ 0& 0& 1\end{array}\right]\end{array}$

利用数值方法，求解所述方程组，取视距范围内实数解，得到被测物体的待测点的六自由度立体定位信息。