LOW-SPEED MANEUVERING ASSISTANT FOR A VEHICLE

Abstract

A maneuvering assistant system for a vehicle combination comprising an interface configured to receive measurements from a camera, processing circuitry implementing a Kalman filter configured to observe an articulation angle of the vehicle combination on the basis of the measurements from the camera, a signal analyzer configured to repeatedly estimate a signal noise of the camera measurement and to adjust the Kalman filter on the basis of statistics derived from the estimated signal noise, and an assistance unit configured to receive an estimate of the articulation angle from the Kalman filter and provide drive actions based on the estimate.

Claims

1. A method for assisting maneuvering of a vehicle combination, comprising: initiating a Kalman filter which observes an articulation angle of the vehicle combination and which is fed with measurements from a camera; repeatedly estimating a signal noise of the camera measurements; adjusting the Kalman filter on the basis of statistics derived from the estimated signal noise; obtaining an estimate of the observed articulation angle from the Kalman filter; and providing maneuvering assistance based on the estimate of the articulation angle.

2. The method of claim 1, wherein the adjusting the Kalman filter comprises adjusting a Kalman filter parameter corresponding to measurement noise covariance.

3. The method of claim 2, wherein the adjusting includes adjusting a gain of the Kalman filter.

4. The method of claim 2, further comprising: detecting a signal fault of the camera measurement; and for as long as the signal fault is detected, applying a maximum value of the measurement noise covariance.

5. The method of claim 1, wherein the camera is located behind a rearmost axle of a vehicle ahead of an articulation point.

6. The method of claim 1, wherein the Kalman filter is fed with measurements from a sensor, and the Kalman filter is invariant with respect to signal noise of the further sensor.

7. The method of claim 1, wherein the maneuvering assistance includes low-speed maneuvering assistance.

8. The method of claim 7, wherein the low-speed maneuvering assistance includes autonomous reversing.

9. The method of claim 1, wherein the Kalman filter is a non-linear Kalman filter.

10. A non-transitory computer program comprising instructions which, when the computer program is executed by a computer, cause the computer to: initiate a Kalman filter which observes an articulation angle of the vehicle combination and which is fed with measurements from a camera; repeatedly estimate a signal noise of the camera measurements; adjust the Kalman filter on the basis of statistics derived from the estimated signal noise; obtain an estimate of the observed articulation angle from the Kalman filter; and provide maneuvering assistance based on the estimate of the articulation angle.

11. The non-transitory computer program of claim 10, wherein the computer program executed by the computer further causes the computer to adjust the Kalman filter parameter corresponding to measurement noise covariance.

12. The non-transitory computer program of claim 10, wherein the computer program executed by the computer further causes the Kalman filter to be fed with measurements from a sensor, and the Kalman filter is invariant with respect to signal noise of the further sensor.

13. The non-transitory computer program of claim 10, wherein the Kalman filter is a non-linear Kalman filter.

14. A maneuvering assistant system for a vehicle combination comprising: an interface configured to receive measurements from a camera; processing circuitry implementing a Kalman filter configured to observe an articulation angle of the vehicle combination on the basis of the measurements from the camera; a signal analyzer configured to: repeatedly estimate a signal noise of the camera measurement; and adjust the Kalman filter on the basis of statistics derived from the estimated signal noise; and an assistance unit configured to receive an estimate of the articulation angle from the Kalman filter and provide drive actions based on the estimate.

15. The maneuvering assistant system of claim 14, wherein the assistance unit is configured with a vehicle model representing the vehicle combination.

16. The maneuvering assistant system of claim 14, wherein the signal analyzer is configured to adjust the Kalman filter the basis of statistics derived from the estimated signal noise by being configured to adjust the Kalman filter parameter corresponding to measurement noise covariance.

17. The maneuvering assistant system of claim 14, wherein the interface is configured to receive the measurements from the camera is located behind a rearmost axle of a vehicle ahead of an articulation point.

18. The maneuvering assistant system of claim 14, wherein the Kalman filter is a non-linear Kalman filter.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Aspects and embodiments are now described, by way of example, with reference to the accompanying drawings, on which:

[0017] FIG. 1 shows a two-unit vehicle combination equipped with a camera (upper part) together with a maneuvering assistant system comprising a Kalman filter and a signal analyzer for adjusting the Kalman filter based on estimated camera signal noise (lower part);

[0018] FIG. 2 is a flowchart of a method for assisting the maneuvering of a vehicle combination;

