基于MEMS传感器的集装箱吊具位姿检测应用

谢雨端,宋亚男,徐荣华

(广东工业大学 自动化学院,广东 广州　510006)



基于MEMS传感器的集装箱吊具位姿检测应用

谢雨端,宋亚男,徐荣华

(广东工业大学 自动化学院,广东 广州510006)

摘要针对现有的集装箱吊具位姿检测中存在的装卸效率低、成本高的问题,研究了基于MEMS传感器的集装箱吊具位姿检测系统。系统以LSM303DLHC、L3GD20传感器为数据采集模块,以STM32F303为微控制器,以上位机界面为实时动态监控,重点分析了基于梯度下降法的姿态融合算法。通过实验数据对比和监控跟踪界面分析,文中所研究的系统实时性好、精度较高、界面友好,并在相关课题中得到了充分验证,是一款值得推广的集装箱吊具位姿检测系统。

关键词MEMS;集装箱吊具;位姿检测;姿态解算

船舶集装箱的发展,对港口岸边的起重机装卸效率、精准度的要求也随着提升。在起重机小车平移的过程中产生的振动、形变及编码器定位误差等都会影响其定位的准确性,由此可导致其在装卸箱时,吊具无法对准集装箱,从而存在安全隐患[1]。

目前,吊具检测系统主要分为两大类:视觉传感器检测系统和基于惯性传感器的非视觉传感检测系统。基于机器视觉的吊具位姿检测方法,能为吊具控制系统提供可靠的反馈数据[2]。MEMS惯性传感器抗冲击能力强、制造成本低、可靠性高,此外还具有体积小、重量轻、易集成、适合大批量生产的优势,是一种适于构建姿态检测系统的惯性传感器[3]。

利用MEMS 陀螺仪、加速度计和磁传感器等惯性传感器组成的姿态检测系统,能通过对重力矢量夹角和系统转动角速度进行测量,从而实时、准确地检测系统的偏转角度。随着科技的发展,基于MEMS传感器的位姿检测系统的功能会更加完善,系统精确性会不断提高。

1系统架构

基于MEMS传感器的集装箱吊具位姿检测系统由LSM303DLHC、L3GD20数据采集模块,STM32F303微控制器,上位机实时动态监控界面3个核心部分组成。微控制器实时读取传感器数据,并对数据进行姿态解算,同时将得到的姿态信息通过USB通讯接口传送到上位机应用程序进行姿态实时显示。

图1　系统架构框图

图2　系统硬件结构图

L3GD20三轴数字陀螺仪,用于检测吊具的转动角速度分量,LSM303DLHC模块内集成了三轴数字加速度计和三轴磁传感器,加速度计用于检测重力加速度在坐标轴上的分量,磁传感器用于检测地球上的磁场强度分量的方向;复位按键用于系统的复位;LED指示用于模拟在吊具旋转过程中的姿态状况;上位机界面用于吊具姿态的实时监控。

在基于MEMS传感器的集装箱吊具位姿检测系统中,测量数据实时性与可靠性决定了吊具控制的准确性,姿态解算的速度和精度将直接影响姿态控制的稳定性和实时性,而界面监控部分更加直观地显示了系统性能。

陀螺仪测量到的各轴角速度通过累计积分可得到导航角、俯仰角和横滚角,但陀螺仪具有温度漂移特性,且存在长期积分误差累计的问题,短期数据精度高,加速度计噪声比较大,且在运动过程中,受吊具振动影响,短期内的可靠性不高。同时由于磁传感器是一种磁阻传感器,很容易受到外部磁场的干扰,不适合短期测量,可用来长时间姿态角的采集[4]。加速度计在地球上测量的是重力加速度,若吊具沿着竖直Z轴旋转,加速度计传感器无法感知吊具的运动的;磁传感器测量的是地球上的磁场方向,若载体沿着Y轴旋转,磁传感器同样无法感知吊具运动的。基于上述问题,需要通过算法的融合,采用加速度计来修正陀螺仪的垂直误差。采用磁罗盘来修正陀螺仪的水平误差,从而得到准确可靠的吊具三维姿态信息。

2姿态解算与修正

2.1　吊具姿态描述

