本發(fā)明屬于工作臺姿態(tài)自動調(diào)平技術(shù)領域。

技術(shù)背景

目前，為了提高施工的安全性與操作的可靠性，需要對特定工況下的施工裝置進行調(diào)平控制，如重型車輛調(diào)平、機床加工、防空火炮控制、海洋鉆井平臺、并聯(lián)機構(gòu)等。

現(xiàn)有調(diào)平方法主要有三點調(diào)平、四點調(diào)平、六點調(diào)平法等，其中，三點調(diào)平法簡單易行，但受到外力時極易傾覆。而六點調(diào)平法算法復雜、調(diào)平工況多變，使得調(diào)平系統(tǒng)難以有效控制，因此，現(xiàn)有調(diào)平方法普遍采用四點調(diào)平法，與本申請有關的技術(shù)是四點調(diào)平法。

目前四點調(diào)平法主要技術(shù)特點是：

(1)采用傾角傳感器檢測平臺傾角。

(2)采用二維姿態(tài)檢測方法，控制四個支腿液壓缸的伸縮量以實現(xiàn)平臺二維姿態(tài)的調(diào)平。

(3)采用“追逐式”調(diào)平法對重型液壓平板車進行調(diào)平。

(4)采用液壓缸式調(diào)平執(zhí)行機構(gòu)進行調(diào)平。

(5)采用“基于plc控制的4臺電缸”進行平臺調(diào)平。

上述四點調(diào)平法的不足是，由于檢測方法和控制方法所限，調(diào)平精度和系統(tǒng)工作效率低。尤其是液壓缸式調(diào)平系統(tǒng)，除了調(diào)平精度和系統(tǒng)工作效率低，所需的液壓泵站與液壓缸等設備均占地面積大，系統(tǒng)復雜。

去年，隨著搭載傳感器的智能手機和游戲機等設備的開發(fā)，傳感器的需求持續(xù)增加。kionix,inc.(總部位于美國紐約州伊薩卡)作為rohm集團旗下的mems傳感器的領軍企業(yè)，開發(fā)出六軸加速度陀螺儀組合傳感器(3軸加速度傳感器+3軸陀螺儀傳感器)，以下簡稱“六軸組合傳感器”，它采用kionix獨家開發(fā)的相位檢測的檢測方式。目前該傳感器還沒有用于自動調(diào)平系統(tǒng)的先例。

技術(shù)實現(xiàn)要素：

為了克服現(xiàn)有技術(shù)的不足，本發(fā)明提出一種基于六軸組合傳感器的四點自動調(diào)平系統(tǒng)。

本發(fā)明同時提出該系統(tǒng)的工作方法。

為達到上述目的，本發(fā)明采取的技術(shù)方案是:

一種基于六軸組合傳感器的四點自動調(diào)平系統(tǒng)，其特征在于，它由六軸組合傳感器、執(zhí)行機構(gòu)、著地檢測開關、激光測距傳感器以及控制器所組成；

所述的執(zhí)行機構(gòu)為四個由伺服電動機驅(qū)動的電缸，四個電缸按矩形布置方式分別布置在被調(diào)平臺下面的四角上；

所述的六軸組合傳感器布置在被調(diào)平臺上，并且處于四個電缸對角線的交點上，用于檢測被調(diào)平臺的當前姿態(tài)和發(fā)出姿態(tài)信息；

所述的著地檢測開關安裝在電缸伸出桿端頭上，每個電缸都安裝有著地檢測開關，用于檢測電缸伸出桿與地面的接觸狀態(tài)；

所述的激光測距傳感器安裝在電缸缸體的側(cè)面，電缸伸出桿端頭上設有向外探出的測距反光擋板，并使激光束與測距反光擋板對準，用于檢測電缸伸出桿的伸縮量；

所述控制器包括調(diào)平系統(tǒng)的控制中樞cpu、外圍模塊和控制四個伺服電動機運轉(zhuǎn)的驅(qū)動模塊；

所述cpu是具有信息獲取、數(shù)據(jù)處理并發(fā)出控制指令作用的單片機最小系統(tǒng)；

所述外圍模塊包括：用于顯示調(diào)平系統(tǒng)運行狀態(tài)信息的顯示裝置，例如顯示電缸伸出桿當前位置、伸出桿的目標位置、被調(diào)平臺當前姿態(tài)等；用于調(diào)平系統(tǒng)啟動、停止的輸入裝置，例如按鈕或觸摸屏；以及為調(diào)平系統(tǒng)提供動力的電源裝置。

本發(fā)明四點自動調(diào)平系統(tǒng)的工作方法是：

第一步：