[0019] FIG. 3 is a schematic drawing of a two-unit vehicle combination with respective momentary longitudinal speeds v.sub.1x, v.sub.2x, yaw angles θ.sub.1, θ.sub.2 and articulation angle ϕ indicated;

[0020] FIG. 4 shows a trailer front side as viewed from a tractor-mounted camera; and

[0021] FIG. 5 shows an articulated construction equipment vehicle, a three-unit vehicle combination and an articulated passenger vehicle.

DETAILED DESCRIPTION

[0022] The aspects of the present disclosure will now be described more fully hereinafter with reference to the accompanying drawings, in which certain embodiments of the invention are shown. These aspects may, however, be embodied in many different forms and should not be construed as limiting; rather, these embodiments are provided by way of example so that this disclosure will be thorough and complete, and to fully convey the scope of all aspects of the invention to those skilled in the art. Like numbers refer to like elements throughout the description.

[0023] FIG. 1 shows a vehicle combination 190 consisting of a tractor 190F and a trailer 190R, and the vehicle combination 19o is associated with a maneuvering assistant system 100. The maneuvering assistant system 100 may be installed onboard one of the vehicle units of the vehicle combination 190 or in an external fixed or moving location, or it may be implemented with networked processing support (‘cloud’ resources). The maneuvering assistant system 100 may have at least the following principal functional units: a signal interface 110, processing circuitry 120 implementing a Kalman filter 121, a signal analyzer 130 and an assistance unit 140.

[0024] In the vehicle combination 190, a camera 191 is arranged to image a view of the tractor 190F or the trailer 190R or both, based on which an articulation angle ϕ can be determined. As illustrated in more detail in FIG. 3, the articulation angle ϕ of the vehicle combination 190 may be understood as the difference between the respective yaw angles θ.sub.1, θ.sub.2 of the vehicle units, which angles represent absolute orientations with respect to a non-rotating reference frame. The orientation of each vehicle unit may be defined as the (yaw) orientation of its longitudinal axis, with which the longitudinal component v.sub.1x, v.sub.2x of its velocity is aligned.

[0025] The camera 191 may be mounted on the tractor 190F and be directed towards the trailer 190R. For example, the camera 191 may be mounted on the rear side of the driver cab, as suggested by FIG. 1, or after the rear axle of the tractor 190F approximately 1 m above ground level, or after the rearmost axle of the tractor 190F ahead of the articulation point. When the camera 191 is mounted on the rear side of the driver cab, its view may be similar to that shown in FIG. 4, where the front face of the trailer 190R during a left turn (non-zero articulation angle) is seen. It is seen in FIG. 4 that the front face of the trailer 190R deviates by an angle α from the horizontal axis L.sub.1 whereas it remains aligned with the vertical axis L.sub.2. Relevant elements may be identified in the camera image by feature tracking. From the angle α, the articulation angle ϕ can be determined on the basis of a theoretical or empirical relation. To be precise, knowledge of the angle α allows the determination of a yaw component of the articulation angle ϕ, which is the main focus of the present disclosure. Some embodiments may additionally include a determination of a pitch component of the articulation angle; image data on which such determination can be based may be obtained from a camera with a similar or modified placement and/or by arranging suitable optical markers (fiducials) on the vehicle combination 190 and/or relying on depth data. As an alternative to mounting on the tractor 190F, the camera 191 may be mounted on the trailer 190R and capture the rear side of the tractor 190F. Further, the camera 191 may be of a wide-angle type and arranged to capture the tractor 190F and trailer 190R in a common scene, whereby the relative position and orientation can be inspected visually. The signal interface 110 of the maneuvering assistant system 100 receives the measurements z.sub.1 from the camera 191.

[0026] The vehicle combination 190 may include at least one further sensor 192 supplying the maneuvering assistant system 100 with measurement data. The further sensor 192 may be a fifth-wheel sensor for direct measurement of the articulation angle ϕ, an inertial position or orientation sensor, a global navigation satellite system (GNSS) receiver, a torque sensor, wheel speed or position sensor, or a steering angle sensor. Further still, the further sensor 192 may be arranged to observe control signals u.sub.driver which the driver provides manually to the driveline of the tractor 190F or to other relevant actuators in the vehicle combination 190.

