JP7234840B2 - position estimator - Google Patents

Description

The present invention relates to a position estimation device.

Research has been conducted on estimating the position of a vehicle for use in driving assistance or automatic driving control of the vehicle.

For example, the position of a vehicle can be determined by comparing an image of the vehicle's surroundings captured by a camera mounted on the vehicle with a map showing features such as lane markings. It is estimated by matching with the features shown in .

In addition, the position of the vehicle is obtained based on the position of the vehicle at the previous position determination time, and the amount of movement and the amount of azimuth angle change calculated from the vehicle speed and yaw rate of the vehicle between the previous and current position determination times. (for example, Patent Document 1).

JP 2017-61265 A

When the vehicle moves due to skidding, the estimated position and azimuth at the current time may change significantly from the position and azimuth at the previous time. When the traveling is automatically controlled, a sudden change in the amount of steering that causes the steering to vibrate may occur.

SUMMARY OF THE INVENTION Accordingly, it is an object of the present invention to provide a position estimating device that estimates the position and azimuth angle of a moving object so as to suppress sudden changes in the amount of steering.

According to one embodiment, a position estimation device is provided. This position estimating device includes a storage unit that stores position information representing the position of a feature, and an allowable yaw rate per unit time that is allowed with respect to changes in the yaw rate of the moving object based on the yaw rate of the moving object at the current time. Based on the permissible yaw rate change amount determination unit that determines the amount of change, the yaw rate of the moving object at the previous time, and the permissible yaw rate change amount, the change in the azimuth angle representing the traveling direction of the moving object between the previous time and the current time is determined. a permissible azimuth angle change amount determination unit that determines an allowable azimuth angle change amount that is permissible with respect to the object, the position of the moving object at the previous time, the movement amount and the azimuth angle change amount of the moving object from the previous time to the current time, a permissible lateral movement range determination unit that determines a permissible lateral movement range in which movement of the moving object in a direction perpendicular to the traveling direction is permitted based on the permissible azimuth angle change amount; and an imaging unit provided on the moving object. An image representing features on the road surface around the moving object obtained at the current time, position information representing the position of the feature stored in the storage unit, the position and azimuth angle of the moving object at the previous time, and the an estimating unit that calculates an estimated position and an estimated azimuth angle of a moving object at the current time based on the amount of movement and the amount of change in the azimuth angle of the moving object between the current time and the estimated azimuth angle and the azimuth of the moving object at the previous time; If the difference from the moving object at the current time is not within the allowable azimuth angle change amount, the estimated azimuth angle is corrected so that the difference from the azimuth angle difference from the moving object at the previous time is within the allowable azimuth angle change amount. and an azimuth angle determination unit that determines the azimuth angle of the moving object at the current time, and corrects the estimated position so that it is within the allowable lateral movement range if the estimated position is not within the allowable lateral movement range, and determines the position of the moving object at the current time. and a position determination unit.

In this position estimating device, the allowable yaw rate change determination unit determines the allowable yaw rate change as follows: is preferably determined based on the yaw rate of the moving object at the current time.

In this position estimating device, the allowable azimuth change amount determination unit multiplies a value obtained by offsetting the yaw rate of the moving object at the previous time by the allowable yaw rate change amount by the time between the previous time and the current time to obtain the allowable azimuth angle Preferably, the amount of change is determined.

In this position estimating device, the allowable lateral movement range determining unit determines the position of the moving object at the current time based on the position of the moving object at the previous time and the amount of movement and the amount of azimuth angle change of the moving object from the previous time to the current time. It is preferable to obtain the second estimated position of , and determine an area having a width of the allowable azimuth angle change around the direction from the position of the moving object at the previous time to the second estimated position as the allowable lateral movement range.

In this position estimating device, it is preferable that the allowable lateral movement range determination section keeps the width of the allowable lateral movement range constant when the amount of movement between the previous time and the current time is greater than a predetermined distance.

Also, according to another embodiment, a computer program for position estimation is provided. This position estimation computer program determines an allowable yaw rate change amount per unit time that is allowed with respect to a change in the yaw rate of the moving object based on the yaw rate of the moving object at the current time; and the allowable yaw rate change amount, determining an allowable azimuth angle change amount that is allowed for a change in the azimuth angle representing the traveling direction of the moving object between the previous time and the current time; based on the position of the moving object, the amount of movement and azimuth angle change of the moving object from the previous time to the current time, and the allowable azimuth angle change amount, movement in the direction orthogonal to the traveling direction of the moving object is permitted an image representing features on the road surface around the moving object at the current time obtained by an imaging unit provided on the moving object; and features stored in a storage unit Estimated position of the moving object at the current time based on the position information representing the position of the moving object, the position and azimuth angle of the moving object at the previous time, and the amount of movement and azimuth angle change of the moving object from the previous time to the current time and when the estimated azimuth angle is obtained and the difference between the estimated azimuth angle and the azimuth angle of the moving object at the previous time is not within the allowable azimuth angle change amount, the difference from the azimuth angle of the moving object at the previous time is the allowable azimuth angle determining the azimuth angle of the moving object at the current time by correcting the estimated azimuth angle so that it is within the change amount; correcting the estimated position to determine the position of the moving object at the current time.

The position estimation device according to the present invention has the effect of estimating the position and azimuth angle of a moving object so as to suppress sudden changes in the amount of steering. Preferably, a driving plan for the moving object is generated based on the position and azimuth angle of the moving object estimated in this way, and the operation of the vehicle is controlled based on this driving plan.

