A Simulation Model to Evaluate and Verify Functions of Autonomous ...

A Simulation Model to Evaluate and Verify Functions of Autonomous Vehicle Based on Simulink® Jie Zhou*, Hui Chen, and Caijing Xiu Tongji University, School of Automotive Studies, Cao An road. 4800, 201804 Shanghai, China [email protected], [email protected], [email protected]

Abstract. The paper presents a simulation model of autonomous vehicle based on Matlab/Simulink®, which is simple and efficient. Concerning about the relationship among autonomous vehicle, road, and obstacle, it forms a ‘Vehicle— Sensor—Controller’ closed-loop control system. The model provides a platform to design and validate the control logic of Local Path Planning, and to design post-process of the raw data from sensors. The paper also presents the simulation results of a PID steering controller for lane following, Artificial Potential Field (APF) method for collision avoidance, and the dynamic planning for overtaking when the obstacle is moving. The simulation model is verified to be effective by a lane change experiment. Keywords: Autonomous Vehicle, Local Path Planning, Simulation, raw data post-process.

1 Introduction The development of autonomous vehicles becomes a hot topic due to the demand of the intelligent transportation systems (ITS) and the military applications [1]. Local path planning plays an important role in the navigation of the autonomous vehicle, because it collects the information from sensors, and decides next move of the vehicle. In most conditions, autonomous vehicle should drive follow the road line and overtake an obstacle. Different methods of local path planning have been developed for meeting the requirements of lane following and collision avoiding. Another point is when the camera and radar detect the environment around the vehicle; the raw data from these sensors should be post process and be translated to different information which can be understood by the local path planner. Many research institutes have developed their own simulator or simulation model for the function development, such as a C++ based platform for testing Path Planning algorithms [2], a Visual C++ 6.0 based AGV path planning system [3], and the Zhao’s * Supported by Tongji University, Shanghai 200092, China, TRW Automotive R&D (Shanghai) Co., Ltd. Science and Technology Commission of Shanghai Municipality(07dz05901) and SLG Chassis Electronic Fund Chairs. M. Xie et al. (Eds.): ICIRA 2009, LNAI 5928, pp. 645–656, 2009. © Springer-Verlag Berlin Heidelberg 2009

646

J. Zhou, H. Chen, and C. Xiu

simulation system based on VC++6.0 [4]. These simulation platforms are vividly and intuitively but the developer have to cost a lot of time to design and create the simulation environment and vehicle’s performance. It is not an easy job for those who are not good at C code programming. Therefore, a simple and efficient model would be expected. Some papers have introduced some simulation platform for parts of the autonomous vehicle, such as Simulation of control system [5], Simulation of Local Path Planning [3]. Some simulation models based on a completed and precise map generate the ideal path, and the simulation will run after the map established, such as the simulation introduced by Shi Jian [6], the road data should be pre-loaded to the model, which can not evaluate the path planning control logic as a closed-loop control system. For these reasons, a model based on Matlab/Simulink® is presented and most of the aspects of autonomous vehicle functions can be simulated as a closed-loop control system, including sensor (as camera and radar in this paper) raw data post process, local path planning, trajectory following, car body control, and so on. The model can be used to design and validate the control logic of local path planning, and to simulate the post process of raw data from sensors according to the various path planning methods. Since the environment information is detected and fed back, the model can simulate the control algorithm when the obstacle is moving. The presented simulation model consists of three modules, as Fig. 1 shows, in which 2 DOF (Degree Of Freedom) vehicle dynamics module receives the steering angle and outputs the current coordinate XY (global coordinate) of the vehicle. The perception module receives the vehicle’s coordinate, compares it with the environment information which is defined in configuration file, then outputs the required information for path planning, and the post process of raw data from environment perception sensors is included in this module. The rest module is local path planner which outputs the steering angle to control the vehicle according to the environment information, and the algorithm of local path planning is in this module.

Fig. 1. Simulation model

Fig. 2. 2 DOF vehicle dynamics module

The model configuration will be described in section 2, the three modules will be detail described in section 3, 4, 5, and examples of lane following, collision avoidance and overtaking will be given in section 6.

A Simulation Model to Evaluate and Verify Functions of Autonomous Vehicle

647

2 Model Configuration The configuration file is written in a M file and is pre-loaded before the simulation running, which contains the definition of the necessary parameters. A. Since the 2DOF vehicle model is used, the vehicle parameters as follow should be defined. [7] z z z z z

The speed of the vehicle u, which is supposed to be a constant value. The mass of the vehicle m. The moment of Inertia around Z axis Iz. The front and rear tire cornering stiffness k1, k2. The length between center of gravity and front axle a, center of gravity and rear axle b.

