CN106274907A

CN106274907A - A kind of many trains splice angle vision measurement optimization method based on Kalman filtering

Info

Publication number: CN106274907A
Application number: CN201610662758.0A
Authority: CN
Inventors: 缪其恒
Original assignee: Zhejiang Zero Run Technology Co Ltd
Current assignee: Zhejiang Zero Run Technology Co Ltd
Priority date: 2016-08-12
Filing date: 2016-08-12
Publication date: 2017-01-04

Abstract

The invention discloses a kind of many trains splice angle vision measurement optimization method based on Kalman filtering, it comprises the following steps: S1, set up Three Degree Of Freedom linear train Vehicular system model；S2, setting initial value；S3, predicted state；S4, renewal Kalman gain；S5, correction state.The present invention utilizes the splice angle measured value of common steering wheel angle sensor signal correction view-based access control model system, can optimize splice angle certainty of measurement and the reliability of visual sensing system, after fusion, system remains to, in the case of the of short duration appearance of visual system wrong or invalid measurement output, work of remaining valid.This programme does not increase additional sensor equipment, and linear Kalman filter efficiency of algorithm is higher, real-time, it is adaptable to onboard system.

Description

A Kalman filter-based optimization method for multi-train articulation angle visual measurement

技术领域technical field

本发明涉及车辆控制领域，尤其是涉及一种基于卡尔曼滤波的多列车铰接角视觉测量优化方法。The invention relates to the field of vehicle control, in particular to a multi-train articulation angle visual measurement optimization method based on Kalman filtering.

背景技术Background technique

铰接角是多列车的重要动力学状态，准确的测量多列车辆铰接角可以有利于此类车辆主动安全系统的应用欲开发，如多列车主动转向与多列车倒车系统等。The articulation angle is an important dynamic state of multiple trains. Accurate measurement of the articulation angle of multiple trains can be beneficial to the development of active safety systems for such vehicles, such as multi-train active steering and multi-train reversing systems.

现有的铰接角测量技术分为如下两类：The existing articulation angle measurement techniques are divided into the following two categories:

1.接触式测量传感器：如旋转电位器安装在重型车第五轮位置。1. Contact measuring sensor: such as a rotary potentiometer installed at the fifth wheel position of a heavy-duty vehicle.

2.非接触式测量传感器：如超声波传感器和视觉传感器等。2. Non-contact measuring sensors: such as ultrasonic sensors and visual sensors.

接触式测量传感器或超声波传感器只适用于某种特定的铰接形式(如第五轮铰接)。由于视觉系统的图像质量易受诸多因素影响(系统振动、天气等)，视觉传感系统的测量量可靠性会受到影响。Contact measuring sensors or ultrasonic sensors are only suitable for a certain type of articulation (eg fifth wheel articulation). Since the image quality of the vision system is susceptible to many factors (system vibration, weather, etc.), the reliability of the measured quantities of the vision sensing system will be affected.

发明内容Contents of the invention

本发明主要是解决现有技术所存在的受限于特定的铰接形式或测量量可靠性不足的技术问题，提供一种不受铰接形式限制、不容易受系统振动或天气干扰的基于卡尔曼滤波的多列车铰接角视觉测量优化方法。The present invention mainly solves the technical problems existing in the prior art that are limited to specific articulation forms or insufficient reliability of measurement quantities, and provides a Kalman filter-based system that is not limited by articulation forms and is not susceptible to system vibration or weather interference. Optimization method for visual measurement of multi-train articulation angles.

本发明针对上述技术问题主要是通过下述技术方案得以解决的：一种基于卡尔曼滤波的多列车铰接角视觉测量优化方法，包括以下步骤：The present invention mainly solves the above-mentioned technical problems through the following technical solutions: a multi-train articulation angle visual measurement optimization method based on Kalman filtering, comprising the following steps:

S1、建立三自由度线性列车车辆系统模型；S1. Establish a three-degree-of-freedom linear train vehicle system model;

