CN107168186A - 基于六轴组合传感器的四点自动调平系统及其工作方法 - Google Patents

Abstract

本发明公开了一种基于六轴组合传感器的四点自动调平系统及其工作方法，所述的四点自动调平系统由六轴组合传感器、执行机构、着地检测开关、激光测距传感器以及控制器所组成；通过六轴组合传感器以欧拉角形式获得当前姿态数据，用坐标变换方法计算执行机构的电缸伸出杆伸缩量，从而简化了调平系统姿态的表示方法与伸缩量计算公式，降低了系统的设计难度，提高了系统工作效率。

Description

基于六轴组合传感器的四点自动调平系统及其工作方法

技术领域

本发明属于工作台姿态自动调平技术领域。

技术背景

目前，为了提高施工的安全性与操作的可靠性，需要对特定工况下的施工装置进行调平控制，如重型车辆调平、机床加工、防空火炮控制、海洋钻井平台、并联机构等。

现有调平方法主要有三点调平、四点调平、六点调平法等，其中，三点调平法简单易行，但受到外力时极易倾覆。而六点调平法算法复杂、调平工况多变，使得调平系统难以有效控制，因此，现有调平方法普遍采用四点调平法，与本申请有关的技术是四点调平法。

目前四点调平法主要技术特点是：

(1)采用倾角传感器检测平台倾角。

(2)采用二维姿态检测方法，控制四个支腿液压缸的伸缩量以实现平台二维姿态的调平。

(3)采用“追逐式”调平法对重型液压平板车进行调平。

(4)采用液压缸式调平执行机构进行调平。

(5)采用“基于PLC控制的4台电缸”进行平台调平。

上述四点调平法的不足是，由于检测方法和控制方法所限，调平精度和系统工作效率低。尤其是液压缸式调平系统，除了调平精度和系统工作效率低，所需的液压泵站与液压缸等设备均占地面积大，系统复杂。

去年，随着搭载传感器的智能手机和游戏机等设备的开发，传感器的需求持续增加。Kionix,Inc.(总部位于美国纽约州伊萨卡)作为ROHM集团旗下的mems传感器的领军企业，开发出六轴加速度陀螺仪组合传感器(3轴加速度传感器+3轴陀螺仪传感器)，以下简称“六轴组合传感器”，它采用Kionix独家开发的相位检测的检测方式。目前该传感器还没有用于自动调平系统的先例。

发明内容

为了克服现有技术的不足，本发明提出一种基于六轴组合传感器的四点自动调平系统。

本发明同时提出该系统的工作方法。

为达到上述目的，本发明采取的技术方案是:

一种基于六轴组合传感器的四点自动调平系统，其特征在于，它由六轴组合传感器、执行机构、着地检测开关、激光测距传感器以及控制器所组成；

所述的执行机构为四个由伺服电动机驱动的电缸，四个电缸按矩形布置方式分别布置在被调平台下面的四角上；

所述的六轴组合传感器布置在被调平台上，并且处于四个电缸对角线的交点上，用于检测被调平台的当前姿态和发出姿态信息；

所述的着地检测开关安装在电缸伸出杆端头上，每个电缸都安装有着地检测开关，用于检测电缸伸出杆与地面的接触状态；

所述的激光测距传感器安装在电缸缸体的侧面，电缸伸出杆端头上设有向外探出的测距反光挡板，并使激光束与测距反光挡板对准，用于检测电缸伸出杆的伸缩量；

所述控制器包括调平系统的控制中枢CPU、外围模块和控制四个伺服电动机运转的驱动模块；

所述CPU是具有信息获取、数据处理并发出控制指令作用的单片机最小系统；

所述外围模块包括：用于显示调平系统运行状态信息的显示装置，例如显示电缸伸出杆当前位置、伸出杆的目标位置、被调平台当前姿态等；用于调平系统启动、停止的输入装置，例如按钮或触摸屏；以及为调平系统提供动力的电源装置。

本发明四点自动调平系统的工作方法是：

第一步：

工作时，打开控制器，启动四个电缸同时运行，使四个电缸伸出杆伸出,当某个电缸伸出杆杆着地时，该端头上的检测开关发出信号，控制器得到着地信号后使该电缸停止运行，直至四个电缸伸出杆全部稳实着地；全部着地后，六轴组合传感器发出被调平台的当前姿态数据信息，控制器获取当前姿态数据信息后转换为欧拉角表达式，表达形式为X-Y-Z欧拉角数据θ_x、θ_y、θ_z；

