JP7318521B2 - Estimation device, estimation method, estimation program - Google Patents

Description

The present disclosure relates to an estimation technique for estimating state quantities including vehicle positions.

2. Description of the Related Art Conventionally, a vehicle is equipped with an optical sensor that outputs an optical image by receiving a beam reflection from a target observed by irradiating the beam to the outside world. The state quantity of the vehicle is estimated based on an optical image output from such an optical sensor by beam irradiation in a predetermined irradiation range.

Now, in the technology disclosed in Patent Document 1, the driving situation is determined based on the scanning result by the LIDAR sensor as an optical sensor, and the direction of the scanning range, which is the irradiation range, is controlled so as to depend on the determined situation. there is

Japanese Patent Publication No. 2019-507326

However, when the optical image output by the optical sensor is used for estimating the state quantity, the accuracy of estimating the state quantity fluctuates if the orientation of the scanning range is changed as in the technique disclosed in Patent Document 1. The principle of variation in estimation accuracy will be described.

As shown in FIG. 28, the error Emo that occurs in the range measurement of the target in the beam orthogonal direction of the optical sensor can be coordinate-converted into a front-back observation error (not shown) in the front-back direction X and a left-right observation error Eoy in the left-right direction Y. . These front-back observation error and left-right observation error Eoy vary independently according to the beam steering angle in the scanning range. Such a phenomenon similarly occurs with respect to errors occurring in target range finding in the beam parallel direction.

Based on this knowledge, when the orientation of the scanning range is changed depending on the driving situation, the distribution of the front-back observation error and the left-right observation error Eoy changes along with the orientation change. As a result, in the state quantity estimation based on the optical image affected by the front-back observation error and the left-right observation error Eoy, the estimation error changes in the front-back direction X and the left-right direction Y due to the orientation change.

An object of the present disclosure is to provide an estimating device that improves the accuracy of estimating state quantities. Another object of the present disclosure is to provide an estimation method that increases the accuracy of estimating state quantities. Yet another object of the present disclosure is to provide an estimation program that improves the accuracy of estimating state quantities.

Technical means of the present disclosure for solving the problems will be described below. It should be noted that the symbols in parentheses described in the claims and this column indicate the correspondence with specific means described in the embodiments described in detail later, and limit the technical scope of the present disclosure. not something to do.

A first aspect of the present disclosure is
The state quantity (Z) including the position of the vehicle (4) equipped with the optical sensor (30) that receives the beam reflection from the target (6) observed by the beam irradiation to the outside and outputs the optical image (Is) is , an estimating device (1) for estimating
Based on the optical image output from the optical sensor , the distance value to the target is digitized according to the beam irradiation in the irradiation range (Θs) that spreads laterally from the reference bearing (Bs) along the longitudinal direction (X). , a state quantity estimator (100) for estimating the state quantity;
An estimation error that occurs in the estimation of the state quantity, the estimation error for the observation residual (Δox, Δoy) due to the matching process of the map information (Im) for the optical image, the front-back estimation error (Eex) in the front-back direction (X) and an estimated error acquisition unit (120) that acquires a left-right estimated error (Eey) in the left-right direction (Y);
Observation errors occurring in the observation of the target by the optical sensor, which are observation errors corresponding to the observation residuals due to the matching process , are the front-back observation error (Eox) in the front-back direction (X) and the left-right observation error in the left-right direction (Y) ( Eoy) in association with the beam steering angle (θ) for each irradiation timing (t) in the irradiation range;
If the larger of the front-back estimation error and the left-right estimation error is defined as the large-side estimation error, and the smaller one of the front-back observation error and the left-right observation error is defined as the small-side observation error, then the large-side estimation error and the small-side observation error are defined as A beam adjustment unit ( 180).

A second aspect of the present disclosure is
The state quantity (Z) including the position of the vehicle (4) equipped with the optical sensor (30) that receives the beam reflection from the target (6) observed by the beam irradiation to the outside and outputs the optical image (Is) is , an estimation method performed by a processor (12) to estimate
Based on the optical image output from the optical sensor , the distance value to the target is digitized according to the beam irradiation in the irradiation range (Θs) that spreads laterally from the reference bearing (Bs) along the longitudinal direction (X). , a state quantity estimation process (S10) for estimating the state quantity;
An estimation error that occurs in the estimation of the state quantity, the estimation error for the observation residual (Δox, Δoy) due to the matching process of the map information (Im) for the optical image, the front-back estimation error (Eex) in the front-back direction (X) and an estimation error acquisition process (S20) for acquiring a left-right estimation error (Eey) in the left-right direction (Y);
Observation errors occurring in the observation of the target by the optical sensor, which are observation errors corresponding to the observation residuals due to the matching process , are the front-back observation error (Eox) in the front-back direction (X) and the left-right observation error in the left-right direction (Y) ( Eoy) is acquired in association with the beam steering angle (θ) for each irradiation timing (t) in the irradiation range (S40);
If the larger of the front-back estimation error and the left-right estimation error is defined as the large-side estimation error, and the smaller one of the front-back observation error and the left-right observation error is defined as the small-side observation error, then the large-side estimation error and the small-side observation error are defined as A beam adjustment process ( S50).

A third aspect of the present disclosure is
The state quantity (Z) including the position of the vehicle (4) equipped with the optical sensor (30) that receives the beam reflection from the target (6) observed by the beam irradiation to the outside and outputs the optical image (Is) is , an estimation program stored in a storage medium (10) and comprising instructions for execution by a processor (12) to estimate,
the instruction is
Based on the optical image output from the optical sensor , the distance value to the target is digitized according to the beam irradiation in the irradiation range (Θs) that spreads laterally from the reference bearing (Bs) along the longitudinal direction (X). , a state quantity estimation process (S10) for estimating a state quantity;
As an estimation error that occurs in the estimation of the state quantity, the error for the observation residual (Δox, Δoy) due to the matching process of the map information (Im) with respect to the optical image, the front-back estimation error (Eex) in the front-back direction (X) and the left and right an estimation error acquisition process (S20) for acquiring a left-right estimation error (Eey) in the direction (Y);
Observation errors occurring in the observation of the target by the optical sensor, which are observation errors corresponding to the observation residuals due to the matching process , are the front-back observation error (Eox) in the front-back direction (X) and the left-right observation error in the left-right direction (Y) ( Eoy) is acquired in association with the beam steering angle (θ) for each irradiation timing (t) in the irradiation range (S40);
If the larger of the front-back estimation error and the left-right estimation error is defined as the large-side estimation error, and the smaller one of the front-back observation error and the left-right observation error is defined as the small-side observation error, then the large-side estimation error and the small-side observation error are defined as A beam adjustment process ( S50).

The beam patterns of the first to third aspects are adjusted to a directivity pattern centered at a specific beam steering angle in the illumination range of the optical sensor extending laterally from the reference orientation along the longitudinal direction. Here, according to a specific beam steering angle, the large estimation error of the front-back estimation error and the left-right estimation error occurring in the state quantity estimation, and the small-side observation error of the front-back observation error and the left-right observation error occurring in the target observation are: same direction. According to this, the beam irradiation can be concentrated on the beam steering angle with a high target observation probability that can reduce the large-side estimation error. Therefore, in the state quantity estimation based on the optical image output from the optical sensor according to the beam irradiation according to the directivity pattern, it is possible to reduce the large-side estimation error and improve the estimation accuracy of the state quantity.