1 is a schematic configuration diagram of a vehicle control system in which a position estimation device is mounted; FIG. 1 is a hardware configuration diagram of an electronic control device that is one embodiment of a position estimation device; FIG. FIG. 3 is a functional block diagram of a processor of an electronic control unit for vehicle control processing including a position estimator; FIG. 10 is a diagram (part 1) for explaining the operation of the allowable yaw rate change amount determination unit; FIG. 10 is a diagram (part 2) for explaining the operation of the allowable yaw rate change amount determination unit; It is a figure explaining operation|movement of the allowable azimuth angle change amount determination part. It is a figure explaining operation|movement of the allowable lateral movement range determination part. It is a figure explaining operation|movement of an estimation part. It is a figure explaining operation|movement of an azimuth determination part. It is a figure explaining operation|movement of a position determination part. FIG. 11 is a diagram illustrating a modification of the operation of the allowable lateral movement range determining section; 4 is an operation flowchart of the vehicle control system including position estimation processing;

The position estimation device will be described below with reference to the drawings. This position estimation device determines an allowable yaw rate change amount per unit time that is allowed for changes in the yaw rate of the moving object based on the yaw rate of the moving object at the current time. The position estimating device calculates an allowable azimuth angle for change in the azimuth angle representing the traveling direction of the moving object between the previous time and the current time based on the yaw rate of the moving object at the previous time and the allowable yaw rate change amount. Determine the amount of change. The previous time and the current time are, for example, the previous image information acquisition time and the current image information acquisition time at which an image representing features around the moving object is captured. Further, the position estimating device calculates the traveling direction of the moving object based on the position of the moving object at the previous time, the amount of movement and azimuth angle change of the moving object between the previous time and the current time, and the allowable azimuth angle change amount. Determines the allowable lateral movement range in which movement in the direction perpendicular to is allowed. The position estimation device calculates the amount of movement of the moving object using, for example, vehicle speed information detected by a sensor provided on the moving object. Also, the position estimation device calculates the azimuth angle change amount of the moving object using, for example, the yaw rate detected by the sensor provided on the moving object. Thereby, the position estimation device determines the allowable range for the amount of movement of the moving object between the times when the position of the vehicle is determined, according to the running condition of the vehicle. The position estimating device includes an image representing features around the moving object at the current time, position information representing the position of the feature stored in the storage unit, the position and azimuth angle of the moving object at the previous time, and the previous time. Based on the amount of movement of the moving object and the amount of change in the azimuth angle between the current times, the estimated position of the moving object and the estimated azimuth angle representing the traveling direction of the moving object at the current time are estimated. If the difference between the estimated azimuth angle and the azimuth angle of the moving object at the previous time is not within the allowable azimuth angle change amount, the position estimation device determines that the difference from the azimuth angle of the moving object at the previous time is within the allowable azimuth angle change amount. The azimuth angle of the moving object at the current time is determined by correcting the estimated azimuth angle so that Further, if the estimated position is not within the allowable lateral movement range, the position estimation device corrects the estimated position so that it is within the allowable lateral movement range, and determines the position of the moving object at the current time. Thereby, the position estimation device can estimate the position and azimuth angle of the moving object so as to suppress abrupt changes in the steering amount.

An example in which the position estimation device is applied to a vehicle control system will be described below. In this example, the position estimation device executes the position estimation process so that the difference between the azimuth angle of the vehicle, which is an example of a moving object, at the current time and the azimuth angle of the vehicle at the previous time is the allowable azimuth angle change. Estimate the position and azimuth of the vehicle by determining the position of the vehicle at the current time of the vehicle to be within the allowable lateral movement range. However, the technical scope of the present invention is not limited to those embodiments, but extends to the invention described in the claims and equivalents thereof.

FIG. 1 is a schematic configuration diagram of a vehicle control system in which a position estimation device is installed. FIG. 2 is a hardware configuration diagram of an electronic control unit, which is one embodiment of the position estimation device.

In this embodiment, a vehicle control system 1 mounted on a vehicle 10 and controlling the vehicle 10 includes a camera 2 that captures an image in front of the vehicle, and a vehicle speed sensor 3 that detects vehicle speed information of the vehicle 10. , and a yaw rate sensor 4 for detecting the yaw rate of the vehicle 10 . The vehicle control system 1 also has a positioning information receiver 5 . Further, the vehicle control system 1 stores wide-area map information of a predetermined area, and based on the positioning information output from the positioning information receiver 5, the vehicle control system 1 obtains map information of an area including the current position represented by the positioning information. It has a map information generating device 6 for output. Furthermore, the vehicle control system 1 is an example of a position estimation device, and has an electronic control unit (ECU) 7 that estimates the position and azimuth angle of the vehicle 10 . The vehicle 10 also has a wireless communication device (not shown), and can communicate with an external server via a base station by wireless communication via this wireless communication device.

The camera 2 photographs a predetermined area in front of the vehicle 10 at every predetermined photographing cycle, and generates an image showing the predetermined area in front of the vehicle 10 . Features such as lane markings on the road surface included in a predetermined area in front of the vehicle 10 are represented in the generated image. The image produced by camera 2 may be a color image or a gray image. The camera 2 is an example of an imaging unit, and includes a two-dimensional detector composed of an array of photoelectric conversion elements sensitive to visible light, such as a CCD or C-MOS, and an object to be photographed on the two-dimensional detector. It has an imaging optical system that forms an image of the area. The camera 2 is installed, for example, in the vehicle interior of the vehicle 10 so as to face the front of the vehicle 10 . Each time an image is generated, the camera 2 outputs the image and the image information acquisition time at which the image was acquired to the ECU 7 via the in-vehicle network 8 . The image generated by the camera 2 is used in the ECU 7 for position estimation processing and processing for detecting objects around the vehicle 10 .

The vehicle speed sensor 3 detects vehicle speed information of the vehicle 10 and outputs the vehicle speed information and the speed information acquisition time at which the vehicle speed information is acquired to the ECU 7 via the in-vehicle network 8 . The vehicle speed sensor 3 is attached to, for example, an axle, detects the number of revolutions of the axle, and outputs a pulse signal proportional to the number of revolutions.

The yaw rate sensor 4 detects the yaw rate of the vehicle 10 and outputs the yaw rate and the yaw rate information acquisition time at which the yaw rate is acquired to the ECU 7 via the in-vehicle network 8 . As the yaw rate sensor 4, for example, an acceleration sensor such as a gyroscope can be used.

