Advanced Controls Topics

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

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.