Information processing device, method, program and storage medium

Abstract

The present invention provides an information processing device capable of suitably estimating the attitude of a vehicle. [Solution] The control unit 15 of the in-vehicle unit 1 acquires multiple location information of features drawn on the road surface near the vehicle from landmark data LD, which is map data containing location information of features. The control unit 15 then calculates a normal vector for an approximate plane obtained based on the multiple location information acquired. The control unit 15 then calculates at least one of the pitch angle or roll angle of the vehicle based on the orientation of the vehicle and the normal vector.

Description

[Technical Field]

[0001] This invention relates to a technique for estimating the attitude of a vehicle. [Background technology]

[0002] Conventionally, technologies have been known that perform self-position estimation based on measurement data from measurement units such as radar and cameras. For example, Patent Document 1 discloses a technology that estimates the self-position by comparing the output of a measurement sensor with location information of features registered in advance on a map. Patent Document 2 also discloses a self-position estimation technology using a Kalman filter. [Prior art documents] [Patent Documents]

[0003] [Patent Document 1] Japanese Patent Publication No. 2013-257742 [Patent Document 2] Japanese Patent Publication No. 2017-72422 [Overview of the project] [Problems that the invention aims to solve]

[0004] Generally, vehicles are constrained to the road surface, and changes in the vehicle's roll angle, pitch angle, and vertical direction are negligibly small, although there are fluctuations due to the suspension. Therefore, in typical vehicle self-position estimation based on measurement data from the measurement unit, the vehicle's planar position and orientation are set as parameters to be estimated. On the other hand, when driving on steep slopes or roads with cross slopes, estimating only the vehicle's planar position and orientation is insufficient to account for changes in pitch angle and roll angle, and the correspondence between the measured position of features measured by the measurement unit and the position of features on the map may not be properly established. In this case, the amount of measurement data used for self-position estimation decreases, and the robustness of self-position estimation may decline. If pitch angle and roll angle are added as estimation parameters to self-position estimation in order to account for changes in pitch angle and roll angle, the computational load increases due to the increase in estimation parameters, and a problem arises where self-position estimation may not be completed stably at the required cycle. Furthermore, while there are methods to determine the pitch and roll angles of a vehicle from IMU (Inertial Measurement Unit) data, there was a problem in that typical IMUs had sensitivity errors and offsets, making it impossible to accurately calculate the pitch and roll angles.

[0005] This invention was made to solve the above-mentioned problems, and its main objective is to provide an information processing device capable of suitably estimating the attitude of a vehicle. [Means for solving the problem]

[0006] The invention described in the claim is an information processing device comprising: an acquisition unit that acquires a plurality of location information of features located near a moving object from map data including location information of features; a normal vector calculation unit that calculates a normal vector for an approximate plane obtained based on the plurality of location information; and an angle calculation unit that calculates at least one of the pitch angle or roll angle of the moving object based on the orientation of the moving object and the normal vector.

[0007] Furthermore, the invention described in the claim is a method performed by an information processing device, characterized in that it obtains multiple location information of features drawn on a road surface that are located near a moving body from map data including location information of features, calculates a normal vector for an approximate plane obtained based on the multiple location information, and calculates at least one of the pitch angle or roll angle of the moving body based on the orientation of the moving body and the normal vector.

[0008] Furthermore, the invention described in the claims is a program that causes a computer to function as an acquisition unit that acquires multiple location information of features drawn on the road surface that are located near a moving body from map data including location information of features; a normal vector calculation unit that calculates a normal vector for an approximate plane obtained based on the multiple location information; and an angle calculation unit that calculates at least one of the pitch angle or roll angle of the moving body based on the orientation of the moving body and the normal vector. [Brief explanation of the drawing]

[0009] [Figure 1] This is a schematic diagram of the driver assistance system. [Figure 2] This is a block diagram showing the functional configuration of an in-vehicle device. [Figure 3] This shows an example of a general data structure for a map database. [Figure 4] This is a diagram showing the state variable vector represented in two-dimensional Cartesian coordinates. [Figure 5] This diagram shows the general relationship between the prediction step and the measurement update step. [Figure 6] This is an example of a functional block in the vehicle position estimation unit. [Figure 7] (A) An overhead view of a vehicle on a road, clearly showing the position of the lane markings. (B) A side view of a vehicle on a road, clearly showing the position of the lane markings. [Figure 8] This is an xy-plane diagram showing the relationship between the vehicle's yaw angle and its direction of travel vector. [Figure 9]This diagram shows the relationship between the angle between the normal vector and the direction of travel vector and the pitch angle of the vehicle. [Figure 10] This is an xy-plane diagram showing the relationship between the vehicle's yaw angle and lateral vector. [Figure 11] This diagram shows the relationship between the angle between the normal vector and the transverse vector and the vehicle's roll angle. [Figure 12] (A) A side view of a vehicle and road traveling on a flat surface. (B) A side view of a vehicle and road traveling on a steeply sloping surface, when no coordinate transformation based on the vehicle's pitch angle is performed on the point cloud data. [Figure 13] This is a side view of a vehicle and road traveling on a road with a steep gradient, when performing coordinate transformations on point cloud data based on the pitch angle of a vehicle. [Figure 14] This is an example of a flowchart illustrating the procedure for estimating the position and orientation of a vehicle. [Figure 15] This is an example flowchart illustrating the procedure for estimating the roll angle and pitch angle of a vehicle. [Modes for carrying out the invention]