The positioning information receiver 5 acquires positioning information representing the current position of the vehicle 10 . For example, the positioning information receiver 5 can be a GPS receiver. Then, the positioning information receiver 5 outputs the positioning information and the positioning information acquisition time at which the positioning information was acquired to the map information generating device 6 every time the positioning information is acquired at a predetermined reception cycle.

The map information generating device 6 has a processor (not shown) and a storage device (not shown) such as a magnetic disk drive or non-volatile semiconductor memory. It stores wide-area map information for a wide range (for example, a range of several square kilometers) including . This wide-area map information is preferably high-precision map information including information representing types and positions of features and structures such as lane markings on roads. Note that the positions of features and structures on roads are represented, for example, by a world coordinate system with a predetermined reference position in real space as the origin. The processor of the map information generation device 6 receives wide area map information from an external server via a base station and stores it in a storage device by wireless communication via a wireless communication device according to the current position of the vehicle 10 . Each time the positioning information is input from the positioning information receiver 5, the processor of the map information generating device 6 refers to the wide-area map information stored in the storage unit to determine the narrow area including the current position represented by the positioning information. The time when the map information and the positioning information (for example, a range of several tens to several hundred meters square) are acquired is output to the ECU 7 via the in-vehicle network 8 . The map information generation device 6 is an example of a storage unit that stores location information of features.

The ECU 7 controls the vehicle 10 . In this embodiment, the ECU 7 estimates the position of the vehicle 10 and the azimuth angle representing the direction of travel based on the image generated by the camera 2, the vehicle speed, and the yaw rate, and controls the vehicle 10 to automatically drive the vehicle 10. do. Therefore, the ECU 7 has a communication interface 21 , a memory 22 and a processor 23 .

The communication interface 21 is an example of a communication section and has an interface circuit for connecting the ECU 7 to the in-vehicle network 8 . That is, the communication interface 21 is connected to the camera 2 and the like via the in-vehicle network 8 . Then, for example, every time an image and image information acquisition time are received from the camera 2, the communication interface 21 passes the received image and image information acquisition time to the processor 23. FIG. Similarly, the communication interface 21 passes the vehicle speed information and the speed information acquisition time to the processor 23 each time it receives the vehicle speed information and the speed information acquisition time from the vehicle speed sensor 3 . Further, the communication interface 21 passes the yaw rate and the yaw rate information acquisition time to the processor 23 every time it receives the yaw rate and the yaw rate information acquisition time from the yaw rate sensor 4 . Further, the communication interface 21 passes the received positioning information, the positioning information acquisition time and the map information to the processor 23 every time it receives the positioning information, the positioning information acquisition time and the map information from the map information generating device 6 .

The memory 22 is an example of a storage unit, and has, for example, a volatile semiconductor memory and a nonvolatile semiconductor memory. The memory 22 stores various data used in the position estimation process executed by the processor 23 of the ECU 7, such as installation position information such as the optical axis direction and installation position of the camera 2, the focal length of the imaging optical system and the image. It stores internal parameters such as angles. In addition, the memory 22 stores the image received from the camera 2 and the image information acquisition time, the vehicle speed information received from the vehicle speed sensor 3 and the speed information acquisition time, the yaw rate received from the yaw rate sensor 4 and the yaw rate information acquisition time, and map information generation. It stores the positioning information received from the device 6, the time when the positioning information was obtained, the map information, and the like.

The processor 23 is an example of a control unit, and has one or more CPUs (Central Processing Units) and their peripheral circuits. Processor 23 may further comprise other arithmetic circuitry such as a logic arithmetic unit, a math unit or a graphics processing unit. When the processor 23 has multiple CPUs, each CPU may have a memory. Then, each time an image is generated by the camera 2, the processor 23 sets the azimuth angle of the vehicle to the permissible azimuth angle change, which is the difference between the azimuth angle of the vehicle at the previous image information acquisition time when the image was generated by the camera 2 and the azimuth angle of the vehicle. and the position of the vehicle is determined to be within the allowable lateral movement range, and a position estimation process is performed to estimate the position and azimuth of the vehicle. Further, the processor 23 converts the azimuth angle of the vehicle 10 at the current position determination time to the vehicle azimuth at the previous position determination time at the position determination time set in a cycle shorter than the cycle in which the images are generated by the camera 2 . The difference from the angle is determined to be within the current allowable azimuth angle change amount, and the vehicle position is determined to be within the current allowable lateral travel range. Furthermore, the processor 23 controls the vehicle 10 to automatically drive the vehicle 10 based on the estimated position and azimuth angle of the vehicle 10 and the relative positional relationship with other objects around the vehicle 10. .

FIG. 3 is a functional block diagram of the processor 23 of the ECU 7 regarding vehicle control processing including position estimation processing. The processor 23 includes an allowable yaw rate change amount determination unit 31, an allowable azimuth angle change amount determination unit 32, an allowable lateral movement range determination unit 33, an estimation unit 34, an azimuth angle determination unit 35, a position determination unit 36, It has an object detection unit 37 , a driving planning unit 38 , and a vehicle control unit 39 . These units of the processor 23 are, for example, functional modules implemented by computer programs running on the processor 23 . Alternatively, each of these units of processor 23 may be a dedicated arithmetic circuit provided in processor 23 . Among these units of the processor 23, an allowable yaw rate change amount determination unit 31, an allowable azimuth angle change amount determination unit 32, an allowable lateral movement range determination unit 33, an estimation unit 34, and an azimuth angle determination unit 35 , and the position determination unit 36 executes position estimation processing.

