CN2695916Y - 一种空间物体姿态及位置测量装置 - Google Patents

Abstract

本实用新型涉及一种空间物体姿态及位置测量装置，其属于空间物体位置和姿态测量技术领域，包括交流驱动电路1、三组正交的磁场发射线圈2及其外围电路、三组正交的磁传感器3及其外围电路、双轴加速度计7及其外围电路、微处理器6、A/D转换器9、PC机10等，以及存储于微处理器6和PC机10中的信号处理及显示软件模块。本实用新型能够得到空间物体相对于某一固定参考点的姿态和位置信息，具有使用方便、启动快、精度高、体积小等优点。

Description

一种空间物体姿态及位置测量装置

一、技术领域

本实用新型涉及一种测量空间物体相对于某一参考点的姿态和位置的装置，特别是涉及空间物体的姿态和位置计算是通过交流磁场的空间耦合和测量地球的重力场、地磁场实现的。

二、背景技术

在三维空间内一个物体的空间位置由笛卡尔坐标(即X、Y、Z坐标值)来确定，在描述姿态时还需要用另外三个参数俯仰角、横滚角及航向角，因此姿态测量系统至少需要获得6个自由度信息，即三个平移自由度和三个旋转自由度，如图2所示。目前传统的空间物体位置和姿态测量技术主要包括：

1、机电式测量系统，通常由体积较小的机械臂构成，其一端固定在一个参考基座上，另一端和被跟踪对象固联。机电式测量系统采用电位计或者光学编码器作为关节传感器，以测量机械臂关节处的旋转角，根据所测得机械臂之间的相对旋转角以及相互连接在一起的两个传感器之间的机械臂长进行计算，最终获得六自由度的方位数据。机电式传感器的测量精度会受到环境温度变化的影响、活动范围受机械结构限制，对关节传感器灵敏度要求很高。

2、声学式测量系统，利用不同声源到达某一特定地点的时间差、相位差或者声压差实现定位和跟踪。一般有连续波相位相干测量法和脉冲波飞行时间测量法两种形式，声学式测量系统跟踪范围有限、受环境声场干扰、与空气湿度有关并且要求发射器与接收器之间不能有物体遮挡。

3、光电式测量系统，利用空间环境光或者由跟踪器控制的光源发出的光在图象投影面上随不同方位而产生的投影变化计算出被跟踪对象的方位。在有光源的情况下，通常使用红外光以避免跟踪器对用户的干扰。光电式测量系统要求光源与探测器可视，跟踪的角度范围有限并且现场其它光源会造成影响。多发射器结构是一种解决方案，但却以系统的复杂性、计算时间及成本为代价。

4、惯性式跟踪器，在惯性导航系统中通过运动物体的初始位置和对线加速度计的输出进行二次积分推算运动物体当前的位置，通过运动物体的初始姿态和对陀螺仪的输出进行积分推算被测物体的姿态。为了避免在积分过程中陀螺的噪声随时间累积而使其输出产生的漂移和随机游走，目前实用的惯性导航系统均采用体积大、价格昂贵的高精度陀螺。

5、直流式电磁测量系统，直流式电磁跟踪系统由发射器、接收器和计算模块组成。直流式电磁跟踪系统的发射器由三组正交的线圈组成，三组线圈一般被严格安装在正交的基准构架上。发射器周期性地依次以直流电流驱动三组发射器线圈，使每一组发射器线圈分时产生一个脉冲的直流电磁场。在上述三个连续的时间区间内，电流脉冲依次作用到三组发射器线圈上，在每一个时间区间，探测器均测得相应的磁场数据。之后再有第四个时间区间，发射器不工作，三个发射轴均不产生脉冲直流磁场，在这个时间段接收器测得环境磁场。利用上述过程中所获得的四组数据可以计算得到接收器和发射器的相对方位信息。

直流式电磁跟踪系统存在如下问题：

