[發明專利]一種水田拖拉機位姿檢測和偏航角提取方法及裝置

有效

• 申請號: 201910362902.2
• 申請日: 2019-04-30
• 公開（公告）號: CN110057364B
• 公開（公告）日: 2020-08-07
• 發明（設計）人: 楊彥;馬芳;羅文華
• 申請（專利權）人: 鹽城工業職業技術學院
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16
• 代理公司: 北京冠和權律師事務所 11399
• 代理人: 朱健;張國香

1.一種水田拖拉機位姿檢測和偏航角提取方法，其特征在于：具體包括以下步驟：

S1、在水田拖拉機初始運行時，初始攝像模塊(1)會對水田拖拉機的運行軌跡進行攝像，此時第一運行軌跡特征采集模塊(2)會根據攝像對水田拖拉機的運行軌跡特征進行采集，從而標準運行軌跡確定模塊(3)對運行軌跡進行確定，作為水田拖拉機后期運行的標準軌跡，待標準運行軌跡確定模塊(3)確定完畢后會將其發送至中央處理模塊(4)；

S2、在水田拖拉機正常運行時，運行攝像模塊(5)會對水田拖拉機的實時運行軌跡進行攝像，此時第二運行軌跡特征采集模塊(6)會根據攝像對水田拖拉機的運行軌跡特征進行采集，從而實時運行軌跡確定模塊(7)對運行軌跡進行確定，作為水田拖拉機運行的實時軌跡，待實時運行軌跡確定模塊(7)確定完畢后會將其發送至中央處理模塊(4)；

S3、待水田拖拉機的標準軌跡和實時軌跡均確定完畢后，中央處理模塊(4)會將標準軌跡和實時軌跡均發送至偏航角提取系統(8)，此時偏航角提取系統(8)中的軌跡對比模塊(801)會對標準軌跡和實時軌跡進行對比，若標準軌跡和實時軌跡不吻合，此時偏航角提取系統(8)中的偏航角計算模塊(802)會對偏航角度進行計算，從而得出最終偏航角；

S4、待偏航角計算模塊(802)計算出偏航角后，會將偏航角數據分別發送至數據接收模塊(9)和驅動輪控制系統(10)中的驅動輪轉向分析模塊(101)，此時驅動輪轉向分析模塊(101)會分析驅動輪(11)回到標準軌跡所要轉向的角度，從而驅動輪轉向控制模塊(102)會根據分析結果來控制驅動輪(11)轉向的角度；

S5、在水田拖拉機運行時，陀螺儀(12)會對其位姿進行監測，陀螺儀(12)會將實時監測數據通過中央處理模塊(4)發送至數據接收模塊(9)，從而數據接收模塊(9)會將接收到的位姿信息和偏航角數據在顯示屏(13)上進行顯示；

所述陀螺儀(12)的輸出端與中央處理模塊(4)的輸入端連接，所述中央處理模塊(4)的輸出端與數據接收模塊(9)的輸入端連接，并且數據接收模塊(9)的輸出端與顯示屏(13)的輸入端連接；

所述S2-S3中的第二運行軌跡特征采集模塊(6)會根據攝像對水田拖拉機的運行軌跡特征進行采集和偏航角提取系統(8)中的偏航角計算模塊(802)會對偏航角度進行計算得出最終偏航角具體步驟如下所述：

首先，將所述初始攝像模塊(1)對水田拖拉機的運行軌跡進行攝像得到的攝像視頻中的每一幀都作為一張圖像進行保存，形成圖像數據庫，則所述圖像數據庫中有P張圖像，并保存所述圖像所對應的軌跡特征；

然后將所述運行攝像模塊(5)實時拍攝的圖像與所述圖像數據庫中的圖像進行匹配，其具體的匹配方式如下：

將所述實時拍攝的圖像進行像素點的提取，得到像素矩陣A，矩陣A包括L行M列，同時因為像素是包含RGB三個值，則像素矩陣A中的每個元素中間都是包含有3個值的集合，利用公式(1)將像素矩陣A中的每個元素都處理為只有一個值的灰度化后的像素矩陣B

其中，B_it為像素矩陣A第i行t列灰度化后的值，AR_it為像素矩陣A第i行t列像素點的R值，AG_it為像素矩陣A第i行t列像素點的G值，AB_it為像素矩陣A第i行t列像素點的B值，i＝1、2、3……L,t＝1、2、3……M,將所有元素都灰度化形成矩陣B后，將矩陣B利用公式(2)計算出相應的奇異特征值，

|B*B^T-λE|＝0

其中B為灰度化后的矩陣B，B^T為矩陣B的轉置，E為L階的單位矩陣，λ為中間求解出來的值，σ為最終求解的奇異特征值，所求解出來的特征值為L個值，將這L個值按從大到小進行排序，形成向量C；

然后分別獲取所述圖像數據庫中的每張圖像的像素點的值，形成相應的像素矩陣Dj，Dj為L行M列的矩陣，然后利用公式(1)將所述像素矩陣Dj進行灰度化，得到相應的灰度化后的矩陣DDj，對于每個灰度化后的矩陣都利用公式(2)計算出相應的奇異特征值并從大到小排序，形成相應的向量Ej,最后將所有的Ej組成一個矩陣F，其中矩陣F是P行L列的矩陣，每一行代表一張圖像數據庫中的圖像的奇異特征值；

然后利用公式(3)計算向量C與矩陣F中的每一行的奇異特征值距離：

其中，ρ_j為所述實時拍攝的圖像與所述圖像數據庫中第j張圖像的奇異特征值距離，e為自然常數，C為向量C，C_i為向量C的第i個值，F_ji為矩陣F的第j行第i列的值，F_j^T為矩陣F的第j行的轉置，L為矩陣F中每行的值的個數，i＝1、2、3……L,j＝1、2、3……P，提取求解的所有的ρ_j中的最小值ρ_S,S小于等于j，所述最小值所對應的所述圖像數據庫中的第S張圖像則為與實時拍攝的圖像最接近的圖像，所述第S張圖像對應的運行軌跡特征則為實時拍攝的圖像所對應的運行軌跡特征；

將第S-2、S-1、S張圖像所確定的運行軌跡特征傳入實時運行軌跡確定模塊(7),利用軌跡對比模塊(801)對實時運行軌跡確定模塊(7)所確定的實時軌跡與標準運行軌跡確定模塊(3)所確定的標準軌跡進行對比,從而判斷是否偏航，若沒有，則繼續運行；若偏航，利用所述偏航角提取系統(8)中的偏航角計算模塊(802)對偏航角度進行計算，計算步驟為以第S-2張圖像所確定運行軌跡特征為原點，正東方向為X軸，正北方向為Y軸，得到對應的第S-1、S張圖像的運行軌跡特征的坐標，利用公式(4)計算偏航角度：

其中，tt為中間參數，[S-1]_x為第S-1張圖像所確定運行軌跡特征的橫坐標，[S-1]_y為第S-1張圖像所確定運行軌跡特征的縱坐標，[S]_x為第S張圖像所確定運行軌跡特征的橫坐標，[S]_y為第S張圖像所確定運行軌跡特征的縱坐標，arcsec(tt)為tt的反余弦值，θ為求解的偏航角度。