JP7843385B2 - Estimation device, control method, program, and storage medium - Google Patents

Description

This invention relates to a technique for estimating the orientation of a measuring unit.

Conventionally, technologies for estimating a vehicle's position based on measurement data from measurement units such as radar and cameras have been known. For example, Patent Document 1 discloses a technology for estimating a vehicle's position by comparing the output of a measurement sensor with location information of features pre-registered on a map. Patent Document 2 also discloses a vehicle position estimation technology using a Kalman filter.

Japanese Patent Publication No. 2013-257742 Japanese Patent Publication No. 2017-72422

Data obtained from measurement units such as radar and cameras is in a coordinate system relative to the measurement unit and depends on the attitude of the measurement unit relative to the vehicle. Therefore, it is necessary to convert it to a coordinate system relative to the vehicle. Consequently, if a shift occurs in the attitude of the measurement unit, it is necessary to accurately detect this shift and reflect it in the measurement unit's data.

This invention was made to solve the above-mentioned problems, and its main objective is to provide an estimation device capable of suitably estimating the orientation of a measuring unit, which measures the distance to an object, relative to a moving object.

The invention described in the claims is,
An estimation device for estimating the orientation of a measuring unit that measures the distance to an object relative to a moving body,
The system includes an estimation unit that estimates the roll and pitch orientation of the measurement unit based on acceleration data output by an acceleration detection unit provided in the measurement unit when the moving unit is traveling or stopped at a predetermined speed.
Furthermore, the invention described in the claims is,
An estimation device for estimating the orientation of a measuring unit that measures the distance to an object relative to a moving body,
The measurement unit has an estimation unit that estimates the attitude of the measurement unit relative to the moving body based on the detection result of the acceleration detection unit provided in the measurement unit when the moving body is moving while accelerating and decelerating.
The estimation unit estimates the position of the measurement unit in the longitudinal 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, or when a bump on the road surface is measured by the measurement unit.
Furthermore, the invention described in the claims is,
An estimation device for estimating the orientation of a measuring unit that measures the distance to an object relative to a moving body,
The measurement unit has an estimation unit that estimates the attitude of the measurement unit relative to the moving body based on the detection result of the acceleration detection unit provided in the measurement unit when the moving body is moving while accelerating and decelerating.
The estimation unit estimates the position of the measurement unit in the left-right direction of the moving body based on the left-right acceleration data of the moving body output by the acceleration detection unit during the rotation of the moving body, the left-right acceleration data 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.

Furthermore, the invention described in the claims is,
A control method performed by an estimation device that estimates the attitude of a measuring unit, which measures the distance to an object, relative to a moving object,
The system includes an estimation step of estimating the orientation of the measurement unit in the roll direction and pitch direction based on acceleration data output by an acceleration detection unit provided in the measurement unit when the moving unit is traveling or stopped at a predetermined speed.
Furthermore, the invention described in the claims is,
A control method performed by an estimation device that estimates the attitude of a measuring unit, which measures the distance to an object, relative to a moving object,
The measurement unit has an estimation step of estimating the attitude of the measurement unit relative to the moving body based on the detection result of the acceleration detection unit provided in the measurement unit when the moving body is moving while accelerating or decelerating.
The estimation step estimates the position of the measuring unit in the longitudinal 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, or when a bump on the road surface is measured by the measuring unit.
Furthermore, the invention described in the claims is,
A control method performed by an estimation device that estimates the attitude of a measuring unit, which measures the distance to an object, relative to a moving object,
The measurement unit has an estimation step of estimating the attitude of the measurement unit relative to the moving body based on the detection result of the acceleration detection unit provided in the measurement unit when the moving body is moving while accelerating or decelerating.
The estimation step estimates the position of the measurement unit in the left-right direction of the moving body based on the left-right acceleration data of the moving body output by the acceleration detection unit during the rotation of the moving body, the left-right acceleration data 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.

Furthermore, the invention described in the claims is,
A program executed by a computer to estimate the orientation of a measuring unit that measures the distance to an object relative to a moving object,
The computer is made to function as an estimation unit that estimates the roll and pitch orientation of the measurement unit based on acceleration data output by the acceleration detection unit provided in the measurement unit when the moving unit is traveling or stopped at a predetermined speed.
Furthermore, the invention described in the claims is,
A program executed by a computer to estimate the orientation of a measuring unit that measures the distance to an object relative to a moving object,
When the moving body is accelerating and decelerating, the computer functions as an estimation unit that estimates the attitude of the measurement unit relative to the moving body based on the detection result of the acceleration detection unit provided in the measurement unit.
The estimation unit estimates the position of the measurement unit in the longitudinal 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, or when a bump on the road surface is measured by the measurement unit.
Furthermore, the invention described in the claims is,
A program executed by a computer to estimate the orientation of a measuring unit that measures the distance to an object relative to a moving object,
When the moving body is accelerating and decelerating, the computer functions as an estimation unit that estimates the attitude of the measurement unit relative to the moving body based on the detection result of the acceleration detection unit provided in the measurement unit.
The estimation unit estimates the position of the measurement unit in the left-right direction of the moving body based on the left-right acceleration data of the moving body output by the acceleration detection unit during the rotation of the moving body, the left-right acceleration data 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.

This is a schematic diagram of the driver assistance system. This is a block diagram showing the functional configuration of an in-vehicle device. This figure shows the relationship between the vehicle coordinate system and the rider coordinate system, represented by two-dimensional coordinates. This figure shows the relationship between the vehicle coordinate system and the rider coordinate system, represented by three-dimensional coordinates. This figure shows the vector of gravitational acceleration in the vehicle coordinate system and the rider coordinate system. This figure shows the z-direction measurements of the road surface taken by the rider while the vehicle was traveling on a flat road surface, before and after a change in the rider's position in the z-direction. This figure shows the magnitude of the z-axis measurement of the rider of a vehicle traveling before and after the starting point of an uphill slope. This graph shows the time-dependent changes in measured values and vehicle pitch angle obtained during vehicle operation. This figure shows the magnitude of the z-axis measurement of the rider of a vehicle traveling in front of and behind a bump. This graph shows the time-dependent changes in measured values and vehicle pitch rate obtained during vehicle operation. This diagram schematically illustrates the effects that occur on a vehicle during a turn. This is an example flowchart showing the procedure for correcting the output of a lider.

