Proceedings of International Conference on Emerging Research in Computing, Information, Communication and Applications (ERCICA-14)
Nonlinear State Estimation Based Predictive Path Planning Algorithm Using Infrastructure-to-Vehicle (I2V) Communication for Intelligent Vehicle D. Ganesh Perumala, *, G. Saravana Kumarb, B. Subathrab, Seshadhri Srinivasanc, Srini Ramaswamyd a
Department of EEE, Kalasalingam University,Krishnan Koil, Srivilliputtur,India Department of ICE, Kalasalingam University,Krishnan Koil, Srivilliputtur,India c International Research Laboratory, Kalasalingam University,Krishnan Koil, Srivilliputtur,India d ABB Global Industries and Services Limited, Indian Corporate Research Center, Bangalore- 560068 b
Abstract In this investigation, we present an Extended Kalman Filter (EKF) based predictive vehicle path-planning algorithm that uses infrastructure-tovehicle (I2V) communication. The estimation method uses the relative distance of the vehicle from the infrastructure, and the distance from other vehicles or obstacles in the road using on-board vehicle sensor as inputs to produce the state estimates of the vehicle. The estimates are then used for computing the control inputs using behvaiour based controllers. Advantage of this path-planning algorithm is it eliminates the need for vehicle-to-vehicle (V2V) communication leading to increased reliability. The path-planning algorithm is illustrated using simulations on an intelligent vehicle model, and results show that the vehicle build with simple controllers, and I2V communication can be used for predictive path-planning. Keywords: Extended Kalman Filter (EKF), predictive control, vehicle path-planning, obstacle avoidance, Automated Highway Systems (AHS), infrastructure-tovehicle (I2V) communication, Intelligent Vehicle (IV).
1. Introduction Automated Highway Systems (AHS) use recent developments in communication, control, and computation to reduce the urban traffic. Implementing AHS requires complete automation of driver tasks to enable coordination among vehicles, and to overcome slow human reflexes [2]. Complete automation of driver tasks requires intelligent vehicle that can sense, communicate and take autonomous decisions based on available information. Additionally, the IV should also have some predictive features such as estimation of obstacles [5]. Further, IVs require communication with traffic infrastructure, as well among vehicles. These communication networks are called infrastructure-to-vehicle (I2V), and vehicle-to-vehicle (V2V), respectively [15]. Network traffic in I2V is low, and periodic, whereas it is frequent, and sporadic in V2V due to information exchange for vehicle coordination. Such, network proliferation in vehicle control application has significant cost benefits but makes the design challenging due to network imperfections such as data-loss, delays, out-of-sequence data, multi-packet transmission etc(see, [3,4] and references therein). In AHS, wireless sensor networks (WSNs) are used for V2V communication due to design inevitabilities. However, data-loss is more pronounced in WSNs compared to wired networks. In [8], Seshadhri et al. showed that data loss in V2V networks affect the safety of the AHS. Therefore, there is an urgent need to propose new algorithms to reduce network traffic in V2V communication, without affecting the road safety. However, these requirements are conflicting as reduced network traffic implies less coordination, and increased data-transmission leads to data-loss. Our objective in this investigation is to shift the network traffic from V2V communication to I2V communication for implementing path-planning algorithms in IVs. For this purpose, we assume that the vehicle has computing capabilities to build the estimate from EKF. This is a reasonable assumption considering the current research in IVs. The use of estimation algorithm for path-planning has been investigated by researchers for some-time now, and a detailed
* Corresponding author. Srini Ramaswmay, ABB Global Industries and Services Ltd, Indian Corporate Research Center, Bangalore 560068, Tel.:+919972893968, E-mail address:
[email protected] 243
Srini Ramaswamy .et.al.
review can be found in [1]-[8,9] and references therein. The available path planning algorithms can be broadly classified as: (i) Cspace discretization [10], (ii) search algorithms (such as Djakshthra algorithm) [11], (iii) re-planning (using probabilistic tools such as RRT, probabilistic road-maps) [9,12], and (iv) path-tracking [13]. These methods are widely used in robotics to map unknown environments. However, extending these methods to vehicle planning offers stiff challenge, this is mainly due to the dynamic traffic conditions, and the communication requirements. Therefore, estimation based methods are more suitable for AHS. Motivated by this many researchers have investigated the use of estimators for vehicle path-planning [6,7,20,21]. Estimation algorithm based on Kalman Filter has been used for reducing the effects of data-loss and V2V traffic in [14]. Advanced driver assist systems (ADAS) for vehicles with packet-dropouts in V2V communication has been investigated in [8]. Estimation based robotic consensus algorithm [16], and its application to formation control in vehicle platoons has been presented in [17]. A detailed review on formation control of vehicles with single integrator, and double integrator dynamics has been presented in [18]. These investigations consider simple vehicle models or use simple linear estimators; usually, vehicle dynamics is non-linear, and more-complex. Moreover, the role of predictive controller based on estimation has not been investigated. Motivated by this, we present EKF based predictive control algorithm for IVs that uses behavior based controllers proposed by Arkin in [19]. The main contribution of this investigation is the EKF based predictive controller algorithm for IVs employing I2V communication, and on-board obstacle measurements. EKF takes measurement input from the infrastructure, and obstacle measurement from the on-board in-vehicle sensor to compute its states. Later, these states and measurement are used to compute the predictive controller gains for vehicle maneuvers. To implement the control, the predictive controller uses three behaviourslocalization, obstacle prediction, go-to-goal, and obstacle avoidance. The first two behaviors are implemented using the estimator, whereas the later are implemented using behavior based controller. The paper is organized into five sections, including the introduction. Section II, presents the problem formulation. Section III, presents the EKF based predictive control algorithm. Simulation results are presented in section IV. Conclusions and future prospects of the investigation are discussed in section V.
2. Problem Formulation
Fig. 1. AHS Scenario
The AHS in our investigation has the following entities, traffic infrastructure consisting of signals, or other equipment used for controlling the traffic, and the communicating with the IV. The obstacles are other vehicles, and human interference on the road. On-board sensors are used to measure the obstacles, and I2V communication is used for localization of the vehicle as shown in Figure 1. Now, consider that V2V communication is used for communication among vehicles, then the considering the vehicles travelling in road, variables that need to be transmitted include velocity, acceleration, lane-changing signals, to name a few. Moreover, these variables need to be transmitted for various vehicles on road. This results in increased information in V2V communication resulting in increased traffic, thereby leading to channel congestion. Alternatively, I2V measurements can be used by the vehicle to determine its relative position with reference to the infrastructure, and such an approach reduces the V2V channel loading. 244
The problem considered in our investigation is to use I2V measurement to design a predictive path-planning algorithm for the vehicle. To perform path planning, the vehicle uses an EKF to estimate the states using measurements from the infrastructure. Then using the state estimates, it determines the distance from the obstacle. Using the estimate of the future state, and the obstacle, it varies the control input to move to the goal. This gives a predictive controller framework, wherein the future state estimate is used to define the current control action. Further, to obtain better control, the controller also uses a behaviour based approach wherein, decisions on control actions are made depending on the nearness to the obstacle. In our controller design the linear velocity is controlled depending on the location of the goal, whereas the steering angle is used to steer past the obstacle. The next section presents the predictive controller for AHS using EKF with I2V measurement. 3. EKF based Predictive Controller for AHS The first step of our algorithm begins with the design of EKF, which in-turn requires vehicle dynamics, observation model, and estimate equations. 3.1. The Process Model: The vehicle dynamics used in the study is the IVSHIP vehicle [5], the rear wheel of the vehicle is controlled using electric motors and the front wheel is controls the angle of the steering. The plant dynamics can be given by
x (t ) V cos y (t ) V sin
(t )
(1)
V sin L
The inputs to the vehicle are the steering angle
and velocity V, L is the wheel-base length of the vehicle. The discretised
model can then be represented as [1]:
xk 1 xk tVk Cos(k ) yk 1 yk tSin(k )
(2)
k 1 k tSin( k ) 3.2. The Observation Model As our objective is to use I2V communication for estimating the relative position and orientation of the vehicle, measurements obtained from the infrastructure are used as reference, this leads to the following observation model,
r Xr Xv
2
Yr Yv X X v r
tan 1
(3)
y [r ] vk 3.3. Estimation Equations Prediction equations of the EKF are [1],
xk 1 k xk tVk cos(k ) yk 1 k xk tVk sin(k ) t sin( ) k k 1 k k f f Pk 1 k Pk k x x
(4)
T
(5)
where f is the system dynamics given by (1)
245
Srini Ramaswamy .et.al.
Update equations of the EKF are,
xˆk 1 k 1 xˆk 1 k K k 1 ( yk h( xˆk 1 k ))
(6)
Pk 1 k 1 Pk 1 k K k 1S k 1K k 1
T
h 1 K k 1 Pk 1 k Sk 1 and x T
where
h h S k 1 Pk 1 k R with R 0 x x T
One can see that the EKF uses measurements from infrastructure to estimate the current state of the vehicle. On-board sensors are used to estimate the obstacles and their distances from the vehicle. Later, these estimates and measurements are used to control the vehicle using behavior based controllers. Prediction using EKF is used to determine the future control action, thereby giving predictive controller framework. 3.4. Behaviour based Predictive Controller Having obtained the state-estimate using the EKF, we now design the predictive controller. The design of predictive controller begins by computing the distance to the obstacle, and the heading. This is computed using equation (3) by the infrastructure and transmitted to the vehicle. The vehicle uses this information to determine the distance and orientation of the obstacle. Then, based on its distance from the obstacle, it uses behavior based controller described in [19]. In our investigation, we have designed two behaviours, one is go-to-goal, and other is obstacle-avoidance. 3.5. EKF based path-planning algorithm The EKF based path-planning algorithm for the vehicle dynamics (1), and with the states estimated using the EKF is shown in table 1. Step 0 Step 1 Step 2 Step 3 Step 4 Step 5
Initialize the states, control inputs, and computation matrices of EKF Obtain the measurement ‘y’ using (3) Estimate the state of the vehicle using equations (4)-(6) Compute ‘u’ is using possible behavior depending on the conditions Apply u and update the measurements Check whether the goal has been reached if yes-stop Else go to step 1 Table 1: EKF based path planning algorithm
4. Simulation Results The working of EKF based path-planning algorithm using measurement from infrastructure is shown in Figure 1. Three sets of infrastructure measurements are taken, and the on-board sensor measurements are used to compute the obstacles. In order, to prevent the vehicle from collision circular sensing paths around the obstacles are constructed. The vehicle trajectory using the state predictions from EKF and behavior based predictive controller is shown in Figure 2, wherein the desired goal is to reach xgoal=20 and ygoal=20, respectively. An integration time of 0.1 sec was used in the simulation. Results indicate that the vehicle maneuvers using the estimate without colliding with the obstacle using measurements from infrastructure. The control inputsvelocity and steering angle for the vehicle trajectory in the simulation with obstacles is shown in Figure 3. One can verify that the control input reaches a steady-state condition on reaching the goal.
246
4 3 2 1 0
0
50
100
150
200
250
300
350
400
450
500
0
50
100
150
200
250
300
350
400
450
500
0.5 0 -0.5 -1 -1.5 -2
Fig. 2: Vehicle trajectory using EKF, and behavior based controller
Fig. 3: Normalized Control Input for the vehicle The performance of the EKF based predictive control algorithm, when there is an obstacle at the goal is shown in Figure 4. One can verify that the vehicle is closer to the goal, and because of the presence of obstacle, the vehicle circles around the goal without reaching the final value. This shows that the proposed controller is intrinsically safe against collision. 5. Conclusions An EKF based path-planning algorithm for intelligent vehicles (IVs) was presented, the estimation algorithm employed I2V communication to estimate the vehicle state relative to the transport infrastructure. Further, on-board sensor measurements were used to detect the distance between adjacent vehicles, and obstacles in the road. The vehicle used the state prediction and measurements on obstacle to control the linear velocity, and steering angle. The velocity of the vehicle was changed depending on the proximity to the goal and obstacles respectively. Hybrid controller using behavior based approach was used for designing the controller. Results showed that the path-planning algorithm was able to achieve better control, and safety. Implementation of the control algorithm in IVSHIP vehicle and distributed implementation are future course of this investigation.
247
Srini Ramaswamy .et.al.
Fig. 4: Collision avoidance with the presence of obstacle at the goal References 1. 2. 3. 4. 5. 6. 7. 8. 9. 10. 11. 12. 13. 14. 15. 16. 17. 18. 19. 20. 21.
Dissanayake, M. G., Newman, P., Clark, S., Durrant-Whyte, H. F., and Csorba, M., `` A solution to the simultaneous localization and map building (SLAM) problem”, IEEE Transactions on Robotics and Automation, 2001,17(3), 229-241. Baskar, L. D., De Schutter, B., Hellendoorn, J., & Papp, Z. (2011). Traffic control and intelligent vehicle highway systems: a survey. IET Intelligent Transport Systems, 5(1), 38-52. Seshadhri, S. (2010). Estimation and design methodologies for networked control systems with communication constraints (Doctoral dissertation, Ph. D. dissertation, Dept. Instrumentation and Control Eng., National Institute of Technology, Tiruchirappalli., Tiruchirappalli, India). Seshadhri, S., & Ayyagari, R. (2011). Dynamic controller for Network Control Systems with random communication delay. International Journal of Systems, Control and Communications, 3(2), 178-193. http://ivship.weebly.com/ Prevost, C. G., Desbiens, A., & Gagnon, E. (2007, July). Extended Kalman filter for state estimation and trajectory prediction of a moving object detected by an unmanned aerial vehicle. In American Control Conference, 2007. ACC'07 (pp. 1805-1810). IEEE. Rafaralahy, H., Richard, E., Boutayeb, M., & Zasadzinski, M. (2008, December). Simultaneous observer based sensor diagnosis and speed estimation of unmanned aerial vehicle. In Decision and Control, 2008. CDC 2008. 47th IEEE Conference on (pp. 2938-2943). IEEE. Srinivasan, S., & Ayyagari, R. (2012). Advanced driver assistance system for AHS over communication links with random packet dropouts. Mechanical Systems and Signal Processing. Kavraki, L. E., Svestka, P., Latombe, J. C., & Overmars, M. H. (1996). Probabilistic roadmaps for path planning in high-dimensional configuration spaces. Robotics and Automation, IEEE Transactions on, 12(4), 566-580. Warren, C. W. (1989, May). Global path planning using artificial potential fields. In Robotics and Automation, 1989. Proceedings., 1989 IEEE International Conference on (pp. 316-321). IEEE. Bohlin, R., & Kavraki, E. E. (2000). Path planning using lazy PRM. In Robotics and Automation, 2000. Proceedings. ICRA'00. IEEE International Conference on(Vol. 1, pp. 521-528). IEEE. Bruce, J., & Veloso, M. (2002). Real-time randomized path planning for robot navigation. In Intelligent Robots and Systems, 2002. IEEE/RSJ International Conference on (Vol. 3, pp. 2383-2388). IEEE. Wit, J., Crane, C. D., & Armstrong, D. (2004). Autonomous ground vehicle path tracking. Journal of Robotic Systems, 21(8), 439-449. Seshadhri, S., & Ayyagari, R. (2011). Platooning over packet-dropping links.International Journal of Vehicle Autonomous Systems, 9(1), 46-62. Tank, T., & Linnartz, J. P. (1997). Vehicle-to-vehicle communications for AVCS platooning. Vehicular Technology, IEEE Transactions on, 46(2), 528536. Srinivasan, S., & Ayyagari, R. (2010, October). Consensus algorithm for robotic agents over packet dropping links. In Biomedical Engineering and Informatics (BMEI), 2010 3rd International Conference on (Vol. 6, pp. 2636-2640). IEEE. Raol, J. R., & Gopal, A. K. (Eds.). (2012). Mobile Intelligent Autonomous Systems. CRC Press. Ren, W., & Beard, R. (2007). Distributed consensus in multi-vehicle cooperative control: theory and applications. Springer. Arkin, R. C. (1998). Behavior-based robotics. MIT press. Baumgartner, E. T., & Skaar, S. B. (1994). An autonomous vision-based mobile robot. Automatic Control, IEEE Transactions on, 39(3), 493-502 Cherubini, A., Spindler, F., & Chaumette, F. (2014). Autonomous Visual Navigation and Laser-based Moving Obstacle Avoidance. IEEE Trans. on Intelligent Transportation Systems. 248
