Model Predictive Control or MPC has emerged as a powerful tool for navigation of car like vehicles. Its strength stems from the fact that it can incorporate constraints on state and control while minimizing a user defined cost function. For car-like vehicles, the optimization underlying the MPC becomes strongly non-linear and non-convex due to the motion model of the vehicle and collision avoidance constraints. Current state of the art approaches rely on iterative techniques like sequential convex programming ,  to solve the underlying optimization , 
. However, without a good heuristic for initialization and under the strict real time constraints demanded by autonomous driving, these iterative techniques can sometime fail to provide any feasible solution. As a prospective remedy to this, most MPC frameworks rely on the use of so called "warm start" initializations for the underlying optimization. Herein solutions computed at previous instants are used as an initialization for solving the optimization problem at the current instant. The MPC frameworks proposed in,  follow an alternate methodology. They directly substitute non-linear motion model with approximate linear form, thus significantly reducing the complexity of the optimization. However, it should be noted that trajectories thus obtained are not guaranteed to be kino-dynamically feasible or collision free.
I-a Overview of Proposed Approach and Contributions
The proposed approach has a similar flavor as that of the concept of path velocity decomposition first proposed in . However, the technical approach is significantly different. Specifically, in contrast to , the proposed approach considers the reactive setting and uses the concept of collision cone  for modeling dynamic collision avoidance. Similar to , we observe that the collision avoidance maneuvers has two components namely the path taken by the robot and the forward velocity along it. In line with this observation, in this paper, we propose an MPC framework wherein the underlying optimization has a two layer structure for separate path and forward velocity optimization. It is essential that both the optimization layers are compatible with each other. That is, the paths obtained at the first layer should allow us to easily obtain collision free forward velocities along it. In the proposed work, this compatibility is naturally achieved given that both the optimizations solves the same collision cone constraints but with different set of variables. The path optimization solves the collision cone constraints with angular velocity as the variable. The forward velocity optimization solves the so called time scaled collision cone (TSCC) constraint ,  which is a projection of collision cone constraints along a given path.
A key feature of the proposed work is that it reduces the reliance on using iterative techniques and solving general non-linear optimizations for computing a collision free trajectory. This is achieved by offloading most of the responsibility of exactly satisfying the collision avoidance constraints to the forward velocity optimization layer for which computationally efficient structures can be derived. To be precise, we extend the concept of TSCC beyond ,  and formulate the forward velocity optimization as a convex quadratic programming problem. Both the optimizations layers are solved repeatedly in receding horizon manner to obtain lane change, overtaking and lane merging scenarios.
The rest of the paper is organized as follows. Section II presents a review of the related works. Section III briefly describes the main notations and symbols used in the formulation. Section IV introduces the path optimization. Section V reviews the time scaling concepts and formulates the forward velocity optimization. Section VI presents the reformulation of the forward velocity optimization into a convex quadratic programming problem. Section VII describes the method of encompassing tangents through which the collision avoidance between rectangular objects can be treated in the same way as that between disk shaped objects. Section VIII presents the simulation results validating our proposed formulation.
Ii Related Work
In this section, we first review the existing approaches for motion planning in dynamic environments for non-holonomic robots in the backdrop of autonomous driving. Subsequently, we review the more niche approaches specifically designed for autonomous driving.
Ii-a Motion Planning in Dynamic Environments.
Motion planning in dynamic environments has been extensively studied through the lens of velocity obstacle (VO)  which are analogous and independently developed along with collision cone (CC) . The strength of (VO) or (CC) concept is that it models dynamic collision avoidance as a first order constraint in combined position-velocity space. As a result, imminent collisions till some future time instants can be modeled based on just current velocity and positions. This feature has been exploited in works like , ,  to formulate a one step MPC framework. Both the concept of VO and CC were originally developed for holonomic robots moving along straight line trajectories. Their extension to non-holonomic robots is non-trivial and still an active area of research. The extension proposed in  is built on the idea that a non-holonomic robot would exhibit a finite tracking error while tracking a holonomic (piece-wise straight line) trajectory. Moreover, these tracking errors can be modeled as a function of the current state and the commanded holonomic velocities. In other words, this abstraction essentially signifies that for a given tracking error, it is possible to define a space of allowed holonomic velocities. Thus,  proceeds by first enlarging the radius of the robot by the tracking error that would be observed while simultaneously constraining the collision free velocity to lie within a specific set of allowed holonomic velocities. This last step of constraining the collision free velocity is the key as the set of allowed holonomic velocities are usually non-convex. A convex approximation can be obtained at the expense of unearthing a reduced space of collision free velocities.
An alternative extension has been proposed in  where the VO constraints for non-holonomic robots are solved by exhaustive control sampling. Various heuristics and off line pre-computations are exploited to make the process computationally efficient. It is clear the effectiveness of this approach depends on the number of control samples which in turn depends on the resolution of discretization. However, it is worth pointing out that the computational efficiency and the resolution of discretization are conflicting criteria in this methodology.
The above discussed formulations were primarily designed for general robotic environments with mobile robots. Consequently, their extension to autonomous driving setting poses some key challenges. For example, the technique of enlarging the size of the robot as proposed in  is not suitable for autonomous driving setting where lanes of the road provide a restricted space for maneuvers. Moreover, such size increment was proposed for only disk shaped robots and their extension to rectangular car-like vehicles is not straightforward. In contrast, the control sampling approach of  is good prospective solution provided good heuristics are available for restriction of search space. A recent approach called control obstacles (CO)  was used for computing collision free trajectories in autonomous driving setting . Herein, the robot’s kinematics as well as constraints are linearized to allow for quick computation. However, as mentioned earlier, the trajectories computed through linearized models are not guaranteed to be collision free. The proposed formulation provides some key advantages over these cited works. one such advantage being that it can work with rectangular geometries of the vehicle and can satisfy the collision avoidance constraints for provided non-holonomic motion model.
Ii-B Motion Planning for Autonomous Driving
Both sampling/graph search based planners and optimization based approaches are currently used for motion planning for autonomous driving. Either of these approaches can be used in a receding horizon manner to develop a MPC framework. Refer  for an extensive survey on different existing approaches. In the context of the proposed work, we will primarily focus on reviewing the optimization based approaches. Works like , ,  uses optimization to refine a coarse collision free trajectory obtained from the sampling based planners. A similar approach is presented in  wherein, the vehicle’s motion model is first approximated as a series of integrators and subsequently optimized trajectories are computed for a set of sampled points. These cited approaches do not explicitly include collision avoidance constraints directly within the optimization. A more holistic approach was presented in  wherein various kino-dynamic and collision avoidance constraints are directly included in the optimization framework. The proposed formulation shares the same holistic nature but differs from  in the technical approach followed. As mentioned earlier, we rely on exploiting structure of path velocity decomposition to limit our reliance on general non-linear optimization techniques. Moreover, unlike , we model collision avoidance between rectangular objects through the concept of encompassing tangents. This is less conservative than the multiple circle approximation used in .
Iii Symbols and Notations
We use normal faced lower cased letters to represent scalars while bold faced small case letters would be used to represent vectors or vector valued functions. For example,and respectively represents the trajectory of the vehicle and the obstacle in some future time horizon. With a slight abuse of notation, we will use and to represent the respective positions at time instant . Both the vehicle and the obstacles are assumed to be disk shaped with radius and respectively. The symbols, , would be used to represent the forward velocity and heading angle of the vehicle at time instant . Matrices would be denoted by bold faced higher case letters.
Iv Path Optimization
Consider the following optimization with variable as the angular velocity acting in a constant manner over the interval and resulting in a trajectory . The forward velocity is kept constant over . Further, .
The first term in the cost function, (1a) is inspired from path following algorithms ,  and aims to align the heading angle of the vehicle to that of the straight line connecting its current position to the goal (refer 2). It is easy to see that under such condition, a non-holonomic vehicle can easily navigate towards the goal. The second term of the cost function penalizes the use of high magnitude of angular velocities and induces smoothness in the resulting trajectory. The weights , trades-off smoothness with goal reaching behavior. The equality constraints (1b) ensures that the position and velocity at time conform to the motion model of the vehicle (refer 3). The inequalities (1c) models the bounds on curvature () of the resulting trajectory as well as magnitude of the angular velocity. The inequalities (5) model the collision avoidance between the vehicle and the dynamic obstacles through the concept of collision cone. Note that we only include such dynamic obstacle in (5) which are seen as converging from the vehicle’s perspective .
Iv-a Simplification and Solution Process
The main challenge in solving optimization (1a)-(5) stems from the non-convex motion model and collision avoidance constraints. Our solution process involves first eliminating the equality constraints (1b) by explicitly substituting its right hand side in (5). We next linearize the resulting collision avoidance constraints to obtain the affine approximation . Consequently, we solve a simpler variant of optimization (1a)-(5) wherein there are no equality constraints and the collision avoidance constraints have an affine form.
V Time Scaled Collision Cone (TSCC)
The optimization in the last section provides us with a trajectory which may be very close to being collision free 111From closeness, we mean that the collision avoidance constraints are very close to the boundary of the feasible region i.e . It could also be completely collision free but in general there is no such guarantee since the optimization in the previous section only dealt with an affine approximation of the collision avoidance constraints. In this section, we apply the concept of TSCC over the trajectory and ensure exact satisfaction of collision avoidance constraints by appropriately modifying the forward velocity profile of the trajectory. To this end, we next present a preview of the time scaling and TSCC concept followed by the construction of the second layer optimization.
V-a Time Scaling
Given a trajectory, , a change in its time scale from to does not alter the geometric path associated with the trajectory. In other words, let correspond to a position on the trajectory at time . Then, a change in time scale from to would mean that the same point on the trajectory would be reached but now at a different time instant . It is clear that this change in time of traversal is simply due to the fact the time scaling transformation changes the forward velocity along the path associated with . The exact change in velocity and acceleration are characterized by the following set of equations.
The term in (6) is called the scaling function and it decides the transformation between the time scales and .
Now, assuming as a linear function in arbitrary time interval , we obtain the following relation .
Equation (7) describes how an arbitrary time interval in the current time scale gets modified to an interval in the new time scale .
Equation (8) suggests that is approximated as a constant in time interval (or alternately ), with its value dependent on the magnitude of the scaling function at the two end of the interval .
Using (6) and the discussions presented in the previous section, the space of velocities that can be achieved at position for the vehicle can be characterized in the following manner in terms of the scaling function.
Where, , and are functions of etc.
V-C Velocity Optimization
We now formulate an optimization problem to compute the scaling function and consequently a collision free forward velocity along the path associated with
The cost function in (13a) seeks to compute such scaling functions which minimizes the deviation of the vehicle from its preferred forward velocity, . This preference velocity calculated based on output of Behavioural layer and average lane speed. The chosen design of the cost function assumes a specially important role in the context of autonomous driving. For example, in overtaking scenarios, should necessarily be higher the dynamic obstacles it is overtaking. Inequalities (13b) enforces the constraints that the forward velocity resulting from time scaling transformation should respect the minimum and maximum bounds. In our formulation, is strictly greater than zero to account for the fact that in autonomous driving scenarios, brining the vehicle to a halt may disrupt the traffic flow. Inequalities (13c)-(13d) enforces the acceleration bounds. For computational reasons, we have split the acceleration bounds into equivalent bounds for the separate and components. Inequalities (13e) is same as (12). Finally, the equality (13f) ensures that the velocity profile resulting from the time scaling transformation has continuity with the current velocity profile at initial time instant .
Vi Simplification Of Velocity Optimization
In this section, we show that velocity optimization (13a)-(13f) can be reformulated to a convex quadratic programming (QP) problem. We start with the convexification of TSCC constraints (10) or (12) and then subsequently show that its intersection space with velocity and acceleration bounds can be described in terms of linear inequalities. For the ease of exposition, we introduce the following change of variables .
With the help of (8), we can also obtain the following relation .
Vi-a Convexification of TSCC Constraints
In our past work , , we have shown the convexity of (12) depends on the sign of , which in turn depends on the relative positions and velocities between the vehicle and the dynamic obstacles. There, we exploited the fact that for each dynamic obstacle (12) represents a single variable quadratic inequality and thus can be solved in closed form. Subsequently, we employed a sorting algorithm to compute the resultant solution space. However, we have observed that such process become computationally expensive especially if many non-convex instances of (12) are present. Thus, here, we present an alternate methodology wherein two new insights are provided. Firstly, we show that even when , inequalities (12) can be converted to a convex form provided we have . Secondly, we show that for the non-convex instance of (12), we can linearize it to obtain an approximation which is more conservative than the original constraints. In other words, satisfaction of linearized constraint guarantees satisfaction of the original constraints.
The following analysis is for one dynamic obstacle but can be easily extended to the general case of obstacles
The solution space of (12) for this case is given by
Using (15), we have
In this section, we start with writing (12) in a slightly different form as below
The above inequality is convex with respect to and whose solution space can be characterized in the following form.
Finally, using (15), we have
The previous two discussed cases are a subset of this case and any of the approaches can be followed.
Inequality (26) is in the so called convex-concave form wherein the first term is convex while the second term is purely concave. We linearize the second term around some initial guess resulting in the following affine approximation for (26).
Vi-B Velocity and Acceleration Bounds
Squaring both sides of (13b), we obtain
Summary: The discussions presented in this section has shown that the TSCC constraints and the velocity and acceleration bounds can all be represented as linear inequalities in terms of variable . Let these inequalities be compactly represented as , where . The optimization (13a)-(13f) can then be presented in the following simple form.
Vii Method of Encompassing Tangents
The formulations presented in the last couple of sections have assumed a disk shape for the vehicle and dynamic obstacles. In this section, we show that using the method of encompassing tangents , the same formulation can be easily adapted for rectangular shaped objects as well. The following presentation is a slight adaptation of the original concept proposed in .
Consider Fig. 1 where (vehicle) and (dynamic obstacle) represent two rectangular objects. A Minkowski sum boundary polygon , can be mathematically represented as , Where . As shown in Fig. 1, the Minkowski sum allows us to reduce the rectangle to a point and enlarge into polygon . The minkowski sum can be computed in time for two polygons with m and n edges. Next, we compute the so called encompassing tangents (solid black line in Fig. 1) which are straight lines drawn from to polygon with an enclosing angle given by
where is the slope of the lines drawn from to vertex of . Following  and as shown in Fig. 1, the collision avoidance between rectangles and can be equivalently modeled as that between a point and the circle constructed on the basis of encompassing tangents. Their velocities will be same as that of and respectively. Note that the is much less conservative than the circumscribing circle of . also has less area than that obtained by representing the polygon with multiple circles .
Viii Simulations Results
Viii-a MPC and Simulation Framework
The employed MPC framework is simple and consists of repeatedly solving the path and velocity optimization and executing the computed trajectory. Nevertheless, few key points are worth pointing out. Firstly, only a part of the current computed trajectory is executed before a new set of computations are initiated. In other words, our planning horizon is much smaller (typically 10 times) than the planning horizon which is typical for an MPC framework. Secondly, in autonomous driving scenarios the vehicle is required to follow a path rather than reach a specific goal position. In our MPC implementation, we induce this behavior by giving a pseudo velocity to the goal position. The magnitude of this pseudo-velocity is tuned to get the best results. Finally, to constrain the vehicle to stay within the road boundaries, we created some fictitious static obstacles along the boundaries and included them in the path optimization framework as dynamic obstacles with zero velocities. Simulations were performed in Gazebo  while optimizations were solved in Python with CvxOpt  as the solver. Gazebo is a 3D simulator developed and supports multiple physics engines. The obstacle locations and their velocities were sensed by LIDAR mounted on top of car. In the Gazebo snapshots shown later, the gray color SUV represents the vehicle while rest of the cars are dynamic obstacles. A simulation video can be found at https://www.youtube.com/watch?v=8re0I9z0LEk& feature=youtu.be
Viii-B Benchmark Scenarios
Fig. 2-2 shows the simulation of overtaking maneuvers in Gazebo. The dynamic obstacles are all moving at a speed of along straight line paths. Fig. 2 shows the path of the concerned vehicle. As can be seen, the vehicle first departs from the lane and comes back to it after overtaking. The forward velocity plots shown in Fig. 2 reveal an interesting trend. The initial forward velocity is around for the vehicle which is significantly less than the dynamic obstacles ahead. Thus, to initiate an overtaking maneuver, we set in (13a). As can be seen from Fig. 2, the vehicle gradually increases its forward velocity and settles down at after overtaking. The angular velocity plots are shown in Fig. 2 and exhibit a smooth profile.
Viii-B2 Overtaking and Following
Fig. 3-3 show the concerned vehicle change lane to overtake a slow moving dynamic obstacle and then slow down to follow the dynamic obstacle ahead. The behavior can also be cross-validated against the forward velocity plots shown in Fig. 3. The forward velocity increases to during the initiation of the overtaking and then decreases to after coming back to the lane. In between, the forward velocity jumps momentarily to to quickly complete the overtaking maneuver. The angular velocity plots shown in Fig. 3 again exhibit a smooth profile.
Viii-B3 Merging and overtaking
Fig. 4-4 show the concerned vehicle first moving along a free lane and then converging to a lane with slow moving dynamic obstacles followed by an overtaking maneuver. As shown in Fig. 4, the forward velocity increases to the preferred magnitude of and then slows down to to converge and overtake. The angular velocity plots are shown in Fig. 2 and is similar in nature to that obtained for the previous cases.
Viii-B4 Overtaking and Lane Change
Fig. 5-5 shows the vehicle changing its lane by overtaking two dynamic obstacles ahead. Due to lack of space, we do not show the forward and angular velocity plots but they show similar smoothness as in the previous cases.
Viii-C Computation Time
The mean computation time for each iteration of our MPC loop observed across all the examples presented in the previous section is shown in Fig. 6. The implementation was done in Python on a 64 bit laptop with 6GB RAM, i5 processor with 2.60 GHz clock speed. As can be seen, the proposed MPC with hierarchical path and velocity optimization can be solved at around for a scenario with 10 obstacles, resulting in an update rate of almost .
Ix Conclusion and Future Work
In this paper, we presented a computationally efficient MPC framework for autonomous driving. The underlying optimization is based on path velocity decomposition paradigm and can exactly satisfy the complex kino-dynamic and collision avoidance constraints. In contrast to existing works, the proposed MPC limits its reliance on solving general non-linear optimizations. Instead, the forward velocity optimization layer assumes most of the responsibility of guaranteeing collision avoidance. We have shown that the computation time of our MPC is very low and it in fact can run at an update rate of about 77 Hz in realistic scenarios.
Our future efforts are focused towards including uncertainty in our MPC framework. A probabilistic variant of TSCC had already been developed in  and we are looking to extend that formulation to autonomous driving setting.
-  S. Boyd, “Sequential convex programming,” Lecture Notes, Stanford University, 2008.
-  J. Schulman, J. Ho, A. X. Lee, I. Awwal, H. Bradlow, and P. Abbeel, “Finding locally optimal, collision-free trajectories with sequential convex optimization.” in Robotics: science and systems, vol. 9, no. 1, 2013, pp. 1–10.
-  A. Stentz, H. Herman, A. Kelly, E. Meyhofer, G. C. Haynes, D. Stager, B. Zajac, J. A. Bagnell, J. Brindza, C. Dellin et al., “Chimp, the cmu highly intelligent mobile platform,” Journal of Field Robotics, vol. 32, no. 2, pp. 209–228, 2015.
-  A. Carvalho, Y. Gao, A. Gray, H. E. Tseng, and F. Borrelli, “Predictive control of an autonomous ground vehicle using an iterative linearization approach,” in Intelligent Transportation Systems-(ITSC), 2013 16th International IEEE Conference on. IEEE, 2013, pp. 2335–2340.
-  J. Nilsson, P. Falcone, M. Ali, and J. Sjöberg, “Receding horizon maneuver generation for automated highway driving,” Control Engineering Practice, vol. 41, pp. 124–133, 2015.
-  J. Nilsson, M. Brännström, J. Fredriksson, and E. Coelingh, “Longitudinal and lateral control for automated yielding maneuvers,” IEEE Transactions on Intelligent Transportation Systems, vol. 17, no. 5, pp. 1404–1414, 2016.
-  K. Kant and S. W. Zucker, “Toward efficient trajectory planning: The path-velocity decomposition,” The International Journal of Robotics Research, vol. 5, no. 3, pp. 72–89, 1986.
-  A. Chakravarthy and D. Ghose, “Obstacle avoidance in a dynamic environment: A collision cone approach,” IEEE Transactions on Systems, Man, and Cybernetics-Part A: Systems and Humans, vol. 28, no. 5, pp. 562–574, 1998.
-  A. K. Singh and K. M. Krishna, “Reactive collision avoidance for multiple robots by non linear time scaling,” in Decision and Control (CDC), 2013 IEEE 52nd Annual Conference on. IEEE, 2013, pp. 952–958.
-  B. Gopalakrishnan, A. K. Singh, and K. M. Krishna, “Time scaled collision cone based trajectory optimization approach for reactive planning in dynamic environments,” in Intelligent Robots and Systems (IROS 2014), 2014 IEEE/RSJ International Conference on. IEEE, 2014, pp. 4169–4176.
-  P. Fiorini and Z. Shiller, “Motion planning in dynamic environments using velocity obstacles,” The International Journal of Robotics Research, vol. 17, no. 7, pp. 760–772, 1998.
-  J. Van den Berg, M. Lin, and D. Manocha, “Reciprocal velocity obstacles for real-time multi-agent navigation,” in Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on. IEEE, 2008, pp. 1928–1935.
-  J. Alonso-Mora, A. Breitenmoser, M. Rufli, P. Beardsley, and R. Siegwart, “Optimal reciprocal collision avoidance for multiple non-holonomic robots,” in Distributed Autonomous Robotic Systems. Springer, 2013, pp. 203–216.
-  D. Wilkie, J. Van Den Berg, and D. Manocha, “Generalized velocity obstacles,” in Intelligent Robots and Systems, 2009. IROS 2009. IEEE/RSJ International Conference on. IEEE, 2009, pp. 5573–5578.
-  D. Bareiss and J. van den Berg, “Generalized reciprocal collision avoidance,” The International Journal of Robotics Research, vol. 34, no. 12, pp. 1501–1514, 2015.
-  A. Best, S. Narang, L. Pasqualin, D. Barber, and D. Manocha, “Autonovi: Autonomous vehicle planning with dynamic maneuvers and traffic constraints,” in Intelligent Robots and Systems (IROS 2017), 2017 IEEE/RSJ International Conference on. IEEE, 2017, p. to appear.
-  B. Paden, M. Čáp, S. Z. Yong, D. Yershov, and E. Frazzoli, “A survey of motion planning and control techniques for self-driving urban vehicles,” IEEE Transactions on Intelligent Vehicles, vol. 1, no. 1, pp. 33–55, 2016.
-  M. McNaughton, C. Urmson, J. M. Dolan, and J.-W. Lee, “Motion planning for autonomous driving with a conformal spatiotemporal lattice,” in Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011, pp. 4889–4895.
-  W. Xu, J. Wei, J. M. Dolan, H. Zhao, and H. Zha, “A real-time motion planner with trajectory optimization for autonomous vehicles,” in Robotics and Automation (ICRA), 2012 IEEE International Conference on. IEEE, 2012, pp. 2061–2067.
-  D. Dolgov, S. Thrun, M. Montemerlo, and J. Diebel, “Path planning for autonomous vehicles in unknown semi-structured environments,” The International Journal of Robotics Research, vol. 29, no. 5, pp. 485–501, 2010.
-  M. Werling, J. Ziegler, S. Kammel, and S. Thrun, “Optimal trajectory generation for dynamic street scenarios in a frenet frame,” in Robotics and Automation (ICRA), 2010 IEEE International Conference on. IEEE, 2010, pp. 987–993.
-  R. S. Wallace, A. Stentz, C. E. Thorpe, H. P. Moravec, W. Whittaker, and T. Kanade, “First results in robot road-following.” in IJCAI, 1985, pp. 1089–1095.
K. Hauser, “Fast interpolation and time-optimization with contact,”The International Journal of Robotics Research, vol. 33, no. 9, pp. 1231–1250, 2014.
-  N. Koenig and A. Howard, “Design and use paradigms for gazebo, an open-source multi-robot simulator,” in Intelligent Robots and Systems, 2004.(IROS 2004). Proceedings. 2004 IEEE/RSJ International Conference on, vol. 3. IEEE, 2004, pp. 2149–2154.
-  M. S. Andersen, J. Dahl, and L. Vandenberghe, “Cvxopt: A python package for convex optimization, version 1.1. 6,” Available at cvxopt. org, vol. 54, 2013.
-  B. Gopalakrishnan, A. K. Singh, and K. M. Krishna, “Closed form characterization of collision free velocities and confidence bounds for non-holonomic robots in uncertain dynamic environments,” in Intelligent Robots and Systems (IROS), 2015 IEEE/RSJ International Conference on. IEEE, 2015, pp. 4961–4968.