S2、设定初始值；S2, setting the initial value;

S3、预测状态；S3. Forecast status;

S4、更新卡尔曼增益；S4, update the Kalman gain;

S5、修正状态。S5. Modify the state.

视觉系统的铰接角测量值以及驾驶员方向盘转角传感器测量值为本系统的输入，拖挂车辆铰接角为本系统的输出。The articulation angle measurement value of the vision system and the steering wheel angle sensor measurement value of the driver are the input of this system, and the articulation angle of the trailer vehicle is the output of this system.

作为优选，所述三自由度线性列车车辆系统模型为：Preferably, the three-degree-of-freedom linear train vehicle system model is:

x_k+1＝Ax_k+Bu_k+w_k x _k+1 ＝Ax _k +Bu _k +w _k

z_k+1＝Cx_k+v_k z _k+1 ＝Cx _k +v _k

其中，k为离散时间序列，x_k为状态量，x_k为四维向量，包括牵引车侧向速度、横摆角速度、铰接角以及铰接角速度；Among them, k is a discrete time series, x _k is a state quantity, and x _k is a four-dimensional vector, including the lateral velocity, yaw rate, articulation angle and articulation angular velocity of the tractor;

z_k为铰接角的观测量；z _k is the observed quantity of articulation angle;

u_k为系统输入量，即方向盘转角；u _k is the input quantity of the system, that is, the steering wheel angle;

w_k为过程噪声；w _k is the process noise;

v_k为观测噪声；v _k is the observation noise;

状态空间矩阵A、B和C详细信息如下：The details of the state space matrices A, B and C are as follows:

$M m = = [\begin{matrix} {m m}_{11} + + {m m}_{22} & ((a a - - b b - - d d)) {m m}_{22} & - - {m m}_{22} d d & 00 \\ {m m}_{11} ((b b - - a a)) & {J J}_{11} & 00 & 00 \\ - - {m m}_{22} d d & {J J}_{22} + + {m m}_{22} d d ((b b - - a a + + d d)) & {J J}_{22} + + {m m}_{22} {d d}^{22} & 00 \\ 00 & 00 & 00 & 11 \end{matrix}];;$

$N N = = [\begin{matrix} \frac{{C C}_{11} + + {C C}_{22} + + {C C}_{33}}{v v} & - - (({m m}_{11} + + {m m}_{22})) v v + + \frac{a a (({C C}_{11} + + {C C}_{22} + + {C C}_{33})) - - {C C}_{22} c c - - {C C}_{33} ((b b + + e e))}{v v} & - - \frac{{C C}_{33} e e}{v v} & - - {C C}_{33} \\ {bC b}_{11} - - \frac{((c c - - b b)) {C C}_{22}}{v v} & - - {m m}_{11} ((b b - - a a)) v v + + \frac{{abC abC}_{11} + + {C C}_{22} ((c c - - b b)) ((c c - - a a))}{v v} & 00 & 00 \\ - - \frac{{C C}_{33} e e}{v v} & \frac{b b - - a a + + e e}{v v} {C C}_{33} e e + + {m m}_{22} d d v v & \frac{{C C}_{33} {e e}^{22}}{v v} & {C C}_{33} e e \\ 00 & 00 & 11 & 00 \end{matrix}];;$

E＝[C₁b 0 0]^T；E=[C ₁ b 0 0] ^T ;

A＝M^-1N；A＝M ^- 1N;

B＝M^-1E；B=M ^- 1E;

C＝[0 0 0 1]；C = [0 0 0 1];

