一種設(shè)計全向輪式果園作業(yè)車主動安全控制指標(biāo)的方法
【專利摘要】本發(fā)明提供了一種設(shè)計全向輪式果園作業(yè)車主動安全控制指標(biāo)的方法，在車輛的防側(cè)翻、傾翻主動安全控制指標(biāo)中，利用全向輪移動的靈活性特點，考慮了橫向和縱向雙方向的穩(wěn)定性控制措施，可以達(dá)到對果園作業(yè)車的全向防側(cè)翻、傾翻控制。同時考慮到果園作業(yè)車測控系統(tǒng)檢測信號中測量誤差的干擾，對控制參數(shù)進(jìn)行濾波處理，抑制了檢測信號中測量誤差干擾的影響，提高果園作業(yè)車主動安全控制系統(tǒng)的抗擾性。
【專利說明】一種設(shè)計全向輪式果園作業(yè)車主動安全控制指標(biāo)的方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及自動控制【技術(shù)領(lǐng)域】，特別涉及一種設(shè)計全向輪式果園作業(yè)車主動安全 控制指標(biāo)的方法。

【背景技術(shù)】
[0002] 我國是世界第一大水果生產(chǎn)國和消費國。近年來我國水果種植業(yè)保持快速發(fā)展的 勢頭，2012年全國水果總產(chǎn)量超過24057萬噸，其中園林水果產(chǎn)量達(dá)15105萬噸。水果種植 業(yè)的快速發(fā)展增大了果園機械的市場需求。傳統(tǒng)果園管理的機械化水平較低，需采用人工 搬梯子的方式進(jìn)行枝條修剪與果實采摘，工作量極其繁重，工作強度很大，具體操作也很不 安全。果園作業(yè)車既是實施果園管理的工作平臺，又是進(jìn)行果務(wù)中轉(zhuǎn)的場所，其配備的升降 平臺可兼顧地面與空中作業(yè)需要，滿足各項果園作業(yè)環(huán)節(jié)的實際要求。因此，果園作業(yè)車在 我國林果機械化作業(yè)的推廣使用能夠有效提高果園作業(yè)的工作效率，解決當(dāng)前勞動力成本 日益上漲的實際問題，已成為世界各國果品產(chǎn)業(yè)化領(lǐng)域競相發(fā)展的主要方向。由于果園工 作環(huán)境較為復(fù)雜（如地表不平整、路況起伏多變等），且升降平臺在空中作業(yè)時存在質(zhì)心偏 高、變載荷等特點，極易發(fā)生側(cè)、傾翻事故。此外，為便于果園作業(yè)，駕駛員需要在升降平臺 上操作，難以實時發(fā)現(xiàn)果園作業(yè)車底盤的不穩(wěn)定情況，很難對側(cè)、傾翻事故作出預(yù)判、及時 對車輛姿態(tài)進(jìn)行相應(yīng)調(diào)整，防止事故發(fā)生。因此，開展適用于果園升降作業(yè)的車輛主動安全 控制方法研究，實現(xiàn)車輛穩(wěn)定性控制，對提高果園作業(yè)車安全性具有重要意義。


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

