|
Carnegie Mellon University |
|
The Robotics Institute. |
|
Robotic Motion Planning, 16735: Project |
|
A Computational design tool based method for Path Planning |
|
Project Goals: · Develop a new optimization (like topology optimization, not incremental) method to generate paths from the start to goal configurations · Implement these paths on the Nomad Scout robot. · State estimation using Kalman filter.
Path Planning I wanted to develop a new optimization based path planning method which relied on techniques borrowed from those used in optimal design of structures. Topology optimization is a technique used to optimally place material in a given domain such that the structure performs the desired function. The most common (definitely not the only one) function required of a structure is that it be stiff. Topology optimization can then be used to optimally distribute material over a given domain (given a certain volume of material) to obtain the stiffest structure.
The above example shows the initial ground structure on the left in which all the truss elements have equal thickness and the stiffest structure obtained from topology optimization (for details refer to Topology Optimization: Theory, Methods, and Applications by Martin P. Bendsoe, Ole Sigmund). The interesting thing to notice is that for the stiffest structure problem the optimal structure always has the maximum material placed around the force lines.
Using this observation we adapt the method to generate paths between start and goal configurations. The idea is discretize the map into a number of nodes, connect each of the two adjacent nodes by beam elements. The design variable for the optimization process is the in plane thickness of the beam. Initially all the elements are assigned a uniform width. A force is then applied between along the line joining the start and goal points. The structure is then optimized for maximum stiffness. The path is then formed by connecting the thickest elements between the start to goal. An idea of how the method is expected to work is shown below.
Formally, the Objective function for the optimization process is given by . x is the vector of
design variables, SE is the strain energy of the structure, K is the stiffness of the structure, U is the vector of admissible displacements at the nodes. Linear finite element method was used to solve for the displacements and the stiffness, gradient of the objective function and the objective function . fmincon from MATLAB was used to carry out the optimization. Some paths obtained in free space (no obstacles) are shown below.
To include obstacles in the formulation, for a node marked as an obstacle all elements connected to it were constrained to have zero thickness throughout the optimization procedure. Below are a few examples of paths obtained with obstacles in the workspace.
Kalman Filter To estimate the state of the system the kalman filter was used. The state of the robot is given by the vector
where x,y are the position co-ordinates of the robot in a fixed ground frame and the θ is the heading angle of the robot. The sensor used by the robot gives information of the heading angle θ and the velocities So we have the state transition equation given by :
The Kalman filter does not include knowledge of the control and hence it is not included in the state transition equation. v(k) is the process noise.
Here t is the time interval between each state update. This depends on the time interval between every call to the program running the robot. This time can be found experimentally by running the robot studying the time interval, or it can also be updated online while the robot is running. The sensor model is as given below:
Where y is the sensor reading, w(k) is the sensor noise and
The Kalman filter follows the standard predict-update procedure using the above system model, sensor model and noise covariance matrices. The state of the system is updated at every step using the kalman filter. So at each iteration as the robot moves, the robot has knowledge of its previous state estimate, system covariance and the current sensor readings and calls the kalman filter to use this information to provide its current state estimate and system covariance. Shown below is a trial run of the Nomad Scout Robot. The unfiltered state is in blue while the state estimate using Kalman filter is in red.
Implementation on the Nomad Scout Robot
· It is assumed that the robot knows the map of the world. · Given the map, a feasible path from start to goal is computed using the algorithm described previously. · The algorithm runs in MATLAB, which generates a file containing the (x,y) co-ordinates and the heading angle at each node. The robot turns only at the nodes. · The robot then starts at the first node and moves along the heading direction at each node until it finds the next node. · At every step the robot maintains an estimate of its state and system covariance, based on the previous state estimate, system covariance and current sensor readings given to the kalman filter. · It uses the state estimate to follow the path generated by the algorithm using a simple curve following algorithm. It also uses the state estimate to detect if it has arrived at a new node.
Above a picture of the robot in the environment is shown, and the path taken by the Scout robot is shown next to it. Every point on the path is the estimated state from the kalman filter.
|
|
Movie: |
|
Movie: |