其中，m₁为牵引车质量,m₂为挂车质量；J₁为牵引车横摆转动惯量,J₂为挂车横摆转动惯量；a为牵引车前轴与牵引车质心的距离，b为牵引车前轴与第五轮的距离，c为牵引车前轴与后轴的距离；d为挂车质心到第五轮的距离，e为后轴到第五轮的距离；C₁为牵引车前轴对应的轮胎的侧偏刚度，C₂为牵引车后轴对应的轮胎的侧偏刚度，C₃为挂车后轴对应的轮胎的侧偏刚度；v为纵向车速，为观测器的系统参数。Among them, m ₁ is the mass of the tractor, m ₂ is the mass of the trailer; J ₁ is the yaw moment of inertia of the tractor, J ₂ is the yaw moment of inertia of the trailer; a is the distance between the front axle of the tractor and the center of mass of the tractor, and b is the traction The distance between the front axle and the fifth wheel, c is the distance between the front axle and the rear axle of the tractor; d is the distance from the center of mass of the trailer to the fifth wheel, e is the distance from the rear axle to the fifth wheel; C ₁ is the front axle of the tractor C ₂ is the cornering stiffness of the tire corresponding to the rear axle of the tractor, C ₃ is the cornering stiffness of the tire corresponding to the rear axle of the trailer; v is the longitudinal vehicle speed, and is the system parameter of the observer.

作为优选，步骤S3中，预测状态依据以下公式确定：As a preference, in step S3, the predicted state is determined according to the following formula:

$\overset{^^}{x x} ((k k + + 11 | | k k)) = = A A \overset{^^}{x x} ((k k | | k k)) + + {Bu Bu}_{k k}$

P(k+1|k)＝AP(k|k)A^T+QP(k+1|k)＝AP(k|k) ^AT +Q

其中，为系统状态的先验估计；P(k+1|k)为系统协方差矩阵的先验估计；Q为系统过程协方差。in, is the prior estimate of the system state; P(k+1|k) is the prior estimate of the system covariance matrix; Q is the system process covariance.

作为优选，步骤S4中，由以下公式实现卡尔曼增益更新：As preferably, in step S4, the Kalman gain update is realized by the following formula:

K_k+1＝P(k+1|k)C^T(C P(k+1|k)C^T+R)^-1 K _k+1 ＝P(k+1|k)C ^T (CP(k+1|k)C ^T +R) ^-1

其中，K_k+1为卡尔曼增益；R为系统测量协方差。Among them, K _k+1 is the Kalman gain; R is the system measurement covariance.

作为优选，步骤S5中，状态修正由以下公式确定：As preferably, in step S5, the state correction is determined by the following formula:

$\overset{^^}{x x} ((k k + + 11 | | k k + + 11)) = = \overset{^^}{x x} ((k k + + 11 | | k k)) + + {K K}_{k k + + 11} (({z z}_{k k + + 11} - - C C \overset{^^}{x x} ((k k + + 11 | | k k))))$

P(k+1|k+1)＝P(k+1|k)-K_k+1C P(k+1|k)P(k+1|k+1)＝P(k+1|k)-K _k+1 CP(k+1|k)

其中，为系统状态的后验估计；P(k+1|k+1)为系统协方差矩阵的后验估计。in, is the posterior estimate of the system state; P(k+1|k+1) is the posterior estimate of the system covariance matrix.

作为优选，所述步骤S2中，x₀的初始值设定为[0 0 0 0]，状态协方差矩阵初始值P₀的初始值设定为0。Preferably, in the step S2, the initial value of x ₀ is set to [0 0 0 0], and the initial value of the state covariance matrix P ₀ is set to 0.

作为优选，系统过程协方差Q为diag(0.2,0.05,0.1,0.2)；系统测量协方差R为0.1。Preferably, the system process covariance Q is diag(0.2,0.05,0.1,0.2); the system measurement covariance R is 0.1.

本发明采用离散卡尔曼滤波算法，可在低速工况下(0-30km/h)优化视觉传感系统铰接角测量输出。The invention adopts a discrete Kalman filtering algorithm, and can optimize the articulation angle measurement output of the visual sensing system under low-speed working conditions (0-30km/h).