The permissible yaw rate change amount determination unit 31 determines an permissible yaw rate change amount per unit time that is permissible for changes in the yaw rate of the vehicle 10 based on the yaw rate of the vehicle 10 at the current image information acquisition time. Specifically, the permissible yaw rate change amount determination unit 31 sets the permissible range of position fluctuation in the radial direction of the vehicle 10, which is assumed to be traveling on a circular track, to be within a predetermined range. The allowable yaw rate change amount is determined based on the yaw rate of the vehicle 10 at the current time. As shown in FIG. 4, it is assumed that the vehicle 10 is positioned at a position 400 at the previous image information acquisition time when the image was generated by the camera 2, and the yaw rate is ω _pre . Between the current image information acquisition time when the image was generated by the camera 2 and the previous image information acquisition time, the vehicle 10 travels on a circular track and is positioned at a position 401 at the current image information acquisition time. , and the yaw rate is ω. The permissible yaw rate change amount determination unit 31 determines the permissible yaw rate change amount Δω _guard at the current image information acquisition time based on the yaw rate ω at the current image information acquisition time. Determine to be within range. In this specification, the current image information acquisition time when an image is generated by the camera 2 is an example of the current time, and the previous image information acquisition time when an image is generated by the camera 2 is an example of the previous time. .

For example, the permissible yaw rate change amount determination unit 31 determines that the vehicle 10 is traveling on a circular track with a centrifugal force of 0.3 G, and the fluctuation range in the radial direction of the vehicle 10 after 0.1 seconds changes by 5 cm. The amount of change in yaw rate at the time may be used as the allowable amount of change in yaw rate. FIG. 5 shows the relationship between the yaw rate variation Δω thus determined and the yaw rate ω.

Moreover, it is preferable that the allowable yaw rate change amount determination section 31 sets the lower limit value of the allowable yaw rate change amount to a value other than zero. This is because if the permissible yaw rate variation is set to zero, the behavior of the vehicle may rather become unstable. The permissible yaw rate change amount determination unit 31 determines the lower limit value of the permissible yaw rate change amount based on the permissible fluctuation amount (for example, 3.5 deg/s ² ) that is permissible when the vehicle 10 is traveling straight. good too.

The permissible yaw rate change amount determination unit 31 can determine the permissible yaw rate change amount as shown in the following formula (1).

Here, α is the slope of the straight line representing the relationship between Δω and ω shown in FIG. 5, φ is the allowable fluctuation amount expressed in radian/ ^s2 , and Δt is It is the time between image information acquisition times. Then, the allowable yaw rate change amount determination unit 31 notifies the allowable azimuth angle change amount determination unit 32 of the determined allowable yaw rate change amount.

Based on the allowable yaw rate change amount and the yaw rate of the vehicle 10 at the previous image information acquisition time when an image was generated by the camera 2, the allowable azimuth angle change amount determination unit 32 determines the allowable yaw rate change amount between the previous image information acquisition time and the current image information acquisition time. An allowable azimuth angle change amount that is allowed for a change in the azimuth angle representing the direction of travel of the vehicle 10 is determined. Specifically, as shown in FIG. 6, the allowable azimuth angle change amount determination unit 32 sets a value offset by the allowable yaw rate change amount Δω _guard from the yaw rate ω _pre of the vehicle 10 at the previous image information acquisition time. The permissible azimuth angle change amount Δθ _guard is determined by multiplying the time Δt between the previous image information acquisition time and the current image information acquisition time. Here, θc is an azimuth angle representing the traveling direction of the vehicle 10 at the current image information acquisition time, and θ _pre is an azimuth angle representing the traveling direction of the vehicle 10 at the previous image information acquisition time. A change Δθ in the azimuth angle representing the traveling direction of the vehicle 10 between the previous image information acquisition time and the current image information acquisition time is represented by the difference between θc and _θpre .

The permissible azimuth angle change amount determination unit 32 can determine the permissible azimuth angle variation amount Δθ _guard as shown in Equation (2) below. Then, the allowable azimuth angle change amount determination unit 32 notifies the allowable azimuth angle change amount Δθ _guard to the allowable lateral movement range determination unit 33 and the azimuth angle determination unit 35 .

The allowable lateral movement range determining unit 33 determines the position of the vehicle 10 at the previous image information acquisition time, the movement amount and the azimuth angle change amount of the vehicle 10 between the previous image information acquisition time and the current image information acquisition time, and the allowable azimuth angle change amount. , a permissible lateral movement range in which movement of the vehicle 10 in a direction perpendicular to the traveling direction is permitted is determined. As shown in FIG. 7, the allowable lateral movement range determination unit 33 determines the position 700 of the vehicle 10 at the previous image information acquisition time, and the movement amount and the azimuth angle change amount of the vehicle 10 between the previous image information acquisition time and the current image information acquisition time. , an estimated position 701 (a position estimated by dead reckoning) of the vehicle 10 at the current image information acquisition time is obtained. The allowable lateral movement range determination unit 33 integrates the vehicle speed obtained from the vehicle speed information of the vehicle 10, obtains the amount of movement of the vehicle 10 between the previous image information acquisition time and the current image information acquisition time, and integrates the yaw rate of the vehicle 10. Then, the azimuth angle change amount of the vehicle 10 between the previous image information acquisition time and the current image information acquisition time is obtained. The permissible lateral movement range determination unit 33 obtains a position estimated by dead reckoning using the position and movement direction of the vehicle 10 at the previous image information acquisition time, the movement amount, and the azimuth angle change amount. Allowable lateral movement range determination unit 33 expresses estimated position 701 in a vehicle coordinate system whose origin is position 700 of vehicle 10 at the previous image information acquisition time notified from position determination unit 36 . In the vehicle coordinate system, the traveling direction of the vehicle is the Zc axis, the direction orthogonal to the Zc axis and parallel to the ground is the Xc axis, and the vertical direction is the Yc axis. The allowable lateral movement range determination unit 33 calculates the movement amount and the azimuth angle change amount of the vehicle 10 between the previous image information acquisition time and the current image information acquisition time based on the vehicle speed information and the yaw rate input during that time. The permissible lateral movement range determination unit 33 determines the width of the permissible azimuth angle change amount centering on the direction α from the position 700 of the vehicle 10 at the previous image information acquisition time to the estimated position 701 of the vehicle 10 at the current image information acquisition time. is determined as the allowable lateral movement range. In the example shown in FIG. 7, the allowable lateral movement range determining unit 33 defines a range between (ω _pre +Δω _guard )Δt/2 and (ω _pre −Δω _guard )Δt/2 centering on the direction α as follows: Determine the allowable lateral movement range. Then, the allowable lateral movement range determination section 33 notifies the position determination section 36 of the allowable lateral movement range.

