JP7206875B2 - Vehicle lateral speed estimator - Google Patents

Description

The present invention relates to a vehicle lateral speed estimating device, and more particularly to a vehicle lateral speed estimating device for estimating a vehicle lateral speed based on a relationship between a traveling road and a plane motion of a vehicle including sideslip.

A vehicle moves in three axial directions of forward/backward motion, lateral motion, and yaw motion with respect to a road surface. In particular, the lateral velocity of the vehicle is an important factor in the behavior of the vehicle during cornering. By making it possible to measure the lateral movement of the vehicle body, for example, the self-position estimation of the vehicle can be performed with high accuracy. Therefore, highly accurate self-position estimation also enables accurate correction of various other sensors that measure the motion of the vehicle. In addition, it is possible to correct the vehicle dynamics model online, and it can be used for online tuning of vehicle control parameters calculated by the vehicle dynamics model.

Lateral motion can be detected with a slip angle sensor, and yaw motion can be detected with a yaw rate sensor. However, the slip angle sensor is a complicated and expensive device that detects the lateral speed of the vehicle by irradiating the road surface with a laser beam from a running vehicle and receiving the laser beam reflected from the road surface. hard to be Therefore, in order to estimate the vehicle lateral velocity of mass-produced vehicles, an estimation method using a vehicle motion model has been proposed.

In Patent Document 1, a yaw rate sensor and a vehicle speed sensor are used to estimate the amount of change in the rotational posture of the vehicle, and the amount of change in the rotational posture of the vehicle with respect to a reference line extending along the road extracted from the camera image is calculated. An invention is disclosed for a road curve information detection device that calculates curve information of a reference line as road curve information based on the difference between the amount of change in the rotational attitude of a vehicle with respect to a predetermined reference.

Patent Document 2 discloses an invention of a vehicle slip angle detection device that estimates a vehicle slip angle based on vehicle speed, yaw rate and lateral acceleration.

In Patent Document 3, the yaw angle of the vehicle is detected from image information around the vehicle acquired by an imaging device such as an in-vehicle camera, and the yaw angle is detected from information obtained from a steering angle sensor and a yaw rate sensor based on a vehicle motion model. An invention of a vehicle physical quantity estimating device that calculates an estimated value and corrects an estimated slip angle value based on a vehicle motion model using an offset error calculated from the estimated yaw angle value and a yaw angle detected by an in-vehicle camera. disclosed.

JP-A-10-318742 JP-A-11-248455 JP 2010-274701 A

However, in the invention described in Patent Document 1, the yaw angle change is calculated without considering the time series continuity, so noise is likely to occur in the estimated yaw rate value, and the offset error of the yaw rate sensor There is a problem that the accuracy of estimation decreases. Further, since the vehicle lateral speed is not taken into account, the accuracy of yaw rate estimation may be further reduced when the vehicle lateral speed changes abruptly at an intersection or the like.

Since the invention described in Patent Document 2 applies a low-pass filter and a high-pass filter according to the characteristics of each sensor, there is a possibility that the output of the estimated value may be delayed. Since the output results of the acceleration sensor and the yaw rate sensor have an offset error that is difficult to remove even with the above filter, there is a problem that the accuracy of the side slip angle estimation is lowered.

The invention described in Patent Document 3 improves the accuracy of slip angle estimation by correcting the vehicle motion model with the yaw rate. In addition, the vehicle motion model used is susceptible to the road surface, tires, etc., and needs to be set according to the environment and vehicle specifications.

The present invention has been made in view of the above problems, and the vehicle lateral speed can be accurately determined from information about the vehicle surroundings detected by a sensing device provided in the vehicle, the vehicle speed detected by the vehicle speed sensor, and the yaw rate detected by the yaw rate sensor. An object of the present invention is to realize a vehicle lateral velocity estimator capable of estimating.