[0010] According to a preferred embodiment of the present invention, the information processing device includes: an acquisition unit that acquires a plurality of location information of features drawn on the road surface that are located near a moving body from map data including location information of features; a normal vector calculation unit that calculates a normal vector for an approximate plane obtained based on the plurality of location information; and an angle calculation unit that calculates at least one of the pitch angle or roll angle of the moving body based on the orientation of the moving body and the normal vector.

[0011] Generally, a moving object is constrained to the road surface, and its pitch and roll angles are determined by the slope of the road surface. Therefore, in this embodiment, the information processing device calculates a normal vector of a plane approximating the road surface based on the positional information of features drawn on the road surface. As a result, the information processing device can suitably calculate at least one of the pitch angle or roll angle of the moving object based on the relationship between the calculated normal vector and the orientation of the moving object.

[0012] In one embodiment of the above-described information processing device, the angle calculation unit calculates the pitch angle based on the dot product of a vector indicating the direction of travel of the moving body on a horizontal plane and the normal vector. In this embodiment, the information processing device can suitably determine the pitch angle of the moving body based on the angle between the direction of travel of the moving body on a horizontal plane and the normal vector of the approximate plane.

[0013] In another embodiment of the information processing device described above, the angle calculation unit calculates the roll angle based on the dot product of a vector indicating the lateral direction of the moving body on a horizontal plane and the normal vector. In this embodiment, the information processing device can suitably determine the roll angle of the moving body based on the angle between the lateral direction of the moving body on a horizontal plane and the normal vector of the approximate plane.

[0014] In another embodiment of the above-described information processing device, the information processing device further includes a position estimation unit that estimates the position of the mobile body by comparing data obtained by coordinate transformation of measurement data of features output by a measurement unit provided on the mobile body based on at least one of the pitch angle or roll angle with location information of the features registered in map data. In this embodiment, the information processing device estimates the position of the mobile body by matching the measurement data output by the measurement unit with location information of features based on map data. At this time, the information processing device coordinates the measurement data based on the calculated pitch angle or roll angle. As a result, the information processing device can suitably estimate the position of the mobile body using the measurement data even when the pitch angle or roll angle of the vehicle changes due to driving on slopes or roads with high transverse gradients.

[0015] In another embodiment of the above-described information processing device, the map data includes location information for multiple features, and the position estimation unit, when features surrounding the moving body are included in the multiple features, sets a predicted range based on the location information of the surrounding features in which the coordinate-transformed data of the surrounding features is expected to be included, and estimates the position of the moving body by comparing the data included in the predicted range with the location information of the surrounding features. The information processing device transforms the coordinates of the measurement data based on the calculated pitch angle or roll angle, so that even when the vehicle is traveling on a road surface where a pitch angle or roll angle occurs, the measurement data of the feature to be measured can be included in the predicted range set based on the location information of the feature. Therefore, even when the pitch angle or roll angle of the vehicle changes due to driving on a slope or a road with a high transverse gradient, the information processing device can suitably estimate the position of the moving body using the measurement data of the features obtained by the measurement unit.

[0016] In another embodiment of the above-described information processing device, the information processing device further comprises a height calculation unit that calculates the height of the moving body from a reference position based on the height of the plane calculated based on the approximate equation of the plane and information on the height of the moving body from the road surface. The reference position is a reference position in an absolute coordinate system used in maps, etc., and for example, indicates a position at an altitude of 0m. According to this embodiment, the information processing device can suitably calculate the height of the moving body from a reference position based on the equation of the plane used to calculate the pitch angle or roll angle.

[0017] According to another preferred embodiment of the present invention, a control method performed by an information processing device is provided, comprising: obtaining a plurality of location information of features drawn on the road surface near a moving body from map data including location information of features; calculating a normal vector for an approximate plane obtained based on the plurality of location information; and calculating at least one of the pitch angle or roll angle of the moving body based on the orientation of the moving body and the normal vector. By performing this control method, the information processing device can suitably calculate at least one of the pitch angle or roll angle of the moving body based on the relationship between the normal vector of the approximate plane based on the plurality of location information of features drawn on the road surface and the orientation of the moving body.

[0018] According to another preferred embodiment of the present invention, the program causes the computer to function as an acquisition unit that acquires a plurality of location information of features drawn on the road surface near a moving body from map data including location information of features; a normal vector calculation unit that calculates a normal vector for an approximate plane obtained based on the plurality of location information; and an angle calculation unit that calculates at least one of the pitch angle or roll angle of the moving body based on the orientation of the moving body and the normal vector. By executing this program, the computer can suitably calculate at least one of the pitch angle or roll angle of the moving body based on the relationship between the normal vector of the approximate plane based on the plurality of location information of features drawn on the road surface and the orientation of the moving body. Preferably, the program is stored in a storage medium. [Examples]

[0019] Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. Note that any character with "^" or "-" above it will be referred to as "A" in this specification for convenience. ^ " or "A - This is represented as (where "A" is any letter).

[0020] (1) Overview of the driver assistance system Figure 1 shows a schematic configuration of the driver assistance system according to this embodiment. The driver assistance system includes an in-vehicle unit 1 that moves with the vehicle, a Lidar (Light Detection and Ranging, or Laser Illuminated Detection and Ranging) 2, a gyro sensor 3, a vehicle speed sensor 4, and a GPS receiver 5.