第二步：

控制器根据欧拉角数据计算四个电缸伸出杆伸缩量，计算过程是：

第2.1步：以当前四个电缸轴心顶点形成的平面矩形FEA’G，建立机身实时坐标系；该坐标系以对角线的交点O'作为机身坐标系的原点，以与A’G、EF平行的方向作为X轴方向，与A’E、GF平行的方向为Y轴方向，并以与所述X轴、Y轴叉乘的方向作为Z轴方向；已知平面矩形FEA’G的边长FG＝EA’＝l₁，FE＝A’G＝l₂；

第2.2步：以四个电缸轴心底部形成的平面矩形ABCD为参考平面，建立参考坐标系；O点为O’沿竖直方向在平面ABCD上的投影，O点为参考坐标系的原点，设参考坐标系所在平面欧拉角θ_x、θ_y的初始值均为0，当沿X轴、Y轴、Z轴的任一轴旋转时，采用右手定则判断欧拉角正负：右手握住转轴，拇指指向转轴的正方向，四指的方向则为欧拉角的正向；

第2.3步：设调平后四个电缸轴心顶点形成的平面为A”B”C”D”；平面A”B”C”D”相对于参考平面ABCD的高度为h，即AA”＝BB”＝CC”＝DD”＝h，F、E、A’、G点上的四个电缸的伸缩量分别为h₁、h₂、h₃、h₄，即FC”＝h₁，ED”＝h₂，A’A”＝h₃，GB”＝h₄，h₁、h₂、h₃、h₄的正/负值代表伸出杆需要伸长/收缩；

按以下四种情况计算：

(1)当θ_x>0且θ_y<＝0时，即F点最高或F、E点同为最高点时，四个电缸伸出杆伸缩量计算如下：

h₁＝h-l₁sin(θ_x)-l₂sin(θ_y)

h₂＝h-l₁sin(θ_x)

h₃＝h

h₄＝h-l₂sin(θ_y)

(2)当θ_x>＝0且θ_y>0时，即E点最高或E、A’点同为最高点时，四个电缸伸出杆伸缩量计算如下：

h₁＝h-l₁sin(θ_x)

h₂＝h-l₁sin(θ_x)-l₂sin(θ_y)

h₃＝h-l₂sin(θ_y)

h₄＝h

(3)当θ_x<0且θ_y>＝0时，即A’点最高或A’、G点同为最高点时，四个电缸伸出杆伸缩量计算如下：

h₁＝h

h₂＝h-l₂sin(θ_y)

h₃＝h-l₁sin(θ_x)-l₂sin(θ_y)

h₄＝h-l₁sin(θ_x)

(4)当θ_x<＝0且θ_y<0时，即G点最高或G、F点同为最高点时，四个电缸伸出杆伸缩量计算如下：

h₁＝h-l₂sin(θ_y)

h₂＝h

h₃＝h-l₁sin(θ_x)

h₄＝h-l₁sin(θ_x)-l₂sin(θ_y)

上述式中，h<＝max(h₁,h₂,h₃,h₄)，且不小于最高点到参考平面的距离；

第三步：

控制器把计算出的四个电缸伸出杆的伸缩量信息，传送给电动机驱动模块，电动机驱动模块分别控制A’、G、F、E点上四个电缸的伺服电机正/反转运行，使电缸伸出杆伸/缩，激光测距传感器随时将伸/缩量信号传输给控制器，当某个电缸伸出杆伸/缩量符合计算值时，控制器将停止信息传送给电动机驱动模块，使该电缸停止运行；

在调平过程中，六轴组合传感器始终动态监测被调平台欧拉角，并始终动态计算电缸伸出杆所需伸缩量，直至θ_x、θ_y满足调平误差范围时，调平操作结束。

本发明的有益效果是：

(1)本发明采用四点动态调平，用六轴组合传感器以欧拉角形式获得当前姿态数据，用坐标变换方法计算电缸伸出杆伸缩量，简化了调平系统姿态的表示方法与伸缩量计算公式，降低了系统的设计难度，提高了系统工作效率。

(2)根据激光测距传感器实时测得电缸伸出杆伸缩量，通过控制器动态控制伸出杆伸缩量，降低了调平误差，提高了调平系统整体稳定性。

