Autonomous vehicle control based on receding horizon method

Autonomous vehicle control based on receding horizon method

IAV2004 - PREPRINTS 5th IFAC/EURON Symposium on Intelligent Autonomous Vehicles Instituto Superior Técnico, Lisboa, Portugal July 5-7, 2004 AUTONOMOU...

248KB Sizes 0 Downloads 30 Views

IAV2004 - PREPRINTS 5th IFAC/EURON Symposium on Intelligent Autonomous Vehicles Instituto Superior Técnico, Lisboa, Portugal July 5-7, 2004

AUTONOMOUS VEHICLE CONTROL BASED ON RECEDING HORIZON METHOD B. Kim*, D. Necsulescu**, J. Sasiadek*** *Defence R&D Canada-Ottawa **Department of Mechanical Engineering, University of Ottawa, ***Department of Mechanical & Aerospace Engineering, Carleton University

Abstract: This paper presents the model, simulation and experimental results of model predictive control (MPC) for motion control of an autonomous vehicle. The simulation and experiments were designed for mining application. In mining industry robotic vehicles have to navigate in tunnels and often have to overcome inclined and rough surfaces. In this case, the MPC was applied using a three-dimensional dynamic model of the robot vehicle. The purpose is to achieve autonomous vehicle motion with the avoidance of wheel-ground slippage or loss of contact while, at the same time, conserving the geometric path planning results by modifying the input commands. Tests were carried out for a vehicle moving on an inclined plane. Results from testing MPC plus feedback linearization controllers are presented and compared with results for controllers using only feedback linearization. Simulation results are presented for a three-wheeled vehicle moving in conditions that permit to illustrate the performance of the proposed controller. The results obtained show that the MPC is an efficient method that contributes to accurate control and navigation of an autonomous vehicle. The results were used to evaluate the performance and operational capabilities of an autonomous unmanned mining vehicle. Copyright © 2004 IFAC Keywords-- Robotics, Autonomous Vehicle, Model Predictive Control

1 INTRODUCTION

requirements for the controller of the vehicle, in particular with regard to vehicle dynamics. For autonomous motion control of unmanned vehicles, an operational space control approach is needed. Such an approach has to be able to generate and correct the trajectory of the vehicle in order to avoid collisions with unexpected obstacles and to avoid vehicle slippage and tip-over. The control approach has to take into account the contact forces between the wheels and the ground. For achieving autonomy, a dynamics based control approach was formulated for a three-wheeled vehicle with front wheel driving and steering. Motion control of the vehicle in operational space is greatly facilitated by the exact input-output feedback linearization of vehicle dynamics [1]. The linearization also permits the development of a real-time collision avoidance scheme using artificial potential field approach thus enhancing autonomy of the vehicle. The sufficient smoothness condition for applying feedback linearization has to be however continuously observed and this requires the avoidance of actuators torque saturation, wheel-ground longitudinal and

The large percentage of operational costs in mining industry is related to the infrastructure maintenance. The cost of ventilation and making underground condition bearable for miners require major investments. These investments can be lowered by application of automation, especially the unmanned, autonomous mining vehicles. Autonomous motion of unmanned vehicles requires an operational space control approach that is able to generate and correct the trajectory of the vehicle in order to avoid collisions with unexpected obstacles, slippage and tip-over. Slippage, loss of contact between the wheels and the ground and vehicle tip-over require to take into account the contact forces between the wheels and the ground. In recent years a variety of autonomous vehicles were developed mostly using kinematics based control. At the present time the trend is to further enhance the features for autonomous operation of vehicles in an open quasi-flat field or on the factory floor. Motion on uneven road conditions imposes complex

775

