Posture estimation device, posture estimation method, program, and storage medium

Abstract

To provide a posture estimation device capable of appropriately estimating an installation posture of a measurement device installed on a mobile vehicle.SOLUTION: A vehicle-mounted device 1 acquires a measured point cloud obtained by measuring lane markings using a LiDAR 2 installed on a vehicle, and derives a center line of the lane markings along a traveling direction of the vehicle on the basis of the measured point cloud. The vehicle-mounted device 1 then estimates an installation posture of the LiDAR 2 on the vehicle based on the direction of the center line of the lane markings relative to a yaw direction reference angle used as a reference by the LiDAR 2.SELECTED DRAWING: Figure 15

Description

[Technical Field]

[0001] The present invention relates to a technique for estimating the attitude of a measurement device. [Background technology]

[0002] Conventionally, there have been known techniques for estimating a vehicle's own position based on measurement data from measurement devices such as radars and cameras. For example, Patent Document 1 discloses a technique for estimating a vehicle's own position by comparing the output of a measurement sensor with position information of features registered in advance on a map. Furthermore, Patent Document 2 discloses a vehicle's own position estimation technique using a Kalman filter. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Application Laid-Open No. 2013-257742 [Patent Document 2] Japanese Patent Application Publication No. 2017-72422 Summary of the Invention [Problem to be solved by the invention]

[0004] The data obtained from the measurement device is a value in a coordinate system based on the measurement device, and is dependent on the attitude of the measurement device relative to the vehicle, so it needs to be converted to a value in a coordinate system based on the vehicle. Therefore, if a deviation occurs in the attitude of the measurement device, it is necessary to accurately detect the deviation and reflect it in the data of the measurement device, or to correct the attitude of the measurement device.