The estimating unit 34 generates image information representing features on the road surface around the vehicle 10 at the current image information acquisition time, location information of the features included in the map information received from the map information generation device 6, Estimation of the vehicle 10 at the current image information acquisition time based on the position and azimuth angle of the vehicle 10 at the image information acquisition time, and the movement amount and azimuth angle change amount of the vehicle 10 between the previous image information acquisition time and the current image information acquisition time. Determine position and estimated azimuth. First, the estimation unit 34 uses the image generated by the camera 2 and the map information to obtain the first estimated position and the first estimated azimuth angle of the vehicle 10 at the current image information acquisition time. Then, the estimation unit 34 obtains the second estimated position and second estimated azimuth angle of the vehicle 10 at the current image information acquisition time by dead reckoning. Then, the estimating unit 34 inputs the first estimated position and the first estimated azimuth angle and the second estimated position and the second estimated azimuth angle to the prediction filter to estimate the vehicle 10 at the current image information acquisition time. Determine position and estimated azimuth.

First, assuming the position and orientation of the vehicle 10, the estimating unit 34 calculates lane markings, which are examples of features around the vehicle 10, represented in the map information received from the map information generation device 6. Projected on the image of the camera 2 generated at the image information acquisition time. For example, the estimating unit 34 determines the position of the vehicle 10 represented by the positioning information received from the positioning information receiver 5 at the current image information acquisition time, and the traveling direction of the vehicle 10 obtained from a plurality of recent positioning information. The corresponding attitude of the vehicle 10 is assumed to be the assumed position and the assumed attitude of the vehicle 10 . According to the assumed position and assumed orientation, the estimation unit 34 obtains a conversion formula from the world coordinate system to the camera coordinate system having the position of the camera 2 as the origin and the optical axis direction of the camera 2 as one axis direction. Such a conversion formula is represented by a combination of a rotation matrix representing rotation between coordinate systems and a translation vector representing translation between coordinate systems. Then, the estimation unit 34 converts the coordinates of the lane lines on the road around the vehicle 10 represented in the world coordinate system, which are included in the map information, into the coordinates of the camera coordinate system according to the conversion formula. Then, based on the camera 2 internal parameters such as the focal length of the camera 2, the estimation unit 34 calculates the lane markings around the vehicle 10 represented in the camera coordinate system by the camera 2 generated at the current image information acquisition time. , and the degree of matching between the lane markings around the vehicle 10 shown on the map and the detection points detected on the image of the camera 2 is calculated. The estimating unit 34 performs the same coordinate system conversion, projection, and match degree calculation processes as described above while changing the assumed positions and assumed orientations by a predetermined amount. , the degree of matching between the lane markings around the vehicle 10 represented in the map information and the detection points detected from the image is calculated. Then, the estimation unit 34 identifies the assumed position and the assumed posture when the degree of matching is maximized, sets the assumed position as the first estimated position of the vehicle 10, and expresses the traveling direction of the vehicle 10 based on the assumed posture. A first estimated azimuth angle is obtained.

The estimation unit 34 also calculates the position and azimuth angle of the vehicle 10 estimated at the previous image information acquisition time when the image was generated by the camera 2, and the image information acquisition time from the previous time when the image was generated by the camera 2 to the current image information acquisition time. A second estimated azimuth angle representing the traveling direction of the vehicle 10 is estimated for the second estimated position of the vehicle 10 at the current image information acquisition time based on the amount and direction of movement of the vehicle 10 between the two positions. The estimating unit 34 is notified of the position of the vehicle 10 at the previous image information acquisition time from the position determination unit 36, and is notified from the azimuth angle determination unit 35 of the azimuth angle of the vehicle 10 at the previous image information acquisition time. there is The estimation unit 34 calculates the amount and direction of movement of the vehicle 10 between the previous image information acquisition time and the current image information acquisition time based on the vehicle speed information and the yaw rate input during that time. Note that the estimating unit 34 calculates the position and azimuth of the vehicle 10 estimated at the latest position determination time instead of the position and azimuth of the vehicle 10 estimated at the previous image information acquisition time when the image was generated by the camera 2. Azimuth angles may also be used.

Then, as shown in FIG. 8, the estimating unit 34 inputs the first estimated position and the first estimated azimuth, and the second estimated position and the second estimated azimuth to a Kalman filter, which is an example of a prediction filter. , the estimated position and the estimated azimuth angle θc of the vehicle 10 at the current image information acquisition time. The estimation unit 34 notifies the estimated azimuth angle θc to the azimuth determination unit 35 and notifies the position determination unit 36 of the estimated position.

If the difference between the estimated azimuth angle θc and the azimuth angle of the vehicle 10 at the previous image information acquisition time when the image was generated by the camera 2 is not within the allowable azimuth angle change amount, the azimuth angle determination unit 35 determines that the previous image The estimated azimuth angle is corrected so that the difference from the azimuth angle of the vehicle 10 at the information acquisition time is within the allowable azimuth angle change amount, and the estimated azimuth angle of the vehicle 10 at the current image information acquisition time is determined. Specifically, as shown in FIG. 9, the azimuth angle determining unit 35 obtains a difference Δθ between the estimated azimuth angle θc and the azimuth angle θ _pre of the vehicle 10 at the previous image information acquisition time. When the difference Δθ is larger than (ω _pre +Δω _guard )Δt, the azimuth angle determination unit 35 corrects the estimated azimuth angle θc so that the difference Δθ is within the allowable azimuth angle change amount. Further, when the difference Δθ is smaller than (ω _pre −Δω _guard )Δt, the azimuth angle determination unit 35 corrects the estimated azimuth angle θc so that the difference Δθ is within the allowable azimuth angle change amount. Further, when the difference Δθ is within the allowable azimuth angle change amount, the azimuth angle determination unit 35 does not correct the estimated azimuth angle θc. The azimuth angle determination unit 35 can determine the estimated azimuth angle θc as shown in Equation (3) below.