本发明利用常见方向盘转角传感器信号修正基于视觉系统的铰接角测量值，能够优化视觉传感系统的铰接角测量精度与可靠性，融合后系统在视觉系统短暂出现错误或无效测量输出的情况下仍能继续有效工作。本方案不增加额外传感设备，且线性卡尔曼滤波算法效率较高，实时性强，适用于车载系统。The present invention uses common steering wheel angle sensor signals to correct the articulation angle measurement value based on the vision system, and can optimize the articulation angle measurement accuracy and reliability of the vision sensing system. can continue to work effectively. This solution does not add additional sensing equipment, and the linear Kalman filter algorithm has high efficiency and strong real-time performance, and is suitable for vehicle-mounted systems.

本发明带来的实质性效果是，不需要额外增加成本，可以有效降低视觉系统测量误差，提升视觉系统对铰接角的测量可靠性。The substantive effect brought by the present invention is that it can effectively reduce the measurement error of the vision system and improve the reliability of the measurement of the articulation angle by the vision system without additional cost.

附图说明Description of drawings

图1是本发明的一种流程图。Fig. 1 is a kind of flowchart of the present invention.

具体实施方式detailed description

下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。The technical solution of the present invention will be further specifically described below through the embodiments and in conjunction with the accompanying drawings.

实施例：本实施例的一种基于卡尔曼滤波的多列车铰接角视觉测量优化方法，流程图如图1所示。Embodiment: A Kalman filter-based visual measurement optimization method for articulation angles of multiple trains in this embodiment, the flow chart of which is shown in FIG. 1 .

视觉系统的铰接角测量值以及驾驶员方向盘转角传感器测量值为本系统的输入，拖挂车辆铰接角为本系统的输出。本发明采用离散卡尔曼滤波算法，可在低速工况下(0-30km/h)优化视觉传感系统铰接角测量输出，详细的算法步骤介绍如下：The articulation angle measurement value of the vision system and the steering wheel angle sensor measurement value of the driver are the input of this system, and the articulation angle of the trailer vehicle is the output of this system. The present invention adopts the discrete Kalman filter algorithm, which can optimize the output of the articulation angle measurement of the visual sensing system under low-speed working conditions (0-30km/h). The detailed algorithm steps are introduced as follows:

1.三自由度线性列车车辆系统模型：1. Three degrees of freedom linear train vehicle system model:

x_k+1＝Ax_k+Bu_k+w_k x _k+1 ＝Ax _k +Bu _k +w _k

z_k+1＝Cx_k+v_k z _k+1 ＝Cx _k +v _k

其中，x_k为状态量，为四维向量，包括牵引车侧向速度，横摆角速度，铰接角以及铰接角速度；Among them, x _k is the state quantity, which is a four-dimensional vector, including the lateral velocity of the tractor, the yaw rate, the articulation angle and the articulation angular velocity;

z_k为观测量(铰接角)；z _k is the observed quantity (hinge angle);

u_k为系统输入(方向盘转角)；u _k is the system input (steering wheel angle);

w_k为过程噪声；w _k is the process noise;

v_k为观测噪声；v _k is the observation noise;

状态空间矩阵A,B,C详细信息如下：The details of the state space matrices A, B, and C are as follows:

E＝[C₁b 0 0]^T；E=[C ₁ b 0 0] ^T ;

A＝M^-1N；A＝M ^- 1N;

B＝M^-1E；B=M ^- 1E;

C＝[0 0 0 1]；C = [0 0 0 1];