It is a schematic diagram for demonstrating the optical sensor mounted in a vehicle with the estimation apparatus by one Embodiment. It is a block diagram which shows the whole structure of the estimation apparatus by one Embodiment. 1 is a block diagram showing a specific configuration of an estimation device according to one embodiment; FIG. FIG. 4 is a schematic diagram for explaining the optical sensor of FIGS. 1 to 3; FIG. 4 is a block diagram showing in detail a state quantity estimation block of FIG. 3; FIG. FIG. 4 is a detailed block diagram of an observation error acquisition block and a beam adjustment block of FIG. 3; FIG. 7 is a schematic diagram for explaining a shortest distance specifying sub-block in FIG. 6; FIG. 7 is a schematic diagram for explaining a ranging error acquisition sub-block in FIG. 6; FIG. 7 is a schematic diagram for explaining a ranging error acquisition sub-block in FIG. 6; FIG. 7 is a schematic diagram for explaining an actual measurement error acquisition sub-block in FIG. 6; FIG. 7 is a schematic diagram for explaining an actual measurement error acquisition sub-block in FIG. 6; FIG. 7 is a schematic diagram for explaining an observation error smoothing sub-block in FIG. 6; FIG. 7 is a schematic diagram for explaining an observation error smoothing sub-block in FIG. 6; FIG. 7 is a schematic diagram for explaining a gain setting sub-block in FIG. 6; FIG. FIG. 7 is a schematic diagram for explaining a gain setting sub-block and a pattern updating sub-block in FIG. 6; FIG. 7 is a schematic diagram for explaining a gain setting sub-block and a pattern updating sub-block in FIG. 6; FIG. 7 is a schematic diagram for explaining a gain limiting sub-block in FIG. 6; FIG. FIG. 7 is a schematic diagram for explaining a gain limiting sub-block in FIG. 6; FIG. FIG. 7 is a schematic diagram for explaining a pattern update sub-block in FIG. 6; FIG. 7 is a schematic diagram for explaining a pattern update sub-block in FIG. 6; FIG. 7 is a schematic diagram for explaining a pattern update sub-block in FIG. 6; FIG. 7 is a schematic diagram for explaining a pattern update sub-block in FIG. 6; FIG. 7 is a schematic diagram for explaining a pattern update sub-block in FIG. 6; 1 is a flow chart showing the flow of an estimation method according to one embodiment; 25 is a flowchart showing in detail S10 of FIG. 24; 25 is a flowchart showing in detail S40 of FIG. 24; 25 is a flowchart showing in detail S50 of FIG. 24; It is a schematic diagram for demonstrating the conventional problem.

An embodiment will be described below with reference to the drawings.

As shown in FIGS. 1 and 2, an estimation device 1 according to one embodiment is mounted on a vehicle 4 together with a map unit 2 and a sensor unit 3. FIG. The vehicle 4 is, for example, an advanced driving assistance vehicle or an automatic driving vehicle, which runs based on the motion estimation result by the estimation device 1 . Of the horizontal directions of the vehicle 4 on the horizontal plane, the straight-ahead direction of the vehicle 4 is defined as the front-rear direction X. As shown in FIG. A lateral direction perpendicular to the straight-ahead direction of the vehicle 4 is defined as a left-right direction Y among the horizontal directions of the vehicle 4 on the horizontal plane.

As shown in FIGS. 2 and 3, the map unit 2 includes at least one type of non-transitional physical storage medium, such as a semiconductor memory, a magnetic medium, an optical medium, etc., for non-temporarily storing or storing the map information Im. (non-transitory tangible storage medium). The map unit 2 may be a database of locators used for advanced driver assistance or automated driving control of the vehicle 4 . The map unit 2 may be the database of a navigation device for navigating the operation of the vehicle 4 . The map unit 2 may be the database of a navigation device for navigating the operation of the vehicle 4 . The map unit 2 may be configured by combining a plurality of types of these databases.

The map unit 2 acquires and stores the latest map information Im through communication with an external center, for example. The map information Im is two-dimensional or three-dimensional data as information representing the driving environment of the vehicle 4 . The map information Im includes at least road information representing at least one of the position, shape, road surface condition, and the like of the road itself. The map information Im may include, for example, label information representing at least one of the positions and shapes of road markings and lane markings. The map information Im may include, for example, structure information representing at least one of the positions and shapes of buildings facing roads and traffic lights.

As shown in FIGS. 1-3, the sensor unit 3 includes at least an optical sensor 30 . The optical sensor 30 is a so-called LIDAR (Light Detection and Ranging/Laser Imaging Detection and Ranging) that can be used for motion estimation of the vehicle 4, for example. The optical sensor 30 receives beam reflection from the target 6 observed by beam irradiation to the outside of the vehicle 4 and outputs an optical image Is. The optical sensor 30 of the present embodiment is arranged in the center of the front portion of the vehicle 4, and scans the external environment from the front to the left and right sides.

As shown in FIG. 3, the optical sensor 30 has a beam element 300, an imaging element 301 and an imaging circuit 302, respectively. An imaging circuit 302 controls the beam element 300 and the imaging element 301 . Specifically, the imaging circuit 302 causes the beam element 300 to intermittently irradiate a laser beam toward the outside of the vehicle 4 in the form of pulsed light of substantially constant intensity, as indicated by dashed arrows in FIGS. At this time, the irradiation range Θs, which is the scanning range of the optical sensor 30, is set to a range extending laterally from the reference bearing Bs along the front-rear direction X. As shown in FIG. In particular, in the irradiation range Θs of this embodiment, the critical end direction Bc is set at the same or different setting angles on both left and right sides around the beam steering axis As centering on the reference direction Bs facing forward of the vehicle 4. range is given. The beam steering axis As, which is the starting point of the irradiation range Θs in the optical sensor 30, is set in the vertical direction of the vehicle 4 on the horizontal plane, or in a direction inclined with respect to the vertical direction.

The imaging circuit 302 switches the beam steering angle θ that determines the beam irradiation direction around the beam steering axis As within the irradiation range Θs at each intermittent irradiation timing t at constant time intervals. At this time, as shown in FIG. 4, the imaging circuit 302 regulates the beam steering angle θ at equal intervals or unequal intervals at each irradiation timing t, so that the irradiation density of the beam within the irradiation range Θs is equal or unequal. Distribute evenly. Therefore, the imaging circuit 302 controls the irradiation density distribution pattern determined by the interval between the beam steering angles θ at each irradiation timing t as the beam pattern Ps of the optical sensor 30 . In this embodiment, the beam steering angle θ in the irradiation range Θs has a negative (minus) value in the area to the left of the reference azimuth Bs, which is zero (0), and a positive (plus) value in the area to the right of the reference azimuth Bs. is defined as the value of Also, in order to facilitate understanding of the explanation, FIG. 4 illustrates only part of the beam steering angle θ at each irradiation timing t.

The image pickup circuit 302 shown in FIG. 3 sequentially exposes a plurality of pixels arranged in a two-dimensional array in the image pickup device 301 for each scanning line (that is, column) corresponding to the beam steering angle θ at each irradiation timing t. In this manner, the imaging circuit 302 that employs a sequential exposure rolling shutter senses beam reflection from the outside by each pixel of the exposed scanning line in the imaging device 301 . The image pickup circuit 302 converts the half value of the beam flight time from the irradiation timing t to the detection of the beam reflection in each pixel of the scanning line into a distance value from the image pickup device 301 to the target 6 . The imaging circuit 302 generates an optical image Is as a so-called distance image or point cloud image by associating the converted distance value with each pixel of each scanning line and converting it into data.

The optical image Is is generated by converting the luminance value corresponding to the beam reflection intensity sensed by each pixel of each scanning line in the imaging device 301 into data together with the distance value in association with each pixel. good too. The imaging element 301 may also have a function of capturing an image of the external environment of the vehicle 4 according to external light sensed during a period in which intermittent beam irradiation is interrupted. In this case, the optical image Is is generated by converting the luminance value corresponding to the external light intensity sensed by each pixel of each scanning line in the imaging element 301 into data together with the distance value in association with each pixel. may

As shown in FIG. 2, the estimation device 1 is connected to the sensor unit 3 via at least one of a LAN (Local Area Network), a wire harness, an internal bus, and the like. The estimating device 1 may be an ECU (Electronic Control Unit) dedicated to driving control that executes advanced driving assistance or automatic driving control of the vehicle 4 . The estimating device 1 may be a locator ECU that is used for advanced driving assistance or automatic driving control of the vehicle 4 . The estimating device 1 may be an ECU of a navigation device that navigates driving of the vehicle 4 . The estimating device 1 may also be used by the imaging circuit 302 of the optical sensor 30 . The estimating device 1 may be configured by a combination of multiple types of these ECUs, circuits 302, and the like that share the functions described below.

The estimation device 1 is a dedicated computer including at least one memory 10 and at least one processor 12 . The memory 10 stores or stores computer-readable programs and data non-temporarily, and includes at least one type of non-transitory storage medium (non-transitory storage medium) such as a semiconductor memory, a magnetic medium, and an optical medium. tangible storage medium). The processor 12 includes, as a core, at least one of a CPU (Central Processing Unit), a GPU (Graphics Processing Unit), and a RISC (Reduced Instruction Set Computer)-CPU.

Processor 12 executes a plurality of instructions contained in an estimation program stored in memory 10 . As a result, the estimating device 1 constructs a plurality of functional blocks for estimating state quantities including the position of the vehicle 4, as shown in FIG. Thus, in the estimation device 1, the estimation program stored in the memory 10 for estimating the state quantity of the vehicle 4 causes the processor 12 to execute a plurality of instructions, thereby constructing a plurality of functional blocks. The multiple functional blocks include state quantity estimation block 100 , estimation error acquisition block 120 , update decision block 140 , observation error acquisition block 160 and beam adjustment block 180 .

