Information processing device, 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] This invention relates to a technique for estimating the attitude of a vehicle. [Background technology]

[0002] Conventionally, there is a known technique for estimating a vehicle's own position by matching shape data of surrounding objects measured using measuring devices such as laser scanners with map information in which the shapes of surrounding objects are pre-stored. For example, Patent Document 1 discloses an autonomous mobile system that determines whether a detected object in a voxel, which is divided into spaces according to a predetermined rule, is a stationary or moving object, and performs matching between map information and measurement data for voxels in which stationary objects exist. Patent Document 2 also discloses a scan matching method that estimates the vehicle's position by matching voxel data, which includes 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 release WO2013 / 076829 [Patent Document 2] International release WO2018 / 221453 [Overview of the project] [Problems that the invention aims to solve]

[0004] Generally, vehicles are constrained to the road surface, and changes in the vehicle's pitch angle, roll angle, and vertical direction are negligibly small, although there are fluctuations due to the suspension. Therefore, in typical scan matching-based vehicle self-position estimation, the vehicle's planar position and orientation are set as parameters to be estimated. On the other hand, when driving on steep slopes or roads with cross slopes, estimating only the vehicle's planar position and orientation is insufficient to account for changes in pitch angle and roll angle, resulting in a failure to match the voxels to be matched with the measurement data, or incorrect matching. If pitch angle and roll angle are added as estimation parameters to self-position estimation to account for changes in pitch angle and roll angle, the computational load increases due to the increase in estimation parameters, leading to the problem that self-position estimation cannot be completed stably at the required cycle. In addition, there are methods to determine the vehicle's pitch angle and roll angle from IMU (Inertial Measurement Unit) data, but typical IMUs have sensitivity errors and offsets, which makes it difficult to accurately calculate the pitch angle and roll angle.

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

[0006] The invention described in the claims includes an acquisition unit that acquires 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, from the position information of each voxel of the object, A normal vector calculation unit calculates a normal vector for an approximate plane obtained based on positional information corresponding to the plurality of road surface voxels, An angle calculation unit that calculates at least one of the pitch angle or roll angle of the moving body based on the orientation of the moving body and the normal vector, It is an information processing device.

[0007] Furthermore, the invention described in the claims is a method performed by an information processing device, From the position information of each object in each voxel, Mobile Extract location information corresponding to multiple surrounding road surface voxels, including the road surface voxel corresponding to the road surface where the planar position is located. Based on the positional information corresponding to the aforementioned multiple road surface voxels, a normal vector is calculated for the approximate plane obtained. This method calculates at least one of the pitch angle or roll angle of the moving body based on the orientation of the moving body and the normal vector.

[0008] Furthermore, the invention described in the claims uses the positional information of each voxel of an object, Mobile An extraction unit that extracts positional information corresponding to multiple surrounding road surface voxels, including the road surface voxel corresponding to the road surface where the planar position is located, A normal vector calculation unit calculates a normal vector for an approximate plane obtained based on positional information corresponding to the plurality of road surface voxels, Angle calculation unit calculates at least one of the pitch angle or roll angle of the moving body based on the orientation of the moving body and the normal vector. It is a program that makes a computer function. [Brief explanation of the drawing]

[0009] [Figure 1] This is a schematic diagram of the driver assistance system. [Figure 2] This is a block diagram showing the functional configuration of an in-vehicle device. [Figure 3] This is a diagram showing the state variable vector represented in two-dimensional Cartesian coordinates. [Figure 4] An example of a general data structure for voxel data is shown. [Figure 5] This is an example of a functional block in the NDT matching section. [Figure 6] (A) An overhead view showing the relationship between the vehicle's position and the road surface voxels. (B) A side view of the vehicle clearly showing the road surface average vector. [Figure 7] This is an xy-plane diagram showing the relationship between the vehicle's yaw angle and its direction of travel vector. [Figure 8] This diagram shows the relationship between the angle between the normal vector and the direction of travel vector and the pitch angle of the vehicle. [Figure 9] This is an xy-plane diagram showing the relationship between the vehicle's yaw angle and lateral vector. [Figure 10] This diagram shows the relationship between the angle between the normal vector and the transverse vector and the vehicle's roll angle. [Figure 11] (A) A side view of a vehicle and road traveling on a flat surface. (B) A side view of a vehicle and road traveling on a steeply sloping surface, when no coordinate transformation based on the vehicle's pitch angle is performed on the point cloud data. [Figure 12] This is a side view of a vehicle and road traveling on a road with a steep gradient, when performing coordinate transformations on point cloud data based on the pitch angle of a vehicle. [Figure 13] This is an example of a flowchart illustrating the procedure for estimating the position and orientation of a vehicle. [Figure 14] This is an example flowchart illustrating the procedure for estimating vehicle height. [Figure 15] This is an example flowchart illustrating the procedure for estimating the roll angle and pitch angle of a vehicle. [Figure 16] The following shows the number of point cloud data points, 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 based on the estimated pitch angle is performed. [Figure 17] The number of point cloud data points, the number of correspondences, the correspondence ratio, the evaluation value, and the pitch angle obtained when pitch angle estimation is not performed are shown below. [Modes for carrying out the invention]