(1)在使用过程中的环境干扰包括地磁干扰、附近永磁体产生的干扰及电路板电源所产生的磁场干扰等直流干扰，由于这些干扰与接收磁场信号均为直流信号，采用常规的信号处理手段(如滤波)无法滤除。为了去除环境磁场干扰，系统设计中采用减去空时间段数据的方法。由于系统更新频率仅为30Hz，第四个时间段所测得的环境磁场数据与第一个时间段的数据相比会存在差异，因此无法迅速跟踪干扰的变化。尽管通过在测量完每一轴后加入空时间段的方法可以在一定程度上弥补这一缺陷，但这样又降低了系统的刷新频率，从而影响了系统的性能。

(2)由于在接收端磁场的强度与接收器和发射线圈之间距离的立方成反比，因此接收端的磁场强度随距离增大衰减很快。由于每个测量轴的地磁场值与被跟踪的物体的姿态有关且随时间变化，因此很难实时去除地磁场的影响并获得被测信号的足够放大。

6、交流式电磁测量系统，交流式电磁测量系统由励磁源、磁接收器和计算模块组成。励磁源是由三组互相正交的、由交流电流驱动的线圈构成，磁接收器由三套分别探测三个励磁源的线圈构成。由于三个磁接收器所测得的三个磁场向量包含了足够的信息，因而可以计算出磁接收器相对于励磁源的方位。交流式电磁跟踪系统通过求解从励磁源到磁接收器的电磁能量传递的逆过程来实现方位的求解。

因为交流式电磁测量系统建立在交变磁场上，在励磁源被交流驱动的同时，励磁源会在周围导体中感应出回路电流(涡流)，进而引入一个二级交流磁场，造成环境磁场的扭曲。会导致跟踪系统中位置和方向结果的计算错误。文献[1]中研究了位置数据快速校正的方法，但是对于姿态数据还没有出现有效的快速校正方法。

三、发明内容

本实用新型的目的是提供一种空间物体姿态及位置测量装置，即新型的基于空间交流磁场耦合及地磁场和重力场的空间位置和姿态测量装置。由于姿态数据的获取是通过测量地磁场和地球的重力场获取的，从而不会受到环境中金属物体所产生的涡流的影响，克服了已有技术中传感器的测量精度易受环境温度、声场、湿度的影响，解决了已有技术中测量跟踪范围、角度范围有限，系统体积大，价格昂贵，无法迅速跟踪干扰的变化，难以实时去除地磁场的影响，易导致跟踪系统中位置和方向结果的计算错误等问题。

本实用新型解决其技术问题所采用的技术方案是：在本装置中集多种传感器为一体，通过对磁传感器和加速度计采集信号的处理，能够得到空间物体的姿态和位置信息。本测量装置包括多个传感器，对传感器的信号进行放大和处理的信号调理电路，进行转换的A/D转换电路，将微处理器与PC机相连的RS232串行接口等。发射源由三组互相正交的闭合线圈构成，接收装置包括测量交流磁场和地磁场的磁传感器和测量地球重力场的加速度计以及外围电路，以及预先存储在微处理器和PC机中的信号处理和显示软件等。

本实用新型的姿态和位置解算方法为：

定义地磁场的水平分量为H₀，H_X1和H_Y1为此时沿坐标轴X₁和Y₁方向磁传感器的输出：

                           H_Y1＝H₀cosψ

航向角ψ可由下式计算：

                           ψ＝arctan(H_X1/H_Y1)

由于余切函数具有多值性，需要根据H_X1和H_Y1的符号来决定航向角的范围，具体方法如下式所示。

当利用加速度计测出俯仰角θ和横滚角γ后，通过如下的坐标变换可以根据此时磁传感器的输出值 计算出被跟踪物体处于水平状态下的三轴分量 如下式所示：

图2中的ρ可以由下式求出：

$\mathrm{\ρ}=\frac{3}{2}\sqrt[6]{\frac{C^2}{P_2\left(X\right)+P_2\left(Y\right)+P_2\left(Z\right)}}$

其中P₂(X)+P₂(Y)+P₂(Z)为三个发射轴作用下，磁传感器各接收轴数据的平方和，C为比例系数。

