CN104494596B - 一种设计全向轮式果园作业车主动安全控制指标的方法 - Google Patents

Abstract

本发明提供了一种设计全向轮式果园作业车主动安全控制指标的方法，在车辆的防侧翻、倾翻主动安全控制指标中，利用全向轮移动的灵活性特点，考虑了横向和纵向双方向的稳定性控制措施，可以达到对果园作业车的全向防侧翻、倾翻控制。同时考虑到果园作业车测控系统检测信号中测量误差的干扰，对控制参数进行滤波处理，抑制了检测信号中测量误差干扰的影响，提高果园作业车主动安全控制系统的抗扰性。

Description

一种设计全向轮式果园作业车主动安全控制指标的方法

技术领域

本发明涉及自动控制技术领域，特别涉及一种设计全向轮式果园作业车主动安全控制指标的方法。

背景技术

我国是世界第一大水果生产国和消费国。近年来我国水果种植业保持快速发展的势头，2012年全国水果总产量超过24057万吨，其中园林水果产量达15105万吨。水果种植业的快速发展增大了果园机械的市场需求。传统果园管理的机械化水平较低，需采用人工搬梯子的方式进行枝条修剪与果实采摘，工作量极其繁重，工作强度很大，具体操作也很不安全。果园作业车既是实施果园管理的工作平台，又是进行果务中转的场所，其配备的升降平台可兼顾地面与空中作业需要，满足各项果园作业环节的实际要求。因此，果园作业车在我国林果机械化作业的推广使用能够有效提高果园作业的工作效率，解决当前劳动力成本日益上涨的实际问题，已成为世界各国果品产业化领域竞相发展的主要方向。由于果园工作环境较为复杂(如地表不平整、路况起伏多变等)，且升降平台在空中作业时存在质心偏高、变载荷等特点，极易发生侧、倾翻事故。此外，为便于果园作业，驾驶员需要在升降平台上操作，难以实时发现果园作业车底盘的不稳定情况，很难对侧、倾翻事故作出预判、及时对车辆姿态进行相应调整，防止事故发生。因此，开展适用于果园升降作业的车辆主动安全控制方法研究，实现车辆稳定性控制，对提高果园作业车安全性具有重要意义。

发明内容

(一)要解决的技术问题

本发明的目的是提供一种主动安全控制指标的设计方法，用于实现对全向轮式果园作业车的车辆姿态进行控制，以保证车辆的安全行驶，同时提高果园作业车行车控制系统的抗扰性。

(二)技术方案：

一种设计全向轮式果园作业车主动安全控制指标的方法，该方法包括：

S1，利用安装在果园作业车质心位置的三轴加速度计及陀螺仪，对车辆x轴方向的倾角θ_x及角速度θ_x'、y轴方向的倾角θ_y及角速度θ_y'进行采样存储样本数据；

S2，根据所存储的样本数据中的k个的样本数据，建立果园作业车x轴比例系数矩阵K_x和y轴方向上的比例系数矩阵K_y；

S3，确定所述k个样本数据中第k-1采样时刻x轴方向的系统倾角及角速度的估计值矩阵X_(k-1)x、y轴方向的系统倾角及角速度的估计值矩阵X_(k-1)y、x轴方向测量方差矩阵P_zx和y轴方向测量方差矩阵P_zy、x轴的先验估计协方差矩阵先验估计协方差矩阵Q_(k-1)x、y轴的先验估计协方差矩阵Q_(k-1)y；

S4，结合S3中确定的P_zx、P_zy、Q_(k-1)x、Q_(k-1)y，计算系统在第k时刻x轴估算偏差增益矩阵k_gkx和y轴的估算偏差增益矩阵k_gky；

S5，获取第k时刻x轴的测量结果矩阵Z_kx和y轴的测量结果矩阵Z_ky，并结合S3和S4中的计算结果，获得第k时刻估计值矩阵X_kx及X_ky；

S6，结合S5中的计算结果，获得果园作业车第k时刻车辆输出加速度控制指令a_kx和a_ky。

