JP5591185B2 - Vehicle position and orientation measurement device - Google Patents

Description

  The present invention relates to a vehicle position / orientation measuring apparatus for measuring the position / orientation of a vehicle in an arbitrary inertial coordinate system.

In the conventional vehicle position and orientation measurement apparatus, it is assumed that the wheel diameter does not change, the moving distance difference and the average moving distance obtained from the rotation speeds of the non-steered left and right wheels, and the tread does not change. The position and orientation were measured (for example, Patent Document 1 and Non-Patent Document 1).
Furthermore, by introducing a correction factor depending on the sum of the wheel speeds and / or the difference between the speeds of the two wheels and / or the time for a given rotation of the wheel or the time to reach or reach the rotational state, A position / orientation measurement method that responds to tread changes that occur during running on a curve has also been proposed (for example, Patent Document 2).

JP 61-274214 A JP-T 59-502153

C. Ming Wang, “Location Estimation and Uncertainty Analysis for Mobile Robots”, IEEE International Conference on Robotics and Automation, pp. 1230-1235, 1988 (Section 2) Masaaki Masaya et al., “Anti-Crosstalk Noise Canceller Based on Nonlinear Sequential Least Squares Method”, IEICE Transactions A, pp. 162-169, 2002 " Takashi Tanizaki, “Kalman filter and adaptive signal processing”, pp. 42-51, Corona, December 2005

  In such a vehicle position / orientation measurement device, when there is a difference in the effective diameter of the left and right non-steering wheels due to differences in the tire air pressure between the left and right, an error occurs in the movement distance calculation of the left and right wheels. There was a problem that posture measurement accuracy deteriorated.

  The present invention has been made to solve the above-described problems, and an object of the present invention is to provide a vehicle position / orientation measurement apparatus that accurately measures the position and orientation of a vehicle.

  The present invention is a vehicle position / orientation measurement device for detecting the position and orientation of a vehicle that steers front wheels, and detects a steering angle detection means for detecting a steering angle and wheel speeds of front, rear, left, and right wheels of the vehicle. Wheel speed detecting means, an actual steering angle calculating means for calculating an actual steering angle based on an output from the steering angle detecting means, a rear tread and a rear wheel wheel radius based on the actual steering angle and the wheel speed, respectively. Tread and wheel radius estimating means for calculating the estimated value of the vehicle, and vehicle position and orientation calculating means for calculating the position and orientation of the vehicle based on the estimated value and the wheel speed of the rear wheel, and for any inertial coordinate system A vehicle position / orientation measurement apparatus or the like that measures the position and orientation of a vehicle.

  According to this invention, since the tread / wheel radius estimating means is provided, not only when the rear tread changes during traveling, but also when the wheel diameter changes due to a decrease in tire air pressure, etc. The posture can be measured with high accuracy.

1 is a configuration diagram of a vehicle position / orientation measurement apparatus according to Embodiment 1 of the present invention; FIG. It is explanatory drawing for demonstrating the principle of this invention. It is a block diagram of the vehicle position and orientation measurement apparatus according to Embodiment 2 of the present invention.

  DESCRIPTION OF EMBODIMENTS Hereinafter, a vehicle position / orientation measuring apparatus according to the present invention will be described with reference to the drawings according to each embodiment. In each embodiment, an example of a device that measures the position and orientation of a vehicle that steers the front wheels is shown, and the same or corresponding parts are denoted by the same reference numerals, and redundant description is omitted.

Embodiment 1 FIG.
1 is a block diagram of a vehicle position and orientation measurement apparatus according to Embodiment 1 of the present invention. This vehicle position / orientation measuring apparatus includes a steering angle sensor 1 for detecting a steering angle θ, a wheel speed sensor 2 (front left wheel), and a wheel speed sensor 3 (right) for detecting the wheel speed of each wheel of the vehicle. Front wheel), wheel speed sensor 4 (left rear wheel), and wheel speed sensor 5 (right rear wheel). The wheel speed sensors 2, 3, 4, and 5 are wheel speed sensors having a known configuration that outputs a detection pulse each time a corresponding wheel rotates by a predetermined angle.

The actual steering angle calculator 6 substitutes the steering angle θ detected by the steering angle sensor 1 into the relational expression (1) obtained in advance through experiments, and calculates actual steering angles δ _Fl and δ _Fr.