其中，m1,m2为牵引车与挂车质量；J1,J2为牵引车与挂车横摆转动惯量；a,b,c分别为牵引车前轴与牵引车质心，第五轮以及后轴的距离；d,e分别为挂车质心以及后轴到第五轮的距离；C1,C2,C3为轮胎侧偏刚度；v为纵向车速设定值(由于此类传感器的适用工况多以低速工况为主，因而此处车速参数设定为15km/h，在车速范围为0-30km/h的情况下，滤波结果均可接受)。Among them, m1, m2 are the masses of the tractor and the trailer; J1, J2 are the yaw moment of inertia of the tractor and the trailer; a, b, c are the distances between the front axle of the tractor and the center of mass of the tractor, the fifth wheel and the rear axle respectively; d, e are the distances from the center of mass of the trailer and the rear axle to the fifth wheel; C1, C2, and C3 are the tire cornering stiffness; v is the set value of the longitudinal speed (because the applicable working conditions of this type of sensor are mostly low-speed working conditions) Mainly, so here the vehicle speed parameter is set to 15km/h, and the filtering results are acceptable when the vehicle speed range is 0-30km/h).

2.初始值设定：2. Initial value setting:

x₀＝[0 0 0 0]x ₀ =[0 0 0 0]

P₀＝0P ₀ =0

3.状态预测：3. State prediction:

P(k+1|k)＝AP(k|k)A^T+QP(k+1|k)＝AP(k|k) ^AT +Q

其中，为系统状态的先验估计；in, is the prior estimate of the system state;

P(k+1|k)为系统协方差矩阵的先验估计；P(k+1|k) is the prior estimate of the system covariance matrix;

Q为系统过程协方差；Q is the system process covariance;

4.卡尔曼增益更新:4. Calman buff update:

其中，K_k+1为卡尔曼增益Among them, K _k+1 is the Kalman gain

R为系统测量协方差R is the systematic measurement covariance

5.状态修正：5. Status correction:

P(k+1|k+1)＝P(k+1|k)-K_k+1C P(k+1|k)P(k+1|k+1)＝P(k+1|k)-K _k+1 CP(k+1|k)

其中，为系统状态的后验估计in, is the posterior estimate of the system state

P(k+1|k+1)为系统协方差矩阵的后验估计P(k+1|k+1) is the posterior estimate of the system covariance matrix

观测器参数主要有过程协方差Q以及测量协方差R，其设置如下：The observer parameters mainly include process covariance Q and measurement covariance R, which are set as follows:

Q＝diag(0.2,0.05,0.1,0.2)Q=diag(0.2,0.05,0.1,0.2)

R＝0.1R=0.1

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。The specific embodiments described herein are merely illustrative of the spirit of the invention. Those skilled in the art to which the present invention belongs can make various modifications or supplements to the described specific embodiments or adopt similar methods to replace them, but they will not deviate from the spirit of the present invention or go beyond the definition of the appended claims range.

尽管本文较多地使用了铰接角、侧向速度、噪声等术语，但并不排除使用其它术语的可能性。使用这些术语仅仅是为了更方便地描述和解释本发明的本质；把它们解释成任何一种附加的限制都是与本发明精神相违背的。Although terms such as articulation angle, lateral velocity, and noise are frequently used in this paper, the possibility of using other terms is not excluded. These terms are used only for the purpose of describing and explaining the essence of the present invention more conveniently; interpreting them as any kind of additional limitation is against the spirit of the present invention.

Claims

1. A visual measurement optimization method for multi-train articulation angles based on Kalman filtering is characterized by comprising the following steps:

s1, establishing a three-degree-of-freedom linear train vehicle system model;

s2, setting an initial value;

s3, predicting the state;

s4, updating Kalman gain;

and S5, correcting the state.

2. The multi-train articulation angle vision measurement optimization method based on Kalman filtering according to claim 1, characterized in that the three-degree-of-freedom linear train vehicle system model is:

x_k+1＝Ax_k+Bu_k+w_k

z_k+1＝Cx_k+v_k

wherein k is a discrete time series, x_kIs a state quantity, x_kThe four-dimensional vector comprises a lateral speed of the tractor, a yaw angular speed, an articulation angle and an articulation angular speed;

z_kis an observation of articulation angle;

u_kthe system input quantity, namely the steering wheel angle;