在基于MEMS传感器的集装箱吊具位姿检测系统中,定义两个基本坐标系:地面坐标系(R系)和载体坐标系(b系)。R系指的是地球上的“东北天”坐标系,b系是吊具自身所形成的坐标系[5]。吊具姿态计算,需要在b系中完成,坐标系之间的转换有3种描述方式:四元数、欧拉角、方向余弦矩阵[6]。

欧拉角简单、直观,但存在奇异问题;四元素计算简便,不存在奇异问题,但表示旋转困难。所以在表示旋转姿态时采用欧拉角,在进行空间旋转的具体计算和插值时,将欧拉角转化为四元素。

R系与b系之间的旋转矩阵(方向余弦矩阵)存在误差,将加速度计的测量数据或者理论数据分别经过旋转矩阵转换在同一坐标系下,利用测量值与理论值的误差来修正旋转矩阵[7]。

2.2　加速度计修正原理

四元素是一个四元矢量,用来描述坐标旋转,可表示为:q=[q0,q1,q2,q3]T,初始值为[1,0,0,0]T,导航角φ,俯仰角θ,横滚角γ分别表示吊具绕Z轴、Y轴和X轴的旋转角度[5]。陀螺仪的测量值[gx,gy,gz]T,单位是rad/s。

(1)

(2)

利用向量积,得到误差向量[eaccx,eaccy,eaccz]T,如式(3)所示

(3)

2.3　磁传感器修正原理

(4)

(5)

该值再和b系中的磁传感器的输出[mx,my,mz]T做向量积得到误差函数如式(6)所示

(6)

故多元向量误差函数为

(7)

对q0、q1、q2、q3求偏导,得到雅可比矩阵为

(8)

基于加速度计、磁传感器融合的梯度计算式

(9)

3姿态融合

利用梯度下降法可求得一组基于加速度计和磁传感器表征的姿态四元数,q结合陀螺仪的数据进行融合[9]。假设由微分方程求解的姿态四元数为q,有

q=α·q+(1-α)qg

(10)

本文讨论直接由反梯度方向的迭代值就直接可以达到目标姿态q[10]

q=-λ·

(11)

在这里梯度下降算法的步长λ与吊具运动的角速度、采样时间相关[7]。当q的收敛速度等于qg的收敛速度时取得的α为最优值[10],即

(12)

式中,β是四元数微分方程求解的姿态算法的收敛速度,该值就是陀螺仪的测量误差,本文取β=1.2。等式右边λ/Vt就是梯度下降算法的收敛速度,进而有

(13)

跟踪物体高速运动时,上式近似为

(14)

上式中,β和Δt较小,α近似为0。

将式(11)和式(14)代入式(10)得

(15)

每个采样周期微处理器读取陀螺仪、加速度计和磁传感器的数据,采样误差和qg,q对上式进行迭代运算,即可实现四元数随时间的更新。最终梯度下降法的姿态融合算法公式为