The state quantity estimation block 100 estimates the state quantity Z of the vehicle 4 based on the optical image Is output from the optical sensor 30 in response to beam irradiation in the irradiation range Θs. Therefore, the state quantity estimation block 100 has sub-blocks 101 and 102 with different functions as shown in FIG.

The latest optical image Is generated by the optical sensor 30 is input to the matching sub-block 101 . The latest map information Im stored in the map unit 2 is input to the matching sub-block 101 . A matching sub-block 101 obtains observation residuals Δox and Δoy by matching the map information Im with the optical image Is.

Specifically, in the matching sub-block 101, the reflection point corresponding to the pixel where the target 6 was observed in the optical image Is is recognized as the observation point. Therefore, the matching sub-block 101 calculates orthogonal coordinates in directions X and Y with respect to the observation point of the target 6 in the optical image Is. In the matching sub-block 101, a coordinate point corresponding to the observation point of the target 6 in the map information Im is defined as a matching point. In particular, the matching point in this embodiment means a coordinate point with the shortest distance to the observation point of the target 6 . The matching sub-block 101 calculates orthogonal coordinates in directions X and Y for matching points in the map information Im. The matching sub-block 101 calculates the vector values of the observation residuals Δox and Δoy in each of the X and Y directions by calculating the difference between the orthogonal coordinates of the observation point and the orthogonal coordinates of the matching point. At this time, the observation residuals Δox and Δoy may be stored in the memory 10 as residual images ΔI (see later-described FIGS. 10 and 11) for each of the directions X and Y corresponding to the optical image Is.

The latest observation residuals Δox and Δoy obtained by matching in the matching subblock 101 are input to the filtering subblock 102 . The filtering sub-block 102 acquires the estimated value of the state quantity Z in the directions X and Y of the vehicle 4 including the positions x and y by filtering based on the observation residuals Δox and Δoy as the matching results. .

Specifically, the filtering sub-block 102 uses at least one of an extended Kalman filter, a particle filter, an unscented Kalman filter, an ensemble Kalman filter, and the like as an estimation filter to which observation residuals Δox and Δoy are input. The filtering sub-block 102 calculates the vector value of the state quantity Z according to Equation 1 below and the error covariance matrix M according to Equation 2 below by passing the observation residuals Δox and Δoy through the estimation filter. Here, x and y in Equation 1 are orthogonal coordinate positions of the vehicle 4 in directions X and Y, respectively. In Equation 1, vx and vy are the running speeds of the vehicle 4 in the X and Y directions, respectively. In Equation 2, σx and σy are error variances of orthogonal coordinate positions in directions X and Y, respectively. In Equation 2, σvx and σvy are error variances of running speeds in directions X and Y, respectively.

As shown in FIG. 3, the estimation error acquisition block 120 receives the latest error covariance matrix M acquired by the filtering sub-block 102 . The estimation error acquisition block 120 acquires estimation errors Eex and Eey based on the error covariance matrix M as errors occurring in the state quantity estimation in the state quantity estimation block 100 by error covariance analysis. Here, the estimated error Eex is defined as a front-rear estimated error in the longitudinal direction X of the vehicle 4 , while the estimated error Eey is defined as a left-right estimated error in the left-right direction Y of the vehicle 4 .

Specifically, the estimation error acquisition block 120 calculates the front-back estimation error Eex according to Equation 3 below using the position error variance σx and the velocity error σvx in the front-back direction X among the diagonal components of the error covariance matrix M. The estimation error acquisition block 120 calculates the left-right estimation error Eey according to the following equation 4 using the position error variance σy and the velocity error σvy in the left-right direction Y among the diagonal components of the error covariance matrix M. Here, α and β in Equations 3 and 4 are design parameters based on, for example, actual vehicle test results or simulation results. The design parameter α of Formula 3 regarding the longitudinal direction X and the design parameter α of Formula 4 regarding the lateral direction Y may be the same or different. Moreover, the design parameter β of Formula 3 regarding the front-back direction X and the design parameter β of Formula 4 regarding the left-right direction Y are the same.

As shown in FIG. 3, the latest estimated errors Eex and Eey obtained by the estimated error obtaining block 120 are input to the update determination block 140 . The update determination block 140 determines whether or not to update the beam pattern Ps in the irradiation range Θs of the optical sensor 30 based on the estimated errors Eex and Eey.

Specifically, the update decision block 140 monitors the estimated errors Eex and Eey in each of the directions X and Y by comparing them with the allowable range. As a result, update decision block 140 makes a maintenance decision to cause optical sensor 30 to maintain beam pattern Ps while both estimated errors Eex and Eey are within their respective allowable ranges. In the case of this maintenance determination, the beam pattern Ps is maintained by the optical sensor 30 by returning to the state quantity estimation block 100 without proceeding to the subsequent blocks 160 and 180 . On the other hand, when at least one of the estimated errors Eex and Eey deviates from the allowable range to the larger value side, the update decision block 140 makes an update decision to update the beam pattern Ps by the optical sensor 30 . In the case of this update determination, the beam pattern Ps is updated by the optical sensor 30 according to the directivity pattern Psd output from the beam adjustment block 180, which will be described in detail later, by proceeding to subsequent blocks 160 and 180. FIG.

Here, the permissible range of each estimation error Eex, Eey may be set to a numerical range equal to or less than the threshold, with the upper limit of the permissible value for estimation accuracy as a threshold, or a value that is difficult to allow for estimation accuracy. With the lower limit as a threshold, it may be set to a numerical range less than the threshold. The permissible range of the estimated error Eex in the longitudinal direction X and the permissible range of the estimated error Eey in the lateral direction Y may be the same or different.

The observation error acquisition block 160 shown in FIG. 3 acquires observation errors Eox and Eoy based on the optical image Is as errors occurring in observation of the target 6 by the optical sensor 30 . Here, the observation error Eox is defined as the front-back observation error in the longitudinal direction X of the vehicle 4 , while the observation error Eoy is defined as the left-right observation error in the left-right direction Y of the vehicle 4 . In order to acquire these observation errors Eox and Eoy by two systems of theory and actual measurement, the observation error acquisition block 160 includes sub-blocks 161, 162, 163, 164 and 165 having different functions as shown in FIG. , 166.

The latest optical image Is generated by the optical sensor 30 is input to the shortest distance specifying sub-block 161 . The shortest distance specifying sub-block 161 specifies the shortest distance observation point to the target 6 in the optical image Is in association with the beam steering angle θ at each irradiation timing t.

Specifically, as shown in FIG. 7( a ), the shortest distance specifying sub-block 161 determines the beam steering angle θ at each irradiation timing t in the optical image Is from each of the scanning lines (that is, columns) individually corresponding to the beam steering angle θ. , the shortest distance pixel Iso having the shortest distance (that is, the smallest distance value) to the beam reflection point is searched. In the shortest distance identification sub-block 161, the reflection point corresponding to the shortest distance pixel Iso on each scanning line is recognized as the shortest distance observation point associated with the beam steering angle θ at each irradiation timing t. As shown in FIGS. 7(b) and 7(c), the shortest distance specifying sub-block 161 converts the distance value at the shortest distance pixel Iso for each searched scanning line to the vector of the shortest distance value Isr at the shortest distance observation point. Extract as a value. The shortest distance identification sub-block 161 extracts the pixel number identifying the shortest distance pixel Iso for each scanning line searched as the vector value of the shortest distance index Isi. In order to facilitate understanding of the explanation, FIG. 7 illustrates only some pixels in the optical image Is.

A ranging error acquisition sub-block 162 shown in FIG. 6 acquires theoretical ranging errors Emo and Emp as errors occurring in ranging according to beam irradiation and beam reflection on the target 6 . Here, as shown in FIG. 8, the ranging error Emo is defined as an orthogonal ranging error in a beam orthogonal direction Dmo orthogonal to the beam whose beam steering angle θ changes at each irradiation timing t. This orthogonal ranging error Emo depends on the distance to the beam reflection point on the target 6, and becomes smaller as the distance becomes shorter. As shown in FIG. 9, the distance measurement error Emp is defined as a parallel distance measurement error in a beam parallel direction Dmp parallel to the beam whose beam steering angle θ changes at each irradiation timing t. This parallel ranging error Emp is an error that occurs constantly in the measurement of the beam flight time required for beam irradiation and beam reflection.