[0021] The on-board unit 1 is electrically connected to the lidar 2, gyro sensor 3, vehicle speed sensor 4, and GPS receiver 5, and estimates the position of the vehicle on which the on-board unit 1 is installed (also called "vehicle position") based on their outputs. Then, based on the estimated vehicle position, the on-board unit 1 performs automatic driving control of the vehicle so that it travels along a set route to a destination. The on-board unit 1 stores information about landmarks such as landmark features and road markings located near the road (called "landmark data LD") and a map database (DB: Database) 10 that includes road data, etc. Then, the on-board unit 1 estimates the vehicle position by comparing the position of the landmarks indicated by the landmark data LD with the measurement results of the lidar 2.

[0022] The lidar 2 discretely measures the distance to an object in the external environment by emitting a pulsed laser within a predetermined angular range in the horizontal and vertical directions, and generates three-dimensional point cloud information indicating the position of the object. In this case, the lidar 2 has an irradiation unit that irradiates laser light while changing the irradiation direction, a light receiving unit that receives reflected light (scattered light) of the irradiated laser light, and an output unit that outputs scan data based on the received signal output by the light receiving unit. The scan data is generated based on the irradiation direction corresponding to the laser light received by the light receiving unit and the response delay time of the laser light, which is determined based on the received signal described above. Generally, the accuracy of the lidar's distance measurement is higher the closer the distance to the object, and lower the accuracy the farther the distance. The lidar 2, gyro sensor 3, vehicle speed sensor 4, and GPS receiver 5 each supply output data to the in-vehicle unit 1. The in-vehicle unit 1 is an example of an "information processing device" in the present invention, and the lidar 2 is an example of a "measurement unit" in the present invention.

[0023] The driver assistance system may also include an inertial measurement unit (IMU) that measures the acceleration and angular velocity of the vehicle in three axes, in place of or in addition to the gyro sensor 3.

[0024] (2) In-vehicle unit configuration Figure 2 is a block diagram showing the functional configuration of the in-vehicle unit 1. The in-vehicle unit 1 mainly consists of an interface 11, a storage unit 12, a communication unit 13, an input unit 14, a control unit 15, and an information output unit 16. Each of these elements is interconnected via a bus line.

[0025] Interface 11 acquires output data from sensors such as the rider 2, gyro sensor 3, vehicle speed sensor 4, and GPS receiver 5, and supplies it to the control unit 15. Interface 11 also supplies signals related to vehicle driving control generated by the control unit 15 to the vehicle's electronic control unit (ECU).

[0026] The storage unit 12 stores programs to be executed by the control unit 15 and information necessary for the control unit 15 to perform predetermined processing. In this embodiment, the storage unit 12 stores a map DB 10 that includes landmark data LD. Figure 3 shows an example of the data structure of the map DB 10. As shown in Figure 3, the map DB 10 includes facility information, road data, and landmark data LD.

[0027] Landmark data LD is information associated with each landmark feature, and includes a landmark ID (equivalent to a landmark index), location information, type information, and size information. Location information indicates the absolute position of the landmark, expressed by latitude and longitude (and elevation), etc. If the landmark is a road marking (a feature drawn on the road surface), the corresponding location information includes at least coordinate data indicating the discrete position of the road marking. Type information indicates the type of landmark and is information that can at least identify whether the target landmark is a road marking or not. Size information represents the size of the landmark and may, for example, be information indicating the length and / or width (width) of the landmark, or it may be information indicating the area of ​​the surface formed on the landmark.

[0028] The map DB 10 may be updated periodically. In this case, for example, the control unit 15 receives partial map information relating to the area to which the vehicle's position belongs from a server device that manages map information via the communication unit 13, and reflects it in the map DB 10. The storage unit 12 does not need to store the map DB 10. In this case, for example, the control unit 15 communicates with a server device that stores map data including landmark data LD via the communication unit 13, and acquires information necessary for vehicle position estimation processing, etc., at the necessary timing.

[0029] The input unit 14 includes buttons, a touch panel, a remote controller, a voice input device, etc., for user operation, and accepts inputs such as specifying a destination for route searching and specifying whether to turn autonomous driving on or off. The information output unit 16 includes, for example, a display or speaker that outputs based on the control of the control unit 15.

[0030] The control unit 15 includes a CPU that executes programs and controls the entire in-vehicle unit 1. In this embodiment, the control unit 15 has an attitude angle calculation unit 17 and a vehicle position estimation unit 18. The control unit 15 is an example of the "acquisition unit," "normal vector calculation unit," "angle calculation unit," "position estimation unit," "height calculation unit," and "computer" that executes programs in the present invention.

[0031] The attitude angle calculation unit 17 calculates at least one of the vehicle's pitch angle or roll angle by referring to landmark data LD. The vehicle position estimation unit 18 estimates the position of the vehicle on which the in-vehicle unit 1 is mounted (vehicle position) based on the output signals of each sensor supplied from the interface 11 and the map DB 10. In this embodiment, as will be described later, the vehicle position estimation unit 18 sets the vehicle's planar position (i.e., position on the horizontal plane specified by latitude and longitude) and yaw angle (i.e., direction) as estimation parameters.

[0032] (3) Overview of vehicle position estimation First, we will explain the overview of the vehicle position estimation process performed by the vehicle position estimation unit 18.

