Trend-aware motion planning for wheeled mobile robots operating in dynamic environments

Planning collision-free trajectories is essential for wheeled mobile robots operating in dynamic environments safely and efficiently. Most current trajectory generation methods focus on achieving optimal trajectories in static maps and considering dynamic obstacles as static depending on the precise motion estimation of the obstacles. However, in realistic applications, dealing with dynamic obstacles that have low reliable motion estimation is a common situation. Furthermore, inaccurate motion estimation leads to poor quality of motion prediction. To generate safe and smooth trajectories in such a dynamic environment, we propose a motion planning algorithm called trend-aware motion planning (TAMP) for dynamic obstacle avoidance, which combines with timed-elastic band. Instead of considering dynamic obstacles as static, our planning approach predicts the moving trends of the obstacles based on the given estimation. Subsequently, the approach generates a trajectory away from dynamic obstacles, meanwhile, avoiding the moving trends of the obstacles. To cope with multiple constraints, an optimization approach is adopted to refine the generated trajectory and minimize the cost. A comparison of our approach against other state-of-the-art methods is conducted. Results show that trajectories generated by TAMP are robust to handle the poor quality of obstacles’ motion prediction and have better efficiency and performance.


Introduction
The generation of safe and feasible trajectories for wheeled mobile robots is essential to accomplish the autonomous navigation in confined and dynamic environments 1,2 without human intervention. In recent decades, reliable motion planning approaches for the mobile robots which operate with neither an onboard human presence nor transmitted control has been spotlighted in plenty of cruise and transportation services.
Many existing motion planning approaches [3][4][5][6][7] have focused on generating the optimal trajectories online instead of conventional paths 8 to deal with increasingly complex practical applications. It is common that mobile robots are deployed under partial-known environments that have the presence of random dynamic obstacles (e.g. pedestrians and other social vehicles) and static objects in most realistic application scenarios. Under this circumstance, the majority of existing path planners [9][10][11] fail to search a collision-free path for mobile robots, due to the conflict caused by moving obstacles. On the contrary, a general motion planner, 12,13 which incorporates temporal and kinodynamic constraints, 14,15 has more success rate in spite of optimality and real-time capability. This is mainly because the obstacles' future proxemic behavior is revealed by adding the time dimension. To execute the planning algorithm in real time and achieve a relatively optimal result simultaneously, the relevant approaches 16,17 usually modify a path pruned from the global path to reduce the computing effort. Meanwhile, they consider trajectory time consumption, kinodynamic, clearance from obstacles, and compliance with the desired path as the constraints in the trajectory optimization.
However, avoiding stochastic moving obstacles is still a tough challenge for the motion planner even all the objectives are optimized sufficiently. Especially when mobile robots encounter a random crossing situation, in spite of having ideal robotic self-localization, 18,19 a general motion planner 20 cannot guarantee to generate a collision-free trajectory. There are two main reasons accounting for the poor planning solution: (1) it is still difficult to acquire precise velocity and acceleration of an unknown object efficiently even with advanced sensors and algorithms at present 21 and (2) most local motion planners only pay close attention to a obstacle's current state instead of the whole obstacle dynamics. For the former case, the actual measurement contains errors and noise inevitably, which leads to the situation that most local motion planners are unable to maintain long-term optimality and can only deal with imminent crossing obstacles using a replanning mechanism. 22,23 As to the latter case, alternatively, the moving trends of obstacles are neglected conventionally, which is a piece of vital information for local motion planners to avoid the obstruction or even collisions caused by dynamic obstacles.
As to solve the problems above, we introduce a new dynamic obstacle avoidance approach called trend-aware motion planning (TAMP) that is capable of generating collision-free trajectories. TAMP is built by the combination of the work of Rösmann et al. 13 and our proposed technique called trend-aware trajectory deformation (TATD) which deforms a given trajectory into a collision-free trajectory for dynamic obstacle avoidance. Due to the inaccurate motion estimation of dynamic obstacles, we hypothesize that crossing obstacles are goal-driven, which indicates the moving direction of the crossing obstacles only varies in an appropriate interval. TAMP takes advantage of dynamic obstacles' motion prediction based on their inaccurate motion estimation to predict their moving trends. By analyzing the limitation of the mobile robot's and dynamic obstacles' moving trends, we formulate the dynamic obstacles avoidance problem into an optimization problem. Furthermore, the optimization problem also has the constraints of kinematics, dynamics, and time consumption. To solve the problem, the multi-objective optimization problem is transformed into an approximative nonlinear squared optimization problem. The objective of TATD is local as it only rests upon a few adjacent configurations, which result in a sparse structure, thus ensuring optimality and efficiency. An iterative mechanism is implemented owing to the computational advantage. Hence, the planned trajectory vertices can be deformed in time with respect to safety and optimality in case of a sudden change or even reverse in the direction of obstacles' movement. TAMP makes the service concept inherent to social mobile robots' motion planning, and it chooses a socially compliant way ahead of a minimum time way, which complies with the social norm. But TAMP still endeavors to minimize the time consumption under this condition. Therefore, as is shown in Figure  1, when the encounter occurs, the mobile robots implemented with TAMP take the detour and bypass behind obstacles' moving direction while maintaining a safe separation from obstacles rather than getting in the way of the obstacles or stuck in place due to the misleading of the disturbed motion prediction.
To show the advantage of our approach experimentally, we have compared TAMP's performance against the timed elastic bands (TEBs) planner 13 and TEB equipped with homology class (HC) 24 exploration. Averagely, our results indicate that TAMP continuously generates more feasible trajectories than the state-of-the-art TEB and TEB þ HC planners, which results in lower path length cost, lower time consumption, and higher stability.
The remainder of this article is structured as follows. In the second section, we review the related work. Then we describe TAMP in the third section. The fourth section shows the experiments and results. Finally, we conclude this article in the final section.