Specifically, as shown in FIG. 6, the latest shortest distance value Isr acquired by the shortest distance specifying sub-block 161 is input to the ranging error acquisition sub-block 162 . The ranging error acquisition sub-block 162 calculates the vector value of the orthogonal ranging error Emo for each scanning line corresponding to the beam steering angle θ at each irradiation timing t according to Equation 5 below using the shortest distance value Isr. 5 where Ean is an angular error inherent in the optical sensor 30 due to, for example, an actuator that adjusts the beam steering angle θ. The ranging error acquisition sub-block 162 fixes the vector value of the parallel ranging error Emp for each scanning line corresponding to the beam steering angle θ at each irradiation timing t to a constant value independent of the beam steering angle θ. At this time, the ranging error acquisition sub-block 162 sets, as the parallel ranging error Emp, a measurement error specific to the optical sensor 30 caused by, for example, a counter that measures the time of flight of the beam.

As shown in FIG. 6, the theoretical error acquisition sub-block 163 receives the latest ranging errors Emo and Emp acquired by the ranging error acquisition sub-block 162 . The theoretical error acquisition sub-block 163 acquires the theoretical values Eoxt and Eoyt of the observation errors Eox and Eoy by coordinate conversion from the ranging errors Emo and Emp. The theoretical values Eoxt and Eoyt of the observation errors Eox and Eoy are hereinafter also referred to as theoretical observation errors Eoxt and Eoyt.

Specifically, the theoretical error acquisition sub-block 163 receives the latest beam steering angle θ, which is adjusted at each irradiation timing t, among the directivity patterns Psd fed back from the beam adjustment block 180, which will be described in detail later. The theoretical error acquisition sub-block 163 calculates the vector values of the theoretical observation errors Eoxt and Eoyt at the shortest distance pixel Iso (that is, the shortest distance observation point) for each latest beam steering angle θ corresponding to each scan line using the following formula: It is calculated by the coordinate transformation according to 6.

As shown in FIGS. 3 and 6, the latest observation residuals Δox and Δoy obtained by the matching sub-block 101 are input to the actual measurement error obtaining sub-block 164 . The latest shortest distance index Isi obtained by the shortest distance identification sub-block 161 is input to the actual measurement error obtaining sub-block 164 . The actual measurement error acquisition sub-block 164 acquires actual measurement values Eoxm and Eoym of the observation errors Eox and Eoy based on the observation residuals Δox and Δoy. The measured values Eoxm and Eoym of the observation errors Eox and Eoy are hereinafter also referred to as actual observation errors Eoxm and Eoym.

Specifically, the actual measurement error acquisition sub-block 164 obtains the observation residual Δox corresponding to the shortest distance index Isi on each scanning line corresponding to the beam steering angle θ at each irradiation timing t as shown in FIG. extract with respect to The actual measurement error acquisition sub-block 164 extracts the observation residual Δoy corresponding to the shortest distance index Isi in each scanning line corresponding to the beam steering angle θ at each irradiation timing t in the horizontal direction Y as shown in FIG. . The actual measurement error acquisition sub-block 164 sets the observation residuals Δox and Δoy at the shortest distance pixel Iso (that is, the shortest distance observation point) corresponding to the shortest distance index Isi thus extracted to the actual measurement errors Eoxm and Eoym. do. 10 and 11 illustrate only some pixels when the observation residuals Δox and Δoy are the residual image ΔI, in order to facilitate understanding of the explanation.

As shown in FIG. 6, the theoretical observation errors Eoxt and Eoyt acquired by the theoretical error acquisition sub-block 163 are input to the observation error synthesis sub-block 165 . The measured observation errors Eoxm and Eoym obtained by the measured error obtaining sub-block 164 are input to the observed error synthesizing sub-block 165 . The observation error synthesizing sub-block 165 synthesizes the theoretical observation errors Eoxt and Eoyt with the actually measured observation errors Eoxm and Eoym, respectively, to obtain the observation error for each scanning line corresponding to the beam steering angle θ at each irradiation timing t. Get Eox and Eoy.

Specifically, the observation error synthesizing sub-block 165 calculates the forward and backward observation error Eox by a weighted synthesizing operation according to Equation 7 below using the theoretical observation error Eoxt and the actually measured observation error Eoxm. The observation error synthesizing sub-block 165 calculates the left-right observation error Eoy by a weighted synthesizing operation according to Equation 8 below using the theoretical observation error Eoyt and the actually measured observation error Eoym. Here, γ in Equations 7 and 8 is a weighting parameter based on, for example, actual vehicle test results or simulation results. The weighting parameter γ in Equation 7 regarding the longitudinal direction X and the weighting parameter γ in Equation 8 regarding the lateral direction Y may be the same or different.

As shown in FIG. 6, the observation errors Eox and Eoy obtained by the observation error synthesizing sub-block 165 are input to the observation error smoothing sub-block 166 . The observation error smoothing sub-block 166 smoothes the observation errors Eox, Eoy for any beam steering angle θ over the illumination range Θs.

Specifically, the observation error smoothing sub-block 166 passes the observation errors Eox and Eoy for each scanning line corresponding to the beam steering angle θ at each irradiation timing t through a smoothing filter. As shown in FIG. 12, the smoothing filter approximates the distribution of the forward and backward observation errors Eox using a polynomial function of several orders with the continuous beam steering angle θ as a variable. As shown in FIG. 13, the smoothing filter approximates the distribution of the left-right observation error Eoy by another polynomial function of several degrees with the continuous beam steering angle θ as a variable.

The beam adjustment block 180 shown in FIG. 3 adjusts the beam pattern Ps at the illumination range Θs to a directivity pattern Psd of illumination density. For that purpose, the beam adjustment block 180 has sub-blocks 181, 182, 183 with different functions as shown in FIG.

As shown in FIGS. 3 and 6, the latest estimated errors Eex and Eey obtained by the estimated error obtaining block 120 are input to the gain setting sub-block 181 . The gain setting sub-block 181 sets adjustment gains δ and ε supplied to the pattern updating sub-block 183, which will be detailed later, based on the ratio between the estimated errors Eex and Eey.

Specifically, in the gain setting sub-block 181, the estimated ratio Re is defined by the following Equation 9 using the estimated errors Eex and Eey. The gain setting sub-block 181 adjusts the adjustment gains δ and ε according to this estimated ratio Re. At this time, the adjustment gains .delta. and .epsilon. may be adjusted, for example, according to a gain table as shown in FIG. 14, or may be adjusted according to an arithmetic expression (not shown).

In the case of FIGS. 15A and 16A, where the estimation ratio Re falls below 1 because the front-rear estimation error Eex is greater than the left-right estimation error Eey, the gain setting sub-block 181 positively (pluses) the first adjustment gain δ. ) is given, and a positive (plus) value is given to the second adjustment gain ε. At this time, at least the former of the first adjustment gain δ and the second adjustment gain ε is variably set such that its absolute value increases as the estimated ratio Re decreases. In the case of FIGS. 15(b) and 16(b), where the estimated ratio Re exceeds 1 because the left-right estimation error Eey is greater than the front-rear estimation error Eex, the gain setting sub-block 181 sets the first adjustment gain δ to a negative value. ) is given, and a positive (plus) value is given to the second adjustment gain ε. At this time, at least the former of the first adjustment gain δ and the second adjustment gain ε is variably set such that its absolute value increases as the estimated ratio Re increases.

As shown in FIG. 6, the gain limiting sub-block 182 receives the adjustment gains δ and ε set by the gain setting sub-block 181 . A gain limiting sub-block 182 applies limiter processing to the adjustment gains δ and ε.

Specifically, as shown in FIG. 17, when the absolute value of the first adjustment gain δ exceeds the first threshold μ, the gain limiting sub-block 182 limits the absolute value of the gain δ to the threshold μ. As shown in FIG. 18, when the absolute value of the second adjustment gain ε exceeds the second threshold ρ, the gain limit sub-block 182 limits the absolute value of the gain ε to the threshold ρ. Other than these cases, the gain limiting sub-block 182 determines the adjustment gains δ and ε with the values set by the gain setting sub-block 181 as they are.

As shown in FIGS. 3 and 6, the pattern update sub-block 183 receives the observation errors Eox and Eoy smoothed by the observation error smoothing sub-block 166 . The pattern update sub-block 183 receives the adjusted gains δ and ε subjected to limiter processing by the gain limit sub-block 182 . The pattern update sub-block 183 updates the beam pattern Ps in the irradiation range Θs to the latest directivity pattern Psd based on the ratio of the observation errors Eox and Eoy as well as the adjustment gains δ and ε.