工作時，打開控制器，啟動四個電缸同時運行，使四個電缸伸出桿伸出,當某個電缸伸出桿桿著地時，該端頭上的檢測開關發(fā)出信號，控制器得到著地信號后使該電缸停止運行，直至四個電缸伸出桿全部穩(wěn)實著地；全部著地后，六軸組合傳感器發(fā)出被調(diào)平臺的當前姿態(tài)數(shù)據(jù)信息，控制器獲取當前姿態(tài)數(shù)據(jù)信息后轉(zhuǎn)換為歐拉角表達式，表達形式為x-y-z歐拉角數(shù)據(jù)θx、θy、θz；

第二步：

控制器根據(jù)歐拉角數(shù)據(jù)計算四個電缸伸出桿伸縮量，計算過程是：

第2.1步：以當前四個電缸軸心頂點形成的平面矩形fea’g，建立機身實時坐標系；該坐標系以對角線的交點o'作為機身坐標系的原點，以與a’g、ef平行的方向作為x軸方向，與a’e、gf平行的方向為y軸方向，并以與所述x軸、y軸叉乘的方向作為z軸方向；已知平面矩形fea’g的邊長fg＝ea’＝l1，fe＝a’g＝l2；

第2.2步：以四個電缸軸心底部形成的平面矩形abcd為參考平面，建立參考坐標系；o點為o’沿豎直方向在平面abcd上的投影，o點為參考坐標系的原點，設參考坐標系所在平面歐拉角θx、θy的初始值均為0，當沿x軸、y軸、z軸的任一軸旋轉(zhuǎn)時，采用右手定則判斷歐拉角正負：右手握住轉(zhuǎn)軸，拇指指向轉(zhuǎn)軸的正方向，四指的方向則為歐拉角的正向；

第2.3步：設調(diào)平后四個電缸軸心頂點形成的平面為a”b”c”d”；平面a”b”c”d”相對于參考平面abcd的高度為h，即aa”＝bb”＝cc”＝dd”＝h，f、e、a’、g點上的四個電缸的伸縮量分別為h1、h2、h3、h4，即fc”＝h1，ed”＝h2，a’a”＝h3，gb”＝h4，h1、h2、h3、h4的正/負值代表伸出桿需要伸長/收縮；

按以下四種情況計算：

(1)當θx>0且θy<＝0時，即f點最高或f、e點同為最高點時，四個電缸伸出桿伸縮量計算如下：

h1＝h-l1sin(θx)-l2sin(θy)

h2＝h-l1sin(θx)

h3＝h

h4＝h-l2sin(θy)

(2)當θx>＝0且θy>0時，即e點最高或e、a’點同為最高點時，四個電缸伸出桿伸縮量計算如下：

h1＝h-l1sin(θx)

h2＝h-l1sin(θx)-l2sin(θy)

h3＝h-l2sin(θy)

h4＝h

(3)當θx<0且θy>＝0時，即a’點最高或a’、g點同為最高點時，四個電缸伸出桿伸縮量計算如下：

h1＝h

h2＝h-l2sin(θy)

h3＝h-l1sin(θx)-l2sin(θy)

h4＝h-l1sin(θx)

(4)當θx<＝0且θy<0時，即g點最高或g、f點同為最高點時，四個電缸伸出桿伸縮量計算如下：

h1＝h-l2sin(θy)

h2＝h

h3＝h-l1sin(θx)

h4＝h-l1sin(θx)-l2sin(θy)

上述式中，h<＝max(h1,h2,h3,h4)，且不小于最高點到參考平面的距離；

第三步：

控制器把計算出的四個電缸伸出桿的伸縮量信息，傳送給電動機驅(qū)動模塊，電動機驅(qū)動模塊分別控制a’、g、f、e點上四個電缸的伺服電機正/反轉(zhuǎn)運行，使電缸伸出桿伸/縮，激光測距傳感器隨時將伸/縮量信號傳輸給控制器，當某個電缸伸出桿伸/縮量符合計算值時，控制器將停止信息傳送給電動機驅(qū)動模塊，使該電缸停止運行；

在調(diào)平過程中，六軸組合傳感器始終動態(tài)監(jiān)測被調(diào)平臺歐拉角，并始終動態(tài)計算電缸伸出桿所需伸縮量，直至θx、θy滿足調(diào)平誤差范圍時，調(diào)平操作結(jié)束。

本發(fā)明的有益效果是：

(1)本發(fā)明采用四點動態(tài)調(diào)平，用六軸組合傳感器以歐拉角形式獲得當前姿態(tài)數(shù)據(jù)，用坐標變換方法計算電缸伸出桿伸縮量，簡化了調(diào)平系統(tǒng)姿態(tài)的表示方法與伸縮量計算公式，降低了系統(tǒng)的設計難度，提高了系統(tǒng)工作效率。

