[發(fā)明專(zhuān)利]一種溫室移動(dòng)作業(yè)平臺(tái)協(xié)同跟隨作業(yè)導(dǎo)航方法及裝置

有效

• 申請(qǐng)?zhí)枺?/td>: 201710133282.6
• 申請(qǐng)日: 2017-03-07
• 公開(kāi)（公告）號(hào): CN106708061B
• 公開(kāi)（公告）日: 2019-07-26
• 發(fā)明（設(shè)計(jì)）人: 王新忠;管澤峰
• 申請(qǐng)（專(zhuān)利權(quán)）人: 江蘇大學(xué)
• 主分類(lèi)號(hào): G05D1/02
• 分類(lèi)號(hào): G05D1/02
• 代理公司: 江蘇縱聯(lián)律師事務(wù)所 32253
• 代理人: 蔡棟

1.一種溫室移動(dòng)作業(yè)平臺(tái)協(xié)同跟隨作業(yè)導(dǎo)航方法，其特征在于：利用一種溫室移動(dòng)作業(yè)平臺(tái)協(xié)同跟隨作業(yè)導(dǎo)航裝置，包括：運(yùn)載平臺(tái)(1)、履帶行走裝置及探測(cè)控制裝置；所述運(yùn)載平臺(tái)(1)用于裝載物料果實(shí)和作業(yè)工具；所述履帶行走裝置用于驅(qū)動(dòng)運(yùn)載平臺(tái)(1)的行駛；所述探測(cè)控制裝置用于檢測(cè)平臺(tái)的行駛路徑偏差以及移動(dòng)平臺(tái)與工作人員之間的距離，并控制移動(dòng)平臺(tái)沿著導(dǎo)引路徑跟隨作業(yè)人員行駛；

所述履帶行走裝置由左伺服電機(jī)驅(qū)動(dòng)器(11)、右伺服電機(jī)驅(qū)動(dòng)器(12)、左伺服電機(jī)(10)、右伺服電機(jī)(9)、蓄電池組(18)、左齒輪減速器(14)、右齒輪減速器(13)、左履帶(8)、右履帶(7)、左驅(qū)動(dòng)輪(16)、右驅(qū)動(dòng)輪(15)、左誘導(dǎo)輪(4)、右誘導(dǎo)輪(3)、左負(fù)重輪(6)、右負(fù)重輪(5)組成；所述左伺服電機(jī)(10)和右伺服電機(jī)(9)的輸出軸分別經(jīng)左齒輪減速器(14)和右齒輪減速器(13)與左驅(qū)動(dòng)輪(16)和右驅(qū)動(dòng)輪(15)相互連接；所述左驅(qū)動(dòng)輪(16)和右驅(qū)動(dòng)輪(15)與左誘導(dǎo)輪(4)和右誘導(dǎo)輪(3)大小相等并通過(guò)左履帶(8)和右履帶(7)分別與位于運(yùn)載平臺(tái)(1)前面左側(cè)的左誘導(dǎo)輪(4)和前面右側(cè)的右誘導(dǎo)輪(3)連接；

履帶行走裝置的左負(fù)重輪(6)安裝在左驅(qū)動(dòng)輪(16)和左誘導(dǎo)輪(4)之間，右負(fù)重輪(5)安裝在右驅(qū)動(dòng)輪(15)和右誘導(dǎo)輪(3)之間，并分別與移動(dòng)平臺(tái)左右兩側(cè)的固定軸相連；

所述探測(cè)控制裝置包括測(cè)距傳感器模塊、磁導(dǎo)航傳感器模塊(20)、溫度檢測(cè)模塊、速度檢測(cè)模塊、無(wú)線(xiàn)發(fā)射模塊和主控制器；所述履帶行走裝置的前測(cè)距傳感器模塊(21)和后測(cè)距傳感器模塊(22)分別安裝在移動(dòng)平臺(tái)的車(chē)體縱向中心面上；測(cè)距傳感器模塊包括超聲波測(cè)距傳感器和紅外傳感器，每個(gè)測(cè)距傳感器模塊的超聲波測(cè)距傳感器安裝在紅外傳感器的正上方并相距1cm；所述運(yùn)載平臺(tái)(1)的底部正中間安裝磁導(dǎo)航傳感器模塊(20)，磁導(dǎo)航傳感器模塊(20)距離地面4cm且關(guān)于移動(dòng)平臺(tái)的車(chē)體縱向中心面對(duì)稱(chēng)；所述速度檢測(cè)模塊包括左編碼器和右編碼器，左編碼器和右編碼器分別與左伺服電機(jī)(10)和右伺服電機(jī)(9)連接；所述無(wú)線(xiàn)發(fā)射模塊包括紅外遙控器和紅外接收頭，紅外接收頭經(jīng)導(dǎo)線(xiàn)與主控制器連接；所述溫度檢測(cè)模塊通過(guò)導(dǎo)線(xiàn)與主控制器連接；所述主控制器安裝在所述運(yùn)載平臺(tái)(1)內(nèi)部，通過(guò)導(dǎo)線(xiàn)與左伺服電機(jī)驅(qū)動(dòng)器(11)和右伺服電機(jī)驅(qū)動(dòng)器(12)相連，左伺服電機(jī)驅(qū)動(dòng)器(11)和右伺服電機(jī)驅(qū)動(dòng)器(12)均引出導(dǎo)線(xiàn)分別連接左伺服電機(jī)(10)、左編碼器和右伺服電機(jī)(9)、右編碼器；所述主控制器經(jīng)導(dǎo)線(xiàn)分別連接前測(cè)距傳感器模塊(21)和后測(cè)距傳感器模塊(22)的超聲波測(cè)距傳感器，再通過(guò)內(nèi)置的模數(shù)轉(zhuǎn)換模塊經(jīng)導(dǎo)線(xiàn)分別連接前后測(cè)距傳感器模塊(21)和后測(cè)距傳感器模塊(22)的紅外傳感器；所述左編碼器和右編碼器的外殼分別與左伺服電機(jī)(10)和右伺服電機(jī)(9)的外殼固定，左編碼器轉(zhuǎn)子與左伺服電機(jī)(10)的轉(zhuǎn)子連接，右編碼器轉(zhuǎn)子與右伺服電機(jī)(9)的轉(zhuǎn)子連接；

