Information processing device, control method, program, and storage medium

Abstract

To provide an information processing device capable of suitably estimating a posture of a vehicle.SOLUTION: A control section 15 of an in-vehicle device 1 acquires a plurality of pieces of position information of a feature which is drawn on a road surface and exists in the vicinity of a vehicle from landmark data LD that is map data including position information of a feature. The control section 15 calculates a normal vector of an approximate plane calculated on the basis of the plurality of pieces of acquired position information. Then, the control section 15 calculates at least one of a pitch angle or a roll angle of an own vehicle on the basis of the orientation of the own vehicle and the normal vector.SELECTED DRAWING: Figure 15

Description

[Technical field]

[0001] The present invention relates to a vehicle attitude estimation technique. [Background technology]

[0002] Conventionally, there are known techniques for estimating a vehicle's own position based on measurement data from a measurement unit such as a radar or a camera. For example, Patent Document 1 discloses a technique for estimating a vehicle's own position by matching the output of a measurement sensor with position information of a feature registered in advance on a map. Patent Document 2 discloses a vehicle's own position estimation technique using a Kalman filter. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] JP 2013-257742 A [Patent Document 2] JP 2017-72422 A Summary of the Invention [Problem to be solved by the invention]

[0004] Generally, a vehicle is constrained to the road surface, and the changes in the roll angle, pitch angle, and vertical direction of the vehicle are negligibly small, although there are fluctuations due to the suspension. Therefore, in a general vehicle self-location estimation based on the measurement data of the measurement unit, the horizontal position and orientation of the vehicle are set as parameters to be estimated. On the other hand, when traveling on a slope with a large gradient or a road including a cross gradient, only estimating the horizontal position and orientation of the vehicle cannot accommodate changes in the pitch angle and roll angle, and the measured position of the feature measured by the measurement unit may not be properly associated with the position of the feature on the map. In this case, the measurement data used for self-location estimation may be reduced, and the robustness of the self-location estimation may be reduced. Here, if the pitch angle and roll angle are added as estimation parameters for self-location estimation to accommodate changes in the pitch angle and roll angle, the calculation load increases due to the increase in the estimation parameters, and the self-location estimation may not be completed stably at the required period. In addition, there are methods to calculate the pitch and roll angles of a vehicle from data from an Inertial Measurement Unit (IMU), but there is a problem that typical IMUs have sensitivity errors and offsets, making it difficult to accurately calculate the pitch and roll angles.

[0005] The present invention has been made in order to solve the above-mentioned problems, and has as its main object 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 claims is an information processing device comprising: an acquisition unit that acquires multiple position information of features located in the vicinity of a moving body from map data including position information of the features; a normal vector calculation unit that calculates a normal vector with respect to an approximated plane obtained based on the multiple position information; and an angle calculation unit that calculates at least one of a pitch angle or a roll angle of the moving body based on the orientation of the moving body and the normal vector. [Brief description of the drawings]

[0007] [Figure 1] FIG. 1 is a schematic configuration diagram of a driving assistance system. [Diagram 2] FIG. 2 is a block diagram showing a functional configuration of an in-vehicle device. [Diagram 3] 2 shows an example of a schematic data structure of a map DB (DataBase). [Figure 4] This is a diagram showing state variable vectors in two-dimensional orthogonal coordinates. [Diagram 5] FIG. 1 illustrates a schematic relationship between a prediction step and a measurement update step. [Figure 6] 4 is an example of a functional block of a vehicle position estimation unit. [Figure 7] 1A is an overhead view of a vehicle on a road with position information of lane markings clearly indicated, and FIG. 1B is a side view of a vehicle on a road with position information of lane markings clearly indicated. [Figure 8] FIG. 2 is an xy plan view showing the relationship between the yaw angle and the traveling direction vector of a vehicle. [Figure 9] FIG. 4 is a diagram showing the relationship between the angle between a normal vector and a traveling direction vector and the pitch angle of the vehicle. [Figure 10] FIG. 2 is an xy plan view showing the relationship between the yaw angle and the lateral vector of a vehicle. [Figure 11] 11 is a diagram showing the relationship between the angle between a normal vector and a lateral vector and the roll angle of the vehicle. FIG. [Figure 12] 1A is a side view of a vehicle and a road traveling on a flat road surface, and FIG. 1B is a side view of a vehicle and a road traveling on a road surface with a large gradient when coordinate transformation based on the pitch angle of the vehicle is not performed on the point cloud data. [Figure 13] FIG. 11 is a side view of a vehicle and a road traveling on a road surface with a steep gradient when coordinate transformation based on the pitch angle of the vehicle is performed on point cloud data. [Figure 14] 10 is an example of a flowchart illustrating a procedure for estimating a position and attitude of a vehicle. [Figure 15] 1 is an example of a flowchart showing a procedure of a process for estimating a roll angle and a pitch angle of a vehicle. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0008] According to a preferred embodiment of the present invention, an information processing device has an acquisition unit that acquires multiple pieces of position information of features drawn on a road surface that are located in the vicinity of a moving body from map data including position information of the features, a normal vector calculation unit that calculates a normal vector with respect to an approximated plane obtained based on the multiple pieces of position information, and an angle calculation unit that calculates at least one of a pitch angle or a roll angle of the moving body based on the vehicle orientation of the moving body and the normal vector.