[0010] According to a preferred embodiment of the present invention, the information processing device includes: an extraction unit that extracts position information of objects in a plurality of unit regions located near a moving body from position information of objects in each unit region that divides space; a normal vector calculation unit that calculates a normal vector for an approximate plane obtained based on the position information of objects in the plurality of unit regions; and an angle calculation unit that calculates at least one of the pitch angle or roll angle of the moving body based on the orientation of the moving body and the normal vector.

[0011] Generally, since a moving object travels on a road surface, it is assumed that a road surface exists near the moving object, and that multiple unit regions near the moving object include the road surface. Furthermore, the moving object is constrained to the road surface, and the pitch angle and roll angle of the moving object are generated according to the slope of the road surface. Taking the above into consideration, in this embodiment, the information processing device calculates a normal vector of a plane approximating the road surface based on the position information of the road surface and the position information of objects in multiple presumed unit regions. As a result, the information processing device can suitably calculate at least one of the pitch angle or roll angle of the moving object based on the relationship between the calculated normal vector and the orientation of the moving object.

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

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

[0014] In another embodiment of the above-described information processing device, the information processing device further includes a position estimation unit that estimates the position of the moving body by comparing the position information of the object with data obtained by coordinate transformation based on at least one of the pitch angle or roll angle from the measurement data output by the measurement unit provided on the moving body for each unit area. In this embodiment, the information processing device estimates the position of the moving body by matching the position information of the object with the measurement data output by the measurement unit. At this time, the information processing device transforms the coordinates of the measurement data based on the calculated pitch angle or roll angle. As a result, even if the pitch angle or roll angle of the vehicle changes due to driving on a slope or a road with a high transverse gradient, the information processing device can accurately match the unit area in which the object to be measured actually exists with the measurement data of the object to be measured.

[0015] In another embodiment of the above-described information processing device, the position information of an object in each unit region includes information on the average vector relating to the position of the object in each unit region, 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 regions. According to this embodiment, the information processing device can suitably calculate the normal vector of a plane approximating the road surface based on the position information of an object in a plurality of unit regions that are presumed to be the position information of the road surface on which the moving object is located.

[0016] In another embodiment of the information processing device described above, the extraction unit extracts the position information of the object in a first unit region where the planar position of the moving object overlaps with that of the unit regions where the position information of the object exists, and in a second unit region adjacent to the first unit region. According to this embodiment, the information processing device can suitably extract the position information of the object in multiple unit regions that are presumed to be the position information of the road surface.

[0017] In another embodiment of the above-described 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 position information of the object in the first unit area and the height information of the moving body from the road surface. The reference position is a reference position in an absolute coordinate system used in maps, etc., and for example, indicates a position at an altitude of 0m. According to this embodiment, the information processing device can suitably calculate the height of the moving body from the reference position using the position information of the object (road surface) in the first unit area where the moving body is presumed to exist. 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 the height information of the moving body from the road surface and the height of the moving body calculated one time earlier is greater than a predetermined value, the height from the reference position calculated one time earlier may be set as the height from the reference position at the current time. This suitably prevents the recognition of the height of the moving body with an error as the current height when an error occurs in the calculated height of the moving body due to the presence of an overpass, guardrail, etc., nearby.

[0018] In another embodiment of the information processing device described above, the object's position information is the position information of a stationary structure including the road surface, and the extraction unit extracts the position information of the road surface in each of the multiple unit areas from map data containing the object's position information for each unit area. In this embodiment, the information processing device can suitably extract the position information of the road surface on which a moving object exists from the map data and suitably calculate the normal vector of the approximate plane necessary for calculating the pitch angle or roll angle.

[0019] According to another preferred embodiment of the present invention, a control method performed by an information processing device involves extracting positional information of objects in a plurality of unit regions located near a moving body from positional information of objects in each unit region that divides space, calculating a normal vector for an approximate plane obtained based on the positional information of objects in the plurality of unit regions, and calculating at least one of the pitch angle or roll angle of the moving body based on the orientation of the moving body and the normal vector. By performing this control method, the information processing device can suitably calculate at least one of the pitch angle or roll angle of the moving body based on the relationship between the normal vector of the approximate plane based on the positional information of the unit regions near the moving body and the orientation of the moving body.

