Estimation device, control method, program and storage medium

Abstract

To provide an estimation device in which an attitude of a measurement unit that measures a distance to an object, for a mobile object can be optimally estimated.SOLUTION: An on-vehicle machine 1 estimates at least an attitude of a lidar 2 that measures a distance to an object for a vehicle, and performs a process to estimate the attitude of the lidar 2 for the vehicle or the like on the basis of a detection result of an acceleration sensor 5 for a lidar which is provided in the lidar 2 when the vehicle travels while accelerating or decelerating.SELECTED DRAWING: Figure 12

Description

Technical Field

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

Background Art

[0002] Conventionally, techniques for estimating the position of a host vehicle based on measurement data of a measurement unit such as a radar or a camera have been known. For example, Patent Document 1 discloses a technique for estimating the self-position by collating the output of a measurement sensor with the position information of a feature registered in advance on a map. Further, Patent Document 2 discloses a technique for estimating the position of a host vehicle using a Kalman filter.

Prior Art Documents

Patent Documents

[0003]

Patent Document 1

Patent Document 2

Summary of the Invention

Problems to be Solved by the Invention

[0004] Data obtained from a measurement unit such as a radar or a camera are values in a coordinate system based on the measurement unit, and are data that depend on the attitude of the measurement unit with respect to the vehicle. Therefore, it is necessary to convert them into values in a coordinate system based on the vehicle. Accordingly, when a deviation occurs in the attitude of the measurement unit, it is necessary to accurately detect the deviation and reflect it in the data of the measurement unit.

[0005] The present invention has been made to solve the above-described problems, and a main object thereof is to provide an estimation device that can suitably estimate the attitude of a measurement unit for measuring the distance to an object with respect to a moving body.

Means for Solving the Problems

[0006] The invention described in the claims is An estimating device that estimates the attitude of a measuring unit for measuring the distance to an object with respect to a moving body, having an estimating unit that estimates the attitude of the measuring unit in the roll direction and the pitch direction based on acceleration data output by an acceleration detection unit provided in the measuring unit when the moving body is traveling or stopped at a predetermined speed. Further, the invention according to the claim is, An estimating device that estimates the attitude of a measuring unit for measuring the distance to an object with respect to a moving body, an estimating unit that estimates the attitude of the measuring unit with respect to the moving body based on a detection result of an acceleration detection unit provided in the measuring unit when the moving body is accelerating or decelerating while traveling, having, wherein the estimating unit estimates the position of the measuring unit in the longitudinal direction of the moving body based on a road point where the gradient changes or a distance between the road point and the moving body when a bump on the road surface is measured by the measuring unit. Further, the invention according to the claim is, An estimating device that estimates the attitude of a measuring unit for measuring the distance to an object with respect to a moving body, an estimating unit that estimates the attitude of the measuring unit with respect to the moving body based on a detection result of an acceleration detection unit provided in the measuring unit when the moving body is accelerating or decelerating while traveling, having, wherein the estimating unit estimates the position of the measuring unit in the lateral direction of the moving body based on the lateral acceleration data of the moving body output by the acceleration detection unit during turning of the moving body, the lateral acceleration data of the moving body output by an acceleration sensor mounted on the moving body, and the yaw rate of the moving body output by a gyro sensor mounted on the moving body.

[0007] Further, the invention according to the claim is, A control method executed by an estimating device that estimates the attitude of a measuring unit for measuring the distance to an object with respect to a moving body, Based on the acceleration data output by the acceleration detection unit provided in the measurement unit when the moving body is traveling or stopped at a predetermined speed, there is an estimation step of estimating the attitude of the measurement unit in the roll direction and the pitch direction. Further, the invention described in the claims is A control method executed by an estimation device that estimates the attitude of a measurement unit for measuring the distance to an object with respect to a moving body, Based on the detection result of the acceleration detection unit provided in the measurement unit when the moving body is accelerating or decelerating and traveling, there is an estimation step of estimating the attitude of the measurement unit with respect to the moving body. In the estimation step, based on the distance between the road point where the gradient changes or the bump on the road surface is measured by the measurement unit and the moving body, the position of the measurement unit in the front-rear direction of the moving body is estimated. Further, the invention described in the claims is A control method executed by an estimation device that estimates the attitude of a measurement unit for measuring the distance to an object with respect to a moving body, Based on the detection result of the acceleration detection unit provided in the measurement unit when the moving body is accelerating or decelerating and traveling, there is an estimation step of estimating the attitude of the measurement unit with respect to the moving body. In the estimation step, based on the acceleration data in the left-right direction of the moving body output by the acceleration detection unit during turning of the moving body, the acceleration data in the left-right direction of the moving body output by the acceleration sensor mounted on the moving body, and the yaw rate of the moving body output by the gyro sensor mounted on the moving body, the position of the measurement unit in the left-right direction of the moving body is estimated.

[0008] Further, the invention described in the claims is A program executed by a computer that estimates the attitude of a measurement unit for measuring the distance to an object with respect to a moving body, An estimation unit that estimates the attitude of the measurement unit in the roll direction and the pitch direction based on the acceleration data output by the acceleration detection unit provided in the measurement unit when the moving body is traveling or stopped at a predetermined speed to cause the computer to function as such. Further, the invention according to the claim is a program executed by a computer that estimates the attitude of a measuring unit for measuring the distance to an object with respect to a moving body, causing the computer to function as an estimation unit that estimates the attitude of the measuring unit with respect to the moving body based on a detection result of an acceleration detection unit provided in the measuring unit when the moving body is accelerating or decelerating and traveling, The estimation unit estimates the position of the measuring unit in the front-rear direction of the moving body based on the distance between the road point where the gradient changes or the bump on the road surface is measured by the measuring unit and the moving body. Further, the invention according to the claim is a program executed by a computer that estimates the attitude of a measuring unit for measuring the distance to an object with respect to a moving body, an estimation unit that estimates the attitude of the measuring unit with respect to the moving body based on a detection result of an acceleration detection unit provided in the measuring unit when the moving body is accelerating or decelerating and traveling causing the computer to function as The estimation unit estimates the position of the measuring unit in the left-right direction of the moving body based on the acceleration data in the left-right direction of the moving body output by the acceleration detection unit during turning of the moving body, the acceleration data in the left-right direction of the moving body output by an acceleration sensor mounted on the moving body, and the yaw rate of the moving body output by a gyro sensor mounted on the moving body.