(3)本发明使用的电缸是将伺服电机与丝杠一体化设计的模块化产品，将伺服电机的旋转运动转换成直线运动；通过伺服电机的精确转速控制、精确转数控制、精确扭矩控制转变成调平时的精确速度控制、精确位置控制、精确推力控制；而且省去了液压泵源等庞大设备，简化了调平系统，节约了成本。

(4)调平过程中，始终动态监测被调平台欧拉角，并始终动态计算电缸伸出杆所需伸缩量，直到各欧拉角达到设定的目标值范围内时，调平控制才会结束，所以调平位置准确。

附图说明

图1是本发明实施例的立体示意图；

图2是本发明实施例的平面布置图；

图3a是本发明实施例的参考坐标系图；

图3b是本发明实施例调平后的平面示意图；

图4是本发明实施例控制器的控制流程图；

图5是本发明实施例系统工作流程图。

图中，1-执行机构，2-被调平台，3-控制器，4-六轴组合传感器，5-电缸伸出杆，6-着地检测开关，7-激光测距传感器，8-测距反光挡板。

具体实施方式

下面参照附图，对本发明进一步说明。

如图1图2所示，一种基于六轴组合传感器的四点自动调平系统，由执行机构1、控制器3、六轴组合传感器4、着地检测开关6以及激光测距传感器7所组成；

所述的执行机构1为四个电缸，四个电缸按矩形布置方式分别布置在被调平台2下面的四角上；

所述的六轴组合传感器4采用Kionix公司开发的型号为KXG07或KXG0708的六轴加速度陀螺仪组合传感器，六轴组合传感器4布置在被调平台2上，并且处于四个电缸对角线的交点上，用于检测被调平台2的当前姿态和发出姿态信息；

所述的着地检测开关6安装在电缸伸出杆5端头上，每个电缸都安装有着地检测开关6，用于检测电缸伸出杆5与地面的接触状态；

所述的激光测距传感器7安装在电缸缸体的侧面，电缸伸出杆5端头上设有向外探出的测距反光挡板8，使激光束与测距反光挡板8对准，用于检测电缸伸出杆5的伸缩量；

如图4所示，所述控制器3包括调平系统的控制中枢CPU、外围模块和控制四个伺服电动机运转的驱动模块；

所述CPU是具有信息获取、数据处理并发出控制指令作用的单片机最小系统；

所述外围模块包括：用于显示调平系统运行状态信息的显示装置，例如显示电缸伸出杆当前位置、伸出杆的目标位置、被调平台当前姿态等；用于调平系统启动、停止的输入装置，例如按钮或触摸屏；以及为调平系统提供动力的电源装置。

所述单片机最小系统为整个系统的控制中枢，具有信息获取、数据处理并发出控制指令的作用。所述信息获取是接收六轴组合传感器与激光测距传感器的信息。所述数据处理是将检测到的六轴组合传感器数据按照设定好的算法进行计算与处理，得到调平所需的伸缩量，同时通过激光测距传感器动态检测四个电缸伸出杆的伸缩量。最后发出控制指令控制四个电缸动作，实现机身调平。

所述显示装置用于显示调平系统运行状态等信息，如电缸伸出杆当前位置、伸出杆的目标位置、调平装置当前姿态、各模块电流值、电压值等。

所述输入装置用于控制调平系统运行，如启动、停止等功能。

所述电源装置用于提供调平系统所需动力。

所述电动机驱动模块用于接收单片机的控制信号，并驱动四个电缸伺服电动机的运转。

本发明实施例四点自动调平系统的工作方法参见图5，步骤是：

第一步：

工作时，打开控制器，启动四个电缸同时运行，使四个电缸伸出杆5伸出,当某个电缸伸出杆杆着地时，该端头上的着地检测开关6发出信号，控制器3得到着地信号后使该电缸停止运行，直至四个电缸伸出杆5全部稳实着地；全部着地后，六轴组合传感器4发出被调平台的当前姿态数据信息，控制器3获取当前姿态数据信息后转换为欧拉角表达式，表达形式为X-Y-Z欧拉角数据θ_x、θ_y、θ_z；

第二步：

控制器根据欧拉角数据计算四个电缸伸出杆伸缩量，计算过程是：

1.以当前四个电缸轴心顶点形成的平面矩形FEA’G，建立机身实时坐标系，如图3a所示；该坐标系以对角线的交点O'作为机身坐标系的原点，以与A’G、EF平行的方向作为X轴方向，与A’E、GF平行的方向为Y轴方向，并以与所述X轴、Y轴叉乘的方向作为Z轴方向；已知平面矩形FEA’G的边长FG＝EA’＝l₁，FE＝A’G＝l₂；