In order to achieve the above object, a vehicle lateral speed estimating device according to claim 1 detects a lateral position deviation between a target route and the vehicle, and a lateral position deviation between the target route and the target route, based on information about the vehicle's surroundings acquired by a sensing device provided in the vehicle. a position measuring unit that calculates a yaw angle deviation of the vehicle; movement of a target point on the target route determined by the lateral position deviation and the yaw angle deviation calculated by the position measuring unit; and motion of the vehicle including sideslip. The vehicle lateral speed of the vehicle is estimated from measured values including the lateral position deviation, the yaw angle deviation, the vehicle longitudinal velocity obtained from the vehicle speed sensor, and the vehicle yaw rate obtained from the yaw rate sensor, using an equation representing the relationship of and a calculation unit.

The vehicle lateral speed estimating device according to claim 2 is the vehicle lateral speed estimating device according to claim 1, wherein the expression representing the relationship is obtained from the vertical component of the moving speed of the target point with respect to the longitudinal direction of the vehicle. , an equation expressing the relationship between the result obtained by subtracting the amount of change in the lateral position deviation from the lateral velocity of the sensing device with respect to the vehicle center of gravity position due to the yaw motion of the vehicle, and the vehicle lateral velocity of the vehicle. there is

The vehicle lateral speed estimating device according to claim 3 is the vehicle lateral speed estimating device according to claim 2, wherein the expression expressing the relationship is the vehicle longitudinal speed, the vehicle yaw rate and the lateral position deviation. It further includes an equation that expresses the relationship between the result of subtracting the product of , and correcting it using the yaw angle deviation, and the moving speed of the target point.

The vehicle lateral speed estimating device according to claim 4 is the vehicle lateral speed estimating device according to any one of claims 1 to 3, wherein the sensing device comprises an image around the vehicle acquired by an imaging device. It is either an image analysis device that analyzes information, LIDAR, or sonar.

Further, the vehicle lateral speed estimating device according to claim 5 is the vehicle lateral speed estimating device according to any one of claims 1 to 4, wherein the calculating unit, based on the expression representing the relationship, Calculated using a prediction unit that predicts the state quantity of the vehicle at the next time from the state quantity of the vehicle including the yaw rate and the vehicle lateral speed estimated last time, and a predetermined observation equation, Differences between the lateral position deviation, the yaw angle deviation, the longitudinal velocity of the vehicle, and the vehicle yaw rate corresponding to the predicted state quantity of the vehicle at the next time, and the respective measured values, and the previous estimated an updating unit that corrects the state quantity of the vehicle predicted by the prediction unit based on the state quantity of the vehicle.

According to the present invention, there is provided a vehicle lateral speed estimating device capable of accurately estimating the vehicle lateral speed from the information around the vehicle detected by a sensing device provided in the vehicle, the vehicle speed detected by the vehicle speed sensor, and the yaw rate detected by the yaw rate sensor. There is an effect that it can be realized.

1 is a block diagram showing an example of the configuration of a vehicle lateral speed estimating device according to an embodiment of the present invention; FIG. It is the schematic which showed the coordinate system in embodiment of this invention. FIG. 4 is an explanatory diagram showing an example of the relationship between the position of an imaging device, the center of gravity of a vehicle, lateral position deviation, and yaw angle deviation; FIG. 4 is an explanatory diagram showing the relationship between the motion of the vehicle, including sideslip, and the target route; FIG. 2 is an explanatory diagram showing an outline of inputs and outputs of the vehicle lateral speed estimating device according to the embodiment of the present invention; It is an example of a functional block diagram of an arithmetic device of the vehicle lateral speed estimation device according to the embodiment of the present invention. It is an example of the functional block diagram of a prior estimation part. It is an example of a functional block diagram of a filtering unit. FIG. 4 is a schematic diagram showing an example of vehicle turning when the lateral position deviation and the yaw angle deviation are large, such as when turning right or left at an intersection; FIG. 10 is a schematic diagram of a case in which it is difficult to obtain information on a long distance in the traveling direction of a vehicle due to a string of traffic jams;