[0020] According to another preferred embodiment of the present invention, the program causes the computer to function as an extraction unit that extracts positional information of objects in a plurality of unit regions located near a moving body from positional information of objects in each unit region that divides space; a normal vector calculation unit that calculates a normal vector for an approximate plane obtained based on the positional information of objects in the plurality of unit regions; and an angle calculation unit that calculates at least one of the pitch angle or roll angle of the moving body based on the orientation of the moving body and the normal vector. By executing this program, the computer can suitably calculate at least one of the pitch angle or roll angle of the moving body based on the relationship between the normal vector of the approximate plane based on the positional information of the unit regions near the moving body and the orientation of the moving body. Preferably, the above program is stored in a storage medium. [Examples]

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

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

[0023] The in-vehicle unit 1 is electrically connected to the lidar 2, gyro sensor 3, vehicle speed sensor 4, and GPS receiver 5, and based on their outputs, it estimates the position of the vehicle on which the in-vehicle unit 1 is installed (also called "vehicle position"). Based on the estimated vehicle position, the in-vehicle unit 1 performs automatic driving control of the vehicle so that it travels along a set route to a destination. The in-vehicle unit 1 stores a map database (DB: Database) 10 that contains voxel data "VD". Voxel data VD is data that records position information of stationary structures for each voxel, which represents a cube (normal grid) that is the smallest unit of three-dimensional space. Voxel data VD includes data that represents the measured point cloud data of stationary structures within each voxel according to a normal distribution, and is used for scan matching using NDT (Normal Distributions Transform), as described later. Furthermore, the in-vehicle unit 1 estimates the vehicle's position on the plane and yaw angle by NDT scan matching, and also estimates the vehicle's height position and at least one of the pitch angle and roll angle based on voxel data VD.

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

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

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

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

[0028] The storage unit 12 stores programs executed by the control unit 15 and information necessary for the control unit 15 to perform predetermined processes. In this embodiment, the storage unit 12 stores the map DB 10, which includes voxel data VD. The map DB 10 may be updated periodically. In this case, for example, the control unit 15 receives partial map information relating to the area to which the vehicle's position belongs from a server device that manages map information via the communication unit 13 and reflects it in the map DB 10. The storage unit 12 does not have 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 to obtain information necessary for vehicle position estimation processing, etc., at the necessary timing.

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

[0030] The control unit 15 includes a CPU that executes programs and controls the entire in-vehicle unit 1. In this embodiment, the control unit 15 has an attitude angle calculation unit 17 and 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 programs in the present invention.

[0031] The attitude angle calculation unit 17 calculates at least one of the vehicle's pitch angle or roll angle by referring to the voxel data VD. The NDT matching unit 18 estimates the vehicle's position by performing NDT-based scan matching (NDT scan matching) based on the point cloud data output from the rider 2 and the voxel data VD corresponding to the voxel 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 the horizontal plane specified by latitude and longitude) and yaw angle (i.e., direction) as estimation parameters in NDT scan matching.

[0032] (3) NDT scan matching-based position estimation Figure 3 shows the vehicle's position to be estimated by the NDT matching unit 18, represented in two-dimensional Cartesian coordinates. As shown in Figure 3, the vehicle's position on a plane defined on a two-dimensional Cartesian coordinate system is represented by the coordinates "(x, y)" and the vehicle's orientation (yaw angle) "ψ". Here, the yaw angle ψ is defined as the angle between the vehicle's direction of travel and the x-axis. The coordinates (x, y) are, for example, an absolute position 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 performs vehicle position estimation using these x, y, and ψ as estimation parameters. For information on the estimation method of the vehicle's pitch angle and roll angle, see "(4) Estimation of attitude angle This will be explained in detail in the section.

[0033] Next, we will explain the voxel data VD used for NDT scan matching. The voxel data VD includes data representing the measured point cloud data of stationary structures within each voxel according to a normal distribution.

[0034] Figure 4 shows an example of a schematic data structure for voxel data VD. Voxel data VD contains parameter information for representing the point cloud within the voxels using a normal distribution. In this embodiment, as shown in Figure 4, it includes voxel ID, voxel coordinates, mean vector, and covariance matrix.

[0035] "Voxel coordinates" refer to absolute three-dimensional coordinates that serve as reference positions such as the center position of each voxel. Each voxel is a cube obtained by dividing space into a grid, and its shape and size are predetermined, so the space of each voxel can be specified by voxel coordinates. Voxel coordinates may be used as voxel IDs.