lateral slippage, and tip-over of the vehicle. Also, the smoothness condition has to be observed for motion on horizontal plane as well as inclined surfaces.[2] The control approach presented in this paper is based on a three dimensional dynamic model, using Newtonian dynamics, of a three wheeled autonomous vehicle with a front wheel steering and driving. A three-dimensional dynamic model of the vehicle is strongly recommended instead of using twodimensional model for the compensation of the inertia of the vehicle. The proposed dynamics model predictive control approach provides the modification of the input commands such that the geometric path planning result is conserved and the smoothness condition for exact linearization is not violated. The approach presented in this paper is suitable for the wheel-ground slippage and tip-over avoidance of the three-wheeled vehicle for inclined plane motion. Test results have the purpose of verifying the proposed controller for simple situations in preparation for the future work in which corrective actions are taken by the controller to avoid smoothness condition violations for more difficult terrain conditions. In order to facilitate the development of the dynamic model and the control law, multiple reference frames should be chosen for wheeled vehicles [3]. One inertial frame, M-Q-P, has the plane M-Q placed on the horizontal ground surface, while the second inertial frame, N-Q-R, contains the inclined plane NQ of the motion of the vehicle. The two inertial frames have a common axis Q and there is an angle between M-Q and N-Q . One moving reference frame X-Y-Z is attached to the vehicle structure (X-axis along the vehicle, Y-axis parallel to the rear axle, and Z-axis perpendicular to X-Y plane originated at O in the vehicle's centre of mass, as shown in Fig. 1). The following coordinate transformation can be defined:Three other moving frames, Xi-Yi-Zi (i=1,2,3), with origins Oi, are attached to the three wheels, with Zi parallel to Z . X1

Q Y1 X2 Y2

Y

* O1

2 Y3

l/2

2 MPC CONTROL ALGORITHM The autonomous vehicle is analyzed for the motion control on an inclined plane subject to wheel slippage and body tip-over avoidance. The vehicle control system is similar to [5]. The overall control scheme contains algorithms required for verifying slippage and tip-over conditions. The external loop position controller and path y(x) planner are defined in operational space. A model based predictive controller is implemented for the Kext which represents the proportional control gain for the angular velocity command 1 (c) of the front wheel. The inner loop controller involves an exact inputoutput linearization controller in curvilinear space (s*) and a linear controller with feedback from the measurement of steering angle and velocity,  and , as well as of the front wheel angular velocity, 1. The model based predictive control scheme was designed specifically for avoiding slippage and loss of wheel-ground contact. Feedback linearization is a powerful technique for facilitating the design of the controller. Moreover, obstacle avoidance algorithms are easier to design for linearized vehicles [5]. Basically, it consists of applying a nonlinear feedback to the system to compensate its nonlinearity, so that the dynamics of the new composed system appear linear. The linearization procedure requires that all of the necessary variables be estimated accurately. Some conditions must be fulfilled to be able to use feedback linearization, first of all the smoothness of the system. Unfortunately, almost no real system is smooth outside a limited operating region. In particular, the three wheel vehicle used for this paper has saturation limits for the driving and steering torques, d and s, respectively. Under feedback control this means that the input signal to the actuator has to be between the bounds (i.e., in the unsaturated

X O

O2

The coordinate transformations give the position (N, Q, R) in N-Q-R plane of a point defined by the position (X, Y, Z) in X-Y-Z plane, where the 2 is the orientation angle between N and X axis of the two inertial frames, N-Q-R and X-Y-Z. To determine the moving reference frame’s position and orientation with regard to the inertial frame, the (algebraic) vectors for absolute positions of the origins and the orientation angles of X, X2, X3 and of the X1 with regard to N are defined [5]. For inclined plane motion analysis, only N-Q and XY components are needed, i.e., 8 components for the 4 position vectors. The vehicle dynamics for the three wheeled vehicle was derived on the basis of general principles of multi-body systems with assumptions presented in [1].

b-c

X3 c

l/2

O3

N

Figure 1. The moving reference frames of the autonomous vehicle.

776

region) to satisfy smoothness condition. In the saturated regions the input command can not be achieved. Feedback linearization tends to increase the dynamic range of the input, and therefore may reduce the unsaturated region and increase the danger of running into the bounds. Reaching the bounds changes the structure of the controlled system, destroys the linearity of the composite system, and, can even lead the plant to instability. Therefore, it may be worth limiting the range of allowable reference values so that the state trajectory does not leave the region in which smoothness condition is satisfied. The sufficient smoothness condition for applying feedback linearization has to be continuously observed and this requires the avoidance of actuators torque saturation, wheel-ground longitudinal and lateral slippage and tip-over of the vehicle for motion on horizontal plane as well as on inclined surfaces . Model Predictive Control (MPC) provides a unified solution to the problems of free motion and contact motion controls. In the model predictive control, the knowledge is represented by analytical models of the autonomous vehicle and the environment. These models are used to predict, using current measurements, the motion variables 1(t) and (t), over a receding horizon extending from the current time up to a fixed interval into the future. The actual control commands, 1 (c) and (c), result from the control objective over the receding horizon. In order to define how well the predicted process output tracks the reference trajectory, a criterion function  is used. γ 1 ( N d  N k i ) 2  γ 2 (Qd  Qk  i ) 2   k i k i   γ 3 ( N• d  N• k i ) 2  γ 4 (Q• d  Q• k i ) 2   k  k i k i   i 0 γ 5* (G z1 ) 2  γ 6* ( Gz 2 ) 2  γ 7* (G z 3 ) 2  k i k i k i   Nh



u*  arg min  k *

for

0, γ 6*   γ 6 ,

for

 0, γ 7*   γ 7 ,

for

*

The controller output sequence u = Kext over the prediction horizon, Nh, is obtained by minimization of  k Then u* is optimal with respect to the criterion function that is minimized. In this paper the Model based Predictive Control, as shown in Kim, Necsulescu, Sasiadek, [5], is used for avoidance of wheel-ground longitudinal and lateral slippage and loss of wheel-ground contact of the vehicle for motion on inclined surfaces. The commands 1(c) and (c) are modified such that the geometric path planning result is conserved and the smoothness condition for exact linearization is not violated. 3. DESCRIPTION OF MOBILE ROBOT The schematic diagram of the experimental setup has been shown in [5] illustrates the components used in this study. The autonomous vehicle, dSPACE digital signal processor, SYSTEM 7 direct drive motor driver, A/D, D/A, and encoder cards, batteries, analog filters, and a PC are the main components. There are two versions of the autonomous vehicle designed and constructed in our laboratory. Both versions have a tricycle configuration equipped with a driving and steering front wheel and two idle rear wheels. The first version has been developed for experimental testing of the sensor fusion of odometers and accelerometers in case of vehicle slippage. In the first version, two direct drive (brushless) DC motors were installed at the front wheel of the robot, one on top of the steering assembly structure for steering purpose and the other on right hand side of the wheel for driving. A counter balancing mass is attached on the other side of the driving motor. This version of autonomous vehicle has a frame with dimension of 0.4 x 0.3 x 0.4 m. The signals between SYSTEM 7 driver and dSPACE are in analog format. The dSPACE DSP generates the torque commands outputs (d, s) and transmits them to the motors via DS2101 D/A converter board. dSPACE controller receives the angular position signals ( 2d, 2s) from DS3001 incremental encoder board and communicates off line with a PC. The servomotor type is an important factor in autonomous vehicle design. Desirable servos are lightweight, compact, easily integrated, efficient, controllable, and maintenance free. Especially, for unmanned vehicle, easy servo maintenance is a desirable feature. Although the direct drive motor outperforms the brushed DC motor, as far as torque control is concerned, it is noted that it is not suitable for autonomous vehicle application because of its

(1)

where, Nh is the receding horizon and (i (i =1~7) are weight factors selected during the MPC design. The weights, are defined as function of sign of Gz , where Gzi (i=1,2,3) are the vertical wheel-ground contact forces of the wheels. The variables N and Q are defined in Fig. 1. Usually the control objectives are given in terms of the minimization of an optimization criterion defined over the receding horizon 0, γ 5*   γ 5 ,

(2)

u

G z1  0

otherwise Gz2  0

otherwise G z3  0

otherwise

777

heavy and balky motor driver unit that increases the size and weight of the vehicle. It was also observed that the thickness of vehicle frame affects significantly the dynamic characteristics of the rigid body assumption. The thickness of the vehicle frame of above version is only 3 mm. These two main factors gave enough reasons to construct the second generation vehicle with thicker and more sturdy frames and with DC motors replacing direct drive motors. A second autonomous vehicle was also used. The vehicle has the dimensions 0.51 x 0.51 x 0.53 m and the mass of 24.5 kg with two batteries installed. The servomotors are DC motors. These DC servomotors have 75.1:1 planetary gears and have a torque output of max. 0.76 Nm. Power for the motors is supplied by two 12 V, 10 Ah batteries connected in series. Each servomotor has a built-in optical incremental encoder which has 1000 steps per revolution at motor shaft resulting in 75100 steps per revolution for output shaft. The frame is made of ½" x 3" aluminum stock for strength. The plate metal used for the box near the rear wheels is made of 3/32" thick aluminum. The dry friction of the DC motors at motor shaft level was identified as approximately 0.0085 Nm. 4. RESULTS The presented results were obtained for two types of control approaches,:

3

Q (m)

2

1

0

-1 0

1

2

3

N (m )

Figure 2. Planned path (>) and the trajectory (B) of the vehicle with Kext = 3.0 3

Q (m)

2

1

0

-1 0

1

2

3

N (m )

Figure 3. Planned path (>) and the trajectory (B) of the vehicle with Kext = 4.0 Figure 3 shows the planned path and the trajectory of the vehicle for Kext increased to 4.0 while other conditions are same as before. The figure shows a prematurely stopped trajectory of the vehicle when attempting to follow the same planned path. The vehicle stops far from the desired destination when one of the vertical forces of the three wheels, Gz3, changes sign after 8 seconds and becomes negative, as shown in Fig. 4. This phenomenon means that a loss of contact has occurred. The Figure 4 shows that the vehicle negotiated the first sharp corner but the centrifugal forces of the vehicle were already out of range for the safe region for stable wheel-ground contact. After the loss of contact, the results can not be used to identify if tipping over occurs, because the model was limited to the case of stable wheel-ground contact and the model based controller has the actual purpose of avoiding tipping over.

A. input-output linearization with linear controller (IOL) B. model predictive control (MPC) plus inputoutput linearization with linear controller A. Results for input-output linearization with linear controller A shown in Fig. 2, the planned path and the trajectory of the vehicle when Kext is equal to 3.0 for initial posture of Ni=0(m), Qi=0(m), and i=-15, and desired posture of Nd=2.0(m), Qd=2.0(m), and d = 90. The figure shows a curved trajectory followed by the vehicle, parallel to the planned one. Figure 3 also shows that the vehicle didn’t reach the target. This result can be explained by the fact that only one control variable, in this case d, can be closed loop controllable while the other variable s, is open loop controllable because of the non-holonomic constraint. The control variable d is function of the curvilinear position error.

100

80

3z

G (N)

60

40

20

0

-2 0 0

10

20

30

40

50

tim e (s e c )

Figure 4 Rear wheel No.3 vertical force transient Gz3(t)

778

B. Results for model predictive control plus inputoutput linearization with linear controller

3

2

N (m)

The Model Predictive Controller (MPC) is used for finding safe Kext by predicting the violation of the loss of contact condition in combination with the Input-Output Linearization (IOL) controller [5]. This paper is an extension of work presented in that paper. The trajectory, with a significant curvature, is negotiated successfully by the vehicle with the Model Predictive Controller (Fig. 5) while, for Input-Output Linearization scheme (Fig. 3), the vehicle was shown to be in danger of being tipped-over.

1

0

0

1 0

2 0

3 0

4 0

5 0

tim e ( s e c )

Figure 7 The transient of N position (planned (>) and generated (B))

3

2

Q (m)

3 1

2

Q (m)

0

1 -1 0

10

20

30

40

50

ti m e (s e c )

0

Figure 8. The transient of Q position (planned (>)and generated (B)) In Figures 9 and 10, the orientation angle (2) and the steering angle (*) of the vehicle, show the smooth variation and reach the desired values.

-1 0

1

2

3

N (m )

Figure 5. Planned path (>) and the trajectory (B) of the vehicle with Kext = 4.0 using MPC The results shown in Fig. 5, obtained with the MPC/IOL controller shown in Fig.2, proved that this controller provides better performance than a controller using only input-output linearization, which generated the results shown in Figure 3. This is also confirmed by the results shown in Fig. 6 for the MPC/IOL control. In this case the vertical force Gz3 does not change its sign any more and remains positive. This is an important change in comparison with the case shown in Fig 4

3

 (rad)

2

1

0

-1 0

10

20

30

40

50

ti m e (s e c )

Figure 9. Transient of the orientation angle () of the vehicle

1 0 0 2

8 0

6 0

 (rad)

3z

G (N)

1

4 0

0

2 0

0 -1 0

0

1 0

2 0

3 0

4 0

1 0

2 0

tim e

5 0

3 0

4 0

5 0

(s e c )

tim e (s e c )

Figure 10. Transient of the steering angle () of the vehicle

Figure 6 Wheel No. 3 vertical force Gz3 transient for MPC and IOL control Results, shown in Figures 7 to 15, also confirm the improvement achieved by combining MPC and IOL control. Figures 7 and 8 display smooth time variation of the positions N and Q of the vehicle.

Figure 11 shows that angular velocity of the front wheel changes with a damped oscillation at the starting of motion and a sudden decrease when avoiding the loss of wheel-ground contact.

779

4

7

3

6 2

5

 (Nm)

0

s

3

-1



 rad/sec)

1

4

2

-2

1

-3 -4

0

0

10

20

30

40

50

ti m e (s e c ) 0

10

20

30

40

50

t im e ( s e c )

Figure 15

Figure 11 Front wheel angular velocity transient

These results confirm the improvement in the performance of the vehicle under MPC and IOL in comparison with the corresponding results for IOL only. 4. CONCLUSIONS The dynamic model based controller testing results, for a wheeled vehicle moving on an inclined plane, show that the events of wheel slippage and vehicle tip-over can be predicted and avoided for various motion conditions. The proposed approach can be used for improving the structural design of the vehicle and for the development and tuning the controller by testing the dynamic behaviour of the vehicle for various conditions of the road and in particular for various slopes. The results are particularly relevant for the development of future autonomous mobile vehicles and for off-road teleoperated vehicles. The proposed predictive control approach proved to be a viable alternative for enhancing the autonomy of robotic vehicles. REFERENCES [1]D. Necsulescu, M. Eghtesad, S. Kalaycioglu, “Dynamic Based Linearization and Control of an Autonomous Mobile Robot”, Proc. of the 2nd Biennial European ASME Conf. on Eng. Systems Design & Analysis, London, England, July 4-7, 1994. [2] D. Necsulescu, B. Kim, A. Villien, “Simulation of an Autonomous Mobile Vehicle Motion on Inclined Surfaces”, Proc. of the 9th European Simulation Multiconference, Prague,, June 5-7, 1995. [3] T.J. Graettinger, B.H. Krogh, “Evaluation and Time-Scaling of Trajectories for Wheeled Mobile Robots”, Trans. of ASME: Journal of DSMC, Vol.111, June 1989, pp. 222-231. [4] C. Canudas de Wit, O.J. Sordalen, “Exponential Stabilization of Mobile Robots with Nonholonomic Constraints,” IEEE Trans. on Automatic Control, Vol. 37, No. 11, pp 1791-1797, Nov. 1992. [5]B.Kim.,D.Necsulescu,.,J.Sasiadek, “Autonomous Vehicle Model Predictive Control”, Proc. of the IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM’01), July, 2001, Como, Italy

Figures 12, and 13 (and Fig. 9) show that the vertical forces between the wheels and the ground are maintained in positive area such that no loss of contact occurs. 100

90

1z

G (N)

80

70

60

50 0

10

20

30

40

50

t im e ( s e c )

Figure 12 Front wheel No 1 vertical force transient 90

80

2z

G (N)

70

60

50

40

30 0

10

20

30

40

50

t im e (s e c )

Figure 13 Front wheel No.2 vertical force transient Figures 14 and 15 show torque variation in time for driving and steering wheels of the vehicle.

12 10 8

d

 (Nm)

6 4 2 0 -2 -4 -6 0

10

20

30

40

Steering wheel torque transient

50

t im e ( s e c )

Figure 14 Driving wheel torque transient

780