BEST MODE FOR CARRYING OUT THE INVENTION Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. As shown in FIG. 1, the vehicle lateral speed estimating device 10 according to the present embodiment includes a storage device 18 for storing data required for calculation by the calculation device 14 and calculation results by the calculation device 14, which will be described later, and a vehicle. a position measuring device 20 that calculates the lateral position deviation between the target route and the vehicle and the yaw angle deviation of the vehicle from the information around the vehicle obtained by the sensing device; An input device 12 to which the longitudinal velocity of the vehicle detected by the vehicle speed sensor 24 and the yaw rate detected by the yaw rate sensor 26 are input, and the lateral velocity of the vehicle based on the input data input from the input device 12 and the data stored in the storage device 18. and a display device 16 such as a CRT or LCD for displaying the vehicle lateral velocity calculated by the computing device 14 .

The in-vehicle sensing device according to the present embodiment is, for example, an imaging device such as an in-vehicle camera, LIDAR (Light Detection and Ranging), or sonar. When an imaging device such as an in-vehicle camera is used as an in-vehicle sensing device, for example, image information around the vehicle acquired by the imaging device is analyzed to detect white lines on the road. When the LIDAR is used as an in-vehicle sensing device, for example, white lines on a road are detected from the scattered light of pulsed laser beams irradiated around the vehicle. When a sonar is used as an in-vehicle sensing device, for example, the white line is identified by utilizing the difference in ultrasonic reflectance between the asphalt road surface and the painted white line.

Next, a coordinate system related to the behavior of vehicle 200 is defined as shown in FIG. The earth coordinate system 204 is a coordinate system in which the gravitational acceleration direction and z _e are parallel to the earth plane, and y _e faces the north direction. The road surface coordinate system 206 is a coordinate system in which z _r is oriented in a direction perpendicular to the road surface through the center of gravity of the vehicle 200 and x _r is oriented in the direction of travel of the vehicle. A vehicle coordinate system 208 is a coordinate system fixed on the vehicle body springs, and z _v is directed vertically upward of the vehicle body, and x _v is directed in the traveling direction of the vehicle body.

In the present embodiment, the vehicle lateral speed of vehicle 200 is calculated using information around the vehicle detected by an in-vehicle sensing device. In the present embodiment, when _calculating the vehicle _{lateral speed} , as shown in FIG. Assume a lateral position deviation e ₁ and a yaw angle deviation e ₂ . Note that the sensor position P _s is located at a distance (the offset distance between the vehicle center of gravity P _r and the sensor position P _s ) l _s in the forward direction of the vehicle 200 from the vehicle center of gravity P _r , and an imaging device such as an in-vehicle camera , LIDAR sensors or sonar. As shown in FIG. 3, the amount of movement of the vehicle 200 in the traveling direction with respect to the sensor position P _s is x _S , and the amount of lateral movement of the vehicle 200 with respect to the sensor position P _s is y _s . be.

Next, FIG. 4 shows the relationship between the motion of the vehicle 200 including sideslip and the target route 220, which is used when estimating the vehicle lateral speed. In general, the vehicle 200 is skidding with respect to the road surface and involves motion with vehicle lateral velocity. With respect to the behavior of the vehicle 200, the target position _Pt on the driving lane is moving with only longitudinal velocity and no vehicle lateral velocity. By using this feature to derive the difference between the motion of the vehicle 200 and the motion of the target position _Pt , it is possible to derive the conditional expression necessary for estimating the vehicle lateral velocity.

In this embodiment, variables are set as shown in FIGS. Assuming that the target position P _t on the target path 220 has only a tangential velocity component U _t , the velocity vector of the target position P _t is V _t =[U _t 0] ^T , and the target position P _t is P _t =[ x _t y _t ] ^T , and the yaw angle tangential to the x-axis direction of the earth coordinate system is defined as ψ _t . Furthermore, assuming that the vehicle 200 has a vehicle lateral velocity (has a vehicle body slip angle) as well as the longitudinal direction, the origin of the road surface coordinate system 206 is P _r =[x _r y _r ] ^T , the yaw angle is ψ _r , and the vehicle 200 is Let ν _r =[U _r V _r ] ^T be the velocity component in each axial direction, as shown in FIG. In this embodiment, in order to facilitate the relationship with sensor information measured with reference to the vehicle coordinate system 208 later, the reference coordinate system of the deviation is the road surface coordinate system 206, and the yaw angle deviation is ψ _e =ψ _t − Define _ψr .