[0009] Generally, a moving body is restrained by a road surface, and a pitch angle and a roll angle of the moving body occur according to the inclination of the road surface. Therefore, in this aspect, the information processing device calculates a normal vector of a plane that approximates the road surface based on the position information of a feature drawn on the road surface. This allows the information processing device to suitably calculate at least one of the pitch angle and the roll angle of the moving body based on the relationship between the calculated normal vector and the orientation of the moving body.

[0010] In one aspect of the information processing device, the angle calculation unit calculates the pitch angle based on an inner product of a vector indicating the direction of travel of the moving object on a horizontal plane and the normal vector. With this aspect, the information processing device can suitably obtain the pitch angle of the moving object based on the angle between the traveling direction of the moving object on the horizontal plane and the normal vector of the approximation plane.

[0011] In another aspect of the information processing device, the angle calculation unit calculates the roll angle based on an inner product of a vector indicating a lateral direction of the moving body on a horizontal plane and the normal vector. With this aspect, the information processing device can suitably obtain the roll angle of the moving body based on an angle between the lateral direction of the moving body on the horizontal plane and the normal vector of the approximation plane.

[0012] In another aspect of the information processing device, the information processing device further includes a position estimation unit that performs position estimation of the moving body by comparing data obtained by performing coordinate conversion of measurement data of a feature output by a measurement unit provided in the moving body based on at least one of the pitch angle or roll angle with position information of the feature registered in map data. In this aspect, the information processing device performs position estimation of the moving body by comparing (matching) the measurement data output by the measurement unit with the position information of the feature based on the map data. At this time, the information processing device performs coordinate conversion of the measurement data based on the calculated pitch angle or roll angle. As a result, the information processing device can suitably perform position estimation of the moving body using the measurement data even if a change in the pitch angle or roll angle of the vehicle occurs due to driving on a slope or a road with a high cross-gradient.

[0013] In another aspect of the information processing device, the map data includes position information of a plurality of features, and when a feature around the moving body is included in the plurality of features, the position estimation unit sets a prediction range in which the coordinate-transformed data of the surrounding feature is predicted to be included based on the position information of the surrounding feature, and compares the data included in the prediction range with the position information of the surrounding feature to estimate the position of the moving body. The information processing device performs coordinate transformation of the measurement data based on the calculated pitch angle or roll angle, so that the measurement data of the measurement target feature can be included in the prediction range set based on the position information of the feature, even when the vehicle runs on a road surface on which a pitch angle or roll angle occurs. Therefore, the information processing device can suitably estimate the position of the moving body using the measurement data of the feature obtained by the measurement unit, even when a change in the pitch angle or roll angle of the vehicle occurs due to running on a slope or a road with a high cross-gradient.

[0014] In another aspect of the information processing device, the information processing device further includes 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 equation of the approximated 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 indicates, for example, a position at an altitude of 0 m. According to this aspect, the information processing device can suitably calculate the height of the moving body from the reference position based on the equation of the plane used to calculate the pitch angle or roll angle.

[0015] According to another preferred embodiment of the present invention, there is provided a control method executed by an information processing device, which includes acquiring a plurality of pieces of position information of features drawn on a road surface that are present near a moving body from map data including position information of the features, calculating a normal vector to an approximated plane determined based on the plurality of pieces of position information, and calculating at least one of a pitch angle or a roll angle of the moving body based on the vehicle orientation of the moving body and the normal vector. By executing this control method, the information processing device can preferably calculate at least one of a pitch angle or a roll angle of the moving body based on the relationship between the normal vector of the approximation plane based on the plurality of pieces of position information of the features drawn on the road surface and the orientation of the moving body.