(2)根據(jù)激光測距傳感器實時測得電缸伸出桿伸縮量，通過控制器動態(tài)控制伸出桿伸縮量，降低了調(diào)平誤差，提高了調(diào)平系統(tǒng)整體穩(wěn)定性。

(3)本發(fā)明使用的電缸是將伺服電機與絲杠一體化設計的模塊化產(chǎn)品，將伺服電機的旋轉(zhuǎn)運動轉(zhuǎn)換成直線運動；通過伺服電機的精確轉(zhuǎn)速控制、精確轉(zhuǎn)數(shù)控制、精確扭矩控制轉(zhuǎn)變成調(diào)平時的精確速度控制、精確位置控制、精確推力控制；而且省去了液壓泵源等龐大設備，簡化了調(diào)平系統(tǒng)，節(jié)約了成本。

(4)調(diào)平過程中，始終動態(tài)監(jiān)測被調(diào)平臺歐拉角，并始終動態(tài)計算電缸伸出桿所需伸縮量，直到各歐拉角達到設定的目標值范圍內(nèi)時，調(diào)平控制才會結(jié)束，所以調(diào)平位置準確。

附圖說明

圖1是本發(fā)明實施例的立體示意圖；

圖2是本發(fā)明實施例的平面布置圖；

圖3a是本發(fā)明實施例的參考坐標系圖；

圖3b是本發(fā)明實施例調(diào)平后的平面示意圖；

圖4是本發(fā)明實施例控制器的控制流程圖；

圖5是本發(fā)明實施例系統(tǒng)工作流程圖。

圖中，1-執(zhí)行機構(gòu)，2-被調(diào)平臺，3-控制器，4-六軸組合傳感器，5-電缸伸出桿，6-著地檢測開關，7-激光測距傳感器，8-測距反光擋板。

具體實施方式

下面參照附圖，對本發(fā)明進一步說明。

如圖1圖2所示，一種基于六軸組合傳感器的四點自動調(diào)平系統(tǒng)，由執(zhí)行機構(gòu)1、控制器3、六軸組合傳感器4、著地檢測開關6以及激光測距傳感器7所組成；

所述的執(zhí)行機構(gòu)1為四個電缸，四個電缸按矩形布置方式分別布置在被調(diào)平臺2下面的四角上；

所述的六軸組合傳感器4采用kionix公司開發(fā)的型號為kxg07或kxg0708的六軸加速度陀螺儀組合傳感器，六軸組合傳感器4布置在被調(diào)平臺2上，并且處于四個電缸對角線的交點上，用于檢測被調(diào)平臺2的當前姿態(tài)和發(fā)出姿態(tài)信息；

所述的著地檢測開關6安裝在電缸伸出桿5端頭上，每個電缸都安裝有著地檢測開關6，用于檢測電缸伸出桿5與地面的接觸狀態(tài)；

所述的激光測距傳感器7安裝在電缸缸體的側(cè)面，電缸伸出桿5端頭上設有向外探出的測距反光擋板8，使激光束與測距反光擋板8對準，用于檢測電缸伸出桿5的伸縮量；

如圖4所示，所述控制器3包括調(diào)平系統(tǒng)的控制中樞cpu、外圍模塊和控制四個伺服電動機運轉(zhuǎn)的驅(qū)動模塊；

所述cpu是具有信息獲取、數(shù)據(jù)處理并發(fā)出控制指令作用的單片機最小系統(tǒng)；

所述外圍模塊包括：用于顯示調(diào)平系統(tǒng)運行狀態(tài)信息的顯示裝置，例如顯示電缸伸出桿當前位置、伸出桿的目標位置、被調(diào)平臺當前姿態(tài)等；用于調(diào)平系統(tǒng)啟動、停止的輸入裝置，例如按鈕或觸摸屏；以及為調(diào)平系統(tǒng)提供動力的電源裝置。

所述單片機最小系統(tǒng)為整個系統(tǒng)的控制中樞，具有信息獲取、數(shù)據(jù)處理并發(fā)出控制指令的作用。所述信息獲取是接收六軸組合傳感器與激光測距傳感器的信息。所述數(shù)據(jù)處理是將檢測到的六軸組合傳感器數(shù)據(jù)按照設定好的算法進行計算與處理，得到調(diào)平所需的伸縮量，同時通過激光測距傳感器動態(tài)檢測四個電缸伸出桿的伸縮量。最后發(fā)出控制指令控制四個電缸動作，實現(xiàn)機身調(diào)平。

所述顯示裝置用于顯示調(diào)平系統(tǒng)運行狀態(tài)等信息，如電缸伸出桿當前位置、伸出桿的目標位置、調(diào)平裝置當前姿態(tài)、各模塊電流值、電壓值等。

所述輸入裝置用于控制調(diào)平系統(tǒng)運行，如啟動、停止等功能。