2.在以四个电缸轴心底部形成的平面矩形ABCD为参考平面，建立参考坐标系，如图3所示；O点为O’沿竖直方向在平面ABCD上的投影，O点为参考坐标系的原点，设参考坐标系所在平面欧拉角θ_x、θ_y的初始值均为0，当沿X轴、Y轴、Z轴的任一轴旋转时，采用右手定则判断欧拉角正负：右手握住转轴，拇指指向转轴的正方向，四指的方向则为欧拉角的正向；

3.设调平后四个电缸轴心顶点形成的平面为A”B”C”D”，如图3b所示；平面A”B”C”D”相对于参考平面ABCD的高度为h，即AA”＝BB”＝CC”＝DD”＝h，F、E、A’、G点上的四个电缸的伸缩量分别为h₁、h₂、h₃、h₄，即FC”＝h₁，ED”＝h₂，A’A”＝h₃，GB”＝h₄，h₁、h₂、h₃、h₄的正/负值代表伸出杆需要伸长/收缩；

按以下四种情况计算：

(1)当θ_x>0且θ_y<＝0时，即F点最高或F、E点同为最高点时，四个电缸伸出杆伸缩量计算如下：

h₁＝h-l₁sin(θ_x)-l₂sin(θ_y)

h₂＝h-l₁sin(θ_x)

h₃＝h

h₄＝h-l₂sin(θ_y)

(2)当θ_x>＝0且θ_y>0时，即E点最高或E、A’点同为最高点时，四个电缸伸出杆伸缩量计算如下：

h₁＝h-l₁sin(θ_x)

h₂＝h-l₁sin(θ_x)-l₂sin(θ_y)

h₃＝h-l₂sin(θ_y)

h₄＝h

(3)当θ_x<0且θ_y>＝0时，即A’点最高或A’、G点同为最高点时，四个电缸伸出杆伸缩量计算如下：

h₁＝h

h₂＝h-l₂sin(θ_y)

h₃＝h-l₁sin(θ_x)-l₂sin(θ_y)

h₄＝h-l₁sin(θ_x)

(4)当θ_x<＝0且θ_y<0时，即G点最高或G、F点同为最高点时，四个电缸伸出杆伸缩量计算如下：

h₁＝h-l₂sin(θ_y)

h₂＝h

h₃＝h-l₁sin(θ_x)

h₄＝h-l₁sin(θ_x)-l₂sin(θ_y)

上述式中，h<＝max(h₁,h₂,h₃,h₄)，且不小于最高点到参考平面的距离。

第三步：

控制器3把计算出的四个电缸伸出杆5的伸缩量信息，传送给电动机驱动模块，电动机驱动模块控制四个电缸的伺服电机正/反转运行，使电缸伸出杆伸/缩，激光测距传感器7随时将伸/缩量信号传输给控制器3，当某个电缸伸出杆5伸/缩量符合计算值时，控制器3将停止信息传送给电动机驱动模块，使该电缸停止运行；

在调平过程中，六轴组合传感器4始终动态监测平台欧拉角，并始终动态计算电缸伸出杆5所需伸缩量，直至θ_x、θ_y满足误差范围时，调平操作结束。

Claims (2)

1.一种基于六轴组合传感器的四点自动调平系统及其工作方法，其特征在于，它由六轴组合传感器、执行机构、着地检测开关、激光测距传感器以及控制器所组成；

所述的执行机构为四个由伺服电动机驱动的电缸，四个电缸按矩形布置方式分别布置在被调平台下面的四角上；

所述的六轴组合传感器布置在被调平台上，并且处于四个电缸对角线的交点上，用于检测被调平台的当前姿态和发出姿态信息；

所述的着地检测开关安装在电缸伸出杆端头上，每个电缸都安装有着地检测开关，用于检测电缸伸出杆与地面的接触状态；

所述的激光测距传感器安装在电缸缸体的侧面，电缸伸出杆端头上设有向外探出的测距反光挡板，并使激光束与测距反光挡板对准，用于检测电缸伸出杆的伸缩量；

所述控制器包括调平系统的控制中枢CPU、外围模块和控制四个伺服电动机运转的驱动模块；

所述CPU是具有信息获取、数据处理并发出控制指令作用的单片机最小系统；