Brief Description of the Drawings

[0009]

Figure 1

Figure 2

Figure 3

Figure 4

Figure 5

Figure 6

Figure 7

Figure 8

Figure 9

Figure 10

Figure 11

Figure 12

Mode for Carrying Out the Invention

[0010] According to a preferred embodiment of the present invention, there is provided an estimation device that estimates the attitude of a measuring unit for measuring the distance to an object with respect to a moving body, the estimation device having an estimation unit that estimates the attitude of the measuring unit with respect to the moving body based on the detection result of an acceleration detection unit provided in the measuring unit when the moving body is accelerating or decelerating while traveling. In this manner, the estimation device can preferably estimate the attitude of the measuring unit in the yaw direction with respect to the vehicle. Note that the mode of "accelerating or decelerating while traveling" includes both the mode of accelerating while traveling and the mode of decelerating while traveling.

[0011] In one aspect of the above-mentioned estimation device, the estimation unit estimates the attitudes of the measurement unit in the roll direction and the pitch direction based on the acceleration data output by the acceleration detection unit when the moving body is traveling or stopped at a predetermined speed, and based on the acceleration data output by the acceleration detection unit when the moving body is accelerating or decelerating and traveling, as well as the estimated attitudes in the roll direction and the pitch direction, estimates the attitude of the measurement unit in the yaw direction. According to this aspect, the estimation device can preferably estimate the attitude of the measurement unit in the roll direction and the pitch direction with respect to the vehicle when the moving body is traveling or stopped at a predetermined speed, and can preferably estimate the attitude of the measurement unit in the yaw direction with respect to the vehicle when the moving body is accelerating or decelerating and traveling.

[0012] In another aspect of the above-mentioned estimation device, the estimation unit estimates the amount of change in the attitude based on the estimated attitude of the measurement unit and the attitude of the measurement unit stored in the storage unit. Thereby, the estimation device can preferably estimate the amount of change in the current attitude with respect to the standard attitude stored in the storage unit.

[0013] In another aspect of the above-mentioned estimation device, the estimation unit estimates the position of the measurement unit in the height direction based on the measurement data of the measurement unit indicating the position of the road surface in the height direction. According to this aspect, the estimation device can preferably estimate the position of the measurement unit in the height direction.

[0014] In another aspect of the above-mentioned estimation device, the estimation unit estimates the position of the measurement unit in the front-rear direction of the moving body based on the distance between the road point and the moving body when the road point where the gradient changes is measured by the measurement unit. According to this aspect, the estimation device can preferably estimate the position of the measurement unit in the front-rear direction of the moving body.

[0015] In another aspect of the above-described estimation device, the estimation unit calculates the distance based on the time difference between the change in the data output by the inclination detection unit that detects the inclination of the moving body in the pitch direction and the change in the measurement data output by the measurement unit. According to this aspect, the estimation device can suitably calculate the distance between the road point when the road point where the gradient changes is measured by the measurement unit and the moving body, and can use the position of the measurement unit in the front-rear direction of the moving body for estimation.

[0016] In another aspect of the above-described estimation device, during turning of the moving body, the estimation unit estimates the position of the measurement unit in the left-right direction of the moving body based on the acceleration data in the left-right direction of the moving body output by the acceleration detection unit, the acceleration data in the left-right direction of the moving body output by the acceleration sensor mounted on the moving body, and the yaw rate of the moving body output by the gyro sensor mounted on the moving body. According to this aspect, the estimation device can suitably estimate the position of the measurement unit in the left-right direction of the moving body.

[0017] In another aspect of the above-described estimation device, the estimation unit estimates the amount of change in the position based on the estimated position of the measurement unit and the position of the measurement unit stored in the storage unit. Thereby, the estimation device can suitably estimate the amount of change in the current position with respect to the standard position of the measurement unit stored in the storage unit.

[0018] In another aspect of the above-described estimation device, it further includes a correction unit that corrects the measurement data output by the measurement unit based on the amount of change. According to this aspect, the estimation device can correct the measurement data of the measurement unit so that the influence of the deviation does not occur even when the posture or position of the measurement unit deviates.

[0019] In another aspect of the above-described estimation device, when the amount of change is equal to or greater than a predetermined amount, it further includes a stop control unit that stops the process based on the measurement data output by the measurement unit. According to this aspect, the estimation device can surely suppress the decrease in the accuracy of various processes using the measurement data of the measurement unit with a large deviation in posture or position.

[0020] According to another preferred embodiment of the present invention, there is provided a control method executed by an estimation device that estimates the attitude of a measurement unit for measuring the distance to an object with respect to a moving body. The method includes an estimation step of estimating the attitude of the measurement unit with respect to the moving body based on the detection result of an acceleration detection unit provided in the measurement unit when the moving body is accelerating or decelerating while traveling. By using this control method, the estimation device can preferably estimate the attitude of the measurement unit in the yaw direction with respect to the vehicle.

[0021] According to another preferred embodiment of the present invention, there is provided a program executed by a computer that estimates the attitude of a measurement unit for measuring the distance to an object with respect to a moving body. The program causes the computer to function as an estimation unit that estimates the attitude of the measurement unit with respect to the moving body based on the detection result of an acceleration detection unit provided in the measurement unit when the moving body is accelerating or decelerating while traveling. By executing this program, the computer can preferably estimate the attitude of the measurement unit in the yaw direction with respect to the vehicle. Preferably, the above program is stored in a storage medium.

