三维姿态测控装置、机械设备及三维姿态的测控方法与流程

本发明涉及工程机械的三维姿态测量领域，特别是涉及一种三维姿态测控装置、机械设备及三维姿态的测试方法。

背景技术：

智能控制系统是工程机械(如工业机器人、压路机、播种机等)生产发展的新潮流，是以信息技术为支撑，根据载体的时空信息，定时、定量地实施一整套现代化操作与管理的控制系统，要求以最经济的投入取得最佳的生产和环境效益。

高精度的载体三维姿态的控制通常是工程机械智能控制系统中最重要的一环，其需要对载体的三维姿态进行准确的测量。

目前工程机械的三维姿态测量主要有两种应用场景：一种是姿态异常监测，比如海上浮标的倾斜监测和渣土车的翻车监测；另一种是姿态跟踪控制，比如工业级机器人机械臂的控制和农机的农具控制。目前三维姿态测量的方法主要电子罗盘法，电子罗盘一般包含两个水平方向的加速度计和一个三轴磁力计，通过两个水平加速度计测量载体倾斜角度，包括载体的俯仰角和横滚角，再通过俯仰角和横滚角对三轴磁力计的测量值进行调平，通过三轴磁力计计算出载体的航向角。

由于电子罗盘单纯依靠加速度计的测量值计算俯仰角和横滚角，因此，加速度计的系统误差和量测噪声会直接影响俯仰角和横滚角的计算精度。俯仰角和横滚角的计算是以重力矢量作为参考，而加速度计并不能区分重力矢量的分量和载体的线加速度信息，因此，当载体存在线加速度时，通过加速度计测量的俯仰角和横滚角会存在较大的误差。三轴磁力计对电磁环境敏感，当三轴磁力计周围的电磁环境发生变化时，会导致三轴磁力计的测量值具有较大的误差，导致航向角计算错误。

技术实现要素：

基于此，有必要提供一种三维姿态测控装置、机械设备及三维姿态测控方法，提高对载体三维姿态的测控精度。

一种三维姿态测控装置，包括：

电路板；

安装于所述电路板一侧的三轴加速度计、三轴磁力计和三个互相垂直的单轴陀螺仪；

安装于所述电路板另一侧的处理控制模块；

所述处理控制模块对所述三轴加速度计输出的线加速度及所述三轴磁力计输出的磁场强度进行预置偏差补偿，并以所述三轴加速度计所处位置的重力矢量和所述三轴磁力计所处位置的地球磁场矢量为参考，根据所述三个单轴陀螺仪输出的旋转角速度分别计算出载体的俯仰角、横滚角、航向角，所述处理控制模块根据载体的俯仰角、横滚角和航向角对所述三轴加速度计输出的线加速度和所述三轴磁力计输出的磁场强度进行坐标转换，并在分别获取转换后的线加速度和磁场强度与对应重力矢量和地球磁场强度的差值后，通过数据融合算法对所述载体的俯仰角、横滚角和航向角进行修正并对所述三轴陀螺仪输出的旋转角速度进行误差补偿。

在其中一个实施例中，所述处理控制模块为CPU。

在其中一个实施例中，所述处理控制模块通过椭球校正算法对所述三轴加速度计输出的线加速度及所述三轴磁力计输出的磁场强度进行预置偏差补偿。

在其中一个实施例中，所述转换后的线加速度与对应重力矢量的差值为：加速度计输出的线加速度投影到重力矢量所在三维坐标系的加速度值与所述重力矢量的差值；所述转换后的磁场强度与对应地球磁场强度的差值为：磁力计输出的磁场强度投影到地球磁场所在的三维坐标系的磁场强度值与所述地球磁场强度的差值。

在其中一个实施例中，所述处理控制模块通过数据融合算法对所述载体的俯仰角、横滚角和航向角进行修正并对陀螺仪输出的旋转角速度进行误差补偿具体为：

所述处理控制模块通过数据融合算法融合三个单轴陀螺仪、三轴磁力计和三轴加速度计的量测值，通过三轴加速计和三轴磁力计的量测值对三个单轴陀螺仪计算的俯仰角、横滚角、航向角进行修正并对所述三个单轴陀螺仪输出的旋转角度的误差进行补偿。

