- sidharthtalia

# Unified 3-dimensional state control using quasi-optimal trajectories

Updated: Apr 7

**Introduction:**

**In 2017, when I was working on the second iteration of my mini-self-driving car, I needed a lightweight local controller whose performance would look like that of an MPC without actually using an MPC. I came up with what I call the bezier curve control system. I’ve applied this system to many different kinds of problems, but they were always two-dimensional problems. I wanted to see if I could create a unified controller that would work in 3D by extending the bezier control system to work in all 3 dimensions. **

**My approach:**

**The system first creates a quasi-optimal trajectory in state space and then finds the control actions required to follow this trajectory. I came up with the Bezier control system when I ran into the problem of not having enough computational power to run an MPC (see also: **__being broke__**) but wanted MPC-like performance. **

**If I was using an MPC, the MPC would create a trajectory from the vehicle's current pose to the goal or to a point on the trajectory at its horizon. The optimal trajectory would, at a minimum:**

**Have the tangent aligned closely with the velocity vector's direction at the vehicle's current pose and goal pose.****Have**__C2 continuity__**.**

**Thus, instead of "finding" the optimal trajectory, it is possible to use the minimum conditions of optimality as boundary conditions for a parametric curve (Bezier curve). This results in a quasi-optimal state trajectory. The following figure illustrates this idea. The arrows at the end are the direction vectors corresponding to the boundary conditions.**

**There are, however, limitations to this, such as if the time horizon is too short, it results in significant curvature as the bezier curve tries to force the trajectory to connect to the reference. Such limitations are an artifact of the bezier system not caring about the model of the vehicle itself.**

**Another limitation of this system is that it can not handle direction changes (3 point turns can not be synthesized for a car).**

**The above figure shows an example of a Bezier curve with Normals and direction vectors being represented by the red and green arrows respectively. This is kind of what the controller "sees". Notice how the normal vector rotates around the trajectory. It indicates the direction of turning force required, and thus the "roll" around the trajectory. The direction vector points along the trajectory and gives me the body frame pitch and yaw changes.**

**The system is designed to follow an arbitrary trajectory, but I generate this trajectory by connecting discrete waypoints with bezier curves. I'm currently working on a research paper/systems paper on this particular controller, so please don't copy my work :P. I will open-source the code after it is published.**

**Future work:**

**Currently, the look-ahead distance is proportional to the speed. This makes sense for the normal use case because the faster you move, the farther you aim. However, for tight turns, it is better to have a shorter look-ahead. Dynamically varying the lookahead to minimize cross-track error is something I will look into in the future.**

**There are likely a few more issues that I haven’t even faced yet. I see this as an open-ended research problem. The code for this implementation is currently unavailable (if you're a potential Ph.D. advisor or looking for Ph.D. students and would like to see the code, please contact me on linkedIn.**