Information processing apparatus, method, program, and storage medium

Abstract

To provide an information processing apparatus capable of suitably estimating the attitude of a vehicle.SOLUTION: A control unit 15 of an in-vehicle apparatus 1 extracts, from voxel data VD representing object position information for each unit region (voxel) obtained by partitioning a space, voxel data VD of a plurality of voxels located in the vicinity of a host vehicle. On the basis of the extracted voxel data VD, the control unit 15 calculates a normal vector to an approximate plane. The control unit 15 then calculates at least one of the pitch angle and the roll angle of the host vehicle on the basis of the orientation of the host 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 has been known a technique for estimating a vehicle's own position by comparing (matching) shape data of surrounding objects measured using a measurement device such as a laser scanner with map information in which the shapes of surrounding objects are stored in advance. For example, Patent Document 1 discloses an autonomous mobile system that determines whether a detected object in a voxel obtained by dividing a space according to a predetermined rule is a stationary object or a moving object, and matches the map information with the measurement data for voxels in which a stationary object exists. Furthermore, Patent Document 2 discloses a scan matching method that estimates a vehicle's own position by comparing voxel data including the mean vector and covariance matrix of stationary objects for each voxel with point cloud data output by a lidar. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] International Publication WO2013 / 076829 [Patent Document 2] International Publication No. WO2018 / 221453 Summary of the Invention [Problem to be solved by the invention]

[0004] Generally, a vehicle is constrained to the road surface, and changes in the vehicle's pitch angle, roll angle, and vertical direction, although subject to suspension fluctuations, are negligibly small. Therefore, in conventional scan-matching vehicle localization, the vehicle's horizontal position and orientation are set as the parameters to be estimated. However, when traveling on roads with steep slopes or cross slopes, estimating the vehicle's horizontal position and orientation alone cannot account for changes in pitch angle and roll angle, resulting in a mismatch or an incorrect association between the voxels to be matched and the measurement data. Adding pitch angle and roll angle as estimation parameters to account for changes in pitch angle and roll angle increases the computational load due to the increase in estimation parameters, resulting in a problem of inability to stably perform localization at the required interval. While there are methods for calculating the vehicle's pitch angle and roll angle from data from an inertial measurement unit (IMU), typical IMUs have sensitivity errors and offsets that make it difficult to accurately calculate pitch angle and roll angle.

[0005] The present invention has been made to solve the above-mentioned problems, and a main object of the present invention is to provide an information processing device that can suitably estimate the attitude of a vehicle. [Means for solving the problem]

[0006] The claimed invention includes an acquisition unit that acquires position information corresponding to a plurality of road surface voxels surrounding the road surface voxel including a road surface voxel corresponding to the road surface on which the planar position of the moving object is located from the position information of the object for each voxel; a normal vector calculation unit that calculates a normal vector with respect to an approximated plane determined based on position information corresponding to the plurality of road surface voxels; an angle calculation unit that calculates at least one of a pitch angle and a roll angle of the moving body based on the orientation of the moving body and the normal vector; The information processing device has the following.

[0007] The claimed invention is a method executed by an information processing device, extracting position information corresponding to a plurality of surrounding road surface voxels including a road surface voxel corresponding to the road surface on which the planar position is located from the position information of the object for each voxel; Calculating a normal vector relative to the approximated plane determined based on the position information corresponding to the plurality of road surface voxels; The method calculates at least one of a pitch angle and a roll angle of the moving body based on the orientation of the moving body and the normal vector.

[0008] The claimed invention also includes an extraction unit that extracts position information corresponding to a plurality of surrounding road surface voxels including a road surface voxel corresponding to the road surface of the road where the planar position is located from the position information of the object for each voxel; a normal vector calculation unit that calculates a normal vector with respect to an approximated plane determined based on position information corresponding to the plurality of road surface voxels; an angle calculation unit that calculates at least one of a pitch angle and a roll angle of the moving body based on the orientation of the moving body and the normal vector; It is a program that makes a computer function as a [Brief explanation of the drawings]

[0009] [Figure 1] FIG. 1 is a schematic configuration diagram of a driving assistance system. [Figure 2] FIG. 2 is a block diagram showing the functional configuration of the vehicle-mounted device. [Figure 3] This is a diagram showing a state variable vector in two-dimensional orthogonal coordinates. [Figure 4] 1 shows an example of a schematic data structure of voxel data. [Figure 5] 1 is an example of a functional block of an NDT matching unit. [Figure 6] (A) is an overhead view showing the correspondence between the vehicle position and road surface voxels, and (B) is a side view of the vehicle showing the road surface average vector. [Figure 7]FIG. 2 is an xy plane view showing the relationship between the yaw angle of the vehicle and the traveling direction vector. [Figure 8] FIG. 10 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 9] FIG. 2 is an xy plan view showing the relationship between the yaw angle of the vehicle and the lateral vector. [Figure 10] FIG. 10 is a diagram showing the relationship between the angle between a normal vector and a lateral vector and the roll angle of the vehicle. [Figure 11] (A) is a side view of a vehicle and road traveling on a flat road surface, and (B) is a side view of a vehicle and road traveling on a road surface with a steep gradient when coordinate transformation based on the vehicle's pitch angle is not performed on the point cloud data. [Figure 12] FIG. 10 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 performed on point cloud data. [Figure 13] 10 is an example of a flowchart illustrating a procedure for estimating the position and attitude of a vehicle. [Figure 14] 10 is an example of a flowchart illustrating a procedure for vehicle height estimation processing. [Figure 15] 10 is an example of a flowchart showing a procedure for estimating a roll angle and a pitch angle of a vehicle. [Figure 16] The figures show the number of point cloud data, the number of correspondences, the correspondence ratio, the evaluation value, and the transition of the pitch angle obtained when the pitch angle is estimated and the coordinate transformation of the point cloud data is performed based on the estimated pitch angle. [Figure 17] The numbers of point cloud data, the number of correspondences, the correspondence ratio, the evaluation value, and the pitch angle obtained when the pitch angle estimation is not performed are shown. DETAILED DESCRIPTION OF THE INVENTION