q(t)=q(t-1)+(qg-β·

(16)

姿态融合算法流程如图3所示。

姿态融合算法修正四元素后,根据四元素到欧拉角的转换式可计算出吊具的姿态欧拉角数据。

(17)

图3　基于梯度下降法的姿态融合算法流程图

图中qg是一个qt相关的表达式,而facc和fmag也是与qt相关的表达式,图中表达的是对qt的迭代运算。

4实验结果分析

基于Python语言实现上位机界面的显示,Python语言语法简捷,具有丰富的模块,可移植性高。本文在界面设计中基于VPython三维图形库,制作实时、交互式的三维动画。通过VPython提供的键盘、鼠标响应事件,实现可手动设置初始吊具坐标系,键盘的space按键或鼠标实现3D模型初始状态的矫正,方便直观。微控制器通过USB接口和主机连接,基于PyUSB模块实现在Windows系统下对USB设备的操作,USBHID设备是通过报告来给传送数据,实时显示吊具姿态和欧拉角,从理论上验证了算法的可行性与跟踪的实时性。

实时界面监控窗口主要由3部分组成:(1)姿态3D实时显示。(2)吊具数据四元素输出和姿态角两种输出。(3)吊具的摆角输出。在此以φ=60°,θ=60°,γ=0°,为实验测试数据,得到数据修正前后的图形。图中heading表示导航角,pitch表示俯仰角,roll表示横滚角,angular表示吊具的摆角。

图4　修正前图形

图5　修正后图形

实验测试中,将基于MEMS传感器的集装箱吊具位姿检测系统在三维空间下旋转,观察监控界面能否跟踪旋转的吊具。同时将未经姿态修正的数据和修正后的数据进行对比,设定值由量角器设定,量角器的精度为分(″),实验数据如表1所示。

表1　实验数据对比表

5结束语

本文介绍了基于MEMS传感器的集装箱吊具位姿检测系统,通过分析姿态测量原理,采用基于梯度下降算法的姿态补偿算法修正姿态,从而得到准确可靠的姿态,最终通过实验数据对比测试,该系统的姿态测量精度在±3°内,该系统硬件部分数据更新率高、短期精度和稳定性好,抗冲击能力强、制造成本低、可靠性高,体积小、易集成,且软件设计部分实时跟踪性与界面友好,是一款值得推广的吊具位姿检测系统。

参考文献

[1]铁道部标准计量研究所.GB1413-85集装箱外部尺寸和额定重量[S].北京:中国标准出版社,1985.

[2]韩云生.基于双目立体视觉的移动机器人目标定位[D].无锡:江南大学,2009.

[3]卢秀和,李建波.基于STM32的四旋翼飞行器姿态控制系统[J].计算机测量与控制,2014,22(3):761-763.

[4]Wang Yangzhu,Li Ning,Chen Xi,et al.Design and implementation of an AHRS based on MEMS sensors and complementary filtering[C].London:Advances in Mechanical Engineering,2014.

[5]秦永元.惯性导航[M].北京:科学出版社,2006.

[6]梁延德,程敏,何福本,等.基于互补滤波器的四旋翼飞行器姿态解算[J].传感器与微系统,2011,30(11):56-58.

[7]朱文杰,王广龙,高凤岐,等.基于MIMU和磁强计的在线实时定姿方法[J].传感技术学报,2013,26(4):536-540.

[8]晁敏,蒋东方,文彩虹.磁罗盘误差分析与校准[J].传感技术学报,2010,23(4):525-531.

[9]Wang Shaohua,Yang Ying.Quadrotor aircraft attitude estimation and control based on Kalman filter[J].Control Theory & Applications,2013,30(9):536-540.

[10]Sebastian O H Madgwick,Andrew J L,Harrison Ravi Vaidyanathan.Estimation of IMU and MARG orientation using gradient descent algorithm[C].NZ:IEEE International Conference on Rehabilitation Robotics,2011.

Position Detection of Container Spreader Based on MEMS Sensors

XIE Yuduan,SONG Yanan,XU Ronghua

(School of Automation,Guangdong University of Technology,Guangzhou 510006,China)

AbstractAccording to the existing problem in position detection system of container spreader,including low handling efficiency highly cost,this paper investigates a position detection system of container spreader based on MEMS sensors.The system consists of the sensor data acquisition modules,the micro-controller and real-time dynamic monitoring.Sensor data acquisition modules contain the LSM303DLHC sensor and the L3GD20 sensor;micro-controller is STM32F303;the real-time dynamic monitoring is the PC interface.The gesture fusion algorithm based on gradient descent is analyzed.Comparison of the experimental data and analysis of the tracking monitoring interface show that the proposed system has good real-time performance,high accuracy and friendly interface.The system has been fully validated in related subjects.

KeywordsMEMS;container spreader;position detection;attitude algorithm

中图分类号TP212

文献标识码A

文章编号1007-7820(2015)10-061-05

doi:10.16180/j.cnki.issn1007-7820.2015.10.017

作者简介:谢雨端(1990—),男,硕士研究生。研究方向:嵌入式软件。E-mail:547604932@qq.com。宋亚男(1976—),女,博士,副教授。研究方向:船舶运动建模与控制。徐荣华(1978—),男,博士,讲师。研究方向:自动化装备。

基金项目:广东省自然科学基金博士启动基金资助项目(S2012040007949);广东工业大学博士启动基金资助项目(12ZK0147);全国工程专业学位研究生教育自选课题资助项目(2014-JY-122);广东工业大学教育教学改革基金资助项目(2013ZY011)

收稿日期:2015-03-12