According to a preferred embodiment of the present invention, an estimation device for estimating the attitude of a measuring unit relative to a moving body, wherein the device includes an estimation unit that estimates the attitude of the measuring unit relative to the moving body based on the detection result of an acceleration detection unit provided on the measuring unit when the moving body is traveling with acceleration and deceleration. In this embodiment, the estimation device can suitably estimate the attitude of the measuring unit in the yaw direction relative to the vehicle. Note that "traveling with acceleration and deceleration" includes both modes of travel where the vehicle is accelerating and modes of travel where the vehicle is decelerating.

In one embodiment of the estimation device described above, the estimation unit estimates the roll and pitch orientations of the measurement unit based on acceleration data output by the acceleration detection unit when the moving body is traveling or stopped at a predetermined speed, and estimates the yaw orientation of the measurement unit based on acceleration data output by the acceleration detection unit and the estimated roll and pitch orientations when the moving body is accelerating or decelerating. In this embodiment, the estimation device can suitably estimate the roll and pitch orientations of the measurement unit relative to the vehicle when the moving body is traveling or stopped at a predetermined speed, and suitably estimate the yaw orientation of the measurement unit relative to the vehicle when the moving body is accelerating or decelerating.

In another embodiment of the estimation device described above, the estimation unit estimates the change in the attitude based on the estimated attitude of the measurement unit and the attitude of the measurement unit stored in the memory unit. This allows the estimation device to suitably estimate the change in the current attitude relative to the standard attitude stored in the memory unit.

In another embodiment of the estimation device described above, 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. In this embodiment, the estimation device can suitably estimate the position of the measurement unit in the height direction.

In another embodiment of the estimation device described above, the estimation unit estimates the position of the measuring unit in the longitudinal direction of the moving body based on the distance between the road point where the gradient changes and the moving body when the road point is measured by the measuring unit. In this embodiment, the estimation device can suitably estimate the position of the measuring unit in the longitudinal direction of the moving body.

In another embodiment of the estimation device described above, the estimation unit calculates the distance based on the time difference between the change in data output by the tilt detection unit, which detects the tilt in the pitch direction of the moving body, and the change in measurement data output by the measurement unit. In this embodiment, the estimation device can suitably calculate the distance between the road point where the gradient changes and the moving body when the measurement unit measures the road point, and can use the position of the measurement unit in the longitudinal direction of the moving body for estimation.

In another embodiment of the estimation device described above, the estimation unit estimates the position of the measurement unit in the left-right direction of the moving body based on the left-right acceleration data of the moving body output by the acceleration detection unit during the rotation of the moving body, the left-right acceleration data 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. In this embodiment, the estimation device can suitably estimate the position of the measurement unit in the left-right direction of the moving body.

In another embodiment of the estimation device described above, 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. This allows the estimation device to suitably estimate the amount of change in the current position relative to the standard position of the measurement unit stored in the storage unit.

In another embodiment of the estimation device described above, a correction unit is further provided to correct the measurement data output by the measurement unit based on the amount of change. In this embodiment, the estimation device can correct the measurement data from the measurement unit so that the effects of the misalignment do not occur, even if the orientation or position of the measurement unit is misaligned.

In another embodiment of the estimation device described above, a stop control unit is further provided that stops processing based on the measurement data output by the measurement unit when the amount of change exceeds a predetermined amount. This embodiment ensures that the estimation device can reliably suppress the decrease in accuracy of various processes using measurement data caused by using measurement data from a measurement unit with large deviations in attitude and position.

According to another preferred embodiment of the present invention, a control method performed by an estimation device for estimating the attitude of a measuring unit, which measures the distance to an object, relative to a moving body, comprises an estimation step of estimating the attitude of the measuring unit relative to the moving body based on the detection result of an acceleration detection unit provided on the measuring unit when the moving body is accelerating and decelerating. By using this control method, the estimation device can suitably estimate the attitude of the measuring unit in the yaw direction relative to the vehicle.

According to another preferred embodiment of the present invention, a computer program is executed to estimate the attitude of a measuring unit that measures the distance to an object relative to a moving object. This program causes the computer to function as an estimation unit that estimates the attitude of the measuring unit relative to the moving object based on the detection results of an acceleration detection unit provided on the measuring unit when the moving object is accelerating and decelerating. By executing this program, the computer can suitably estimate the attitude of the measuring unit in the yaw direction relative to the vehicle. Preferably, the program is stored in a storage medium.

The following describes preferred embodiments of the present invention with reference to the drawings.

[Schematic configuration]
Figure 1 is a schematic diagram of the driver assistance system according to this embodiment. The driver assistance system shown in Figure 1 includes an on-board unit 1 mounted on the vehicle and performing control related to driver assistance for 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.

The in-vehicle unit 1 is electrically connected to the lidar 2, gyro sensor 3, vehicle body acceleration sensor 4, and lidar acceleration sensor 5, and acquires their output data. It also stores a map database (DB: DataBase) 10 containing road data and information about features located near the road. Based on the output data and the map DB 10, the in-vehicle unit 1 estimates the vehicle's position (also called "vehicle position") and performs control related to vehicle driving assistance, such as automatic driving control, based on the estimated vehicle position. Furthermore, the in-vehicle unit 1 estimates the attitude and position of the lidar 2 based on the outputs of the lidar 2, gyro sensor 3, vehicle body acceleration sensor 4, and lidar acceleration sensor 5. Based on this estimation result, the in-vehicle unit 1 performs processing such as correcting each measured value of the point cloud data output by the lidar 2. The in-vehicle unit 1 is an example of the "estimation device" in this invention.