所述的方法具體包括以下步驟：

步驟一，導(dǎo)航裝置接收到啟動(dòng)信號(hào)后，主控制器進(jìn)入初始化程序，先判斷程序是否有異常，若程序正常則選擇工作模式，在手動(dòng)模式下由紅外遙控器遙控移動(dòng)平臺(tái)行駛，在跟隨作業(yè)模式下則啟動(dòng)測(cè)距子程序，通過(guò)測(cè)距傳感器模塊的超聲波傳感器和紅外傳感器檢測(cè)行進(jìn)方向上障礙物的距離；

步驟二，為了補(bǔ)償溫室溫度對(duì)聲波在空氣中傳播速度的影響，通過(guò)溫度檢測(cè)模塊實(shí)時(shí)測(cè)量當(dāng)前溫室的溫度值T，得超聲波的傳播速度V為：

步驟三，測(cè)量時(shí)每組測(cè)距傳感器模塊的超聲波傳感器和紅外傳感器分別采集移動(dòng)平臺(tái)與作業(yè)人員之間的距離三次，通過(guò)自適應(yīng)加權(quán)融合算法對(duì)測(cè)距傳感器模塊采集的數(shù)據(jù)進(jìn)行處理，通過(guò)權(quán)值的計(jì)算和分配得到計(jì)算后的測(cè)量距離；

步驟四，當(dāng)測(cè)距傳感器模塊的實(shí)際測(cè)量值小于設(shè)置的安全跟隨距離時(shí)，主控制器控制左伺服電機(jī)和右伺服電機(jī)停止轉(zhuǎn)動(dòng)；當(dāng)移動(dòng)平臺(tái)的測(cè)距傳感器模塊的實(shí)際測(cè)量距離大于設(shè)置的安全跟隨距離時(shí)，主控制器控制左伺服電機(jī)和右伺服電機(jī)運(yùn)行；

步驟五，磁導(dǎo)航傳感器每個(gè)探測(cè)點(diǎn)之間距離1cm，移動(dòng)平臺(tái)在行駛過(guò)程中通過(guò)16位數(shù)字式磁導(dǎo)航傳感器檢測(cè)鋪設(shè)在行駛路徑上的導(dǎo)引磁條2產(chǎn)生的磁性，主控制器通過(guò)中斷方式每150ms采集一次磁導(dǎo)航傳感器的偏移量，通過(guò)磁導(dǎo)航傳感器左右兩側(cè)探測(cè)點(diǎn)輸出的不同位置判斷移動(dòng)平臺(tái)偏離行駛路徑的程度，主控制器根據(jù)移動(dòng)平臺(tái)的偏移量以及偏移量的變化率控制對(duì)左右伺服電機(jī)和右伺服電機(jī)進(jìn)行PWM調(diào)速，當(dāng)導(dǎo)引磁條在移動(dòng)平臺(tái)的正中間時(shí)，磁導(dǎo)航傳感器中間的四個(gè)輸出點(diǎn)導(dǎo)通，當(dāng)磁導(dǎo)航傳感器的輸出點(diǎn)左移，移動(dòng)平臺(tái)重心偏向右壟，主控制器分別給左伺服電機(jī)和右伺服電機(jī)減小和增加相同的占空比，從而實(shí)現(xiàn)作業(yè)平臺(tái)的左轉(zhuǎn)調(diào)速；當(dāng)磁導(dǎo)航傳感器的輸出點(diǎn)右移，移動(dòng)平臺(tái)重心偏向左壟，主控制器分別給左伺服電機(jī)和右伺服電機(jī)增加和減小相同的占空比，從而實(shí)現(xiàn)作業(yè)平臺(tái)的右轉(zhuǎn)調(diào)速，通過(guò)不斷調(diào)整行駛位姿，實(shí)現(xiàn)移動(dòng)平臺(tái)沿著導(dǎo)引路徑行走。

2.根據(jù)權(quán)利要求1所述的一種溫室移動(dòng)作業(yè)平臺(tái)協(xié)同跟隨作業(yè)導(dǎo)航方法，其特征在于：步驟三中，所述的自適應(yīng)加權(quán)融合算法，具體計(jì)算過(guò)程如下：

過(guò)程一，為超聲波傳感器進(jìn)行3次測(cè)量的測(cè)距均值，為超聲波測(cè)距傳感器的測(cè)距方差，計(jì)算公式如下

過(guò)程二，為紅外傳感器進(jìn)行3次測(cè)量的測(cè)距均值，為紅外傳感器的測(cè)距方差，計(jì)算公式如下

過(guò)程三，超聲波傳感器和纖外傳感器的加權(quán)因子分別為W_p、W_q，計(jì)算公式如下

過(guò)程四，通過(guò)自適應(yīng)加權(quán)融合后的測(cè)距均值計(jì)算公式如下