Related work
In this section, we describe the approaches relevant to the collision-free path or trajectory generation for dynamic obstacle avoidance. Modeling dynamic obstacles as static obstacles in a short replanning period is a common way adopted by many approaches. Devin Connell and Hung Manh La 2 use the RRT* 22 algorithm as well as Sven Koenig and Maxim Likhachev 23 implement the dynamic A* algorithm for replanning in a dynamic environment with random, unpredictable moving obstacles. Although the generated paths are approximate optimal within the limits, the risk of colliding with the dynamic obstacles is still existing without incorporating the time dimension. When the replanning time expires, the approaches will fail to give the path in time. On the contrary, our planner takes the time interval of adjacent configurations into account for avoiding collision cautiously.
Velocity obstacles 25 are used to compute appropriate velocities to avoid collisions with dynamic obstacles. Dynamic window approach 26 searches the optimal control speed of the robot corresponding to the optimal trajectory in the velocity space to avoid local obstacles. Both the approaches deal with dynamic obstacles with a time interval, however, they can run into local minima easily and get stuck without other solutions. TAMP, on the other hand, implements a planning process with an appropriate time horizon. Thus, it has sufficient space and time intervals to optimize near-term objective functions for generating safer and more socially compliant trajectories.
Phillips and Likhachev 27 and Narayanan et al. 28 describe a safe interval technique that gives a robot the ability to safely navigate in the dynamic environment with the presence of moving obstacles. However, the technique assumes that the future trajectories of dynamic obstacles are predicted accurately, which is difficult to maintain in actual applications. Generally speaking, the moving obstacles' motion is probabilistic so that it can only be approximated in a local interval. TAMP, on the other hand, is prepared for the situation that it only demands the current velocity and position of the obstacles instead of the whole accurate future trajectory, and simple estimation of the trajectory implemented over a local time horizon is sufficient for the planner to avoid the collision.
Park et al. 7 describe a dynamic obstacle modeling technique that computes a conservative bound on the position of the moving obstacles based on the predicted motion and then avoids collisions by solving an optimization problem. Instead of only modeling the future position of dynamic obstacles, TAMP attaches importance to the moving trends of obstacles.