Then, the azimuth angle determination unit 35 notifies the operation planning unit 38 and the vehicle control unit 39 of the estimated azimuth angle θc at the current image information acquisition time. Also, the azimuth determining unit 35 notifies the estimating unit 34 of the estimated azimuth angle θc at the current image information acquisition time.

In addition, the azimuth angle determining unit 35 determines the position of the vehicle 10 at the latest image information acquisition time at which the image was generated by the camera 2 at the position determination time set in a period shorter than the period at which the image is generated by the camera 2. , and the amount and direction of movement of the vehicle 10 between the latest image information acquisition time and the position determination time. The azimuth angle determination unit 35 calculates the movement amount and movement direction of the vehicle 10 between the latest image information acquisition time and the position determination time based on the vehicle speed information and yaw rate input during that time. The azimuth determination unit 35 determines the difference between the estimated azimuth angle of the vehicle 10 at the current position determination time and the estimated azimuth angle of the vehicle 10 at the previous position determination time to the latest determined immediately before the current position determination time. The estimated azimuth angle is determined by correcting it so that it is within the allowable azimuth angle change amount at the image information acquisition time. Then, the azimuth angle determination unit 35 notifies the operation planning unit 38 and the vehicle control unit 39 of the estimated azimuth angle. In addition, the azimuth determining unit 35 notifies the allowable lateral movement range determining unit 33, the estimating unit 34, and the position determining unit 36 of the estimated position and estimated azimuth angle of the vehicle 10 at the current position determination time along with the position determination time. .

If the estimated position notified from the estimating unit 34 is not within the allowable lateral movement range, the position determination unit 36 corrects the estimated position so that it is within the allowable lateral movement range, and corrects the estimated position to be within the allowable lateral movement range. determines the estimated position of the vehicle 10 at the image information acquisition time of . In FIG. 10, an estimated position 1002 notified from the estimating unit 34 is expressed in a vehicle coordinate system having a position 1000 of the vehicle 10 at the previous image information acquisition time as an origin. The position determining unit 36 compares the estimated position 1002 with the allowable lateral movement range, and if the estimated position 1002 is not within the allowable lateral movement range, determines the estimated position of the vehicle 10 from the estimated position 1002 to the allowable lateral movement range. A corrected estimated position 1003 is obtained by moving in a direction perpendicular to the traveling direction α of the vehicle 10 to the boundary. On the other hand, if the estimated position 1002 is within the allowable lateral movement range, the position determination unit 36 does not correct the estimated position. The position determining unit 36 then notifies the allowable lateral movement range determining unit 33 and the estimating unit 34 of the estimated position of the vehicle 10 at the current image information acquisition time. The position determining unit 36 also converts the estimated position of the vehicle 10 at the current image information acquisition time into the world coordinate system, and notifies the operation planning unit 38 and the vehicle control unit 39 of it.

In addition, the position determination unit 36 determines the estimated position of the vehicle 10 at the position determination time, which is notified from the azimuth angle determination unit 35 and is set in a cycle shorter than the cycle in which the images are generated by the camera 2. If not within the allowable lateral movement range, correct the estimated position so that it is within the allowable lateral movement range. The position determination unit 36 uses the latest allowable lateral movement range determined immediately before the current position determination time as the allowable lateral movement range. The position determination unit 36 converts the estimated position into the world coordinate system and notifies the operation planning unit 38 and the vehicle control unit 39 of it.

The object detection unit 37 acquires an image generated by the camera 2 and detects other objects around the vehicle 10 based on this image. The object detection unit 37 detects an object represented in the image by, for example, inputting the image into the classifier. As a discriminator, for example, a deep neural network (DNN) pre-trained to detect an object represented in an input image can be used. The object detection unit 37 may use a discriminator other than the DNN. For example, the object detection unit 37 receives, as an identifier, a feature amount (for example, Histograms of Oriented Gradients, HOG) calculated from a window set on an image, and the window represents an object to be detected. A pre-trained support vector machine (SVM) may be used to output the confidence that Alternatively, the object detection unit 37 may detect an object region by performing template matching between a template representing an object to be detected and an image. Further, the object detection unit 37 tracks the object detected from the latest image by associating the object detected from the latest image with the object detected from the previous image according to the tracking process based on the optical flow. good too. The object detection unit 37 may then estimate the relative velocity of the object with respect to the vehicle 10 based on changes in the size of the object being tracked on the image over time. Then, the object detection unit 37 determines whether the objects detected based on each image are the same, and determines the objects determined to be the same as one object. The object detection unit 37 notifies the operation planning unit 38 of information indicating the position of the detected object. Note that the object detection unit 37 may detect other objects around the vehicle 10 based on measurement results obtained by a sensor that acquires a range image, such as a lidar sensor.