FIG. 5 is an explanatory diagram showing an outline of inputs and outputs of the vehicle lateral speed estimation device 10 according to the present embodiment. The lateral position deviation e ₁ and the yaw angle deviation e ₂ from the position measuring device 20 are input to the computing device 14 constituting the vehicle lateral speed estimating device 10 . Further, the longitudinal velocity _Ur of the vehicle 200 (hereinafter abbreviated as "vehicle longitudinal velocity _Ur ") is obtained from the vehicle speed sensor 24, and the yaw rate _Rr detected by the yaw rate sensor 26 is obtained from the arithmetic unit 14. is entered in

The arithmetic unit 14 estimates and outputs the vehicle lateral velocity V _r from the lateral position deviation e ₁ , the yaw angle deviation e ₂ , the vehicle longitudinal velocity U _r and the yaw rate R _r by arithmetic processing to be described later.

If the deviation vector of the apparent target position P _t from the road surface coordinate system 206 is P _e =[x _e y _e ], the differentiation of the deviation vector P _e is expressed by the following two equations.

In the position measuring device 20, the sensor position P is determined by analyzing the image information around the vehicle 200 acquired by an imaging device such as an in-vehicle camera by a known method, or from the information around the vehicle acquired using LIDAR or sonar. A lateral position deviation e ₁ and a yaw angle deviation e ₂ with respect to the target point P _t with respect to _s are extracted. Since the imaging device, LIDAR, and sonar are types of sensors, the lateral position deviation e ₁ and the yaw angle deviation e ₂ extracted from the information acquired by these devices are used as sensor information acquired by the sensors. Furthermore, the following three equations are obtained by modifying the above equation using the vehicle longitudinal velocity U _r detected by the vehicle speed sensor 24 and the yaw rate R _r detected by the yaw rate sensor as sensor information.

Furthermore, the following equations (1) and (2) are obtained by arranging the above three equations with respect to the vehicle lateral velocity V _r and the appropriate yaw rate R _r .

The above equation (1) indicates that the vehicle lateral velocity V _r is the vertical component of the vehicle longitudinal velocity U _r with respect to the target path 220, in other words, the y-coordinate component of the vehicle longitudinal velocity _Ur in the vehicle coordinate system 208, U _t sine ₂ Then, the amount of change in the lateral position deviation e ₁ from the target path (differential value of e ₁ ) and the lateral velocity R _r l _s of the sensor of the position measuring device 20 with respect to the vehicle center of gravity position P _r associated with the yaw motion of the vehicle 200 are It shows that it matches what you subtracted. Note that the above equation (1) is obtained from the vertical component of the moving speed of the target point P _t with respect to the longitudinal direction of the vehicle, the amount of change in the lateral position deviation e ₁ , and the position measuring device 20 is an example of an equation expressing the relationship between the result of subtracting the lateral velocity of 20 sensors and the vehicle lateral velocity V _r .

In the above equation (2), the moving speed U _t of the target point P _t on the target path 220 is obtained by subtracting R _r e ₁ obtained by multiplying the yaw rate R _r of the vehicle 200 and the lateral position deviation e ₁ from the vehicle longitudinal speed U _r . , the turning radius is corrected, and further _divided by the cosine _cos2 of the yaw angle deviation e2. Note that the above equation (2) is obtained by subtracting the product of the yaw rate R _r and the lateral position deviation e ₁ from the vehicle longitudinal velocity U _r and correcting it using the yaw angle deviation e ₂ , and the target point P It is an example of an expression expressing a relationship equal to the moving speed of _t .