The methodology of trend-aware motion planning
Trend-aware motion planning is composed of TATD and TEB. TATD is committed to deforming the basic robotic trajectory which is generated in accordance with TEB's dynamics and kinematics constraints into the collisionfree trajectory. To perform dynamic obstacle avoidance, we begin to model a wheeled mobile robot and local obstacles.

Objects representation
For the dynamic obstacle avoidance concept presented in this work, it is essential to describe robots' and local obstacles' models, including physical models and motion models.
In our method, the robot and local obstacles are represented in configuration space where every point corresponds to a unique configuration of the robot. In the configuration space, every configuration of the robot can be represented as a point and the local obstacles can be inflating at all directions by a certain radius.
With respect to the adoption of motion models, the robot uses a differential motion model. And the local obstacles are divided into static obstacles and dynamic obstacles. The dynamic obstacles use a constant motion model, while the static obstacles' motion does not have to be predicted.

Trajectories prediction
In this work, we assume that we have the current states of the robot and dynamic obstacles. In addition, the estimation of dynamic obstacles' velocity may not be accurate. In spite of the inaccuracy, the trajectories of the robot and dynamic obstacles are predicted in a time horizon T.
In the time horizon T, a sequence of robot's configurations connects an initial and a final configuration: where c i denotes a configuration including position x i ; y i and orientation q i . All adjacent configurations are tied with the time intervals Dt i , and the time horizon T is represented as Defining the robot's trajectory is composed of robot's configuration and time intervals. We assume that there are N dynamic obstacles in the environment and these obstacles' positions vary along with time. Regardless of the inaccurate velocity, the movements of dynamic obstacles are predicted in the near future with the constant motion model. Then we have where the current position and velocity are donated as p o j and v o j , respectively, and O j (t) represents the predicted state of the dynamic obstacles at moment t. Define the predicted trajectory of the dynamic obstacles as With time going by, the estimated states of the robot and dynamic obstacles are renewed in real time for reacting on dynamic environments, and both the predicted trajectories will be updated in time. It is worth mentioning that the positions and orientations of the robot and dynamic obstacles are all related to one consistent coordinate system. By this time, the motion predicted trajectories of the robot and dynamic obstacles are constructed.

The constraints of TATD
Based on the predicted trajectories of the robot and dynamic obstacles, there are existing possibilities that the robot and dynamic obstacles will collide with each other at some certain moment. To keep the minimum safety, the minimum distance between the robot and dynamic obstacles must be considered. Let d c i ; O j ðtÞ be the minimum distance in the Euclidean metric between the robot's configuration c i and the obstacle's state O j ðtÞ. The distance constraint z i is defined as in which d min is denoted as a minimum clearance between the configurations of the robot and dynamic obstacles.
To cope with the inaccurate motion prediction of a dynamic obstacle, when the dynamic obstacle's predicted trajectory moves across the predicted robot's trajectory, we adopt the moving trends of the robot and dynamic obstacles to evaluate the collision possibility. Therefore, we create a trend-aware method to detect the potential collision.
For every configuration of the predicted trajectories of the robot and dynamic obstacles, we project the moving trends based on its own orientation or velocity's direction where the predicted position of the robot is denoted as p i which consists of x i and y i . v i represents the velocity of the robot. t c i and t j i are the trend parameters of the robot and the jth dynamic obstacle, respectively, and both the trend parameters determine the size of the trend. Cðt c i Þ and O j ðt j i Þ represent the trend state of the robot and the jth dynamic obstacle, respectively, and a sequence of trend states constitutes a trend trajectory.
When the distance between the configurations of the robot and dynamic obstacles is close enough, the trend projection mentioned above will engage. The distance between the configurations of the robot and the jth dynamic obstacle is computed as A modified sigmoid function is implemented on the distance result where g 2 ½0; 1 is the conditional judgment, d req is given an empirical value, and k denotes a scaling factor.
When g is unequal to zero, it means the existence of the dynamic obstacle is threatening the safety of the robot. Hence, the collision detection is implemented. First it is necessary to compute v i Â v o j for ensuring whether the trend trajectories of the robot and jth dynamic obstacle are neither paralleled nor colinear. Once the trend trajectories are neither paralleled nor colinear, the detection of a collision is computed by detecting the trend trajectories' intersection If t c i or t j i is greater than or equal to zero, we consider that there is a potential collision between the robot and the jth dynamic obstacle. The possibility of collision can be greatly reduced if the trend constraint s i is satisfied, and the trend constraint s i is defined as s i ¼ ½s min ; s min ; :::; s min T À½t 0 i ; t 1 i ; :: where s min is denoted as a minimal intersection distance to maintain the safe clearance between the robot and the jth dynamic obstacle. The constraints of TATD are built and ready for trendaware trajectory optimization.