The tread / wheel radius estimator 7 outputs the actual rudder angles δ _Fl , δ _Fr output from the actual rudder angle calculator 6 and the respective pulse signals P _Fl , P output from the wheel speed sensors 2, 3, 4, 5. _{fr, P Rl,} from _{P Rr,} estimates front tread _{T R,} the rear tread _{T R,} the left front wheel radius _{r Fl,} right front wheel radius _{r fr,} the rear left wheel radius _{r Rl,} the right rear wheel radius _{r Rr.} The principle will be described with reference to FIG. Assuming that the sideslip angles of the left front wheel 11, the right front wheel 12, the left rear wheel 13, and the right rear wheel 14 of the vehicle are all 0,

Holds. However,
v _Fl , v _Fr , v _Rl , v _Rr : movement speeds of the wheels 11, 12, 13, 14 respectively v _x , v _y : x-axis component and y-axis component of the center-of-gravity velocity, respectively (the inertial coordinate system is arbitrary)
(Dot) γ: attitude angle of the vehicle (yaw angle: angle formed with the x-axis of the vehicle direction on the reference coordinates xy) change amount of γ per unit time (angular velocity)
L _F : Length between front wheel axis center (center of line connecting left front wheel 11 and right front wheel 12) and vehicle center of gravity c.g. L _R : Rear wheel axis center (left rear wheel 13 and right rear wheel 14 It is the length between the center of the connecting line) and the vehicle center of gravity c.g. For example, when it is considered that the vehicle is performing a steady motion at a low speed as in reverse parking, such an assumption is established.

  Furthermore, if we organize the above formula,

Is obtained. L indicates a foil base. By the way, the number of output pulses P _F1 , P _Fr , P _R1 , P _Rr from the wheel speed sensors 2, 3, 4, 5 and the moving speeds v _F1 , v _Fr , v _{Rl of the} wheels 11, 12, 13, 14 , V _Rr , the relational expression

Holds. However, N _Fl , N _Fr , N _Rl , and N _Rr are the number of pulses when each wheel makes one rotation, T is the sampling time, and n is a positive integer. Therefore, in the steady motion state considered here, the steering speed is sufficiently low,

((Dot) δ _Fl , (dot) δ _Fr is the change amount per unit time (actual rudder angular velocity) of the actual rudder angle δ _Fl , δ _Fr ) (9), (10),

Holds. Therefore, from this relational expression (12), a relational expression that satisfies the error amount between the wheel radius and the true value, which is the actual value during running of the tread, and the nominal value (nominal value) published by the manufacturer is derived. By applying a parameter estimation method using a non-linear sequential least square method, an extended Kalman filter or the like to the relational expression, from the actual steering angles δ _Fl , δ _Fr and the number of pulses P _Fl , P _Fr , _{PR Rl} , _{PR Rr} Thus, it is possible to obtain an estimated value of the wheel radius and tread obtained by adding an error correction to the nominal value.

For example, the wheel radii and the true values of the treads r _Fl , r _Fr , r _Rl , r _Rr , T _F , and T _R are nominal values r ^* _Fl , r ^* _Fr , r ^* _Rl , r ^* _Rr , T ^* _F , Using T ^* _R and scale factor errors Δr _Fl , Δr _Fr , Δr _Rl , Δr _Rr , ΔT _F , ΔT _R ,

Since can be expressed as, by substituting equation (13) into equation (12), the error amount _{Δr Fl, Δr Fr, Δr Rl} , Δr Rr, ΔT F, the relationship to be satisfied by the [Delta] T _R obtained. For the non-linear sequential least square method and the extended Kalman filter, see Non-Patent Documents 2 and 3 above, respectively.

The vehicle position / orientation calculator 8 includes rear wheel (left rear, right rear) wheel radius and rear tread estimated values r _Rl ^† , r _Rr ^† , T _R ^† output from the tread / wheel radius estimator 7, The plane position and attitude of the vehicle are calculated from the pulse signals P _Rl and P _Rr output from the wheel speed sensors 4 and 5 of the wheels (left rear and right rear), respectively. The principle will be described with reference to FIG. From the kinematic model of the vehicle, the position x, y of the rear wheel axle center is a relational expression.

((Dot) x, (dot) y is the amount of change per unit time of the x and y coordinate values). Therefore, when the relational expression obtained by substituting the relational expressions (6) and (7) into the relational expression (14) and the relational expression (6) are discretely approximated with the sampling time T (0 <T << 1), the rear wheel shaft is obtained. Difference equation to be satisfied by center position x (nT), y (nT) and posture angle (yaw angle) γ (nT)