[0010] According to a preferred embodiment of the present invention, an information processing device includes an extraction unit that extracts position information of an object in a plurality of unit areas present in the vicinity of a moving body from position information of the object in each unit area obtained by dividing a space; a normal vector calculation unit that calculates a normal vector with respect to an approximated plane obtained based on the position information of the object in the plurality of unit areas; 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.

[0011] Generally, since a moving body travels on a road surface, it is assumed that a road surface exists near the moving body and that the road surface is included in multiple unit areas near the moving body. Furthermore, the moving body is constrained by the road surface, and a pitch angle and a roll angle of the moving body occur according to the inclination of the road surface. Taking the above into consideration, in this aspect, the information processing device calculates a normal vector of a plane that approximates the road surface based on position information of the road surface and position information of objects in the estimated multiple unit areas. This allows the information processing device to preferably 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.

[0012] In one aspect of the 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 object on a horizontal plane and the normal vector. With this aspect, the information processing device can calculate the angle between the direction of travel of the moving object on the horizontal plane and the normal vector of the approximation plane, and thereby suitably obtain the pitch angle of the moving object.

[0013] 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 object on a horizontal plane and the normal vector. With this aspect, the information processing device can calculate the angle between the lateral direction of the moving object on the horizontal plane and the normal vector of the approximation plane, thereby preferably determining the roll angle of the moving object.

[0014] In another aspect of the information processing device, the information processing device further includes a position estimation unit that estimates the position of the moving body by matching, for each unit area, position information of the object with data obtained by coordinate transformation of measurement data output by a measurement unit provided on the moving body based on at least one of the pitch angle and roll angle. In this aspect, the information processing device estimates the position of the moving body by matching (matching) the position information of the object with the measurement data output by the measurement unit. At this time, the information processing device performs coordinate transformation of the measurement data based on the calculated pitch angle or roll angle. As a result, the information processing device can accurately associate the unit area in which the measurement object actually exists with the measurement data of the measurement object and perform matching, even if the pitch angle or roll angle of the vehicle changes due to traveling on a slope or a road with a high cross-slope.

[0015] In another aspect of the information processing device, the position information of the object for each unit area includes information on an average vector related to the position of the object for each unit area, and the normal vector calculation unit calculates the normal vector based on the coordinates of each of the average vectors in the plurality of unit areas. According to this aspect, the information processing device can suitably calculate the normal vector of a plane that approximates a road surface on which a moving object exists, based on position information of the object in the plurality of unit areas that is estimated to be position information of the road surface on which the moving object exists.

[0016] In another aspect of the information processing device, the extraction unit extracts the position information of the object in a first unit area whose planar position overlaps with that of the moving body and a second unit area adjacent to the first unit area, among the unit areas in which the position information of the object exists. According to this aspect, the information processing device can suitably extract the position information of the object in a plurality of unit areas that are estimated to be position information of a road surface.

[0017] 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 object from a reference position based on position information of the object in the first unit area and information about the moving object's height 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 preferably calculate the moving object's height from the reference position using position information of the object (road surface) in the first unit area where the moving object is presumed to be present. In a preferred example, if the difference between the height from the reference position calculated based on the position information of the object in the first unit area and information about the moving object's height from the road surface and the height from the reference position calculated one time ago is greater than a predetermined value, the height calculation unit may set the height from the reference position calculated one time ago as the height from the reference position at the current time. This preferably prevents the moving object's height with the error from being recognized as the moving object's current height when an error occurs in the calculated moving object's height due to the presence of a grade-separated intersection, guardrail, or the like nearby.

[0018] In another aspect of the information processing device, the object position information is position information of stationary structures including a road surface, and the extraction unit extracts position information of the road surface in the plurality of unit areas from map data including position information of objects for each of the unit areas. With this aspect, the information processing device can preferably extract position information of the road surface on which the mobile object is located from the map data and preferably calculate a normal vector of an approximation plane required for calculating a pitch angle or a roll angle.

[0019] According to another preferred embodiment of the present invention, there is provided a control method executed by an information processing device, which extracts position information of objects in a plurality of unit areas near a moving body from position information of the objects in each unit area obtained by dividing a space, calculates a normal vector to an approximated plane obtained based on the position information of the objects in the plurality of unit areas, and 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. By executing this control method, the information processing device can preferably calculate at least one of the pitch angle or the roll angle of the moving body based on the relationship between the normal vector of the approximated plane based on the position information of the unit areas near the moving body and the orientation of the moving body.

[0020] According to another preferred embodiment of the present invention, the program causes a computer to function as an extraction unit that extracts position information of objects in multiple unit areas near a moving body from position information of the objects in each unit area obtained by dividing a space; a normal vector calculation unit that calculates a normal vector to an approximated plane obtained based on the position information of the objects in the multiple unit areas; 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. By executing this program, the computer can preferably calculate at least one of the pitch angle or the roll angle of the moving body based on the relationship between the normal vector of the approximated plane based on position information of the unit areas near the moving body and the orientation of the moving body. Preferably, the program is stored in a storage medium. [Example]