B. The position of the road. The road is a continuous curve which is fit by several points. C. The position of the obstacle. The obstacle’s position in this model is defined as one point, which means the nearest point of the obstacle from the vehicle. The obstacle’s position is measured from the radar, and the point will be calculated and chosen from the radar’s output in real situation. The obstacle’s position can be defined as other ways, like the shape of the obstacle or the occupied area of it, which depends on the navigation algorithm used in Local path planner. Both of the position of road and obstacle is defined in the global XY coordinate, and they will be transformed to vehicle’s local coordinate in the perception module. After loading the parameters, the World Map containing road and obstacle can be drawn like Fig. 3.

Fig. 3. Road and Obstacle on World Map

3 2DOF Vehicle Dynamic Module The 2DOF vehicle dynamic module, which is also named as bicycle model, is responsible for calculating the dynamic coordinate of the vehicle. The input of the module is steering angle of front wheel, and output is XY coordinate of the vehicle. [7]

648

J. Zhou, H. Chen, and C. Xiu

1 ⎧ ⎪⎪( k1 + k2 ) β + u ( ak1 − bk2 ) ωγ − k1δ = m (υ ′ + uωγ ) ⎨ ⎪ ( ak − bk ) β + 1 ( a 2 k + b 2 k ) ω − ak δ = Izω ′ γ γ 1 2 1 2 1 ⎪⎩ u

(1)

In (1), ν means lateral velocity, ωr means vehicle yaw rate, δ means steering angle of front wheel, β means vehicle side slip angle, and

⎧X = ⎪⎪ ⎨ ⎪Y = ⎪⎩

