基于六軸組合傳感器的四點(diǎn)自動(dòng)調(diào)平系統(tǒng)及其工作方法與流程

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

技術(shù)背景

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

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

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

(1)采用傾角傳感器檢測(cè)平臺(tái)傾角。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

第一步：

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

第二步：

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

第2.1步：以當(dāng)前四個(gè)電缸軸心頂點(diǎn)形成的平面矩形fea’g，建立機(jī)身實(shí)時(shí)坐標(biāo)系；該坐標(biāo)系以對(duì)角線的交點(diǎn)o'作為機(jī)身坐標(biāo)系的原點(diǎn)，以與a’g、ef平行的方向作為x軸方向，與a’e、gf平行的方向?yàn)閥軸方向，并以與所述x軸、y軸叉乘的方向作為z軸方向；已知平面矩形fea’g的邊長(zhǎng)fg＝ea’＝l1，fe＝a’g＝l2；

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

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

按以下四種情況計(jì)算：

(1)當(dāng)θx>0且θy<＝0時(shí)，即f點(diǎn)最高或f、e點(diǎn)同為最高點(diǎn)時(shí)，四個(gè)電缸伸出桿伸縮量計(jì)算如下：

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

h2＝h-l1sin(θx)

h3＝h

h4＝h-l2sin(θy)

(2)當(dāng)θx>＝0且θy>0時(shí)，即e點(diǎn)最高或e、a’點(diǎn)同為最高點(diǎn)時(shí)，四個(gè)電缸伸出桿伸縮量計(jì)算如下：

h1＝h-l1sin(θx)

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

h3＝h-l2sin(θy)

h4＝h

(3)當(dāng)θx<0且θy>＝0時(shí)，即a’點(diǎn)最高或a’、g點(diǎn)同為最高點(diǎn)時(shí)，四個(gè)電缸伸出桿伸縮量計(jì)算如下：

h1＝h

h2＝h-l2sin(θy)

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

h4＝h-l1sin(θx)

(4)當(dāng)θx<＝0且θy<0時(shí)，即g點(diǎn)最高或g、f點(diǎn)同為最高點(diǎn)時(shí)，四個(gè)電缸伸出桿伸縮量計(jì)算如下：

h1＝h-l2sin(θy)

h2＝h

h3＝h-l1sin(θx)

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

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

第三步：

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

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

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

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

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

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

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

附圖說明

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

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

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

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

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

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

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

具體實(shí)施方式

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

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

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

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

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

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

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

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

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

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

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

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

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

所述電動(dòng)機(jī)驅(qū)動(dòng)模塊用于接收單片機(jī)的控制信號(hào)，并驅(qū)動(dòng)四個(gè)電缸伺服電動(dòng)機(jī)的運(yùn)轉(zhuǎn)。

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

第一步：

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

第二步：

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

1.以當(dāng)前四個(gè)電缸軸心頂點(diǎn)形成的平面矩形fea’g，建立機(jī)身實(shí)時(shí)坐標(biāo)系，如圖3a所示；該坐標(biāo)系以對(duì)角線的交點(diǎn)o'作為機(jī)身坐標(biāo)系的原點(diǎn)，以與a’g、ef平行的方向作為x軸方向，與a’e、gf平行的方向?yàn)閥軸方向，并以與所述x軸、y軸叉乘的方向作為z軸方向；已知平面矩形fea’g的邊長(zhǎng)fg＝ea’＝l1，fe＝a’g＝l2；

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

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

按以下四種情況計(jì)算：

(1)當(dāng)θx>0且θy<＝0時(shí)，即f點(diǎn)最高或f、e點(diǎn)同為最高點(diǎn)時(shí)，四個(gè)電缸伸出桿伸縮量計(jì)算如下：

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

h2＝h-l1sin(θx)

h3＝h

h4＝h-l2sin(θy)

(2)當(dāng)θx>＝0且θy>0時(shí)，即e點(diǎn)最高或e、a’點(diǎn)同為最高點(diǎn)時(shí)，四個(gè)電缸伸出桿伸縮量計(jì)算如下：

h1＝h-l1sin(θx)

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

h3＝h-l2sin(θy)

h4＝h

(3)當(dāng)θx<0且θy>＝0時(shí)，即a’點(diǎn)最高或a’、g點(diǎn)同為最高點(diǎn)時(shí)，四個(gè)電缸伸出桿伸縮量計(jì)算如下：

h1＝h

h2＝h-l2sin(θy)

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

h4＝h-l1sin(θx)

(4)當(dāng)θx<＝0且θy<0時(shí)，即g點(diǎn)最高或g、f點(diǎn)同為最高點(diǎn)時(shí)，四個(gè)電缸伸出桿伸縮量計(jì)算如下：

h1＝h-l2sin(θy)

h2＝h

h3＝h-l1sin(θx)

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

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

第三步：

控制器3把計(jì)算出的四個(gè)電缸伸出桿5的伸縮量信息，傳送給電動(dòng)機(jī)驅(qū)動(dòng)模塊，電動(dòng)機(jī)驅(qū)動(dòng)模塊控制四個(gè)電缸的伺服電機(jī)正/反轉(zhuǎn)運(yùn)行，使電缸伸出桿伸/縮，激光測(cè)距傳感器7隨時(shí)將伸/縮量信號(hào)傳輸給控制器3，當(dāng)某個(gè)電缸伸出桿5伸/縮量符合計(jì)算值時(shí)，控制器3將停止信息傳送給電動(dòng)機(jī)驅(qū)動(dòng)模塊，使該電缸停止運(yùn)行；

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