Specifically, in the pattern update sub-block 183, the observation ratio Ro shown in FIG. 20 is defined by the following equation 10 using the observation errors Eox and Eoy shown in FIG. The pattern update sub-block 183 calculates the irradiation density update value in the irradiation range Θs by the density function Fd according to the following equation 11 using the observation ratio Ro together with the adjustment gains δ and ε based on the estimated ratio Re described above. Here, the density function Fd of Equation 11 is based on the observed ratio Ro and the adjustment gains δ and ε with respect to the ratio Ro. As shown, it becomes an arithmetic expression converted into a function. Here, θl in Equation 11 is the value of the beam steering angle θ at the critical end azimuth Bc on the left side of the reference azimuth Bs in the irradiation range Θs, that is, on the negative (minus) side. In Equation 11, θr is the value of the beam steering angle θ at the critical end direction Bc on the right side of the irradiation range θs with respect to the reference direction Bs, that is, on the positive (plus) side. As part of Equation 11, the linear calculation portion using the observation ratio Ro and the adjustment gains δ and ε satisfies Equation 12 below. For these reasons, in Equation 11, the irradiation density update value is normalized by the beam amplitude in the irradiation range Θs.

The pattern update sub-block 183 converts the updated irradiation density calculated by the density function Fd into a cumulative distribution function Fc accumulated for an arbitrary beam steering angle θ in the irradiation range Θs, as shown in FIG. The pattern updating sub-block 183 samples the beam steering angle θ corresponding to each equally spaced irradiation timing t in the cumulative distribution function Fc, as shown in FIG. The pattern update sub-block 183 outputs, as a directivity pattern Psd, a distribution pattern of irradiation density update values determined by the latest interval between the beam steering angles θ sampled at each irradiation timing t. In order to facilitate understanding of the explanation, FIG. 23 illustrates only part of the beam steering angle θ at each irradiation timing t. FIG. 23 representatively shows an example of a directivity pattern Psd that gives the beam pattern Ps of FIG.

Here, in the pattern update sub-block 183, the larger one of the estimated errors Eex and Eey is defined as the larger estimated error, and the smaller one is defined as the smaller estimated error. In the pattern update sub-block 183, the larger side of the observation errors Eox and Eoy for each beam steering angle θ at each irradiation timing t is defined as the larger side observation error, and the smaller side is defined as the smaller side observation error. Under these definitions, the directivity pattern Psd satisfies the density function Fd of Eq.

The directivity pattern Psd that satisfies the density function Fd of Equation 11 gives beam irradiation directivity according to the adjustment gains δ and ε based on the estimated ratio Re from the viewpoint of the estimated errors Eex and Eey. Specifically, in the case of FIG. 15A, the directivity pattern Psd is on the side that includes the angle θ in the reference azimuth Bs as the steering angle θ that is closest to the direction X of the large-side estimation error Eex in the irradiation range Θs. , to bias the irradiation density to the higher side in the front region. At this time, the directivity pattern Psd is set such that the higher-side bias ratio of the irradiation density in the front region is set to be larger as the estimated ratio Re is smaller by setting the adjustment gains δ and ε. Further, in the case of FIG. 15(a), the directivity pattern Psd is on the side of the irradiation range Θs that includes the angle θ at the critical end azimuth Bc as the steering angle θ closest to the direction Y of the small side estimation error Eey. Illumination density is biased to the lower side in the remaining lateral regions.

On the other hand, in the case of FIG. 15B, the directivity pattern Psd is on the side that includes the angle θ in the critical end direction Bc as the steering angle θ that is closest to the direction Y of the large-side estimation error Eey in the irradiation range θs. The irradiation density is biased to the higher side in the left and right lateral regions. At this time, the directivity pattern Psd is set such that the higher the estimated ratio Re, the higher the irradiation density bias rate in each lateral region, particularly by setting the adjustment gains δ and ε. Further, in the case of FIG. 15B, the directivity pattern Psd is on the side including the angle θ in the reference direction Bs as the steering angle θ closest to the direction X of the small side estimation error Eex in the irradiation range θs. Biasing the irradiation density to the lower side in the front region.

The directivity pattern Psd that satisfies the density function Fd of Equation 11 gives the beam irradiation another directivity based on the observation ratio Ro from the viewpoint of the observation errors Eox and Eoy. Specifically, in the case of FIG. 15A, the directivity pattern Psd has a high irradiation density at the beam steering angle θ corresponding to the large side estimation error Eex and the small side observation error Eox in the same direction X in the irradiation range θs. bias to the side. At this time, in particular, due to the positive correlation corresponding to the positive first adjustment gain δ, in the directivity pattern Psd, the higher the observation ratio Ro, the larger the high-side bias rate of the irradiation density at the corresponding angle θ of the small-side observation error Eox is set. do. Further, in the case of FIG. 15A, the directivity pattern Psd shifts the irradiation density to the lower side at another steering angle θ corresponding to the large side estimation error Eex and the small side observation error Eoy in the different direction Y in the irradiation range Θs. bias. In such a directivity pattern Psd, the large-side estimated error Eex and the small-side observation error Eoy are different from the different direction angle θ of the different directions X and Y, respectively. Focus the beam illumination on the same directional angle θ of X. As a result, beam irradiation is concentrated on the same-direction angle θ at which the probability of observation of the target 6 at which the large-side estimation error Eex can be reduced is higher than at the opposite-direction angle θ at which the probability of observation of the target 6 is low.

On the other hand, in the case of FIG. 15B, the directivity pattern Psd shifts the irradiation density to the high side at the beam steering angle θ corresponding to the large side estimation error Eey and the small side observation error Eoy in the same direction Y in the irradiation range Θs. bias. At this time, in particular, due to the negative correlation corresponding to the negative first adjustment gain δ, in the directivity pattern Psd, the smaller the observation ratio Ro, the larger the high-side bias rate of the irradiation density at the angle θ corresponding to the small-side observation error Eoy is set. do. Further, in the case of FIG. 15(b), the directivity pattern Psd shifts the irradiation density to the lower side at another steering angle θ corresponding to the large-side estimation error Eey and the small-side observation error Eox in the different direction X in the irradiation range θs. bias. In such a directivity pattern Psd, the large-side estimated error Eey and the small-side observation error Eox are different from the different direction angles θ of the different directions Y and X, respectively. Focus the beam irradiation on the Y co-directional angle θ. As a result, the beam irradiation is concentrated on the same-direction angle θ at which the observation probability of the target 6 is low rather than at the same-direction angle θ at which the large-side estimation error Eey can be reduced.

As described above, the optical sensor 30 controls the beam pattern Ps of the irradiation range Θs according to the latest directivity pattern Psd output from the pattern updating sub-block 183 as shown in FIGS. The next estimation of the state quantity Z is performed according to the beam irradiation with the directivity pattern Psd controlled in this manner. When the next update is determined based on the estimated errors Eex and Eey generated at this time, data regarding the latest directivity pattern Psd is fed back from the pattern update sub-block 183 to the theoretical error acquisition sub-block 163. .

As described above, in this embodiment, the state quantity estimation block 100 corresponds to the "state quantity estimation section", and the estimation error acquisition block 120 corresponds to the "estimation error acquisition section". Further, in this embodiment, the observation error acquisition block 160 corresponds to the "observation error acquisition unit", and the beam adjustment block 180 corresponds to the "beam adjustment unit".

The flow of the estimation method for estimating the state quantity Z of the vehicle 4 by the estimation device 1 in cooperation with the functional blocks 100, 120, 140, 160 and 180 will be described below with reference to FIGS. 24 to 27. FIG. Note that this flow is executed at each estimation timing when the vehicle 4 requires motion estimation or at each shutter frame of the optical sensor 30 . Also, in this flow, "S" means multiple steps executed by multiple instructions included in the estimation program.

As shown in FIG. 24, in S10, the state quantity estimation block 100 estimates the state quantity Z of the vehicle 4 based on the optical image Is output from the optical sensor 30 in response to beam irradiation in the irradiation range Θs.

As shown in FIG. 25, in S101 of S10, the matching sub-block 101 acquires observation residuals Δox and Δoy by matching the map information Im with the optical image Is. In S102 following S101 in S10, the filtering sub-block 102 obtains an estimated value of the state quantity Z in the directions X and Y of the vehicle 4 by filtering based on the observation residuals Δox and Δoy from this matching.