[0016] According to another preferred embodiment of the present invention, the program causes a computer to function as an acquisition unit that acquires multiple pieces of position information of features drawn on a road surface that are present near a moving body from map data including position information of the features, a normal vector calculation unit that calculates a normal vector to an approximated plane determined based on the multiple pieces of position information, and an angle calculation unit that calculates at least one of a pitch angle or a roll angle of the moving body based on the vehicle orientation of the moving body and the normal vector. By executing this program, the computer can preferably calculate at least one of a pitch angle or a roll angle of the moving body based on the relationship between the normal vector of the approximated plane based on the multiple pieces of position information of features drawn on the road surface and the orientation of the moving body. Preferably, the above program is stored in a storage medium. EXAMPLES

[0017] Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. In this specification, for the sake of convenience, a character with "^" or "-" added to any symbol is referred to as "A ^ " or "A - ” (where “A” is any letter).

[0018] (1) Overview of the driving assistance system 1 is a schematic configuration of a driving assistance system according to the present embodiment. The driving assistance system includes an on-board device 1 that moves together with a vehicle, which is a moving body, 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.

[0019] The vehicle-mounted device 1 is electrically connected to the LIDAR 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5, and estimates the position of the vehicle in which the vehicle-mounted device 1 is installed (also referred to as "vehicle position") based on the outputs of these sensors. Then, based on the estimation result of the vehicle position, the vehicle-mounted device 1 performs automatic driving control of the vehicle so that the vehicle travels along a route to a set destination. The vehicle-mounted device 1 stores a map database (DB: DataBase) 10 including information (referred to as "landmark data LD") on landmarks such as land features and lane lines that serve as landmarks near roads, as well as road data and the like. Then, the vehicle-mounted device 1 compares the positions of the landmarks indicated by this landmark data LD with the measurement results of the LIDAR 2 to estimate the vehicle position.

[0020] The lidar 2 measures the distance to an object in the outside world discretely by emitting a pulsed laser in 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 a laser beam while changing the irradiation direction, a light receiving unit that receives reflected light (scattered light) of the irradiated laser beam, and an output unit that outputs scan data based on the light receiving signal output by the light receiving unit. The scan data is generated based on the irradiation direction corresponding to the laser beam received by the light receiving unit and the response delay time of the laser beam specified based on the above-mentioned light receiving signal. In general, the closer the distance to the object, the higher the accuracy of the distance measurement value of the lidar, and the farther the distance, the lower the accuracy. The lidar 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5 each supply output data to the vehicle-mounted device 1. The vehicle-mounted device 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.

[0021] Note that the driving assistance system may have, instead of or in addition to the gyro sensor 3, an inertial measurement unit (IMU) that measures the acceleration and angular velocity of the measurement vehicle in three axial directions.

[0022] (2) Vehicle-mounted device configuration 2 is a block diagram showing the functional configuration of the vehicle-mounted device 1. The vehicle-mounted device 1 mainly includes 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. These elements are connected to each other via a bus line.

[0023] The interface 11 acquires output data from sensors such as the rider 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5, and supplies the data to the control unit 15. The interface 11 also supplies signals related to vehicle travel control generated by the control unit 15 to an electronic control unit (ECU) of the vehicle.

[0024] The storage unit 12 stores programs executed by the control unit 15 and information necessary for the control unit 15 to execute predetermined processes. In this embodiment, the storage unit 12 stores a map DB 10 including landmark data LD. Fig. 3 shows an example of the data structure of the map DB 10. As shown in Fig. 3, the map DB 10 includes facility information, road data, and landmark data LD.

[0025] The landmark data LD is information in which information about each landmark feature is associated with the landmark, and here includes a landmark ID equivalent to an index of the landmark, location information, type information, and size information. The location information indicates the absolute location of the landmark expressed by latitude and longitude (and altitude), etc. In addition, when the landmark is a latitude line (a feature drawn on the road surface), the corresponding location information includes at least coordinate data indicating the discrete positions of the latitude line. The type information is information indicating the type of the landmark, and is information that can at least identify whether the target landmark is a latitude line or not. The size information is information that indicates the size of the landmark, and may be, for example, information indicating the vertical and / or horizontal length (width) of the landmark, or information indicating the area of ​​the surface formed on the landmark.

[0026] The map DB 10 may be updated periodically. In this case, for example, the control unit 15 receives partial map information related to the area to which the vehicle position belongs from a server device that manages map information via the communication unit 13, and reflects the partial map information 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 the landmark data LD via the communication unit 13, thereby acquiring information required for the vehicle position estimation process and the like at the required timing.

[0027] The input unit 14 is a button, a touch panel, a remote controller, a voice input device, etc. for a user to operate, and accepts inputs specifying a destination for route search, inputs specifying on / off of autonomous driving, etc. The information output unit 16 is, for example, a display, a speaker, etc. that perform output based on the control of the control unit 15.