Robotic fundamental constraints
The robot's kinematics and dynamics are the fundamental components for local motion planning, and a stable and reliable trajectory optimization method needs to subject to the following constraints where h i represents the non-holonomic kinematics constraint and n i and a i represent the dynamics constraint. As is mentioned above, the robot uses a differential model. The robot's motion control only has two degrees of freedom. The non-holonomic kinematics constraint requires that the robot's two adjacent configurations c i and c iþ1 are located on a common arc of constant curvature. Let b i represent the angular difference between the robot's configuration c i and its motion direction d i ¼ ½x iþ1 À x i ; y iþ1 À y i ; 0 T ; similarly, b iþ1 represents the angular difference between the robot's configuration c iþ1 and the motion direction d i . Only if both the angular differences b i and b iþ1 are equal, the common arc of the constant curvature can be obtained. The non-holonomic kinematics constraint is defined as The dynamical constraint is implemented for limiting the velocity and acceleration of the robot. The mean translational and rotational velocities are computed according to the Euclidean or angular distance between two consecutive configurations c i , c iþ1 and the time interval The velocity constraint is defined as where v max and ! max are the maximum translational and rotational velocities of the robot, respectively. The acceleration a i is related to two consecutive translational velocities v i and v iþ1 . Thus, the acceleration a i is computed as The acceleration constraint is defined as where a max is the minimum acceleration of the robot.

Trajectory optimization
To acquire the optimized robot trajectory, the multiobjective optimization problem mentioned above is mapped into an approximative nonlinear squared optimization problem. The trend constraint is represented in terms of the quadratic penalty with weight w s where f s ðPÞ is the trend objective function. Additional constraints h i , n i , and a i are represented in a similar fashion. The optimal trajectory P Ã is obtained by solving the approximated optimization problem PÃ ¼ arg min where objective function fðPÞ contains all individual objective functions, w T denotes a weight vector which consists of all weight factors, and the total transition time is approximated by P nÀ1 i¼0 Dt 2 i . As is depicted in Figure 2, the trajectory optimization problem is transformed into a hyper-graph that has configurations and time intervals as vertices. The edges are connected by the vertices, representing objective functions or constraint functions. The formulation (22) is solved by a sparse variant of the Levenberg-Marquardt algorithm which is utilized by the graph optimization framework g2o. 29 The optimization of the trend objective function moves the vertices of the robot's configurations away from the predicted obstacle states. Meanwhile, it deforms the The moving obstacle's position varies in time but is a fixed vertex, which means its current position is not a variable and cannot be affected by the optimization procedure. Edge t i connects two vertices, and it represents the objective function which contains trend and distance constraint. TATD: trend-aware trajectory deformation .   T9   T0  T1   T3   T4   T5   T6   T7   T8   T10   T11   T12   T13  T14   T4   T3   T2   T1   T0   T5   T6   T7   T8   T9   T10   T7   T6   T5   T2 Dynamic obstacle A Dynamic obstacle B Figure 1. A single-trajectory generation using trend-aware motion planning. T represents for time stamp. The robot states in T0 and T14 represent start and goal, respectively. T4 and T8 are the crucial moment in which TATD dynamically deforms the trajectory away from the moving obstacles with respect to the trends of the obstacles. TATD: trend-aware trajectory deformation.
vertices into a curve shape that avoids the moving trends of obstacles with the lowest cost.