The lidar 2 discretely measures the distance to an object in the external environment by emitting a pulsed laser within a predetermined angular range in the horizontal and vertical directions, and generates three-dimensional point cloud information indicating the position of the object. In this case, the lidar 2 includes an irradiation unit that irradiates laser light while changing the irradiation direction, a light receiving unit that receives reflected (scattered) light from the irradiated laser light, and an output unit that outputs scan data based on the received signal output by the light receiving unit. The scan data is generated based on the irradiation direction corresponding to the laser light received by the light receiving unit and the distance to the object in that irradiation direction, which is determined based on the received signal described above, and is supplied to the in-vehicle device 1. In this embodiment, as an example, the lidar 2 is provided in the front and rear portions of the vehicle, respectively. The lidar 2 is an example of a "measurement unit" in this invention.

The gyro sensor 3 is installed on the vehicle and supplies an output signal corresponding to the vehicle's yaw rate to the on-board unit 1. The vehicle body acceleration sensor 4 is a three-axis acceleration sensor installed on the vehicle and supplies detection signals corresponding to the acceleration data of the three axes corresponding to the vehicle's direction of travel, lateral direction, and height direction to the on-board unit 1. The gyro sensor 3 and the vehicle body acceleration sensor 4 are examples of the "tilt detection unit" in this invention. The rider acceleration sensor 5 is a three-axis acceleration sensor installed on each rider 2 and supplies detection signals corresponding to the acceleration data of the three axes of the installed rider 2 to the on-board unit 1. The rider acceleration sensor 5 is an example of the "acceleration detection unit" in this invention.

Figure 2 is a block diagram showing the functional configuration of the in-vehicle unit 2. The in-vehicle unit 2 mainly comprises 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 bus lines.

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

The storage unit 12 stores programs executed by the control unit 15 and information necessary for the control unit 15 to perform predetermined processing. In this embodiment, the storage unit 12 has a map DB 10 and rider placement information IL. The rider placement information IL is information regarding the relative three-dimensional position and attitude of each rider 2 at a certain reference time (for example, immediately after alignment adjustment of rider 2, when no attitude or positional deviation has occurred). In this embodiment, the attitude of rider 2, etc., is represented by the roll angle, pitch angle, and yaw angle (i.e., Euler angle). The rider placement information IL may be information regarding the position and attitude measured at the above-mentioned reference time, or it may be information regarding the position and attitude of rider 2 estimated by the in-vehicle unit 1 through the position and attitude estimation processing of rider 2 described later.

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

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