Example

[0022] Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings.

[0023] [Schematic Configuration] FIG. 1 is a schematic configuration diagram of a driving support system according to this embodiment. The driving support system shown in FIG. 1 includes an in-vehicle device 1 mounted on a vehicle and performing control related to driving support of the vehicle, a lidar (Light Detection and Ranging, or Laser Illuminated Detection And Ranging) 2, a gyro sensor 3, a vehicle body acceleration sensor 4, and a lidar acceleration sensor 5.

[0024] The in-vehicle device 1 is electrically connected to the lidar 2, the gyro sensor 3, the vehicle body acceleration sensor 4, and the lidar acceleration sensor 5, and acquires the output data thereof. Further, it stores a map database (DB: DataBase) 10 that stores road data and feature information regarding features provided near the road. Then, based on the above-described output data and the map DB 10, the in-vehicle device 1 estimates the position of the vehicle (also referred to as the "own vehicle position"), and performs control related to driving support of the vehicle such as automatic driving control based on the estimation result of the own vehicle position. Further, the in-vehicle device 1 estimates the attitude and position of the lidar 2 based on the outputs of the lidar 2, the gyro sensor 3, the vehicle body acceleration sensor 4, and the lidar acceleration sensor 5. Then, based on this estimation result, the in-vehicle device 1 performs processing such as correcting each measurement value of the point cloud data output by the lidar 2. The in-vehicle device 1 is an example of the "estimation device" in the present invention.

[0025] The lidar 2 discretely measures the distance to an object existing in the external world by emitting a pulsed laser with respect to 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 includes an irradiation unit that irradiates laser light while changing the irradiation direction, a light receiving unit that receives the reflected light (scattered light) of the irradiated laser light, and an output unit that outputs scan data based on the light reception 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 the irradiation direction of the laser light specified based on the above-described light reception signal, and is supplied to the in-vehicle device 1. In the present embodiment, as an example, the lidar 2 is provided at the front part and the rear part of the vehicle, respectively. The lidar 2 is an example of the "measurement unit" in the present invention.

[0026] The gyro sensor 3 is provided in the vehicle and supplies an output signal corresponding to the yaw rate of the vehicle body to the in-vehicle unit 1. The vehicle body acceleration sensor 4 is a three-axis acceleration sensor provided in the vehicle, and supplies a detection signal corresponding to three-axis acceleration data corresponding to the traveling direction, side direction, and height direction of the vehicle body to the in-vehicle unit 1. The gyro sensor 3 and the vehicle body acceleration sensor 4 are an example of the "tilt detection unit" in the present invention. The rider acceleration sensor 5 is a three-axis acceleration sensor provided in each rider 2, and supplies a detection signal corresponding to the three-axis acceleration data of the installed rider 2 to the in-vehicle unit 1 respectively. The rider acceleration sensor 5 is an example of the "acceleration detection unit" in the present invention.

[0027] Figure 2 is a block diagram showing the functional configuration of the in-vehicle unit 2. The in-vehicle unit 2 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.

[0028] The interface 11 acquires output data from sensors such as the rider 2, the gyro sensor 3, the vehicle body acceleration sensor 4, and the rider acceleration sensor 5, and supplies it to the control unit 15. Further, the interface 11 supplies a signal related to the travel control of the vehicle generated by the control unit 15 to the electronic control unit (ECU) of the vehicle.

[0029] The storage unit 12 stores the programs executed by the control unit 15 and the information necessary for the control unit 15 to execute predetermined processes. In this embodiment, the storage unit 12 has a map DB 10 and rider installation information IL. The rider installation information IL is information regarding the relative three-dimensional positions and postures of the respective riders 2 at a certain reference time (for example, when there is no posture / position deviation such as immediately after the alignment adjustment of rider 2). In this embodiment, the posture of the rider 2 or the like is represented by a roll angle, a pitch angle, and a yaw angle (i.e., Euler angles). The rider installation information IL may be information regarding the position and posture measured at the above-described reference time, or may be information regarding the position and posture of the rider 2 estimated by the in-vehicle device 1 by the estimation process of the position and posture of the rider 2 described later.

[0030] The input unit 14 includes buttons, a touch panel, a remote controller, a voice input device, etc. that are operated by the user, and accepts inputs such as specifying a destination for route search and specifying on and off of automatic driving. The information output unit 16 is, for example, a display, a speaker, etc. that outputs based on the control of the control unit 15.

[0031] The control unit 15 includes a CPU that executes programs and the like, and controls the entire in-vehicle device 1. Based on the output signals of the respective sensors supplied from the interface 11 and the map DB 10, the control unit 15 estimates the position of the host vehicle, and performs control related to driving support of the vehicle including automatic driving control based on the estimation result of the host vehicle position. At this time, when using the output data of the lidar 2, the control unit 15 converts the measurement data output by the lidar 2 from the coordinate system based on the lidar 2 to the coordinate system based on the vehicle, with reference to the attitude and position of the lidar 2 recorded in the lidar installation information IL. Further, in the present embodiment, the control unit 15 estimates the current (i.e., at the time of the processing reference) position and attitude of the lidar 2 with respect to the vehicle, calculates the amount of change with respect to the position and attitude recorded in the lidar installation information IL, and corrects the measurement data output by the lidar 2 based on the amount of change. Thereby, even when the position or attitude of the lidar 2 is displaced, the control unit 15 corrects the measurement data output by the lidar 2 so as not to be affected by the displacement. The control unit 15 is an example of the "estimation unit", "correction unit", "stop control unit" in the present invention, and the "computer" that executes the program.