[0036] "Mean vector" and "covariance matrix" refer to the mean vector and covariance matrix corresponding to parameters when expressing a point cloud in a target voxel by a normal distribution. The coordinates of an arbitrary point "i" in an arbitrary voxel "n" are defined as X n (i)=[x n (i), y n (i), z n (i)] T and let the number of point clouds in voxel n be "N n ", then the mean vector "μ n " and covariance matrix "V n " for voxel n are represented by the following formula (1) and formula (2) respectively.

[0037] [Math.]]

[0038] [Math.]]

[0039] Next, an outline of NDT scan matching using voxel data VD is described.

[0040] NDT scan matching assuming a vehicle uses the amount of movement in a road plane (referred to as xy coordinates herein) and the orientation of the vehicle as elements of an estimation parameter P=[t x , t y , t ψ T that is to be estimated. 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] Furthermore, the point cloud data obtained by LIDA2 is mapped to the voxels to be matched, and the coordinates of any point in the corresponding voxel n are determined. X L (j) = [x n (j), y n (j), z n (j)] T Therefore, X in voxel n L (j) Average value "L' n This can be expressed by the following equation (3).

[0042]

number

[0043]

number

[0044]

number

[0045] The in-vehicle unit 1 then calculates an overall evaluation function value (also called the "overall evaluation function value") "E(k)" for all voxels subject to matching, as shown by the following equation (6).

[0046]

number

[0047]

number

[0048] Figure 5 shows an example of the functional blocks of the NDT matching unit 18. As shown in Figure 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 mapping block 24, and a position correction block 25.

[0049] The dead reckoning block 21 uses the vehicle's speed and angular velocity based on the outputs of the gyro sensor 3, vehicle speed sensor 4, and GPS receiver 5 to determine the distance traveled and the change in direction from the previous time. The position prediction block 22 calculates the estimated vehicle position X at time k-1 calculated in the previous measurement update step. ^ Adding the calculated distance traveled and change in direction to (k-1), we predict the vehicle's position X at time k. - Calculate (k).

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

[0051] The point cloud data mapping block 24 maps the point cloud data to voxels by comparing the point cloud data in the world coordinate system output by the coordinate transformation block 23 with the voxel data VD, which is represented in the same world coordinate system. The position correction block 25 calculates individual evaluation function values ​​based on equation (5) for each voxel that has been mapped to the point cloud data, and calculates the estimated parameter P that maximizes the overall evaluation function value E(k) based on equation (6). Then, based on equation (7), the position correction block 25 calculates the predicted vehicle position X output by the position prediction block 22. - By applying the estimated parameter P obtained at time k to (k), the estimated vehicle position X can be obtained. ^ Calculate (k).

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

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

[0054] The attitude angle calculation unit 17 refers to the voxel data VD and uses the voxels corresponding to the road surface (also called "road surface voxels") to determine the vehicle's planar position x, y (i.e., (x)) as predicted or estimated by the NDT matching unit 18. - , y - ) or (x^ , y ^ The system searches for the road surface voxel where the vehicle's position x and y are located. The attitude angle calculation unit 17 then acquires voxel data VD corresponding to n surrounding road surface voxels, including the road surface voxel where the vehicle's planar position x and y are located (also called the "vehicle position road surface voxel"). "n" is any integer greater than or equal to 3. The vehicle position road surface voxel is an example of the "first unit region" in this invention, and the n-1 surrounding road surface voxels other than the vehicle position road surface voxel are examples of the "second unit region" in this invention.