[0033] The vehicle position estimation unit 18 corrects the vehicle position estimated from the output data of the gyro sensor 3, vehicle speed sensor 4, and / or GPS receiver 5, based on the distance and angle measurements taken by the lidar 2 to landmarks and the location information of landmarks extracted from the map DB 10. In this embodiment, the vehicle position estimation unit 18 alternately performs a prediction step, which predicts the vehicle position from the output data of the gyro sensor 3, vehicle speed sensor 4, etc., based on a state estimation method using Bayesian estimation, and a measurement update step, which corrects the predicted value of the vehicle position calculated in the previous prediction step. Various filters developed for Bayesian estimation can be used as state estimation filters in these steps, such as extended Kalman filters, unscented Kalman filters, and particle filters. Thus, various methods have been proposed for position estimation based on Bayesian estimation.

[0034] In this embodiment, as an example, the vehicle position estimation unit 18 performs vehicle position estimation using an extended Kalman filter. Below, we will briefly explain vehicle position estimation using an extended Kalman filter.

[0035] Figure 4 shows the state variable vector represented in two-dimensional Cartesian coordinates. As shown in Figure 4, the position of the vehicle on a plane defined on the two-dimensional Cartesian coordinate system is represented by the coordinates "(x, y)" and the vehicle's orientation (yaw angle) "ψ". Here, the yaw angle ψ is defined as the angle between the vehicle's direction of travel and the x-axis. The coordinates (x, y) are either an absolute position corresponding to, for example, a combination of latitude and longitude, or a world coordinate indicating a position with a predetermined point as the origin.

[0036] Figure 5 shows a schematic relationship between the prediction step and the measurement update step. Figure 6 shows an example of the functional blocks of the vehicle position estimation unit 18. As shown in Figure 6, the vehicle position estimation unit 18 includes a dead reckoning block 21, a position prediction block 22, a coordinate transformation block 23, a landmark search and extraction block 24, and a position correction block 25.

[0037] As shown in Figure 5, the vehicle position estimation unit 18 sequentially calculates and updates the estimated value of the state variable vector "X" that indicates the vehicle's position by repeating the prediction step and the measurement update step. In Figure 5, the state variable vector of the reference time (i.e., the current time) "t" to be calculated is "X". - It is written as "(t)" or "X^(t)" ("State variable vector X(t) = (x(t), y(t), ψ(t))"). T (This is written as "). Here, the provisional estimate (predicted value) estimated in the prediction step is preceded by a "-" above the character representing the predicted value, and the more accurate estimate updated in the measurement update step is preceded by a "^" above the character representing the value.

[0038] Specifically, as shown in Figures 5 and 6, in the prediction step, the dead reckoning block 21 of the vehicle position estimation unit 18 takes the vehicle's moving speed "v" and angular velocity "ω" (collectively referred to as "control value u(t)=(v(t), ω(t))").T ), hereinafter denoted as ")", is used to obtain the travel distance and azimuth change from the previous time. The position prediction block 22 of the own vehicle position estimation unit 18 uses the state variable vector X at time t-1 calculated in the immediately preceding measurement update step ^ (t-1), adds the obtained travel distance and azimuth change to obtain a predicted value of the own vehicle position at time t (also referred to as "predicted own vehicle position") X - (t) is calculated. At the same time, the covariance matrix "P corresponding to the error distribution of the predicted own vehicle position X - (t)" (t) is calculated from the covariance matrix "P - (t-1)" at time t-1 calculated in the immediately preceding measurement update step. ^ .

[0039] In the measurement update step, the coordinate conversion block 23 of the own vehicle position estimation unit 18 converts the point cloud data output from the lidar 2 into the world coordinate system, which is the same coordinate system as the map DB 10. In this case, the coordinate conversion block 23 performs coordinate conversion on the point cloud data output by the lidar 2 at time t based on the predicted own vehicle position output by the position prediction block 22 at time t (that is, the planar position and azimuth of the vehicle) and the vehicle height and attitude angle (here, at least one of a pitch angle or a roll angle) output by the attitude angle calculation unit 17 at time t. Details of this coordinate conversion will be described later.

[0040] Then, the landmark search and extraction block 24 of the vehicle position estimation unit 18 associates the position vector of the landmark to be measured, which is registered in the map DB 10, with the point cloud data of the LIDA 2, which has been converted to the world coordinate system by the coordinate transformation block 23. In this case, the landmark search and extraction block 24 determines whether or not there is a landmark within the detection range of the LIDA 2, based on the landmark data LD and the predicted vehicle position output by the position prediction block 22. If a detectable landmark is registered in the landmark data LD, the landmark search and extraction block 24 sets a prediction window "Wp" that defines the range in which to detect the landmark. In this case, the landmark search and extraction block 24 sets a prediction window Wp of a predetermined size centered on the position information of the target landmark based on the landmark data LD in the coordinate (world coordinate) space of the coordinate-transformed point cloud data of the LIDA 2. The prediction window Wp may be a cube, for example. Then, within the prediction window Wp, the landmark search and extraction block 24 determines whether or not there are measurement points of high reflectivity point cloud data that have a reflectivity of a predetermined threshold or higher. Then, if such a measurement point exists, the landmark search and extraction block 24 determines that the above correspondence has been made, and the measured value "Z(t)" by the rider 2 of the matching landmark and the predicted vehicle position X - (t) and the measured predicted value "Z" is a vector value indicating the location of landmarks based on the map DB10. - The landmark search and extraction block 24 determines the position of the measurement point extracted from the point cloud data within the prediction window Wp, which is a representative position (for example, a position based on the average of coordinate values), as the measurement value Z(t) of the target landmark.

