CN1563888A - 空间物体姿态及位置测量系统 - Google Patents

Abstract

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

Description

空间物体姿态及位置测量系统

一、技术领域

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

二、背景技术

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

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

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

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

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

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

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

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

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

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

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

三、发明内容

本发明的目的是克服已有技术的不足之处，设计出一种新型的基于空间交流磁场耦合及地磁场和重力场的空间位置和姿态测量系统。本发明集多种传感器为一体，通过对磁传感器和加速度计采集信号的处理，能够得到空间物体的姿态和位置信息。由于姿态数据的获取是通过测量地磁场和地球的重力场获取的，从而不会受到环境中金属物体所产生的涡流的影响。

本发明设计出一种新型的基于空间交流磁场耦合及地磁场和重力场的空间位置和姿态测量系统，包括多个传感器，对传感器的信号进行放大和处理的信号调理电路，进行转换的A/D转换电路，将微处理器与PC机相连的RS232串行接口等，其特征在于所说的传感器包括测量交流磁场和地磁场的磁传感器和测量地球重力场的加速度计以及外围电路，以及预先存储在微处理器和PC机中的信号处理和显示软件等。

本发明的姿态和位置解算方法为：

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

                              H_X1＝H₀sinψ

                              H_Y1＝H₀cosψ

航向角ψ可由下式计算：

                           ψ＝arctan(H_X1/H_Y1)

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

当利用加速度计测出俯仰角θ和横滚角γ后，通过如下的坐标变换可以根据此时磁传感器的输出值

计算出被跟踪物体处于水平状态下的三轴分量 如下式所示：

三轴正交的发射线圈的驱动电流如图2和下式所示：

式中n＝1、2、3，分别代表三轴励磁线圈的驱动电流，T为励磁信号周期，f为交流振荡源频率，I为流过励磁线圈的交流电流的幅值。

当交流式发射线圈的发射轴和接收磁阻传感器的敏感轴如图3所示时，距坐标轴原点O距离为r的点Or的磁场的强度H(t)的径向分量和法向分量分别由下式给出：

$H_Q\left(t\right)=\frac{\mathrm{NIA}\cos \left(2\mathrm{\πft}\right)}{{2\mathrm{\πr}}^3}$

$H_I\left(t\right)=\frac{\mathrm{NIA}\cos \left(2\mathrm{\πft}\right)}{{4\mathrm{\πr}}^3}$

式中Q和I分别代表径向( X轴方向)和法向( Y轴及 Z轴方向)的磁场强度分量。由于在磁传感器测得的信号中，总是不同程度的掺入了各种随机性的干扰，即使假定磁传感器所测得的信号仅为发射磁场信号，由于接收信号幅度较小，在对信号进一步处理之前需要采用放大电路和带通滤波电路对信号进行预处理，构成测量系统的元器件及测量系统本身均会引入噪声。另外由于H_Q(t)，H_I(t)都是与时间t有关的量，在不同时间的测量值不相同。为消除时间t对计算结果的影响和提高接收信噪比，在算法的设计中采用把一个与被测信号同频同相位的单位参考信号与磁传感器的测量值相乘并在时间T内对结果积分的方法，如下式所示：

$H_Q=\frac{1}{T}{\∫}_0^T\frac{\mathrm{NIA}\cos 2\mathrm{\πft}}{{2\mathrm{\π\ρ}}^3}\cos 2\mathrm{\πftdt}\≈\frac{\mathrm{NIA}}{{4\mathrm{\π\ρ}}^3}$

$H_I=\frac{1}{T}{\∫}_0^T\frac{\mathrm{NIA}\cos 2\mathrm{\πft}}{{4\mathrm{\π\ρ}}^3}\cos 2\mathrm{\πftdt}\≈\frac{\mathrm{NIA}}{{8\mathrm{\π\ρ}}^3}$

上式中假定参考信号与被测信号同频同相，在交流式电磁跟踪系统中由于参考信号和励磁信号采用同一振荡源，同频的条件很容易满足。但是参考信号和被测信号之间往往存在相位差，此时上式可以写作：

因此计算结果与被测信号和参考信号的相位差有关，对测量结果的准确性产生影响，当相位差为90°时输出为零，会导致完全错误的跟踪结果。为了解决这一问题，在算法的设计中采用一对正交的参考信号r₁(t)和r₂(t)，参考信号的表达式为：

                       r₁(t)＝cos(2πft+)

                       r₂(t)＝sin(2πft+)

以上算法是通过A/D转换器采样传感器信号和参考信号后由计算机计算下式来实现的。

其中f_s为采样频率，N＝int(T*f_s)为A/D转换器在一个周期T内的采样点数。

对上述二式的运算结果进行矢量相加，可以得到：

$H_Q=\sqrt{H_{Q1}^2+H_{Q2}^2}=\frac{\mathrm{NIA}}{{4\mathrm{\π\ρ}}^3}$

从而消除了参考信号与被测信号之间相位差的影响。

为加快运算速度，本算法采用下式所示的递推算法完成：

$H_{Q1}\left(i\right)=\frac{H_Q(i/f_s)r_1(i/f_s)+(i-1)H_{Q1}(i-1)}{i}$

$H_{Q2}\left(i\right)=\frac{H_Q(i/f_s)r_2(i/f_s)+(i-1)H_{Q2}(i-1)}{i}$

式中H_Q1(i)、H_Q2(i)代表第i次采样后互相关算法的计算值，H(i/f_s)、r₁(i/f_s)、R₂(i/f_s)分别为三轴传感器接收到的磁场强度和参考信号的采样值。

四、附图说明

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

图2为本发明的磁场发射线圈各轴驱动电流

图3为本发明的磁场耦合关系

图4为本发明的总体结构及工作原理框图

图5为本发明的发射部分结构原理图

图6位本发明的接收部分结构原理图

其中：1-交流驱动电路，2-三组正交的磁发射线圈，3-三轴磁传感器，4-仪表放大器，5-交流耦合器，6-微处理器，7-双轴加速度计，8-信号调理电路，9-A/D转换器，10-PC机，11-晶体振荡源，12-计数器，13-波形转换器，14-单片机，15-RS232串口，16-串口数据，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机

五、具体实施方式

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

本发明的总体结构及工作原理如图4所示，包括发射源装置和接收装置。发射源由三组正交的磁发射线圈2及其外围电路构成，接收装置由三轴磁传感器3及其外围电路、双轴加速度计7及其外围电路、A/D转换器9、微处理器6、RS232串口15、PC机10等构成。发射源装置的交流驱动电路1依次驱动三组正交的发射线圈2及其外围电路，分时发送具有一定频率的按照正弦规律变化的交流磁场。在每一个时间段三轴磁传感器3同时接收发射源发射的磁场和地磁场，分别通过仪表放大器4和交流耦合器5后与双轴加速度计7所测量的地球重力信号一起通过信号调理电路8和A/D转换器9送入PC机10。通过将采样频率取为被测信号频率的4m倍(其中m为1、2、...)，则正交的参考信号可以通过仅对一路参考信号进行采样后产生。如下式所示：

                              r₂(i)＝r₁(i+m)

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

本发明各部件的实施例及各部分的工作原理如图5及图6所示(以所产生的交流正弦驱动信号的工作频率为9kHz为例)：

驱动时序信号产生、采样同步信号产生等，PC机10主要包括数据采集、数字滤波、数字相关运算、姿态及位置解算、图形接口、用户界面等。

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

图6为接收装置的原理框图，正交的三轴磁传感器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