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

权利要求书

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满足调平误差范围时，调平操作结束。