[0055] Figure 6(A) is an overhead view showing the correspondence between the vehicle's position and the road surface voxels. In Figure 6(A), the vehicle's planar position is x, y (here, (x ^ , y ^ The vehicle's position road surface voxel "Vo5" where the vehicle is located, and the road surface voxels "Vo1" to "Vo4" and "Vo6" to "Vo9" in front of, behind, to the left and right of the vehicle's position road surface voxel Vo5 are shown. In this example, "n=9" is set, and the attitude angle calculation unit 17 obtains 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 of the road surface in the world coordinate system (also called the "road surface average vector") contained in each of the n road surface voxels, based on the information of the voxel coordinates and average vectors contained in the voxel data VD corresponding to each of the n road surface voxels.

[0057] Figure 6(B) is a side view of the vehicle showing the average road surface vector for each road surface voxel. In Figure 6(B), the coordinate positions "M4" to "M6" of the average road surface vectors corresponding to each of the road surface voxels Vo4 to Vo6 shown in Figure 6(A) are shown. The attitude angle calculation unit 17 calculates the average road surface 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, since "n=9".

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

[0059]

number

[0060]

number

[0061]

number

[0062]

number

[0063]

number

[0064]

number

[0065]

number

[0066]

number

[0067]

number

[0068]

number

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

[0070]

number

[0071] (4-2) Estimation of Roll 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 voxel data VD, similar to the calculation of the pitch angle.

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

[0073]

number

[0074]

number

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

[0076]

number

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

[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 perform the rotation matrix "R" shown in the following equation (22). θ This generates ".

[0079]

number

[0080]

number

[0081]

number

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

[0083]

number

[0084]

number

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

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

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

[0088] Figure 11(A) is a side view of a vehicle traveling on a flat road surface and the road. In this case, the vehicle height "z" is calculated from the reference position in the world coordinate system (for example, the position where the elevation is 0). ^ " is the height obtained by adding the vehicle's height from the road surface (also called "vehicle reference position z0") to the height from the reference position in the world coordinate system to the road surface. Then, the height "d" from the vehicle at a certain measurement point "Ptag1" measured by Rider 2 is used as the vehicle height z ^ "z" added to ^ "+d" corresponds to the actual height of measurement point Ptag1 in the world coordinate system. Therefore, in this case, the mapping process performed by the point cloud data mapping block 24 maps the target measurement point Ptag1 to the voxel "V1" that actually contains measurement point Ptag1.

[0089] Figure 11(B) is a side view of a vehicle and road traveling on a steeply sloping surface, where no coordinate transformation based on the vehicle's pitch angle is performed on the point cloud data. In this case, the road surface gradient causes a pitch angle in the vehicle, so the point cloud data points upward. Therefore, the height in the world coordinate system of the 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 ^ It does not become "+d". As a result, in the mapping process performed by the point cloud data mapping block 24, the height of the measurement point Ptag2 in the world coordinate system is "z ^ Because it is calculated as "+d", it is not associated with voxel "V2" where the measurement point Ptag2 actually exists, and is incorrectly associated with voxel "V2α".

[0090] Figure 12 is a side view of a vehicle and road traveling on a steeply sloping surface, when performing a coordinate transformation based on the vehicle's pitch angle on point cloud data according to this embodiment. In this case, the point cloud data output by LIDA2 is transformed into a world coordinate system that takes the vehicle's pitch angle into account by a rotation matrix using the pitch angle obtained by the calculation method according to the embodiment. Therefore, in this case, the height of the measurement point "Ptag3" located at a height d from the vehicle in the world coordinate system is "z ^ This results in "+d". Therefore, in the mapping process performed by the point cloud data mapping block 24, the measurement point Ptag3 is mapped to the voxel "V3" where the measurement point Ptag3 actually exists (i.e., the correct matching target), and NDT scan matching is suitably performed.

[0091] Even when driving on roads with cross slopes such as banks, the onboard unit 1 can perform coordinate transformations on the point cloud data based on the vehicle's roll angle, thereby appropriately associating each measurement point in the point cloud data output by the rider 2 with the correct matching voxel.

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

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

[0094] First, the dead reckoning block 21 of the NDT matching unit 18 obtains the travel distance and azimuth change from the previous time using the moving speed and angular velocity of the vehicle based on outputs from the gyro sensor 3, vehicle speed sensor 4, GPS receiver 5 and the like. Accordingly, the position prediction block 22 of the NDT matching unit 18 obtains the estimated host vehicle position x acquired one time step before (the immediately preceding processing time) ^ , y ^ , ψ ^ to calculate the predicted host vehicle position x at the current time - , y - , ψ - (step S11).

[0095] Next, the attitude angle calculation unit 17 performs estimation processing for the vehicle height (for example, the altitude at which the vehicle is located) to calculate the predicted vehicle height "z - " (step S12). This processing will be described later with reference to FIG. 14. Further, the attitude angle calculation unit 17 performs estimation processing for the roll angle and pitch angle of the vehicle to calculate the predicted roll angle "φ - " and the predicted pitch angle "θ - " (step S13). This processing will be described later with reference to FIG. 15.

[0096] Then, the coordinate conversion block 23 of the NDT matching unit 18 generates a rotation matrix R (see formula (25)) based on the roll angle and pitch angle calculated in step S13 (step S14). Then, the coordinate conversion block 23 converts the point cloud data into data in the world coordinate system (step S15). Thereafter, the NDT matching unit 18 (the point cloud data association block 24 and the position correction block 25) performs NDT matching using the coordinate-converted point cloud data and voxel data VD to obtain the estimated host vehicle position x at the current time ^ , y ^ , ψ ^ (step S16). Thereafter, the attitude angle calculation unit 17 uses the calculated estimated host vehicle position x at the current time ^ , y ^ , ψ ^ to perform the same vehicle height estimation processing as in step S12 again, thereby obtaining the estimated vehicle height "z ^is calculated (step S17). This processing will be described later with reference to FIG. 14. Further, the attitude angle calculation unit 17 calculates the estimated host vehicle position x at the calculated current time ^ , y ^ , ψ ^ and the estimated vehicle height z ^ are used to perform estimation processing of the roll angle and pitch angle of the vehicle, thereby obtaining the estimated roll angle φ ^ and the estimated pitch angle θ ^ are calculated (step S18). This processing will be described later with reference to FIG. 15.

[0097] (6-2) Vehicle Height Estimation Processing FIG. 14 is an example of a flowchart showing the procedure of vehicle height estimation processing executed in step S12 and step S17 of FIG. 13. In the vehicle height estimation processing, the attitude angle calculation unit 17 determines a road surface voxel at the host vehicle position based on the predicted or estimated planar position of the vehicle, and adds the vehicle reference position z0 (height of the vehicle from the road surface) to the z-coordinate of the average vector of the road surface voxel at the host vehicle position, and determines the value obtained by the addition as the vehicle height to be obtained. Further, the in-vehicle device 1 sets the voxel index idz of the road surface voxel at the host vehicle position with low reliability to "-1" as the road surface voxel at the host vehicle position, and labels it so that it is not used in the roll angle and pitch angle estimation processing described later.

[0098] First, the attitude angle calculation unit 17 calculates the predicted host vehicle position x - , y - or the estimated host vehicle position x ^ , y ^ , acquires the voxel indexes (idx, idy) of the voxel including the position, and searches for a voxel having the voxel indexes (idx, idy) (step S21). That is, the attitude angle calculation unit 17 refers to the voxel data VD, and searches for voxels that are at the same position in the xy plane as the predicted host vehicle position x - , y - or the estimated host vehicle position x ^ , y ^ in the height direction. In the case of step S12 in FIG. 13, the attitude angle calculation unit 17 uses the predicted host vehicle position x - , y -The voxel containing the voxel index (idx, idy) is searched for. In addition, the attitude angle calculation unit 17, in the case of step S17 in Figure 13, estimates the vehicle's position x ^ , y ^ Search for voxels that have the voxel index (idx, idy) of the voxel containing the specified value.

[0099] Next, in step S21, the attitude angle calculation unit 17 determines whether or not one or more voxels have been detected (step S22). If no voxels are detected (step S22; No), the attitude angle calculation unit 17 determines the predicted vehicle height z one time step earlier. - or estimated vehicle height z ^ The predicted vehicle height z that should be determined is - or estimated vehicle height z ^ This is determined (step S27). Furthermore, in step S27, the attitude angle calculation unit 17 determines a provisional road surface voxel for the vehicle's position where 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 considers the detected voxels as candidates for the vehicle's position road surface voxel, reads the z-coordinate of each candidate (the z-coordinate of the average 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, for example, in the storage unit 12.

[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 average vector of each candidate, and the predicted vehicle height z from one time point ago. - or estimated vehicle height z ^ The candidate road surface voxel for the vehicle's position that has the closest value is selected as the road surface voxel for the vehicle's position (step S24).

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

[0103] Then, the attitude angle calculation unit 17 calculates the desired predicted vehicle height z by adding the vehicle reference position z0 to the z coordinate of the average vector of the road surface voxel at the vehicle's position. - or estimated vehicle height z ^ This is determined (step S26). On the other hand, if the above difference is greater than a predetermined value (step S25; No), the attitude angle calculation unit 17 determines the predicted vehicle height z one time step earlier. - or estimated vehicle height z ^ The predicted vehicle height z that should be determined is - or estimated vehicle height z ^ This is determined (step S27). Furthermore, in step S27, the attitude angle calculation unit 17 sets the voxel index idz of the road surface voxel at the vehicle's position selected in step S24 to "-1".

[0104] (6-3) Roll angle and pitch angle estimation process Figure 15 is an example flowchart showing the procedure for estimating the roll angle and pitch angle of a vehicle, which is performed in steps S13 and S18 of Figure 13. In Figure 15, the in-vehicle device 1 estimates both the roll angle and pitch angle of the vehicle as an example, but it is also possible to perform a process that estimates only one of the roll angle or pitch angle of the vehicle.

[0105] First, the attitude angle calculation unit 17 refers to the voxel index (idx, idy, idz) of the road surface voxel at the vehicle's position, which was determined in the vehicle height estimation process of step S12 or step S17 performed immediately before (step S31). Then, the attitude angle calculation unit 17 determines whether the voxel index idz of the road surface voxel at the vehicle's position is set to "-1" (step S32). If the voxel index idz of the road surface voxel at the vehicle's position is "-1" (step S32; Yes), the attitude angle calculation unit 17 determines that the reliability of the target road surface voxel at the vehicle's position is low or that the road surface voxel at the vehicle's position could not be detected. Therefore, in this case, the attitude angle calculation unit 17 calculates the roll angle φ(φ) calculated one time step earlier. - or φ ^ ) or pitch angle θ(θ - or θ ^ ) is the roll angle φ(φ) at the current time that we want to find. - or φ ^ ) or pitch angle θ(θ - or θ ^ ) is defined as (Step S38).

[0106] On the other hand, if the voxel index idz of the road surface voxel at the vehicle's position is not "-1" (step S32; No), the attitude angle calculation unit 17 obtains voxel data VD of n voxels surrounding the road surface voxel at the vehicle's position, including the road surface voxel at the vehicle's position (step S33). For example, the attitude angle calculation unit 17 considers voxels that differ from the road surface voxel at the vehicle's position by at least one of the voxel indices idx and idy, and that have the same or one different voxel index idz, together with the road surface voxel at the vehicle's position, as the aforementioned n voxels.

[0107] Then, the attitude angle calculation unit 17 reads the x, y, and z coordinates of the average vector contained in each of the n voxel data VD for the aforementioned 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 the normal vector Vn shown in equation (15) (step S35). Then, the attitude angle calculation unit 17 calculates the predicted or estimated yaw angle (direction) of the vehicle ψ(ψ - or ψ ^ Using ), the direction vector Vx shown in equation (16) and the lateral vector V shown by equation (19) are used. Y The attitude angle calculation unit 17 then calculates the dot product of the normal vector Vn and the direction of travel vector Vx based on equation (17), and also calculates the dot product of the normal vector Vn and the lateral vector Vx based on equation (20). Y Calculate the dot product and find the roll angle φ(φ) at the current time. - or φ ^ ) or pitch angle θ(θ - or θ ^ Calculate (Step S37).

[0108] (7) Experimental example The applicant drove a vehicle equipped with a RIDA in the same configuration as shown in Figure 1 on a road including a steep slope, performed self-position estimation based on NDT scan matching, and collected the necessary data. The RIDA mounted on the vehicle had a maximum measuring distance of 25m and a horizontal angle of approximately 120 degrees for measuring distance.

[0109] Figures 16(A) to (E) show the number of point cloud data points, the number of correspondences, the correspondence ratio, the evaluation value, and the transition of the pitch angle obtained on the aforementioned road 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. Figures 17(A) to (E) show the number of point cloud data points, the number of correspondences, the correspondence ratio, the evaluation value, and the pitch angle obtained on the aforementioned road 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 for each processing time, and "Number of correspondences" indicates the number of measurement points out of the number of point cloud data that were able to be matched with voxels. Furthermore, "Coordination 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 estimated parameters defined in NDT scan matching. "Pitch angle" indicates the pitch angle of the vehicle estimated by the vehicle. Note that in the cases of Figures 17(A) to (E), pitch angle estimation was not performed, so the pitch angle shown in Figure 17(E) is always 0 degrees. Also, the period indicated by the dashed arrow 58 in Figures 16(B) to (E) and the period indicated by the dashed arrow 59 in Figures 17(B) to (E) are periods when the vehicle was traveling on roads where there was no surrounding voxel data VD, and therefore NDT scan matching was not performed in those areas.

[0111] In Figure 16(E), the absolute value of the pitch angle temporarily increases during the periods indicated by dashed lines 60 and 61, corresponding to the period when the vehicle is traveling on an incline. Even during such periods of travel on an incline, as shown in Figure 16(C), the correspondence ratio is maintained at the same level as during other periods of travel on flat roads, as the decrease in the number of correspondences is suppressed by the coordinate transformation of the point cloud data that takes the pitch angle into consideration.

[0112] On the other hand, if pitch angle estimation and coordinate transformation of point cloud data based on pitch angle are not performed, the correspondence ratio corresponding to the period corresponding to the period of driving on the slope mentioned above temporarily decreases significantly, as shown in dashed boxes 70 and 71 in Figure 17(C). It should be noted that if a decrease in the number of correspondences and the correspondence ratio occurs, it is presumed that the robustness of NDT scan matching decreases and the reliability of the estimated vehicle position is low.

[0113] Thus, according to this embodiment, by estimating the pitch angle and performing coordinate transformation of point cloud data based on the pitch angle, the decrease in the number of corresponding point cloud data and the corresponding ratio can be suitably suppressed even on slopes, and the robustness of self-position estimation by NDT scan matching can be suitably improved. Similarly, by estimating the roll angle and performing coordinate transformation of point cloud data based on the roll angle, the decrease in the number of corresponding point cloud data and the corresponding ratio can be suitably suppressed even on roads with high transverse gradients, and the robustness of self-position estimation by NDT scan matching can be suitably improved.

[0114] As described above, the control unit 15 of the in-vehicle device 1 according to this embodiment extracts voxel data VD of multiple voxels located near the vehicle from the voxel data VD, which is the position information of objects for each unit region (voxel) that divides space. The control unit 15 then calculates a normal vector for an approximate plane obtained based on the voxel data VD of the multiple extracted voxels. The control unit 15 then calculates at least one of the pitch angle or roll angle of the vehicle based on the orientation of the vehicle and the normal vector. In this embodiment, the in-vehicle device 1 can calculate at least one of the pitch angle or roll angle with high accuracy based on the voxel data VD.

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

[0116] (Variation 1) In the flowchart shown in Figure 13, the in-vehicle unit 1 performs the vehicle height estimation process shown in Figure 14, thereby determining the vehicle height (predicted vehicle height z). - or estimated vehicle height z ^ ) was calculated. However, the method for calculating the vehicle height is not limited to this.

[0117] For example, the in-vehicle unit 1 may add vehicle height as an additional estimation parameter in its self-position estimation based on NDT scan matching. In this case, the in-vehicle unit 1 performs self-position estimation using four variables (x, y, z, ψ) as state variables for the self-position, taking into account the coordinates (x, y) and yaw angle ψ shown in Figure 3, as well as the coordinates of the z axis perpendicular to the x and y axes. In this embodiment as well, the in-vehicle unit 1 can suitably estimate the vehicle height.

[0118] (Modification 2) Even if NDT scan matching is not performed, the in-vehicle device 1 may estimate at least one of the vehicle's pitch angle or roll angle based on the embodiment.

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

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

[0121] (Modification 4) The voxel data VD is not limited to a data structure that includes a mean vector and a covariance matrix, as shown in Figure 3. For example, the voxel data VD may include the point cloud data measured by the measurement and maintenance vehicle used to calculate the mean vector and covariance matrix. In this case, the on-board unit 1 generates the matrix C shown in equation (10) by, for example, referring to the voxel data VD and calculating the mean vector for each voxel.

[0122] Furthermore, this embodiment is not limited to scan matching by NDT, and other scan matching methods such as ICP (Iterative Closest Point) may be applied. In this case as well, the in-vehicle unit 1 calculates attitude angles (pitch angle and / or roll angle) based on the embodiment, and estimates the vehicle's position in terms of planar position and orientation using arbitrary scan matching.

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

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

Claims

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

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

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

4. The information processing apparatus according to any one of claims 1 to 3, further comprising a position estimation unit that performs position estimation of the moving body by comparing the 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 or roll angle for each of the voxels.

5. The position information of the object for each voxel includes information on the average vector relating to the position of the object for each voxel, The information processing apparatus according to any one of claims 1 to 4, wherein the normal vector calculation unit calculates the normal vector based on the coordinates of each of the average vectors in the plurality of road surface voxels.

6. The information processing apparatus 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 adjacent to the first road surface voxel, from among the voxels in which the position information of the object exists.

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

8. The position information of the object is the position information of a stationary structure including the road surface, The information processing apparatus according to any one of claims 1 to 7, wherein the acquisition unit acquires location information corresponding to the plurality of road surface voxels from map data including location information of objects for each voxel.

9. A method executed by an information processing device, From the position information of each voxel, the position information corresponding to multiple surrounding road surface voxels, including the road surface voxel corresponding to the road surface on which the moving object's planar position is located, is obtained. Based on the positional information corresponding to the aforementioned multiple road surface voxels, a normal vector is calculated for the approximate plane obtained. A method for calculating at least one of the pitch angle or roll angle of a moving body based on the orientation of the moving body and the normal vector.

10. An acquisition unit that acquires position information corresponding to multiple surrounding road surface voxels, including the road surface voxel corresponding to the road surface on which the planar position of the moving object is located, from the position information of each voxel of the object, A normal vector calculation unit calculates a normal vector for an approximate plane obtained based on positional information corresponding to the plurality of road surface voxels, Angle calculation unit calculates at least one of the pitch angle or roll angle of the moving body based on the orientation of the moving body and the normal vector. A program that makes a computer function.

11. A storage medium storing the program described in claim 10.

Images