[0041] Then, the position correction block 25 of the self-vehicle position estimation unit 18 uses the measured value Z(t) and the measured predicted value Z as shown in equation (1) below. - The difference value from (t) is multiplied by the Kalman gain "K(t)", and this is used to predict the vehicle's position X - By adding this to (t), the updated state variable vector (also called "estimated vehicle position") X is obtained.^ Calculate (t).

[0042]

number

[0043] Furthermore, if the vehicle position estimation unit 18 can associate the position vectors of multiple features registered in the map DB 10 with the scan data of the lidar 2, it may perform a measurement update step based on any selected set of measurement prediction values ​​and measurement values, or it may perform the measurement update step multiple times based on all the measurement prediction values ​​and measurement values ​​that can be associated. When using multiple measurement prediction values ​​and measurement values, the vehicle position estimation unit 18 should consider that the lidar measurement accuracy deteriorates the further the feature is from the lidar 2, and should reduce the weighting for the feature as the distance between the lidar 2 and the feature increases.

[0044] In this way, the prediction step and the measurement update step are repeatedly performed, and the predicted vehicle position X - (t) and estimated vehicle position X ^ The most likely position of the vehicle is calculated by sequentially calculating (t).

[0045] (4) Calculation of attitude angle Next, we will explain how the vehicle's attitude angles, namely the pitch angle and roll angle, are calculated by the attitude angle calculation unit 17 using landmark data LD.

[0046] (4-1) Calculation of pitch angle First, we will explain how the vehicle's pitch angle is calculated by the attitude angle calculation unit 17.

[0047] The attitude angle calculation unit 17 refers to the landmark data LD and the vehicle position estimation unit 18 predicts or estimates the vehicle's planar position x, y (i.e., (x - , y - ) or (x ^ , y ^ The system searches for lane markings (white lines) in the vicinity of the lane markings. The attitude angle calculation unit 17 then refers to the landmark data LD corresponding to the searched lane markings and obtains the position information of "n" world coordinate systems for those lane markings. When the landmark is a lane marking, the landmark data LD contains coordinate data of latitude, longitude, and altitude indicating discrete positions (discrete points) of the lane markings, for example, at intervals of several meters. The position information of the n world coordinate systems for the lane markings obtained by the attitude angle calculation unit 17 is an example of "multiple position information of features drawn on the road surface."

[0048] Figure 7(A) is an overhead view of a vehicle on a road with the location information of lane markings 60 to 62 clearly indicated. Figure 7(B) is a side view of a vehicle on a road with the location information of lane marking 62 clearly indicated. In Figure 7(A), a vehicle is traveling on a road where lane markings 60 to 62 exist. Lane marking 60 has locations P1 to P3 indicated by the location information registered in the landmark data LD, lane marking 61 has locations P4 to P6 indicated by the location information registered in the landmark data LD, and lane marking 62 has locations P7 to P14 indicated by the location information registered in the landmark data LD.

[0049] In this case, the attitude angle calculation unit 17 refers to the landmark data LD and the vehicle position estimation unit 18 predicts or estimates the vehicle's planar position x, y (here, (x ^ , y ^ The system recognizes that there are demarcation lines 60-62 within a predetermined distance from the system. The attitude angle calculation unit 17 then refers to landmark data LD corresponding to the demarcation lines 60-62 to obtain world coordinate system position information for a total of 14 (i.e., n=14) positions P1-P14 on the demarcation lines 60-62.

[0050] Next, the attitude angle calculation unit 17 considers the road surface as a plane and expresses the equation of the plane approximating the road surface by the following equation (2).

[0051]

number

[0052]

number

[0053]

number

[0054]

number

[0055]

number

[0056]

number

[0057]

number

[0058]

number

[0059]

number

[0060]

number

[0061] Figure 9 shows the relationship between the angle θ' between the normal vector Vn and the direction of travel vector Vx and the pitch angle θ of the vehicle. As shown in Figure 9, the angle θ' between the normal vector Vn and the direction of travel vector Vx is 90 degrees (i.e., π / 2) greater than the pitch angle θ of the vehicle. Also, since the vehicle is constrained to the road surface, the inclination of the road surface in the direction of travel of the vehicle is equal to the pitch angle θ of the vehicle. Therefore, the attitude angle calculation unit 17 calculates the pitch angle θ based on the following equation (12).

[0062]

number

[0063] (4-2) Estimation of Roll Angle The attitude angle calculation unit 17 calculates the normal vector Vn shown in equation (9) based on n section line position vectors based on landmark data LD, similar to the calculation of the pitch angle.

[0064] Furthermore, the attitude angle calculation unit 17 calculates the vehicle's lateral vector "V" on the xy plane. Y The yaw angle is determined based on the yaw angle predicted or estimated by the vehicle position estimation unit 18. Figure 10 shows the yaw angle of the vehicle (here ψ ^ (Assume) and the horizontal vector V Y This is an xy-plane diagram showing the relationship between the horizontal vector V. As shown in Figure 10, the horizontal vector VY This is the yaw angle (here ψ) predicted or estimated by the vehicle position estimation unit 18. ^ This corresponds to the direction obtained by rotating (let's call it) by 90 degrees (π / 2) along the xy-plane. Therefore, the horizontal vector V Y This is given by the following equation (13).