为了计算α和β，用f₃来表示3轴磁传感器的输出，f₃与f₀的关系为：

$f_3=T_{\mathrm{\ψ}}{T}_{\mathrm{\θ}}{T}_{\mathrm{\γ}}{T}_{-\mathrm{\α}}{T}_{-\mathrm{\β}}\frac{\mathrm{NAI}}{4\mathrm{\π}{\mathrm{\ρ}}^3}\left[\begin{array}{ccc}1& 0& 0\\ 0& -\frac{1}{2}& 0\\ 0& 0& -\frac{1}{2}\end{array}\right]T_{\mathrm{\β}}{T}_{\mathrm{\α}}{f}_0$

定义矩阵M为：

$M=T_{\mathrm{\ψ}}^{-1}{T}_{\mathrm{\θ}}^{-1}{T}_{\mathrm{\γ}}^{-1}{f}_3=\frac{\mathrm{NAI}}{4\mathrm{\π}{\mathrm{\ρ}}^3}\left[\begin{array}{ccc}\frac{3}{2}{\cos }^2\mathrm{\α}{\cos }^2\mathrm{\β}-\frac{1}{2}& \frac{3}{2}\cos \mathrm{\α}{\sin \mathrm{\α}\cos }^2\mathrm{\β}& -\frac{3}{2}\cos \mathrm{\α}\cos \mathrm{\β}\sin \mathrm{\β}\\ \frac{3}{2}\cos \mathrm{\α}\sin \mathrm{\α}{\cos }^2\mathrm{\β}& \frac{3}{2}{\sin }^2\mathrm{\α}{\cos }^2\mathrm{\β}-\frac{1}{2}& -\frac{3}{2}\sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\β}\\ -\frac{3}{2}\cos \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\β}& -\frac{3}{2}\sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\β}& 1-\frac{3}{2}{\cos }^2\mathrm{\β}\end{array}\right]$

由于M是一个已知的矩阵，可以通过3个旋转角度和3轴磁传感器的输出计算得到，因此α和β可由下式计算得到：

$1-\frac{3}{2}{\cos }^2\mathrm{\β}=M_{33}$ 即 $\mathrm{\β}=\mathrm{co}{s}^{-1}\sqrt{\frac{2}{3}(1-M_{33})}$

$\tan \mathrm{\α}=\frac{M_{23}}{M_{32}}$ 即 $\mathrm{\α}=\mathrm{ta}{n}^{-1}\frac{M_{23}}{M_{32}}$

四、附图说明

图1为总体结构及工作原理框图

图2为空间物体相对于某一参考点的位置和姿态信息

图3为发射源装置原理框图

图4为接收装置原理框图

其中：1-交流驱动电路，2-三组正交的磁发射线圈，3-三轴磁传感器，4-仪表放大器，5-交流耦合器，6-微处理器，7-双轴加速度计，8-信号调理电路，9-A/D转换器，10-PC机，11-晶体振荡源，12-计数器，13-波形转换器，14-单片机，15-RS232串口，17-PC机，18-控制信号，19-选择开关，20、21、22-功放电路，23、24、25-三轴正交的线圈，26、27、28-三轴磁传感器，30、35、40-共模放大电路，31、36、41-滤波放大电路，32、37、42-运算放大器组，33、38、43-低通滤波器，34、39、44-仪表放大器，45-A/D转换器，46-PC机

五、具体实施方式

本实用新型基于交流磁场耦合及地磁场和重力场进行姿态和位置测量结合附图及实施例详细说明如下：

本实用新型的总体结构及工作原理如图1所示，包括发射源装置和接收装置。发射源由三组正交的磁发射线圈2及其外围电路构成，接收装置由三轴磁传感器3及其外围电路、双轴加速度计7及其外围电路、A/D转换器9、微处理器6、RS232接口电路、PC机10等构成。发射源装置的交流驱动电路1依次驱动三组正交的发射线圈2及其外围电路，分时发送具有一定频率的按照正弦规律变化的交流磁场。在每一个时间段三轴磁传感器3同时接收发射源发射的磁场和地磁场，分别通过仪表放大器4和交流耦合器5后与双轴加速度计7所测量的地球重力信号一起通过信号调理电路8和A/D转换器9送入PC机10。