Experiments and analysis
In this section, to demonstrate the advantages of our TAMP algorithm, we ran simulations in the presence of the multiple moving obstacles. We compare TAMP with TEB and TEB þ HC. The three planners have the same kinematic and dynamic constraints, however, TAMP is different from the other planners in dealing with dynamic obstacle. The trajectories generated by the local motion planners are implemented to control the mobile robot motion directly. The local motion planners need to generate and deform trajectories consecutively based on the given global path so that the robot can reach the desired goal in case of the unexpected moving obstacles. Algorithms are performed on a PC with a 3.7-GHz Intel i7 CPU and 32 GB RAM. It is worth mentioning that TAMP, TEB, and TEB þ HC are implemented on the same non-holonomic mobile robot with the same parameter configuration.

Vertical crossing obstacles scenario with precise state estimation
In this scenario, to compare the performance of the three different motion planners, the robot starts from the same starting point at the same time and reaches the same end point by avoiding dynamic obstacles. The obstacles deployed the same and their accelerations are the same. The obstacles are moving vertically with constant acceleration. The precise states of the obstacles are given, and the motion planners perform online trajectories generation for the incoming obstacles based on the given data. As is shown in Figure 3, there are three frozen moments of each planner, which is time stamp 1, time stamp 2, and time stamp 3. The three time stamps represent that each planner generates a trajectory three times. Each planner's time stamp 1 represents the first responding to an approaching dynamic obstacle. The last two time stamps show the follow-up response. Figure 3(a) to (c) indicates that TAMP deforms the generated trajectory to bypass the moving obstacle while avoiding the moving trend of the obstacle; furthermore, the smoothness and stability of the trajectory are guaranteed. According to the simulation, the constantly changing speed of the obstacle has little effect on the shape of the trajectory. In contrast, Figure 3(d) to (f) shows that TEB tries to plan a trajectory to avoid the obstacle despite of its moving trend due to the current low speed of the obstacle, however, when the obstacle is accelerating, which force TEB to change the original plan continuously. The changing process is disturbing for the robot controller and might cause a collision with other obstacles. TEB þ HC plans multiple trajectories in Figure 3(g) to (i) based on Voronoi diagrams and chooses the least cost one. Nevertheless, this method introduces a new dilemma that the cost of each alternative trajectory is variable due to the unstable obstacles, which leads to a disturbing result.
The performance of TAMP shows that although TAMP is using the constant motion model for dynamic obstacles motion prediction, it is still robust to avoid the moving trends of the dynamic obstacles with constant acceleration and maintain the minimum cost.