The operation planning unit 38 acquires the position of the vehicle 10 at the image information acquisition time or the position determination time when the image was generated by the camera 2, information indicating the position of the detected object, and map information. The operation planning unit 38 generates one or more planned travel routes for the vehicle 10 based on these pieces of information. The planned travel route is represented, for example, as a set of target positions of the vehicle 10 at each time from the current time to a predetermined time ahead. The operation planning unit 38 determines the relative positions of the vehicle 10 and other objects according to the position of the vehicle 10, the structure on the road represented on the map, and the positional relationship between the detected other objects. Infer relationships. For example, the operation planning unit 38 identifies the lane in which the other object is traveling according to the positional relationship between the lane indicated on the map and the other object, thereby allowing the other object and the vehicle 10 to Determine whether or not the vehicles are traveling in the same lane. For example, the operation planning unit 38 determines that the other object is traveling in the lane identified by two adjacent lane markings that sandwich the horizontal center position of the other object. Similarly, the operation planning unit 38 determines that the vehicle 10 is traveling in the lane identified by two adjacent lane markings that sandwich the vehicle 10 . Then, the operation planning unit 38 determines whether or not the lane in which the vehicle 10 is traveling is the same as the lane in which another object is traveling. In addition, since the distance between two adjacent lane markings represented on the map is known and the internal parameters such as the focal length of the camera are known, the distance between two adjacent lane markings on the image is known. The distance allows estimation of the distance from the vehicle 10 . Therefore, the driving planning unit 38 estimates the distance from the vehicle 10 to the other object based on the distance between two adjacent lane markings represented on the map at the position of the other object on the image. may In this way, the operation planning unit 38 estimates the relative positional relationship between other objects and the vehicle 10 based on the positional relationship with structures on the road represented on the map. Therefore, the driving planning unit 38 can accurately estimate the relative positional relationship between other objects and the vehicle 10 even if features on the road such as lane markings in the image are unclear.

The operation planning unit 38 estimates a future trajectory based on the current and past trajectories of the detected other objects, and based on the lane in which the detected other objects are traveling and the relative distance, the other objects and the vehicle 10 travel in different lanes, or the planned travel route of the vehicle 10 is generated so that the relative distance from the vehicle 10 to another object is greater than or equal to a predetermined distance. Note that the operation planning unit 38 may generate a plurality of planned travel routes. In this case, the operation planning unit 38 may select the route that minimizes the sum of the absolute values of the acceleration of the vehicle 10 from among the plurality of planned travel routes. The operation planning unit 38 notifies the vehicle control unit 39 of the generated planned travel route.

The vehicle control unit 39 controls the position of the vehicle 10 at the image information acquisition time or the position determination time at which the image is generated by the camera 2, the vehicle speed and yaw rate, and the notified planned travel route. Each part of the vehicle 10 is controlled so as to travel along the planned travel route. For example, the vehicle control unit 39 obtains the steering angle, acceleration, and angular acceleration of the vehicle 10 according to the notified planned travel route and the current vehicle speed and yaw rate of the vehicle 10, and calculates the steering angle, acceleration, and angular acceleration. The steering amount, accelerator opening, or brake amount is set so as to The vehicle control unit 39 then outputs a control signal corresponding to the set steering amount to an actuator (not shown) that controls the steered wheels of the vehicle 10 . The vehicle control unit 39 also obtains a fuel injection amount according to the set accelerator opening, and outputs a control signal corresponding to the fuel injection amount to a fuel injection device (not shown) of the engine of the vehicle 10 . Alternatively, the vehicle control unit 39 outputs a control signal corresponding to the set brake amount to the brake (not shown) of the vehicle 10 .

FIG. 12 is an operation flowchart of vehicle control processing including position estimation processing executed by the processor 23 . The processor 23 executes vehicle control processing according to the operation flowchart shown in FIG. 12 at each image information acquisition time set in a predetermined cycle. Note that in the operation flowchart shown below, the processing of steps S1201 to S1206 corresponds to the position/orientation estimation processing.

First, the permissible yaw rate change amount determination unit 31 of the processor 23 determines the permissible yaw rate change amount per unit time that is permissible with respect to the yaw rate change of the vehicle 10 based on the yaw rate of the vehicle 10 at the current image information acquisition time. Determine (step S1201).

Next, the permissible azimuth angle change amount determination unit 32 of the processor 23 determines the yaw rate of the vehicle 10 at the previous image information acquisition time and the permissible yaw rate change amount, and determines the yaw rate of the vehicle 10 between the previous image information acquisition time and the current image information acquisition time. A permissible azimuth angle change amount that is allowed for a change in the azimuth angle representing the direction of travel of is determined (step S1202).

Next, the allowable lateral movement range determination unit 33 of the processor 23 determines the position of the vehicle 10 at the previous image information acquisition time, the movement amount and the azimuth angle change amount of the vehicle 10 between the previous image information acquisition time and the current image information acquisition time, Based on the allowable azimuth angle change amount, the allowable lateral movement range in which the vehicle 10 is allowed to move in the direction perpendicular to the traveling direction is determined (step S1203).

Next, the estimating unit 34 of the processor 23 acquires an image representing features around the vehicle 10 at the current image information acquisition time obtained by the imaging unit provided in the vehicle 10, and the storage unit of the map information generating device 6. position information representing the position of a feature stored in the , the position and azimuth angle of the vehicle 10 at the previous image information acquisition time, and the movement amount and azimuth angle change amount of the vehicle 10 between the previous image information acquisition time and the current image information acquisition time. , the estimated position and the estimated azimuth angle of the vehicle 10 at the current image information acquisition time are obtained (step S1204).

Next, if the difference between the estimated azimuth angle and the azimuth angle of the vehicle 10 at the previous image information acquisition time is not within the allowable azimuth angle change amount, the azimuth angle determination unit 35 of the processor 23 determines The estimated azimuth angle is corrected so that the difference from the azimuth angle of the vehicle 10 is within the allowable azimuth angle change amount, and the azimuth angle of the vehicle 10 at the current image information acquisition time is determined (step S1205). Also, the azimuth angle determining unit 35 of the processor 23 determines the azimuth angle of the vehicle 10 at the position determination time.

Next, if the estimated position is not within the allowable lateral movement range, the position determination unit 36 of the processor 23 corrects the estimated position so that it is within the allowable lateral movement range, and determines the position of the vehicle 10 at the current image information acquisition time. A position is determined (step S1206). Also, the azimuth angle determination unit 35 of the processor 23 determines the position of the vehicle 10 at the position determination time.

Next, the object detection unit 37 of the processor 23 detects another object around the vehicle 10 based on the image generated by the camera 2 (step S1207).