β=

) ) (∫ ( ( ∫ sin ( β + ∫ ω dt ) dt ) ∗ t

0

υ

u

.

t

cos β + ∫ ωγ dt dt ∗ u 2 + v 2 0

t

t

0

0

γ

(2)

u +v 2

2

The dynamic XY coordinate of the vehicle is calculated by (2), which can be compared with the position of road.

4 Perception Module The Perception module is responsible for the post process of raw data from camera and radar, and expressing the relationship among autonomous vehicle, road, and obstacle as well. 4.1 Camera Data In most situations, camera is used to detect the road line for an autonomous vehicle, and it will lead the vehicle to follow a lane. We choose the Lane Detector product of Vislab, University of Parma, IT [8], which will send out several coordinate of the points on the road line as raw data. See Fig. 4. [8] In order to control the vehicle to follow the lane, and moved in a smooth, steady trajectory, the local path planner need the detail information of the road line. See Fig. 5. 1: Express the two road lines with two 3-order polynomials. 2: L1 means the distance between the center line of the vehicle and the left road line. L2 means the distance between the center line of the vehicle and the right road line. 3: The angle θ means the angle between the center line of the vehicle and the center line of the road. 4: The radius R of the road. Such information can be calculated by following post process methods and be expressed in the xy coordinate, which is vehicle’s local coordinate: 1: Use Matlab function ‘polyfit’ to fitting the points. X=[0 5 10 15 ]; Y=[0 0.5 -1 0.5]; coff=polyfit(X,Y,3); 2: L1 and L2 are calculated by the Y value of road line and vehicle. 3: Angle θ is calculated by the gradient of road line and vehicle.

A Simulation Model to Evaluate and Verify Functions of Autonomous Vehicle

Fig. 4. Points of road line (raw data)

θ = arctan(

649

Fig. 5. Required data of path planner

ytvehicle − ytvehicle 1 0 ) − arctan k xroad vehicle vehicle t1 xt1 − xt 0

(3)

k xroad means the gradient of the road line on the vehicle’s current position. t1 4: R is calculated by the road lines 3-order polynomials. [9]

(1 + ( y′) ) R=

3 2 2

y′′

(4)

y′ , y′′ mean the 1st and 2nd derivative of road line. Since all these equations are based on dynamic vehicle’s xy coordinate, the four values (L1, L2, θ, R) can be directly used by the Local path planner. See Fig. 6 4.2 Radar Data In most situations, radar is used to detect the obstacle for an autonomous vehicle, and it will lead the vehicle to avoid a collision. The LMS291 Laser Measurement Systems is chose as our sensor [10], which will send out the position of the obstacle in polar coordinates and they are transformed to be a XY coordinate which is more convenient for us to do further process. See Fig. 7. [10] The max scan field can be configured as 180 degree, and the resolution as 0.5 degree, so the max number of measured values is 361.

650

J. Zhou, H. Chen, and C. Xiu

Fig. 6. Camera raw data post process

Fig. 7. Radar scan map

When the radar is mounted on the vehicle, the output data is the obstacle’s position in the vehicle’s local coordinate. But when the perception module is used to simulate the output of radar, the data should be transformed into vehicle’s local coordinate just like they were measured by the radar mounted on the vehicle, because the obstacle’s position is defined with a global coordinate in the configuration file. The coordinate transformation matrix is as follow ⎧⎪ x ′ = ( x − X ) cos ( alfa ) + ( y − Y ) sin ( alfa ) ⎨ ⎪⎩ y ' = − ( x − X ) sin ( alfa ) + ( y − Y ) cos ( alfa )

(5)

In (5), x and y mean the global coordinate of the obstacle, X and Y mean the dynamic global coordinate of the vehicle, and alfa means the direction of the vehicle.

ytvehicle − ytvehicle 1 0 alfa = arctan( vehicle ) xt1 − xtvehicle 0

(6)

Here x’ and y’ mean the obstacle’s position expressed in vehicle’s local coordinate, which can be directly used by the Local path planner.

5 Local Path Planner Module The Local path planner module is responsible for calculating the steering angle of the front wheel according to the information provided by Perception module. Variety of local path planning methods can be developed in this module. Parameters of the local path planning algorithms can be tuned, designed and validated by running the whole model. According to different methods, such as Vector Pursuit Path Tracking [11], Optimal Feedback [6], Preview Follower Theory [12], the Perception module should be changed slightly to meet the requirement.

A Simulation Model to Evaluate and Verify Functions of Autonomous Vehicle

651

In this paper, a PID steering controller is used to keep L1 equals L2, so that the vehicle will drive in the middle of the lane. See Fig. 8.The angle θ and the R will be also considered in future work.

Fig. 8. PID controller for lane keeping

In this paper, the Artificial Potential Field method is introduced for collision avoidance. In the traditional artificial potential field methods, an obstacle is considered as a point of highest potential, and a goal as a point of lowest potential [13]. The attractive force towards the goal (Fa) and the repulsive force (Fr) from an obstacle are defined respectively with a function of the distance of them. And the vehicle should drive in the direction of the resultant force (Fart), so that it can avoid a collision while go toward the goal. See Fig. 9 and 10. goal Obstacle

Fa

Fart

Fr Fig. 9. Artificial Potential Field Method

Fig. 10. APF for collision avoidance

In this paper, the aim point’s angle error is used to control the vehicle to perform overtaking. Suppose the speed of the obstacle is lower than that of the vehicle, when the vehicle is D1 behind the moving obstacle, the road line of the adjacent lane which is provided by camera, will guide the vehicle to move into that lane, and the original lane’s information will guide the vehicle drive back when it is far enough (D2) in front of the moving obstacle. See Fig. 11 and 12. As Fig. 13 shows, the steering angle is proportional to the angle θ, which keeps the vehicle driving towards the aim point.

652

J. Zhou, H. Chen, and C. Xiu

Fig. 11. Overtaking

Fig. 12. Drive back

Fig. 13. Steering control logic for overtaking

6 Simulation Results As Fig. 14, 15 shows, the blue line means the road line, and the square in Fig. 15 is an obstacle, the red line is the trajectory of the vehicle. The performance of Lane Following and Collision Avoidance are acceptable. The parameter of the vehicle is as follow: m=1359.8kg, Iz=1992.54 kg*m2, a=1.06282m, b=1.48518m, k1= 52480 N/rad, k2=88416N/rad, and vehicle speed is u=5km/h. Notice that the center point of the road which is 10m ahead of the dynamic vehicle’s position is chose as goal on each simulation step.

Fig. 14. Lane following and the steering angle output

A Simulation Model to Evaluate and Verify Functions of Autonomous Vehicle

653

Fig. 15. Collision Avoid and the steering angle output

When performing a simulation of overtaking, as Fig. 16 shows, the vehicle can dynamically plan the suitable path to go. The speed of the vehicle is 5.5m/s, shown in blue and the obstacle is moving with 2m/s (b), 3m/s (c), shown in red. In (a), the obstacle is not moving.

(a)

(b)

(c)

Fig. 16. (a) Overtaking, (b) Overtaking, (c) Overtaking

The distance of the vehicle runs in the adjacent lane is obviously different according to the difference of the speed of the obstacle. The simulation of dynamic planning is based on the closed-loop of the control system and the dynamic detection of the virtual sensors.

7 Experiment Result In order to verify the usage of the simulation model, a lane change simulation and the vehicle experiment is conducted. A sine function steering angle like A*sin(wt) is sent to the chassis system when the obstacle is within a certain distance in front of the vehicle. The parameters A and w are decided by road width (ydes) and the parameters of vehicle, such as a, b, m, k1, k2 described in Chapter 2. The relationship of them is described as follow equation (7) ydes[( a + b) 2 + mu 2 ( u 2 ( a + b)

a b − )] kr k f

=

2π A

ω2

(7)

654

J. Zhou, H. Chen, and C. Xiu

In which, w is firstly determined according to the vehicle speed and the distance from the obstacle, and then A is calculated by equation (7) assuming that u is a constant value during the lane change period. In this paper, the ydes is set to 4m and 5m, u is set to 2m/s, and while the trigger condition is that the obstacle is within 15m in front of the vehicle. The w is chose as 0.6283rad/s, and A is 0.1508 and 0.1885. The parameters and trigger condition are set same both in simulation and experiment, and the control logic is completed by Simlink/Stateflow.

Fig. 17. Steering Angle Command

Fig. 18. Experimental Vehicle

Fig. 19. Trigger control

Fig. 20. Control logic

A Simulation Model to Evaluate and Verify Functions of Autonomous Vehicle

655

We use GPS/INS RT3050 to record the trajectory of the vehicle in red line and compare it with the simulation result in blue line, as Figure 21, Figure 22 show, the obstacle is at position (35, 0)/(20,0), the vehicle is triggered at (20, 0)/(5,0), and send the sine signal to the steering system. The two lines match well enough to verify the effectiveness of the simulation model.

Fig. 21. 4m Lane Change Simulation and Experiment

Fig. 22. 5m Lane Change Simulation and Experiment

8 Conclusion This paper presents a simulation model which could provides a platform to design and validate the control logic of Local Path Planning, and to design post-process of the raw

656

J. Zhou, H. Chen, and C. Xiu

data from sensors, which has been proved to be effective through a lane change experiment. Other applications, like Automatic Parking [14], Adaptive Cruise Control (ACC) [14], the method of local path planning and sensor data post-process can also be developed by this model. The environment can be edited and detected dynamically. This model will be complemented more details, including the blind area of the camera, the delays of the actuators or sensors, to approach a much more real situation. Also, some visual software like Virtual Reality Toolbox TM [15] will be used to make the simulation results more intuitive in future.

References 1. Xie, M., Chen, H., Zhang, X.F., Guo, X., Yu, Z.P.: Development of Navigation System for Autonomous Vehicle to Meet the DARPA Urban Grand Challenge. In: Proceedings of the 2007 IEEE Intelligent Transportation Systems Conference, Seattle, WA, USA, September 30- October 3 (2007) 2. Rubin, R.: DARPA Urban Challenge, a C++ based platform for testing Path Planning Algorithms: An application of Game Theory and Neural Networks, http://mpra.ub.uni-muenchen.de/4603/ 3. Minghua, Z.: Research and Simulation of Path Plan Algorithm for AGV of Automated Storage and Retrieva1 System, Graduate thesis, Jiangsu University (2006) 4. Chunxia, Z., Zhenmin, T., Jianfeng, L., Jingyu, Y., Chuancai, L.: The Simulation System for the Local Path Planning of the Autonomous Vehicle. Journal of Nanjing University of Science and Technology 26(6) (December 2002) 5. WenFeng, Y.: Research on Intelligent Vehicle’s Hierarchical Architecture and Route Following Algorithm, Graduate thesis, Nanjin Tech. and Industry College (2001) 6. Jian, S., Bin, Z.: Simulation of Automatic Driving Vehicle. Automotive Engineering 22(2) (2000) 7. Zhisheng, Y.: Automobile Theory. China Machine Press (2004) ISBN 7-111-02076-6 8. VisLab Embedded Lane Detector Technical Information Version 1.1.0, Vislab.it, Parma, Italy, http://vislab.it/ 9. Xuefei, Z., Zhuoping, Y., Hui, C., Ming, X.: A Method of Local Path Planning and Trajectory Generation for ALV. In: 2007 SAE-China, Tianjin, China (November 2007) 10. LMS200/211/221/291Laser Measurement Systems Technical description, http://www.sick.com 11. Wit, J.S.: Vector pursuit path tracking for autonomous ground vehicle, Ph.D. Dissertation, Department of Mechanical Engineering, University of Florida (2000) 12. Zhenhai, G., Xin, G., Konghui, G.: Application of preview follower theory and driver model in the research of vehicle intelligent handling. Journal of Traffic and Transportation Engineering 2(2), 64–66 (2002) 13. Vadakkepat, P., Tan, K.C., Wang, M.L.: Evolutionary artificial potential fields and their application in real time robot path planning. In: Congress on Evolutionary Computation, Proceedings of the 2000, vol. 1, pp. 256–263 (2000) 14. Bishop, R.: Intelligent Vehicle Technology and Trends. Artech House Publishers, London (2005) 15. http://www.mathworks.cn/products/virtualreality/