Advanced Controls/Motion Planning

During my graduate studies I have participated in a variety of smaller robotic controls and motion planning projects. Here is a collection of some of those projects:

A* Graph Search

To solve the challenge of having a mobile robot catch a moving target, I employed the informed A* path planning algorithm. The heuristic (h value) used is the ideal heuristic for an 8-connected grid: (sqrt(2)) * (min(deltaX,deltaY)) + (max(deltaX,deltaY) – min(deltaX,deltaY)). The Informed A* algorithm is guaranteed to generate an optimal path. Completeness is ensured given the map is finitely large and a valid path exists between the robot and goal. This algorithm can consistently find fully optimal paths within 0.05 seconds for the smaller test map, and 0.25 seconds for the larger test map.
The planner is written fully in c code. The open list is implemented as a binary heap priority queue, while the closed list is implemented as a boolean variable associated with each node in the graph. The graph is explicitly represented as a grid matrix, where each element in the matrix represents a node in the graph. Each element in the matrix stores a cell structure containing the h value, g value, backpointer index, and a closed list boolean for that node. This implementation is shown to be fast and reliable in practice.

Robot trying to catch a moving target with a A* graph search planner on an 8-connected grid. The full path is re-planned at every time step.

RRT-Connect Search

To solve the challenge of moving an N degree of freedom arm to a target arm configuration, I employed the RRT-Connect algorithm. The search step size (epsilon) is set to pi/25, which gave relatively good quality paths while keeping run-time fast. Each path extension is discretized into pi/100 segments and then each segment is checked for validity. This fine discretization ensures the robot is never in an invalid configuration during the search. The run-time of the algorithm consistently solves in less than 0.1 second for 5 DOF arms, and usually solves in 0.025 seconds or less. This algorithm can guarantee search completeness in the limit of samples, meaning that a valid path will be found if one exists given infinite samples. On the other hand, no such guarantees on optimality are given. Some post-processing is done to improve the quality of the paths, but there is no guarantee on these improvements. The post-processing algorithm used is the short-cutting algorithm.

5DOF plan
Joint-space plan for a 5 degree-of-freedom robot arm between a initial and goal configuration while avoiding obstacles.

Iterative Learning Control

A helicopter-like system, with a single horizontal and vertical rotor, was studied to explore iterative learning control useful for feed-forward control of repeated tasks.aerocopter

The device accepts two inputs in the form of vertical and horizontal rotor motor voltages and outputs both the pitch and yaw angles of the rotary mechanism.

Iterative Learning Control was combined with the existing feedback controller using a parallel architecture (Non-causal Iterative Learning Control), where the learned feed-forward signal is directly added to the control signal for improved tracking performance. The architecture is shown below in block diagram notation.


The effectiveness of ILC in improving the tracking performance of the aero system to a square wave can be seen in the following plots:


The final controller with the combined feedback control law and feed-forward inputs learned through ILC were implemented on an Arduino embedded system. The square wave tracking performance can be seen in the following image:

quanser aero

Inverse Kinematics Through Optimization

The inverse kinematics problem for a robot arm of arbitrary number of actuated links is difficult to solve analytically. Here the problem is posed as a function optimization with the objective function cost being the difference between desired pose and calculated pose (through straightforward direct kinematics) of the robot end-effector. A model of a snake robot was created that orients the distal link (gripper) to a desired position and orientation while avoiding obstacles and satisfying joint limits and constraints.

In order to optimize for a set of joint angles that specifies a given robot configuration, the forward kinematics of an arbitrarily long snake robot was derived. The forward kinematics were derived assuming that the snake was constructed of N links, where N was between 1 and 10 links. In addition, it was assumed that each link was connected by a series of rotary actuators which rotated in the euler angles of roll, pitch, and yaw, in that order. After these assumptions were made, the forward kinematics were derived by utilizing a series of homogeneous transformation matrices. For each link, a homogeneous transformation matrix for the roll rotation, pitch rotation, yaw rotation, and link displacement is defined. Then, the overall homogenous transformation matrix between the global coordinate frame and the end-effector coordinate frame can be computed by simply multiplying all of these transformation matrices together. Because this derivation is recursive, it is straightforward to write an algorithm that can calculate the forward kinematics for an N link snake.