By estimating the unknown variable that best fits the data acquired by the position measuring device 20, the vehicle speed sensor 24, and the yaw rate sensor 26 using the above equations (1) and (2), the vehicle lateral velocity V _r estimation becomes possible.

Although the above equations (1) and (2) do not approximate the motion and take a nonlinear form, by directly using the above equations (1) and (2) and applying them to the nonlinear Kalman filter, , accurate estimation is possible even when the vehicle 200 greatly deviates from the target route 220 . An extended Kalman filter may also be applied by local linearization with respect to the state variables.

In the present embodiment, the relationship between the target route 220 and the motion of the vehicle 200 is used to define the state equation f(x), which is utilized in the pre-estimation and filtering steps in the computing device 14 . Details of the pre-estimation and filtering steps are described below.

First, information on lateral position deviation e ₁ and yaw angle deviation e ₂ with respect to target path 220 obtained from position measuring device 20 , vehicle longitudinal velocity U _r obtained from vehicle speed sensor 24 , and yaw rate R _r obtained from yaw rate sensor 26 . The state equation f(x) when estimating the vehicle lateral velocity _Vr using and is defined as follows.

The state equation f(x) in this case satisfies the following formula (3) with respect to the state quantity x(t) at time t.

A state equation f(x) that satisfies the relationship of the above equation (3) with respect to the above state quantity x(t) is formulated as follows.

Equation (4) is a differential equation showing the relationship between the motion of target route 220 and vehicle 200 .
Also, in this embodiment, the system noise for each state is defined as follows.

Next, we define the observables as follows.

An observation equation h(x) that satisfies y=h(x) defines correspondence between a state quantity (right side) and an observation quantity (left side) as in the following equations.

However, the observation noise for each observable is defined as follows.

In the Kalman filter, each state equation f(x) including equation (4) is discretized and used. Therefore, the input-output relationship of the function f(x) is used in the form of x(k)=f(x(k-1)) for times t=k, k-1.

FIG. 6 is an example of a functional block diagram of the arithmetic device 14 of the vehicle lateral speed estimation device 10 according to the present embodiment. As shown in FIG. 6, the arithmetic unit 14 uses the state equation f(x) to estimate the state based on the relationship between the driving lane, which is the target route 220, and the plane motion of the vehicle 200 including skidding. , a prior estimation unit 102 that estimates corresponding sensor information using the observation equation h(x), and a filtering unit 104 that corrects the prior estimation value output by the prior estimation unit 102 using the sensor information. .

Various Kalman filters, including linear and nonlinear methods, have been proposed. In this embodiment, based on the fact that the equation of state f(x) is nonlinear as described above, an example of utilizing a nonlinear Kalman filter will be shown. Among them, an unscented Kalman filter, which does not require linearization of the state equation f(x) and has a relatively small computational load, is adopted as an example. If the state equation f(x) is linear, a linear Kalman filter may be used, or a nonlinear Kalman filter other than the unscented Kalman filter may be used.

FIG. 7 is an example of a functional block diagram of the prior estimation unit 102. As shown in FIG. As shown in FIG. 7, the prior estimation unit 102 includes a first unscented transform unit 102A and a second unscented transform unit 102B.

The first unscented transform unit 102A performs a first unscented transfer to update the state quantity based on the equation of state f(x), and outputs the mean of x and the covariance matrix of x.

The second unscented transforming unit 102B uses the average of the state quantity x and the covariance matrix of the state quantity x output by the first unscented transforming unit 102A, and transforms it into the corresponding observable quantity according to the observation equation h(x). Perform a second unscented transformation to

The purpose of the unscented transformation is to accurately obtain the mean and covariance matrix of the observed quantity y below in the transformation by a certain nonlinear function y=f(x).

Therefore, this embodiment is characterized by approximating the probability density function using 2n+1 samples (sigma points) corresponding to the mean and standard deviation.

As shown in FIG. 7, first unscented transforming section 102A includes sigma point weighting factor section 102A1, function transforming section 102A2, and U transforming section 102A3. Second unscented transforming section 102B includes sigma point weighting factor section 102B1, function transforming section 102B2, and U transforming section 102B3.