[0032] [Estimation of Lidar Position and Attitude] Next, a method for estimating the position and attitude of the lidar 2 will be described. The in-vehicle device 1 executes the following processing for each lidar 2.

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

[0034] FIG. 3 is a diagram showing the relationship between the vehicle coordinate system and the lidar coordinate system represented by two-dimensional coordinates. Here, the vehicle coordinate system has the center of the vehicle as the origin, and the coordinate axis "x b " along the traveling direction of the vehicle and the coordinate axis "yb has "". Also, the lidar coordinate system has a coordinate axis "x" along the front direction of lidar 2 (see arrow A2). L and a coordinate axis "y" along the side direction of lidar 2. L has.

[0035] Here, the yaw angle of lidar 2 with respect to the vehicle coordinate system is "L", and the position of lidar 2 is [L ψ0 , L x0 , L y0 . T When this is the case, the measurement point [x b (k), y b (k)] at time "k" as seen from the vehicle coordinate system is converted to the coordinates [x T (k), y ψ0 (k)] of the lidar coordinate system by the following equation (1) using the rotation matrix "C". L (k), y L (k)] T is converted to.

[0036]

Equation

[0037]

Equation

[0038] The roll angle of the lidar 2 with respect to the vehicle coordinate system is "L φ0 ", the pitch angle is "L θ0 ", and the yaw angle is "L ψ0 ". When the position on the x-axis of the lidar 2 is "L b ", the position on the y-axis is "L x0 ", and the position on the z-axis is "L b ", the measurement point [x y0 (k), y b (k), z z0 (k)] at time "k" as seen from the vehicle coordinate system b0 (k), y b0 (k), z b0 (k)] T is converted to the coordinates [x φ0 (k), y θ0 (k), z ψ0 (k)] of the lidar coordinate system by the following equation (3) using the direction cosine matrix "C0" represented by the rotation matrices "C L0 (k), y L0 (k), z L0 (k)] T corresponding to roll, pitch, and yaw.

[0039]

Equation

[0040]

Equation

[0041] (2) Estimation of roll angle and pitch angle Next, the estimation method of the roll angle L φ0 and pitch angle L θ0 of the lidar 2 will be described. As will be described below, the in - vehicle device 1 estimates the roll angle L φ0 and pitch angle L θ0 of the lidar 2 based on the three - axis acceleration output values obtained from the lidar acceleration sensor 5 installed in the target lidar 2. Hereinafter, for convenience of explanation, it is assumed that the lidar acceleration sensor 5 measures the accelerations of the three axes of the lidar coordinate system.