As shown in FIG. 24, in S20 following S10, the estimation error acquisition block 120 acquires estimation errors Eex and Eey based on the error covariance matrix M as errors occurring in the state quantity estimation in the state quantity estimation block 100. FIG. Further, in subsequent S30, the update determination block 140 determines whether or not to update the beam pattern Ps in the irradiation range Θs of the optical sensor 30 based on the estimated errors Eex and Eey. If a negative determination is made in S30, that is, if the beam pattern Ps is determined to be maintained, the optical sensor 30 maintains the beam pattern Ps, and execution of the current flow ends.

If an affirmative determination is made in S30, that is, if the beam pattern Ps is determined to be updated, the current flow proceeds to S40. In S<b>40 , the observation error acquisition block 160 acquires observation errors Eox and Eoy based on the optical image Is as errors occurring in observation of the target 6 by the optical sensor 30 .

As shown in FIG. 26, in S401 of S40, the shortest distance specifying sub-block 161 specifies the shortest distance observation point to the target 6 in the optical image Is in association with the beam steering angle θ at each irradiation timing t. In S402 following S401 in S40, the ranging error acquisition sub-block 162 acquires theoretical ranging errors Emo and Emp as errors occurring in ranging according to beam irradiation and beam reflection on the target 6. FIG.

In S403 following S402 in S40, the theoretical error acquisition sub-block 163 acquires theoretical observation errors Eoxt and Eoyt by coordinate conversion from the ranging errors Emo and Emp. In S404 following S403 in S40, the actual measurement error acquisition sub-block 164 acquires the actual measurement errors Eoxm and Eoym based on the observation residuals Δox and Δoy.

In S405 following S404 in S40, the observation error synthesis sub-block 165 obtains observation errors Eox and Eoy by synthesizing the theoretical observation errors Eoxt and Eoyt with the actually measured observation errors Eoxm and Eoym, respectively. In S406 following S405 in S40, the observation error smoothing sub-block 166 smoothes the observation errors Eox and Eoy for an arbitrary beam steering angle θ in the illumination range Θs.

As shown in FIG. 24, in S50 following 40, the beam adjustment block 180 adjusts the beam pattern Ps in the irradiation range Θs to the directivity pattern Psd that determines the latest irradiation density.

As shown in FIG. 27, in S501 of S50, the gain setting sub-block 181 sets adjustment gains δ and ε based on the estimated ratio Re between the estimated errors Eex and Eey. In S502 following S501 in S50, the gain limiting sub-block 182 performs limiter processing on the adjustment gains δ and ε.

In S503 following S502 in S50, the pattern update sub-block 183 updates the beam pattern Ps in the irradiation range Θs to the latest orientation based on the observation ratio Ro between the observation errors Eox and Eoy and the adjustment gains δ and ε. Update to pattern Psd. As a result, in the optical sensor 30, the beam pattern Ps is controlled to the directivity pattern Psd, and the directivity pattern Psd is fed back to the theoretical error acquisition sub-block 163, and execution of the current flow ends.

The directivity pattern Psd updated in S503 biases the irradiation angle to the high side in the region on the steering angle θ side that is closest to the direction of the large estimation error in the irradiation range θs from the viewpoint of the estimation error. In the estimation error viewpoint, the directivity pattern Psd also biases the illumination angle to the low side in the region of the illumination range Θs on the steering angle θ side that is closest to the direction of the small side estimation error. On the other hand, in the observation error viewpoint, the directivity pattern Psd biases the irradiation density to the high side at the beam steering angle θ corresponding to the small-side observation error in the same direction as the large-side estimation error in the irradiation range Θs. The directivity pattern Psd also biases the irradiation density to the low side at the beam steering angle .theta. The directivity pattern Psd based on these observation error viewpoints provides directivity so that the beam irradiation is concentrated at the beam steering angle θ at which the large-side estimation error and the small-side observation error are in the same direction. The latest directivity pattern Psd commanded from the pattern update sub-block 183 to the optical sensor 30 is determined by synthesizing the biases of both viewpoints by the density function Fd of Equation (11).

As described so far, in this embodiment, S10 corresponds to the "state quantity estimation process" and S20 corresponds to the "estimation error acquisition process." Further, in this embodiment, S40 corresponds to the "observation error acquisition process", and S50 corresponds to the "beam adjustment process".

(Effect)
The effects of this embodiment described above will be described below.

The beam pattern Ps of this embodiment is adjusted to a directivity pattern Psd centered at a specific beam steering angle θ in the irradiation range Θs of the optical sensor 30 extending laterally from the reference bearing Bs along the longitudinal direction X. Here, according to a specific beam steering angle θ, the large estimation error of the front-back estimation error Eex and the left-right estimation error Eey occurring in the estimation of the state quantity Z, the front-back observation error Eox and the left-right observation error Eoy occurring in the observation of the target 6 of which the smaller side observation error is in the same direction. According to this, the beam irradiation can be concentrated on the beam steering angle θ at which the probability of observing the target 6 is high, which can reduce the large-side estimation error. Therefore, in estimating the state quantity Z based on the optical image Is output from the optical sensor 30 in response to beam irradiation according to the directivity pattern Psd, it is possible to reduce the large side estimation error and improve the estimation accuracy of the state quantity Z. Become.

The directivity pattern Psd according to this embodiment has a high irradiation density at the beam steering angle θ corresponding to the large-side estimation error and the small-side observation error in the same direction, and the large-side estimation error and the small-side observation error in the opposite direction. The irradiation density at the corresponding beam steering angle θ is biased to the lower side. According to this, for the beam steering angle θ at which the probability of observation of the target 6 capable of reducing the large-side estimation error is high, compared to the beam steering angle θ at which the probability of observation of the target 6 capable of reducing the large-side estimation error is low, Beam irradiation can be concentrated preferentially. Therefore, based on the optical image Is according to the beam irradiation according to the directivity pattern Psd, it is possible to reduce the large-side estimation error and, in turn, improve the estimation accuracy of the state quantity Z. FIG.

According to the present embodiment, at the shortest distance observation point of the target 6 acquired in association with the beam steering angle θ at each irradiation timing t, the observation errors Eox and Eoy with the smallest component in the beam orthogonal direction Dmo are large. The side estimation error and the small side observation error in the same direction can be obtained correctly. According to this, based on the optical image Is according to the beam irradiation concentrated aiming at the beam steering angle θ of the small side observation error, the effect of reducing the large side estimation error and the estimation accuracy of the state quantity Z are improved. can be increased.

According to the present embodiment, the theoretical values Eoxt and Eoyt can be obtained with high accuracy by coordinate conversion from the orthogonal ranging error Emo and the parallel ranging error Emp occurring in the observation of the target 6. From the observation errors Eox and Eoy, large The side estimation error and the small side observation error in the same direction can be obtained correctly. According to this, based on the optical image Is according to the beam irradiation concentrated aiming at the beam steering angle θ of the small side observation error, the effect of reducing the large side estimation error and the estimation accuracy of the state quantity Z are improved. can be increased.

According to this embodiment, the coordinate transformation from the orthogonal ranging error Emo and parallel ranging error Emp to the respective theoretical values Eoxt and Eoyt of the observation errors Eox and Eoy is based on the directivity pattern Psd fed back from S50 of the beam adjustment block 180. Based on it increases accuracy. According to this, beam irradiation can be concentrated by accurately targeting the beam steering angle θ of the small-side observation error that is in the same direction as the large-side estimation error. Therefore, based on the optical image Is according to such beam irradiation, it is possible to increase the estimation accuracy of the state quantity Z while securing the effect of reducing the large-side estimation error.

According to the present embodiment, each of the measured values of the observation errors Eox and Eoy can be obtained with high accuracy based on the observation residuals Δox and Δoy between the observation points of the target 6 in the optical image Is and the matching points in the map information Im. Eoxm and Eoym are combined with the respective theoretical values Eoxt and Eoyt of those errors. According to this, from the observation errors Eox and Eoy synthesized with increased accuracy by the two systems of theory and actual measurement, the beam steering angle θ of the small side observation error in the same direction as the large side estimation error can be accurately set. You can aim and concentrate your beam. Therefore, based on the optical image Is according to such beam irradiation, it is possible to increase the estimation accuracy of the state quantity Z while securing the effect of reducing the large-side estimation error.

According to this embodiment, the front-back observation error Eox and the left-right observation error Eoy are smoothed with respect to the beam steering angle θ. According to this, it is possible to suppress the deterioration of the acquisition accuracy of the small-side observation error in the same direction as the large-side estimation error due to the variation of the observation errors Eox and Eoy with respect to the beam steering angle θ due to disturbance. can. Therefore, based on the optical image Is according to the beam irradiation concentrated aiming at the beam steering angle θ of the small-side observation error, it is possible to suppress the increase of the large-side estimation error due to the disturbance. Therefore, it is possible to ensure high estimation accuracy of the state quantity Z. FIG.