所述電源裝置用于提供調(diào)平系統(tǒng)所需動力。

所述電動機驅(qū)動模塊用于接收單片機的控制信號，并驅(qū)動四個電缸伺服電動機的運轉(zhuǎn)。

本發(fā)明實施例四點自動調(diào)平系統(tǒng)的工作方法參見圖5，步驟是：

第一步：

工作時，打開控制器，啟動四個電缸同時運行，使四個電缸伸出桿5伸出,當某個電缸伸出桿桿著地時，該端頭上的著地檢測開關6發(fā)出信號，控制器3得到著地信號后使該電缸停止運行，直至四個電缸伸出桿5全部穩(wěn)實著地；全部著地后，六軸組合傳感器4發(fā)出被調(diào)平臺的當前姿態(tài)數(shù)據(jù)信息，控制器3獲取當前姿態(tài)數(shù)據(jù)信息后轉(zhuǎn)換為歐拉角表達式，表達形式為x-y-z歐拉角數(shù)據(jù)θx、θy、θz；

第二步：

控制器根據(jù)歐拉角數(shù)據(jù)計算四個電缸伸出桿伸縮量，計算過程是：

1.以當前四個電缸軸心頂點形成的平面矩形fea’g，建立機身實時坐標系，如圖3a所示；該坐標系以對角線的交點o'作為機身坐標系的原點，以與a’g、ef平行的方向作為x軸方向，與a’e、gf平行的方向為y軸方向，并以與所述x軸、y軸叉乘的方向作為z軸方向；已知平面矩形fea’g的邊長fg＝ea’＝l1，fe＝a’g＝l2；

2.在以四個電缸軸心底部形成的平面矩形abcd為參考平面，建立參考坐標系，如圖3所示；o點為o’沿豎直方向在平面abcd上的投影，o點為參考坐標系的原點，設參考坐標系所在平面歐拉角θx、θy的初始值均為0，當沿x軸、y軸、z軸的任一軸旋轉(zhuǎn)時，采用右手定則判斷歐拉角正負：右手握住轉(zhuǎn)軸，拇指指向轉(zhuǎn)軸的正方向，四指的方向則為歐拉角的正向；

3.設調(diào)平后四個電缸軸心頂點形成的平面為a”b”c”d”，如圖3b所示；平面a”b”c”d”相對于參考平面abcd的高度為h，即aa”＝bb”＝cc”＝dd”＝h，f、e、a’、g點上的四個電缸的伸縮量分別為h1、h2、h3、h4，即fc”＝h1，ed”＝h2，a’a”＝h3，gb”＝h4，h1、h2、h3、h4的正/負值代表伸出桿需要伸長/收縮；

按以下四種情況計算：

(1)當θx>0且θy<＝0時，即f點最高或f、e點同為最高點時，四個電缸伸出桿伸縮量計算如下：

h1＝h-l1sin(θx)-l2sin(θy)

h2＝h-l1sin(θx)

h3＝h

h4＝h-l2sin(θy)

(2)當θx>＝0且θy>0時，即e點最高或e、a’點同為最高點時，四個電缸伸出桿伸縮量計算如下：

h1＝h-l1sin(θx)

h2＝h-l1sin(θx)-l2sin(θy)

h3＝h-l2sin(θy)

h4＝h

(3)當θx<0且θy>＝0時，即a’點最高或a’、g點同為最高點時，四個電缸伸出桿伸縮量計算如下：

h1＝h

h2＝h-l2sin(θy)

h3＝h-l1sin(θx)-l2sin(θy)

h4＝h-l1sin(θx)

(4)當θx<＝0且θy<0時，即g點最高或g、f點同為最高點時，四個電缸伸出桿伸縮量計算如下：

h1＝h-l2sin(θy)

h2＝h

h3＝h-l1sin(θx)

h4＝h-l1sin(θx)-l2sin(θy)

上述式中，h<＝max(h1,h2,h3,h4)，且不小于最高點到參考平面的距離。

第三步：

控制器3把計算出的四個電缸伸出桿5的伸縮量信息，傳送給電動機驅(qū)動模塊，電動機驅(qū)動模塊控制四個電缸的伺服電機正/反轉(zhuǎn)運行，使電缸伸出桿伸/縮，激光測距傳感器7隨時將伸/縮量信號傳輸給控制器3，當某個電缸伸出桿5伸/縮量符合計算值時，控制器3將停止信息傳送給電動機驅(qū)動模塊，使該電缸停止運行；

在調(diào)平過程中，六軸組合傳感器4始終動態(tài)監(jiān)測平臺歐拉角，并始終動態(tài)計算電缸伸出桿5所需伸縮量，直至θx、θy滿足誤差范圍時，調(diào)平操作結(jié)束。