[0028] The control unit 15 includes a CPU that executes a program and controls the entire in-vehicle device 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 the program in the present invention.

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

[0030] (3) Overview of vehicle position estimation First, an overview of the vehicle position estimation process performed by the vehicle position estimating unit 18 will be described.

[0031] The vehicle position estimation unit 18 corrects the vehicle position estimated from the output data of the gyro sensor 3, the vehicle speed sensor 4, and / or the GPS receiver 5 based on the measurement values ​​of the distance and angle from the lidar 2 to the landmark and the position information of the landmark extracted from the map DB 10. In this embodiment, the vehicle position estimation unit 18 alternately executes a prediction step of predicting the vehicle position from the output data of the gyro sensor 3, the vehicle speed sensor 4, etc., based on a state estimation method using Bayesian estimation, and a measurement update step of correcting the predicted value of the vehicle position calculated in the immediately preceding prediction step. As the state estimation filter used in these steps, various filters developed to perform Bayesian estimation can be used, such as an extended Kalman filter, an unscented Kalman filter, and a particle filter. As described above, various methods have been proposed for position estimation based on Bayesian estimation.

[0032] In this embodiment, as an example, the vehicle position estimation unit 18 estimates the vehicle position using an extended Kalman filter. In the following, the vehicle position estimation using the extended Kalman filter will be briefly described.

[0033] Fig. 4 is a diagram showing state variable vectors in two-dimensional Cartesian coordinates. As shown in Fig. 4, the vehicle position on a plane defined on the two-dimensional Cartesian coordinates of xy is represented by coordinates "(x, y)" and the vehicle's orientation (yaw angle) "ψ". Here, the yaw angle ψ is defined as the angle between the vehicle's traveling direction and the x-axis. The coordinates (x, y) are, for example, absolute positions corresponding to a combination of latitude and longitude, or world coordinates indicating a position with a specified point as the origin.

[0034] Fig. 5 is a diagram showing a schematic relationship between the prediction step and the measurement update step. Fig. 6 shows an example of functional blocks of the vehicle position estimation unit 18. As shown in Fig. 6, the vehicle position estimation unit 18 has a dead reckoning block 21, a position prediction block 22, a coordinate conversion block 23, a landmark search / extraction block 24, and a position correction block 25.

[0035] As shown in Fig. 5, the vehicle position estimation unit 18 repeats a prediction step and a measurement update step to sequentially calculate and update the estimated value of the state variable vector "X" indicating the vehicle position. Note that in Fig. 5, the state variable vector at the reference time (i.e., the current time) "t" to be calculated is expressed as "X - (t)" or "X^(t)" ("state variable vector X(t) = (x(t), y(t), ψ(t)) T "). Here, a provisional estimate (forecasted value) estimated in the prediction step is indicated by adding a "-" above the character representing the forecasted value, and a more accurate estimate updated in the measurement update step is indicated by adding a "^" above the character representing the value.

[0036] Specifically, as shown in FIG. 5 and FIG. 6, in the prediction step, the dead reckoning block 21 of the vehicle position estimation unit 18 calculates the vehicle's moving speed "v" and angular velocity "ω" (collectively referred to as the "control value u(t)=(v(t), ω(t)) T "). The position prediction block 22 of the vehicle position estimation unit 18 calculates the movement distance and the direction change from the previous time t-1 using the state variable vector X ^ Add the calculated travel distance and direction change to (t-1) to calculate the predicted vehicle position at time t (also called the predicted vehicle position). X - (t) is calculated. At the same time, the predicted vehicle position X - The covariance matrix P - (t)” is the covariance matrix “P ^ (t-1)".