Is obtained (see Non-Patent Document 1 above). However,

It is. Furthermore, in the steady motion state considered here, the vehicle acceleration is sufficiently small,

From the relational expression (10),

However, since the rear wheel wheel radius and the true value of the rear tread are unknown, they are output from the tread / wheel radius estimator 7, that is, the rear wheel wheel radius and the estimated value of the rear tread r _Rl ^† , r. _If replaced with _Rr ^† and T _R ^† , the relational expression (19) is obtained.

Accordingly, by appropriately setting the initial values according to the setting of the inertial coordinate system describing the vehicle motion, the positions x (nT), y (nT) and the posture angle (yaw angle) γ in an arbitrary inertial coordinate system. (nT) can be calculated based on the relational expressions (15) and (19) from the rear wheel wheel radius and rear tread estimated values r _Rl ^† , r _Rr ^† , T _R ^† and pulse signals P _Rl , P _Rr. .

  According to the above configuration, the tread / wheel radius estimator 7 estimates the wheel radius and tread online, and the vehicle position / orientation calculator 8 calculates the position and orientation of the vehicle based on the result. Even when the tread or rear wheel diameter changes and differs from the nominal value, the position and posture of the vehicle can be measured with high accuracy.

Embodiment 2. FIG.
FIG. 3 is a block diagram of a vehicle position / orientation measurement apparatus according to Embodiment 2 of the present invention. This device has a wheel speed sensor (left front wheel) 2, a wheel speed sensor (right front wheel) 3, a wheel speed sensor (left rear wheel) 4, a wheel speed sensor (right rear wheel) for detecting the wheel speed of each wheel of the vehicle. No sensors other than 5) are required. According to this configuration, the steering angle sensor 1 and the actual steering angle calculator 6 shown in FIG. 1 are not required, and the vehicle position and orientation measurement accuracy is not deteriorated due to a detection error based on the steering mechanism.

The principle of the tread / wheel radius estimator 7a shown in FIG. 3 will be described. If δ _Fl and δ _Fr are eliminated from the relational expression (12),

Holds. When a parameter estimation method such as a non-linear sequential least square method or an extended Kalman filter is applied to the relational expression that satisfies the error amount between the wheel radius and the true value of the tread and the nominal value that can be derived from this relational expression (20), From only P _Fl , P _Fr , P _Rl , and P _Rr , it is possible to obtain the estimated values of the wheel radius obtained by adding error correction to the nominal value, or the tread, or both the wheel radius and the tread.

  1 Steering angle sensor (steering angle detection means), 2, 3, 4, 5 Wheel speed sensor (wheel speed detection means), 6 Actual steering angle calculator (actual steering angle calculation means), 7, 7a Tread / wheel radius estimation (Tread / wheel radius estimating means), 8 vehicle position / orientation calculator (vehicle position / orientation calculating means), 11, 12, 13, 14 wheels.

Claims (2)

A vehicle position and orientation measurement device for detecting the position and orientation of a vehicle that steers front wheels,
Steering angle detection means for detecting the steering angle;
Wheel speed detection means for detecting the wheel speed of the front, rear, left and right four wheels of the vehicle;
Actual steering angle calculation means for calculating an actual steering angle based on the output from the steering angle detection means;
A tread / wheel radius estimating means for calculating an estimated value of each of the rear tread and the rear wheel radius based on the actual steering angle and the wheel speed;
Vehicle position and orientation calculation means for calculating the position and orientation of the vehicle based on the estimated value and the wheel speed of the rear wheel;
A vehicle position / orientation measurement apparatus for measuring a position and an attitude of a vehicle with respect to an arbitrary inertial coordinate system. A vehicle position and orientation measurement device for detecting the position and orientation of a vehicle that steers front wheels,
Wheel speed detection means for detecting the wheel speed of the front, rear, left and right four wheels of the vehicle;
A tread / wheel radius estimating means for calculating respective estimated values of the rear tread and the rear wheel radius based on the wheel speed;
Vehicle position and orientation calculation means for calculating the position and orientation of the vehicle based on the estimated value and the wheel speed of the rear wheel;
A vehicle position / orientation measurement apparatus for measuring a position and an attitude of a vehicle with respect to an arbitrary inertial coordinate system.

Images