[0065]

number

[0066]

number

[0067] Figure 11 shows the normal vector Vn and the transverse vector V Y This figure shows the relationship between the angle φ' formed by the vehicle and the roll angle φ. As shown in Figure 11, the normal vector Vn and the lateral vector V Y The angle φ' formed by the vehicle is 90 degrees (i.e., π / 2) greater than the vehicle's roll angle φ. Also, since the vehicle is constrained to the road surface, the inclination of the road surface in the lateral direction of the vehicle is equal to the vehicle's roll angle φ. Therefore, the attitude angle calculation unit 17 calculates the roll angle φ based on the following equation (15).

[0068]

number

[0069] (5) Coordinate transformation of point cloud data Next, we will explain the coordinate transformation of the point cloud data based on the pitch angle calculated by the attitude angle calculation unit 17.

[0070] The coordinate transformation block 23 of the vehicle position estimation unit 18 uses the pitch angle θ calculated by the attitude angle calculation unit 17 to perform the rotation matrix "R" shown in the following equation (16). θ This generates ".

[0071]

number

[0072]

number

[0073]

number

[0074] Next, we will explain the coordinate transformation that takes into account both the pitch angle θ and the roll angle φ. The coordinate transformation block 23 is a rotation matrix R based on the pitch angle θ calculated by the attitude angle calculation unit 17. θ And, the rotation matrix "R" based on the roll angle φ φ Using this, the matrix X represents the n 3D data output by lider 2. L The rotation matrix "R" multiplied by is created based on the following equation (19).

[0075]

number

[0076]

number

[0077] Furthermore, the coordinate transformation block 23 may perform a process to transform point cloud data representing each of the three-dimensional positions relative to the lidar 2, based on the combination of distance and scan angle measured by the lidar 2, into a vehicle coordinate system. The vehicle coordinate system is the vehicle's coordinate system with the vehicle's direction of travel and lateral direction as axes. In this case, the coordinate transformation block 23 transforms the point cloud data from the coordinate system relative to the lidar 2 to the vehicle coordinate system based on information about the installation position and installation angle of the lidar 2 relative to the vehicle, and then transforms the point cloud data transformed into the vehicle coordinate system into the world coordinate system using the method described above. The process of transforming point cloud data output by a lidar installed on a vehicle into a vehicle coordinate system is disclosed, for example, in International Publication WO2019 / 188745.

[0078] Furthermore, the coordinate transformation block 23 may perform a coordinate transformation that considers only the roll angle φ among the pitch angle θ and roll angle φ. In this case, the coordinate transformation block 23 uses the rotation matrix R shown in equation (19). φ This is the matrix X representing the n 3D data points output by rider 2. L You should take advantage of that.

[0079] Next, the effects of the coordinate transformation described above will be explained in more detail with reference to Figures 12 and 13.

[0080] Figure 12(A) is a side view of a vehicle traveling on a flat road surface and the road. In this case, the vehicle position estimation unit 18 refers to landmark data LD and, if it extracts a sign "L1" located around the current position as a landmark, it obtains the height (e.g., elevation) "h" of the sign L1 from the reference position of the sign L1 from the position information of the sign L1. Then, the vehicle position estimation unit 18 obtains the predicted or estimated vehicle height z (here, the estimated vehicle height z) ^ The difference between (and) the label L1 is "Hz ^ The prediction window Wp is set to a position higher than the vehicle. The vehicle position estimation unit 18 then calculates the measured value Z(t) based on the average value of the measurement points of the point cloud data of the lidar 2 with high reflectivity that are within the prediction window Wp, and performs vehicle position estimation based on equation (1).

[0081] Figure 12(B) is a side view of a vehicle and road traveling on a steeply sloping surface, where no coordinate transformation based on the vehicle's pitch angle is performed on the point cloud data. In Figure 12(B), the point cloud data points upward because a pitch angle is generated in the vehicle due to the road surface gradient. Therefore, the vehicle position estimation unit 18 calculates the vehicle height z ^ The difference between the height of marker L1 based on landmark data LD and "Hz" ^ If a prediction window Wp is set with a center higher than the vehicle, the marker L1 will not be included in the prediction window Wp. Therefore, in this case, the vehicle position estimation unit 18 cannot detect the measurement point of the point cloud data of the lidar 2 corresponding to the marker L1, and cannot perform the vehicle position estimation process using the marker L1 as a landmark.

[0082] Figure 13 is a side view of a vehicle and road traveling on a road with a steep gradient, when a coordinate transformation based on the vehicle's pitch angle is performed on point cloud data according to this embodiment. In this case, the point cloud data output by lidar 2 is transformed into a world coordinate system that takes the vehicle's pitch angle into account by a rotation matrix using the pitch angle obtained by the calculation method according to the embodiment. Therefore, in this case, the height from the vehicle to each measurement point in the point cloud data is the height in the world coordinate system. Thus, in this case, the vehicle position estimation unit 18 determines the vehicle height z ^ The difference between the height of sign L1 and the height of sign L1 is "Hz ^ By setting a prediction window Wp centered at a position higher than the vehicle, it is possible to extract measurement points corresponding to marker L1 from the point cloud data output by rider 2.