在其中一个实施例中，所述数据融合算法为Kalman滤波数据融合算法。

以上所述三维姿态测控装置中，电路板的一侧集成有三轴加速度计、三轴磁力计和三个互相垂直的单轴陀螺仪，另一侧集成有处理控制模块，与将三轴加速度计、三轴磁力计、三个互相垂直的单轴陀螺仪及处理控制模块集成在电路板的同一侧相比，减小了电路板的面积，进而缩小了整个三维姿态测控装置的体积；对三轴加速度计输出的线加速度及三轴磁力计输出的磁场强度进行预置偏差补偿，可使三轴加速度计和三轴磁力计输出的值更加精准；根据三轴加速度计输出的线加速度与重力矢量存在的差值和三轴磁力计输出的磁场强度与地球磁场存在的差值，分别计算出俯仰角、横滚角、航向角及三个单轴陀螺仪输出的旋转角度的误差值，可以提升测量的俯仰角、横滚角、航向角和旋转角度的准确性，依次对载体的三维姿态进行控制，可有效提升对载体三维姿态的稳定性。

一种机械设备，所述机械设备安装有如以上所述的三维姿态测控装置。

所述机械设备安装有以上所述三维姿态测控装置，可有效提升测量和调控载体三维姿态的精确性。

一种三维姿态的测控方法，基于以上所述的三维姿态测控装置，所述方法包括：

获取所述三轴加速度计静态输出的俯仰角、横滚角及所述三轴磁力计静态输出的航向角；

对所述三轴加速度计输出的线加速度和三轴磁力计输出的磁场强度进行预置偏差补偿；

分别获取三轴加速度计输出的线加速度与对应重力矢量的差值和三轴磁力计输出的磁场强度与对应地球磁场强度的差值；

根据所述差值分别计算所述俯仰角、横滚角、航向角及所述三个单轴陀螺仪输出的旋转角度的误差值；

根据所述俯仰角、横滚角、航向角及所述三个单轴陀螺仪输出的旋转角度的误差值调整载体的三维姿态。

在其中一个实施例中，通过椭球校正算法对所述三轴加速度计输出的线加速度和三轴磁力计输出的磁场强度预置偏差补偿。

在其中一个实施例中，所述三轴加速度计输出的线加速度与对应重力矢量存在的差值为：三轴加速度计输出的线加速度投影到重力矢量所在三维坐标系的加速度值与所述重力矢量的差值；所述三轴磁力计输出的磁场强度与对应地球磁场存在的差值为：三轴磁力计输出的磁场强度投影到地球磁场所在的三维坐标系的磁场强度值与所述地球磁场强度的差值。

以上所述三维姿态的测控方法中，对三轴加速度计输出的线加速度及三轴磁力计输出的磁场强度进行预置偏差补偿，可使三轴加速度计和三轴磁力计输出的值更加精准；根据三轴加速度计输出的线加速度与重力矢量存在的差值和三轴磁力计输出的磁场强度与地球磁场存在的差值，分别计算出俯仰角、横滚角、航向角及三个单轴陀螺仪输出的旋转角度的误差值，可以提升测量的俯仰角、横滚角、航向角和旋转角度的准确性，依次对载体的三维姿态进行控制，可有效提升对载体三维姿态的稳定性。

附图说明

图1为一实施例的三维姿态测控装置的结构示意图；

图2为一实施例的三维姿态的测控方法的流程示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

如图1所示，一实施例的三维姿态测控装置包括：

电路板150；

安装于电路板150一侧的三轴加速度计120、三轴磁力计130和三个互相垂直的单轴陀螺仪110；

安装于电路板150另一侧的处理控制模块140；