[0037] In the measurement update step, the coordinate transformation block 23 of the vehicle position estimation unit 18 transforms the point cloud data output from the LIDAR 2 into a world coordinate system, which is the same coordinate system as the map DB 10. In this case, the coordinate transformation block 23 performs coordinate transformation of the point cloud data output from the LIDAR 2 at time t, based on the predicted vehicle position (i.e., the vehicle's planar position and orientation) output from the position prediction block 22 at time t, and the vehicle height and attitude angle (here, at least one of the pitch angle and roll angle) output from the attitude angle calculation unit 17 at time t. Details of this coordinate transformation will be described later.

[0038] Then, the landmark search / extraction block 24 of the vehicle position estimation unit 18 associates the position vector of the landmark of the measurement target registered in the map DB 10 with the point cloud data of the LIDAR 2 converted into the world coordinate system by the coordinate conversion block 23. In this case, the landmark search / extraction block 24 determines whether or not a landmark exists within the detection range of the LIDAR 2 based on the landmark data LD and the predicted vehicle position output by the position prediction block 22. Then, when a detectable landmark is registered in the landmark data LD, the landmark search / extraction block 24 sets a prediction window "Wp" that determines the range in which the landmark is detected. In this case, the landmark search / 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-converted point cloud data of the LIDAR 2. The prediction window Wp may be, for example, a cube. Then, the landmark search / extraction block 24 determines whether or not there is a measurement point of the point cloud data with a high reflectance that has a reflectance equal to or higher than a predetermined threshold within the prediction window Wp. If such a measurement point exists, the landmark search / extraction block 24 determines that the above-mentioned association has been established, and calculates the measurement value “Z(t)” of the landmark by the LIDAR 2 with which the association has been established and the predicted vehicle position X - (t) and the measurement prediction value “Z -In this case, the landmark search / extraction block 24 calculates a representative position (e.g., a position based on an average of coordinate values) of the positions indicated by the measurement points extracted from the point cloud data within the prediction window Wp as the measurement value Z(t) of the target landmark.

[0039] Then, the position correction block 25 of the vehicle position estimation unit 18 calculates the measured value Z(t) and the predicted measured value Z(t) as shown in the following equation (1). - The difference between the vehicle position and the target position is multiplied by the Kalman gain "K(t)". - (t), the updated state variable vector (also called the "estimated vehicle position") X ^ Calculate (t).

[0040]

number

[0041] When the vehicle position estimation unit 18 is able to associate the position vectors of the features registered in the map DB 10 with the scan data of the LIDAR 2 for a plurality of features, the vehicle position estimation unit 18 may perform the measurement update step based on any selected set of measurement predicted values ​​and measurement values, etc., or may perform the measurement update step multiple times based on all the associated measurement predicted values ​​and measurement values, etc. When using a plurality of measurement predicted values ​​and measurement values, etc., the vehicle position estimation unit 18 may consider that the LIDAR measurement accuracy deteriorates as the feature is farther from the LIDAR 2, and may reduce the weighting for the feature as the distance between the LIDAR 2 and the feature increases.

[0042] In this way, the prediction step and the measurement update step are repeated, and the predicted vehicle position X - (t) and estimated vehicle position X ^ By successively calculating (t), the most probable vehicle position is calculated.

[0043] (4) Calculating attitude angle Next, a method for calculating the pitch angle and roll angle, which are attitude angles of the vehicle, by the attitude angle calculation unit 17 using the landmark data LD will be described.

[0044] (4-1) Calculating the pitch angle First, a method for calculating the pitch angle of the vehicle by the attitude angle calculation unit 17 will be described.

[0045] The attitude angle calculation unit 17 refers to the landmark data LD and calculates the vehicle plane position x, y (i.e., (x - , y - ) or (x ^ , y ^ )) for a marking line (white line). The attitude angle calculation unit 17 then refers to the landmark data LD corresponding to the marking line found, and acquires position information of "n" pieces of the marking line in a world coordinate system. When the landmark is a marking line, the landmark data LD includes, as position information, coordinate data of latitude, longitude, and altitude indicating discrete positions (discrete points) of the marking line, for example, at intervals of several meters. The position information of the n pieces of the marking line in a world coordinate system acquired by the attitude angle calculation unit 17 is an example of "multiple position information of features drawn on the road surface."

[0046] Fig. 7(A) is an overhead view of a vehicle on a road indicating the position information of demarcation lines 60-62. Fig. 7(B) is a side view of a vehicle on a road indicating the position information of demarcation line 62. In Fig. 7(A), a vehicle is traveling on a road on which demarcation lines 60-62 exist. Demarcation line 60 has positions P1-P3 indicated by position information registered in the landmark data LD, demarcation line 61 has positions P4-P6 indicated by position information registered in the landmark data LD, and demarcation line 62 has positions P7-P14 indicated by position information registered in the landmark data LD.

[0047] In this case, the attitude angle calculation unit 17 refers to the landmark data LD and calculates the vehicle plane position x, y (here, (x ^ , y ^ The attitude angle calculation unit 17 then recognizes that the demarcation lines 60-62 exist within a predetermined distance from the landmark data LD corresponding to the demarcation lines 60-62. The attitude angle calculation unit 17 then acquires position information in the world coordinate system of a total of 14 positions P1-P14 (i.e., n=14) on the demarcation lines 60-62.

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

[0049]

number

[0050]

number

[0051]

number

[0052]

number

[0053]

number

[0054]

number

[0055]

number

[0056]

number

[0057]

number

[0058]

number

[0059] Fig. 9 is a diagram showing the relationship between the angle θ' between the normal vector Vn and the traveling direction vector Vx, and the pitch angle θ of the vehicle. As shown in Fig. 9, the angle θ' between the normal vector Vn and the traveling direction vector Vx is 90 degrees (i.e., π / 2) larger than the pitch angle θ of the vehicle. In addition, since the vehicle is restrained to the road surface, the inclination of the road surface in the traveling direction 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).

[0060]

number

[0061] (4-2) Roll angle estimation Similar to the calculation of the pitch angle, the attitude angle calculation unit 17 calculates a normal vector Vn shown in Equation (9) based on n lane line position vectors based on the landmark data LD.

[0062] In addition, the attitude angle calculation unit 17 calculates the lateral vector "V Y " is determined based on the yaw angle predicted or estimated by the vehicle position estimation unit 18. FIG. 10 shows the vehicle yaw angle (here, ψ ^ ) and the horizontal vector V Y As shown in FIG. 10, the horizontal vector V Y is the yaw angle (here, ψ ^ This corresponds to the direction of the horizontal vector V Y is given by the following equation (13).

[0063]

number

[0064]

number

[0065] Figure 11 shows the normal vector Vn and the horizontal vector V Y 11 is a diagram showing the relationship between the angle φ' between the normal vector Vn and the lateral vector VY The angle φ' between the vehicle and the road surface is 90 degrees (i.e., π / 2) larger than the roll angle φ of the vehicle. In addition, since the vehicle is restrained on the road surface, the inclination of the road surface in the lateral direction of the vehicle is equal to the roll angle φ of the vehicle. Therefore, the attitude angle calculation unit 17 calculates the roll angle φ based on the following equation (15).

[0066]

number

[0067] (5) Coordinate conversion of point cloud data Next, coordinate conversion of point cloud data based on the pitch angle calculated by the attitude angle calculation unit 17 will be described.

[0068] 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 calculate a rotation matrix "R θ " is generated.

[0069]

number

[0070]

number

[0071]

number

[0072] Next, a coordinate transformation that takes into account both the pitch angle θ and the roll angle φ will be described. The coordinate transformation block 23 calculates a rotation matrix R based on the pitch angle θ calculated by the attitude angle calculation unit 17. θ and the rotation matrix "R φ " and the matrix X indicating n pieces of three-dimensional data output by the LIDAR 2. L The rotation matrix “R” to be multiplied is created based on the following equation (19).

[0073]

number

[0074]

number

[0075] The coordinate conversion block 23 may further perform a process of converting the point cloud data indicating each of the three-dimensional positions based on the LIDAR 2 based on the combination of the distance and scan angle measured by the LIDAR 2 into a vehicle coordinate system. The vehicle coordinate system is a coordinate system of the vehicle with the vehicle's traveling direction and lateral direction as axes. In this case, the coordinate conversion block 23 converts the point cloud data from the coordinate system based on the LIDAR 2 to the vehicle coordinate system based on information on the installation position and installation angle of the LIDAR 2 with respect to the vehicle, and further converts the point cloud data converted into the vehicle coordinate system into the world coordinate system by the above-mentioned method. The process of converting the point cloud data output by the LIDAR installed on the vehicle into the vehicle coordinate system is disclosed, for example, in International Publication WO2019 / 188745.

[0076] Furthermore, the coordinate transformation block 23 may perform coordinate transformation taking into account only the roll angle φ of the pitch angle θ and the roll angle φ. In this case, the coordinate transformation block 23 performs coordinate transformation taking into account the rotation matrix R φ Let X be the matrix X representing n pieces of 3D data output by LIDAR 2. L Just multiply it by .

[0077] Next, the effect of the above-mentioned coordinate transformation will be further explained with reference to FIG. 12 and FIG.

[0078] FIG. 12(A) is a side view of a vehicle and a road traveling on a flat road surface. In this case, the vehicle position estimation unit 18 refers to the landmark data LD, and when it extracts a sign "L1" existing around the current position as a landmark, it obtains the height (e.g., altitude) "h" of the sign L1 from the reference position from the position information of the sign L1. Then, the vehicle position estimation unit 18 calculates the predicted or estimated vehicle height z (here, the estimated vehicle height z ^ The difference between the signal L1 and the signal L2 is "hz ^ A prediction window Wp is set at a position higher than the vehicle by 100 m. Then, the vehicle position estimation unit 18 calculates a measurement value Z(t) based on the average value of the measurement points of the point cloud data of the lidar 2 having high reflection intensity within the prediction window Wp, and performs vehicle position estimation based on the formula (1).

[0079] FIG. 12B is a side view of a vehicle and a road traveling on a road surface with a steep gradient when coordinate conversion based on the pitch angle of the vehicle is not performed on the point cloud data. In FIG. 12B, the road surface gradient causes a pitch angle of the vehicle, so the point cloud data is oriented upward. Therefore, the vehicle position estimation unit 18 calculates the vehicle height z ^ and the height of the sign L1 based on the landmark data LD, "hz ^ If a prediction window Wp is set with its center at a position higher than the vehicle by 100 cm, the sign 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 that corresponds to the sign L1, and cannot execute the vehicle position estimation process using the sign L1 as a landmark.

[0080] FIG. 13 is a side view of a vehicle and road traveling on a road surface with a steep gradient when coordinate conversion based on the pitch angle of the vehicle is performed on the point cloud data based on this embodiment. In this case, the point cloud data output by the LIDAR 2 is converted to a world coordinate system taking into account the pitch angle of the vehicle by a rotation matrix using the pitch angle calculated by the calculation method based on the embodiment. Therefore, in this case, the height from the vehicle to each measurement point of the point cloud data is the height in the world coordinate system. Therefore, in this case, the vehicle position estimation unit 18 calculates the vehicle height z ^ and the height of the sign L1, "hz ^ By setting a prediction window Wp centered at a position higher than the vehicle by 100 m, it is possible to extract measurement points corresponding to the sign L1 from the point cloud data output by the LIDAR 2.

[0081] Even when traveling on a road with a cross gradient such as a bank, the vehicle-mounted device 1 can perform coordinate transformation of the point cloud data based on the roll angle of the vehicle, and thereby suitably extract measurement points of the landmarks to be measured from the point cloud data based on the set prediction window Wp.

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

[0083] (6-1) Overview of vehicle position and attitude estimation process Fig. 14 is an example of a flowchart showing the procedure of the vehicle position and attitude estimation process. The attitude angle calculation unit 17 and the vehicle position estimation unit 18 of the vehicle-mounted device 1 repeatedly execute the process of the flowchart shown in Fig. 14 at each predetermined time interval when the vehicle position and attitude should be estimated. The symbols displayed to the right of each step in Fig. 14 represent the elements calculated in each step.

[0084] First, the dead reckoning block 21 of the vehicle position estimation unit 18 uses the vehicle's moving speed and angular velocity based on the outputs of the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5 to calculate the moving distance and azimuth change from the previous time. As a result, the position prediction block 22 of the vehicle position estimation unit 18 calculates the estimated vehicle position x obtained one time ago (the immediately preceding processing time). ^ , y ^ , ψ ^ From the current predicted vehicle position x - , y - , ψ - is calculated (step S11).

[0085] Next, the attitude angle calculation unit 17 estimates the vehicle height (e.g., altitude), roll angle, and pitch angle to calculate a predicted vehicle height “z - ", predicted roll angle "φ - " and predicted pitch angle "θ - This process will be described later with reference to FIG.

[0086] 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). The coordinate transformation block 23 then transforms the point cloud data into data in the world coordinate system (step S14). Thereafter, 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 point cloud data after the coordinate transformation and the landmark data LD, and calculates an estimated vehicle position x ^ , y ^ , ψ ^ (Step S15). After that, the attitude angle calculation unit 17 calculates the estimated vehicle position x ^ , y ^ , ψ ^ Using the above, the vehicle height, roll angle, and pitch angle estimation process is performed again in the same manner as in step S12 to estimate the vehicle height "z ^ ", estimated roll angle "φ ^ " and the estimated pitch angle "θ ^ This process will be described later with reference to FIG.

[0087] (6-2) Vehicle height, roll angle, and pitch angle estimation process FIG. 15 is an example of a flowchart showing the procedure of the vehicle height, roll angle, and pitch angle estimation process executed in step S12 and step S16 of FIG.

[0088] First, the attitude angle calculation unit 17 refers to the landmark data LD and calculates the predicted vehicle position x - , y - Or estimated vehicle position x ^ , y ^ For example, the attitude angle calculation unit 17 extracts position information of a marking line existing around the predicted vehicle position x - , y - Or estimated vehicle position x ^ , y ^ The landmark data LD of the demarcation line including the coordinate data indicating a position within a predetermined distance from the landmark data LD is extracted from the map DB 10.

[0089] Next, the attitude angle calculation unit 17 determines whether the number of coordinate data of the lane lines extracted in step S21 is less than three (step S22). If the number of coordinate data of the lane lines extracted in step S21 is less than three (step S22; Yes), the attitude angle calculation unit 17 calculates the predicted vehicle height z - or estimated vehicle height z ^ The expected vehicle height z - or estimated vehicle height z ^ (Step S28). Furthermore, in step S28, the attitude angle calculation unit 17 determines the roll angle φ (φ - Or φ ^ ) or pitch angle θ(θ - Or θ ^ ) at the current time to be calculated, and the roll angle φ(φ - Or φ ^ ) or pitch angle θ(θ - Or θ ^ ) is defined as follows.

[0090] On the other hand, if the number n of coordinate data of the lane lines extracted in step S21 is 3 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 the n lane lines 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 squares method based on equation (7), and calculates the predicted vehicle position x - , y - Or estimated vehicle position x ^ , y ^ By substituting into the plane equation of equation (2), the predicted vehicle height z - or estimated vehicle height z ^ is calculated (step S24).

[0091] Next, the attitude angle calculation unit 17 specifies the normal vector Vn shown in the formula (9) from the calculated coefficient vector a (step S25). Then, the attitude angle calculation unit 17 specifies the predicted or estimated yaw angle (orientation) ψ(ψ - Or ψ ^) to obtain the forward vector Vx shown in equation (10) and the lateral vector V shown in equation (13) Y After that, the attitude angle calculation unit 17 calculates the inner product of the normal vector Vn and the traveling direction vector Vx based on the formula (11), and calculates the inner product of the normal vector Vn and the lateral direction vector V based on the formula (14). Y Calculate the inner product of and obtain the roll angle φ(φ - Or φ ^ ) or pitch angle θ(θ - Or θ ^ ) is calculated (step S27).

[0092] As described above, the control unit 15 of the vehicle-mounted device 1 according to this embodiment acquires multiple pieces of position information of features drawn on the road surface that are present in the vicinity of the vehicle from the landmark data LD, which is map data including the position information of the features. Then, the control unit 15 calculates a normal vector to the approximate plane obtained based on the multiple pieces of acquired position information. Then, the control unit 15 calculates at least one of the pitch angle and roll angle of the vehicle based on the orientation of the vehicle and the normal vector. With this aspect, the vehicle-mounted device 1 can calculate at least one of the pitch angle and roll angle with high accuracy based on the landmark data LD.

[0093] (7) Variations Preferred modifications of the above-described embodiment will now be described. The following modifications may be combined and applied to these embodiments.

[0094] (Variation 1) Even if the vehicle-mounted device 1 does not estimate the vehicle position based on the formula (1), it may estimate at least one of the pitch angle and roll angle of the vehicle based on the embodiment.

[0095] In this case, for example, the vehicle-mounted device 1 repeatedly estimates at least one of the pitch angle and roll angle of the vehicle based on the landmark data LD by repeatedly executing steps S11 and S12 in Fig. 14. Even in this case, the vehicle-mounted device 1 can use the estimated pitch angle and / or roll angle for various applications such as coordinate conversion of output data of an external sensor such as the lidar 2 or slope (bank) detection processing.

[0096] (Variation 2) The configuration of the driving assistance system shown in Fig. 1 is an example, and the configuration of the driving assistance system to which the present invention can be applied is not limited to the configuration shown in Fig. 1. For example, instead of having the on-board device 1, the driving assistance system may have an electronic control device of the vehicle execute the processing of the attitude angle calculation unit 17 and the vehicle position estimation unit 18 of the on-board device 1. In this case, the map DB 10 is stored in, for example, a storage unit in the vehicle or a server device that performs data communication with the vehicle, and the electronic control device of the vehicle refers to this map DB 10 to execute the estimation of the roll angle and / or pitch angle and the estimation of the vehicle position based on Equation (1), etc.

[0097] Although the present invention has been described above with reference to the embodiments, the present invention is not limited to the above-mentioned embodiments. Various modifications that can be understood by a person skilled in the art can be made to the configuration and details of the present invention within the scope of the present invention. In other words, the present invention naturally includes various modifications and corrections that a person skilled in the art could make in accordance with the entire disclosure, including the claims, and the technical ideas. In addition, the disclosures of the above cited patent documents and the like are incorporated herein by reference. [Explanation of symbols]

[0098] 1 On-vehicle device 2. Rider 3 Gyro sensor 4 Vehicle speed sensor 5 GPS receiver 10 Map DB

Claims

[Claim 1] an acquisition unit that acquires a plurality of pieces of position information of features drawn on a road surface that are present in the vicinity of the mobile object from map data including the position information of the features; a normal vector calculation unit that calculates a normal vector with respect to the approximated plane obtained based on the plurality of pieces of position information; an angle calculation unit that calculates at least one of a pitch angle and a roll angle of the moving body based on a vehicle orientation of the moving body and the normal vector; An information processing device having the above configuration.

Images