[0021] Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. For the sake of convenience, in this specification, a character with "^" or "-" above any symbol will be referred to as "A ^ " or "A - " (where "A" is any letter).

[0022] (1) Overview of the driving assistance system 1 shows a schematic configuration of a driving assistance system according to this 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.

[0023] The vehicle-mounted device 1 is electrically connected to a lidar 2, a gyro sensor 3, a vehicle speed sensor 4, and a GPS receiver 5, and estimates the position of the vehicle in which the vehicle-mounted device 1 is installed (also referred to as "subject vehicle position") based on the outputs of these sensors. Then, based on the result of estimating the subject 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 voxel data "VD." The voxel data VD is data that records position information of stationary structures for each voxel, which represents a cube (regular lattice), the smallest unit of three-dimensional space. The voxel data VD includes data that represents measured point cloud data of stationary structures within each voxel using a normal distribution, and is used for scan matching using NDT (Normal Distributions Transform), as described below. The vehicle-mounted device 1 also estimates the vehicle's planar position and yaw angle by NDT scan matching, and estimates the vehicle's height position and at least one of the pitch angle and roll angle based on the voxel data VD.

[0024] The LIDAR 2 emits a pulsed laser beam within a predetermined angular range in the horizontal and vertical directions to discretely measure the distance to an object in the external world and generate three-dimensional point cloud information indicating the position of the object. In this case, the LIDAR 2 includes 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 light receiving 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 determined based on the above-mentioned light receiving signal. Note that, generally, the closer the distance to the object, the higher the accuracy of the LIDAR's distance measurement value, and the farther the distance, the lower the accuracy. The LIDAR 2, gyro sensor 3, vehicle speed sensor 4, and GPS receiver 5 each supply output data to the vehicle-mounted device 1. Note that 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.

[0025] The driving assistance system may include, 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.

[0026] (2) In-vehicle 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 interconnected via a bus line.

[0027] The interface 11 acquires output data from sensors such as the lidar 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 driving control generated by the control unit 15 to an electronic control unit (ECU) of the vehicle.

[0028] The storage unit 12 stores programs executed by the control unit 15 and information required for the control unit 15 to execute predetermined processes. In this embodiment, the storage unit 12 stores a map DB 10 including voxel data VD. 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 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 voxel data VD via the communication unit 13, thereby acquiring information required for vehicle position estimation processing and the like at the required timing.

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

[0030] 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 an NDT matching unit 18. The control unit 15 is an example of the "extraction unit," "normal vector calculation unit," "angle calculation unit," "position estimation unit," "height calculation unit," and "computer" that executes the program in the present invention.

[0031] The attitude angle calculation unit 17 refers to the voxel data VD and calculates at least one of the pitch angle and roll angle of the vehicle. The NDT matching unit 18 estimates the vehicle position by performing scan matching based on NDT (NDT scan matching) based on the point cloud data output from the LIDAR 2 and the voxel data VD corresponding to the voxels to which the point cloud data belongs. In this embodiment, as will be described later, the NDT matching unit 18 sets the vehicle's planar position (i.e., position on a horizontal plane specified by latitude and longitude) and yaw angle (i.e., orientation) as estimation parameters in NDT scan matching.

[0032] (3) Position estimation based on NDT scan matching FIG. 3 is a diagram showing the vehicle position to be estimated by the NDT matching unit 18 in two-dimensional Cartesian coordinates. As shown in FIG. 3, the vehicle position on a plane defined on the two-dimensional Cartesian coordinates of xy is expressed by coordinates "(x, y)" and the vehicle's heading (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 predetermined point as the origin. The NDT matching unit 18 then estimates the vehicle position using these x, y, and ψ as estimation parameters. Note that the method for estimating the vehicle's pitch angle and roll angle is described in "(4) Attitude angle estimation This is explained in more detail in the " section.

[0033] Next, we will explain the voxel data VD used in NDT scan matching. The voxel data VD includes data in which measured point cloud data of a stationary structure in each voxel is expressed using a normal distribution.

[0034] Fig. 4 shows an example of a schematic data structure of the voxel data VD. The voxel data VD includes parameter information when expressing a point group in a voxel using a normal distribution, and in this embodiment, as shown in Fig. 4, includes a voxel ID, voxel coordinates, a mean vector, and a covariance matrix.

[0035] "Voxel coordinates" indicate the absolute three-dimensional coordinates of a reference position, such as the center position of each voxel. Each voxel is a cube that divides space into a grid, and since its shape and size are predetermined, it is possible to identify the space of each voxel using its voxel coordinates. Voxel coordinates may also be used as a voxel ID.

[0036] The "mean vector" and "covariance matrix" refer to the mean vector and covariance matrix, which are parameters when expressing the point cloud in the target voxel as a normal distribution. Note that the coordinates of an arbitrary point "i" in an arbitrary voxel "n" are X n (i)=[x n (i), y n (i), z n (i)] T and the number of points in voxel n is defined as "N n ", then the mean vector at voxel n is "μ n ” and the covariance matrix “V n " are expressed by the following formulas (1) and (2), respectively.

[0037]

number

[0038]

number

[0039] Next, an overview of NDT scan matching using voxel data VD will be explained.

[0040] Scan matching using NDT, which assumes a vehicle, estimates parameters based on the amount of movement within the road plane (here, xy coordinates) and the vehicle's orientation. P=[t x , t y , t ψ ] T Here, "t x" indicates the amount of movement in the x direction, and "t y " indicates the amount of movement in the y direction, and "t ψ " indicates the yaw angle.

