Collision Avoidance of a Kinodynamically Constrained System from Passive Agents

Robot motion planning in dynamic environments is significantly difficult, especially when the future trajectories of dynamic obstacles are only predictable over a short time interval and can change frequently. Moreover, a robot’s kinodynamic constraints make the task more challenging. This paper proposes a novel collision avoidance scheme for navigating a kinodynamically constrained robot among multiple passive agents with partially predictable behavior. For this purpose, this paper presents a new approach that maps collision avoidance and kinodynamic constraints on robot motion as geometrical bounds of its control space. This was achieved by extending the concept of nonlinear velocity obstacles to incorporate the robot’s kinodynamic constraints. The proposed concept of bounded control space was used to design a collision avoidance strategy for a car-like robot by employing a predict-plan-act framework. The results of simulated experiments demonstrate the effectiveness of the proposed algorithm when compared to existing velocity obstacle based approaches. Keywords-collision avoidance; dynamic environment; motion planning; navigation; mobile robot; kinodynamic constraint; velocity obstacle; pedestrian environment


INTRODUCTION
Most modern applications of mobile robots require them to navigate in a dynamic environment among moving obstacles such as people, pets, cars, etc. Robots working in such environments must be able to successfully navigate while multiple agents are moving around them. To achieve this objective, mobile robots need to generate a short collision-free path in real-time concerning an obstacle's future position. Furthermore, the generated path should be system compatible, as most of the mobile robots have kinodynamic constraints, such as car-like robots [1,2].
Many motion planning approaches, such as in [3][4][5], were proposed to navigate a robot with kinodynamic constraints in a static environment, and extended their application in dynamic environments [6][7][8]. Some important works in the direction of robot navigation in a dynamic environment were introduced in [9][10][11], focusing on a complete trajectory plan to the goal. These approaches don't perform well in a dynamic environment, especially when it is possible to predict only partially the future motion of other agents, e.g. pedestrian environment, for many reasons. The first reason is that these approaches don't consider predicting the future motion of other agents, thus the robot becomes blinded to a possible future collision. Secondly, a limited time is available for computing a solution in a dynamic environment. Therefore, it is very likely that the planner will be unable to compute the complete collision-free trajectory during the available time slot. Other works, such as [12,13], proposed approaches that search in state time-space to compute a system compliant collision-free trajectory to the goal, based on the other agent's future path. These methods assumed that the complete path of the agents is known in advance, limiting their application on pedestrian or multiple passive agents environments. The moving agents have free will in such environments. Their future behavior can only be partially predictable, if at all. Agents' future behavior is predicted through various on-line prediction approaches, such as the ones proposed in [14][15][16][17]. In such prediction approaches, the model of the future motion is obtained at an instant and it is valid for a short period. Therefore, a navigation approach should be designed to take into account the duration of the environment model's validity and the limited available time for computing a collision-free solution. An alternative approach to achieve motion planning is to plan a motion locally. In such approaches, the robot employs a continuous predict-act-plan cycle. In each cycle, the robot must compute its action based on predictions to avoid static and moving obstacles as it moves towards the goal. Some of the dominant works in this direction were based on the concept of Velocity Obstacle (VO) [18], which considers the future behaviors for collision avoidance. The VO-based navigation approach consists of planning a local motion toward the next (sub) goal, which is extracted from a global waypoint plan. VO was formulated for a simple robot model, where the obstacle path was assumed to be along a straight line. In [19], the Nonlinear Velocity Obstacle (NLVO) concept was proposed for collision avoidance of a robotic system with a linear equation of motion, where the passive agent was assumed to be moving along an arbitrary possibly nonlinear path. To expand the concept of VO, authors in [20] considered the kinematic car-like robot avoiding collision with multiple passive agents, proposing the concept of Generalized Velocity Obstacle (GVO). This approach principally acts as the opposite of NLVO, where a car-like system with a nonlinear motion equation should avoid collision with obstacles having linear motion equations (straight paths). VO was expanded in [21] proposing the Acceleration Velocity Obstacle (AVO) method for a system with acceleration constraints. This approach was proposed for reciprocal collision avoidance among active agents, where each one shares the responsibility. This approach assumed that every active agent involved in a collision should run a similar collision avoidance algorithm for avoiding it. In [22], the concept of safe control space for collision avoidance was proposed to deal with various sources of un-modeled uncertainties. Some other studies which examined collision avoidance were presented in [23][24].
This study addressed the problem of navigating a car-like robot system under kinodynamic constraints among multiple passive agents with partially predictable behaviors. At first, the idea of NLVO [19] was extended to define a generalized approach for handling collisions of a car-like robot with obstacles moving along arbitrary paths. A formulation was given to represent geometrically the bounds of robot motion as bounds of the robot's control space. The bounded control space was named Valid Control Space. Moreover, this paper provides a framework for finding the optimal control within the bounded control space, which can be used to locally navigate a robot to a goal. The proposed method's performance was evaluated for a dynamic environment similar to [20], where the entire responsibility of collision avoidance belongs to the robot. As shown in Figure 1, passive agents' predicted trajectories can change frequently. Simulations showed that the proposed approach performs better than existing VO based approaches.

