一種自主跟隨車輛航向控制裝置及控制方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及車輛導(dǎo)航與自動控制技術(shù)領(lǐng)域，重點(diǎn)應(yīng)用于農(nóng)業(yè)作業(yè)車輛自主跟隨和 協(xié)同作業(yè)領(lǐng)域。
【背景技術(shù)】
[0002] 農(nóng)業(yè)生產(chǎn)中自主車輛導(dǎo)航方式主要有GPS導(dǎo)航和路標(biāo)導(dǎo)航。GPS導(dǎo)航精度受到視 野中衛(wèi)星的幾何分布狀況、星歷誤差、時(shí)鐘誤差、傳播誤差、多路徑誤差以及接收機(jī)噪聲等 因素影響，同時(shí)其可靠性也受到高大樹木、山坡以及建筑物等因素影響。路標(biāo)導(dǎo)航中主要運(yùn) 用機(jī)器視覺技術(shù)和激光雷達(dá)掃描技術(shù)識別作物行、壟等自然路標(biāo)，但路徑特征的丟失（如 植株的缺失、大面積雜草出現(xiàn)及路標(biāo)環(huán)境雜亂復(fù)雜）和視覺傳感器自身局限性（如動態(tài)范 圍、分辨率、漂移及噪聲）及戶外使用約束條件（如光照）等都可能會導(dǎo)致路標(biāo)導(dǎo)航失效。
[0003] 車輛引導(dǎo)與自主跟隨能彌補(bǔ)自主導(dǎo)航車輛的智能不足，并提高安全性。國內(nèi)外對 車輛跟隨控制和人機(jī)協(xié)同已取得一定的研究成果：日本京都大學(xué)農(nóng)業(yè)機(jī)械化研究生院研究 了人機(jī)協(xié)同自動跟隨車輛系統(tǒng)收割水稻；北海道大學(xué)農(nóng)業(yè)車輛系統(tǒng)工程研究生院研究了農(nóng) 業(yè)作業(yè)主-從機(jī)器人系統(tǒng)；德國芬特公司研究的芬特同步導(dǎo)航通信系統(tǒng)，實(shí)現(xiàn)一個駕駛員 同時(shí)操縱兩輛拖拉機(jī)作業(yè)；上海交通大學(xué)智能車實(shí)驗(yàn)室實(shí)現(xiàn)了針對城市環(huán)境的多智能車跟 隨協(xié)作。但是，實(shí)現(xiàn)主-從跟隨控制大多數(shù)采用機(jī)器視覺、GPS、激光掃描等多傳感器融合技 術(shù)，成本高昂且結(jié)構(gòu)復(fù)雜，在農(nóng)業(yè)領(lǐng)域的相關(guān)研究較少。
[0004] 小型電動車相比拖拉機(jī)體積小,車身矮，在果園環(huán)境中作業(yè)時(shí)具備良好的通過性 能，避免果樹枝葉和果實(shí)遭受損傷，同時(shí)電動車沒有尾氣污染，作業(yè)更加環(huán)保，在實(shí)際中有 較好的應(yīng)用前景。本發(fā)明以自主改裝的兩輛小型電動車構(gòu)建了車輛自主跟隨系統(tǒng)，在參考 車輛速度、轉(zhuǎn)向和跟隨導(dǎo)航控制研究成果的基礎(chǔ)上，設(shè)計(jì)了一套基于紅外傳感器的車輛自 主跟隨控制系統(tǒng)，并設(shè)計(jì)了相應(yīng)的控制算法。在果園環(huán)境下開展了車輛自主跟隨試驗(yàn)，果園 的性能控制實(shí)驗(yàn)結(jié)果表明：本發(fā)明能有效實(shí)現(xiàn)車輛的自主跟隨，性能可靠、成本低廉和結(jié)構(gòu) 簡易。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明的目的在于提供一種自主跟隨車輛航向控制系統(tǒng)的硬件實(shí)現(xiàn)形式和控制 方法的設(shè)計(jì)。
[0006] 該控制系統(tǒng)應(yīng)用紅外傳感器檢測車輛跟隨系統(tǒng)相對航向偏角，應(yīng)用絕對編碼器實(shí) 現(xiàn)前輪偏轉(zhuǎn)角的測量，應(yīng)用步進(jìn)電機(jī)實(shí)現(xiàn)前輪偏轉(zhuǎn)角的控制，建立了車輛自主跟隨的控制 系統(tǒng)模型，設(shè)計(jì)了跟隨控制算法，通過開展的果園環(huán)境控制實(shí)驗(yàn)，驗(yàn)證了本發(fā)明能有效檢測 相對航向偏轉(zhuǎn)角，所設(shè)計(jì)的控制算法能有效實(shí)現(xiàn)對跟隨車輛航向角的控制，實(shí)現(xiàn)車輛的自 主跟隨，體現(xiàn)了本發(fā)明在實(shí)際農(nóng)業(yè)作業(yè)領(lǐng)域的應(yīng)用前景。
[0007] 本發(fā)明的技術(shù)方案是：
[0008] -種自主跟隨車輛航向控制裝置及控制方法，它包括由主控制器、檢測系統(tǒng)和轉(zhuǎn) 向控制系統(tǒng)構(gòu)成的控制系統(tǒng)硬件結(jié)構(gòu)。主控制器采用數(shù)字信號處理器TMS320F28335,檢測 系統(tǒng)實(shí)現(xiàn)跟隨車前輪轉(zhuǎn)角和跟隨車相對引導(dǎo)車的航向偏角的檢測，轉(zhuǎn)向控制系統(tǒng)采用步進(jìn) 電機(jī)配合鏈輪傳動機(jī)構(gòu)帶動方向盤旋轉(zhuǎn)實(shí)現(xiàn)車輛轉(zhuǎn)彎。
[0009] 本發(fā)明的檢測系統(tǒng)包括前輪轉(zhuǎn)角檢測機(jī)構(gòu)和相對航向偏角檢測機(jī)構(gòu)。前輪轉(zhuǎn)角檢 測機(jī)構(gòu)中的絕對型旋轉(zhuǎn)編碼安裝于前輪導(dǎo)向軸上，主控器的AD模塊采集絕對型旋轉(zhuǎn)編碼 器的輸出電壓，通過計(jì)算獲知（B-2)、對跟隨車的航向角速度進(jìn)行積分運(yùn)算計(jì)算出車輛 航向偏轉(zhuǎn)角3。
[0010] 本發(fā)明根據(jù)前輪轉(zhuǎn)向角S和車輛航向偏轉(zhuǎn)角0之間具有的動態(tài)傳遞關(guān)系，設(shè)計(jì) 了航向跟隨閉環(huán)控制系統(tǒng)結(jié)構(gòu)。
[0011] 本發(fā)明根據(jù)航向跟隨閉環(huán)控制系統(tǒng)結(jié)構(gòu)設(shè)計(jì)了航向跟隨ro閉環(huán)控制器，建立了 跟隨車實(shí)際航向偏轉(zhuǎn)角0和目標(biāo)相對航向偏轉(zhuǎn)角Q之間偏差e與前輪偏轉(zhuǎn)角R之間的傳 遞關(guān)系。
[0012]本發(fā)明根據(jù)航向跟隨ro閉環(huán)控制器設(shè)計(jì)了跟隨控制算法流程，跟隨控制算法流 程中撤銷控制作用條件是所有紅外傳感器接收到反射信號，航向偏轉(zhuǎn)角P值與控制目標(biāo) e的差值絕對值小于設(shè)定的閾值a0，前輪轉(zhuǎn)向角量值小于設(shè)定的閾值a8。
[0013] 本發(fā)明搭建的電動車輛自主跟隨控制系統(tǒng)，完成了果園環(huán)境自主跟隨試驗(yàn)，試驗(yàn) 結(jié)果證明本發(fā)明能穩(wěn)定可靠地實(shí)現(xiàn)車輛的自主跟隨。
[0014] 本發(fā)明的有益效果：
[0015] 本發(fā)明中相對航向偏角檢測機(jī)構(gòu)能穩(wěn)定可靠地檢測跟隨車相對于引導(dǎo)車的航向 偏角，并且結(jié)構(gòu)簡易，成本低廉。
[0016] 本發(fā)明提出了一種實(shí)現(xiàn)車輛自主跟隨航向控制的系統(tǒng)硬件實(shí)現(xiàn)形式、控制系統(tǒng)建 模方法和控制算法的實(shí)現(xiàn)形式。
【附圖說明】
[0017] 圖1是本發(fā)明的控制系統(tǒng)硬件結(jié)構(gòu)圖；
[0018] 圖2是本發(fā)明的前輪轉(zhuǎn)角檢測機(jī)構(gòu)圖；
[0019] 圖3是本發(fā)明的相對航向偏角檢測機(jī)構(gòu)圖；
[0020] 圖4是本發(fā)明的車輛自主跟隨控制系統(tǒng)示意圖；
[0021] 圖5是本發(fā)明的轉(zhuǎn)向系統(tǒng)閉環(huán)控制結(jié)構(gòu)圖；
[0022] 圖6是本發(fā)明的前輪轉(zhuǎn)角S和航向角0動態(tài)傳遞關(guān)系圖；
[0023] 圖7是本發(fā)明的航向跟隨閉環(huán)控制系統(tǒng)結(jié)構(gòu)圖；
[0024] 圖8是本發(fā)明的航向跟隨ro閉環(huán)控制器結(jié)構(gòu)圖；
[0025] 圖9是本發(fā)明的跟隨控制算法流程圖；
[0026] 其中：1、主控器；2、檢測系統(tǒng)；3、轉(zhuǎn)向控制系統(tǒng)；4、絕對型旋轉(zhuǎn)編碼器；5、跟隨車 前輪導(dǎo)向軸；6、安裝于跟隨車前端的紅外傳感器安裝板；7、左側(cè)紅外傳感器；8、右側(cè)紅外 傳感器；9、安裝于引導(dǎo)車后端的反射板；10、引導(dǎo)車；11、跟隨車
【具體實(shí)施方式】
[0027] -種自主跟隨車輛航向控制裝置及控制方法，由主控制器1、檢測系統(tǒng)2和轉(zhuǎn)向控 制系統(tǒng)3構(gòu)成控制系統(tǒng)硬件結(jié)構(gòu)（如圖1)，其次包括跟隨控制系統(tǒng)的模型建立、跟隨控制系 統(tǒng)控制器設(shè)計(jì)和跟隨控制算法的設(shè)計(jì)。
[0028] 下面結(jié)合附圖和實(shí)施對本發(fā)明作進(jìn)一步的說明。
[0029] (1)檢測系統(tǒng)具體實(shí)施方案
[0030] 前輪轉(zhuǎn)角檢測機(jī)構(gòu)（如圖2)中的絕對型旋轉(zhuǎn)編碼器4,電壓輸出量程為0_5v，對 應(yīng)測量角度為0-360°，通過主控器1的AD模塊采集絕對型旋轉(zhuǎn)編碼器4的輸出電壓，經(jīng)過 計(jì)算可獲知前輪轉(zhuǎn)向角S。
[0031] 相對航向偏角檢測機(jī)構(gòu)（如圖3)中在跟隨車11前端的紅外傳感器安裝板6上左 右對稱布置反射型紅外傳感器共8個，引導(dǎo)車10后端安裝有反射板9,初始對中后，反射板 9尺寸寬于紅外傳感器安裝尺寸，此時(shí)所有傳感器均能接收到反射信號，當(dāng)引導(dǎo)車10和跟 隨車11產(chǎn)生航向偏角時(shí)，安裝在跟隨車11上左側(cè)紅外傳感器7和右側(cè)紅外傳感器8將根 據(jù)轉(zhuǎn)向的變化和偏角的增大而逐步丟失反射信號，根據(jù)紅外傳感器的安裝位置可以獲知相 應(yīng)的偏轉(zhuǎn)角度，實(shí)現(xiàn)相對航向偏角的檢測。當(dāng)引導(dǎo)車10左轉(zhuǎn)，最右側(cè)紅外傳感器丟失反射 信號時(shí)，如圖3所示，假定引導(dǎo)車10反射板9寬度為W，跟隨車11外端傳感器與反射板9外 端的距離為al，則相對航向偏轉(zhuǎn)角0 i可由以下公式計(jì)算：
[0032] (2)轉(zhuǎn)向控制系統(tǒng)具體實(shí)施方案
[0033] 轉(zhuǎn)向控制系統(tǒng)3采用步進(jìn)電機(jī)配合鏈輪傳動機(jī)構(gòu)帶動方向盤旋轉(zhuǎn)實(shí)現(xiàn)車輛轉(zhuǎn)彎， 步進(jìn)電機(jī)采用定速轉(zhuǎn)動，帶動前輪的轉(zhuǎn)向角速度為，理論上對積分運(yùn)算可計(jì)算出前 輪的轉(zhuǎn)向角S，實(shí)際系統(tǒng)中S可以通過絕對型旋轉(zhuǎn)編碼器4測量所得。在控制系統(tǒng)建模時(shí) 可以將步進(jìn)電機(jī)等效成一個典型繼電器單元，設(shè)計(jì)的轉(zhuǎn)向系統(tǒng)閉環(huán)控制結(jié)構(gòu)（如圖5)中， 前輪目標(biāo)控制偏轉(zhuǎn)角R與S差值的極性決定步進(jìn)電機(jī)的旋轉(zhuǎn)方向，為了減少步進(jìn)電機(jī)在目 標(biāo)值附近的顫振現(xiàn)象，可以人為限定一個最小偏差允許范圍，此時(shí)，步進(jìn)電機(jī)的運(yùn)行方式具 有帶死區(qū)的繼電器特征。
[0034] (3)跟隨控制系統(tǒng)建模具體實(shí)施方案
[0035] 前輪轉(zhuǎn)向角S和車輛航向偏轉(zhuǎn)角3之間具有確定的動態(tài)傳遞關(guān)系（如圖6)，當(dāng) 跟隨車11的縱向速度為V，跟隨車11前后輪軸距為L，采用以下公式可求取跟隨車11的角 速度
|對跟隨車11的角速度進(jìn)行積分運(yùn)算可計(jì)算出車輛航向偏轉(zhuǎn)角3。
[0036] 結(jié)合轉(zhuǎn)向系統(tǒng)閉環(huán)控制結(jié)構(gòu)（如圖5)與前輪轉(zhuǎn)向角S和車輛航向