[0003] (一）要解決的技術(shù)問題
[0004] 本發(fā)明的目的是提供一種主動安全控制指標(biāo)的設(shè)計方法，用于實現(xiàn)對全向輪式果 園作業(yè)車的車輛姿態(tài)進(jìn)行控制，以保證車輛的安全行駛，同時提高果園作業(yè)車行車控制系 統(tǒng)的抗擾性。
[0005] (二）技術(shù)方案：
[0006] -種設(shè)計全向輪式果園作業(yè)車主動安全控制指標(biāo)的方法，該方法包括：
[0007] S1，利用安裝在果園作業(yè)車質(zhì)心位置的三軸加速度計及陀螺儀，對車輛x軸方向 的傾角ex及角速度9x'、y軸方向的傾角ey及角速度9 /進(jìn)行采樣存儲樣本數(shù)據(jù)；
[0008] S2,根據(jù)所存儲的樣本數(shù)據(jù)中的k個的樣本數(shù)據(jù)，建立果園作業(yè)車X軸比例系數(shù)矩 陣心和y軸方向上的比例系數(shù)矩陣Ky;
[0009] S3,確定所述K個樣本數(shù)據(jù)中第k-1采樣時刻x軸方向的系統(tǒng)傾角及角速度的估 計值矩陣y軸方向的系統(tǒng)傾角及角速度的估計值矩陣Xo^y、x軸方向測量方差矩陣 Pzx和y軸方向測量方差矩陣Pzy、x軸的先驗估計協(xié)方差矩陣先驗估計協(xié)方差矩陣(^_1)!￡、y 軸的先驗估計協(xié)方差矩陣Qo^y;
[0010] S4,結(jié)合S3中確定的Pzx、Pzy、〇&_1)!￡、Q^y，計算系統(tǒng)在第k時刻x軸估算偏差增 益矩陣kgkx和y軸的估算偏差增益矩陣kgky;
[0011] S5,獲取第k時刻X軸的測量結(jié)果矩陣Zkjpy軸的測量結(jié)果矩陣Zky，并結(jié)合S3和 S4中的計算結(jié)果，獲得第k時刻估計值矩陣Xkx&Xky;
[0012] S6,結(jié)合S5中的計算結(jié)果，獲得果園作業(yè)車第k時刻車輛輸出加速度控制指令akx 和 &ky。
[0013] 進(jìn)一步的，Kx= [kxl，kx2]，kxl為x軸方向上的回復(fù)力系數(shù)，kx2為x軸方向上的阻 尼系數(shù)，Ky為y軸方向上的比例系數(shù)矩陣Ky=[kyl，ky2]，1^為y軸方向上的回復(fù)力系數(shù)及 ky2Sy軸方向上的阻尼系數(shù)。
[0014] 進(jìn)一步的，步驟S6包括：根據(jù)如下公式獲取加速度控制指令akdPaky:

【權(quán)利要求】
1. 一種設(shè)計全向輪式果園作業(yè)車主動安全控制指標(biāo)的方法，其特征在于，包括： Sl，利用安裝在果園作業(yè)車質(zhì)心位置的三軸加速度計及陀螺儀，對車輛X軸方向的傾 角ex&角速度0 x'、y軸方向的傾角ey及角速度0 /進(jìn)行采樣存儲樣本數(shù)據(jù)； 52, 根據(jù)所存儲的樣本數(shù)據(jù)中的k個的樣本數(shù)據(jù)，建立果園作業(yè)車X軸比例系數(shù)矩陣Kx 和y軸方向上的比例系數(shù)矩陣Ky; 53, 確定所述K個樣本數(shù)據(jù)中第k-l采樣時刻X軸方向的系統(tǒng)傾角及角速度的估計值 矩陣X(lrt)x、y軸方向的系統(tǒng)傾角及角速度的估計值矩陣X (k_1)y、x軸方向的測量方差矩陣Pzx、 y軸方向的測量方差矩陣P zy、X軸的先驗估計協(xié)方差矩陣先驗估計協(xié)方差矩陣Q(k_1)x、y軸 的先驗估計協(xié)方差矩陣Q (lrt)y; 54, 結(jié)合S3中確定的Pzx、Pzy、Q(k_1)x、Q (k_1)y，計算系統(tǒng)在第k時刻X軸估算偏差增益矩 陣kgkdP y軸的估算偏差增益矩陣k gky; 55, 獲取第k時刻X軸的測量結(jié)果矩陣ZkdP y軸的測量結(jié)果矩陣Z ky，并結(jié)合S3和S4 中的計算結(jié)果，獲得第k時刻估計值矩陣Xkx& X ky; 56, 結(jié)合S5中的計算結(jié)果，獲得果園作業(yè)車第k時刻車輛輸出加速度控制指令&1；!￡和 aky°
2. 如權(quán)利要求1所述的方法，其特征在于，Kx= [kxl，kx2]，kxlS X軸方向上的回復(fù)力 系數(shù)，kx2為X軸方向上的阻尼系數(shù)，K y為y軸方向上的比例系數(shù)矩陣K y= [k yl，ky2]，kyA y軸方向上的回復(fù)力系數(shù)及k y2S y軸方向上的阻尼系數(shù)。
3. 如權(quán)利要求2所述的方法，其特征在于，步驟S6包括：根據(jù)如下公式獲取加速度控 制指令akjP aky: ?=K% aky~X ^At. 9 其中，Xkx為第k時刻在X軸上傾角及角速度的估計值矩陣，Xkx= [e kx，e'kx] =Xa^ x+kgkXXIX(Zkx-X (k_1)x); 其中，Xky為第k時刻在y軸上傾角及角速度的估計值矩陣Xky= [e ky，e'ky] =Xa^ y+kgkyX I X (Zky-X(k_1)y)。
4. 如權(quán)利要求1-3任一項所述的方法，其特征在于，Zkx= [Zk0x，Zkr J，Zkex為第k時 刻測得的X軸方向的傾角，Zkrx為第k時刻測得的X軸方向的角速度；Z ky= [Zk0y，Zke，y]， Zkey為第k時刻測得的y軸方向的傾角，Z ke，y為第k時刻測得的y軸方向的角速度。
5. 如權(quán)利要求1-3任一項所述的方法，其特征在于，
其中，kgekx為第k時刻X軸傾角的估算偏差增益，k ge，kx為第k時刻X軸角角速度的估 算偏差增益，I = [1，〇] ;kg0ky為第k時刻y軸傾角的估算偏差增益，kge，ky為第k時刻y軸 傾角角速度的估算偏差增益。
6. 如權(quán)利要求1-3任一項所述的方法，其特征在于，P zx和P zy的計算方法按照無偏 估計方法確定，首先確定k次采樣值的平均值%之后再計算Pzx和P zy:其中，
7. 如權(quán)利要求1或2所述的方法，其特征在于，還包括：S6,結(jié)合S4中結(jié)果，獲得第k時 刻的X軸和y軸的先驗估計協(xié)方差矩陣分別為Q kdP Q ky; 第k時刻的先驗估計協(xié)方差矩陣QkdP Q ky: Qkx= Q (k-i)xX (H-I1XKgkx) Qky= Q (k-l)y X (H-I1XKgky)
【文檔編號】B60W30/04GK104494596SQ201410647985
【公開日】2015年4月8日 申請日期:2014年11月14日 優(yōu)先權(quán)日:2014年11月14日
【發(fā)明者】王新, 趙斯琪, 徐松兵, 王力揚, 李為為, 王書茂 申請人:中國農(nóng)業(yè)大學(xué)