Slant crossing obstacles scenario with disturbed state estimation
The following experiment compares several metrics to evaluate the attribute of the approaches. Due to the uncertainty of the dynamic obstacles, we measure the whole process that the non-holonomic robot bypasses the moving obstacles until it reaches the target configuration instead of a partial process within a short time horizon. Hence, the safety and global optima can be quantified. We evaluate the performance of three methods by several metrics.
(i) The traveled path shows the complete path length from the start to the end, which indicates the odometry cost of avoiding dynamic obstacles. (ii) The consumed time directly reveals the efficiency of the planner, which implies the average speed of the robot is higher or lower.
The translational velocity profile shows the stability of the robot controlled by the motion planner, which implies how smooth the planned trajectory is.
When the moving obstacles are crossing in front of the robot unexpectedly and blocking the original trajectory of the robot, a retreat behavior might be triggered to keep the robot away from a collision threat. A retreat behavior has a primary influence on trajectory smoothness, which pursuits safety for avoiding unexpected moving obstacles. The frequency of retreat behavior can be reduced by implementing a better motion planner.
In the experiment, the dynamic environment is deployed as a 25 Â 6 m 2 grid map with 14 obstacles which are moving along specific trajectories with constant acceleration. When the speed of the obstacles reaches the upper limit or zero, the acceleration will change the current sign. A random disturbance is applied to the dynamic obstacles' velocity estimation so that the planners cannot rely on the motion prediction of the dynamic obstacles. When the dynamic obstacles reach the bottom or top of the grid map, their vertical movement directions will be reversed. The planning complexity is increased massively in this scenario. The initial configuration and the final destination are set at feasible locations in the grid map, respectively. All the planners are given a feasible path straight to the goal without colliding with static barriers. Moreover, the planners are initialized at the same moment. The maximum linear and angular velocities of the mobile robot are limited to 0.4 m/s and 0.6 rad/s, respectively. Figure 4 depicts that three complete simulations implementing different methods, respectively, in one environment to show the exact state of the robot controlled by the planners. The three different lines represent the traveled paths of the robot implementing the three different methods. The red and green lines are not smooth, which means retreat behaviors are triggered. TAMP constantly seeks a more feasible and smoother piecewise path instead of being blocked by the obstacles and having to proceed with a retreat behavior. And TAMP always avoids the moving trends of the dynamic obstacles such that the influence of the inaccurate obstacles' motion prediction is minimized. Therefore, TAMP results in generating foresighted evasive maneuvers. As giving the random disturbance to the obstacles' velocity value, TEB and TEB þ HC can only deal with the imminent crossing obstacle, however, TAMP already takes actions about the obstacles at the end of the planning horizon, which maintains the relatively long-term solution robustly and optimizes the trajectory efficiently.
Then each planner is executed 10 times repeatedly for the same challenging scenario in which the dynamic obstacles' orientation provided to each planner may reach a deviation of 40 . It is illustrated in 5(a) that TAMP outperforms other methods in maintaining higher velocities of the robot and the outliers of TAMP are barely negative. As is shown in Table 1, the interquartile range of TAMP's velocity is lower than other methods, which means smoother and control-friendly trajectories are generated iteratively. The poor quality of the obstacles' motion prediction causes TEB and TEB þ HC to oscillate easily. As is shown in Figure 5(b) and (c), TEB and TEB þ HC take more detours than TAMP, which lead to longer path and time consumption. And their results are scattered in a relatively large range, which implies that TAMP is more stable than other methods. The standard deviation in Table 1 can also demonstrate that every attempt of TEB and TEB þ HC (a) (b) (c) Figure 5. Comparison between approaches. The red dots on (a) are the outliers of the major velocity. When the outliers are less than zero, the robot is retreating for avoiding a collision. The detailed data are presented in Table 1: (a) velocity profile (m/s), (b) path length (m), and (c) time consuming (s). Table 1. The metric data for the methods to compare with velocity v, path length l, and time consumption t are depicted in Figure 5. in completing the experiment is more probabilistic than TAMP. The sudden change in the moving direction of the robot brings major pressure into the hardware of the robot. The results highlight that TEB and TEB þ HC have more trouble in dealing with the slant crossing obstacles with poor state estimation, while TAMP can guide the robot slips out of the dynamic dilemma.

Conclusion
In this article, we proposed a novel approach for avoiding dynamic obstacles, called trend-aware motion planning (TAMP). For the purpose of guaranteeing the safety of the generated trajectory, TAMP is designed to deal with the dynamic obstacles under inaccurate motion prediction by taking the moving trends of the obstacles into account. TATD moves configuration vertices away from the moving obstacles' future possible moving trends, and TAMP maintains the continuity and smoothness of the trajectory simultaneously. According to our experiments, TAMP generates more stable and safer trajectories and results in smaller path length and lower time consumption averagely than the state-of-the-art TEB and TEB þ HC planners. In general, TAMP outperforms TEB and TEB þ HC in dynamic environments with moving obstacles. Further work includes adding curvature constraints on TAMP and applying TAMP to three-dimensional space for micro aerial vehicles.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.