Sigma point X _i : i=0, 1, 2, . is selected as

However, the scaling factor κ is chosen such that κ≧0. Also, the weight for the sigma point is defined as follows.

The conversion of each sigma point by the nonlinear function f(x) in the function conversion section 102A2 of the first unscented conversion section 102A is as follows. The following is the conversion by the state equation f(x) in the function conversion unit 102A2 of the first unscented conversion unit 102A, but the observation equation h(x) in the function conversion unit 102B2 of the second unscented conversion unit 102B is used. The transformation yields the observed value.

U transforming section 102A3 of first unscented transforming section 102A uses the value transformed by function f(x) and the aforementioned weighting factor to obtain the following average value of state quantity x and state quantity x: Calculate the covariance matrix. Q _n in the following formula is system noise.

U conversion section 102B3 of second unscented conversion section 102B performs the following calculations.

FIG. 8 is an example of a functional block diagram of the filtering unit 104. As shown in FIG. The filtering unit 104 compares the difference between the observed value corresponding to the predicted value of the state quantity calculated by the U conversion unit 102B3 and the actually observed value, and corrects the predicted value of the state quantity. I do.

The process of feeding back the predicted value of the state quantity with the actually observed value is called Kalman Gain, which is calculated by the following equation. Note that R _n in the following equation is observation noise.

Next, using this Kalman gain, processing for correcting the advance prediction value of the state quantity is performed as follows.

The vehicle lateral velocity V _r is estimated by repeating the above-described processing of the pre-estimating unit 102 and the filtering unit 104 for each time step.

When estimating the vehicle lateral velocity V _r , x(k) and P _x (k) output from the filtering unit 104 are input to the previous estimation unit 102 as the previous values, and are subjected to two-stage unscented transformation as described above. Calculate the prior predicted value of the state quantity and the corresponding observed value. The calculated predicted value of the state quantity is corrected by the filtering unit 104 using the Kalman gain and the latest observed value actually observed. x(k) and P _x (k) output by the filtering unit 104 are temporarily held in the storage device 18, and x(k) and P _x (k) held in the storage device 18 are pre-estimated as previous values. input to section 102 .

The vehicle lateral velocity V _r is estimated by repeating the calculation of the advance predicted value of the state quantity and the correction with the latest actually observed value.

According to the vehicle lateral speed estimating apparatus 10 according to the present embodiment, as shown in equation (1), the vehicle lateral speed V _r of the vehicle 200 is obtained from the vertical component of the moving speed of the target point P _t with respect to the vehicle longitudinal direction. Based on the result of subtracting the amount of change in the lateral position deviation e ₁ and the vehicle lateral velocity detected by the sensor of the position measuring device 20 with respect to the vehicle center of gravity position due to the yaw motion of the vehicle 200, the vehicle lateral velocity V _r is calculated. I'm guessing.

Further, vehicle lateral speed estimating apparatus 10 according to the present embodiment calculates moving speed U _t of target point P _t on target route 220 from vehicle longitudinal speed U _r as shown in equation (2). The turning radius is corrected by subtracting R _r e ₁ obtained by multiplying the appropriate yaw rate R _r by the lateral position deviation e ₁ , and is estimated by dividing it by the cosine cos ₂ of the yaw angle deviation e ₂ . .

Since neither equations ( ₁ ) nor ( ₂ ) approximates the motion, as shown in FIG. Even if the vehicle 200 greatly deviates from the target route 220, the lateral position deviation e ₁ between the target route 220 and the vehicle 200 and the yaw angle deviation e ₂ of the vehicle can be calculated from the information around the vehicle acquired by the sensing device provided in the vehicle. The vehicle lateral velocity V _r can be accurately estimated from the data acquired by the position measuring device 20 , the vehicle speed sensor 24 and the yaw rate sensor 26 .

