[发明专利]一种六自由度立体定位装置和方法

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

4.如权利要求1～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′₃)，满足方程组

[x1′,y1′,z1′]T=R[r1,0,0]T+[x,y,z]T[x2′,y2′,z2′]T=R[r1cos(A1),r1sin(A1),0]T+[x,y,z]T[x3′,y3′,z3′]T=R[r1cos(A2),r1sin(A2),0]T+[x,y,z,]T||[x1′,y1′,z1′]T-[x1,y1,z1]T||2=L1||[x1′,y1′,z1′]T-[x2,y2,z2]T||2=L2||[x2′,y2′,z2′]T-[x2,y2,z2]T||2=L3||[x2′,y2′,z2′]T-[x3,y3,z3]T||2=L4||[x3′,y3′,z3′]T-[x3,y3,z3]T||2=L5||[x3′,y3′,z3′]T-[x1,y1,z1]T||2=L6]]>

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

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

Rx(γ)=1000cosγ-sinγ0sinγcosγ,Ry(β)=cosβ0sinβ010-sinβ0cosβ,Rz(α)=cosα-sinα0sinαcosα0001]]>

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