[0083] Even when driving on roads with transverse gradients such as banks, the on-board unit 1 can perform coordinate transformations on the point cloud data based on the vehicle's roll angle, thereby suitably extracting measurement points of landmarks to be measured from the point cloud data based on the set prediction window Wp.

[0084] (6) Processing flow Next, the specific processing flow of the vehicle position and attitude estimation process, including the estimation of the pitch angle and roll angle described above, will be explained with reference to the flowchart.

[0085] (6-1) Overview of Vehicle Position and Attitude Estimation Process Figure 14 is an example of a flowchart showing the procedure for estimating the vehicle's position and attitude. The attitude angle calculation unit 17 and the vehicle position estimation unit 18 of the in-vehicle unit 1 repeatedly execute the process shown in the flowchart of Figure 14 at predetermined time intervals when the vehicle's position and attitude should be estimated. The symbols shown to the right of each step in Figure 14 represent the elements calculated in each step.

[0086] First, the dead reckoning block 21 of the vehicle position estimation unit 18 uses the vehicle's movement speed and angular velocity based on the outputs of the gyro sensor 3, vehicle speed sensor 4, and GPS receiver 5 to determine the distance traveled and the change in direction from the previous time. Based on this, the position prediction block 22 of the vehicle position estimation unit 18 calculates the estimated vehicle position x obtained one time step earlier (the most recent processing time). ^ , y ^ ψ ^ From there, the predicted vehicle position x at the current time - , y - ψ - Calculate (Step S11).

[0087] Next, the attitude angle calculation unit 17 performs estimation processing of the vehicle height (e.g., altitude), roll angle, and pitch angle, thereby predicting the vehicle height "z - ", predicted roll angle "φ - " and predicted pitch angle "θ - The result is calculated (Step S12). This process will be described later with reference to Figure 15.

[0088] Then, the coordinate transformation block 23 of the vehicle position estimation unit 18 generates a rotation matrix R (see equation (19)) based on the roll angle and pitch angle calculated in step S12 (step S13). Then, the coordinate transformation block 23 converts the point cloud data into world coordinate system data (step S14). After that, the vehicle position estimation unit 18 (landmark search / extraction block 24 and position correction block 25) performs vehicle position estimation processing based on equation (1) using the coordinate-transformed point cloud data and landmark data LD, and estimates the vehicle position x at the current time. ^ , y ^ ψ ^ The attitude angle calculation unit 17 then calculates the estimated vehicle position x at the current time. ^ , y ^ ψ ^ Using this, the same vehicle height, roll angle, and pitch angle estimation process as in step S12 is performed again, and the estimated vehicle height "z ^ ", estimated roll angle "φ ^ " and estimated pitch angle "θ ^ The result is calculated (Step S16). This process will be described later with reference to Figure 15.

[0089] (6-2) Estimation process for vehicle height, roll angle, and pitch angle Figure 15 is an example flowchart showing the procedure for estimating vehicle height, roll angle, and pitch angle, which is performed in steps S12 and S16 of Figure 14.

[0090] First, the attitude angle calculation unit 17 refers to the landmark data LD and predicts the vehicle's position x - , y - or estimated vehicle position x ^ , y ^ The position information of the lane markings in the vicinity is extracted (step S21). For example, the attitude angle calculation unit 17 calculates the predicted vehicle position x - , y - or estimated vehicle position x ^ , y ^ Landmark data LD for lane boundaries, including coordinate data indicating a location within a predetermined distance, is extracted from map DB10.

[0091] Next, the attitude angle calculation unit 17 determines whether or not the number of coordinate data of the lane markings extracted in step S21 is less than three (step S22). Then, when the number of coordinate data of the lane markings extracted in step S21 is less than three (step S22; Yes), the attitude angle calculation unit 17 sets the predicted vehicle height z one time step earlier - or the estimated vehicle height z ^ as the predicted vehicle height z to be obtained - or the estimated vehicle height z ^ (step S28). Further, in step S28, the attitude angle calculation unit 17 uses the roll angle φ calculated one time step earlier (φ - or φ ^ ) or the pitch angle θ (θ - or θ ^ ) as the roll angle φ at the current time to be obtained (φ - or φ ^ ) or the pitch angle θ (θ - or θ ^ ).

[0092] On the other hand, when the number n of coordinate data of the lane markings extracted in step S21 is three or more (n≧3) (step S22; No), the attitude angle calculation unit 17 creates a matrix C and a vector b from the coordinate data of n lane markings in the world coordinate system according to equations (4) and (5) (step S23). Next, the attitude angle calculation unit 17 calculates a coefficient vector a by the least square method based on equation (7), and substitutes the predicted own vehicle position x - , y - or the estimated own vehicle position x ^ , y ^ into the plane equation of equation (2), thereby calculating the predicted vehicle height z at the current time - or the estimated vehicle height z ^ (step S24).

