自动化测试
计算机测量与控制. 2002. 10(6) Computer Measurement &Control
・363・
文章编号:1671-4598(2002) 06-0363-03 中图分类号:TM93 文献标识码:A
测量运动物体姿态的三自由度定位算法的研究
刘 越, 王涌天, 胡晓明
(北京理工大学光电工程系, 北京 100081)
摘要:研究了一种采用加速度计、磁阻传感器和单片机的三自由度定位装置及其算法, 给出了A/D 转换器的位数
与系统分辨率的关系, 研究了环境磁场对测量结果的影响, 。该装置可用于运动物体航向、俯仰和倾斜三个方向的姿态角的测量。
关键词:地磁测量; 定位算法; A/D 转换; 单片机
of the Rigid Body
L IU Yue , WAN G Y ong 2tian , HU Xiao 2ming
(Department of Opto -electronic Engineering , Beijing Institute of Technology , Beijing 100081, China )
Abstract :A 3DOF (Degree of Freedom ) tracking system which is com posed of accelerometers , magnetoresistive sens ors and MCU and its alg orithm are described. The relationshi p between the number of bits of A/D converter and the res olution of the s ystem is dis 2cussed. The calibration alg orithm to com pensate the influence of the environmental magnetic field is presented. The error of the calibration alg orithm is als o analyzed. The proposed system can be used to measure the accurate values of headin g , pitch and roll.
K ey w ords :measurement of geomagnetic field ; tracking algorithm ; A/D converter ; MCU
1 引言
在海洋船舶定向、钻井设备测斜、飞行器姿态参照、GPS 死区推估及虚拟现实系统用户头手跟踪中均会遇到运动物体的姿态测量问题, 即计算运动物体在参考空间中的方位[1]。目前所采用的基于陀螺仪和加速度计的定位装置存在体积大、价格昂贵的缺点, 同时陀螺会随着时间产生偏移[5], 需要定期进行校正, 这在很大程度上限制了其应用的范围。文章研究了一种采用加速度计、磁阻传感器和单片机实现的高精度定位装置及其算法, 该装置通过测量地球的重力场及磁场来给出精确的定位信息, 由于采用全固态传感器, 具有精度高、性能稳定、体积小、价格低廉、功耗低的特点, 系统原理如图1所示。
图1 系统原理图
2
应用加速度计及磁阻传感器的定位算法
收稿日期:2001-11-20。
基金项目:国家自然科学基金资助项目(60104007, 60025513) .
作者简介:刘越(1968-) , 男, 吉林省长春市人, 副教授, 博士后, 主要从事微弱信号检测理论与技术、虚拟现实人机交互技术及高精度数字罗盘技术方向的研究。
王涌天(1957-) , 男, 北京市人, 研究员, 博导, 主要从事光学设计及CAD 、衍射光学器件、虚拟现实系统和增强现实系统的设计及应用等方向的研究。
在刚体动力学中运动物体的姿态(即运动物体在参考空间中的方位) 通过与运动物体相固结的运动参考坐标系和定参考坐标系之间的夹角表示[8]。两坐标系的原点均取在运动物体的质心, 定参考坐标系X 轴水平指东, Z 轴沿地垂线指天顶, Y 轴水平指北。与运动物体相固结的坐标系称为运动物体坐标系, 其Y C 轴沿运动物体纵轴指向前方, X C 轴沿运动物体横轴指向右方, Z C 轴沿运动物体纵轴指向上方。定参考坐标系与运动物体坐标系的关系如图2所示。
设运动物体坐标系初始时与定参考坐标系重合, 根据上述定义运动物体的任意姿态均可通过下述3次顺序转动得到:(1) 绕Z 轴转航向角(heading ) ; (2) 绕X 1轴转俯仰角(pitch ) ; (3) 绕Y 2轴转倾斜角(roll
) 。 图2 运动物体坐标系
与定参考坐标系
・364・计算机测量与控制
第10卷
θ cos 0 -sin 0 1 0θsin θ 0 cos
1 0 0
0 cos γ sin =H C (6) 0 -sin γ cos 211 运动物体处于水平状态下姿态角的测量算法
下面首先考察一种简单的情况, 即运动物体仅绕Z 轴旋转航向角, 相当于图1中X 1, Y 1, Z 1轴的位置, 此时俯仰角及倾斜角均为零。
为测量此时地磁场在运动物体坐标系X 1, Y 1, Z 1三轴的分量, 在定位系统中将三轴磁阻传感器的敏感轴沿运动物体坐标系坐标轴放置。已知地磁场的大小为015～016Gs , 其水平分量H 0与地球表面平行, 指向地磁北极。定义H X 1和H Y 1为此时X 1和Y 1方向磁阻传感器的输出,
H X 1=H 0sin ΨH Y 1=H 0cos Ψ
(1) ) (3)
H 1=
由式(6) 可以得到:
H X 1=H XC cos +H YC sin γsin θ-H ZC cos γsin θH Y 1=H YC cos γ+H ZC sin γ
θ-H YC cos θθH Z 1=H XC sin sin γ+H ZC cos γcos
(7)
从而可应用式(3) 及(4) 直接计算航向角。
3 系统参数的选择
311 A/D :
ΨH X 1/11°的测量精度
0。已知地磁场在水平Y 1200m Gs , (3) 可估计A/D 传感器的分辨率。由:
(8) arctg (H X 1/H Y 1) =011°可得:
H X 1/H Y 1=1/573
(9)
, 需要根据H X 1和H Y 1的符号来决定航向角的范围, 具体方法如式(4) 所示。
180°-arctg (H X 1/H Y 1) , 当H Y 1
360°-arctg (H X 1/H Y 1) , 当H Y 1>0, H X 1
Ψ=
arctg (H X 1/H Y 1) , 当H X 1>0, H Y 10
(4)
212 运动物体处于倾斜状态下姿态角的测量算法
当运动物体处于倾斜状态时, 由图2可以看出,
由于俯仰角及倾斜角不再为零, 三轴磁阻传感器所测得的地磁场的分量与水平状态下的地磁场三轴分量会有很大不同, 如果仍然采用式(3) 来计算运动物体的航向角会给测量结果带来较大的误差。在这种情况下必须首先测得运动物体的俯仰角及倾斜角, 通过方向余弦矩阵[8]将此时三轴磁阻传感器的输出值转换为水平状态下地磁场的X 1轴及Y 1轴分量, 进而测得此时航向角的准确值。
由于地球的重力场方向在任何时间和地点都与地面垂直, 因此重力场是测量倾斜角的最理想的参照物[7]。在系统的设计中采用加速度计来测量倾斜角, 加速度计是利用具有一定质量的物体对地球重力场作出响应, 进而引起某些参数的变化来测量倾斜角的。当加速度计处于水平时, 输出为0, 当加速度计与水
平面之间存在一定倾斜角时, 加速度计的输出值与倾斜角的正弦成正比。
设此时三轴磁阻传感器的输出为:
ω
这说明1/573的变化会带来011°的变化。当采用10位A/D 时, 由于最高位为符号位(所测得的磁场范围为±200m Gs ) , 从而可以保证测得1/512的变化率。考虑到环境干扰磁场的影响, 系统能够测量的磁场范围还需进一步扩大, 因此选定为±800m Gs , 此时A/D 转换器的位数应为12位, 磁阻传感器所需要测得的最小磁场为018Gs/2048=0139m
Gs 。系统所选用的磁阻传感器HMC1021及HMC1022最小可测
μGs 的磁场, 小于该磁场的1/4, 完全满足量低达85
要求。312 环境磁场的影响及补偿算法
由于定向是基于测量地磁场的三轴分量, 定位系统附近永磁体或及带电导线所产生的磁场影响系统的测量精度。为研究环境磁场对测量结果产生的干扰, 首先考察一个无环境干扰磁场的理想情况。在保持运动物体坐标系的Z C 轴与参考坐标系的Z 轴方向相同的情况下, 将三轴磁阻传感器在水平面内旋转, 此时输出的H X 1与H Y 1的关系为一个以原点为圆心的圆。如式(10) 及图3(a ) 所示。
2222222H x 1+H y 1=H 0sin Ψ+H 0cos Ψ=H 0
(10)
当运动物体附近存在永磁体或通电导线时, 它们对地磁场的干扰为在运动物体的X 1轴和Y 1轴方向叠
H C =[H XC , H YC , H ZC ]
ω
T
(5)
当利用加速度计测出θ和γ后, 通过如下的坐标变换可以通过所测得的H C 计算出运动物体处于水平状态下的三轴分量H 1=[H X 1, H Y 1, H Z 1]T 。
ω
图3 环境磁场对测量结果的影响
第6期刘越等:测量运动物体姿态的三自由度定位算法的研究
变换可得:
・365・
加了一个固定幅度的分量H x 0及H y 0, 表现在该圆的
圆心不再位于原点, 如图3(b ) 所示。
根据上述讨论, 为补偿环境磁场的影响, 定位系统在保持水平的条件下旋转一周或两周, 找出此时输出与X ′轴及Y ′轴的交点, 即图3(b ) 中a (x 1, y 1) , b (x 2, y 2) , c (x 3, y 1) , d (x 2, y 3) 4点。通过计算:
H X 0=
a 2a =O ′a (1-cos ∠a 1Oa ) =01003O ′a (13)
由式(11) 及式(13) 可见只要定位系统旋转一周的
时间大于2s 即可保证由此带来的相对误差小于3/1000, 这在实际应用中很容易满足, 因此采用该方法可有效的去除环境磁场的干扰。
参考文献:
[1]张学孚, 陆怡良. 磁通门技术[M ].北京:国防工业出
版社, 1995.
[2]孙静. [M ].北京:国防
, [3. [M ].北京:国防工业出版社,
[4]CARUSO. M. J. . Application of Magnetoresistive Sensors
in Navigation Systems [J].Sensors and Actuators. 1997, (2) :15-21.
[5]李开生, 张慧慧, 费仁元, 等. 定位传感器及其融合技术
综述[J].计算机自动测量与控制, 2001, 9(4) :1-3. [6]Sensor Products Data Sheet , 1and 2Axis Magnetoresistive
Microcircuits [DB].Honeywell SSEC , 1999.
[7]丁衡高. 海陆空天显神威-惯性技术纵横谈[M ].北京/
广州:清华大学出版社/暨南大学出版社, 2000.
[8]贾书惠. 刚体动力学[M].北京:高等教育出版社, 1987.
2
, H Y 0=
2
(11)
(H X 0, H Y 0) , 式(3) 变为:即可求出此时的圆心坐标O ′
Ψ=arctg [(H X 1-H X 0) /(H Y 1-H Y 0) ]
(12)
在单片机的软件设计中采用求采样值中X 、Y
方向的最大及最小值的方法来找到X ′轴及Y 点。, Y ′况, 由于该系统的采样频率为20Hz , 即每秒钟能够采集20组三轴传感器数据, 在调整时要求旋转一周的时间不少于2s , 并要求定位系统匀速旋转, 此时可采集40组数据。下面考察一种极端情况, 即由此带来最大误差的情况。当所采集的点位于图3(b ) 中a 1点位置时, 此时∠a 1Oa =360°/(4032) =415°, 由此带来的误差为图中a 2a 的长度, 通过三角(上接第359页)
及其贮存、打印或网上传送。
统结构没有原则性改变的状况, 系统设计以工业控制用计算机为基础, 外配必要的16位单片机板卡和数字化的超声模块, 组成崭新的智能化超声波探伤系统。系统组成结构如图3所示。
整机由通道、单片机及计算机处理三部分组成。利用16位单片机高速运算特性, 使得计算机从容管理实时控制系统, 并使整机硬件电路拥有足够的扩展余量。时钟卡给出同步脉冲和选择通道的零脉冲。A/D 卡采集伤波峰值幅度(Fv ) 、边缘波峰值幅度(B v ) 。I/O 卡控制通道增益、抑制、报警电平等调节量。闸门卡形成伤波闸门(Fg ) 、边缘波闸门(B g ) 。C/T 卡测量伤波时间(Ft ) 、边缘波时间(B t ) 。通道部分完成信号的发射、放大、采集、整形、显示、报警。与传统探伤仪的区别是系统全部受计算机控制, 像衰减量、抑制量、闸门位置及宽度调节、探伤初始状态的设置、增益闭环自动控制的深度等, 全部由键盘输入指令来完成, 不仅操作简便和节省大量调试时间, 而且还消除了模拟电路特有的温度漂移, 时间漂移等不稳定性。
计算机可调用多种用户应用软件, 可执行各种不同的国内外探伤标准, 最终完成对缺陷形状、尺寸、位置、数量、密集度、合格与判废等资料的图文报告
4 结束语
综上所述, 这种工业控制计算机管理控制的智能化多通道超声波自动探伤系统, 向智能化检测仪器方向迈出了重要的一步, 克服了许多人为因素的影响。不但开发了模拟系统所不可能达到的产品功能, 而且在软件开发上下了功夫, 所编制的智能化判伤软件, 其抗干扰能力和判伤准确性比普通的数字式探伤仪提高许多。电磁方法机械跟踪和电子微跟踪相结合的跟踪系统的高精度能保证一次信号的稳定提取, 满足探伤要求。同时系统设计具有兼容性、通用性, 能够满足不同生产的要求。经过华油钢管公司、巨龙钢管公司、胜利制管公司、华俊钢管公司、天力特种管公司、资阳石油钢管厂、宝鸡石油钢管厂等多家企业用于卷板、中厚钢板, 无缝钢管、螺旋、直缝焊管的焊缝和管体探伤的一年多现场使用, 运行良好。
参考文献:
[1]杨金岩, 郑应强. 一种用于超声波检测的高速数据采集
卡[J].计算机自动测量与控制, 2000, 8(5) :20-22. [2]张晓峰, 等. 多探头超声波车流检测系统的设计[J].
计算机自动测量与控制, 2000, 8(5) :28-
30.