According to the present embodiment, the irradiation density is functionized with respect to the beam steering angle θ based on the observation ratio Ro between the front-back observation error Eox and the left-right observation error Eoy, so that the beam is directed to the directivity pattern Psd that satisfies the density function Fd. A pattern Ps is adjusted. According to this, according to the directivity pattern Psd that correctly reflects the beam steering angle θ of the large side estimation error and the same direction error among the small side observation errors determined by the observation ratio Ro in the density function Fd, the beam is irradiated to the angle θ. can be concentrated. Therefore, based on the optical image Is according to such beam irradiation, it is possible to reduce the large-side estimation error and improve the estimation accuracy of the state quantity Z by extension.

According to this embodiment, the adjustment gains δ and ε for the observed ratio Ro of the density function Fd are set based on the estimated ratio Re between the front-rear estimated error Eex and the left-right estimated error Eey. As a result, the directivity pattern Psd that satisfies the density function Fd can reflect not only the observation errors Eox and Eoy but also the estimated errors Eex and Eey themselves. Therefore, based on the optical image Is according to the beam irradiation according to the directivity pattern Psd, it is possible to appropriately reduce the large-side estimation error and improve the estimation accuracy of the state quantity Z.

The beam pattern Ps according to the present embodiment is updated when at least one of the front-rear estimation error Eex and the left-right estimation error Eey deviates from the allowable range to the larger side. As a result, while the estimation errors Eex and Eey are small, the beam pattern Ps is stabilized to improve robustness. It is possible to improve the accuracy.

(Other embodiments)
Although one embodiment has been described above, the present disclosure is not to be construed as being limited to that embodiment, and can be applied to various embodiments within the scope of the present disclosure.

The estimation device 1 of the modification may be a dedicated computer that includes at least one of digital circuits and analog circuits as a processor. Here, especially digital circuits include, for example, ASIC (Application Specific Integrated Circuit), FPGA (Field Programmable Gate Array), SOC (System on a Chip), PGA (Programmable Gate Array), and CPLD (Complex Programmable Logic Device). at least one of Such digital circuits may also include memory storing programs.

In a modification, the function of the estimation device 1 may be systematically shared by at least one in-vehicle computer mounted on the vehicle 4 and at least one external center computer capable of communicating with the vehicle 4. good. Alternatively, the map unit 2 may be at least one external database accessible by communication with the vehicle 4 .

In the optical sensor 30 of the modified example, the reference direction Bs along the front-rear direction X faces the rear of the vehicle 4, so that the external environment may be scanned from the rear to the left and right sides. In the optical sensor 30 of the modified example, the directivity pattern Psd may be updated by adjusting the beam intensity for each equal or unequal beam steering angle θ at each irradiation timing t.

Alternatively, the average distance observation point instead of the shortest distance observation point may be extracted by sub-blocks 161 and S401 and utilized by sub-blocks 162, 164 and S402, S404. In modified sub-blocks 183 and S503, the directivity pattern Psd may not be fed back to sub-blocks 163 and S403.

Alternatively, the update decision blocks 140 and S30 may be omitted. In a variant, sub-blocks 165 and S405 may be omitted, along with sub-blocks 162, 163 and S402, S403. Alternatively, sub-blocks 165 and S405 may be omitted along with sub-blocks 164 and S404. Alternatively, sub-blocks 166 and S406 may be omitted.

In the modified sub-blocks 181 and S501, the adjustment gains δ and ε may be set to constant values. Alternatively, sub-blocks 182 and S502 may be omitted. In the modified sub-blocks 183 and S503, the directivity pattern Psd may be updated based on, for example, a table instead of the density function Fd and the cumulative distribution function Fc. In sub-blocks 183 and S503 of the modified example, even if the directivity pattern Psd is updated so as to maintain the irradiation density at a predetermined density in the area on the steering angle θ side that is closest to the direction of the small-side estimation error in the irradiation range Θs, good.

1 estimation device 4 vehicle 6 target 10 memory 12 processor 30 optical sensor 100 state quantity estimation block 120 estimation error acquisition block 140 update judgment block 160 observation error acquisition block 180 beam adjustment block Bs reference Azimuth, Dmo: beam orthogonal direction, Dmp: beam parallel direction, Eex: front-rear estimation error, Eey: left-right estimation error, Emo: orthogonal ranging error, Emp: parallel ranging error, Eoxm, Eoym: actual measurement/observation error, Eoxt, Eoyt: theoretical value/ Theoretical observation error, Fd Density function, Im Map information, Is Optical image, Ps Beam pattern, Psd Directional pattern, Re Estimated ratio, Ro Observation ratio, X Front-back direction, Y Left-right direction, Z State quantity, t Irradiation timing, Δox, Δoy observation residual, Θs illumination range, δ first adjustment gain, ε second adjustment gain, θ beam steering angle

Claims (30)