本实用新型的软件程序分为微处理器6和PC机10两部分，其中微处理器6主要包括：驱动时序信号产生、采样同步信号产生等，PC机10主要包括数据采集、数字滤波、数字相关运算、姿态及位置解算、图形接口、用户界面等。

图3为发射源部分的原理框图，晶体振荡源11产生频率为12MHz的方波振荡信号，通过计数器12分频后分别产生频率为9kHz的方波信号及作为波形转换器的时钟信号的频率为900kHz的方波信号，波形转换器13将方波信号转换为正弦信号，单片机14产生控制信号18，通过多路选择开关19依次选通作为功放电路20、21、22的输入信号，单片机14通过RS232串口15与PC机17通信，功放电路20、21、22对输入信号进行功率放大后依次驱动三轴正交的线圈23、24、25，发射出正交的交变磁场。

图4为接收装置的原理框图，正交的三轴磁传感器26、27、28接收到发射线圈发射的交流磁场和地磁场，其中交流部分通过去共模放大电路30、35、40；中心频率为9kHz的三阶滤波放大电路31、36、41；运算放大器组32、37、42后通过16位A/D转换器45送入PC机46中，直流部分通过低通滤波器33、38、43和仪表放大器34、39、44后通过16位A/D转换器45送入PC机46中，双轴加速度计的输出29通过16位A/D转换器45送入PC机46中，PC机经过数字滤波、位置和姿态解算后即可以得到空间物体相对于发射源所在的参考点的位置和姿态，并且通过图形用户界面在PC机46上显示出来。发射源装置可以固定在空间某一确定位置，接收装置安装在被测物体上进行位置和姿态测量。针对于同一发射源，可以采用多组接收装置测量不同空间物体的不同部位的位置和姿态。

本实施例可以实现空间物体的位置和姿态测量，角度测量范围为0°-360°，精度为0.5°，距离测量范围为30cm-200cm，精度为2mm，可以适应于工作环境中存在非铁磁性金属物体情况下的姿态和位置测量。

                                             参考文献

1、M.Czernuszenko，D.Sandin，T.DeFanti，Line of Sight Method for Tracker Calibration in Projection-Based VRSystems，in Proceedings of 2nd International Immersive Projection Technology Workshop，Ames，Iowa，May 11-12，1998.

Claims (7)

1、本实用新型涉及一种空间物体姿态及位置测量装置，包括由交流驱动电路和磁场发射线圈及其外围电路构成的发射部分，由磁传感器及其外围电路、A/D转换器、微处理器、PC机以及存储于微处理器和PC机中的信号处理及显示软件模块构成的接收部分，其特征在于：a、发射部分是由交变电流来驱动三组互相正交的磁场发射线圈组成；b、接收部分是由测量地磁场的三组正交的磁传感器和测量地球的重力场的双轴加速度计组成。

2、如权利要求1所述的一种空间物体姿态及位置测量装置，其特征是用于驱动发射部分交变电流为正弦的。

3、如权利要求1所述的一种空间物体姿态及位置测量装置，其特征是发射部分固定于空间确定位置，接收部分安装在被测物体上。

4、如权利要求3所述的一种空间物体姿态及位置测量装置，其特征是针对于同一发射源，可以采用多组接收装置测量不同空间物体或同一空间物体的不同部位的位置和姿态。

5、如权利要求1所述的一种空间物体姿态及位置测量装置，其特征是通过A/D转换器采样被测信号和同频参考信号后，由计算机进行数字相关运算以计算被测信号的幅值。

6、如权利要求5所述的一种空间物体姿态及位置测量装置，其特征是通过一对正交的参考信号的采样值进行数字相关计算来消除被测信号和参考信号的相位差。

7、如权利要求6所述的一种空间物体姿态及位置测量装置，其特征是一对正交的参考信号是通过对一路参考信号的采样值进行后处理获得的。

Images