[發(fā)明專利]基于六軸組合傳感器的四點(diǎn)自動(dòng)調(diào)平系統(tǒng)及其工作方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201710542355.7
• 申請(qǐng)日: 2017-07-05
• 公開（公告）號(hào): CN107168186B
• 公開（公告）日: 2019-02-22
• 發(fā)明（設(shè)計(jì)）人: 李東民;陳志強(qiáng);張硯強(qiáng);李佳;馬國(guó)芳
• 申請(qǐng)（專利權(quán)）人: 山東科技大學(xué)
• 主分類號(hào): G05B19/042
• 分類號(hào): G05B19/042;G05D3/12
• 代理公司: 濟(jì)南金迪知識(shí)產(chǎn)權(quán)代理有限公司 37219
• 代理人: 段毅凡

1.一種基于六軸組合傳感器的四點(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)信息的顯示裝置，用于調(diào)平系統(tǒng)啟動(dòng)、停止的輸入裝置，以及為調(diào)平系統(tǒng)提供動(dòng)力的電源裝置。

2.一種如權(quán)利要求1所述的四點(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ì)算過(guò)程是：

第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'＝l₁，F(xiàn)E＝A'G＝l₂；

第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(xiàn)、E、A'、G點(diǎn)上的四個(gè)電缸的伸縮量分別為h₁、h₂、h₃、h₄，即FC”＝h₁，ED”＝h₂，A'A”＝h₃，GB”＝h₄，h₁、h₂、h₃、h₄的正/負(fù)值代表伸出桿需要伸長(zhǎng)/收縮；

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

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

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

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

h₃＝h

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

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

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

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

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

h₄＝h

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

h₁＝h

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

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

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

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

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₄)，且不小于最高點(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)平過(guò)程中，六軸組合傳感器始終動(dòng)態(tài)監(jiān)測(cè)被調(diào)平臺(tái)歐拉角，并始終動(dòng)態(tài)計(jì)算電缸伸出桿所需伸縮量，直至θ_x、θ_y滿足調(diào)平誤差范圍時(shí)，調(diào)平操作結(jié)束。