Objective Function:
1) Final pose cost: The final pose cost is computed by taking the 2-norm of the vector difference between the current pose (computed with forward kinematics) and the desired pose specified by the function. This cost is multiplied by a weight of 5 because it has high priority.
2) Joints near limit cost: The joint limit cost is computed by first taking the mean of the joint limits for each type of joint (roll, pitch, yaw). Then, the current joint angle of each joint is subtracted by its joint limit mean, and then the 2-norm of this difference matrix is found. This cost is multiplied by a weight of 0.25 because it does not have very high priority. 1 The final cost of the solution of the snake robot is then the sum of the joint angle cost and the final position cost.
Constraints: One inequality constraint ensures that the snake robot never enters any obstacles. This was computed by first discretizing each link into a set of n points, where n was 10 in our algorithm. The distance of each of these points to every obstacle sphere is then computed, and the closest point to each obstacle is saved. If the closest point to the obstacle is within the obstacle then a check variable is set to +1, and if it is not within the obstacle it is set to 0. The check variables for each obstacles are then summed and saved as an inequality constraint. That way, if that sum is positive (some points are within obstacles), it will not satisfy the inequality constraint.
Bounds: In addition to having a soft cost in the criterion function, the upper and lower joint limits are also rigidly constrained.

This optimization problem was solved using both Matlab’s fmincon function, and CMAES, a evolutionary optimization solver.


Gradient Descent for Inverse Kinematics of a Snake Robot
Gradient Descent for Inverse Kinematics of a Snake Robot

Path Planning Algorithm Comparison

Global Path-Planning algorithms are used to find a “shortest” allowed path from one vertex to another through a connected graph when all environment information is known. Some commonly used algorithms for global path planning are Dijkstra’s algorithm and A* algorithm. RRT (rapidly-exploring random trees) algorithm, in comparison can plan a path in configuration-space without generating a connected graph, but instead building a probabilistic tree. These three algorithms for some given randomized environment will be compared using the metric of time required for the algorithm to compute an optimal path for different environment sizes.

Graph Search

Dijkstra’s algorithm works as follows:
1) Assign every node an infinity distance value and the start node as 0 distance. Put all nodes into an unvisited set.
2) Set the start node as the current node and mark as visited.
3) Check the distances of all neighboring nodes to the current node and add current distance. If this new distance is shorter than then current distance assigned to each distance, assign that shorter distance as the new distance.
4) Mark the current node as visited.
5) Select the unvisited node with the smallest distance to start as the new current node and go back to step 3.

A* is very similar to Dijkstra’s algorithm, with the only difference being that in step 5, instead of selecting the unvisited node with the smallest distance to the start node as the new current node, a heuristic cost value is added to the distance value. In the case of a mobile robot in a 2D environment, this cost function is the Euclidian distance of the current point with the goal point. Then the unvisited node with the smallest cost function is chosen as the next current node. This increases the speed of the algorithm by ensuring that less unnecessary nodes are checked.


Probabilistic Search

RRT, as a random sampling method, does not rely on graph representation to function properly, so the environment map can be used as is with no modification. RRT works by:
1.Randomly sampling a point in configuration space and checking if this point is in an valid (it is not in an obstacle).
2.If this point is not in an obstacle, the nearest vertex in the tree is found that can be connected to the point without the line created passing through an obstacle.
3.A segment is added in the tree connecting the randomly sampled point and the nearest tree vertex.
4.Continue from step 1 until the randomly sampled point is within a certain bound from the goal point. Then create a tree segment between that point and the goal point.

This algorithm can be improved by adding a sampling bias in which at each random sample, there is a 10% chance that the point sampled will be the goal point. This improves the speed and quality of the algorithm. RRT is not guaranteed to find the shortest path, but only guaranteed to find some path between the start and goal points. Because paths found by RRT are usually jagged and non-optimal, these paths can be improved by smoothing the path afterwards.

RRT 2D Path Planning

I then ran an experiment in which I ran Dijkstra’s, A*, and RRT to find paths in randomly generated environments of varying grid size. For each grid size, 50 random environments were generated and the three algorithms were run on these environments. The amount of time that each algorithm took to complete was logged, and the average of these logged speeds for each grid size can be found in the plot below. As can be seen, A* is always roughly twice as fast as Dijkstra’s algorithm, and RRT is slower for small grid sizes, but much faster for any grid size above 70.