[0041] In addition, the point cloud data obtained by the LIDAR 2 is associated with the voxels to be matched, and the coordinates of any point in the corresponding voxel n are calculated. X L (j)=[x n (j), y n (j), z n (j)] T Then, X at voxel n L The average value of (j) "L' n " is expressed by the following equation (3).

[0042]

number

[0043]

number

[0044]

number

[0045] Then, the vehicle-mounted device 1 calculates an overall evaluation function value (also called an "overall evaluation function value") "E(k)" for all voxels to be matched, as shown in the following equation (6).

[0046]

number

[0047]

number

[0048] 5 shows an example of functional blocks of the NDT matching unit 18. As shown in FIG. 5, the NDT matching unit 18 includes a dead reckoning block 21, a position prediction block 22, a coordinate transformation block 23, a point cloud data association block 24, and a position correction block 25.

[0049] The dead reckoning block 21 calculates the travel distance and change in direction from the previous time using the travel speed and angular velocity of the vehicle based on the outputs of the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5. The position prediction block 22 calculates the estimated vehicle position X at time k-1 calculated in the immediately preceding measurement update step. ^ Add the calculated travel distance and heading change to (k-1) to calculate the predicted vehicle position X at time k. - Calculate (k).

[0050] The coordinate transformation block 23 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 k based on the predicted vehicle position (i.e., the planar position and orientation of the vehicle) output from the position prediction block 22 at time k 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 k. Details of this coordinate transformation will be described later.

[0051] The point cloud data correspondence block 24 matches the point cloud data in the world coordinate system output by the coordinate transformation block 23 with the voxel data VD expressed in the same world coordinate system, thereby associating the point cloud data with the voxels. The position correction block 25 calculates an individual evaluation function value based on equation (5) for each voxel associated with the point cloud data, and calculates an estimated parameter P that maximizes the overall evaluation function value E(k) based on equation (6). Then, the position correction block 25 calculates the predicted vehicle position X output by the position prediction block 22 based on equation (7). - By applying the estimation parameter P calculated at time k to (k), the estimated vehicle position X ^ Calculate (k).

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