II. VALID CONTROL SPACE
This section presents the idea of Valid Control Space, which addresses the difficulty of overcoming the kinodynamic constraints of a car-like robot. This was achieved by introducing a control obstacle concept, which was a generalization of the NLVO.

A. Notations and Assumptions
ܵ ⊂ Թ denotes the robot's state space, and ‫ݏ‬ ሺ‫ݐ‬ሻ denotes the state of the robot ܴ at time ‫.ݐ‬ The robot workspace is Թ ௗ , where typically ݀ ൌ 2. The robot position at time t in robot configuration space, denoted by ‫‬ ሺ‫ݐ‬ሻ, is obtained from the state of the system through a nonlinear projection function: ‫‬ ሺ‫ݐ‬ሻ ൌ ݄ሺ‫ݏ‬ ሺ‫ݐ‬ሻሻ (1) where ݄ ∈ ܵ → Թ ௗ . Furthermore, it is assumed that the dimension of workspace is equal to the robot's input control space ܷ ⊂ Թ ௗ . The robot's state transition equation is probably a nonlinear function ݂: ܵ ൈ ܷ → Թ : ‫ݏ‬ሶ ሺ‫ݐ‬ሻ ൌ ݂ሺ‫ݏ‬ ሺ‫ݐ‬ሻ, ‫ݑ‬ሺ‫ݐ‬ሻሻ (2) where ‫ݏ‬ ሺ‫ݐ‬ሻ ∈ ܵ is the robot state at time ‫,ݐ‬ and ‫ݑ‬ሺ‫ݐ‬ሻ ∈ ܷ is the control input. The state at any given time greater than zero for a given current state ‫ݏ‬ ൌ ‫ݏ‬ ሺ0ሻ and a constant input control ‫ݑ‬ ൌ ‫ݑ‬ሺ0ሻ is given by: where ݃: ܵ ൈ ܷ ൈ Թ → ܵ is the solution of (2). Multiple simulation snapshots at different time instances. The robot in green starts from its initial position at (5,10) and moves towards the goal marked in yellow while avoiding collision with the multiple passive agents marked in red.

B. Dynamic Obstacle Representation
This study assumed that there will be a system in addition to the mobile robot, as introduced in [16][17], tracking passive agent states and estimating their future trajectories. Each agent has a disc-like shape with a radius and a trajectory. The list of passive agents and their future behaviors is given as the set of points representing time and their estimated future positions.