处理控制模块140对三轴加速度计120输出的线加速度及三轴磁力计130输出的磁场强度进行预置偏差补偿，并以三轴加速度120计所处位置的重力矢量和所述三轴磁力计130所处位置的地球磁场矢量为参考，根据三个单轴陀螺仪110输出的旋转角速度分别计算出载体的俯仰角、横滚角、航向角，处理控制模块140根据载体的俯仰角、横滚角和航向角对三轴加速度计120输出的线加速度和三轴磁力计130输出的磁场强度进行坐标转换，并在分别获取转换后的线加速度和磁场强度与对应重力矢量和地球磁场强度的差值后，通过数据融合算法对所述载体的俯仰角、横滚角和航向角进行修正并对三轴陀螺仪110输出的旋转角速度进行误差补偿。

以上所述三维姿态测控装置中，电路板的一侧集成有三轴加速度计、三轴磁力计和三个互相垂直的单轴陀螺仪，另一侧集成有处理控制模块，与将三轴加速度计、三轴磁力计、三个互相垂直的单轴陀螺仪及处理控制模块集成在电路板的同一侧相比，减小了电路板的面积，进而缩小了整个三维姿态测控装置的体积；对三轴加速度计输出的线加速度和三轴磁力计输出的磁场强度进行预置偏差补偿，可使三轴加速度计和三轴磁力计输出的值更加精准；根据三轴加速度计输出的线加速度与重力矢量存在的差值和三轴磁力计输出的磁场强度与地球磁场存在的差值，分别计算出俯仰角、横滚角、航向角及三个单轴陀螺仪输出的旋转角度的误差值，可以提升测量的俯仰角、横滚角、航向角和旋转角度的准确性，依次对载体的三维姿态进行控制，可有效提升对载体三维姿态的稳定性。

如图1所示，图1中所显示的排列方式只是实现本实施例的最优选择，其中，三个互相垂直的单轴陀螺仪110的其中一个设置为垂直于电路板150，另外两个与电路板150平行设置，平行设置的两个单轴陀螺仪110的垂直夹角处安装三轴加速度计120和三轴磁力计130。这样结构设计可最大程度地减小电路板150的面积，缩小整个三维姿态测控装置的体积。

本实施例中，处理控制模块140为CPU处理器，其安装于电路板150的另一侧，与将三轴加速度计、三轴磁力计、三个互相垂直的单轴陀螺仪及处理控制模块集成在电路板的同一侧相比，减小了电路板的面积，进而缩小了整个三维姿态测控装置的体积。

可以知道的是，三个单轴陀螺仪110测量的是载体绕三个正交轴(三个单轴陀螺仪的三个相互正交轴)正方向上的旋转角速度，通过角度积分运算可以确定载体绕三个正交轴转过的角度，进而测量角度的变化量。但测试时需要载体的初始姿态，且由于陀螺仪的测量值中含有零偏、交叉耦合和随机噪声等误差，其角度积分运算的误差会随时间累积，所以需要对角度积分运算计算的角度进行校正，并对陀螺仪的测量误差进行补偿。因此，通常陀螺仪测量姿态具有短期精度高、动态性好、误差长期漂移的特点。

可以知道的是，静态情况下，三轴加速度计120可以测量重力的反作用力在载体三个正交坐标轴上的分量，从而确定载体的倾斜情况，包括载体的俯仰角(pitch)和横滚角(roll)。三轴加速度计120是直接通过离散的量测值计算角度，不需要经过积分运算，因此，三轴加速度计120计算角度的误差不会随时间累积。静态情况下，三轴加速度计120计算角度的精度可以达到0.1°以上。但在动态情况下，由于三轴加速度计120只能测量载体的比力，即除重力以外的力的合力，而不能从其测量值中辨识载体的运动加速度，载体的运动加速度越大，计算角度的误差也越大，且三轴加速度计的零偏，交叉耦合和随机噪声等误差也会影响其计算角度的精度。因此，通常三轴加速度计具有静态精度高、动态精度低、系统偏差、误差不随时间累积的特点。

可以知道的是，三轴磁力计130可以测量地球磁场在载体三个正交坐标轴系上的分量，水平放置磁力计时可以通过三轴磁力计130的水平轴测得的地球磁场分量计算出航向角，因此，使用三轴磁力计130测量航向角首先需要利用载体的俯仰角和横滚角对三轴磁力计130的量测值进行调平，理想情况下三轴磁力计计算的航向角的精度可以达到1°以上，但磁力计的零偏，交叉耦合和随机噪声等误差会影响航向角的计算精度，尤其当磁力计周围的电磁环境发生变化时，磁力计的测量值会受到明显的干扰从而导致航向角计算错误。因此，磁力计测量航向角存在易受电磁环境干扰的特点。