Next, the operation planning unit 38 of the processor 23 determines the position of the vehicle 10 at the current image information acquisition time and the position determination time, information indicating the position of the detected object, map information, etc. A planned travel route is generated (step S1208).

Next, the vehicle control unit 39 of the processor 23 travels along the planned travel route based on the position of the vehicle 10 at the current image information acquisition time and the position determination time, the vehicle speed and yaw rate, and the planned travel route. Each part of the vehicle 10 is controlled so as to do so (step S1209).

As described above, the position estimation device determines the azimuth angle of the vehicle at the current time so that the difference from the azimuth angle of the vehicle at the previous time is within the allowable azimuth angle change amount, and , the position of the vehicle at the current time of the vehicle is determined to be within the allowable lateral movement range, and the azimuth angle representing the position and heading of the vehicle is estimated. As a result, the position and azimuth angle of the moving object can be estimated so as to suppress sudden changes in the steering amount.

In automatic driving control of a vehicle, the vehicle is controlled based on the current position of the vehicle. Due to skidding, etc., the estimated position at the current time may change significantly from the position at the previous time. Based on such a position, the driving of the vehicle is automatically controlled. Then, a sudden change in the amount of steering that causes the steering to vibrate may occur.

In this way, as a measure to suppress the occurrence of a sudden change in the amount of steering, for example, in the process of estimating the current position, Certain restrictions may apply.

However, setting a certain limit on the estimated amount of movement of the vehicle, regardless of the driving conditions of the vehicle, may result in insufficient followability of the vehicle's driving conditions with respect to estimation of the amount of movement. , does not necessarily improve the accuracy of the position of the vehicle.

In the position estimating device of the present embodiment, the allowable range for the amount of movement of the moving object between the times when the position of the vehicle is determined is determined according to the running state of the vehicle, and the steering is vibrated by controlling the vehicle. The occurrence of such a sudden change in steering amount is suppressed.

Next, another example in which the allowable lateral movement range determining section determines the allowable lateral movement range will be described below with reference to FIG. 11 .

The allowable lateral movement range determination unit 33 determines the allowable lateral movement range in a direction orthogonal to the traveling direction α of the vehicle 10 when the amount of movement between the previous image information acquisition time when the image was generated by the camera 2 and the current image information acquisition time is greater than a predetermined distance. Make the width of the horizontal movement range constant. As shown in FIG. 11 , the allowable lateral movement range determining unit 33 determines a vehicle position 1100 at the previous image information acquisition time when an image was generated by the camera 2 and a vehicle position 1101 at the current image information acquisition time. The distance is obtained, and if this distance is greater than a predetermined distance (eg, 5 to 15 m), the width of the allowable lateral movement range in the direction perpendicular to the traveling direction α of the vehicle 10 is made constant. By setting an upper limit for the amount of lateral movement of the vehicle 10 in this way, the position and azimuth angle of the vehicle 10 at the current image information acquisition time when an image is generated by the camera 2 is the same as the previous image information. It is possible to determine the position of the moving object at the current time that does not change abnormally significantly with respect to the position and azimuth of the vehicle 10 at the acquisition time.

In the present invention, the position estimating device and the position estimating computer program of the above-described embodiments can be modified as appropriate without departing from the gist of the present invention.

For example, in the above-described embodiments, the estimated position of the moving object is corrected so that it falls within the permissible lateral movement range in which lateral movement of the moving object in the direction perpendicular to the traveling direction is permitted. This is because the lateral movement has a large effect on the occurrence of a sudden change in the amount of steering that causes the steering to vibrate. Similarly, the position of the moving object is estimated so that the allowable range of movement of the moving object in the direction of travel is set, and the position of the moving object is within the allowable range of movement in the direction of travel of the moving object. You may

REFERENCE SIGNS LIST 1 vehicle control system 2 camera 3 vehicle speed sensor 4 yaw rate sensor 5 positioning information receiver 6 map information generation device 7 electronic control device 8 in-vehicle network 10 vehicle 21 communication interface 22 memory 23 processor 31 allowable yaw rate variation determination unit 32 allowable azimuth angle Change amount determination unit 33 Allowable lateral movement range determination unit 34 Estimation unit 35 Azimuth determination unit 36 Position determination unit 37 Object detection unit 38 Operation planning unit 39 Vehicle control unit

Claims (1)

a storage unit that stores position information representing the position of a feature;
a permissible yaw rate variation determination unit that determines an permissible yaw rate variation per unit time that is permissible with respect to changes in the yaw rate of the moving object, based on the yaw rate of the moving object at the current time;
An allowable azimuth angle change amount that is allowed for a change in the azimuth angle representing the traveling direction of the moving object between the previous time and the current time based on the yaw rate of the moving object at the previous time and the allowable yaw rate change amount. A permissible azimuth angle change determination unit that determines
perpendicular to the traveling direction of the moving object based on the position of the moving object at the previous time, the amount of movement and azimuth angle change of the moving object between the previous time and the current time, and the allowable azimuth angle change amount; an allowable lateral movement range determination unit that determines an allowable lateral movement range in which movement in a direction is permitted;
an image representing features around the moving object at the current time obtained by an imaging unit provided on the moving object; position information representing the position of the features stored in the storage unit; an estimating unit for obtaining an estimated position and an estimated azimuth angle of the moving object at the current time based on the position and the azimuth angle of the moving object and the amount of movement and the azimuth angle change amount of the moving object between the previous time and the current time; ,
If the difference between the estimated azimuth angle and the azimuth angle of the moving object at the previous time is not within the allowable azimuth angle change amount, the difference from the azimuth angle of the moving object at the previous time is within the allowable azimuth angle change amount. an azimuth determination unit that determines the azimuth angle of the moving object at the current time by correcting the estimated azimuth angle so that
a position determination unit that, if the estimated position is not within the allowable lateral movement range, corrects the estimated position so that it is within the allowable lateral movement range, and determines the position of the moving object at the current time;
A position estimator having

Images