C. Control Obstacle
The geometry of the robot was considered to be its bounding circle, as in [18]. The mobile robot shares its workspace with several passive agents. ‫ܣ‬ and ܱ are sets of points representing the robot and the i-th agent geometry respectively, while ‫ܤ‬ is the Minkowski sum of ‫ܣ‬ and ܱ geometries ‫ܤ‬ ൌ ‫ܱ⨁ܣ‬ . At ‫ݐ‬ ൌ 0, the position robot's center and the i-th passive agent are represented by ‫‬ ሺ0ሻ and ‫‬ ሺ0ሻ respectively. The predicted position of the i-th passive agent at ‫ݐ‬ 0 is given as ‫‬ ሺ‫ݐ‬ሻ. The robot will avoid collision with the The temporary component of control obstacle ܷܱ ሺ‫ݐ‬ሻ induced for the robot due to i-th agent at time ‫ݐ‬ ∈ ሾ0; ߬ሿ, can be defined as: where ܷܱ ሺ‫ݐ‬ሻ is the set of all ‫ݑ‬ ∈ ܷ for which the relative position vector ݄൫݃ሺ‫ݏ‬ , ‫,ݑ‬ ‫ݐ‬ሻ൯ െ ‫‬ ሺ‫ݐ‬ሻ will be inside ‫ܤ‬ at time ‫ݐ‬ ∈ ሾ0, ߬ሿ. Thus, the control obstacle induced for robot over time horizon ሾ0, ߬ሿ due to the i-th agent can be defined as: where ܷܱ ఛ is the set of all ‫ݑ‬ ∈ ܷ that will maneuver the robot towards a collision with an i-th agent at time ‫ݐ‬ ∈ ሾ0, ߬ሿ. The robot will remain collision-free from the i-th agent within the time horizon ሾ0, ߬ሿ if the selected robot's control input does not belong in the control obstacle ‫ݑ(‬ ∉ ܷܱ ఛ ).
This collision avoidance approach can be extended for multiple dynamic agents. ܰ ൌ ሼ1, 2, … nሽ is the index of the dynamic agents to avoid. The control obstacle induced for the robot by the dynamic agents can be given as: A robot selecting ‫ݑ‬ ∉ ܷܱ ఛ will navigate without collision with n dynamic agents for at-least ߬ seconds.

D. Valid Control Space
The constraints of robot motion are defined by constraint functions ݂ ሺ‫,ݏ‬ ‫ݐ‬ሻ bounded in ሾ‫ܯ‬ ି ; ‫ܯ‬ ା ሿ, where ሺ‫ܯ‬ ି ; ‫ܯ‬ ା ∈ Թሻ. These constraints restrict the admissible controls for the robot in a control space over a time horizon ሾ0, ߬ ሿ. The admissible space of the controls is denoted by ܷ ௗ . The valid control space at time ‫ݐ‬ ∈ ሾ0, ߬ሿ, denoted by ܷ ௩ௗ ఛ ሺ‫ݐ‬ሻ, is: ఛ models all possible sub-trajectories satisfying all constraints. Every point chosen in ܷ ௩ௗ ఛ will satisfy the kinodynamic constraints and collision avoidance over ‫ݐ‬ ∈ ሾ0, ߬ሿ. This approach is based on the geometrical construction of ܷ ௩ௗ ఛ as it is described in forthcoming sections.