w_kis process noise;

v_kto observe noise;

the state space matrix A, B and C are detailed as follows:

M = [\begin{matrix} m_{1} + m_{2} & (a - b - d) m_{2} & - m_{2} d & 0 \\ m_{1} (b - a) & J_{1} & 0 & 0 \\ - m_{2} d & J_{2} + m_{2} d (b - a + d) & J_{2} + m_{2} d^{2} & 0 \\ 0 & 0 & 0 & 1 \end{matrix}];

N = [\begin{matrix} \frac{C_{1} + C_{2} + C_{3}}{v} & - (m_{1} + m_{2}) v + \frac{a (C_{1} + C_{2} + C_{3}) - C_{2} c - C_{3} (b + e)}{v} & - \frac{C_{3} e}{v} & - C_{3} \\ {bC}_{1} - \frac{(c - b) C_{2}}{v} & - m_{1} (b - a) v + \frac{{abC}_{1} + C_{2} (c - b) (c - a)}{v} & 0 & 0 \\ - \frac{C_{3} e}{v} & \frac{b - a + e}{v} C_{3} e + m_{2} d v & \frac{C_{3} e^{2}}{v} & C_{3} e \\ 0 & 0 & 1 & 0 \end{matrix}];

E＝[C₁b 0 0]^T；

A＝M^-1N；

B＝M^-1E；

C＝[0 0 0 1]；

wherein m is₁Mass m of the tractor₂The trailer mass; j. the design is a square₁Yaw moment of inertia for tractors, J₂Yawing moment of inertia for the trailer; a is the distance between the front axle of the tractor and the center of mass of the tractor, b is the distance between the front axle of the tractor and the fifth wheel, and c is the distance between the front axle of the tractor and the rear axle; d is the distance from the center of mass of the trailer to the fifth wheel, and e is the distance from the rear axle to the fifth wheel; c₁Cornering stiffness of the corresponding tyre for the front axle of the tractor, C₂Cornering stiffness of the corresponding tyre for the rear axle of the tractor, C₃Cornering stiffness for the tires corresponding to the rear axle of the trailer; v is the longitudinal vehicle speed.

3. The Kalman filtering-based visual measurement optimization method for multi-train articulation angles according to claim 2, wherein in step S3, the predicted state is determined according to the following formula:

\hat{x} (k + 1 | k) = A \hat{x} (k | k) + {Bu}_{k}

P(k+1|k)＝AP(k|k)A^T+Q

wherein,is a priori estimate of the system state; p (k +1| k) is a prior estimate of the system covariance matrix; q is the system process covariance.

4. The Kalman filtering based multi-train articulation angle vision measurement optimization method of claim 3, wherein in step S4, Kalman gain update is implemented by the following formula:

K_k+1＝P(k+1|k)C^T(C P(k+1|k)C^T+R)^-1

wherein, K_k+1Is the Kalman gain; and R is the system measurement covariance.

5. The Kalman filtering based multi-train articulation angle vision measurement optimization method of claim 4, wherein in step S5, the state correction is determined by the following formula:

\hat{x} (k + 1 | k + 1) = \hat{x} (k + 1 | k) + K_{k + 1} (z_{k + 1} - C \hat{x} (k + 1 | k))

P(k+1|k+1)＝P(k+1|k)-K_k+1C P(k+1|k)

wherein,is a posterior estimate of the system state; p (k +1| k +1) is the posterior estimate of the system covariance matrix.

6. The Kalman filtering based multi-train articulation angle vision measurement optimization method according to any one of claims 1 to 5, characterized in that in the step S2, x is₀Is set to [ 0000 ]]Initial value of state covariance matrix P₀Is set to 0.

7. The Kalman filtering based multi-train articulation angle vision measurement optimization method of claim 6, characterized in that the system process covariance Q is diag (0.2,0.05,0.1, 0.2); the system measurement covariance R was 0.1.