Localization, Planning and Controls of Ackermann Steering Robot

Ayush Chakraborty, Evan Lockwood, Allyson Hur, Krishna Suresh

Dec. 8th 2022

LOCALIZATION

TRACKING WHEEL HARDWARE

When attempting to control our steering based robot we noticed a large amount of drift in our odometry estimate when we were purely using the feedforward dynamics to integrate the robot’s current state. To a navX-MXP 9-axis IMU.

ODOMETRY IMPLEMENTATION

In order to track the robot’s odometry, we needed information about the displacement during the given time step and the direction of that travel. The displacement was obtained by tracking the current amount of steps elapsed since the last time step. This was extracted by reading the encoder, which returns the total number of incremented steps since initialization. The difference between the current value and the previous value becomes the total number of steps traveled during this time step. To convert this to fix this issue we wanted to implement a sensor based position tracking system. We are using a free spinning omni-wheel with a high resolution encoder to track the forward displacement of the robot as we drive. Additionally, we mounted an Inertial Measurement Unit to accurately measure the current robot heading which is fusing with the tracker wheel data to estimate the robot state. More specifically, we used physical displacement, we determined the arclength of the tracking wheel’s displacement based on the following formula:

Once the displacement has been computed, the x and y position of the robot needs to be updated by incrementing them by the components of displacement in both directions. This was decomposed based on the pitch obtained by the IMU. To obtain data from the IMU, we implemented a navX-MXP library in C++ [1]. Using this library, we created a ROS publisher that regularly publishes the current yaw of the IMU. Then, through a separate ROS node we subscribe to the IMU to constantly update the current yaw of the robot. Using this angle, the displacement is decomposed into horizontal and vertical components and used to increment the current x and y position of the robot.

PLANNING

MOTION PROFILING

[1]

Motion profiling is a controls algorithm that defines one-dimensional position, speed, and acceleration for every timestep. The main purpose of the algorithm is to help the robot move smoothly between two positions by determining the required acceleration and deceleration that impacts the forces on the system and the torque required from the motor. We utilized the trapezoidal motion profile rather than the triangular motion profile to sustain a specific maximum velocity for an amount of time and include applications where velocity is steady.

The rectangular portion of the trapezoid represents the constant velocity portion of the motion, where the time for constant velocity equals the distance the velocity is sustained for divided by the maximum constant velocity, so

The acceleration and deceleration portions of the motion profile are represented by the right triangles on each side of the rectangle. To determine how much time is used for acceleration and deceleration, the different parts of the trapezoid of summed up, with the base representing time and height representing velocity, so that

After plugging in the total distance and the time for constant velocity from before into the equation above, the result should be the sum of the time for acceleration and the time of deceleration, and since ta=td, the sum can be divided by 2 to solve each of their values.

REEDS-SHEPP

[4]

For this project, we will be using a Reeds-Shepp car for motion planning. As in a Dubins car, where the car can move forward, move in a left arc, and move in a right arc, a Reeds-Shepp car can use all the movements, in addition to moving in these directions backward. These types of cars are unable to move more abstractly (i.e. sideways, diagonal) due to their physical constraints; the only directions it can move in is based on the angle of the front tires and whether it is in forward.

A Reeds-Shepp car can be simplified into three parameters:

Where u₁ determines the direction of the car (-1 or 1 for reverse and forward, respectively) and u₂ is the positive or negative tangent of the max turning angle of the car.

CUBIC SPLINES

[2]

Cubic splines are piecewise cubic functions that interpolate a set of data points, which helps create a path for the robot where it can run smoothly. While linear interpolation has discontinuous derivatives at some of the points, cubic spline interpolations are continuous in the zeroth through second derivatives and pass through all the data points. We are using an existing implementation of cubic splines [3] to generate these paths.

RRT