E. Example -Single Integrator
The concept of ܷ ௩ௗ ఛ is explained considering a mobile robot with simple dynamics as of a single integrator. System's position at any time ‫ݐ‬ for constant input ‫ݑ‬ can be given as ‫‬ ሺ‫ݐ‬ሻ ൌ ‫‬ ሺ0ሻ ‫,ݑݐ‬ where ‫‬ ሺ0ሻ is the current position of the robot. The motion of the system is constrained by ඥ‫‬ሶ ሺ‫ݐ‬ሻ ‫ݒ‬ ௫ , where ‫ݒ‬ ௫ is the system's maximum speed limit. As the state transition equation of a single integrator is ‫‬ሶ ሺ‫ݐ‬ሻ ൌ ‫,ݑ‬ ܷ ௗ will be a set of input where ‫‖ݑ‖‬ ‫ݒ‬ ௫ . ܷ ௗ has the shape of a disc with a center at the origin and a radius of ‫ݒ‬ ௫ in robot's input control space. ܷܱ ሺ‫ݐ‬ሻ will be equivalent to ܰ‫ܱܸܮ‬ሺ‫ݐ‬ሻ for the single integrator. It also has the shape of a disc, with its center at ‫(‬ ሺ‫ݐ‬ሻ െ ‫‬ ሺ0ሻሻ/‫ݐ‬ and radius equal to ሺ‫ݎ‬ ‫ݎ‬ ሻ/‫ݐ‬ in robot input control space ܷ, where ‫ݎ‬ and ‫ݎ‬ are the radius of the robot and the ݅ െ ‫݄ݐ‬ passive agent. Figure 2 shows ܷ ௩ௗ ఛ where ܷܱ ሺ‫ݐ‬ሻ is considered at equally spaced time instances by ‫ݐߜ‬ ൌ ‫ݏ1.0‬ and up to ߬ ൌ ‫ݏ5‬ time horizon. The other parameters were set as: ‫‬ ሺ0ሻ =(0,0), ‫ݎ‬ ൌ ‫ݎ‬ ൌ 0.4, ‫ݒ‬ ௫ ൌ 1 ‫,ݏ/ݐ݅݊ݑ‬ and ‫‬ ሺ‫ݐ‬ሻ ൌ ሺ2,0ሻ. III. COLLISION AVOIDANCE APPROACH This section presents the proposed collision avoidance approach based on the concept of ܷ ௩ௗ ఛ , formulating the problem of robot navigation in a dynamic environment as an optimization problem. Given a random starting point, the proposed procedure will navigate the robot towards the goal without collision with any moving passive agent present. The goal position is assumed to be extracted from some waypoint plans for global navigation. The proposed algorithm allows the robot to quickly re-plan its course as new passive agents are discovered or new information regarding their future predicted states are received. ‫ݑ‬ is a control input to the kinodynamic model that leads the robot towards its goal. The exact value of ‫ݑ‬ ൌ ‫ݑ‬ , which is required to reach the goal, can be found by solving the equation ݄ሺ݃ሺ‫ݏ‬ , ‫,ݑ‬ ߬ሻሻ ൌ ‫‬ . If this point belongs to ܷ ௩ௗ then all motion constraints are satisfied and the shape of the trajectory can be defined. Otherwise, control ‫ݑ‬ * that is given to the robot is the solution that minimizes the following cost: This navigation problem is formalized as the problem of finding the ‫ݑ‬ ∈ ܷ ௩ௗ ఛ that is nearest to ‫ݑ‬ , in terms of the Euclidean distance between them in control space ܷ . The solution lies in the boundaries of ܷ ௩ௗ ఛ and is denoted by ߲ܷ ௩ௗ ఛ . The procedure used to find a roughly optimal solution is as follows: ܷ ௗ ఛ represents a square grid of size ݀ denoted by ܷ ௗ ఛ (݀), and the grid elements belonging to ܷܱ ఛ are discarded. The algorithm in Figure 3 summarizes the procedure for finding and selecting an optimal control input by the robot for collision avoidance. A quadtree [25] was used to obtain precise tiling with rectangles over the boundaries of ܷ ௩ௗ ఛ . The best point is selected from all the remarkable points (center and/or corners) of rectangles. A rough optimization is performed by obtaining an optimal point from all rectangles, i.e. the controls that are closest to the preferred control.
Predict-plan-act Cycle: After each time step, the robot receives the predicted states of passive agents over a future time horizon of ߬ seconds, and ܷ ௩ௗ ఛ is built. The best control ‫ݑ‬ * ∈ ܷ ௩ௗ ఛ is computed with (9). The trajectory corresponding to ‫ݑ‬ * will be collision-free for the next ߬ seconds. The computed trajectory is executed for one scan cycle of Δ‫ݐ‬ ≪ ߬, until a new scan cycle begins. This results in a continuous predict-plan-act navigation framework. Fig. 3. Algorithm: find best control input u*.

IV. IMPLEMENTATION AND RESULTS
The proposed approach was applied to a car-like robot model for evaluating its performance.

