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

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 kind of many trains splice angle vision measurement optimization method based on Kalman filtering

Technical field

The present invention relates to field of vehicle control, especially relate to a kind of many trains splice angle vision based on Kalman filtering Measure optimization method.

Background technology

Splice angle is the important motivity state of many trains, measures many train of vehicles splice angle accurately and can be conducive to this type of The application of active safety systems of vehicles is intended to develop, such as multiple row car active steering and many trains backing system etc..

Existing splice angle is measured technology and is divided into following two classes:

1. contact type measurement sensor: take turns position as rotational potentiometer is arranged on heavy goods vehicles the 5th.

2. non-contact measurement sensor: such as ultrasonic sensor and vision sensor etc..

Contact type measurement sensor or ultrasonic sensor are only applicable to certain specific articulated form (as the 5th takes turns hinge Connect).Owing to the picture quality of visual system is easily affected (system vibration, weather etc.) by factors, the survey of visual sensing system Amount amount reliability can be affected.

Summary of the invention

The present invention mainly solves to be limited to specific articulated form or measurement amount reliability not existing for prior art The technical problem of foot, it is provided that a kind of do not limited by articulated form, be not susceptible to system vibration or weather disturbs based on Kalman Many trains splice angle vision measurement optimization method of filtering.

The present invention is directed to what above-mentioned technical problem was mainly addressed by following technical proposals: a kind of based on Kalman Many trains splice angle vision measurement optimization method of filtering, 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.

Splice angle measured value and driver's steering wheel angle sensor measured value of visual system are the input of native system, Tractor-trailer splice angle is the output of native system.

As preferably, described Three Degree Of Freedom linear train Vehicular system model is:

x_k+1=Ax_k+Bu_k+w_k

z_k+1=Cx_k+v_k

Wherein, k is discrete-time series, x_kFor quantity of state, x_kIt is four dimensional vectors, including tractor side velocity, yaw angle Speed, splice angle and splice angle speed；

z_kObserved quantity for splice angle；

u_kFor system input quantity, i.e. steering wheel angle；

w_kFor process noise；

v_kFor observation noise；

State space matrices A, B and C details are as follows:

$M=\left[\begin{array}{cccc}{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{array}\right];$

$N=\left[\begin{array}{cccc}\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\\ {\mathrm{bC}}_1-\frac{(c-b)C_2}{v}& -m_1(b-a)v+\frac{{\mathrm{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}dv& \frac{C_3{e}^2}{v}& C_{3}e\\ 0& 0& 1& 0\end{array}\right];$

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

A=M^-1N；

B=M^-1E；

C=[0 00 1]；

Wherein, m₁For tractor quality, m₂For trailer quality；J₁For tractor yaw rotation inertia, J₂Turn for trailer yaw Dynamic inertia；A is the distance of tractor front axle and tractor barycenter, and b is tractor front axle and the 5th distance taken turns, and c is tractor Front axle and the distance of rear axle；D be trailer barycenter to the 5th distance taken turns, e is that rear axle is to the 5th distance taken turns；C₁Before tractor The cornering stiffness of the tire that axle is corresponding, C₂For the cornering stiffness of tire corresponding to tractor rear axle, C₃For the wheel that trailer rear axle is corresponding The cornering stiffness of tire；V is longitudinal speed, for the systematic parameter of observer.

As preferably, in step S3, it was predicted that state determines according to below equation:

$\hat{x}(k+1|k)=A\hat{x}\left(k\right|k)+{\mathrm{Bu}}_k$

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

Wherein,Prior estimate for system mode；P (k+1 | k) is that the priori of system covariance matrix is estimated Meter；Q is systematic procedure covariance.

As preferably, in step S4, below equation realize Kalman gain and update:

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

Wherein, K_k+1For Kalman gain；R is systematic survey covariance.

As preferably, in step S5, state revision is determined by below equation:

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

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

Wherein,Posterior estimator for system mode；P (k+1 | k+1) it is system covariance matrix Posterior estimator.

As preferably, in described step S2, x₀Initial value be set as [0 00 0], state covariance matrix initial value P₀ Initial value be set as 0.

As preferably, systematic procedure covariance Q is diag (0.2,0.05,0.1,0.2)；Systematic survey covariance R is 0.1。

The present invention uses Kalman Filtering for Discrete algorithm, (0-30km/h) can optimize visual sensing system under speed operation Splice angle measures output.

The present invention utilizes the splice angle measured value of common steering wheel angle sensor signal correction view-based access control model system, it is possible to Optimizing splice angle certainty of measurement and the reliability of visual sensing system, after fusion, system is in visual system of short duration appearance mistake or nothing Effect remains to, in the case of measuring 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.

The substantial effect that the present invention brings is, it is not necessary to additionally increase cost, can effectively reduce visual system and measure Error, promotes the visual system Measurement reliability to splice angle.

Accompanying drawing explanation

Fig. 1 is a kind of flow chart of the present invention.

Detailed description of the invention

Below by embodiment, and combine accompanying drawing, technical scheme is described in further detail.

Embodiment: a kind of based on Kalman filtering many trains splice angle vision measurement optimization method of the present embodiment, stream Journey figure is as shown in Figure 1.

Splice angle measured value and driver's steering wheel angle sensor measured value of visual system are the input of native system, Tractor-trailer splice angle is the output of native system.The present invention uses Kalman Filtering for Discrete algorithm, can be at (0-under speed operation 30km/h) optimizing visual sensing system splice angle and measure output, detailed algorithm steps is described below:

1. Three Degree Of Freedom linear train Vehicular system model:

x_k+1=Ax_k+Bu_k+w_k

z_k+1=Cx_k+v_k

Wherein, x_kFor quantity of state, it is four dimensional vectors, including tractor side velocity, yaw velocity, splice angle and hinge Connect angular velocity；

z_kFor observed quantity (splice angle)；

u_k(steering wheel angle) is inputted for system；

w_kFor process noise；

v_kFor observation noise；

State space matrices A, B, C details are as follows:

$M=\left[\begin{array}{cccc}{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{array}\right];$

$N=\left[\begin{array}{cccc}\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\\ {\mathrm{bC}}_1-\frac{(c-b)C_2}{v}& -m_1(b-a)v+\frac{{\mathrm{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}dv& \frac{C_3{e}^2}{v}& C_{3}e\\ 0& 0& 1& 0\end{array}\right];$

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

A=M^-1N；

B=M^-1E；

C=[0 00 1]；

Wherein, m1, m2 are tractor and trailer quality；J1, J2 are tractor and trailer yaw rotation inertia；A, b, c divide Not Wei tractor front axle and tractor barycenter, the 5th takes turns and the distance of rear axle；D, e are respectively trailer barycenter and rear axle to Five distances taken turns；C1, C2, C3 are tire cornering stiffness；V is longitudinal speed setting value (applicable working condition due to sensors with auxiliary electrode Many based on speed operation, thus speed parameter is set as 15km/h herein, in the case of vehicle speed range is 0-30km/h, filter Ripple result is the most acceptable).

2. initial value sets:

x₀=[0 00 0]

P₀=0

3. status predication:

$\hat{x}(k+1|k)=A\hat{x}\left(k\right|k)+{\mathrm{Bu}}_k$

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

Wherein,Prior estimate for system mode；

P (k+1 | k) is the prior estimate of system covariance matrix；

Q is systematic procedure covariance；

4. Kalman gain updates:

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

Wherein, K_k+1For Kalman gain

R is systematic survey covariance

5. state revision:

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

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

Wherein,Posterior estimator for system mode

P (k+1 | k+1) is the Posterior estimator of system covariance matrix

Observer parameter mainly has process covariance Q and measures covariance R, and it is provided that

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

R=0.1

Specific embodiment described herein is only to present invention spirit explanation for example.Technology neck belonging to the present invention Described specific embodiment can be made various amendment or supplements or use similar mode to replace by the technical staff in territory Generation, but without departing from the spirit of the present invention or surmount scope defined in appended claims.

Although the most more employing the terms such as splice angle, side velocity, noise, but it is not precluded from using other term Probability.Use these terms to be only used to more easily to describe and explain the essence of the present invention；It is construed as appointing What a kind of additional restriction is all contrary with spirit of the present invention.

Claims (7)

1. many trains splice angle vision measurement optimization method based on Kalman filtering, it is characterised in that include following step Rapid:

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.

A kind of many trains splice angle vision measurement optimization method based on Kalman filtering the most according to claim 1, its Being characterised by, described Three Degree Of Freedom linear train Vehicular system model is:

x_k+1=Ax_k+Bu_k+w_k

z_k+1=Cx_k+v_k

Wherein, k is discrete-time series, x_kFor quantity of state, x_kIt is four dimensional vectors, including tractor side velocity, yaw angle speed Degree, splice angle and splice angle speed；

z_kObserved quantity for splice angle；

u_kFor system input quantity, i.e. steering wheel angle；

w_kFor process noise；

v_kFor observation noise；

State space matrices A, B and C details are as follows:

$M=\left[\begin{array}{cccc}{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{array}\right];$

$N=\left[\begin{array}{cccc}\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\\ {\mathrm{bC}}_1-\frac{(c-b)C_2}{v}& -m_1(b-a)v+\frac{{\mathrm{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}dv& \frac{C_3{e}^2}{v}& C_{3}e\\ 0& 0& 1& 0\end{array}\right];$

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

A=M^-1N；

B=M^-1E；

C=[0 00 1]；

Wherein, m₁For tractor quality, m₂For trailer quality；J₁For tractor yaw rotation inertia, J₂Rotate used for trailer yaw Amount；A is the distance of tractor front axle and tractor barycenter, and b is tractor front axle and the 5th distance taken turns, and c is tractor front axle Distance with rear axle；D be trailer barycenter to the 5th distance taken turns, e is that rear axle is to the 5th distance taken turns；C₁For tractor front axle pair The cornering stiffness of the tire answered, C₂For the cornering stiffness of tire corresponding to tractor rear axle, C₃For tire corresponding to trailer rear axle Cornering stiffness；V is longitudinal speed.

A kind of many trains splice angle vision measurement optimization method based on Kalman filtering the most according to claim 2, its It is characterised by, in step S3, it was predicted that state determines according to below equation:

$\hat{x}(k+1|k)=A\hat{x}\left(k\right|k)+{\mathrm{Bu}}_k$

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

Wherein,Prior estimate for system mode；P (k+1 | k) is the prior estimate of system covariance matrix；Q For systematic procedure covariance.

A kind of many trains splice angle vision measurement optimization method based on Kalman filtering the most according to claim 3, its It is characterised by, in step S4, below equation realizes Kalman gain and update:

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

Wherein, K_k+1For Kalman gain；R is systematic survey covariance.

A kind of many trains splice angle vision measurement optimization method based on Kalman filtering the most according to claim 4, its Being characterised by, in step S5, state revision is determined by below equation:

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

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

Wherein,Posterior estimator for system mode；P (k+1 | k+1) is the posteriority of system covariance matrix Estimate.

A kind of many trains splice angle vision based on Kalman filtering the most as claimed in any of claims 1 to 5 is surveyed Amount optimization method, it is characterised in that in described step S2, x₀Initial value be set as at the beginning of [0 00 0], state covariance matrix Initial value P₀Initial value be set as 0.

A kind of many trains splice angle vision measurement optimization method based on Kalman filtering the most according to claim 6, its Being characterised by, systematic procedure covariance Q is diag (0.2,0.05,0.1,0.2)；Systematic survey covariance R is 0.1.