[0053] (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.

[0054] The attitude angle calculation unit 17 refers to the voxel data VD and calculates the vehicle plane position x, y (i.e., (x - , y - ) or (x^ , y ^ )) is located. Then, the attitude angle calculation unit 17 acquires voxel data VD corresponding to "n" surrounding road surface voxels including the road surface voxel where the host vehicle planar position x, y is located (also referred to as "host vehicle position road surface voxel"). "n" is an arbitrary integer equal to or greater than 3. The host vehicle position road surface voxel is an example of a "first unit area" in the present invention, and the n-1 surrounding road surface voxels other than the host vehicle position road surface voxel are an example of a "second unit area" in the present invention.

[0055] FIG. 6A is a bird's-eye view showing the correspondence between the vehicle position and road surface voxels. In FIG. 6A, the vehicle plane position x, y (here, (x ^ , y ^ )) is located, and road surface voxels "Vo1" to "Vo4" and "Vo6" to "Vo9" to the front, rear, left and right of the vehicle position road surface voxel Vo5 are shown. In this example, "n=9" is set, and the attitude angle calculation unit 17 acquires voxel data VD corresponding to the nine road surface voxels Vo1 to Vo9 from the map DB 10.

[0056] Next, the attitude angle calculation unit 17 calculates the average vector in the world coordinate system of the road surface contained in each of the n road surface voxels (also called the "road surface average vector") based on the information on the voxel coordinates and average vector contained in the voxel data VD corresponding to each of the n road surface voxels.

[0057] Fig. 6(B) is a side view of the vehicle clearly showing the road surface average vector for each road surface voxel. Fig. 6(B) shows coordinate positions "M4" to "M6" of the road surface average vector corresponding to each of the road surface voxels Vo4 to Vo6 shown in Fig. 6(A). Since "n=9", the attitude angle calculation unit 17 calculates the road surface average vectors corresponding to the nine road surface voxels Vo1 to Vo9 based on the voxel data VD corresponding to the nine road surface voxels Vo1 to Vo9.

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

[0059]

number

[0060]

number

[0061]

number

[0062]

number

[0063]

number

[0064]

number

[0065]

number

[0066]

number

[0067]

number

[0068]

number

[0069] FIG. 8 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. 8, the angle θ' between the normal vector Vn and the traveling direction vector Vx is larger than the pitch angle θ of the vehicle by 90 degrees (i.e., π / 2). Furthermore, since the vehicle is constrained 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 (18).

[0070]

number

[0071] (4-2) Roll angle estimation Similar to the calculation of the pitch angle, the attitude angle calculation unit 17 calculates the normal vector Vn shown in equation (15) based on n road surface average vectors based on the voxel data VD.

[0072] The attitude angle calculation unit 17 calculates the lateral vector "V Y " is determined based on the yaw angle predicted or estimated by the NDT matching unit 18. FIG. 9 shows the relationship between the yaw angle of the vehicle (here, ψ ^ ) and the horizontal vector V Y 9 is an xy plane diagram showing the relationship between the horizontal vector V Y is the yaw angle (here, ψ ^ The horizontal vector V corresponds to the direction rotated by 90 degrees (π / 2) along the xy plane. Y is given by the following equation (19).

[0073]

number

[0074]

number

[0075] Figure 10 shows the normal vector Vn and the horizontal vector V Y 10 is a diagram showing the relationship between the angle φ' formed by the normal vector Vn and the lateral vector V and the roll angle φ of the vehicle. Y The angle φ' between these is larger than the roll angle φ of the vehicle by 90 degrees (i.e., π / 2). Furthermore, 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 roll angle φ of the vehicle. Therefore, the attitude angle calculation unit 17 calculates the roll angle φ based on the following equation (21).

[0076]

number

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

[0078] The coordinate transformation block 23 of the NDT matching unit 18 uses the pitch angle θ calculated by the attitude angle calculation unit 17 to calculate the rotation matrix "R θ " is generated.

[0079]

number

[0080]

number

[0081]

number

[0082] 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" based on the roll angle φ φ " and a matrix X indicating n pieces of three-dimensional data output by LIDAR 2. L The rotation matrix "R" to be multiplied is created based on the following equation (25).

[0083]

number

[0084]

number

[0085] The coordinate transformation block 23 may further perform processing to convert point cloud data indicating each three-dimensional position relative to the LIDAR 2 based on a combination of the distance and scan angle measured by the LIDAR 2 into a vehicle coordinate system. The vehicle coordinate system is a vehicle coordinate system with the vehicle's traveling direction and lateral direction as its axes. In this case, the coordinate transformation block 23 converts 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 further converts the point cloud data converted into the vehicle coordinate system into a world coordinate system using the method described above. The processing to convert 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.

[0086] 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 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 φ is a matrix X that indicates n pieces of 3D data output by LIDAR 2. L Just multiply by this.

[0087] Next, the effect of the above-mentioned coordinate transformation will be further explained with reference to FIGS.

[0088] FIG. 11(A) is a side view of a vehicle and road traveling on a flat road surface. In this case, the vehicle height "z" from a reference position in the world coordinate system (for example, a position where the altitude is 0) is ^ " is the height obtained by adding the vehicle height from the road surface (also called "vehicle reference position z0") to the height from the reference position of the world coordinate system to the road surface. Then, the height "d" from the vehicle of a certain measurement point "Ptag1" measured by Rider 2 is calculated as the vehicle height z ^ "z" added to ^ +d" coincides with the actual height of the measurement point Ptag1 in the world coordinate system. Therefore, in this case, in the association process executed by the point cloud data association block 24, the target measurement point Ptag1 is associated with the voxel "V1" that actually includes the measurement point Ptag1.

[0089] FIG. 11(B) is a side view of a vehicle and road traveling on a road with a steep gradient when coordinate transformation based on the vehicle's pitch angle is not performed on the point cloud data. In this case, the road gradient causes a pitch angle on the vehicle, so the point cloud data is oriented upward. Therefore, the height in the world coordinate system of measurement point "Ptag2" located at a height d from the vehicle is calculated by multiplying the height d by the vehicle height z. ^ "z" added to ^ As a result, in the association process executed by the point cloud data association block 24, the height of the measurement point Ptag2 in the world coordinate system is not "z ^ +d", so that the measurement point Ptag2 is not associated with the voxel "V2" where it actually exists, but is erroneously associated with the voxel "V2α".

[0090] FIG. 12 is a side view of a vehicle and road traveling on a road surface with a large gradient when coordinate transformation based on the pitch angle of the vehicle is performed on point cloud data based on this embodiment. In this case, the point cloud data output by the LIDAR 2 is transformed into a world coordinate system that takes into account the pitch angle of the vehicle by a rotation matrix using the pitch angle calculated by the calculation method based on this embodiment. Therefore, in this case, the height in the world coordinate system of the measurement point "Ptag3" located at a height d from the vehicle is calculated as "z ^ +d". Therefore, in the matching process executed by the point cloud data matching block 24, the measurement point Ptag3 is matched with the voxel "V3" (i.e., the correct matching target) in which the measurement point Ptag3 actually exists, and NDT scan matching is performed appropriately.

[0091] Even when traveling on a road with a cross slope such as a bank, the on-board device 1 can perform coordinate transformation on the point cloud data based on the roll angle of the vehicle, thereby properly associating each measurement point of the point cloud data output by the lidar 2 with the correct voxel to be matched.

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

[0093] (6-1) Overview of vehicle position and attitude estimation process Fig. 13 is an example of a flowchart showing the procedure for estimating the position and attitude of a vehicle. The attitude angle calculation unit 17 and the NDT matching unit 18 of the on-vehicle device 1 repeatedly execute the process of the flowchart shown in Fig. 13 at predetermined time intervals when the vehicle position and attitude should be estimated. Note that the symbols displayed to the right of each step in Fig. 13 represent the elements calculated in each step.

[0094] First, the dead reckoning block 21 of the NDT matching unit 18 calculates the travel distance and change in direction from the previous time using the travel speed and angular velocity of the vehicle based on the outputs of the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5. As a result, the position prediction block 22 of the NDT matching unit 18 calculates the estimated vehicle position x obtained one time before (the immediately preceding processing time). ^ , y ^ , ψ ^ From the current predicted vehicle position x - , y - , ψ - is calculated (step S11).

[0095] Next, the attitude angle calculation unit 17 performs a process of estimating the vehicle height (for example, the altitude at which the vehicle is located) to calculate the predicted vehicle height “z - This process will be described later with reference to FIG. 14. Furthermore, the attitude angle calculation unit 17 performs a process of estimating the roll angle and pitch angle of the vehicle to calculate the predicted roll angle "φ - " and predicted pitch angle "θ - This process will be described later with reference to FIG.

[0096] Then, the coordinate transformation block 23 of the NDT matching unit 18 generates a rotation matrix R (see equation (25)) based on the roll angle and pitch angle calculated in step S13 (step S14). Then, the coordinate transformation block 23 transforms the point cloud data into data in the world coordinate system (step S15). Thereafter, the NDT matching unit 18 (point cloud data association block 24 and position correction block 25) performs NDT matching using the point cloud data after the coordinate transformation and the voxel data VD, and calculates an estimated vehicle position x at the current time. ^ , y ^ , ψ ^ (Step S16). Then, the attitude angle calculation unit 17 calculates the estimated vehicle position x at the current time. ^ , y ^ , ψ ^ By using the same vehicle height estimation process as in step S12 again, the estimated vehicle height "z ^This process will be described later with reference to FIG. 14. Furthermore, the attitude angle calculation unit 17 calculates the estimated vehicle position x ^ , y ^ , ψ ^ and estimated vehicle height z ^ By using these, the roll angle and pitch angle of the vehicle are estimated, and the estimated roll angle "φ ^ " and estimated pitch angle "θ ^ This process will be described later with reference to FIG.

[0097] (6-2) Vehicle height estimation processing Fig. 14 is an example of a flowchart showing the procedure of the vehicle height estimation process executed in step S12 and step S17 of Fig. 13. In the vehicle height estimation process, the attitude angle calculation unit 17 determines the host vehicle position road surface voxel based on the predicted or estimated planar position of the vehicle, and determines the value obtained by adding the vehicle reference position z0 (the height of the vehicle from the road surface) to the z coordinate of the mean vector of the host vehicle position road surface voxel as the vehicle height to be calculated. Furthermore, the in-vehicle device 1 sets the voxel index idz of the host vehicle position road surface voxel that is unreliable as the host vehicle position road surface voxel to "-1" and labels it so that it will not be used in the roll angle / pitch angle estimation process described later.

[0098] First, the attitude angle calculation unit 17 calculates the predicted vehicle position x - , y - or estimated vehicle position x ^ , y ^ The attitude angle calculation unit 17 obtains the voxel index (idx, idy) of the voxel containing the predicted vehicle position x - , y - or estimated vehicle position x ^ , y ^ In the case of step S12 in FIG. 13, the attitude angle calculation unit 17 searches for a voxel in the height direction that is at the same position as the predicted vehicle position x - , y -In the case of step S17 in FIG. 13, the attitude angle calculation unit 17 searches for a voxel having a voxel index (idx, idy) of a voxel that includes the estimated vehicle position x ^ , y ^ Search for the voxel with the voxel index (idx, idy) of the voxel that contains

[0099] Next, the attitude angle calculation unit 17 determines whether or not one or more voxels have been detected in step S21 (step S22). If no voxels have been detected (step S22; No), 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 S27) Furthermore, in step S27, the attitude angle calculation unit 17 determines a provisional vehicle position road surface voxel in which the voxel index idz is "-1".

[0100] On the other hand, if one or more voxels are detected in step S21 (step S22; Yes), the attitude angle calculation unit 17 determines the detected voxels as candidates for the vehicle position road surface voxel, reads the z coordinate of each candidate (z coordinate of the mean vector of the candidate) from the voxel data VD, and adds the vehicle reference position z0 to each z coordinate (step S23). The vehicle reference position z0 is stored in advance in the storage unit 12, for example.

[0101] Then, the attitude angle calculation unit 17 calculates the value obtained by adding the vehicle reference position z0 to the z coordinate of the mean vector of each candidate, and calculates the predicted vehicle height z from one time point before. - or estimated vehicle height z ^ The candidate for the vehicle position road surface voxel having the value closest to is selected as the vehicle position road surface voxel (step S24).

[0102] Then, the attitude angle calculation unit 17 calculates a value obtained by adding the vehicle reference position z0 to the z coordinate of the mean vector of the selected road surface voxel at the vehicle position, and the predicted vehicle height z from one time before. - or estimated vehicle height z^ If the difference is equal to or smaller than the predetermined value (step S25; Yes), the process of step S26 is executed. In this case, the predetermined value is set to, for example, the upper limit of the range of height fluctuation that the vehicle may experience on a slope or the like within the time interval from one time before to the current time.

[0103] Then, the attitude angle calculation unit 17 calculates the value obtained by adding the vehicle reference position z0 to the z coordinate of the mean vector of the road surface voxel at the vehicle position as the predicted vehicle height z - or estimated vehicle height z ^ On the other hand, if the difference is greater than the predetermined value (step S25; No), the attitude angle calculation unit 17 determines the predicted vehicle height z - or estimated vehicle height z ^ The expected vehicle height z - or estimated vehicle height z ^ (step S27) Furthermore, in step S27, the attitude angle calculation unit 17 sets the voxel index idz of the host vehicle position road surface voxel selected in step S24 to "-1".

[0104] (6-3) Roll angle and pitch angle estimation processing Fig. 15 is an example of a flowchart showing the procedure of the process of estimating the roll angle and pitch angle of the vehicle, which is executed in step S13 and step S18 of Fig. 13. Note that in Fig. 15, the on-vehicle device 1 estimates both the roll angle and pitch angle of the vehicle as an example, but the on-vehicle device 1 may perform a process of estimating only one of the roll angle and pitch angle of the vehicle.

[0105] First, the attitude angle calculation unit 17 refers to the voxel index (idx, idy, idz) of the host vehicle position road surface voxel determined in the vehicle height estimation process of step S12 or step S17 executed immediately before (step S31). Then, the attitude angle calculation unit 17 determines whether the voxel index idz of the host vehicle position road surface voxel is set to "-1" (step S32). Then, if the voxel index idz of the host vehicle position road surface voxel is "-1" (step S32; Yes), the attitude angle calculation unit 17 determines that the reliability of the target host vehicle position road surface voxel is low or that the host vehicle position road surface voxel could not be detected. Therefore, in this case, the attitude angle calculation unit 17 uses the roll angle φ (φ - or φ ^ ) or pitch angle θ(θ - or θ ^ ) to calculate the roll angle φ(φ - or φ ^ ) or pitch angle θ(θ - or θ ^ ) (step S38).

[0106] On the other hand, if the voxel index idz of the host vehicle position road surface voxel is not "-1" (step S32; No), the attitude angle calculation unit 17 acquires voxel data VD of n voxels surrounding the host vehicle position road surface voxel, including the host vehicle position road surface voxel (step S33). For example, the attitude angle calculation unit 17 regards voxels that differ from the host vehicle position road surface voxel in at least one of voxel indexes idx, idy and that have the same voxel index idz or a voxel index idz that differs by one, together with the host vehicle position road surface voxel, as the above-mentioned n voxels.

[0107] Then, the attitude angle calculation unit 17 reads the x, y, and z coordinates of the average vector included in each of the voxel data VD for the above-mentioned n voxels, and creates a matrix C and a vector b according to equation (10) (step S34). Next, the attitude angle calculation unit 17 calculates a coefficient vector a by the least squares method based on equation (13), and identifies a normal vector Vn shown in equation (15) (step S35). Then, the attitude angle calculation unit 17 calculates the predicted or estimated yaw angle (orientation) ψ(ψ - or ψ ^ ) to obtain the forward vector Vx shown in equation (16) and the lateral vector V shown in equation (19). Y (Step S36). Thereafter, the attitude angle calculation unit 17 calculates the inner product of the normal vector Vn and the traveling direction vector Vx based on the equation (17), and calculates the inner product of the normal vector Vn and the lateral direction vector Vx based on the equation (20). Y Calculate the inner product of the roll angle φ(φ - or φ ^ ) or pitch angle θ(θ - or θ ^ ) is calculated (step S37).

[0108] (7) Experimental example The applicant ran a vehicle equipped with a lidar similar to the configuration shown in Figure 1 on a road including a steep slope, performed vehicle position estimation based on NDT scan matching, and collected the necessary data. The maximum ranging distance of the lidar mounted on the vehicle was 25 m, and the horizontal angle at which ranging was performed was approximately 120 degrees.

[0109] 16(A) to 16(E) respectively show the number of point cloud data, the number of correspondences, the correspondence ratio, the evaluation value, and the transition of the pitch angle obtained on the road described above when the pitch angle is estimated based on this embodiment and the coordinate transformation of the point cloud data is performed based on the estimated pitch angle. Also, FIG. 17(A) to 17(E) respectively show the number of point cloud data, the number of correspondences, the correspondence ratio, the evaluation value, and the pitch angle obtained on the road described above when the pitch angle is not estimated.

[0110] Here, "number of point cloud data" indicates the number of measurement points in the point cloud data obtained from the lidar at each processing time, and "number of correspondences" indicates the number of measurement points among the number of point cloud data that were successfully matched with voxels. Furthermore, "matching ratio" indicates the ratio of the number of correspondences to the number of point cloud data, and "evaluation value" indicates the overall evaluation function value corresponding to the estimation parameters determined in NDT scan matching. "Pitch angle" indicates the vehicle pitch angle estimated by the vehicle. Note that, since pitch angle estimation was not performed in the cases of FIGS. 17(A) to 17(E), the pitch angle shown in FIG. 17(E) is always 0 degrees. Furthermore, the periods indicated by dashed arrows 58 in FIGS. 16(B) to 16(E) and dashed arrows 59 in FIGS. 17(B) to 17(E) are periods during which the vehicle was traveling on roads with no voxel data VD present in the vicinity, and thus NDT scan matching was not performed.

[0111] In Fig. 16(E), the absolute value of the pitch angle temporarily increases during the periods indicated by dashed frames 60 and 61, which correspond to periods when the vehicle is traveling uphill. Even during such periods of traveling uphill, as shown in Fig. 16(C), the coordinate transformation of the point cloud data taking the pitch angle into consideration prevents a decrease in the number of correspondences, and the correspondence ratio remains the same as during periods of traveling on other flat roads.

[0112] On the other hand, when pitch angle estimation and coordinate transformation of point cloud data based on the pitch angle are not performed, the correspondence ratio corresponding to the period corresponding to the above-mentioned hill driving period temporarily drops significantly, as shown in dashed frame 70 and dashed frame 71 in Fig. 17(C). Note that when the number of correspondences and the correspondence ratio decrease, it is inferred that the robustness of NDT scan matching decreases and the reliability of the estimated vehicle position is low.

[0113] As described above, according to this embodiment, by estimating the pitch angle and performing coordinate transformation of the point cloud data based on the pitch angle, it is possible to preferably suppress a decrease in the number of correspondences and the correspondence ratio of the point cloud data even on a slope, and it is possible to preferably improve the robustness of the vehicle position estimation by NDT scan matching. Similarly, by estimating the roll angle and performing coordinate transformation of the point cloud data based on the roll angle, it is possible to preferably suppress a decrease in the number of correspondences and the correspondence ratio of the point cloud data even on a road with a high transverse gradient, and it is possible to preferably improve the robustness of the vehicle position estimation by NDT scan matching.

[0114] As described above, the control unit 15 of the vehicle-mounted device 1 according to this embodiment extracts voxel data VD of multiple voxels existing near the vehicle from voxel data VD, which is position information of objects for each unit region (voxel) obtained by dividing a space. The control unit 15 then calculates a normal vector to the approximate plane obtained based on the voxel data VD of the extracted multiple voxels. The control unit 15 then 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. This allows the vehicle-mounted device 1 to calculate at least one of the pitch angle and roll angle with high accuracy based on the voxel data VD.

[0115] (8) Variations The following describes preferred modifications of the above-described embodiment. The following modifications may be applied to these embodiments in combination.

[0116] (Variation 1) In the flowchart of FIG. 13 and the like, the vehicle-mounted device 1 performs the vehicle height estimation process shown in FIG. 14 to calculate the vehicle height (predicted vehicle height z - or estimated vehicle height z ^ However, the method for calculating the vehicle height is not limited to this.

[0117] For example, the vehicle-mounted device 1 may further add the vehicle height as an estimation parameter in the vehicle position estimation based on NDT scan matching. In this case, the vehicle-mounted device 1 performs vehicle position estimation using four variables (x, y, z, ψ) as state variables for the vehicle position, taking into account the coordinate of the z axis perpendicular to the x axis and y axis in addition to the coordinates (x, y) and yaw angle ψ shown in Fig. 3. This mode also allows the vehicle-mounted device 1 to preferably estimate the vehicle height.

[0118] (Variation 2) Even when the vehicle-mounted device 1 does not perform NDT scan matching, it may estimate at least one of the pitch angle and roll angle of the vehicle based on the embodiment.

[0119] 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 voxel data VD by repeatedly executing steps S11 to S13 in Fig. 13. 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 from an external sensor such as the lidar 2 or slope (bank) detection processing.

[0120] (Variation 3) The configuration of the driving assistance system shown in Fig. 1 is an example, and the configuration of a 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 unit of the vehicle execute the processing of the attitude angle calculation unit 17 and the NDT matching 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 communicates data with the vehicle, and the electronic control unit of the vehicle refers to this map DB 10 to execute estimation of the roll angle and / or pitch angle and estimation of the vehicle's position based on NDT scan matching.

[0121] (Variation 4) The voxel data VD is not limited to a data structure including a mean vector and a covariance matrix as shown in Fig. 3. For example, the voxel data VD may include point cloud data measured by a measurement and maintenance vehicle that is used to calculate the mean vector and covariance matrix. In this case, the vehicle-mounted device 1, for example, references the voxel data VD and calculates the mean vector for each voxel to generate the matrix C shown in equation (10).

[0122] Furthermore, this embodiment is not limited to scan matching using NDT, and other scan matching such as ICP (Iterative Closest Point) may be applied. Even in this case, as in the embodiment, the vehicle-mounted device 1 calculates the attitude angle (pitch angle and / or roll angle) based on the embodiment, and estimates the vehicle position regarding the planar position and orientation by any scan matching.

[0123] Although the present invention has been described above with reference to the embodiments, the present invention is not limited to the above embodiments. Various modifications within the scope of the present invention that would be understood by those skilled in the art can be made to the configuration and details of the present invention. In other words, the present invention naturally includes various modifications and alterations that would be possible for those skilled in the art based on the entire disclosure, including the claims, and the technical ideas. Furthermore, the disclosures of the above-cited patent documents and other documents are incorporated herein by reference. [Explanation of symbols]

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

Claims

1. an acquisition unit that acquires, from the object position information for each voxel, position information corresponding to a plurality of surrounding road surface voxels including a road surface voxel corresponding to the road surface on which the planar position of the moving object is located; a normal vector calculation unit that calculates a normal vector with respect to an approximated plane determined based on position information corresponding to the plurality of road surface voxels; an angle calculation unit that calculates at least one of a pitch angle and a roll angle of the moving body based on the orientation of the moving body and the normal vector; An information processing device having the above.

2. The information processing device according to claim 1 , wherein the angle calculation unit calculates the pitch angle based on an inner product of a vector indicating a direction of a traveling direction of the moving object 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 an inner product of a vector indicating a lateral direction of the moving object on a horizontal plane and the normal vector.

4. The information processing device 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, for each voxel, position information of the object with data obtained by coordinate transformation of measurement data output by a measurement unit provided on the moving body based on at least one of the pitch angle and the roll angle.

5. the position information of the object for each voxel includes information of a mean vector relating to the position of the object for each voxel, 5. The information processing device according to claim 1, wherein the normal vector calculation unit calculates the normal vector based on coordinates of the average vector of each of the plurality of road surface voxels.

6. The information processing device according to any one of claims 1 to 5, wherein the acquisition unit acquires position information corresponding to a first road surface voxel that overlaps with the position of the moving body and a second road surface voxel that is adjacent to the first road surface voxel, from among the voxels in which the position information of the object exists.

7. The information processing device according to claim 6 , further comprising a height calculation unit that calculates a height of the moving body from a reference position based on position information corresponding to the first road surface voxel and information on a height of the moving body from a road surface.

8. the object position information is position information of a stationary structure including the road surface, The information processing device according to claim 1 , wherein the acquisition unit acquires position information corresponding to the plurality of road surface voxels from map data including position information of an object for each voxel.

9. A method executed by an information processing device, From the position information of the object for each voxel, position information corresponding to a plurality of surrounding road surface voxels including a road surface voxel corresponding to the road surface on which the planar position is located is obtained; Calculating a normal vector relative to the approximated plane determined based on the position information corresponding to the plurality of road surface voxels; A method for calculating at least one of a pitch angle and a roll angle of the moving body based on the orientation of the moving body and the normal vector.

10. an acquisition unit that acquires, from the position information of the object for each voxel, position information corresponding to a plurality of surrounding road surface voxels including a road surface voxel corresponding to the road surface of the road where the planar position is located; a normal vector calculation unit that calculates a normal vector with respect to an approximated plane determined based on position information corresponding to the plurality of road surface voxels; an angle calculation unit that calculates at least one of a pitch angle and a 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 as a

11. A storage medium storing the program according to claim 10.

Images