A. Considered Robot Model
A robot with car-like kinodynamic was considered with the following parameters: its real-wheel axle center is ‫‬ ൌ ሾ‫ݔ‬ , ‫ݕ‬ ሿ ் , its orientation is ߠ, and its steering wheel angle is ߶. The state transition equations of the robot are given by: where ‫ݒ‬ is the input speed control. As in [20], the curvature was taken directly as input, while the steering angle ߶ can be computed for ݇ as ߶ ൌ tan ିଵ ‫,ܮ݇‬ where ‫ܮ‬is the wheelbase of the car. The motion of a car-like robot is considered to be constrained as: where ‫ݒ‬ is the maximum speed constraint and ݇ is the robot's maximum curvature constraint. ܷ ௗ for this system was defined by ݇ and ‫ݒ‬ , and its geometry was a rectangular region of the input control space. Robot's position at time ‫ݐ‬ can be obtained by integrating (10) assuming that controls will remain constant over the time horizon ሾ0, ‫ݐ‬ሿ, as: where ‫‬ is the current position and ߠ is the current orientation of the robot, and the rotation matrix ܴሺߠ ሻ is:

B. Implementation Details -Simulations Setup
The maximum speed and curvature were set to ‫ݏ/ݐ݅݊ݑ5.1‬ and ‫ݐ݅݊ݑ5.1‬ ିଵ respectively. The radius of the robot was set to 1. An open environment was constructed for the simulations, bounded by (0,0) and (22,22). This environment was set to be crowded by multiple passive agents of circular shape and radius equal to one. The scan cycle was set to Δ‫50.0=ݐ‬s. Each passive agent was set to move along a specific trajectory, i.e. straight lines or arcs, with speed less than or equal to 1. The probability of an agent changing its path and speed within one second was set to 0.2. The interaction time horizon ߬ was set to 3.5s. Figure 1 shows the snapshots of simulations at multiple time instances, where the green marked robot successively avoids multiple red marked passive agents and reaches the yellow marked goal. Experiments are carried out on an Intel Core i5-3550/4GB RAM computer and the proposed algorithm was implemented using C++.

C. Performance Results
Each result value was a mean of 20 trials. Figure 4 shows the performance comparison with grid size ݀ as 16 and 32. Comparisons were drawn for a varying number of passive agents using three parameters. The time needed for computing optimal control ‫ݑ‬ * in a scan cycle is shown in Figure 4(a). The time needed by the robot to reach the goal is shown in Figure  4(b). The success rate, which indicates the percentage of the trail in which the robot successfully reaches the goal from the starting position, is shown in Figure 4(c). For d=16 the computation time is much less on average, whereas its success rate is nearly the same when comparing it with d=32. However, when d=32 the robot takes a more direct path to the goal, and the time to reach a goal is less on average, as shown in Figure  4(b).

D. Comparative Analysis
The proposed approach was compared with GVO and AVO for a grid size of d=16. Figure 5 shows the comparison of the proposed approach with a GVO having 256 controls sampled. Results showed that the proposed algorithm performed better than GVO on all three parameters. The proposed approach's computation time and the time to goal was much less, while the success rate was averagely higher compared to GVO. An additional advantage emerges as GVO is restricted to a linear equation of motion of passive agents, while the proposed approach has no such restriction.  Figure 6 shows a comparison of the proposed approach with AVO. The reciprocal collision avoidance aspect of AVO was removed to make a relative comparison, while the linear programming based optimization aspect was kept. The results showed better performance for the proposed approach ܷ ௩ௗ ఛ on all three parameters, pointing out its effectiveness on the considered problem.
V. CONCLUSION This paper presented a new approach that maps different constraints on a robotic system as geometrical bounds on the robot's input control space, allowing the computation of the optimal control input for collision-free local navigation using Quadtree. As it was shown, this computation can be executed in real-time. Furthermore, the proposed collision avoidance approach had superior performance compared to other velocitybased approaches for a considered environment. Moreover, an implementation of the proposed approach on a car-like robot was presented. This implementation can be easily adapted to other types of robotic systems, which is an additional advantage which can be studied in the future.