[0093] Next, the attitude angle calculation unit 17 identifies the normal vector Vn shown in equation (9) from the calculated coefficient vector a (step S25). Then, the attitude angle calculation unit 17 uses the predicted or estimated vehicle yaw angle (azimuth) ψ (ψ - or ψ ^Using ), the direction vector Vx shown in equation (10) and the lateral vector V shown in equation (13) are used. Y The attitude angle calculation unit 17 then calculates the dot product of the normal vector Vn and the direction of travel vector Vx based on equation (11), and also calculates the dot product of the normal vector Vn and the lateral vector Vx based on equation (14). Y Calculate the dot product and find the roll angle φ(φ) at the current time. - or φ ^ ) or pitch angle θ(θ - or θ ^ Calculate (Step S27).

[0094] As described above, the control unit 15 of the in-vehicle unit 1 according to this embodiment acquires multiple location information of features drawn on the road surface near the vehicle from landmark data LD, which is map data containing location information of features. The control unit 15 then calculates a normal vector for an approximate plane obtained based on the multiple location information acquired. The control unit 15 then calculates at least one of the pitch angle or roll angle of the vehicle based on the orientation of the vehicle and the normal vector. In this configuration, the in-vehicle unit 1 can calculate at least one of the pitch angle or roll angle with high accuracy based on landmark data LD.

[0095] (7) Variation The following describes suitable modifications of the above-described embodiments. These modifications may be applied in combination to these embodiments.

[0096] (Variation 1) Even if the in-vehicle unit 1 does not perform self-position estimation based on equation (1), it may still perform estimation of at least one of the vehicle's pitch angle or roll angle based on the embodiment.

[0097] In this case, for example, the in-vehicle unit 1 repeatedly performs steps S11 and S12 in Figure 14 to repeatedly estimate at least one of the vehicle's pitch angle or roll angle based on the landmark data LD. Even in this case, the in-vehicle unit 1 can use the estimated pitch angle and / or roll angle for various applications such as coordinate transformation of output data from external sensors such as the lidar 2 or slope (bank) detection processing.

[0098] (Modification 2) The configuration of the driver assistance system shown in Figure 1 is an example, and the configuration of a driver assistance system to which the present invention can be applied is not limited to the configuration shown in Figure 1. For example, instead of having an in-vehicle unit 1, the driver assistance system may have the vehicle's electronic control unit perform the processing of the attitude angle calculation unit 17 and the vehicle position estimation unit 18 of the in-vehicle unit 1. In this case, the map DB 10 is stored, for example, in a storage unit in the vehicle or in a server device that communicates data with the vehicle, and the vehicle's electronic control unit refers to this map DB 10 to perform the estimation of the roll angle and / or pitch angle and the estimation of the vehicle position based on equation (1), etc.

[0099] Although the present invention has been described above with reference to embodiments, the present invention is not limited to the above embodiments. Various modifications to the structure and details of the present invention can be made that are understandable to those skilled in the art within the scope of the present invention. That is, the present invention naturally includes the full disclosure, including the claims, and various modifications and alterations that those skilled in the art could make in accordance with the technical idea. Furthermore, each disclosure of the above-mentioned patent documents and other references is incorporated herein by reference. [Explanation of symbols]

[0100] 1 On-vehicle device 2 Riders 3. Gyroscope sensor 4. Vehicle speed sensor 5 GPS receivers 10 Map Database

Claims

1. An acquisition unit that acquires multiple location information of features drawn on the road surface that are located near a moving object from map data that includes location information of features, A normal vector calculation unit calculates a normal vector for an approximated plane obtained based on the aforementioned plurality of positional information, An angle calculation unit that calculates at least one of the pitch angle or roll angle of the moving body based on the orientation of the moving body and the normal vector, An information processing device having

2. The information processing apparatus according to claim 1, wherein the angle calculation unit calculates the pitch angle based on the dot product of a vector indicating the direction of travel of the moving body on a horizontal plane and the normal vector.

3. The information processing apparatus according to claim 1, wherein the angle calculation unit calculates the roll angle based on the dot product of a vector indicating the lateral orientation of the moving body on a horizontal plane and the normal vector.

4. The information processing apparatus according to any one of claims 1 to 3, further comprising a position estimation unit that estimates the position of the moving body by comparing data obtained by coordinate transformation of measurement data of features output by a measurement unit provided on the moving body based on at least one of the pitch angle or roll angle with position information of features registered in map data.

5. The aforementioned map data contains location information for multiple features, The position estimation unit, If the features surrounding the moving object are included in the plurality of features, the predicted range in which the coordinate-transformed data of the surrounding features is expected to be included is set based on the location information of the surrounding features. The information processing device according to claim 4, which performs position estimation of the moving object by comparing the data included in the prediction range with the position information of the surrounding features.

6. The information processing apparatus according to any one of claims 1 to 5, further comprising a height calculation unit that calculates the height of the moving body from a reference position based on the height of the plane from a reference position calculated based on the equation of the approximated plane and information on the height of the moving body from the road surface.

7. A method executed by an information processing device, From map data containing location information of features, obtain location information for multiple features drawn on the road surface that are located near a moving object. Based on the aforementioned multiple positional information, a normal vector is calculated for the approximated plane. A method for calculating at least one of the pitch angle or roll angle of a moving body based on the orientation of the moving body and the normal vector.

8. An acquisition unit that acquires multiple location information of features drawn on the road surface that are located near a moving object from map data that includes location information of features, A normal vector calculation unit calculates a normal vector for an approximated plane obtained based on the aforementioned plurality of positional information, Angle calculation unit calculates at least one of the pitch angle or roll angle of the moving body based on the orientation of the moving body and the normal vector. A program that makes a computer function.

9. A storage medium storing the program described in claim 8.

Images