所述外围模块包括：用于显示调平系统运行状态信息的显示装置，用于调平系统启动、停止的输入装置，以及为调平系统提供动力的电源装置。

2.一种如权利要求1所述的四点自动调平系统的工作方法，其特征在于，

第一步：

工作时，打开控制器，启动四个电缸同时运行，使四个电缸伸出杆伸出,当某个电缸伸出杆杆着地时，该端头上的检测开关发出信号，控制器得到着地信号后使该电缸停止运行，直至四个电缸伸出杆全部稳实着地；全部着地后，六轴组合传感器发出被调平台的当前姿态数据信息，控制器获取当前姿态数据信息后转换为欧拉角表达式，表达形式为X-Y-Z欧拉角数据θ_x、θ_y、θ_z；

第二步：

控制器根据欧拉角数据计算四个电缸伸出杆伸缩量，计算过程是：

第2.1步：以当前四个电缸轴心顶点形成的平面矩形FEA'G，建立机身实时坐标系；该坐标系以对角线的交点O'作为机身坐标系的原点，以与A'G、EF平行的方向作为X轴方向，与A'E、GF平行的方向为Y轴方向，并以与所述X轴、Y轴叉乘的方向作为Z轴方向；已知平面矩形FEA'G的边长FG＝EA'＝l₁，FE＝A'G＝l₂；

第2.2步：以四个电缸轴心底部形成的平面矩形ABCD为参考平面，建立参考坐标系；O点为O'沿竖直方向在平面ABCD上的投影，O点为参考坐标系的原点，设参考坐标系所在平面欧拉角θ_x、θ_y的初始值均为0，当沿X轴、Y轴、Z轴的任一轴旋转时，采用右手定则判断欧拉角正负：右手握住转轴，拇指指向转轴的正方向，四指的方向则为欧拉角的正向；

第2.3步：设调平后四个电缸轴心顶点形成的平面为A”B”C”D”；平面A”B”C”D”相对于参考平面ABCD的高度为h，即AA”＝BB”＝CC”＝DD”＝h，F、E、A'、G点上的四个电缸的伸缩量分别为h₁、h₂、h₃、h₄，即FC”＝h₁，ED”＝h₂，A'A”＝h₃，GB”＝h₄，h₁、h₂、h₃、h₄的正/负值代表伸出杆需要伸长/收缩；

按以下四种情况计算：

(1)当θ_x>0且θ_y<＝0时，即F点最高或F、E点同为最高点时，四个电缸伸出杆伸缩量计算如下：

h₁＝h-l₁sin(θ_x)-l₂sin(θ_y)

h₂＝h-l₁sin(θ_x)

h₃＝h

h₄＝h-l₂sin(θ_y)

(2)当θ_x>＝0且θ_y>0时，即E点最高或E、A'点同为最高点时，四个电缸伸出杆伸缩量计算如下：

h₁＝h-l₁sin(θ_x)

h₂＝h-l₁sin(θ_x)-l₂sin(θ_y)

h₃＝h-l₂sin(θ_y)

h₄＝h

(3)当θ_x<0且θ_y>＝0时，即A'点最高或A'、G点同为最高点时，四个电缸伸出杆伸缩量计算如下：

h₁＝h

h₂＝h-l₂sin(θ_y)

h₃＝h-l₁sin(θ_x)-l₂sin(θ_y)

h₄＝h-l₁sin(θ_x)

(4)当θ_x<＝0且θ_y<0时，即G点最高或G、F点同为最高点时，四个电缸伸出杆伸缩量计算如下：

h₁＝h-l₂sin(θ_y)

h₂＝h

h₃＝h-l₁sin(θ_x)

h₄＝h-l₁sin(θ_x)-l₂sin(θ_y)

上述式中，h<＝max(h₁,h₂,h₃,h₄)，且不小于最高点到参考平面的距离；

第三步：

控制器把计算出的四个电缸伸出杆的伸缩量信息，传送给电动机驱动模块，电动机驱动模块分别控制A'、G、F、E点上四个电缸的伺服电机正/反转运行，使电缸伸出杆伸/缩，激光测距传感器随时将伸/缩量信号传输给控制器，当某个电缸伸出杆伸/缩量符合计算值时，控制器将停止信息传送给电动机驱动模块，使该电缸停止运行；

在调平过程中，六轴组合传感器始终动态监测被调平台欧拉角，并始终动态计算电缸伸出杆所需伸缩量，直至θ_x、θ_y满足调平误差范围时，调平操作结束。