一種設計全向輪式果園作業車主動安全控制指標的方法
【專利摘要】本發明提供了一種設計全向輪式果園作業車主動安全控制指標的方法，在車輛的防側翻、傾翻主動安全控制指標中，利用全向輪移動的靈活性特點，考慮了橫向和縱向雙方向的穩定性控制措施，可以達到對果園作業車的全向防側翻、傾翻控制。同時考慮到果園作業車測控系統檢測信號中測量誤差的干擾，對控制參數進行濾波處理，抑制了檢測信號中測量誤差干擾的影響，提高果園作業車主動安全控制系統的抗擾性。
【專利說明】一種設計全向輪式果園作業車主動安全控制指標的方法
【技術領域】
[0001] 本發明涉及自動控制【技術領域】，特別涉及一種設計全向輪式果園作業車主動安全 控制指標的方法。
【背景技術】
[0002] 我國是世界第一大水果生產國和消費國。近年來我國水果種植業保持快速發展的 勢頭，2012年全國水果總產量超過24057萬噸，其中園林水果產量達15105萬噸。水果種植 業的快速發展增大了果園機械的市場需求。傳統果園管理的機械化水平較低，需采用人工 搬梯子的方式進行枝條修剪與果實采摘，工作量極其繁重，工作強度很大，具體操作也很不 安全。果園作業車既是實施果園管理的工作平臺，又是進行果務中轉的場所，其配備的升降 平臺可兼顧地面與空中作業需要，滿足各項果園作業環節的實際要求。因此，果園作業車在 我國林果機械化作業的推廣使用能夠有效提高果園作業的工作效率，解決當前勞動力成本 日益上漲的實際問題，已成為世界各國果品產業化領域競相發展的主要方向。由于果園工 作環境較為復雜（如地表不平整、路況起伏多變等），且升降平臺在空中作業時存在質心偏 高、變載荷等特點，極易發生側、傾翻事故。此外，為便于果園作業，駕駛員需要在升降平臺 上操作，難以實時發現果園作業車底盤的不穩定情況，很難對側、傾翻事故作出預判、及時 對車輛姿態進行相應調整，防止事故發生。因此，開展適用于果園升降作業的車輛主動安全 控制方法研究，實現車輛穩定性控制，對提高果園作業車安全性具有重要意義。
【發明內容】
[0003] (一）要解決的技術問題
[0004] 本發明的目的是提供一種主動安全控制指標的設計方法，用于實現對全向輪式果 園作業車的車輛姿態進行控制，以保證車輛的安全行駛，同時提高果園作業車行車控制系 統的抗擾性。
[0005] (二）技術方案：
[0006] -種設計全向輪式果園作業車主動安全控制指標的方法，該方法包括：
[0007] S1，利用安裝在果園作業車質心位置的三軸加速度計及陀螺儀，對車輛x軸方向 的傾角ex及角速度9x'、y軸方向的傾角ey及角速度9 /進行采樣存儲樣本數據；
[0008] S2,根據所存儲的樣本數據中的k個的樣本數據，建立果園作業車X軸比例系數矩 陣心和y軸方向上的比例系數矩陣Ky;
[0009] S3,確定所述K個樣本數據中第k-1采樣時刻x軸方向的系統傾角及角速度的估 計值矩陣y軸方向的系統傾角及角速度的估計值矩陣Xo^y、x軸方向測量方差矩陣 Pzx和y軸方向測量方差矩陣Pzy、x軸的先驗估計協方差矩陣先驗估計協方差矩陣(^_1)!￡、y 軸的先驗估計協方差矩陣Qo^y;
[0010] S4,結合S3中確定的Pzx、Pzy、〇&_1)!￡、Q^y，計算系統在第k時刻x軸估算偏差增 益矩陣kgkx和y軸的估算偏差增益矩陣kgky;
[0011] S5,獲取第k時刻X軸的測量結果矩陣Zkjpy軸的測量結果矩陣Zky，并結合S3和 S4中的計算結果，獲得第k時刻估計值矩陣Xkx&Xky;
[0012] S6,結合S5中的計算結果，獲得果園作業車第k時刻車輛輸出加速度控制指令akx 和 &ky。
[0013] 進一步的，Kx= [kxl，kx2]，kxl為x軸方向上的回復力系數，kx2為x軸方向上的阻 尼系數，Ky為y軸方向上的比例系數矩陣Ky=[kyl，ky2]，1^為y軸方向上的回復力系數及 ky2Sy軸方向上的阻尼系數。
[0014] 進一步的，步驟S6包括：根據如下公式獲取加速度控制指令akdPaky:
【權利要求】
1. 一種設計全向輪式果園作業車主動安全控制指標的方法，其特征在于，包括： Sl，利用安裝在果園作業車質心位置的三軸加速度計及陀螺儀，對車輛X軸方向的傾 角ex&角速度0 x'、y軸方向的傾角ey及角速度0 /進行采樣存儲樣本數據； 52, 根據所存儲的樣本數據中的k個的樣本數據，建立果園作業車X軸比例系數矩陣Kx 和y軸方向上的比例系數矩陣Ky; 53, 確定所述K個樣本數據中第k-l采樣時刻X軸方向的系統傾角及角速度的估計值 矩陣X(lrt)x、y軸方向的系統傾角及角速度的估計值矩陣X (k_1)y、x軸方向的測量方差矩陣Pzx、 y軸方向的測量方差矩陣P zy、X軸的先驗估計協方差矩陣先驗估計協方差矩陣Q(k_1)x、y軸 的先驗估計協方差矩陣Q (lrt)y; 54, 結合S3中確定的Pzx、Pzy、Q(k_1)x、Q (k_1)y，計算系統在第k時刻X軸估算偏差增益矩 陣kgkdP y軸的估算偏差增益矩陣k gky; 55, 獲取第k時刻X軸的測量結果矩陣ZkdP y軸的測量結果矩陣Z ky，并結合S3和S4 中的計算結果，獲得第k時刻估計值矩陣Xkx& X ky; 56, 結合S5中的計算結果，獲得果園作業車第k時刻車輛輸出加速度控制指令&1；!￡和 aky°
2. 如權利要求1所述的方法，其特征在于，Kx= [kxl，kx2]，kxlS X軸方向上的回復力 系數，kx2為X軸方向上的阻尼系數，K y為y軸方向上的比例系數矩陣K y= [k yl，ky2]，kyA y軸方向上的回復力系數及k y2S y軸方向上的阻尼系數。
3. 如權利要求2所述的方法，其特征在于，步驟S6包括：根據如下公式獲取加速度控 制指令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. 如權利要求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. 如權利要求1-3任一項所述的方法，其特征在于，
其中，kgekx為第k時刻X軸傾角的估算偏差增益，k ge，kx為第k時刻X軸角角速度的估 算偏差增益，I = [1，〇] ;kg0ky為第k時刻y軸傾角的估算偏差增益，kge，ky為第k時刻y軸 傾角角速度的估算偏差增益。
6. 如權利要求1-3任一項所述的方法，其特征在于，P zx和P zy的計算方法按照無偏 估計方法確定，首先確定k次采樣值的平均值%之后再計算Pzx和P zy:其中，
7. 如權利要求1或2所述的方法，其特征在于，還包括：S6,結合S4中結果，獲得第k時 刻的X軸和y軸的先驗估計協方差矩陣分別為Q kdP Q ky; 第k時刻的先驗估計協方差矩陣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日 優先權日:2014年11月14日
【發明者】王新, 趙斯琪, 徐松兵, 王力揚, 李為為, 王書茂 申請人:中國農業大學