In estimating the vehicle lateral velocity V _r using equations (1) and (2), the curvature ρ of the road is unnecessary, and the lateral position deviation e ₁ and the yaw angle deviation e ₂ are used. _When calculating the curvature ρ, it is necessary to _obtain information on the road alignment far in the direction in which the vehicle 200 is traveling. It is sufficient to detect by

Therefore, according to the vehicle lateral speed estimation device 10 according to the present embodiment, at night when it is difficult to obtain an image, or as shown in FIG. Even if the field of view is obstructed and it is not possible to acquire the information of the road alignment in the distance, the lateral position deviation _e1 and the yaw angle deviation _e2 of the vehicle 200A can be detected by detecting the white line directly under the vehicle 200A with LIDAR or the like. , the vehicle lateral velocity V _r can be accurately estimated.

As described above, the vehicle lateral speed estimation device 10 according to the present embodiment uses the lateral position deviation e ₁ and the yaw angle deviation e ₂ , which are information about the periphery of the vehicle 200 detected by the sensing device included in the vehicle 200, the vehicle speed The vehicle lateral velocity V _r can be accurately estimated from the vehicle longitudinal velocity U _r detected by the sensor 24 and the yaw rate R _r detected by the yaw rate sensor 26 .

10 vehicle lateral speed estimation device 12 input device 14 calculation device 16 display device 18 storage device 20 position measurement unit 22 imaging device 24 vehicle speed sensor 26 yaw rate sensor 102 prior estimation unit 102A first unscented conversion unit 102A1 sigma point weighting coefficient unit 102A2 function Conversion unit 102A3 U conversion unit 102B Second unscented conversion unit 102B1 Sigma point weight coefficient unit 102B2 Function conversion unit 102B3 U conversion unit 104 Filtering unit 200 Vehicle 204 Earth coordinate system 206 Road surface coordinate system 208 Vehicle coordinate system 220 Target route

Claims (5)

a position measuring unit that calculates a lateral positional deviation between a target route and the vehicle and a yaw angle deviation between the target route and the vehicle from information around the vehicle acquired by a sensing device provided in the vehicle;
Using an equation expressing the relationship between the movement of the target point on the target path determined by the lateral position deviation and the yaw angle deviation calculated by the position measuring unit and the motion of the vehicle including sideslip, the lateral position deviation a calculation unit for estimating the vehicle lateral speed of the vehicle from actual measurements including the yaw angle deviation, the vehicle longitudinal speed obtained from the vehicle speed sensor, and the vehicle yaw rate obtained from the yaw rate sensor;
vehicle lateral speed estimator including The expression expressing the relationship is obtained from the vertical component of the moving speed of the target point with respect to the longitudinal direction of the vehicle, the amount of change in the lateral position deviation, and the lateral velocity of the sensing device with respect to the vehicle center of gravity position due to the yaw motion of the vehicle. 2. The vehicle lateral speed estimating device according to claim 1, further comprising an equation representing a relationship in which the result of subtracting is equal to the vehicle lateral speed of the vehicle. The expression representing the relationship is obtained by subtracting the product of the vehicle yaw rate and the lateral position deviation from the vehicle longitudinal velocity, correcting the result using the yaw angle deviation, and the movement speed of the target point. 3. The vehicle lateral speed estimator of claim 2, further comprising an equation representing an equality relationship. The vehicle lateral speed estimation device according to any one of claims 1 to 3, wherein the sensing device is an image analysis device that analyzes image information around the vehicle acquired by an imaging device, LIDAR, or sonar. . The computing unit predicts the state quantity of the vehicle at the next time based on the state quantity of the vehicle including the yaw rate and the vehicle lateral speed estimated last time based on the expression representing the relationship. ,
the lateral position deviation, the yaw angle deviation, the vehicle longitudinal velocity, and the vehicle yaw rate, which correspond to the predicted state quantity of the vehicle at the next time, which are calculated using a predetermined observation equation; an updating unit that corrects the state quantity of the vehicle predicted by the prediction unit based on the difference from the actual measurement value and the state quantity of the vehicle that was previously estimated;
The vehicle lateral speed estimating device according to any one of claims 1 to 4, comprising:

Images