[Estimation of the Rider's position and attitude]
Next, the method for estimating the position and attitude of Rider 2 will be explained. The in-vehicle unit 1 performs the following processing for each Rider 2.

(1) Coordinate system transformation
The 3D coordinates of each measurement point in the 3D point cloud data acquired by LIDA2 are expressed in a coordinate system based on the position and orientation of LIDA2 (also called the "LIDA coordinate system"), and need to be converted to a coordinate system based on the position and orientation of the vehicle (also called the "vehicle coordinate system"). Here, we will first explain the conversion between the LIDA coordinate system and the vehicle coordinate system.

Figure 3 shows the relationship between the vehicle coordinate system and the rider coordinate system, represented by two-dimensional coordinates. Here, the vehicle coordinate system has the center of the vehicle as the origin and has coordinate axes "x _b " along the direction of travel of the vehicle and coordinate axes "y _b " along the side direction of the vehicle. The rider coordinate system has coordinate axes "x _L " along the front direction of rider 2 (see arrow A2) and coordinate axes "y _L " along the side direction of rider 2.

Here, if we denote the yaw angle of rider 2 with respect to the vehicle coordinate system as "L _ψ0 " and the position of rider 2 as [L _x0 , L _y0 ] ^T , then the measurement point [x _b (k), y _b (k)] ^T at time " _k " as seen from the vehicle coordinate system is transformed into coordinates [x _L (k), y _L (k)] ^T in the rider coordinate system by the following equation (1) using the rotation matrix "C ψ0".

On the other hand, the transformation from the lidar coordinate system to the vehicle coordinate system can be done using the inverse matrix (transpose matrix) of the rotation matrix. Therefore, the measurement point [x _L (k), y _L (k)] ^T obtained in the lidar coordinate system at time k can be transformed into vehicle coordinates [x _b (k), y _b (k)] ^T by the following equation (2).

Figure 4 shows the relationship between the vehicle coordinate system and the rider coordinate system, represented by three-dimensional coordinates. Here, the coordinate axis perpendicular to the coordinate axes x _b and y _b is denoted as "z _b ", and the coordinate axis perpendicular to the coordinate axes x _L and y _L is denoted as "z _L ".

If the roll angle of Rider 2 relative to the vehicle coordinate system is "L _φ0 ", the pitch angle is "L _θ0 ", and the yaw angle is "L _ψ0 ", and the position of Rider 2 on the coordinate axis x _b is "L _x0 ", the position on the coordinate axis y _b is "L _y0 ", and the position on the coordinate axis z _b is "L _z0 ", then the measurement point [x _b0 (k), y _b0 (k), z _b0 (k)] ^T at time "k" as seen from the vehicle coordinate system is transformed into coordinates [x _L0 (k), y _{L0 (k} ), z _L0 (k)] T in the Rider coordinate system by the following equation (3) using the direction cosine matrix " _C0 ", which is represented by the rotation matrices "C _φ0 ", "C _θ0 ", and "C _ψ0 ", respectively, corresponding to roll, pitch, and ^yaw .

On the other hand, the transformation from the rider coordinate system to the vehicle coordinate system can be done using the inverse (transpose) of the direction cosine matrix. Therefore, the measurement point [x _L0 (k), y _L0 (k), z _L0 (k)] ^T obtained in the rider coordinate system at time k can be transformed into vehicle coordinates [x _b0 (k), y _b0 (k), z _b0 (k)] ^T by the following equation (4).

Hereafter, the directions along each coordinate axis x _b , y _b , and z _b in the vehicle coordinate system will also be simply referred to as the "x direction,""ydirection," and "z direction," respectively.

(2) Estimation of roll angle and pitch angle
Next, the method for estimating the roll angle L _φ0 and pitch angle L _θ0 of the rider 2 will be explained. As described below, the in-vehicle unit 1 estimates the roll angle L _φ0 and pitch angle L _θ0 of the rider 2 based on the three-axis acceleration output values obtained from the rider acceleration sensor 5 installed on the target rider 2. For the sake of explanation, it will be assumed that the rider acceleration sensor 5 measures the acceleration of the three axes of the rider coordinate system.

When a vehicle is stationary on a level surface or traveling at a constant speed, the only acceleration in the vehicle coordinate system is the gravitational acceleration g in the z direction. Figure 5(A) shows the vector of gravitational acceleration g in the vehicle coordinate system, and Figure 5(B) shows the vector of gravitational acceleration g in the lidar coordinate system. Therefore, the output value [α _x , α _y , α _z ] ^T of the lidar coordinate system of the lidar acceleration sensor 5 is given by the following equation (5).

Now, using _αy and _αz from equation (5), we can calculate " _αy / _αz " and obtain the following equation (6).

Therefore, the roll angle L _φ0 of rider 2 is expressed by the following equation (7), which does not use the acceleration due to gravity g.

Furthermore, by using α _y and α _z in equation (5) to calculate "α _y ² + α _z ² ", we obtain the following equation (8). By eliminating the gravitational acceleration g using equation (8) and α _x in equation (5), we obtain the following equation (9).

Therefore, the pitch angle L _θ0 of rider 2 is expressed by the following equation (10), which does not use the acceleration due to gravity g.

As described above, when the in-vehicle unit 1 is stationary or traveling at a constant speed on a level surface, it can calculate the roll angle L _φ0 and pitch angle L _θ0 of the rider 2 based on the output value of the rider acceleration sensor 5 by referring to equations (7) and (10). The in-vehicle unit 1 may also determine whether its current position is on a level surface based on the output of the gyro sensor 3 or the vehicle body acceleration sensor 4, or it may determine this by referring to information regarding the inclination angle of the road data corresponding to the vehicle's current position from the map DB 10. Furthermore, the in-vehicle unit 1 may also determine whether the vehicle is stationary or traveling at a constant speed based on the output of the vehicle body acceleration sensor 4, or it may determine this based on the output of a vehicle speed sensor (not shown).

(3) Estimation of Yaw Angle
Next, the method for estimating the yaw angle _Lψ0 of the rider 2 will be explained. As described below, the in-vehicle unit 1 uses the calculated roll angle _Lφ0 and pitch angle _Lθ0 of the rider 2 to estimate the yaw angle _Lψ0 based on the three-axis acceleration output values obtained from the rider's acceleration sensor 5 while the vehicle is accelerating or decelerating on a straight road.

When a vehicle is accelerating or decelerating on a straight road with acceleration "α", acceleration α is generated in the x direction and gravitational acceleration g is generated in the z direction in the vehicle coordinate system. Therefore, the following equation (11) holds for the output value [α _x , α _y , α _z ] ^T of the lidar coordinate system of the lidar acceleration sensor 5.

Here, in order to eliminate acceleration α and gravitational acceleration g, we perform the calculation described below.

First, using _αy and _αz from equation (11), the following equations (12) and (13) hold true, respectively.

Then, subtracting equation (12) from equation (13) yields the following equation (14).

Similarly, using _αy and _αz from equation (11), the following equations (15) and (16) hold true, respectively.

Then, adding equation (15) and equation (16), we obtain the following equation (17).

Furthermore, adding the equation obtained by multiplying equation (17) by sinL _θ0 and the equation obtained by multiplying α _x in equation (11) by cosL _θ0 yields the following equation (18).

Then, dividing equation (14) by equation (18) yields the following equation (19).

Therefore, the yaw angle _Lψ0 of rider 2 is expressed by the following equation (20), which does not use acceleration α and gravitational acceleration g.

Based on the above, the on-board unit 1 can calculate the yaw angle _Lψ0 of the rider 2 by referring to equation (20) based on the output value of the rider acceleration sensor 5 obtained while accelerating or decelerating on a straight road. The on-board unit 1 may also determine whether the vehicle is traveling on a straight road based on the output of the vehicle body acceleration sensor 4, or by referring to road data of the road corresponding to the current position from the map DB 10. Furthermore, the on-board unit 1 may determine whether the vehicle is accelerating or decelerating based on the output of the vehicle body acceleration sensor 4, or based on the output of a vehicle speed sensor (not shown).

(4) Calculation of change in posture
Next, we will provide a supplementary explanation regarding the calculation of the changes in the pitch angle, roll angle, and yaw angle of Rider 2 from the time of generation of the Rider placement information IL to the current time, which is the processing reference point. In the following, the pitch angle, roll angle, and yaw angle of Rider 2 recorded in the Rider placement information IL (i.e., the initial state when there is no attitude or positional deviation of Rider 2) will be denoted as "L _φ0 ", "L _θ0 ", and "L _ψ0 ", respectively.

Due to some influence, the roll angle of lidar 2 changes by "ΔL _φ ", the pitch angle by "ΔL _θ ", and the yaw angle by "ΔL _ψ " from the time the lidar installation information IL was generated, as shown in equation (21) below, and the roll angle at the processing reference time becomes "L _φ ", the pitch angle by "L _θ ", and the yaw angle by "L _ψ ".

Similarly, as shown in equation (22) below, the position of lidar 2 on the coordinate axis x _b changes by "ΔL _x0 ", the position on the coordinate axis y _b changes by "ΔL _y0 ", and the position on the coordinate axis z _b changes by "ΔL _z0 " from the time the lidar installation information IL was generated, and the position on the coordinate axis x _b at the processing reference time becomes "L _x ", the position on the coordinate axis y _b changes by "L _y ", and the position on the coordinate axis z _b changes by "L _z ".

Furthermore, suppose that due to the aforementioned changes in the attitude and position of rider 2, the measurement point obtained from rider 2 at time k changes from [x _L0 (k), y _L0 (k), z _L0 (k)] ^T to [x _L (k), y _L (k), z _L (k)] ^T. In this case, the conversion of the measurement point at time k from the vehicle coordinate system to the rider coordinate system is given by equation (23).

Here, since equation (23) is in the same form as equation (3), the roll angle L _φ , pitch angle L _θ , and yaw angle L _ψ of the rider 2 can be calculated using the same equation as equations (7), (10), and (20). Therefore, the onboard unit 1 can suitably calculate the change in roll angle ΔL _φ , the change in pitch angle ΔL _θ , and the change in yaw angle ΔL _ψ by calculating the difference between these roll angle L _φ , pitch angle L _θ , and yaw angle L _ψ and the roll angle L _φ0 , pitch angle L _θ0 , and yaw angle L _ψ0 recorded in the rider installation information IL. Furthermore, the in-vehicle unit 1 may store the roll angle L φ0, pitch angle L θ0, and yaw angle L _ψ0 of the rider 2, estimated based on equations (7), (10), and (20), as rider installation information IL at the first time the attitude estimation process of the rider 2 can be performed after the rider 2 is mounted on the vehicle (i.e. _, alignment adjustment ₎ .

(5) Calculation of the change in position in the z direction
Next, the method for calculating the change in position of the rider 2 in the z direction will be explained. The onboard unit 1 calculates the change in position of the rider 2 in the z direction, "ΔL _z ", based on the z-direction values of the measurement points of the rider 2 measured on the road surface when the vehicle is stopped or traveling at a constant speed on a flat road.

The coordinates [x _b (k), y _b (k), z _b (k)] ^T of the measurement point at time k in the vehicle coordinate system at the processing reference time are expressed by the following equation (24), using the direction cosine matrix "C", similar to the coordinates [x _b0 (k), y _b0 (k), z _b0 (k)] ^T shown by equation (4).

Here, if the roll angle L _φ , pitch angle L _θ , and pitch angle L _ψ at the processing reference point are accurately determined, then "C ^-1 [x _L (k), y _L (k), z _L (k)] ^T " is correctly converted to values in the vehicle coordinate system. Therefore, when the vehicle is stationary or traveling at a constant speed on a flat road surface, the pitch fluctuation of the vehicle is small, so the z-direction measurement value of the lidar 2 illuminating the road surface becomes the height from the mounting position of the lidar 2 to the road surface (i.e., position L _z ).

Figure 6(A) shows the z-direction measurement value z _b0 (k) of the road surface measured by Rider 2 when the vehicle is traveling on a flat road surface before the change in Rider 2's position L _z , and Figure 6(B) shows the z-direction measurement value z _b (k) of the road surface measured by Rider 2 when the vehicle is traveling on a flat road surface after the change in Rider 2's position L _z . As shown in Figures 6(A) and (B), the measured value z _b0 (k) and the measured value z _b (k) are the same length as the z-direction positions L _z0 and L _z of Rider 2, respectively.

Therefore, it can be seen that the difference between the measured value z _b0 (k) of the road surface calculated by equation (4) and the measured value z _b (k) of the road surface after the change in posture is equal to the amount of change in the z direction ΔL _z . In this case, it is desirable that the measured values z _b0 (k) and z _b (k) used are the average and time average of multiple scanning lines, respectively.

Taking the above into consideration, the in-vehicle unit 1 calculates the change amount ΔL _z based on the following equation (25).

Therefore, after measuring the initial position L _z0 of the rider 2, the on-board unit 1 calculates the average of the measured road surface values z _b0 (k) when the vehicle is stopped or traveling at a constant speed on a flat road surface, and records this along with the initial position L _z0 of the rider 2 in the rider installation information IL. As a result, the on-board unit 1 can suitably calculate the position L _z and the change amount ΔL _z at the processing reference time by referring to the rider installation information IL.

Furthermore, the on-board unit 1 may estimate the suspension stroke amount (i.e., the amount of sag from the fully extended position) of the vehicle at the time of measurement of the initial position L _z0 and at the processing reference time, and correct the change amount ΔL _z in the z direction based on the difference between the estimated stroke amounts. In this case, the on-board unit 1 may measure the suspension stroke amount based on a stroke sensor or the like provided on the vehicle's suspension, or it may estimate the suspension stroke amount based on the number of passengers in the vehicle. This makes it possible to calculate the change amount ΔL _z more accurately.

(6) Calculation of the change in position in the x direction
Next, the method for calculating the change in position of the rider 2 in the x-direction will be explained. The onboard unit 1 calculates the change in position Lx in the x-direction based on the time difference between the change in the z-direction measurement of the rider 2 and the change in pitch rate obtained from the gyro sensor 3, when the unit is near the start or end of a slope, or when passing over a _bump on the road surface.

Figures 7(A) to 7(G) are diagrams showing the magnitude of the z-axis measured value z _b (k) of a specific scanline of the rider 2 of a vehicle traveling before and after the starting point 50 of an uphill slope, represented by line segments 51 to 57. Figure 8(A) is a graph showing the time change of the measured value z _b (k) measured while the vehicle is traveling as shown in Figures 7(A) to 7(G), and Figure 8(B) is a graph showing the time change of the vehicle body pitch angle (integral value of pitch rate) over the same period as in Figure 8(A). The numbers 51 to 57 in Figures 8(A) and 8(B) indicate the positions corresponding to the measured value z _b (k) shown by line segments 51 to 57 in Figures 7(A) to 7(G), respectively.

Just before the start or end of a slope, such as an uphill or downhill, the measurement value of the rider 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 in Figure 7, as shown in Figure 8(A), the measurement value z _b (k) gradually changes from the time "t1" (see Figure 7(B)) when the rider 2 illuminates the starting point 50 (i.e., the point where the gradient changes), and the measurement value _{z b (k) is at its minimum at the time "t2" (see Figure 7(D)) when the front wheel of the vehicle approaches the starting point 50. After that, the measurement value z b (} k) gradually increases, and when the rear wheel approaches the starting point 50 (see Figure 7(F)), the measurement value z _b (k) is the same as the measurement value z _b (k) when driving on a flat road surface.

Taking the above into consideration, the in-vehicle device 1 calculates the time interval "_{Δt" from time t1 (see Figure 7(B)) when the measured value z b (} k) begins to decrease to time t2 (see Figure 7(D)) when the measured value z b(k) is at its minimum.

Here, the time interval Δt corresponds to the time required for the vehicle to travel the distance d shown in Figure 7(A), where distance d corresponds to the distance from the starting point 50 of the slope to the front wheels of the vehicle when the starting point 50 is detected on a specific scanline. The time interval Δt is an example of the "time difference" in this invention.

Then, as shown in equation (26) below, the in-vehicle device 1 calculates the distance d by determining the vehicle's speed "v" from the vehicle speed pulse and multiplying it by the time interval Δt.

Furthermore, as shown in Figure 8(B), the on-board unit 1 can also determine the time t2 when the front wheels of the vehicle body reach the point of gradient change (starting point 50 in Figure 7) based on the pitch angle (vehicle body pitch angle in Figure 8(B)) measured by the gyro sensor 3 mounted on the vehicle body or rider 2.

Furthermore, the on-board unit 1 can similarly measure the distance d when passing over bumps on the road surface. Figures 9(A) to 9(G) are diagrams showing the magnitude of the measured value z _{b (k) in the z direction of the rider 2 of a vehicle traveling before and after a bump 60, represented by line segments 61 to 66. Figure 10(A) is a graph showing the time change of the measured value z b (} k) obtained when the vehicle shown in Figures 9(A) to 9(G) is traveling, and Figure 10(B) is a graph showing the time change of the vehicle body pitch rate over the same period as in Figure 10(A). Note that the numbers 61 to 66 in Figures 10(A) and 10(B) indicate the positions corresponding to the measured value z _b (k) shown by line segments 61 to 66 in Figures 9(A) to 9(G), respectively. In this case, as shown in Figure 10(A), the measured value z _b (k) in the z direction of the vehicle's rider 2 temporarily decreases at time t3 (see Figure 9( _B )) when the bump 60 is illuminated, and temporarily increases at time t4 (see Figure 9(D)) when the front wheels of the vehicle exceed the bump 60. Therefore, the on-board unit 1 calculates the time interval Δt from time t3 (see Figure 9(B)) when the measured value _{z b (} k) temporarily decreased to time t4 (see Figure 9(D)) when the measured value z _b (k) temporarily increased. With this, the on-board unit 1 can also suitably calculate the distance d based on equation (26). As shown in Figure 10(B), the on-board unit 1 can also determine the time t4 when the front wheels of the vehicle exceed the bump 60 based on the pitch rate (vehicle pitch rate in Figure 10(B)) measured by the gyro sensor 3 mounted on the vehicle or rider 2.

Next, we will explain how to calculate the position change amount _ΔLx from the distance d. The in-vehicle device 1 stores the distance " _d0 " before the position of the lidar 2 changes, and can calculate the position change amount _ΔLx in the x direction of the lidar 2 by taking the difference with distance d, as shown in equation (27) below.

In this case, for example, after measuring the initial position L _{x 0} of the rider 2, the on-board unit 1 calculates the distance d ₀ when it first passes near the start or end of the slope, or when it passes over a bump on the road surface, and records this distance d 0 together with the initial position L _{x 0} of the rider 2 in the rider installation information IL. As a result, the on-board unit 1 can refer to the rider installation information IL and suitably calculate the position L _x and the change amount ΔL _x at the processing reference time based on equation (27).

(7) Calculation of the change in position in the y direction
Next, the method for calculating the change in position of the rider 2 in the y-direction will be explained. During vehicle rotation, the onboard unit 1 calculates the position _Ly of the rider 2 in the y-direction from the y-direction output value of the rider acceleration sensor 5, the y-direction output value of the vehicle body acceleration sensor 4, and the output value of the gyro sensor 3.

Figure 11 is a schematic diagram illustrating the effects on a vehicle during a turn. Generally, the velocity "V _G " and centripetal acceleration "α _G " of the vehicle's center of gravity during a turn are given by the following equations (28) and (29).

Here, in equation (28), "r _G " represents the distance from the pivot point to the vehicle's center of gravity. Also, the directions of velocity V _G and centripetal acceleration α _G are orthogonal. For a rigid body, the angular velocity is the same everywhere, so the velocity "V _A " and centripetal acceleration "α _A" at point A, the origin of the vehicle coordinate system, which is set at a distance of [A _x , A _y ] ^T from the center of gravity, are given by the following equations (30) and (31).

From Figure 11, the relationship between the centripetal acceleration _αA and the vehicle's lateral component " _αAy " is given by the following equation (32).

Therefore, the vehicle's lateral component α _Ay is expressed by the following equation (33).

The vehicle lateral component α _Ay on the left side of equation (33) can be measured as an output in the y-axis direction if an acceleration sensor is mounted at point A on the vehicle. Here, if we assume that rider 2 is mounted at a distance of [L _x , L _y ] ^T from point A, then equation (34), which is similar to equation (33), holds for the vehicle lateral component "α _{(L + A)y} " at the position of rider 2, and by using equation (34) and equation (33), we can obtain the following equation (35).

Therefore, the position L _y is expressed by the following equation (36).

Equation (36) shows 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's yaw rate, it will give the y-direction position of the rider 2 relative to the origin of the vehicle coordinate system. Therefore, the on-board unit 1 can determine the position _Ly of the rider 2 by calculating equation (36). The on-board unit 1 can also calculate the change amount _ΔLy by referring to the initial position _Ly0 stored in the rider installation information IL.

[Processing Flow]
Figure 12 is an example flowchart showing the procedure for correcting the output of lidar 2. The in-vehicle unit 1 repeatedly executes the process shown in Figure 12 at predetermined timings.

First, the on-board unit 1 calculates the change in roll angle _ΔLφ and the change in pitch angle _ΔLθ of the rider 2 from the output value of the rider acceleration sensor 5 while the vehicle is stopped or traveling at a constant speed on a level road (step S101). In this case, the on-board unit 1 calculates the roll angle _Lφ and pitch angle _Lθ of the rider 2 based on equations equivalent to equations (7) and (10), and calculates the difference between these and the roll angle _Lφ0 and pitch angle _Lθ0 recorded in the rider installation information IL as the change amounts _ΔLφ and _ΔLθ .

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

Next, the on-board unit 1 calculates the change in position _{ΔLz in the z direction from the z-direction measurement value of the lidar 2 that illuminates the road surface while the vehicle is stopped or traveling at a constant speed on a flat road (step S103). In this case, the on-board unit 1 calculates the average of the road surface measurement value z b} (k) 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 between this average and the road surface measurement value z _b0 (k) recorded in the lidar installation information IL under the same conditions (see equation (25)).

Next, the on-board unit 1 calculates the change in position in the x direction ΔLx by calculating the time interval Δt between the change in the z-direction measurement value of the rider 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 (step S104). In this case, the on-board unit 1 calculates the distance d by multiplying the time interval Δt by the vehicle's travel speed v, and calculates the difference between this distance _d0 , which is stored in the rider installation information IL beforehand, as the change amount _ΔLz (see equation (26)).

Next, the in-vehicle unit 1 calculates the change in position in the y-direction ΔLy from the y-direction output value of the rider acceleration sensor 5, the y-direction output value of the vehicle body acceleration sensor 4, and the output value of the gyro sensor ₃ while the vehicle is turning (step S105). Specifically, the in-vehicle unit 1 calculates the position _Ly at the processing reference time based on equation (36), and calculates the difference between this position and the initial position _Ly0 stored in the rider installation information IL as the change in position in the y-direction _ΔLy .

Next, the in-vehicle unit 1 determines whether any of the change amounts ΔL _φ , ΔL _θ , ΔL _ψ , ΔL _x , ΔL _y , and ΔL _z calculated in steps S101 to S105 are greater than or equal to a predetermined threshold (step S106). The aforementioned threshold is used to determine whether the measurement data of the rider 2 can be used after the correction processing of the measurement data of the rider 2 in step S108, which will be described later, and is set in advance, for example, based on experiments. Then, if any of the change amounts ΔL _φ , ΔL _θ , ΔL _ψ , ΔL _x , ΔL _y , and ΔL _z calculated in steps S101 to S105 exceed a predetermined threshold (step S106; Yes), the in-vehicle unit 1 stops using the output data of the target Rider 2 (i.e., using it for obstacle detection, vehicle position estimation, etc.) and outputs a warning via the information output unit 16 that it is necessary to perform alignment adjustment again on the target Rider 2 (step S107). This reliably suppresses the reduction in safety that may occur due to using measurement data of a Rider 2 whose attitude and position have been significantly misaligned due to an accident or the like. The thresholds mentioned above are just one example of the "determined amount" in this invention.

On the other hand, if none of the change amounts ΔL _φ , ΔL _θ , ΔL _ψ , ΔL _x , ΔL _y , and ΔL _z are greater than a predetermined threshold (step S106; No), the in-vehicle device 1 corrects each measured value of the point cloud data output by the lidar 2 based on these change amounts (step S108). In this case, the in-vehicle device 1 stores, for example, a map showing the correction amount for each magnitude of change amount, and corrects the measured values by referring to the map. Alternatively, the measured values may be corrected using a predetermined percentage of the change amount as the correction amount.

As described above, the in-vehicle unit 1 in this embodiment at least estimates the attitude of the rider 2 relative to the vehicle, which measures the distance to an object. It performs processing such as estimating the attitude of the rider 2 relative to the vehicle based on the detection results of the rider acceleration sensor 5 installed on the rider 2 when the vehicle is accelerating and decelerating. This allows the in-vehicle unit 1 to perform processing to convert the measurement data output by the rider 2 into the vehicle coordinate system with high accuracy, and to determine whether the rider 2 is usable or not.

[Variations]
The following describes suitable modifications for the examples. The following modifications may be applied in combination to the examples.

(Variation 1)
In step S108 of Figure 12, instead of correcting each measured value of the point cloud data output by the rider 2 based on the change amounts calculated in steps S101 to S105, the in-vehicle device 1 may convert each measured value to the vehicle coordinate system based on the estimated attitude and position of the rider 2 at the processing reference time calculated in steps S101 to S105.

In this case, the in-vehicle unit 1 may use the roll angle L _φ , pitch angle L _θ , yaw angle L _ψ , x-direction position L _x , y-direction position L _y , and z-direction position L _z calculated in steps S101 to S105 to convert each measured 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 then perform self-vehicle position estimation, automatic driving control, etc., based on the converted data.

In other examples, if each rider 2 is equipped with an adjustment mechanism such as an actuator for correcting the attitude and position of each rider 2, the onboard unit 1 may, instead of performing the process in step S108, control the adjustment mechanism to correct the attitude and position of the rider 2 by the amount of change calculated in steps S101 to S105.

(Variation 2)
The configuration of the driver assistance system shown in Figure 1 is an example, and the configuration of the driver assistance system to which the present invention can be applied is not limited to the configuration shown in Figure 1. For example, instead of having an in-vehicle unit 1, the driver assistance system may have the vehicle's electronic control unit perform the processing shown in Figure 12, etc. In this case, the lidar installation information IL is stored, for example, in a storage unit in the vehicle, and the vehicle's electronic control unit is configured to receive output data from various sensors such as the lidar 2.

1. In-vehicle unit 2. RIDER 3. Gyro sensor 4. Accelerometer for vehicle body 5. Accelerometer for RIDER 10. Map database

Claims (17)

An estimation device for estimating the orientation of a measuring unit that measures the distance to an object relative to a moving body,
An estimation device having an estimation unit that estimates the attitude of the measurement unit in the roll direction and pitch direction based on acceleration data output by an acceleration detection unit provided in the measurement unit when the moving unit is traveling or stopped at a predetermined speed. The estimation device according to claim 1, wherein the estimation unit estimates the yaw direction attitude of the measurement unit based on the acceleration data output by the acceleration detection unit when the moving body is accelerating and decelerating, and the estimated roll direction and pitch direction attitudes. The estimation device according to claim 1 or 2, wherein the estimation unit estimates the amount of change in the posture based on the estimated posture of the measurement unit and the posture of the measurement unit stored in the memory unit. The estimation device according to any one of claims 1 to 3, wherein 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. 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 the storage unit. The estimation device according to claim 3 or 5, further comprising a correction unit that corrects the measurement data output by the measurement unit based on the aforementioned change. The estimation device according to claim 3 or 5, further comprising a stop control unit that stops processing based on measurement data output by the measurement unit when the amount of change exceeds a predetermined amount. An estimation device for estimating the orientation of a measuring unit relative to a moving body, which measures the distance to an object,
The measurement unit has an estimation unit that estimates the attitude of the measurement unit relative to the moving body based on the detection result of the acceleration detection unit provided in the measurement unit when the moving body is moving while accelerating and decelerating.
The estimation unit is an estimation device that estimates the position of the measuring unit in the longitudinal 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, or a bump on the road surface, is measured by the measuring unit. The estimation device according to claim 8, wherein the estimation unit calculates the distance based on the time difference between the change in data output by the tilt detection unit, which detects the tilt in the pitch direction of the moving body, and the change in measurement data output by the measurement unit. An estimation device for estimating the orientation of a measuring unit relative to a moving body, which measures the distance to an object,
The measurement unit has an estimation unit that estimates the attitude of the measurement unit relative to the moving body based on the detection result of the acceleration detection unit provided in the measurement unit when the moving body is moving while accelerating and decelerating.
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 the left-right acceleration data of the moving body output by the acceleration detection unit during the rotation of the moving body, the 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. A control method performed by an estimation device that estimates the attitude of a measuring unit, which measures the distance to an object, relative to a moving object,
A control method comprising an estimation step of estimating the attitude of the measurement unit in the roll direction and pitch direction based on acceleration data output by an acceleration detection unit provided in the measurement unit when the moving unit is traveling or stopped at a predetermined speed. A control method performed by an estimation device that estimates the attitude of a measuring unit, which measures the distance to an object, relative to a moving object,
The measurement unit has an estimation step of estimating the attitude of the measurement unit relative to the moving body based on the detection result of the acceleration detection unit provided in the measurement unit when the moving body is moving while accelerating or decelerating.
The estimation step is a control method that estimates the position of the measuring unit in the longitudinal 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, or a bump on the road surface, is measured by the measuring unit. A control method performed by an estimation device that estimates the attitude of a measuring unit, which measures the distance to an object, relative to a moving object,
The measurement unit has an estimation step of estimating the attitude of the measurement unit relative to the moving body based on the detection result of the acceleration detection unit provided in the measurement unit when the moving body is moving while accelerating or decelerating.
The estimation step is a control method that estimates the position of the measurement unit in the left-right direction of the moving body based on the left-right acceleration data of the moving body output by the acceleration detection unit during the rotation of the moving body, the left-right acceleration data 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. A program executed by a computer to estimate the orientation of a measuring unit that measures the distance to an object relative to a moving object,
A program that causes the computer to function as an estimation unit that estimates the attitude of the measurement unit in the roll direction and pitch direction based on acceleration data output by an acceleration detection unit provided in the measurement unit when the moving unit is traveling or stopped at a predetermined speed. A program executed by a computer to estimate the orientation of a measuring unit that measures the distance to an object relative to a moving object,
When the moving body is accelerating and decelerating, the computer functions as an estimation unit that estimates the attitude of the measurement unit relative to the moving body based on the detection result of the acceleration detection unit provided in the measurement unit.
The estimation unit is a program that estimates the position of the measurement unit in the longitudinal 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, or when a bump on the road surface is measured by the measurement unit. A program executed by a computer to estimate the orientation of a measuring unit that measures the distance to an object relative to a moving object,
When the moving body is accelerating and decelerating, the computer functions as an estimation unit that estimates the attitude of the measurement unit relative to the moving body based on the detection result of the acceleration detection unit provided in the measurement unit.
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 the left-right acceleration data of the moving body output by the acceleration detection unit during the rotation of the moving body, the left-right acceleration data 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. A storage medium storing the program described in any one of claims 14 to 16.