[0027] The assistance unit 140 is configured with a vehicle model that describes the vehicle combination 190 and can be utilized to determine drive actions u (e.g., acceleration/braking signals, steering angle signals, and other commands directed to the driveline of the tractor 190F or to other relevant actuators in the vehicle combination 190) for causing the vehicle combination 190 to move as desired or for helping a human driver to maneuver the vehicle combination 190 as desired. The assistance unit 140 may be configured primarily for low-speed maneuvering assistance. The vehicle model in the assistance unit 140 may be an inverse model which maps desired movements (e.g., a forward or reverse path) to a sequence of drive actions. Alternatively, the vehicle model may be a forward model allowing the assistance unit 140 to simulate the effect of a tentative sequence of drive actions and to adjust these if necessary. For a description of a generic reversing assistant, reference is made to the applicant's patent EP2997429B1. The assistance unit 140 generates the drive actions u on the basis of a vehicle state estimated by the Kalman filter 121, including an estimate {circumflex over (ϕ)} of the articulation angle, and an operator's input (not shown) representing the desired movements of the vehicle combination 190.

[0028] To illustrate a basic embodiment of the invention, a linear Kalman filter 121 implemented as software code to be executed in discrete time by the processing circuitry 120 will now be described. The Kalman filter 121 maintains a state {circumflex over (x)} which estimates the true state x of the vehicle combination 190. The true state x is assumed to evolve with time as:

x(k)=Fx(k−1)+Bu(k)+w(k),

where F is a state-transition model matrix (optionally time-dependent), B is a control-input model matrix (optionally time-dependent), u(k) is the drive-action signal at time k, and w(k) is process noise assumed to have normal distribution with covariance Q and zero mean. The state x may have one or more components, one of which may be the articulation angle ϕ. Alternatively, the articulation angle ϕ may be derivable from the state x. Further components of the state x may be position, velocity, steering angle, a pitch component of the articulation angle and—in the case of a three-unit vehicle combination 400 (FIG. 5)—a further articulation angle. It is moreover assumed that an observation z(k) of the true state is given by

z(k)=Hx(k)+v(k),

where H is an observation model matrix and v(k) is measurement noise likewise assumed to be normally distributed with covariance R (measurement noise covariance) and zero mean. The measurement noise covariance matrix R is diagonal if the noise terms are uncorrelated. As indicated in FIG. 1, the measurement vector z(k) at least includes a signal z.sub.1(k) from the camera 191 and a signal z.sub.2(k) from the further sensor(s) 192. The camera signal z.sub.1(k) is assumed to include not only the true measurement y.sub.1(k) but also a noise term v.sub.1(k); the further sensor signal z.sub.2 is modeled similarly.

[0029] The Kalman filter 121 is configured to evolve the estimated state {circumflex over (x)} according to the following prediction operation:

{circumflex over (x)}(k|k−1)=F{circumflex over (x)}(k−1|k−1)+Bu(k),

that is, the a posteriori state estimate at time k given observations up to time k−1 is computed from the previous state estimate {circumflex over (x)}(k−1|k−1) and the drive-action signal u(k). The measurement signal z is fed to the Kalman filter 121 via an update operation:

{circumflex over (x)}(k|k)={circumflex over (x)}(k|k−1)+K(k){tilde over (y)}(k),

where the so-called innovation {tilde over (y)} is given by

{tilde over (y)}(k)=z.sub.k−H{circumflex over (x)}(k|k−1).

According to Kalman filter theory, the optimal value of the Kalman gain K(k), to be used in the update operation, is:

K(k)=P(k|k−1)H.sup.TS(k).sup.−1,

where the predicted estimate covariance

P(k|k−1)=FP(k−1|k−1)F.sup.T+Q,

H.sup.T is transpose of H, and the innovation covariance

S(k)=HP(k|k−1)H.sup.T+R.

[0030] As indicated in FIG. 1, the Kalman filter 121 is fed with approximate values of the matrices B, F, H, Q. These values, which may have been determined in a prototype, tuning or testing phase prior to commercial deployment (e.g., using an Autocovariance Least-Squares or a Field Kalman Filter algorithm), would generally be maintained unchanged during operation. According to some embodiments herein, however, at least one component of the measurement noise covariance R is repeatedly estimated. For this purpose, the signal analyzer 130 downstream of the signal interface 110 repeatedly estimates the covariance {circumflex over (r)}.sub.11 of the noise v.sub.1 included the camera signal z.sub.1. On this basis, if the noise terms v.sub.1, v.sub.2 are assumed to be uncorrelated, the signal analyzer 130 may estimate the measurement noise covariance matrix as