单轴陀螺仪、三轴加速度计和三轴磁力计各自能够完成载体三维姿态测量的部分工作，有各自明显的优点。其中，单轴陀螺仪短期精度高、动态性好；三轴加速度计误差不积累、静态精度高；三轴磁力计能指北、误差不累积。同时，单轴陀螺仪、三轴加速度计和三轴磁力计也有各自明显的缺点，其中，单轴陀螺仪误差会积累且需要初始姿态；三轴加速度计会受载体加速度和系统误差的影；三轴磁力计会受电磁干扰和系统误差的影响。

本实施例中，综合设置有单轴陀螺仪、三轴加速度计和三轴磁力，通过三者的互补性，通过椭球校正算法补偿三轴加速度计和三轴磁力计的系统误差，主要是零偏和交叉耦合误差，再通过数据融合算法将单轴陀螺仪、三轴加速度计和三轴磁力计的量测数据进行融合从而实现单轴陀螺仪、三轴加速度计和三轴磁力计的优势互补。

具体的，本实施例中，处理控制模块140通过椭球校正算法对三轴加速度计输出的线加速度及三轴磁力计输出的磁场强度进行预置偏差补偿。椭球校正算法可以对三轴加速度计和三轴磁力计的零偏、交叉耦合等系统偏差进行补偿，从而极大提高三轴加速度计和三轴磁力计的原始观测值的精度。进一步通过三轴加速度计和三轴磁力计的静态输出的载体的俯仰角、横滚角和航向角即可作为三个单轴陀螺仪姿态更新的初始值。

通常三维姿态的解算存在误差，因此，将线加速度和磁场强度投影到导航系后其值与重力矢量和地球磁场不符，本实施例利用它们的差值通过Kalman滤波数据融合算法估计出三维姿态和陀螺仪仪器参数的误差并对三维姿态和陀螺仪参数进行校正。因为单轴陀螺仪积分解算的短期精度高，可以利用差值判别三轴加速度计和三轴磁力计是否受到干扰，从而提高整个三维姿态测控装置的鲁棒性。

其中，将线加速度和磁场矢量投影到导航系后其值与重力矢量和地球磁场的值不符，即三轴加速度计120输出与重力矢量存在的误差和三轴磁力计输出与地球磁场存在的误差。三轴加速度计存在的重力误差为三轴加速度计输出的线加速度投影到重力矢量所在三维坐标系的加速度值与所述重力矢量的差值，三维坐标系有三个极轴，三轴加速度计输出的线加速度投影到每一个极轴的值与重力矢量在对应极轴分量的差值即为重力误差值，误差值具有3个，分别为在三个极轴上的重力误差。三轴磁力计存在的磁力误差为三轴磁力计输出的磁场强度投影到地球磁场所在的三维坐标系的磁场强度值与所述地球磁场强度的差值，同理，三维坐标系有三个极轴，磁力计输出的磁场强度投影到每一个极轴的值与地球磁场在对应极轴上的分量的差值即为磁力误差值，误差值为3个，分别为在三个极轴上的磁力误差。

本实施例中，处理控制模块通过Kalman滤波数据融合算法分别计算出俯仰角、横滚角、航向角及三个单轴陀螺仪输出的旋转角度的误差值，并根据误差值调整载体的三维姿态。由此，本实施例可有效提高载体三维姿态的测量和调控精度。

本实施例还提供了一种机械设备，机械设备安装有如以上所述的三维姿态测控装置。

根据以上可知，机械设备安装有以上所述三维姿态测控装置，，可有效提升测量和调控载体三维姿态的精确性。

如图2所示，为一实施例的三维姿态的测控方法的流程示意图，所述测控方法基于以上所述的三维姿态测控装置。具体的，所述方法包括步骤S110至步骤S150。

步骤S110，获取三轴加速度计输出的俯仰角、横滚角及三轴磁力计静态输出的航向角；

