[發明專利]一種青飼機自動拋送系統機械臂運動控制方法

有效

• 申請號: 201911253619.2
• 申請日: 2019-12-09
• 公開（公告）號: CN111090258B
• 公開（公告）日: 2021-05-25
• 發明（設計）人: 何創新;班訓康;苗中華;李楠;劉成良
• 申請（專利權）人: 上海大學
• 主分類號: G05B19/19
• 分類號: G05B19/19
• 代理公司: 深圳市易美諾知識產權代理事務所(普通合伙) 44520
• 代理人: 朱為甫

一種青飼機自動拋送系統機械臂運動控制方法，包括有如下的步驟：利用RGB?D相機識別車斗，并定位車斗在機械臂坐標系下的位置；根據車斗定位的范圍，在車斗范圍內設置物料的多個監測點，并獲取各個監測點的滿度值；按一定順序依次遍歷車斗內監測點的物料的滿度值，將滿度值與預設滿度值進行比較，找出滿度值小于預設滿度值的首個未滿監測點；判斷首個未滿監測點所位于機械臂的方向，在該方向下的機械臂應轉計數累計加一次，同時相反方向的機械臂應轉計數清零，其中方向包括左方和右方；判斷機械臂應轉計數累計是否達到三次及以上，是則機械臂向該方向轉動。控制械臂運動對車斗進行自動填充物料。

技術領域

本發明涉及機械臂控制與農業自動化技術領域，尤其是涉及一種青飼機自動拋送系統機械臂運動控制方法。

背景技術

青飼機是一種常用農業收獲機械，主要用來收獲青綠牧草、燕麥、甜菜莖葉、玉米等青飼作物。青飼機大體由前方割臺、機身喂入裝置及切碎拋送裝置組成。前方割臺為一個裝有多把甩刀旋轉切碎器用于高速收割作物，機身收集作物并將其切碎，然后由拋送裝置拋出，青飼機拋出的物料由后方或側方同向行駛的運輸車承接。

具有自動拋送系統的青飼機，機身收集作物并將其切碎，然后由固定在機械臂上的RGB-D相機識別和定位車斗位姿，然后基于RGB-D相機和機械臂坐標變換矩陣，得出車斗在機械臂坐標系下的位姿，自動調整機械臂實現物料的精準拋投。

如公開號為CN109920006A的中國發明專利公開了一種青飼機自動拋送系統位姿變換矩陣的標定方法，得到的位姿變換矩陣就是用于將車斗在相機坐標系下RGB-D相機獲得到的坐標數據轉換為機械臂坐標系統下的坐標數據。

如公開號為CN108550141A的中國發明專利公開了一種基于深度視覺信息的運動車斗自動識別與定位方法，利用RGB-D對運動車斗自動識別與定位。

青飼機自動拋送系統利用深度相機實現對運輸車車斗進行自動識別與定位以及對車斗內物料滿度進行檢測，基于車斗的識別與定位和物料滿度檢測，對機械臂進行運動控制實現物料精準的拋灑在車斗未滿區域。

目前物料的噴灑主要通過操作員人眼觀察車斗內物料的情況，然后通過手柄控制機械臂進行物料的噴灑。通過操作人員手動控制的方式，需要增加一名專業的操作人員增加了收割成本；由于運輸車和收獲車相對行駛速度會有所不同，導致操作人員要時刻觀察車斗狀況頻繁操作手柄，造成人力勞頓和物料的浪費。

發明內容

本發明的目的是針對現有技術存在的不足，提出了一種青飼機自動拋送系統機械臂運動控制方法，控制械臂運動對車斗進行自動填充物料。

為達到上述目的，本發明采用的技術方案如下：

建立機械臂坐標系，機械臂坐標系的Z軸與水平面垂直向上，X軸與Y軸呈水平，Y軸隨著機械臂運動，X軸為滿足笛卡爾坐標系右手定則確得。

機械臂運動有三個自由度，第一自由度為機械臂坐標系下機械臂繞Z軸的轉動自由度，第二自由度為機械臂坐標系下機械臂繞X軸的轉動自由度，第三自由度為機械臂坐標系下機械臂拋送口處擋板在YZ平面上擺動自由度。

第一自由度可以控制機械臂進行水平面的左右轉動，第二自由度可以控制機械臂上下的升降，第三自由度可以控制機械臂的擋板進行上下擺動控制物料的噴射遠近。

對于車斗的識別與定位可以參考公開號為CN108550141A的中國發明專利公開。

安裝RGB-D相機，并在安裝位置進行相機坐標系與機械臂坐標系的準確標定，其標定方法可以參考公開號為CN109920006A的中國發明專利，RGB-D相機實時獲取的點云數據轉換為機械臂坐標系下的點云數據，這些點可能是車斗的點，也可能是車斗中的物料點，還可能是拋送在空中的物料柱的點。