Rapidly-exploring random trees (RRT) is a path planning algorithm used to create a tree of nodes for a robot to travel between in order to locate the shortest path between points A and B. The process is simple: nodes are randomly generated and a path is created between the newly generated node and the next closest node. But randomly generating nodes is time consuming and computationally expensive, so some logic and parameters are put in place to prevent any redundancies or points that are clearly impossible/inefficient to reach. For example, a maximum connection distance is set to make sure that nodes are not expanding too far into undesirable areas and if there is an obstacle between the new node and the next nearest node, then the node is not added. RRT is computationally fast compared to other path planning algorithms such as A* due to the maximum connection distance parameter, which decreases the amount of nodes necessary to find a valid path. However RRT is not the most optimal way to find the best path to the goal. Nodes are being randomly generated, so while it essentially guarantees it will find a valid path between A and B, the chances of it being the most optimal are slim.

RRT* builds upon the RRT framework and allows for an asymptotically optimal algorithm by incurring a higher computational cost. RRT* addresses the issues with RRT by allowing for edge rewiring to minimize the cost to each node. This is computed by finding each node in a specified search radius and checking if a re-connection allows the node to have a shorter distance to the start.

To create a motion planning algorithm for a Reeds-Shepp car, we combined used the RRT* framework with the unique motion of a Reeds-Shepp vehicle. By adding these curved paths to RRT* we can then calculate an optimal path to our endpoint.

CONTROLS

Ackermann Dynamics

One key element of this project we aimed to explore was model based control techniques which we can test and validate in simulation before applying to the real system. Since aim to perform full state feedback control we first needed to create a fully Markov dynamics model which relates the current state and control inputs to the first derivative of the state:

Where f is a nonlinear vector valued function that represents a system of coupled nonlinear ODEs. We define the state to be:

This system gives us the coupled dynamics as:

These equations were derived through an understanding of the relationship between the ICC (instantaneous center of curvature and the wheelbase of the robot)

[5]

Linearization

With the robot modeled as a nonlinear dynamical system we have a clear description of how control inputs impact the movement of the robot, however, many standard control algorithms rely on linear systems which can be exploited to provide closed form optimal control. Even though our system is nonlinear, we can use the tools of linearization to provide locally linear dynamics which approximate the robot’s behavior in a region near the linearization point.

We can first find the first order Taylor series approximation of the nonlinear system by computing the jacobian at the current state:

This provides us we a dynamics matrix which we can use to construct a dynamical linear system in the form:

Now that we have a linear system we will be able to perform optimal control for states in the nearby region to our current state through use of a Linear Quadratic Regulator.

LQR

LQR is a widely used control method for optimally controlling a linear system with quadratic cost. Using our previously computed linear system we now need to formulate the cost to reach a goal state in a quadratic form. The typical method for representing this cost is by summing the control effort and state error over all time. An additional two matrices are used to weight the individual control and state error values for the overall cost function.

Once we have this quadratic cost function, we are looking for the control policy which minimizes the cost and gives us optimal control.

The K matrix is a control matrix which correlates the current state to the best control input we need to take to achieve our goal state. So to achieve optimal control we need to find this K matrix such that it can be used in the system as such:

Now, with this quadratic cost and linear system we can find the K matrix by solving the Riccati equations which we did not focus on for this project. Instead, we used the python controls toolbox to convert from A, B, Q, and R matrices to the optimal K control matrix.

Finally, we now have a method for achieving a goal state given our current state:

However, attempting to directly approach a far away state results in inconsistent behavior due to the linearization being inaccurate for the long term optimal control policy. To fix this issue we are exploring nonlinear model predictive control methods but in the short term we have shifted to use a planned trajectory to follow rather than only inputting waypoints. We then used the cubic splines and motion profiles generated in the previous section to better pass through waypoints and more consistently control the robot:

REFERENCES

[1]https://www.motioncontroltips.com/what-is-a-motion-profile/

[2]https://stackoverflow.com/questions/53517842/cubic-spline-interpolation-vs-polynomial-interpolation

[3]https://github.com/AtsushiSakai/PythonRobotics/tree/master

[4] https://www.ri.cmu.edu/pub_files/pub4/pivtoraiko_mihail_2007_1/pivtoraiko_mihail_2007_1.pdf

[5] https://www.xarg.org/book/kinematics/ackerman-steering/

Page updated

Google Sites

Report abuse