步骤S120，对三轴加速度计输出的线加速度和三轴磁力计输出的磁场强度进行预置偏差补偿；

步骤S130，分别获取三轴加速度计输出的线加速度与对应重力矢量的差值和三轴磁力计输出的磁场强度与对应地球磁场强度的差值；

步骤S140，根据差值分别计算俯仰角、横滚角、航向角及三个单轴陀螺仪输出的旋转角度的误差值；

步骤S150，根据俯仰角、横滚角、航向角及三个单轴陀螺仪输出的旋转角度的误差值调整载体的三维姿态。

以上所述三维姿态的测控方法中，对三轴加速度计输出的线加速度及三轴磁力计输出的磁场强度进行预置偏差补偿，可使三轴加速度计和三轴磁力计输出的值更加精准；根据三轴加速度计输出的线加速度与与重力矢量存在的差值和三轴磁力计输出的磁场强度与地球磁场存在的差值，分别计算出俯仰角、横滚角、航向角及三个单轴陀螺仪输出的旋转角度的误差值，可以提升测量的俯仰角、横滚角、航向角和旋转角度的准确性，依次对载体的三维姿态进行控制，可有效提升对载体三维姿态的稳定性。

本实施例中，综合设置有单轴陀螺仪、三轴加速度计和三轴磁力，通过三者的互补性，通过椭球校正算法补偿三轴加速度计和三轴磁力计的系统误差，主要是零偏和交叉耦合误差，再通过数据融合算法将单轴陀螺仪、三轴加速度计和三轴磁力计的量测数据进行融合从而实现单轴陀螺仪、三轴加速度计和三轴磁力计的优势互补。

本实施例中，处理控制模块140通过椭球校正算法对三轴加速度计输出的线加速度及三轴磁力计输出的磁场强度进行预置偏差补偿。即步骤S120通过椭球校正算法对三轴加速度计和三轴磁力计进行预置偏差补偿。椭球校正算法可以对三轴加速度计和三轴磁力计的零偏、交叉耦合等系统偏差进行补偿，从而极大提高三轴加速度计和三轴磁力计的原始观测值的精度。进一步通过三轴加速度计和三轴磁力计的静态输出的载体的俯仰角、横滚角和航向角即可作为三个单轴陀螺仪姿态更新的初始值。

通常三维姿态的解算存在误差，因此，将线加速度和和磁场强度投影到导航系后其值与重力矢量和地球磁场的分量不符，本实施例利用它们的差值通过Kalman滤波数据融合算法估计出三维姿态和陀螺仪仪器参数的误差并对三维姿态和陀螺仪参数进行校正。因为单轴陀螺仪积分解算的短期精度高，可以利用差值判别三轴加速度计和三轴磁力计是否受到干扰，从而提高整个三维姿态测控装置的鲁棒性。

其中，将线加速度和和磁场强度投影到导航系后其值与重力矢量和地球磁场的分量不符，即三轴加速度计120输出与重力矢量存在的误差和三轴磁力计输出与地球磁场存在的误差。三轴加速度计存在的重力误差为加速度计输出的线加速度投影到重力矢量所在三维坐标系的加速度值与所述重力矢量的差值，三维坐标系有三个极轴，加速度计输出的线加速度投影到每一个极轴的值与重力矢量在对应极轴分量的差值即为重力误差值，误差值具有3个，分别为在三个极轴上的重力误差。三轴磁力计存在的磁力误差为磁力计输出的磁场强度投影到地球磁场所在的三维坐标系的磁场强度值与所述地球磁场强度的差值，同理，三维坐标系有三个极轴，磁力计输出的磁场强度投影到每一个极轴的值与地球磁场在对应极轴上的分量的差值即为磁力误差值，误差值为3个，分别为在三个极轴上的磁力误差。本实施例中，处理控制模块通过Kalman滤波数据融合算法分别计算出俯仰角、横滚角、航向角及三个单轴陀螺仪输出的旋转角度的误差值，并根据误差值调整载体的三维姿态。由此，本实施例可有效提高载体三维姿态的测量和调控精度。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。