The state quantity (Z) including the position of the vehicle (4) equipped with the optical sensor (30) that receives the beam reflection from the target (6) observed by the beam irradiation to the outside and outputs the optical image (Is) is , an estimating device (1) for estimating
The distance value to the target is digitized in accordance with the beam irradiation in the irradiation range (Θs) spreading laterally from the reference direction (Bs) along the front-rear direction (X), and the optical sensor outputs the data. a state quantity estimation unit (100) for estimating the state quantity based on an image;
An estimation error occurring in the estimation of the state quantity, which is an estimation error for the observation residuals (Δox, Δoy) due to the matching processing of the map information (Im) with respect to the optical image, is the front-back estimation error (Eex ) and a left-right estimation error (Eey) in the left-right direction (Y);
An observation error occurring in observation of the target by the optical sensor, which is an observation error corresponding to the observation residual due to the matching process , is an observation error (Eox) in the front-back direction (X) and an observation error acquisition unit (160) that acquires the left-right observation error (Eoy) in association with the beam steering angle (θ) for each irradiation timing (t) in the irradiation range;
A larger side of the front-back estimation error and the left-right estimation error is defined as a large-side estimation error, and a smaller side of the front-back observation error and the left-right observation error is defined as a small-side observation error. A beam pattern that is an irradiation density distribution corresponding to the beam steering angle for each irradiation timing in the irradiation range to a directivity pattern (Psd) that is concentrated at the beam steering angle in which the small-side observation error is in the same direction. a beam adjuster (180) for adjusting (Ps). The beam adjustment unit biases the irradiation density at the beam steering angle corresponding to the small-side observation error in the same direction as the large-side estimation error to a higher side, the directivity pattern having the large-side estimation error and 2. An estimator according to claim 1, wherein said beam pattern is adjusted to said pointing pattern that biases to a lower side the irradiation density at said beam steering angle corresponding to said minor side observation error in different directions. The observation error acquisition unit identifies the shortest distance observation point to the target in association with the beam steering angle for each irradiation timing, and acquires the front-back observation error and the left-right observation error at the shortest distance observation point. The estimation device according to claim 1 or 2. The observation error acquisition unit obtains an orthogonal ranging error (Emo) in a beam orthogonal direction (Dmo) and a parallel ranging error (Emp) in a beam parallel direction (Dmp) as errors occurring in ranging of the target by the optical sensor. 4. The estimation device according to any one of claims 1 to 3, wherein each theoretical value (Eoxt, Eoyt) of the front-back observation error and the left-right observation error is obtained by coordinate transformation from . 5. The observation error acquisition unit acquires theoretical values (Eoxt, Eoyt) of the front-back observation error and the left-right observation error by the coordinate conversion based on the directivity pattern fed back from the beam adjustment unit. The estimating device described in . The state quantity estimating unit estimates the state quantity based on the matching process of the map information with respect to the optical image,
The observation error acquisition unit acquires each measured value (Eoxm, Eoym) of the front-back observation error and the left -right observation error based on the observation residual between the target observation point and the matching point in the map information, 6. An estimating apparatus according to claim 4 or 5, wherein each measured value is synthesized with each theoretical value (Eoxt, Eoyt) of said front-back observation error and said left-right observation error. The estimation apparatus according to any one of claims 1 to 6, wherein the observation error acquisition unit smoothes the front-back observation error and the left-right observation error with respect to the beam steering angle. The beam adjustment unit converts the irradiation density into a function of the beam steering angle based on an observation ratio (Ro) between the front-rear observation error and the left-right observation error, and transforms the directivity pattern into the directivity pattern that satisfies a density function (Fd). The estimation device according to any one of claims 1 to 7, which adjusts the beam pattern. 9. The beam adjustment unit according to claim 8, wherein the adjustment gain (δ, ε) for the observation ratio in the density function is set based on an estimated ratio (Re) between the front-rear estimation error and the left-right estimation error. estimation device. The estimation device according to any one of claims 1 to 9, wherein the beam adjustment unit updates the beam pattern when at least one of the front-rear estimation error and the left-right estimation error deviates from an allowable range to a larger side. . The state quantity (Z) including the position of the vehicle (4) equipped with the optical sensor (30) that receives the beam reflection from the target (6) observed by the beam irradiation to the outside and outputs the optical image (Is) is , an estimation method performed by a processor (12) to estimate
The distance value to the target is digitized in accordance with the beam irradiation in the irradiation range (Θs) spreading laterally from the reference direction (Bs) along the front-rear direction (X), and the optical sensor outputs the data. a state quantity estimation process (S10) for estimating the state quantity based on the image;
An estimation error occurring in the estimation of the state quantity, which is an estimation error for the observation residuals (Δox, Δoy) due to the matching processing of the map information (Im) with respect to the optical image, is the front-back estimation error (Eex ) and a left-right estimation error (Eey) in the left-right direction (Y);
An observation error occurring in observation of the target by the optical sensor, which is an observation error corresponding to the observation residual due to the matching process , is an observation error (Eox) in the front-back direction (X) and an observation error acquisition process (S40) for acquiring the left-right observation error (Eoy) in association with the beam steering angle (θ) for each irradiation timing (t) in the irradiation range;
A larger side of the front-back estimation error and the left-right estimation error is defined as a large-side estimation error, and a smaller side of the front-back observation error and the left-right observation error is defined as a small-side observation error. A beam pattern that is an irradiation density distribution corresponding to the beam steering angle for each irradiation timing in the irradiation range to a directivity pattern (Psd) that is concentrated at the beam steering angle in which the small-side observation error is in the same direction. a beam adjustment process (S50) to adjust (Ps). The beam adjustment process comprises: the pointing pattern biasing the irradiation density at the beam steering angle corresponding to the small-side observation error in the same direction as the large-side estimation error to a higher side; 12. The estimation method of claim 11, wherein the beam pattern is adjusted to the pointing pattern that biases the illumination density at the beam steering angle corresponding to the minor side observation error in the opposite direction to the low side. The observation error acquisition process identifies a shortest distance observation point to the target in association with the beam steering angle for each irradiation timing, and acquires the front-back observation error and the left-right observation error at the shortest distance observation point. The estimation method according to claim 11 or 12. In the observation error acquisition process, as errors occurring in the range measurement of the target by the optical sensor, orthogonal ranging error (Emo) in the beam orthogonal direction (Dmo) and parallel ranging error (Emp) in the beam parallel direction (Dmp) 14. The estimation method according to any one of claims 11 to 13, wherein the theoretical values (Eoxt, Eoyt) of the front-back observation error and the left-right observation error are obtained by coordinate transformation from . 14. The observation error acquisition process acquires theoretical values (Eoxt, Eoyt) of the front-back observation error and the left-right observation error by the coordinate transformation based on the pointing pattern fed back from the beam adjustment process. Estimation method described in . The state quantity estimation process estimates the state quantity based on the matching process of the map information with the optical image;
The observation error acquisition process acquires each measured value (Eoxm, Eoym) of the front-back observation error and the left -right observation error based on the observation residual between the target observation point and the matching point in the map information, 16. The estimation method according to claim 14 or 15, wherein each of the measured values is combined with each theoretical value (Eoxt, Eoyt) of the front-back observation error and the left-right observation error. The estimation method according to any one of claims 11 to 16, wherein the observation error acquisition process smoothes the front-back observation error and the left-right observation error with respect to the beam steering angle. The beam adjustment process converts the irradiation density into a density function (Fd), which is a function of the beam steering angle based on the observation ratio (Ro) of the front-back observation error and the left-right observation error. The estimation method according to any one of claims 11 to 17, wherein the beam pattern is adjusted. 19. The beam adjustment process according to claim 18, wherein the adjustment gain ([delta], [epsilon]) for the observation ratio in the density function is set based on an estimated ratio (Re) between the front-back estimation error and the left-right estimation error. estimation method. The estimation method according to any one of claims 11 to 19, wherein the beam adjustment process updates the beam pattern when at least one of the front-rear estimation error and the left-right estimation error deviates from an allowable range to a larger side. . The state quantity (Z) including the position of the vehicle (4) equipped with the optical sensor (30) that receives the beam reflection from the target (6) observed by the beam irradiation to the outside and outputs the optical image (Is) is , an estimation program stored in a storage medium (10) and comprising instructions for execution by a processor (12) to estimate,
Said instruction
The distance value to the target is digitized in accordance with the beam irradiation in the irradiation range (Θs) spreading laterally from the reference direction (Bs) along the front-rear direction (X), and the optical sensor outputs the data. a state quantity estimation process (S10) for estimating the state quantity based on the image;
An estimation error occurring in the estimation of the state quantity, which is an estimation error for the observation residuals (Δox, Δoy) due to the matching processing of the map information (Im) with respect to the optical image, is the front-back estimation error (Eex ) and the left-right estimation error (Eey) in the left-right direction (Y); an estimated error acquisition process (S20);
An observation error occurring in observation of the target by the optical sensor, which is an observation error corresponding to the observation residual due to the matching process , is an observation error (Eox) in the front-back direction (X) and an observation error acquisition process (S40) for acquiring the left-right observation error (Eoy) in association with the beam steering angle (θ) for each irradiation timing (t) in the irradiation range;
A larger side of the front-back estimation error and the left-right estimation error is defined as a large-side estimation error, and a smaller side of the front-back observation error and the left-right observation error is defined as a small-side observation error. A beam pattern that is an irradiation density distribution corresponding to the beam steering angle for each irradiation timing in the irradiation range to a directivity pattern (Psd) that is concentrated at the beam steering angle in which the small-side observation error is in the same direction. and a beam adjustment process (S50) to adjust (Ps). The beam adjustment process comprises: the pointing pattern biasing the irradiation density at the beam steering angle corresponding to the small-side observation error in the same direction as the large-side estimation error to a higher side; 22. The estimation program according to claim 21, causing the beam pattern to be adjusted to the directivity pattern that biases the irradiation density at the beam steering angle corresponding to the minor side observation error in the opposite direction to the low side. The observation error acquisition process specifies the shortest distance observation to the target in association with the beam steering angle for each irradiation timing, and acquires the front-back observation error and the left-right observation error at the shortest distance observation points. The estimation program according to claim 21 or 22. In the observation error acquisition process, as errors occurring in the range measurement of the target by the optical sensor, orthogonal ranging error (Emo) in the beam orthogonal direction (Dmo) and parallel ranging error (Emp) in the beam parallel direction (Dmp) 24. The estimation program according to any one of claims 21 to 23, wherein each theoretical value (Eoxt, Eoyt) of said front-back observation error and said left-right observation error is obtained by coordinate transformation from . 25. The observation error acquisition process acquires theoretical values (Eoxt, Eoyt) of the front-back observation error and the left-right observation error by the coordinate transformation based on the pointing pattern fed back from the beam adjustment process. Estimation program described in . The state quantity estimation process estimates the state quantity based on the matching process of the map information with respect to the optical image;
The observation error acquisition process acquires each measured value (Eoxm, Eoym) of the front-back observation error and the left -right observation error based on the observation residual between the target observation point and the matching point in the map information, 26. The estimation program according to claim 24 or 25, which synthesizes each of the measured values with the theoretical values (Eoxt, Eoyt) of the front-back observation error and the left-right observation error. The estimation program according to any one of claims 21 to 26, wherein the observation error acquisition process smoothes the front-back observation error and the left-right observation error with respect to the beam steering angle. The beam adjustment process converts the irradiation density into a density function (Fd), which is a function of the beam steering angle based on the observation ratio (Ro) of the front-back observation error and the left-right observation error. The estimation program according to any one of claims 21 to 27, which adjusts the beam pattern. 29. The beam adjustment process according to claim 28, wherein the adjustment gain (δ, ε) for the observation ratio in the density function is set based on an estimated ratio (Re) between the front-back estimation error and the left-right estimation error. estimation program. The estimation program according to any one of claims 21 to 29, wherein the beam adjustment process updates the beam pattern when at least one of the front-rear estimation error and the left-right estimation error deviates from an allowable range to a larger side. .

Images