[00001]$\hat{R}=\left[\begin{array}{cc}{\hat{r}}_{11}& 0\\ 0& r_{22}\end{array}\right],$

where r.sub.22, which relates to the further sensor 192, may have been estimated prior to commercial deployment and be maintained unchanged during operation, similar to matrices B, F, H, Q. In some embodiments, the signal analyzer 130 may estimate a different statistical indicator, such as standard deviation, variance, range, interquartile range—e.g. using a specialized chipset which is optimized for such estimation—which is subsequently converted into the measurement noise covariance matrix estimate {circumflex over (R)}.

[0031] For either one of these estimates, the notation {circumflex over (R)}(k) may be used, which is intended to signify that a new value may be assigned to the measurement noise covariance matrix estimate every epoch k; it not to be taken to mean that epoch-wise updating is an essential feature of the present invention. The estimate {circumflex over (R)}(k) is fed to the Kalman filter 121 in addition to these matrices, which causes the innovation covariance S(k) to change as per

S(k)=HP(k|k−1)H.sup.T+{circumflex over (R)}(k),

which in turn adjusts the Kalman gain K(k). Because the Kalman gain K(k) has an inverse dependence on {circumflex over (R)}(k), an increase of the estimated covariance {circumflex over (r)}.sub.11 will decrease the camera signal's z.sub.1 influence on the updated state {circumflex over (x)}(k|k). A covariance increase may signify that the camera signal z.sub.2 has become noisier; the inventor has realized that this may correspond to poor sensor conditions, such as camera lens reflections at low sun angle, ambient light outside design range etc., which are likely to corrupt the camera signal z.sub.1 or render it invalid. Rather than relying excessively on the camera signal z.sub.1 in such conditions, the Kalman filter 121 will put more trust in the prediction operation reflecting the state model, thereby marginally approaching a dead-reckoning setup for those components of the state x that are normally updated on the basis of the camera signal z.sub.1. Conversely, a relatively noise-free camera signal will be given significant weight when the Kalman filter state {circumflex over (x)}(k|k−1) is to be updated into {circumflex over (x)}(k|k). Accordingly, the quantitative properties of the Kalman filter 121 adapt responsively when the camera signal quality varies. Because a change in {circumflex over (R)}(k) affects the next increment of the updated state {circumflex over (x)}(k|k) rather than the state itself, the Kalman filter 121 operates uniformly and stably. Further, because the measurement noise of the camera 191 is directly related to the estimation accuracy, each adjustment of the Kalman gain is fully justified.

[0032] The Kalman gain indicated above is optimal in the sense that it minimizes the residual error. There exist further strategies for setting the Kalman gain in given circumstances F, H, Q, R, to improve the state estimation quality in various regards, e.g., by reducing a statistical variability measure of the estimated state. The above-described technique, of performing a continuous estimation on the noise of the camera signal z.sub.2, remains applicable also in combination with these alternative strategies for setting the Kalman gain K(k). Generally, the measurement noise matrix estimate {circumflex over (R)}(k) may be substituted for all occurrences of the measurement noise matrix R in the basic version of these strategies, whereby the quantitative properties of the Kalman filter 121 are updated in a controlled fashion.

[0033] Other embodiments may employ a nonlinear vehicle model. A nonlinear kinematic model of a vehicle combination 190 at variable speed is described in P. Nilsson et al., “Single-track models of an A-double heavy vehicle combination”, Technical reports in Applied Mechanics 2013:08, Chalmers University of Technology (2013). The same report also describes an approximate linear model for constant speed, which may be used with the above-described embodiment with the standard Kalman filter. In embodiments where a nonlinear model is preferred, an extended Kalman filter may be used, in which matrices F, H are replaced with general functions ƒ, h. It suffices to modify the above equations in to a new [0034] prediction operation: {circumflex over (x)}(k|k−1)=ƒ({circumflex over (x)}(k−1|k−1),u(k)), and [0035] innovation: {tilde over (y)}(k)=z(k)−h({circumflex over (x)}(k|k−1)),
and the matrices F, H in the Kalman gain and predicted estimate covariance are replaced by the Jacobians

[00002]${{F\left(k\right)=\frac{\partial f}{\partial x}.Math.}_{\hat{x}(k-1.Math.k-1),u\left(k\right)}H\left(k\right)=\frac{\partial h}{\partial x}.Math.}_{\hat{x}(k-1.Math.k)}$