[0042] When the vehicle is stopped or traveling at a constant speed in a horizontal place, the acceleration in the vehicle coordinate system is only the gravitational acceleration g in the z - direction. Fig. 5(A) is a diagram showing the vector of the gravitational acceleration g in the vehicle coordinate system, and Fig. 5(B) is a diagram showing the vector of the gravitational acceleration g in the lidar coordinate system. Therefore, the output values [α x ,α y ,α z T of the lidar acceleration sensor 5 in the lidar coordinate system satisfy the following equation (5).

[0043]

Equation

[0044]

Equation

[0045]

Equation

[0046]

Number

[0047]

Number

[0048]

Number

[0049] (3) Estimation of yaw angle Next, the yaw angle L of the lidar 2 ψ0The estimation method will be described. As described below, the in-vehicle unit 1 uses the calculated roll angle L and pitch angle L of the lidar 2 to estimate the yaw angle L based on the three-axis acceleration output values obtained from the lidar acceleration sensor 5 when the vehicle is accelerating or decelerating on a straight road. φ0 and pitch angle L θ0 to estimate the yaw angle L based on the three-axis acceleration output values obtained from the lidar acceleration sensor 5 when the vehicle is accelerating or decelerating on a straight road. ψ0

[0050] When the vehicle is accelerating or decelerating with an acceleration “α” on a straight road, in the vehicle coordinate system, an acceleration α occurs in the x direction and a gravitational acceleration g occurs in the z direction. Therefore, for the output values [α x , α y , α z of the lidar acceleration sensor 5 in the lidar coordinate system, the following equation (11) holds. T That is, the following equation (11) holds.

[0051]

Equation

[0052] First, using α y , α z in Equation (11), the following equations (12) and (13) hold respectively.

[0053]

Equation

[0054]

Equation

[0055] Then, subtracting Equation (12) from Equation (13), the following Equation (14) is obtained.

[0056]

Equation

[0057] ​ Similarly, for α in formula (11) y , α z , the following formulas (15) and (16) are established respectively.

[0058] [Number]

[0059] [Number] And by adding formula (15) and formula (16), the following formula (17) is obtained.

[0060] [Number] Furthermore, by adding the formula obtained by multiplying formula (17) by sinL θ0 and the formula obtained by multiplying α in formula (11) x by cosL θ0 , the following formula (18) is obtained.

[0061] [Number] And by dividing formula (14) by formula (18), the following formula (19) is obtained.

[0062] [Number] Therefore, the yaw angle L ψ0 of the lidar 2 is represented by the following formula (20) without using the acceleration α and the gravitational acceleration g.

[0063] [Number] As described above, based on the output value of the lidar acceleration sensor 5 obtained while the vehicle-mounted device 1 is accelerating or decelerating on a straight road, by referring to formula (20), the vehicle-mounted device 1 can obtain the yaw angle L ψ0It can be calculated. Note that the in-vehicle device 1 may determine whether the vehicle is traveling on a straight road based on the output of the vehicle body acceleration sensor 4, or may determine by referring to the road data of the road corresponding to the current position from the map DB10. Further, the in-vehicle device 1 may determine whether the vehicle is accelerating or decelerating based on the output of the vehicle body acceleration sensor 4, or may determine based on the output of a vehicle speed sensor (not shown).

[0064] (4) Calculation of attitude change amount Next, a supplementary explanation will be given regarding the calculation of the change amounts of the pitch angle, roll angle, and yaw angle of the lidar 2 from the time of generating the lidar installation information IL to the processing reference time point, which is the current time. Hereinafter, the pitch angle, roll angle, and yaw angle of the lidar 2 recorded in the lidar installation information IL (i.e., at the initial time when there is no attitude and position deviation of the lidar 2) are respectively referred to as "L φ0 ", "L θ0 ", "L ψ0 ".

[0065] Due to some influence, as shown in the following formula (21), the roll angle of the lidar 2 is "ΔL φ ", the pitch angle is "ΔL θ ", and the yaw angle is "ΔL ψ ", respectively changing from the time of generating the lidar installation information IL, and the roll angle at the processing reference time point is "L φ ", the pitch angle is "L θ ", and the pitch angle is "L ψ ".

[0066]

Equation

[0067]

Number

[0068]

Number

[0069] (5) Calculation of position change amount in z direction Next, a method for calculating the amount of change in position of the LIDAR 2 in the z direction will be described. The vehicle-mounted device 1 calculates the amount of change in position of the LIDAR 2 in the z direction, "ΔL z " is calculated.

[0070] The coordinates of the measurement point at time k in the vehicle coordinate system at the processing reference point [x b (k), y b (k), z b (k)] T is the coordinate [x b0 (k), y b0 (k), z b0 (k)] T Similarly, using the direction cosine matrix “C”, it is expressed by the following equation (24).

[0071]

number

[0072] FIG. 6(A) shows the position of rider 2, L zThe measured value z in the z direction of the road surface measured by the rider 2 while the vehicle is traveling on a flat road surface before the change b0 (k) is shown, and FIG. 6(B) shows the position L of the rider 2 z The measured value z in the z direction of the road surface measured by the rider 2 while the vehicle is traveling on a flat road surface after the change b (k) is shown. As shown in FIGS. 6(A) and 6(B), the measured value z b0 (k) and the measured value z b (k) are respectively the same length as the position L of the rider 2 in the z direction z0 、L z respectively.

[0073] Therefore, it can be seen that the difference between the measured value z of the road surface calculated by Equation (4) b0 (k) and the measured value z of the road surface after the attitude change b (k) is equal to the change amount ΔL in the z direction z . The measured value z used in this case b0 (k) and the measured value z b (k) are preferably the average of multiple scan lines and the time average.

[0074] Taking the above into consideration, the in-vehicle device 1 calculates the change amount ΔL based on the following Equation (25) z .

[0075]

Equation

[0076] Note that the in-vehicle device 1 has the initial position L z0Estimate the stroke amount of the vehicle suspension (i.e., the amount of sinking from the fully extended position) at the time of measurement and the processing reference time, respectively, and based on the difference between the estimated stroke amounts, the change amount ΔL in the z direction z may be corrected. In this case, the in-vehicle device 1 may measure the stroke amount of the suspension based on a stroke sensor or the like provided in the vehicle suspension, or may estimate the stroke amount of the suspension based on the number of passengers in the vehicle. Thereby, the change amount ΔL z can be calculated more accurately.

[0077] (6) Calculation of position change amount in x direction Next, a method for calculating the change amount of the position of the lidar 2 in the x direction will be described. The in-vehicle device 1 calculates the change amount L in the x direction based on the time difference between the change in the z-direction measurement value of the lidar 2 and the change in the pitch rate obtained from the gyro sensor 3 when the vehicle is near the start or end of a slope or when passing over a bump on the road surface. x is calculated.

[0078] Figures 7(A) to 7(G) are diagrams showing the magnitude of the measured value z b (k) in the z direction of a specific scan line of the lidar 2 of a vehicle traveling around the start point 50 of an uphill slope, represented by line segments 51 to 57. Further, Fig. 8(A) is a graph showing the time change of the measured value z b (k) measured when the vehicle shown in Figs. 7(A) to 7(G) is traveling, and Fig. 8(B) is a graph showing the time change of the vehicle body pitch angle (integral value of the pitch rate) in the same period as Fig. 8(A). Note that the numbers 51 to 57 in Figs. 8(A) and 8(B) respectively indicate the positions corresponding to the measured values z b (k) indicated by the line segments 51 to 57 in Figs. 7(A) to 7(G).

[0079] Immediately before the start or end of a sloped road surface such as an uphill or downhill slope, the measured value of the lidar 2 in the z direction with the road surface as the measurement point changes near the start or end of the slope. In the example of Fig. 7, as shown in Fig. 8(A), from the time "t1" (see Fig. 7(B)) when the lidar 2 irradiates the start point 50 (i.e., the change point of the slope), the measured value zb (k) changes gradually, and at the time "t2" (see Fig. 7(D)) when the front wheel of the vehicle approaches the starting point 50, the measured value z b (k) becomes the minimum. After that, the measured value z b (k) gradually increases, and when the rear wheel approaches the starting point 50 (see Fig. 7(F)), the measured value z b (k) is the same as the measured value z b (k) when traveling on a flat road surface.

[0080] Taking the above into consideration, the in-vehicle device 1 calculates the time interval "Δt" from the time t1 (see Fig. 7(B)) when the measured value z b (k) starts to decrease to the time t2 (see Fig. 7(D)) when the measured value z b (k) becomes the minimum.

[0081] Here, the time interval Δt corresponds to the time required for the vehicle to travel the distance d shown in Fig. 7(A), and the distance d corresponds to the distance from the starting point 50 to the front wheel of the vehicle when the starting point 50 of the slope is detected on a specific scan line. The time interval Δt is an example of the "time difference" in the present invention.

[0082] Then, as shown in the following formula (26), the in-vehicle device 1 calculates the distance d by obtaining the traveling speed "v" of the vehicle from the vehicle speed pulse or the like and multiplying it by the time interval Δt.

[0083]

Equation

[0084] In addition, the in-vehicle device 1 can similarly measure the distance d when passing over a bump on the road surface. Figs. 9(A) to 9(G) show the measured value z in the z direction of the lidar 2 of the vehicle traveling before and after the bump 60 b10(A) is a diagram showing the magnitude of the measured value z (k) obtained while the vehicle shown in FIG. 9(A) to FIG. 9(G) is traveling. b 9(A) to 9(G), and FIG. 10(B) is a graph showing the time change of the vehicle body pitch rate during the same period as FIG. 10(A). Note that numbers 61 to 66 in FIG. 10(A) and FIG. 10(B) indicate the measured values ​​z b In this case, as shown in FIG. 10(A), the z-direction measurement value z b (k) is the measured value z at time t3 (see FIG. 9(B)) when the bump 60 is irradiated. b (k) temporarily decreases, and at time t4 (see Figure 9(D)), the measured value z b (k) temporarily increases. Therefore, the vehicle-mounted device 1 detects the measured value z b The measured value z from time t3 (see Fig. 9(B)) when (k) temporarily drops b The time interval up to time t4 (see FIG. 9(D)) when (k) temporarily becomes large is calculated as the time interval Δt. This also allows the vehicle-mounted device 1 to suitably calculate the distance d based on the formula (26). As shown in FIG. 10(B), the vehicle-mounted device 1 can also determine the time t4 when the front wheel of the vehicle body passes over the bump 60 based on the pitch rate (vehicle body pitch rate in FIG. 10(B)) measured by the gyro sensor 3 mounted on the vehicle body or the rider 2.

[0085] Next, the position change amount ΔL from the distance d x The vehicle-mounted device 1 stores the distance "d0" before the position of the rider 2 changes, and calculates the amount of change in the position of the rider 2 in the x direction ΔL by taking the difference from the distance d as shown in the following equation (27). x can be calculated.

[0086]

number

[0087] (7) Calculation of position change amount in y direction Figure 11 is a diagram schematically showing the actions occurring in a turning vehicle. Generally, the speed "V" and the centripetal acceleration "α" of the vehicle center of gravity point during turning are given by the following Equations (28) and (29). y is calculated.

[0088] Here, "r" in Equation (28) indicates the distance from the turning center point to the vehicle center of gravity point. Also, the directions of the speed V and the centripetal acceleration α are orthogonal. For a rigid body, since the angular velocity is the same everywhere, the speed "V" and the centripetal acceleration "α" of point A of the vehicle coordinate origin set at a position [A, A] away from the center of gravity point are given by the following Equations (30) and (31). G " G " are given by the following Equations (28) and (29).

[0089]

Equation

[0090]

Equation

[0091]

Number

[0092]

Number

[0093]

Number

[0094]

Number

[0095]

Number

[0096]

Number

[0097] ​ [Number] Equation (36) indicates that if the difference between the outputs of the vehicle acceleration sensor 4 and the rider acceleration sensor 5 mounted on the vehicle is divided by the square of the vehicle yaw rate, it will be the y-direction position of the rider 2 with respect to the origin of the vehicle coordinate system. Therefore, the in-vehicle device 1 can grasp the position L of the rider 2 by calculating Equation (36). y In addition, the in-vehicle device 1 can also calculate the change amount ΔL by referring to the initial position L stored in the rider installation information IL. y0 by referring to the initial position L stored in the rider installation information IL. y can also calculate the change amount ΔL.

[0098] [Processing Flow] Figure 12 is an example of a flowchart showing the procedure for correcting the output of the rider 2. The in-vehicle device 1 repeatedly executes the process shown in Figure 12 at a predetermined timing.

[0099] First, when the vehicle is stopped or traveling at a constant speed on a horizontal road, the in-vehicle device 1 calculates the change amount ΔL of the roll angle and the change amount ΔL of the pitch angle of the rider 2 from the output value of the rider acceleration sensor 5 (step S101). In this case, the in-vehicle device 1 calculates the roll angle L and the pitch angle L of the rider 2 based on equations equivalent to equations (7) and (10), and calculates the differences from the roll angle L and the pitch angle L recorded in the rider installation information IL as the change amounts ΔL and ΔL. φ and the change amount ΔL of the pitch angle θ from the output value of the rider acceleration sensor 5. In this case, the in-vehicle device 1 calculates the roll angle L and the pitch angle L of the rider 2 based on equations equivalent to equations (7) and (10), and calculates the differences from the roll angle L and the pitch angle L recorded in the rider installation information IL as the change amounts ΔL and ΔL. φ pitch angle L θ of the rider 2, and calculates the differences from the roll angle L and the pitch angle L recorded in the rider installation information IL as the change amounts ΔL and ΔL. φ0 pitch angle L θ0 from the roll angle L and the pitch angle L recorded in the rider installation information IL as the change amounts ΔL and ΔL. φ ΔL θ as the change amounts ΔL and ΔL.

[0100] Next, when the vehicle is accelerating or decelerating on a straight road, the in-vehicle device 1 calculates the change amount ΔL of the yaw angle of the rider 2 from the output value of the rider acceleration sensor 5 (step S102). In this case, the in-vehicle device 1 calculates the yaw angle L of the rider 2 based on an equation equivalent to equation (20), and calculates the difference from the yaw angle L recorded in the rider installation information IL as the change amount ΔL. ψ from the output value of the rider acceleration sensor 5. In this case, the in-vehicle device 1 calculates the yaw angle L of the rider 2 based on an equation equivalent to equation (20), and calculates the difference from the yaw angle L recorded in the rider installation information IL as the change amount ΔL. ψ yaw angle L of the rider 2, and calculates the difference from the yaw angle L recorded in the rider installation information IL as the change amount ΔL. ψ0 yaw angle L recorded in the rider installation information IL as the change amount ΔL. ψ as the change amount ΔL.

[0101] Next, when the vehicle is stopped or traveling at a constant speed on a flat road, the in-vehicle device 1 calculates the amount of change in the z-direction position ΔL from the measured value in the z-direction of the lidar 2 that irradiates the road surface z (Step S103). In this case, the in-vehicle device 1 calculates the average of the measured values z b (k) of the road surface when the vehicle is stopped or traveling at a constant speed on a flat road surface, and calculates the change amount ΔLz by taking the difference from the average of the measured values z b0 (k) of the road surface under the same conditions recorded in the lidar installation information IL (see Equation (25)).

[0102] Next, the in-vehicle device 1 calculates the amount of change in the x-direction position ΔL by calculating the time interval Δt between the change in the measured value in the z-direction of the lidar 2 and the change in the output value of the gyro sensor when near the start or end of a slope or when passing over a bump on the road surface x (Step S104). In this case, the in-vehicle device 1 calculates the distance d by multiplying the traveling speed v of the vehicle by the time interval Δt, and calculates the difference from the distance d0 previously stored in the lidar installation information IL as the change amount ΔL z (see Equation (26)).

[0103] Next, when the vehicle is turning, the in-vehicle device 1 calculates the amount of change in the y-direction position ΔL from the output value in the y-direction of the lidar acceleration sensor 5, the output value in the y-direction of the vehicle body acceleration sensor 4, and the output value of the gyro sensor 3 y (Step S105). Specifically, the in-vehicle device 1 calculates the position L at the processing reference time based on Equation (36) y , and calculates the difference from the initial position L y0 stored in the lidar installation information IL as the amount of change in the y-direction position ΔL y .

[0104] Next, the in-vehicle device 1 calculates the change amounts ΔL φ , ΔL θ , ΔL ψ , ΔL x , ΔL y , ΔL zDetermine whether there is any value that is equal to or greater than a predetermined threshold (step S106). The above-mentioned threshold is a threshold for determining whether the measurement data of the lidar 2 can continue to be used by performing the correction process of the measurement data of the lidar 2 in step S108 described later, and is set in advance based on experiments or the like, for example. Then, the in-vehicle device 1 uses the change amounts ΔL φ ΔL θ ΔL ψ ΔL x ΔL y ΔL z calculated in steps S101 to S105. If there is any value that is equal to or greater than the predetermined threshold among them (step S106; Yes), the use of the output data of the target lidar 2 (that is, the use for obstacle detection, vehicle position estimation, etc.) is terminated, and a warning indicating that it is necessary to perform realignment adjustment again for the target lidar 2 is output by the information output unit 16 (step S107). This reliably suppresses a decrease in safety or the like caused by using the measurement data of the lidar 2 whose posture and position have deviated significantly due to an accident or the like. The above-mentioned threshold is an example of the "predetermined amount" in the present invention.

[0105] On the other hand, if the in-vehicle device 1 determines that there is no value that is equal to or greater than the predetermined threshold among the change amounts ΔL φ ΔL θ ΔL ψ ΔL x ΔL y ΔL z (step S106; No), based on these change amounts, each measured value of the point cloud data output by the lidar 2 is corrected (step S108). In this case, the in-vehicle device 1 stores, for example, a map or the like indicating the correction amount of the measured value for each magnitude of the change amounts, and corrects the above-mentioned measured value by referring to the map or the like. Alternatively, the measured value may be corrected by using a value of a predetermined ratio of the change amount as the correction amount of the measured value.

[0106] As described above, the in-vehicle device 1 in this embodiment estimates at least the attitude of the lidar 2 with respect to the vehicle for measuring the distance to the object, and performs processes such as estimating the attitude of the lidar 2 with respect to the vehicle based on the detection results of the lidar acceleration sensor 5 provided in the lidar 2 when the vehicle is accelerating or decelerating. Thereby, the in-vehicle device 1 can execute the process of converting the measurement data output by the lidar 2 into the vehicle coordinate system with high accuracy, or can determine whether the lidar 2 can be used.

[0107] [Modification Example] Hereinafter, modification examples suitable for the embodiment will be described. The following modification examples may be applied to the embodiment in combination.

[0108] (Modification Example 1) In step S108 of FIG. 12, instead of correcting each measurement value of the point cloud data output by the lidar 2 based on each change amount calculated in steps S101 to S105, the in-vehicle device 1 may convert each measurement value into the vehicle coordinate system based on each estimated value of the attitude and position of the lidar 2 at the processing reference time calculated in steps S101 to S105.

[0109] In this case, the in-vehicle device 1 uses the roll angle L φ , pitch angle L θ , yaw angle L ψ , x-direction position L x , y-direction position L y , z-direction position L z calculated in steps S101 to S105 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 (24), and based on the converted data, perform self-vehicle position estimation, automatic driving control, etc. may be executed.

[0110] In another example, when each lidar 2 is provided with an adjustment mechanism such as an actuator for correcting the attitude and position of each lidar 2, instead of the process of step S108, the in-vehicle device 1 calculates the change amount calculated in steps S101 to S105. It is also possible to perform control to drive the adjustment mechanism so as to correct the attitude and position of the lidar 2 by the amount.

[0111] (Modification Example 2) The configuration of the driving support system shown in FIG. 1 is an example, and the configuration of the driving support system to which the present invention is applicable is not limited to the configuration shown in FIG. 1. For example, instead of having the in-vehicle unit 1, the electronic control unit of the vehicle may execute the processes shown in FIG. 12 or the like. In this case, the rider installation information IL is stored, for example, in a storage unit in the vehicle, and the electronic control unit of the vehicle is configured to be able to receive output data from various sensors such as the rider 2.

Explanation of Reference Numerals

[0112] 1 In-vehicle unit 2 Rider 3 Gyro sensor 4 Vehicle body acceleration sensor 5 Rider acceleration sensor 10 Map DB

Claims

1. An estimation device that estimates an attitude of a measurement unit that measures a distance to an object with respect to a moving object, comprising: an estimation device having an estimation unit that estimates the roll and pitch orientations of the measurement unit based on acceleration data output by an acceleration detection unit provided in the measurement unit when the moving body is traveling or stopped at a predetermined speed.

2. 2. The estimation device according to claim 1, wherein the estimation unit estimates the yaw direction attitude of the measurement unit based on acceleration data output by the acceleration detection unit when the moving body is accelerating and decelerating while traveling and the estimated roll direction and pitch direction attitudes.

3. The estimation device according to claim 1 , wherein the estimation unit estimates the amount of change in the orientation based on the estimated orientation of the measurement unit and the orientation of the measurement unit stored in a storage unit.

4. 4. The estimation device according to claim 1, wherein the estimation unit estimates the heightwise position of the measurement unit based on measurement data of the measurement unit indicating the position of a road surface in the heightwise direction.

5. The estimation device according to claim 4 , wherein the estimation unit estimates the amount of change in the position based on the estimated position of the measurement unit and the position of the measurement unit stored in a storage unit.

6. The estimation device according to claim 3 , further comprising a correction unit that corrects the measurement data output by the measurement unit based on the amount of change.

7. The estimation device according to claim 3 , further comprising a stop control unit that stops processing based on the measurement data output by the measurement unit when the amount of change is equal to or greater than a predetermined amount.

8. An estimation device that estimates an attitude of a measurement unit that measures a distance to an object with respect to a moving object, comprising: an estimation unit that estimates an attitude of the measurement unit with respect to the moving object based on a detection result of an acceleration detection unit provided in the measurement unit when the moving object is traveling with acceleration and deceleration; having The estimation unit is an estimation device that estimates the position of the measurement unit in the forward / backward direction of the moving body based on the distance between the moving body and a road point where the gradient changes or a bump on the road surface is measured by the measurement unit.

9. 9. The estimation device according to claim 8, wherein the estimation unit calculates the distance based on a time difference between a change in data output by a tilt detection unit that detects a tilt in a pitch direction of the moving body and a change in measurement data output by the measurement unit.

10. An estimation device that estimates an attitude of a measurement unit that measures a distance to an object with respect to a moving object, comprising: an estimation unit that estimates an attitude of the measurement unit with respect to the moving object based on a detection result of an acceleration detection unit provided in the measurement unit when the moving object is traveling with acceleration and deceleration; having The estimation unit is an estimation device that estimates the position of the measurement unit in the left-right direction of the moving body based on left-right acceleration data of the moving body output by the acceleration detection unit while the moving body is turning, left-right acceleration data of the moving body output by an acceleration sensor mounted on the moving body, and the yaw rate of the moving body output by a gyro sensor mounted on the moving body.

11. A control method executed by an estimation device that estimates an attitude of a measurement unit that measures a distance to an object with respect to a moving object, comprising: A control method comprising an estimation step of estimating the roll and pitch orientations of the measurement unit based on acceleration data output by an acceleration detection unit provided in the measurement unit when the moving body is traveling or stopped at a predetermined speed.

12. A control method executed by an estimation device that estimates an attitude of a measurement unit that measures a distance to an object with respect to a moving object, comprising: an estimation step of estimating an attitude of the measurement unit with respect to the moving object based on a detection result of an acceleration detection unit provided in the measurement unit when the moving object is accelerating and decelerating while traveling; The estimation process is a control method for estimating the position of the measurement unit in the forward / backward direction of the moving body based on the distance between the moving body and a road point where a gradient changes or a bump on the road surface is measured by the measurement unit.

13. A control method executed by an estimation device that estimates an attitude of a measurement unit that measures a distance to an object with respect to a moving object, comprising: an estimation step of estimating an attitude of the measurement unit with respect to the moving object based on a detection result of an acceleration detection unit provided in the measurement unit when the moving object is accelerating and decelerating while traveling; The estimation process is a control method for estimating the position of the measurement unit in the left-right direction of the moving body based on left-right acceleration data of the moving body output by the acceleration detection unit while the moving body is turning, left-right acceleration data of the moving body output by an acceleration sensor mounted on the moving body, and the yaw rate of the moving body output by a gyro sensor mounted on the moving body.

14. A program executed by a computer to estimate an attitude of a measurement unit that measures a distance to a target object with respect to a moving object, an estimation unit that estimates the attitude of the measurement unit in the roll direction and the pitch direction based on acceleration data output from an acceleration detection unit provided in the measurement unit when the moving body is traveling at a predetermined speed or is stopped; A program for causing the computer to function as a

15. A program executed by a computer to estimate an attitude of a measurement unit that measures a distance to a target object with respect to a moving object, causing the computer to function as an estimation unit that estimates an attitude of the measurement unit with respect to the moving object based on a detection result of an acceleration detection unit provided in the measurement unit when the moving object is accelerating and decelerating while traveling; The estimation unit is a program that estimates the position of the measurement unit in the forward / backward direction of the moving body based on the distance between the moving body and a road point where a gradient changes or a bump on the road surface is measured by the measurement unit.

16. A program executed by a computer to estimate an attitude of a measurement unit that measures a distance to a target object with respect to a moving object, an estimation unit that estimates an attitude of the measurement unit with respect to the moving object based on a detection result of an acceleration detection unit provided in the measurement unit when the moving object is traveling with acceleration and deceleration; and causing the computer to function as The estimation unit is a program that estimates the position of the measurement unit in the left-right direction of the moving body based on left-right acceleration data of the moving body output by the acceleration detection unit while the moving body is turning, left-right acceleration data of the moving body output by an acceleration sensor mounted on the moving body, and the yaw rate of the moving body output by a gyro sensor mounted on the moving body.

17. A storage medium storing the program according to any one of claims 14 to 16.

Images