Our group conducts multidisciplinary, application-oriented research which we continue to focus on topics of practical trajectory generation ranging from robotics to autonomous (ground, air) vehicles: search for the optimal or near-optimal paths linking the given initial and target configurations that optimize one or multiple objective functions (eg the path length) and satisfy the hard and soft constraints imposed on the vehicle, such as avoiding obstacles or response to changes and uncertainty in the environment.
Autonomous driving (AD), as the future of transportation , is empowered by the embodied intelligence in motion planning.Our work aims to meet the need to ensure feasibility, safety and guaranteed or predictable performance of trajectory generation for AD or autonomous vehicles (AV). Our emphasis on solving problems on motion/ path/ trajectory planning is by applying the solution strategies rooted in optimization (numerical and population-based metaheuristics search) and control, and applying machine learning to robotics and control. Including:
navigation: Design and developement of a sensor-based autonomous navigation system requires that a mobile robot, autonomous vehicles, drones, quadrotors/quadcoptors navigating in unstructured, cluttered environments is capable of exploring (interacting with its environment) and planning how to travel through an uncertain environment and executing the plan to accomplish the missions, e.g. mapping a region, gathering data, surveillance (target seeking, following or pursuit, and border patrol) in potentially hostile environment.Safety guarantee is crucial to path planning. To ensure a dynamically feasible, collision-free, smooth path can be found, the path planner must evaluate the quality and inequality constraints such as obstacle avoidance and the vehicle motion limitations ( such as control bounds), in order to complete the missions inside the constrained workspace.To meet the requirement, the robot needs to solve the path/motion planning problem, i.e. generate a static or dynamic obstacle-avoiding path/trajectory that is optimal with respect to one or more kinematic or dynamic criteria. The path design procedure can be computationally time consuming so that it is imperative to develop a computationally simpler ( and low storage) procedure to make the online path computation tractable and less prone to faulty calculations.
uav(autonomous flight technology):The use of unmanned aerial vehicles (UAVs) are increasing in a wide range of industrial, military, and agricultural applications such as Search-and-Rescue (SAR)SAR, delivery, infrastructure inspection,weather forecast, thanks to their excellent mobility and flexibility.
The generated trajectories for uav must be ensured as safety-guaranteed, flyable (kinematically and dynamically feasible) in airspace, and minimizing the transport time for a transport service flight.
In most cases, unicycle is used as a well-posed kinematic representation of differential-driven nonholonomic mobile robots.A differential-drive mobile robot has two fixed motor- actuated wheels on the left and right side, and one caster wheel for stability.Our results report what can be obtained using a unicycle model that is simpler and has low computational load to facilitate future work based on more general vehicle model, since it is plausible that a number of model limitations might have influenced the results obtained.
The following are some of our aims:
1. proof-of-concepts experiments for mobile robot navigation on unknown indoor, structured environments
--SLAM is the problem that the robot needs to build the map of the exploring environment and localize itself simultaneously. Accurate localization requires an accurate map, and an accurate map cannot be built without accurate localization. We develop a 3D mapping system using a rotating laser range finder based on DT +linear simplex for map alignment
http://www.youtube.com/watch?v=ceBAgnJhack
--Waypoint navigation system in polar coordinate for limit cycle avoidance in unknown environment with nonconvex obstacles (e.g. U-shape or deadend) to complement goal-directed reactive navigation
http://youtu.be/m19xuV7E9R4
http://youtu.be/jGuq4SsEN9Q
--surveillance mobile robot for warning, alert, guarding, intruder detection by performing smoother wall following and human detection using interval type-2 fuzzy logic.The objective of the surveillance robot is to detect the intruders entering the perimeter of the territory and inform or alarm the defender.The system has two primary components, i.e. wall following based on interval type-2 fuzzy logic control and sensing of human in executing trajectories or actions.The human detection sensor allows the effective detection of a close-in human in the wall-following route, thus enabling wall following could be performed in a human friendly manner.
https://www.youtube.com/watch?v=aj_6KkYbbX4
https://www.youtube.com/watch?v=gdGf0CkaoNY
--The harmonic potential field (HPF) is efficient for mobile robots to generate smooth path without local minima for obstacle avoidance. We implemented the integration of the pure pursuit algorithm and HPF-based streamline paths compatible with the curvature constraint to develop an obstacle avoidance system. The system has been demonstrated works well in an environment with sparsely distributed circular obstacles that make possible the search of streamlines with curvatures within the vehicle''s nonholonomic constraint.
2.The ability to plan a path and then to follow the planned path on flat and non-flat terrain in an optimal fashion is central to indoor and outdoor mobile robots navigation. Such a planner requires an accurate geometric 3D environment map that encodes real terrain geometry. We propose a geodesic path planner based on gradient-descent of an energy function that is model independent, analytical and operate on the terrain modeled as a parametric surface or smooth manifold to compute a length minimizing path on a given surface. It generates continuously differentiable locally geodesic paths based on nonlinear geodesic equations determined by surface primitives on which the mobile robot moves. The planner developed is implemented in a planning application and validated through a simulated uav flight that follows the shortest length path towards the goal position.
3.streamline-based path planning
Harmonic potentail function (HPF) is the solution to two-point boundary value problem of Laplace equation in a closed domain. The occurrence of local minima (halting at a positions other than the target such as in very cluttered environments and tight spaces) can be eliminated by the HPF, enabling mobile robot navigation to the goal without stuck in local minim. The streamlines, defined as the gardient of HPF at each point of the domain, serve as the guidance vector field for navigation without trapped in local minima.Streamline-based path planning solves the navigation problem in a way that mimics a fluid flow in nature.The obstacle avoidance system we develop is built on a priori selection of streamlines around the circle, which are fast computing primitive paths derived from Laplace equation, thus making the system real-time. It integrates with pure pursuit for transition to another streamline for curvature-constrained mobile robot.To enhance the accuracy of gradient computation, log-space harmonic function is adopted.For guaranteed obstacle avoidance, the obstacle radius is enlarged to account for the robot radius and safety distance, where safety distance is set to compensate for localization errors and navigation accuracy. In the experiment, left turn is the default action for avoiding a detected obstacle and maintain at least 0.4m to the detected obstacle to be avoided.In narrow passage not considered for this proof of concept experiment, this default action may cause navigation difficulty if not impossible since the streamline can become infeasible when the curvature constraint is accounted for.
4. Time-optimal trajectory or path -constrained time optimal trajectory (speed planning): the objective function is the terminal time to finish the task ( as fast/early as possible). Time-optimal solution is platform -specific ( i.e. a dynamic model-based control problem requiring the consideration of vehicle dynamics in terms of dynamics model and its parameters and input constraint ) and path-dependent when moving along a given path.
-The objective of this study is to provide a safer and more efficient option for drivers in emergency situations. Specifically, the study seeks to compare the efficiency and maneuverability of various G2 S-shape lane-change parametric polynomial curves, to determine which path allows for a more aggressive velocity profile while adhering to the box constraints.To ensure a time-optimal speed planning, the lane change curve with a higher achievable velocity that was maintained for a longer period was found to be favorable, while taking into account the constraint of the rest boundary (where the vehicle initially starts from rest and comes to a full stop at the goal). On a straight road, a quintic Bezier curve was found to be sufficient for lane change.
-Time-optimal constant speed collision avoidance forward flight trajectory of Dubins'' vehicle via Hamilton-Jacobi (HJ) equation.The path must be flyable and safe where in our study both collision avoidance and minimum travelling time are simultaneously considered.
This part of work broadly aligns with the numerical optimal control framework that implements trajectory optimization ( indirect methods). Dubins vehicle is an ideal kinematic model of constant speed, constant altitude uav.The path planning problem is reformulated as a time-optimal control problem of Dubins vehicle with input constraint and safety constraint given by signed distance function.Boundary value problem of HJB equation (a first-order nonlinear PDE) satisfied by the value function is used to deal with the time-optimal or shortest flyable and safe path planning of unit speed curvature-constrained Dubins vehicle considering stringent requirements on maneuverability and power consumption.The time-optimal path corresponds to the characteristics of HJB equation with a heading normal to the evolving level set, where Dubins vehicle has three possible moves : forward, curvature-constrained left turn and right turn.
To solve a specific problem, a numerical solver among many others is instantiated. The assessment of accuracy or optimality has the impact on the choice of numerical solvers. We evaluated which scheme reduces more the drift caused by error accumulation in the computed state trajectory, thus better optimality / accuracy. We verify that consistent and convergent numerical schemes that guarantee near optimality are appropriate for near-shortest path generation in obstructed environments for Dubins vehicle. Two upwind finite -difference fast sweeping schemes （semi-Lagrange and Godunov Hamiltonian) on an equidistant grid and explicit Euler time-stepper with fixed time step size are employed to obtain viscolsity solutions of value function. Convergence and rate of convergence are investigated via numerical implementation by comparing the results on a series of successively refined grids. A comparative study of the accuracy or optimality of numerical solutions with comparison to analytical Dubins-like paths and Hybrid A* is provided.Resolution, which restricts the heading to a finite number of directions, highly affects the rate of convergence of the iterative scheme. The heading error, which is more significant than the position error, plays a dominant role in the convergence accuracy of numerical schemes.
-Motion generation by machine learning (models) is at the intersection of planning, learning and control .To endow the vehicle/robot with the ability to learn by leveraging the navigation/ steering knowledge from experience,
To endow the vehicle/robot with the ability to learn by leveraging navigation /steering knowledge from experience , model-based reinforcement learning (MBRL) first learns a forward dynamics prediction model （trajectories ）of the system from the dataset (a batch of measurements of the state of the system and the cumulative reward) and then and then plan and control using the learned dynamics model.MBRL is sample efficient, and its performance depends on the modeling error.
a need to generate feasible speed profiles when motion uncertainty (imperfect model knowledge or external disturbances) is considered: Determine the time-optimal control that produces at each point along the given path the maximum allowed velocity. The solution is the determination of switching structure in the phase plane.We consider a data-driven problem of inferring the near time-optimality of both the control and trajectory from observations of their rewards in executing a path following task in the contex of MBRL.
Aim: Learn the (feedback) control function for the vehicle with bounds on longitudinal acceleration to follow a path to reach a goal by maximizing the distrace traveled (as fast as possible), while mitigating the effects of external disturbances of the dynamics model and the uncertainty (inaccuracies) in model parameters, typically relys on replanning.We demonstrate a MBRL framework, PILCO ,is used as a planner to generate a near optimal velocity on a predefined linear track: PILCO can help design an acceleration controller able to learn near speed-optimized trajectory on a vehicle governed by double-integrator dynamics with uncertain mass and viscous friction subject to acceleration input magnitude constraint.
At first, the reward measures how fast the vehicle progresses (discounted sum of the length measured along the path per sampling time) on the track to reach the target (subject to tolerance corresponding to the bound on the uncertainty, which may be unknown) within the learning horizon in each simulated episode.This transforms the free terminal time of time-optimal control to fixed distance optimal control, which is easier and solvable. The algorithm models a distribution over transition functions (velocity profiles) controlled under admissible acceleration input (i.e. mapping from acceleration policy to the velocity ）at each sampled position along the path on the track with Gaussian Processes, successfully reducing the effect of model biases and parametric model uncertainties such as mass and friction coefficient over mass ratio can be explicitly learned within the context of a known damped double integator model. Then, trajectory prediction (through MBRL, we are provided with a finite set of trajectories for the vehicle in the specific state to state steering scenario) and policy evaluation are done through iterative cascades of Gaussian posteriors and deterministic approximate inference. Finally, analytic gradients (an explicit computation of analytic gradient of discounted cumulative reward function in terms of distance metrics, which typically approximate the minimum time to reach between states, with respect to actions is provided in PILCO) are used for policy improvement. A simulation and a sim2real experiment of an autonomous car completing a rest-to-rest linear locomotion is documented with reality gap to see how well learning control developed for simulation can be used to real vehicle or if online adaptation is needed for the real world experiment. Time-optimality and data efficiency of the task are shown in the simulation results, and learning under real-world circumstances （lateral slipping, Coulomb friction and aerodynamic drag are present) is proved possible with our methodology adapted to uncertainty in environment. Code is available at https://github.com/brianhcliao/PILCO_AlphaBot
5.TSPN and TSP-CPP
TSPN:a problem of practical relevance to data gethering in wireless sensor network using data mule （and other applications like using uav for mapping a region, and disinfecting an area) The network is composed of randomly deployed sensor nodes, which are data sources, and one data mule for carrying out data collection by traversal of whole network. The objective of TSPN is to find the minimum distance route that passes the neighborhood of each node.TSPN is composed of two asymmetric subproblems:determination of the order of visit ( a combinatorial problem) and the determination of visiting sites ( a continuous optimization problem).The two problems are coupled in the sense that the solution of one subproblem requires the solution of the other sub problem.We develop an efficient evolutionary path planner for a variant of Traveling Salesman Problem with Neighborhood (TSPN). We develop an open source code of a very scalable clustering-based genetic algorithm as TSPN solver : consisting of clustering the nodes and then routes these clusters
https://github.com/shao-you/CBGA,
which is very scalable and efficient due to the use of clustering and can deal with the general case of neighborhoods with varying radii and various amount of overlaps. This method leads to good solutions of TSPN in most of our simulations.
TSP-CPP
Coverage path planning (CPP) is a fundamental task to many applications for machining, cleaning, mine sweeping, lawn mowing, precision agriculture and performing missions with UAVs applied to such as geographic mapping,agricultural monitoring, aerial monitoring and surveillance, SAR, aerial picking and delivery ( e.g. medical supply), weather forecast and air quality monitoring to optimize the routing through the region. One approach for a known environment with obstacles is to decompose the environment into cells such that each cell can be covered individually. We can then decide the visiting order of cells to connect those intra-cell paths together. Finding the shortest inter-cell path that visits every cell and returns to the origin cell is similar to the traveling salesman problem (TSP), and the additional variation to consider is that there are multiple intra-cell paths for each cell resulting from different choices of entry and exit points in each cell, and therefore affect the inter-cell path. This integrated traveling salesman and coverage path planning problem is called TSP-CPP, similar to TSP with neighborhood (TSPN). The solution to TSP-CPP requires the simultaneously determination of visiting order of sites with null or minimum repetition and the transition points of each visiting site. Recent approaches for TSP-CPP include dynamic programming (DP) for TSP adapted to TSP-CPP, or determination using brute force enumerative search on combinations of entry and exit points of every cell and then solve each combination of entry and exit points with TSP-solver. Both of them suffer from exponential complexity and are prohibitive for complex environments with large number of cells. We propose an appropriate genetic algorithm implementation for TSP-CPP to achieve a good balance between efficiency and path optimality with number of cells beyond the limit of DP on a laptop PC. Our approach is demonstrated to find the true optimal solution as DP in all simulation environments and one hundred times faster than DP approach for maps decomposed with large cell number.
GA codes for TSP-CPP https://github.com/WJTung/GA-TSPCPP
6.collision detection
Collision detection in robotics has arisen primarily in the context of obstacle avoidance and path planning, in which robot geometry is tested for collision with geometric models of environment obstacles along the trajectory followed by the robot. For the application of assembly or path planning, the most expensive part of planning is ensuring that there is no collision between objects/ parts with increasingly more complex geometry and number of objects.
-Exact collision detection for scaling tolerance
In this work, we deal with collision detection of a pair of convex polyhedral with the variability in size (dimension) but not shape, i.e. scaled convex polyhedra.A methodology for quickly verifying two convex polyhedral parts, each with its size scaled by a factor, could be mated by identifying conditions under which we can achieve desirable mating configurations for toleranced polyhedral parts.
-Swept volume (sv) reconstruction using deep learning model of signed distance function
HRC system (e.g. robot-assisted surgery, feeding or dressing, etc. ) that makes the human and the robot working together in a shared workspace to complete a common task has been shown to increase considerably the overall capabilities of the system, but also increases the complexity and required effort for safe operation guarantee.
• Deep neural network is used to learn an implicit signed distance function from point clouds of swept volume in a way of reducing the memory barrier of discrete resolution methods such as grid-, mesh- or voxel-based methods. This technique is accurate and is extended to learning multiple swept volumes from the same trajectory class.
.The learned - SDF -guided collision detector can be used to determine the safety of a given interval so that a motion interval with no collision of workers in the human-robot collaboration could be estimated by SDF of SV generated by a given trajectory. Combining clearance and maximum speed between objects allows us to reduce the number of collision calls over guaranteed no -collision time interval.
demo of learned-sdf-guided continuous collision detection for pick-and-place task: https://youtu.be/dDvTaYYy9mk
An implementation of DRRT with SDF on GPU achieves 4x -62x faster path computation in a simulation in a simple dynamic environment with one moving AGV ,depending on AGV speed.
https://youtube.com/shorts/W1dtQ3viSEg?feature=share
7. G3 smooth path design
The range of diverse driving scenarios that a trajectory planning module should cope with makes the task of trajectory design very challenging.Generating smooth, kinematically feasible and safe trajectories with as low computation ( fast) as possible in practical scenarios such as urban driving is demanded by real-time safe operation of autonomous vehicles. G3 curves ensure comfort and smooth change in curvature derivative and acceleration. G3 Path/motion/ Trajectory planning is concerned with finding or computing or designing a subclass of all trajectories (of the the unicycle ) that connect the given terminal states/poses/configurations (I.e. meet the interpolation constraints) by composing path/motion primitives at some time instants (traversed time) with third order geometric continuity, where the orientation, curvature and curvature derivative at the starting and end points are prespecified.
To account for arbitrary nonzero curvature and arbitrary curvature derivative at either one or both ends of the curve and incorporation of constraints for handling more complex driving scenarios and road layouts, we proceed by the use of a single directional (forward), smooth turning path based on 7th degree Bézier curves to meet the curvature constraint. The family of 7th degree Bézier curves in our design is equivalent to an eta-3 spline interpolating imposed boundary conditions (two positions with associated tangent direction, curvature and curvature derivative
) with control points defined by two parameters of end speeds.The restriction of zero curvature at both ends of the path such as driving on straight roads is alleviated by the use of 7th degree Bézier curve.
In a given driving scenario, we consider the feasibility problem of simplified eta-3 spline trajectory planning that finds a feasible forward trajectory if one exists. A box constraint of direct product of two parameter intervals encodes the diversity of solutions, each is verified for feasibility.An advantage of using the 7th degree Bézier form is we can generate various paths through selection of suitable parameter values in a more intuitive manner. Specifying nonzero boundary curvature and incorporating collision avoidance (so that the vehicle remains within the road boundaries) at the same time can be achieved more easily and intuitively to some extent via the Bézier form , and its closed-form provides a low complexity of trajectory computation. It is used fordesigning the maneuvers of entering the roundabout or in-roundabout lane change by placing control points taking into account the entire lane width of a circulatory roadway.
Adopting this computationally simpler approach, the starting velocity and the ending velocity of the forward (single-directional) trajectory are the design parameters. The feasible space of path-defining parameters is reduced to a box. This allows us to find a feasible trajectory by iteratively searching the feasible parameter space for determining/ refining or direct optimization the control points of its natural equivalent 7th degree Bézier curve for a given maneuver time （the duration to execute the trajectory) T until a satisfactory G3 trajectory is found.
|