进一步的，K_x＝[k_x1,k_x2]，k_x1为x轴方向上的回复力系数，k_x2为x轴方向上的阻尼系数，K_y为y轴方向上的比例系数矩阵K_y＝[k_y1,k_y2]，k_y1为y轴方向上的回复力系数及k_y2为y轴方向上的阻尼系数。

进一步的，步骤S6包括：根据如下公式获取加速度控制指令a_kx和a_ky：

其中，X_kx为第k时刻在x轴上倾角及角速度的估计值矩阵，X_kx＝[θ_kx,θ’_kx]＝X_(k-1)x+k_gkx×I×(Z_kx-X_(k-1)x)；

其中，X_ky为第k时刻在y轴上倾角及角速度的估计值矩阵X_ky＝[θ_ky,θ’_ky]＝X_(k-1)y+k_gky×I×(Z_ky-X_(k-1)y)。

进一步的，Z_kx＝[Z_kθx,Z_kθ'x]，Z_kθx为第k时刻测得的x轴方向的倾角，Z_kθ'x为第k时刻测得的x轴方向的角速度；Z_ky＝[Z_kθy,Z_kθ'y]，Z_kθy为第k时刻测得的y轴方向的倾角，Z_kθ'y为第k时刻测得的y轴方向的角速度。

进一步的，

其中，k_gθkx为第k时刻x轴倾角的估算偏差增益，k_gθ'kx为第k时刻x轴角角速度的估算偏差增益,I＝[1,0]；k_gθky为第k时刻y轴倾角的估算偏差增益，k_gθ'ky为第k时刻y轴角角速度的估算偏差增益。

进一步的，P_zx和P_zy的计算方法按照无偏估计方法确定,首先确定k次采样值的平均值和之后再计算P_zx和P_zy：其中，其中，n＝1,2…k。

进一步的，该方法还包括：S6，结合S4中结果，获得第k时刻的x轴和y轴的先验估计协方差矩阵分别为Q_kx和Q_ky；

第k时刻的先验估计协方差矩阵Q_kx和Q_ky：

Q_kx＝Q_(k-1)x×(H-I^T×K_gkx)

Q_ky＝Q_(k-1)y×(H-I^T×K_gky)

其中，I＝[1,0]。

(三)有益效果

本发明所给出的全向轮式果园作业车主动安全控制指标的设计方法，在车辆的防侧翻、倾翻主动安全控制指标中，利用全向轮移动的灵活性特点，考虑了横向(x轴)和纵向(y轴)双方向的稳定性控制措施，可以达到对果园作业车的全向防倾翻控制。同时考虑到果园作业车测控系统检测信号中测量误差的干扰，对控制参数进行滤波处理，抑制了检测信号中测量误差干扰的影响，提高果园作业车主动安全控制系统的抗扰性。

附图说明

图1是本发明提供的一种设计全向轮式果园作业车主动安全控制指标的方法的流程示意图。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

本发明提供了一种设计全向轮式果园作业车主动安全控制指标的方法，如图1所示，该方法包括：

S1，利用安装在果园作业车质心位置的三轴加速度计及陀螺仪，对车辆x轴方向的倾角θ_x及角速度θ_x'、y轴方向的倾角θ_y及角速度θ_y'进行采样存储样本数据；

S2，根据所存储的样本数据中的k个的样本数据，建立果园作业车x轴比例系数矩阵K_x和y轴方向上的比例系数矩阵K_y；

S3，确定所述k个样本数据中第k-1采样时刻x轴方向的系统倾角及角速度的估计值矩阵X_(k-1)x、y轴方向的系统倾角及角速度的估计值矩阵X_(k-1)y、测量方差矩阵P_zx和P_zy、x轴的先验估计协方差矩阵先验估计协方差矩阵Q_(k-1)x、y轴的先验估计协方差矩阵Q_(k-1)y；

S4，结合S3中确定的P_zx、P_zy、Q_(k-1)x、Q_(k-1)y，计算系统在第k时刻x轴估算偏差增益矩阵k_gkx和y轴的估算偏差增益矩阵k_gky；

S5，获取第k时刻x轴的测量结果矩阵Z_kx和y轴的测量结果矩阵Z_ky，并结合S3和S4中的计算结果，获得第k时刻估计值矩阵X_kx及X_ky；

S6，结合S5中的计算结果，获得果园作业车第k时刻车辆输出加速度控制指令a_kx和a_ky。

本发明所给出的全向轮式果园作业车主动安全控制指标的设计方法，在车辆的防侧翻、倾翻主动安全控制指标中，利用全向轮移动的灵活性特点，考虑了横向(x轴)和纵向(y轴)双方向的稳定性控制措施，可以达到对果园作业车的全向防倾翻控制。同时考虑到果园作业车测控系统检测信号中测量误差的干扰，对控制参数进行滤波处理，抑制了检测信号中测量误差干扰的影响，提高果园作业车主动安全控制系统的抗扰性。

进一步的，K_x＝[k_x1,k_x2]，k_x1为x轴方向上的回复力系数，k_x2为x轴方向上的阻尼系数，K_y为y轴方向上的比例系数矩阵K_y＝[k_y1,k_y2]，k_y1为y轴方向上的回复力系数及k_y2为y轴方向上的阻尼系数。

进一步的，步骤S6包括：根据如下公式获取加速度控制指令a_kx和a_ky：

其中，X_kx为第k时刻在x轴上倾角及角速度的估计值矩阵，X_kx＝[θ_kx,θ’_kx]＝X_(k-1)x+k_gkx×I×(Z_kx-X_(k-1)x)；

其中，X_ky为第k时刻在y轴上倾角及角速度的估计值矩阵X_ky＝[θ_ky,θ’_ky]＝X_(k-1)y+k_gky×I×(Z_ky-X_(k-1)y)。

进一步的，Z_kx＝[Z_kθx,Z_kθ'x]，Z_kθx为第k时刻测得的x轴方向的倾角，Z_kθ'x为第k时刻测得的x轴方向的角速度；Z_ky＝[Z_kθy,Z_kθ'y]，Z_kθy为第k时刻测得的y轴方向的倾角，Z_kθ'y为第k时刻测得的y轴方向的角速度。

进一步的，

其中，k_gθkx为第k时刻x轴倾角的估算偏差增益，k_gθ'kx为第k时刻x轴角角速度的估算偏差增益,I＝[1,0]；k_gθky为第k时刻y轴倾角的估算偏差增益，k_gθ'ky为第k时刻y轴角角速度的估算偏差增益。

进一步的，P_zx和P_zy的计算方法按照无偏估计方法确定,首先确定k次采样值的平均值和之后再计算P_zx和P_zy：其中，其中，n＝1,2…k。

进一步的，该方法还包括：S7，结合S4中结果，获得第k时刻的x轴和y轴的先验估计协方差矩阵分别为Q_kx和Q_ky；

第k时刻的先验估计协方差矩阵Q_kx和Q_ky：

Q_kx＝Q_(k-1)x×(H-I^T×K_gkx)

Q_ky＝Q_(k-1)y×(H-I^T×K_gky)

其中，I＝[1,0]。

这里计算出的Q_kx和Q_ky可供后续的迭代计算使用，降低了计算难度。

以上实施方式仅用于说明本发明，而并非对本发明的限制，有关技术领域的普通技术人员，在不脱离本发明的精神和范围的情况下，还可以做出各种变化和变型，因此所有等同的技术方案也属于本发明的范畴，本发明的专利保护范围应由权利要求限定。

Claims (7)

1.一种设计全向轮式果园作业车主动安全控制指标的方法，其特征在于，包括：

S1，利用安装在果园作业车质心位置的三轴加速度计及陀螺仪，对车辆x轴方向的倾角θ_x及角速度θ_x'、y轴方向的倾角θ_y及角速度θ_y'进行采样存储样本数据；

S2，根据所存储的样本数据中的k个样本数据，建立果园作业车x轴比例系数矩阵K_x和y轴方向上的比例系数矩阵K_y；

S3，确定所述k个样本数据中第k-1采样时刻x轴方向的系统倾角及角速度的估计值矩阵X_(k-1)x、y轴方向的系统倾角及角速度的估计值矩阵X_(k-1)y、x轴方向的测量方差矩阵P_zx、y轴方向的测量方差矩阵P_zy、x轴的先验估计协方差矩阵先验估计协方差矩阵Q_(k-1)x、y轴的先验估计协方差矩阵Q_(k-1)y；

S4，结合S3中确定的P_zx、P_zy、Q_(k-1)x、Q_(k-1)y，计算系统在第k时刻x轴估算偏差增益矩阵k_gkx和y轴的估算偏差增益矩阵k_gky；

S5，获取第k时刻x轴的测量结果矩阵Z_kx和y轴的测量结果矩阵Z_ky，并结合S3和S4中的计算结果，获得第k时刻估计值矩阵X_kx及X_ky；

S6，结合S5中的计算结果，获得果园作业车第k时刻车辆输出加速度控制指令a_kx和a_ky。

2.如权利要求1所述的方法，其特征在于，K_x＝[k_x1,k_x2]，k_x1为x轴方向上的回复力系数，k_x2为x轴方向上的阻尼系数，K_y为y轴方向上的比例系数矩阵K_y＝[k_y1,k_y2]，k_y1为y轴方向上的回复力系数及k_y2为y轴方向上的阻尼系数。

3.如权利要求2所述的方法，其特征在于，步骤S6包括：根据如下公式获取加速度控制指令a_kx和a_ky：

$a_{kx}=K_x\×X_{kx}^T;$

$a_{ky}=K_y\×X_{ky}^T;$

其中，X_kx为第k时刻在x轴上倾角及角速度的估计值矩阵，X_kx＝[θ_kx,θ_k'_x]＝X_(k-1)x+k_gkx×I×(Z_kx-X_(k-1)x)；

其中，X_ky为第k时刻在y轴上倾角及角速度的估计值矩阵X_ky＝[θ_ky,θ'_ky]＝X_(k-1)y+k_gky×I×(Z_ky-X_(k-1)y)；其中，I＝[1,0]。

4.如权利要求1-3任一项所述的方法，其特征在于，Z_kx＝[Z_kθx,Z_kθ'x]，Z_kθx为第k时刻测得的x轴方向的倾角，Z_kθ'x为第k时刻测得的x轴方向的角速度；Z_ky＝[Z_kθy,Z_kθ'y]，Z_kθy为第k时刻测得的y轴方向的倾角，Z_kθ'y为第k时刻测得的y轴方向的角速度。

5.如权利要求1-3任一项所述的方法，其特征在于，

$k_{gkx}=\[k_{g\mathrm{\θ}kx},k_{{\mathrm{g\θ}}^{\′}kx}\]=I^T\frac{I\×Q_{(k-1)x}}{I\×(Q_{(k-1)x}+P_{zx})}$

$k_{gky}=\[k_{g\mathrm{\θ}ky},k_{{\mathrm{g\θ}}^{\′}ky}\]=I^T\frac{I\×Q_{(k-1)y}}{I\×(Q_{(k-1)y}+P_{zy})}$

其中，k_gθkx为第k时刻x轴倾角的估算偏差增益，k_gθ'kx为第k时刻x轴角角速度的估算偏差增益,I＝[1,0]；k_gθky为第k时刻y轴倾角的估算偏差增益，k_gθ'ky为第k时刻y轴倾角角速度的估算偏差增益。

6.如权利要求1-3任一项所述的方法，其特征在于，P_zx和P_zy的计算方法按照无偏估计方法确定,首先确定k次采样值的平均值和之后再计算P_zx和P_zy：其中， 其中，n＝1,2…k。

7.如权利要求1或2所述的方法，其特征在于，还包括：S6，结合S4中结果，获得第k时刻的x轴和y轴的先验估计协方差矩阵分别为Q_kx和Q_ky；

第k时刻的先验估计协方差矩阵Q_kx和Q_ky：

Q_kx＝Q_(k-1)x×(H-I^T×K_gkx)

Q_ky＝Q_(k-1)y×(H-I^T×K_gky)

其中，I＝[1,0]。