[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 attitude estimation device that can suitably estimate the mounting attitude of a measuring device attached to a moving body with respect to the moving body. [Means for solving the problem]

[0006] The claimed invention is an attitude estimation device comprising: an acquisition means for acquiring, from a measurement point cloud output by a measurement device attached to a moving body, a target measurement point cloud that has a measurement surface that is parallel to the road surface of the lane on which the moving body is traveling and that measures an area in which it is estimated that features formed along the lane exist; a calculation means for calculating a line on the measurement surface that follows the traveling direction of the moving body based on a plurality of points extracted from the target measurement point cloud for each scanning line of the measurement device; and an estimation means for assuming that the direction along the lane is the traveling direction of the moving body and estimating the mounting attitude of the measurement device on the moving body based on the angle between the direction referenced by the measurement device and the direction of the line.

[0007] The claimed invention is a posture estimation method executed by a posture estimation device, comprising: an acquisition step of acquiring, from a measurement point cloud output by a measurement device attached to a moving body, a target measurement point cloud that has a measurement surface that is parallel to the road surface of the lane on which the moving body is traveling and that measures an area in which it is estimated that features formed along the lane exist; a calculation step of calculating a line on the measurement surface that follows the traveling direction of the moving body based on a plurality of points extracted from the target measurement point cloud for each scanning line of the measurement device; and an estimation step of estimating the mounting posture of the measurement device on the moving body based on the angle between the direction referenced by the measurement device and the direction of the line, assuming that the direction along the lane is the traveling direction of the moving body.

[0008] The invention described in the claims is a program executed by a computer, which causes the computer to sequentially execute the following processes: obtain, from a measurement point cloud output by a measurement device attached to a moving body, a target measurement point cloud having a measurement surface parallel to the road surface of the lane on which the moving body is traveling, and measuring an area in which it is estimated that features formed along the lane exist; calculate a line on the measurement surface along the traveling direction of the moving body based on a plurality of points extracted from the target measurement point cloud for each scanning line of the measurement device; regard the direction along the lane as the traveling direction of the moving body; and estimate the mounting attitude of the measurement device on the moving body based on the angle between the direction referenced by the measurement device and the direction of the line. [Brief explanation of the drawings]

[0009] [Figure 1] FIG. 1 is a schematic configuration diagram of a vehicle system. [Figure 2] FIG. 2 is a block diagram showing the functional configuration of the vehicle-mounted device. [Figure 3] FIG. 2 is a diagram showing the relationship between a vehicle coordinate system and a rider coordinate system expressed by two-dimensional coordinates. [Figure 4] FIG. 2 is a diagram showing the relationship between a vehicle coordinate system and a rider coordinate system expressed by three-dimensional coordinates. [Figure 5] 1 shows a bird's-eye view of the area around a vehicle when the vehicle is traveling near a road sign that is a feature to be detected when estimating the roll angle of the lidar. [Figure 6] This shows the positional relationship between the road sign and the target measurement point cloud when the measured surface of the road sign is viewed from the front. [Figure 7] 1 shows the target measurement point cloud and the extracted outer edge point cloud in the lidar coordinate system. [Figure 8] The approximate straight lines of each side of the quadrangle formed by the outer boundary points are shown. [Figure 9] 10 is a flowchart showing a procedure for roll angle estimation processing. [Figure 10] 10 shows a bird's-eye view of the area around a vehicle when the vehicle is traveling near a road sign that is a feature to be detected when estimating the pitch angle of the lidar. [Figure 11] 10 is a flowchart showing a procedure for a pitch angle estimation process. [Figure 12] 1 shows a bird's-eye view of the area around a vehicle when the vehicle is traveling along a lane marking that is a feature to be detected when estimating the lidar yaw angle. [Figure 13] FIG. 10 is a diagram showing the center point of the detection window with a circle. [Figure 14] The angle between the center line of the lane marking and the yaw direction reference angle calculated for each detection window is shown. [Figure 15] 10 is a flowchart showing the procedure of a yaw angle estimation process. [Figure 16] 10 is a flowchart illustrating a specific example of processing based on estimated values ​​of a roll angle, a pitch angle, and a yaw angle. DETAILED DESCRIPTION OF THE INVENTION

[0010] According to a preferred embodiment of the present invention, the posture estimation device includes an acquisition means for acquiring a cloud of measurement points obtained by measuring features whose measurement surface is parallel to a road surface on which the mobile body is traveling and formed along the road surface using a measurement device attached to the mobile body, a calculation means for calculating a line on the measurement surface that follows the traveling direction of the mobile body based on the measurement point cloud, and an estimation means for estimating the attachment posture of the measurement device to the mobile body based on the direction of the line relative to a direction used as a reference by the measurement device.With this aspect, the posture estimation device can preferably estimate the attachment posture of the measurement device to the mobile body based on a feature whose measurement surface is parallel to the road surface on which the mobile body is traveling and formed along the road surface.

[0011] In one aspect of the posture estimation device, the acquisition means recognizes the position of the feature based on feature information about the feature, and acquires a measurement point cloud measured by the measurement device at that position as the measurement point cloud of the feature. With this aspect, the posture estimation device can grasp the position of a feature having a vertical plane along the travel path of the mobile object or a vertical plane along the width direction of the travel path, and suitably acquire the measurement point cloud of the feature.

[0012] In another aspect of the attitude estimation device, the estimation means estimates the attitude of the measurement device in the yaw direction based on the angle between the line and a reference line in the yaw direction of the measurement device. This aspect of the attitude estimation device allows the attitude estimation device to preferably estimate the attitude of the measurement device in the yaw direction.

[0013] In another aspect of the attitude estimation device, the estimation means estimates the attitude of the measurement device in the yaw direction by averaging angles between the reference line and the multiple lines calculated by the calculation means based on different measurement point clouds. This aspect of the attitude estimation device allows for more accurate estimation of the attitude of the measurement device in the yaw direction.

[0014] In another aspect of the attitude estimation device, the calculation means calculates a plurality of points on the measurement surface along the traveling direction of the moving object based on the measurement point cloud and calculates the line from the plurality of points, and the estimation means estimates the attitude of the measurement device in the yaw direction by averaging the angles between the plurality of lines calculated by the calculation means based on different measurement point clouds and the reference line based on the number of points used in calculating each of the lines. In this aspect, the attitude estimation device can more accurately estimate the attitude of the measurement device in the yaw direction by weighting the calculated lines in a way that suitably takes into account the accuracy of each of the calculated lines.

[0015] In a preferred example, the feature may be a dividing line or a curb provided on the road. In another preferred example, the measurement device may be an optical scanning device that scans light.

[0016] In another aspect of the above-described posture estimation device, the reference direction is a direction that is regarded as the traveling direction of the moving body in the coordinate system of the measurement device when there is no deviation in the attachment posture, and when there is a deviation in the attachment posture, the reference direction indicates a direction different from the traveling direction of the moving body according to the deviation. With this aspect, the posture estimation device can preferably estimate the attachment posture of the measurement device to the moving body based on the calculated deviation of the reference direction with respect to a line on the measurement surface.

[0017] According to another preferred embodiment of the present invention, there is provided a control method executed by an attitude estimation device, the control method comprising: an acquisition step of acquiring a measurement point cloud obtained by measuring features formed along a road surface on which a mobile body is traveling and whose measurement surface is parallel to the road surface on which the mobile body is traveling, using a measurement device attached to the mobile body; a calculation step of calculating a line on the measurement surface that follows the traveling direction of the mobile body based on the measurement point cloud; and an estimation step of estimating the attachment attitude of the measurement device to the mobile body based on the direction of the line relative to a direction that the measurement device uses as a reference. By executing this control method, the attitude estimation device can preferably estimate the attachment attitude of the measurement device to the mobile body.

[0018] According to another preferred embodiment of the present invention, there is provided a computer-executable program that causes the computer to function as: an acquisition means for acquiring a measurement point cloud obtained by measuring features formed along a road surface on which a mobile body is traveling and whose measurement surface is parallel to the road surface on which the mobile body is traveling, using a measurement device attached to the mobile body; a calculation means for calculating a line on the measurement surface along the traveling direction of the mobile body based on the measurement point cloud; and an estimation means for estimating the attachment orientation of the measurement device to the mobile body based on the direction of the line relative to a direction that the measurement device uses as a reference. By executing this program, the computer can preferably estimate the attachment orientation of the measurement device to the mobile body. Preferably, the program is stored in a storage medium. [Example]

[0019] Preferred embodiments of the present invention will now be described with reference to the drawings.

[0020] [Schematic configuration] Fig. 1 is a schematic configuration diagram of a vehicle system according to this embodiment. The vehicle system shown in Fig. 1 includes an on-board device 1 that controls driving assistance for the vehicle, and a group of sensors such as a Lidar (Light Detection and Ranging or Laser Illuminated Detection and Ranging) 2, a gyro sensor 3, an acceleration sensor 4, and a GPS receiver 5.

[0021] The vehicle-mounted device 1 is electrically connected to a group of sensors, such as a LIDAR 2, a gyro sensor 3, an acceleration sensor 4, and a GPS receiver 5, and acquires output data from these sensors. The vehicle-mounted device 1 also stores a map database (DB) 10 that stores road data and feature information related to features located near roads. The vehicle-mounted device 1 estimates the position of the vehicle based on the output data and the map DB 10, and performs control related to vehicle driving assistance, such as automatic driving control, based on the position estimation result. The vehicle-mounted device 1 also estimates the attitude of the LIDAR 2 based on the output of the LIDAR 2, etc. The vehicle-mounted device 1 then performs processing such as correcting each measurement value of the point cloud data output by the LIDAR 2 based on the estimation result. The vehicle-mounted device 1 is an example of an "attitude estimation device" in the present invention.

[0022] 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 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 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 distance to the object in that irradiation direction of the laser light identified based on the above-mentioned light receiving signal, and is supplied to the vehicle-mounted device 1. The lidar 2 is an example of a "measurement device" in the present invention.

[0023] 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, an input unit 14, a control unit 15, and an information output unit 16. These elements are interconnected via a bus line.

[0024] The interface 11 acquires output data from sensors such as the lidar 2, the gyro sensor 3, the acceleration 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 the vehicle's electronic control unit (ECU).

[0025] 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 has a map DB 10 and LIDAR installation information IL. The map DB 10 is a database including, for example, road data, facility data, and feature data around the roads. The road data includes lane network data for route search, road shape data, traffic law data, and the like. The feature data includes information (e.g., position information and type information) on signs such as road signs, road markings such as stop lines, road dividing lines such as white lines, and structures along the roads. The feature data may also include highly accurate point cloud information of features to be used for estimating the vehicle's position. In addition, the map DB may store various data required for position estimation.

[0026] The rider installation information IL is information about the attitude and position of the rider 2 relative to the vehicle at a certain reference time (for example, immediately after the alignment of the rider 2 is adjusted, when there is no attitude deviation). In this embodiment, the attitude of the rider 2, etc. is represented by a roll angle, a pitch angle, and a yaw angle (i.e., Euler angles). When a process for estimating the attitude of the rider 2, which will be described later, is executed, the rider installation information IL may be updated based on the estimation result.

[0027] 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.

[0028] The control unit 15 includes a CPU that executes programs and controls the entire in-vehicle device 1. The control unit 15 estimates the vehicle's position based on the output signals of each sensor supplied from the interface 11 and the map DB 10, and performs control related to vehicle driving assistance, including autonomous driving control, based on the estimated vehicle position. When using the output data of the LIDAR 2, the control unit 15 converts the measurement data output by the LIDAR 2 from a coordinate system based on the LIDAR 2 to a coordinate system based on the vehicle (hereinafter referred to as the "reference coordinate system"), based on the attitude and position included in the installation information of the LIDAR 2 recorded in the LIDAR installation information IL. Furthermore, in this embodiment, the control unit 15 estimates the current (i.e., processing reference time) attitude of the LIDAR 2 relative to the vehicle, calculates the amount of change from the attitude recorded in the LIDAR installation information IL, and corrects the measurement data output by the LIDAR 2 based on the amount of change. As a result, even if a deviation occurs in the attitude of the LIDAR 2, the control unit 15 corrects the measurement data output by the LIDAR 2 so that it is not affected by the deviation. The control unit 15 is an example of a "computer" that executes a program in the present invention.

[0029] Coordinate System Transformation The three-dimensional coordinates indicated by each measurement point of the three-dimensional point cloud data acquired by the lidar 2 are expressed in a coordinate system based on the position and attitude of the lidar 2 (also called the "lidar coordinate system"), and need to be converted into a coordinate system based on the position and attitude of the vehicle (also called the "vehicle coordinate system"). Here, the conversion between the lidar coordinate system and the vehicle coordinate system will be described.

[0030] 3 is a diagram showing the relationship between the vehicle coordinate system and the rider coordinate system expressed in two-dimensional coordinates. Here, the vehicle coordinate system has the center of the vehicle as its origin, and has a coordinate axis "xb" along the vehicle's traveling direction and a coordinate axis "yb" along the vehicle's lateral direction. The rider coordinate system also has a coordinate axis "xL" along the front direction of the rider 2 (see arrow A2) and a coordinate axis "yL" along the rider 2's lateral direction.

[0031] Here, the yaw angle of the rider 2 relative to the vehicle coordinate system is "Lψ", and the position of the rider 2 is [Lx, Ly] TThen, the measurement point at time "k" seen from the vehicle coordinate system [xb(k), yb(k)] T is expressed as the coordinates [xL(k), yL(k)] in the lidar coordinate system using the following equation (1) using the rotation matrix "Cψ". T is converted to

[0032]

number

[0033]

number

[0034] If the roll angle of Rider 2 relative to the vehicle coordinate system is "Lφ", the pitch angle is "Lθ", and the yaw angle is "Lψ", and the position of Rider 2 on the coordinate axis xb is "Lx", the position on the coordinate axis yb is "Ly", and the position on the coordinate axis zb is "Lz", the measurement point at time "k" as seen from the vehicle coordinate system is [xb(k), yb(k), zb(k)] T is expressed as the coordinates [xL(k), yL(k), zL(k)] in the lidar coordinate system by the following equation (3) using the direction cosine matrix “C” represented by the rotation matrices “Cφ”, “Cθ”, and “Cψ” corresponding to roll, pitch, and yaw. T is converted to

[0035]

number

[0036]

number

[0037] [Roll angle estimation] First, we will explain the method for estimating the roll angle Lφ of the LIDAR 2. The vehicle-mounted device 1 regards square (rectangular) road signs having a surface perpendicular to the traveling direction of the road (i.e., facing the road) as target features to be detected by the LIDAR 2 (also referred to as "detection target features Ftag"), and estimates the roll angle Lφ of the LIDAR 2 by calculating the inclination of the road sign based on the measurement point cloud measured by the LIDAR 2.

[0038] Fig. 5 shows an overhead view of the area around the vehicle when the vehicle is traveling near a road sign 22 that is a target feature Ftag to be detected when estimating the roll angle of the LIDAR 2. In Fig. 5, a dashed circle 20 indicates the range in which the LIDAR 2 can detect features (also referred to as the "LIDAR detection range"), and a dashed frame 21 indicates a detection window for detecting the road sign 22 that is the target feature Ftag to be detected.

[0039] In this case, the vehicle-mounted device 1 recognizes that a rectangular road sign 22 having a surface perpendicular to the traveling direction of the road is present within the LIDAR detection range by referring to the map DB 10, and sets a detection window shown in the dashed frame 21 based on feature data related to the road sign 22. In this case, the vehicle-mounted device 1 pre-stores, for example, type information of road signs that are to be detection target features Ftag, and determines whether a road sign of the type indicated by the type information is present within the LIDAR detection range by referring to the feature data in the map DB 10. Note that if the feature data in the map DB 10 includes information on the size and / or shape of features, the vehicle-mounted device 1 may determine the size and / or shape of the detection window based on the information on the size and / or shape of the road sign 22 that is to be the detection target feature Ftag. Then, the vehicle-mounted device 1 extracts, from the measurement point clouds measured by the LIDAR 2, the measurement point clouds that are present within the set detection window (also referred to as "target measurement point cloud Ptag") as the measurement point cloud of the road sign 22.

[0040] 6(A) to 6(C) show the positional relationship between the target measurement point cloud Ptag and the scanning line 23 of the laser light by the LIDAR 2 (i.e., the line connecting the points irradiated with the laser light in time series) when the measurement surface of the road sign 22 is viewed from the front. For ease of explanation, the initial attitude angles Lφ, Lθ, and LΨ of the LIDAR 2 are set to 0 degrees. Here, FIG. 6(A) shows the relationship between the target measurement point cloud Ptag and the scanning line 23 of the LIDAR 2 when there is no deviation in the roll direction of the LIDAR 2, and FIGS. 6(B) and 6(C) show the relationship between the target measurement point cloud Ptag and the scanning line 23 of the LIDAR 2 when there is a deviation in the roll direction ΔLφ. Here, FIG. 6(B) shows the relationship between the target measurement point cloud Ptag and the scanning line 23 of the LIDAR 2 in the yb-zb plane of the vehicle coordinate system, and FIG. 6(C) shows the relationship between the target measurement point cloud Ptag and the scanning line 23 of the LIDAR 2 in the yb'-zb' plane of the reference coordinate system. For ease of explanation, it is assumed here that when there is no posture deviation of the rider 2, the longitudinal direction of the road sign 22 is parallel to the yb axis, and the vehicle is located on a flat road.

[0041] As shown in Fig. 6(A), when there is no deviation in the roll angle of the LIDAR 2, the LIDAR 2 performs scanning parallel to the lateral direction (i.e., the horizontal or longitudinal direction) of the road sign 22. In this case, the number of points in the longitudinal direction of the target measurement point cloud Ptag is the same (6 in Fig. 6(A)) regardless of the position in the lateral direction.

[0042] On the other hand, when there is a deviation in the roll angle of the LIDAR 2, the scanning line is inclined obliquely with respect to the road sign 22. Here, the longitudinal direction of the road sign 22 is not inclined with respect to the vehicle, but the scanning line of the LIDAR 2 is inclined with respect to the vehicle. Therefore, when observed in the vehicle coordinate system, the yb axis and the longitudinal direction of the road sign 22 are parallel, as shown in FIG. 6(B), and the scanning line of the LIDAR 2 is inclined with respect to the yb axis. On the other hand, when observed in the reference coordinate system, the yb' axis and the scanning line of the LIDAR 2 are parallel, as shown in FIG. 6(C), and the longitudinal direction of the road sign 22 is inclined with respect to the yb' axis.

[0043] Next, the processing after extraction of the target measurement point group Ptag will be described. First, the vehicle-mounted device 1 determines the received light intensity for each measurement point of the target measurement point group Ptag using a predetermined threshold, and excludes measurement points corresponding to received light intensities below the predetermined threshold from the target measurement point group Ptag. Then, the vehicle-mounted device 1 extracts a point group constituting the outer edge (also referred to as the "outer edge point group Pout") from the target measurement point group Ptag after the exclusion process based on the received light intensity. For example, the vehicle-mounted device 1 extracts measurement points of the target measurement point group Ptag that have no adjacent measurement points in at least one direction (up, down, left, or right) as the outer edge point group Pout.

[0044] Fig. 7(A) shows the target measurement point cloud Ptag in the lidar coordinate system, and Fig. 7(B) shows the target measurement point cloud Ptag after excluding measurement points corresponding to received light intensities below a predetermined threshold. Fig. 7(C) shows the outer edge point cloud Pout extracted from the target measurement point cloud Ptag shown in Fig. 7(B). As shown in Figs. 7(A) and (B), among the measurement points shown in Fig. 7(A), measurement points that are partially missing due to only partial reflection of the laser light have received light intensities below a predetermined threshold and are therefore excluded from the target measurement point cloud Ptag. As shown in Figs. 7(B) and (C), measurement points that do not have adjacent measurement points in at least one direction (up, down, left, or right) are extracted as the outer edge point cloud Pout.

[0045] Next, the vehicle-mounted device 1 calculates the inclination of the longitudinal direction of the road sign 22 from the outer edge point group Pout. Here, since the outer edge point group Pout forms a quadrangle, the outer edge point group Pout is classified for each side of this quadrangle, and the inclination of an approximate line for each side is calculated from the outer edge point group Pout classified for each side by a regression analysis method such as the least squares method.

[0046] Figure 8(A) shows a diagram in which the outer boundary point group Pout that forms the top side of a quadrangle is extracted and a straight line is drawn using the least squares method. Figure 8(B) shows a diagram in which the outer boundary point group Pout that forms the bottom side of a quadrangle is extracted and a straight line is drawn using the least squares method. Figure 8(C) shows a diagram in which the outer boundary point group Pout that forms the left side of a quadrangle is extracted and a straight line is drawn using the least squares method. Figure 8(D) shows a diagram in which the outer boundary point group Pout that forms the right side of a quadrangle is extracted and a straight line is drawn using the least squares method.

[0047] 8(A) to 8(D), and calculates the inclinations "φ1" to "φ4" of each line relative to the yb' axis. Then, the vehicle-mounted device 1 calculates the average of these inclinations "φ1" to "φ4" as the inclination "φ" of the road sign 22, as shown in the following equation (5).

[0048]

number

[0049] Preferably, when calculating the gradient φ, the vehicle-mounted device 1 may weight each gradient φ1 to φ4 based on the number of outer edge points Pout that form each side of the quadrangle. In this case, if the number of outer edge points Pout that form the top side of the quadrangle is "n1," the number of outer edge points Pout that form the bottom side of the quadrangle is "n2," the number of outer edge points Pout that form the left side of the quadrangle is "n3," and the number of outer edge points Pout that form the right side of the quadrangle is "n4," the vehicle-mounted device 1 calculates the gradient φ by performing a weighted average based on the following formula (6).

[0050]

number

[0051]

number

[0052]

number

[0053]

number

[0054] Preferably, taking into consideration the possibility that the road sign 22, which is the detection target feature Ftag, is tilted relative to the horizontal direction, measurement errors, etc., the vehicle-mounted device 1 performs the above-mentioned calculation of the roll angle Lφ of the LIDAR 2 for many road signs, which are the detection target feature Ftag, and averages these values. This allows the vehicle-mounted device 1 to preferably calculate the likely tilt Lφ of the LIDAR 2 in the roll direction.

[0055] Furthermore, since the roll angle φ0 of the vehicle body can be considered to be 0° on average, by extending the averaging time described above, it becomes unnecessary to acquire the roll angle φ0 of the vehicle body. For example, when N sufficiently large tilts φ are acquired, the vehicle-mounted device 1 may determine the roll angle Lφ of the rider 2 by the following equation (10).

[0056]

number

[0057] FIG. 9 is a flowchart showing the procedure of the roll angle estimation process.

[0058] First, the vehicle-mounted device 1 refers to the map DB 10 and sets a detection window for the detection target feature Ftag around the vehicle (step S101). In this case, the vehicle-mounted device 1 selects a rectangular road sign having a surface perpendicular to the traveling direction of the road as the detection target feature Ftag, and identifies the above-mentioned road sign that exists within a predetermined distance from the current position from the map DB 10. Then, the vehicle-mounted device 1 sets the detection window by acquiring position information etc. related to the identified road sign from the map DB 10.

[0059] Next, the vehicle-mounted device 1 acquires the measurement point cloud of the lidar 2 within the detection window set in step S101 as a point cloud of the detection target feature Ftag (i.e., target measurement point cloud Ptag) (step S102).The vehicle-mounted device 1 then extracts an outer edge point cloud Pout that forms the outer edge of these point clouds from the target measurement point cloud Ptag acquired in step S102 (step S103).

[0060] Then, the vehicle-mounted device 1 calculates the lateral gradient φ of the outer edge point group Pout extracted in step S103 (step S104). In this case, as described with reference to Figures 8(A) to 8(D), the vehicle-mounted device 1 obtains gradients φ1 to φ4 of the straight lines corresponding to each side from the points corresponding to each side of the quadrangle formed by the outer edge point group Pout, and calculates the gradient φ by using one of equations (5) to (8).

[0061] Thereafter, the vehicle-mounted device 1 calculates the roll angle Lφ of the lidar 2 by averaging the tilt φ and / or by subtracting it from the roll angle φ0 of the vehicle body (step S105). In the averaging process described above, the vehicle-mounted device 1 executes the processes of steps S101 to S104 for multiple detection target features Ftag to obtain multiple tilts φ, and calculates the average value of the obtained multiple tilts φ as the tilt Lφ.

[0062] [Pitch angle estimation] Next, we will explain the method for estimating the pitch angle Lθ of the LIDAR 2. The vehicle-mounted device 1 regards a rectangular road sign having a surface perpendicular to the width direction of the road (i.e., a surface oriented horizontally to the road) as the detection target feature Ftag, and estimates the pitch angle of the LIDAR 2 by calculating the inclination of the road sign based on the measurement point cloud output by the LIDAR 2.

[0063] Fig. 10 shows an overhead view of the area around the vehicle when the vehicle is traveling near a road sign 24 that is a target feature Ftag to be detected when estimating the pitch angle of the LIDAR 2. In Fig. 10, a dashed circle 20 indicates the LIDAR detection range in which the LIDAR 2 can detect features, and a dashed frame 21 indicates a detection window set for the road sign 24 that is the target feature Ftag to be detected.

[0064] In this case, the vehicle-mounted device 1 refers to the map DB 10 to recognize that a rectangular road sign 24 having a surface perpendicular to the width direction of the road is present within the LIDAR detection range shown in the dashed circle 20, and sets a detection window shown in the dashed frame 21 based on position information and the like related to the road sign 24. In this case, the vehicle-mounted device 1 stores, for example, type information and the like of road signs that are to be detected as target feature Ftag, and determines whether or not a road sign of the type indicated by the type information is present within the LIDAR detection range by referring to the feature data in the map DB 10. Then, the vehicle-mounted device 1 extracts, from the measurement point clouds output by the LIDAR 2, the measurement point clouds that are present within the set detection window as the target measurement point cloud Ptag.

[0065] Next, the vehicle-mounted device 1 estimates the pitch angle of the lidar 2 by performing the same process as the roll angle estimation method on the extracted target measurement point group Ptag.

[0066] Specifically, the vehicle-mounted device 1 first evaluates the received light intensity for each measurement point of the target measurement point group Ptag using a predetermined threshold, and excludes measurement points corresponding to received light intensities below the predetermined threshold from the target measurement point group Ptag. The vehicle-mounted device 1 then extracts an outer edge point group Pout that constitutes the outer edge from the target measurement point group Ptag after the exclusion process based on the received light intensity. Next, the vehicle-mounted device 1 calculates the slopes "θ1" to "θ4" of the approximate lines of each side of the quadrangle constituted by the outer edge point group Pout using a regression analysis method such as the least squares method. The vehicle-mounted device 1 then calculates the slope "θ" of the road sign 22 by performing an averaging process based on the slopes θ1 to θ4. Note that the vehicle-mounted device 1 may also calculate the slope θ by a weighted average that takes into account the number of points n1 to n4 that constitute each side of the quadrangle and at least one of the vertical interval IV and the horizontal interval IH, as in equations (6) to (8).

[0067] Furthermore, the vehicle-mounted device 1 calculates the pitch angle "θ0" of the vehicle body based on the outputs of the gyro sensor 3 and the acceleration sensor 4, and calculates the difference between this and the tilt θ to calculate the pitch angle Lθ of the rider 2. In this case, the vehicle-mounted device 1 calculates the tilt Lθ in the pitch direction of the rider 2 based on the following equation (11).

[0068]

number

[0069]

number

[0070] FIG. 11 is a flowchart showing the procedure of the pitch angle estimation process.

[0071] First, the vehicle-mounted device 1 refers to the map DB 10 and sets a detection window for the detection target feature Ftag around the vehicle (step S201). In this case, the vehicle-mounted device 1 selects a rectangular road sign having a surface perpendicular to the width direction of the road as the detection target feature Ftag, and identifies the above-mentioned road sign that exists within a predetermined distance from the current position from the map DB 10. Then, the vehicle-mounted device 1 sets the detection window by acquiring position information and the like related to the identified road sign from the map DB 10.

[0072] Next, the vehicle-mounted device 1 acquires the measurement point cloud of the lidar 2 within the detection window set in step S201 as a point cloud of the detection target feature Ftag (i.e., target measurement point cloud Ptag) (step S202).Then, the vehicle-mounted device 1 extracts an outer edge point cloud Pout that forms the outer edge of the point cloud from the point cloud of the detection target feature Ftag acquired in step S202 (step S203).

[0073] Then, the vehicle-mounted device 1 calculates the tilt θ in the longitudinal direction of the outer edge point group Pout extracted in step S203 (step S204). After that, the vehicle-mounted device 1 calculates the tilt Lθ in the pitch direction of the rider 2 by averaging the tilt θ and / or performing a difference process with the pitch angle θ0 of the vehicle body (step S205). In the averaging process described above, the vehicle-mounted device 1 executes the processes of steps S201 to S204 on multiple target measurement point groups Ptag to calculate multiple tilts θ, and calculates the average value of these as the tilt Lθ.

[0074] [Yaw angle estimation] Next, we will explain a method for estimating the yaw angle Lψ of the LIDAR 2. The onboard device 1 regards a lane marking such as a white line as a detection target feature Ftag, and estimates the yaw angle Lψ of the LIDAR 2 by calculating the direction of the center line of the lane marking based on the measurement point cloud measured by the LIDAR 2.

[0075] FIG. 12 shows an overhead view of the vehicle surroundings when the vehicle is traveling along a lane marking that serves as a target feature Ftag for estimating the yaw angle of the LIDAR 2. In this example, a continuous lane marking 30 exists on the left side of the vehicle, and a broken lane marking 31 exists intermittently on the right side of the vehicle. Arrow 25 indicates the reference direction of the yaw direction when no deviation occurs in the LIDAR 2. This direction is obtained by rotating the coordinate axis "xL" of the LIDAR 2 by the yaw angle LΨ of the LIDAR 2, and coincides with the coordinate axis "xb" along the vehicle's traveling direction. In other words, the yaw angle LΨ of the LIDAR 2 indicates the yaw angle (also referred to as the "yaw direction reference angle") that is considered to be the vehicle's traveling direction if no deviation occurs. Meanwhile, arrow 26 indicates the yaw direction reference angle when a deviation ΔLΨ occurs in the LIDAR 2 in the yaw direction. If the yaw direction reference angle is not corrected, it will no longer match the vehicle's traveling direction. The yaw direction reference angle is pre-stored in, for example, the storage unit 12.

[0076] In this case, for example, the map DB 10 contains lane marking information indicating the discrete coordinate positions of the continuous line 30 and the discrete coordinate positions of the dashed line 31. The vehicle-mounted device 1 then refers to this lane marking information, extracts the closest coordinate position from a position a predetermined distance (for example, 5 m) away from the vehicle in each of the directions of the left front, left rear, right front, and right rear of the vehicle, and sets a rectangular area centered on the extracted coordinate position as a detection window indicated by dashed frame 21A to 21D.

[0077] Next, the vehicle-mounted device 1 extracts, as a target measurement point group Ptag, a point group that is on the road surface and has a reflection intensity equal to or greater than a predetermined threshold within the detection window from the measurement point group measured by the lidar 2. Then, the vehicle-mounted device 1 obtains the center point of the target measurement point group Ptag for each scanning line, and calculates a straight line (see arrows 27A to 27D) that passes through these center points for each detection window as the center line of the lane marking.

[0078] 13(A) is a diagram showing the center point of the detection window of dashed frame 21A by a circle, and FIG. 13(B) is a diagram showing the center point of the detection window of dashed frame 21B by a circle. Also, in FIGS. 13(A) and (B), scanning lines 28 of the laser light from LIDAR 2 are clearly shown. Note that, because the laser light from LIDAR 2 that is incident on the lane markings is emitted in a downward diagonal direction, the spacing between the scanning lines becomes shorter the closer they are to the vehicle.

[0079] 13(A) and 13(B), the vehicle-mounted device 1 calculates a center point for each scanning line 28. The center point may be a position obtained by averaging the coordinate positions indicated by the measurement points for each scanning line 28, or may be a midpoint between the measurement points at the left and right ends of each scanning line 28, or may be a measurement point located midway along each scanning line 28. Then, the vehicle-mounted device 1 calculates the center lines of the lane markings (see arrows 27A and 27B) for each detection window from the calculated center points using a regression analysis method such as the least squares method.

[0080] The vehicle-mounted device 1 then calculates the angles between the center lines of the lane markings calculated for each detection window and the yaw direction reference angle (see arrow 26 in FIG. 12) as inclinations "ψ1" to "ψ4." FIGS. 14(A) to 14(D) are diagrams showing the inclinations ψ1 to ψ4, respectively. As shown in FIGS. 14(A) to 14(D), the inclinations ψ1 to ψ4 correspond to the angles between arrows 27A to 27D indicating the center lines of the lane markings and arrow 26 indicating the yaw direction reference angle.

[0081] Then, the vehicle-mounted device 1 calculates the inclination ψ by averaging the inclinations ψ1 to ψ4 as shown in the following equation (13).

[0082]

number

[0083]

number

[0084] Next, a method for calculating the yaw angle Lψ of the rider 2 from the inclination ψ will be described. Because lane markings such as white lines are drawn along the road, the yaw angle of the vehicle body can be considered to be approximately constant (i.e., parallel to the roadway). Therefore, unlike methods for estimating roll angles and pitch angles, the vehicle-mounted device 1 calculates the inclination ψ as a deviation ΔLψ of the yaw angle of the rider 2 (i.e., "ΔLψ = ψ"). However, during normal driving, lane changes and vehicle body sway within the lane occur. Therefore, it is advisable for the vehicle-mounted device 1 to execute a process for calculating the inclination of the center line for many lane markings and then average the results. This allows the vehicle-mounted device 1 to suitably calculate a likely deviation ΔLψ of the yaw angle of the rider 2. For example, when N inclinations ψ are acquired, the yaw angle Lψ of the rider 2 is expressed by the following equation (15).

[0085]

number

[0086] FIG. 15 is a flowchart showing the procedure of the yaw angle estimation process.

[0087] First, the vehicle-mounted device 1 sets one or more detection windows for the lane markings with reference to the map DB 10 (step S301). Then, the vehicle-mounted device 1 acquires the measurement point cloud of the lidar 2 within the detection window set in step S301 as the point cloud of the lane markings (i.e., the target measurement point cloud Ptag) (step S302).

[0088] The vehicle-mounted device 1 then calculates the center line of the lane markings within each of the set detection windows (step S303). In this case, the vehicle-mounted device 1 obtains the center point of the target measurement point group Ptag for each scan line of the lidar 2, and calculates the center line of the lane markings for each detection window from the center point based on the least squares method or the like. The vehicle-mounted device 1 then calculates the angle ψ between the yaw angle reference angle stored in advance in the storage unit 12 or the like and the center line of the lane markings (step S304). In this case, if two or more detection windows are set in step S301, the vehicle-mounted device 1 obtains the angle between the yaw angle reference angle and the center line of the lane markings for each detection window, and calculates the angle ψ by averaging these angles or averaging them weighted by the number of center points.

[0089] Then, the vehicle-mounted device 1 averages the angles ψ calculated by executing steps S301 to S304 for different lane lines, and calculates the yaw angle Lψ of the rider 2 based on equation (15) (step S305).

[0090] A specific example of the process based on the estimated values ​​of the roll angle, pitch angle, and yaw angle explained above will now be described. Fig. 16 is a flowchart showing a specific example of the process based on the estimated values ​​of the roll angle, pitch angle, and yaw angle.

[0091] First, the vehicle-mounted device 1 executes the processing of the flowchart in Fig. 9, Fig. 11, or Fig. 15 to determine whether or not it has calculated any one of the roll angle Lφ, pitch angle Lθ, or yaw angle Lψ of the LIDAR 2 (step S401). Then, if the vehicle-mounted device 1 has calculated any one of the roll angle Lφ, pitch angle Lθ, or yaw angle Lψ of the LIDAR 2 (step S401; Yes), it determines whether or not the calculated angle has changed by a predetermined angle or more from the angle recorded in the LIDAR installation information IL (step S402). The above-mentioned threshold is a threshold for determining whether or not the measurement data of the LIDAR 2 can continue to be used by performing a correction process on the measurement data of the LIDAR 2 in step S404, which will be described later, and is set in advance based on, for example, experiments.

[0092] If the calculated angle has changed by a predetermined angle or more from the angle recorded in the LIDAR installation information IL (step S402; Yes), the on-board device 1 stops using the output data of the target LIDAR 2 (i.e., use for obstacle detection, vehicle position estimation, etc.) and outputs a warning to the effect that the target LIDAR 2 needs to be realigned via the information output unit 16 (step S403). This reliably prevents a decrease in safety, etc., caused by using measurement data from a LIDAR 2 whose attitude or position has shifted significantly due to an accident, etc.

[0093] On the other hand, if the calculated angle has not changed by more than a predetermined angle from the angle recorded in the LIDAR installation information IL (step S402; No), the vehicle-mounted device 1 corrects each measurement value of the point cloud data output by the LIDAR 2 based on the amount of change in the calculated angle from the angle recorded in the LIDAR installation information IL (step S404). In this case, the vehicle-mounted device 1 stores, for example, a map or the like indicating the amount of correction for the measurement value with respect to the amount of change, and corrects the measurement value by referring to the map or the like. Alternatively, the measurement value may be corrected using a value of a predetermined percentage of the amount of change as the amount of correction for the measurement value.

[0094] As described above, the vehicle-mounted device 1 in this embodiment acquires the target measurement point cloud Ptag obtained by measuring road signs having surfaces perpendicular to the traveling direction of the vehicle's roadway or perpendicular to the width direction of the roadway using the lidar 2 attached to the vehicle. The vehicle-mounted device 1 then estimates the mounting attitude of the lidar 2 on the vehicle based on the tilt of the longitudinal direction of the road sign relative to a coordinate system based on the installation information of the lidar 2, which is calculated from the target measurement point cloud Ptag. This allows the vehicle-mounted device 1 to accurately estimate the mounting attitude of the lidar 2 on the vehicle in the roll direction and pitch direction.

[0095] As described above, the vehicle-mounted device 1 in this embodiment acquires a cloud of measurement points obtained by measuring the lane markings using the LIDAR 2 attached to the vehicle, and calculates the center line of the lane markings along the vehicle's traveling direction based on the cloud of measurement points. The vehicle-mounted device 1 then estimates the mounting attitude of the LIDAR 2 on the vehicle based on the direction of the center line of the lane markings relative to the yaw direction reference angle used as a reference by the LIDAR 2. This allows the vehicle-mounted device 1 to suitably estimate the mounting attitude of the LIDAR 2 on the vehicle in the yaw direction.

[0096] [Variations] Modifications suitable for the embodiment will be described below. The following modifications may be applied to the embodiment in combination.

[0097] (Variation 1) In the description of step S303 in Figures 13 and 15, the vehicle-mounted device 1 calculates the center line of the lane marking by calculating the center point of the lane marking in the width direction within the detection window. Alternatively, the vehicle-mounted device 1 may extract the right or left endpoint of the scanning line within the detection window and calculate a straight line passing through the right or left end of the lane marking. This also allows the vehicle-mounted device 1 to preferably identify a line parallel to the roadway and calculate the angle ψ.

[0098] (Variation 2) In the yaw angle estimation process described with reference to Figures 12 to 15, the vehicle-mounted device 1 considered the lane markings as the detection target features Ftag. Alternatively, or in addition, the vehicle-mounted device 1 may consider a curb or the like as the detection target feature Ftag. In this way, the vehicle-mounted device 1 may perform the yaw angle estimation process described with reference to Figures 12 to 15 by considering, as the detection target feature Ftag, any feature whose measurement surface is parallel to the road surface and is formed along the road surface, in addition to the lane markings.

[0099] (Variation 3) In step S404 of FIG. 16, instead of correcting each measurement value of the point cloud data output by the LIDAR 2, the vehicle-mounted device 1 may convert each measurement value of the point cloud data output by the LIDAR 2 into the vehicle coordinate system based on the roll angle, pitch angle, and yaw angle calculated by processing the flowchart of FIG. 9, FIG. 11, or FIG. 15.

[0100] In this case, the vehicle-mounted device 1 may use the calculated roll angle Lφ, pitch angle Lθ, and yaw angle Lψ to convert each measurement value of the point cloud data output by the lidar 2 from the lidar coordinate system to the vehicle body coordinate system based on equation (4), and perform vehicle position estimation, automatic driving control, etc. based on the converted data.

[0101] In another example, if each rider 2 is equipped with an adjustment mechanism such as an actuator for correcting the attitude of each rider 2, the vehicle-mounted device 1 may control the driving of the adjustment mechanism to correct the attitude of the rider 2 by an amount that is a deviation from the angle recorded in the rider installation information IL, instead of processing step S404 in Figure 16.

[0102] (Variation 4) The configuration of the vehicle system shown in Fig. 1 is one example, and the configuration of a vehicle system to which the present invention can be applied is not limited to the configuration shown in Fig. 1. For example, instead of having an onboard device 1, the vehicle system may have an electronic control device of the vehicle that executes the processes shown in Figs. 9, 11, 15, 16, etc. In this case, the LIDAR installation information IL is stored in, for example, a storage unit in the vehicle, and the electronic control device of the vehicle is configured to be able to receive output data from various sensors such as the LIDAR 2.

[0103] (Variation 5) The vehicle system may include a plurality of LIDARs 2. In this case, the vehicle-mounted device 1 estimates the roll angle, pitch angle, and yaw angle of each LIDAR 2 by executing the processes of the flowcharts in Figures 9, 11, and 15 for each LIDAR 2. [Explanation of symbols]

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

Claims

1. an acquisition means for acquiring, from the measurement point cloud output by a measurement device attached to the mobile body, a target measurement point cloud that has a measurement surface that is parallel to the road surface of the lane on which the mobile body is traveling and that measures an area in which features formed along the lane are estimated to exist; a calculation means for calculating a line on the measurement surface along the traveling direction of the moving object based on a plurality of points extracted from the target measurement point cloud for each scanning line of the measurement device; an estimation means for estimating the mounting orientation of the measurement device to the moving object based on the angle between the direction of the line and the direction of the lane, regarding the direction along the lane as the traveling direction of the moving object; A posture estimation device having:

2. The posture estimation device according to claim 1 , wherein the acquisition means recognizes the range based on map information indicating the positions of the features.

3. 3. The attitude estimation device according to claim 1, wherein the estimation means estimates the attitude of the measurement device in the yaw direction based on an angle between the line and a reference line in the yaw direction of the measurement device.

4. 4. The attitude estimation device according to claim 3, wherein the estimation means estimates the attitude of the measurement device in the yaw direction by averaging angles between the reference line and the multiple lines calculated by the calculation means based on the different target measurement point clouds.

5. 4. The attitude estimation device according to claim 3, wherein the estimation means estimates the attitude of the measurement device in the yaw direction by averaging angles between the reference line and the multiple lines calculated by the calculation means based on different target measurement point clouds, based on the number of points used in calculating each of the lines.

6. 6. The posture estimation device according to claim 1, wherein the feature is a lane marking or a curb.

7. 7. The posture estimation apparatus according to claim 1, wherein the measurement device is an optical scanning device that scans light.

8. The posture estimation device according to any one of claims 1 to 7, wherein the reference direction is a direction that is regarded as the traveling direction of the moving body in the coordinate system of the measurement device when there is no deviation in the mounting posture, and when there is a deviation in the mounting posture, indicates a direction different from the traveling direction of the moving body depending on the deviation.

9. 1. A computer-implemented method for pose estimation, comprising: an acquisition step of acquiring, from the measurement point cloud output by a measurement device attached to the mobile body, a target measurement point cloud that has a measurement surface parallel to the road surface of the lane on which the mobile body is traveling and that measures an area in which features formed along the lane are estimated to exist; a calculation step of calculating a line on the measurement surface along the traveling direction of the moving object based on a plurality of points extracted from the target measurement point cloud for each scanning line of the measurement device; an estimation step of estimating the mounting orientation of the measurement device to the moving object based on the angle between the direction of the line and the direction of the lane, regarding the direction along the lane as the traveling direction of the moving object; A posture estimation method having

10. A computer-executable program, From the measurement point cloud output by the measurement device attached to the mobile body, a target measurement point cloud is obtained that has a measurement surface that is parallel to the road surface of the lane on which the mobile body is traveling and that measures an area in which features formed along the lane are estimated to exist; calculating a line on the measurement surface along the traveling direction of the moving object based on a plurality of points extracted from the target measurement point cloud for each scanning line of the measurement device; A program that causes the computer to sequentially execute processes that consider the direction along the lane as the traveling direction of the moving body, and estimate the mounting orientation of the measuring device to the moving body based on the angle between the direction of the line and the direction that the measuring device uses as its reference.

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

Images