which represent a linearization at the predicted state of the Kalman filter. Substituting these Jacobians into the basic expressions, the Kalman gain changes into

[00003]$K\left(k\right)=P(k.Math.k-1){H\left(k\right)}^T{S\left(k\right)}^{-1}=\hspace{1em}[F\left(k\right)P\left(k-1.Math.k-1){F\left(k\right)}^T+Q\right]{H\left(k\right)}^T[H\left(k\right){P\left(k.Math.k-1){H\left(k\right)}^T+\hat{R}\left(k\right)\right]}^{-1}.$

[0036] In variations of the present embodiment, an unscented Kalman filter or an ensemble Kalman filter may be used.

[0037] As seen above, the linearization in itself causes H, K to be time-dependent quantities. It is furthermore possible to use time-dependent functions ƒ=ƒ(⋅,⋅,k), h=h(⋅,k) to model the system, which produces modified [0038] prediction operation: {circumflex over (x)}(k|k−1)=ƒ({circumflex over (x)}(k−1|k−1),u(k),k), and [0039] innovation: {tilde over (y)}(k)=z(k)—h({circumflex over (x)}(k|k−1),k).

[0040] In one embodiment, the signal analyzer 130 is configured with a dedicated functionality or dedicated software routine for detecting a signal fault affecting the signal path from the camera 191. A signal fault may correspond to a hardware failure (e.g., cables, switches, interfaces), a software execution failure or communication failure during runtime, by which the camera signal z.sub.1 does not reach the signal interface 110 in an orderly fashion. The camera signal z.sub.1 may for example take values outside the defined range. In addition to the camera signal z.sub.1 itself, there may exist observable secondary indicators such as network health status parameters allowing to conclude a signal failure. In this case the signal analyzer 130 may be configured to not attempt to estimate the covariance of the signal that it receives (or wait for the signal to reappear), but rather apply a maximum value of the measurement noise covariance for as long as the signal fault is detected. This may include setting {circumflex over (r)}.sub.11=r.sub.max, where r.sub.max is the largest (float) number that can be represented in the processing circuitry 120. Conceptually, the signal analyzer 130 may be adapted to set {circumflex over (r)}.sub.11=∞, whereby S.sup.−1=0 and thus K(k)=0. If the Kalman filter 121 maintains a state component {circumflex over (x)}.sub.j(k) that is updated only on the basis of the camera signal z.sub.1, the zeroing of the corresponding element of the Kalman gain matrix K (k) (position (j,1)) will cause {circumflex over (x)}.sub.j(k) to be predicted on a pure dead-reckoning basis. The signal analyzer's 130 transfers into and out of the emergency mode in response to a signal fault does not in themselves imply a sudden change in the state maintained by the Kalman filter 121.

[0041] FIG. 2 summarizes the above-described behavior of the maneuvering assistant system 100 as a method 200 for assisting maneuvering of a vehicle combination 190.

[0042] In a first step 210 of the method 200, a Kalman filter 121 which observes an articulation angle ϕ of the vehicle combination 190 and which is fed with measurements from a camera 191 is initiated. As explained above, the articulation angle ϕ may be a component of a state (variable) maintained by the Kalman filter 121, or it may be derivable from one or more other components of the state.

[0043] In a second step 212, the signal noise of the camera measurements is estimated in a continuous fashion. In a third step 214, the Kalman filter 121 is adjusted on the basis of statistics derived from the estimated signal noise. The statistics may include a covariance {circumflex over (r)}.sub.11. Steps 212 and 214 may be repeated as long as needed, similar to the prediction and update operation of the Kalman filter 121 which serve to maintain the estimated state {circumflex over (x)}.

[0044] In a fourth step 216, a command is received from an operator (who may be a driver of the vehicle combination 190) to initiate maneuvering assistance. To effectuate this, in fifth and sixth steps 218, 220, the maneuvering assistant system 100 obtains an estimate {circumflex over (ϕ)} of the articulation angle from the Kalman filter 121 and provides maneuvering assistance based thereon, possibly by reference to a vehicle model.

[0045] FIG. 5 shows an articulated construction equipment vehicle (wheel loader) 300, a three-unit vehicle combination 400 and an articulated bus 500, each of which is understood to be a “vehicle combination” in the sense of the present disclosure.

[0046] The aspects of the present disclosure have mainly been described above with reference to a few embodiments. However, as is readily appreciated by a person skilled in the art, other embodiments than the ones disclosed above are equally possible within the scope of the invention, as defined by the appended patent claims.