I’ve been working on this paper for about a month now for english, and I felt that my professor just wasn’t enough of an audience for this as much as people on here are. I really hope that other programmers will be able to learn from what I’ve learned and possible even implement some of this stuff.
I would also LOVE any feedback on my research and what all of you have found in your programming experience so please comment below or dm me.
Thanks for reading!
In the VEX robotics Competition (VRC) programmers usually begin coding time-based algorithms for the autonomous portion of the competition. This results in inconsistent, inaccurate chassis movements, resulting in gaining a disadvantage for the remainder of the match.
Only some VEX programmers, sadly, recognize time-based algorithms are ineffective. The “big brained” programmers typically switch their motion logic to either: motion profiling, or a proportional integral derivative loop (PID). Using these advanced algorithms allows programmers (who have a small amount of time) to program a more consistent autonomous routine faster. In this paper, I am going to show research from experts and my own testing to determine the best algorithm for the most accurate control of the robot’s movement.
Now the two simplest ways to program an autonomous (besides time-based) are either using PID or Motion Profiling. If you aren’t already familiar with both of these algorithms, then you should. PID (a position, integral, and derivative) controller is the simplest of the closed loop controllers. A closed loop controller is simply a function with an input and an output with certain parameters that changes what the output will be (chassisControllerPid.cpp).
An example of a closed loop PID controller would be: a V5 motor turning a certain amount of degrees and getting the error (amount of degrees until the target degree) using the integrated encoder, and manipulating variables to control the output motor velocity or voltage. Thus making the motor slow down enough to be at the correct input (Gillard 2-16).
Motion profiling (sometimes called ramping) is somewhat similar to PID where it slows down to meet the desired target; however, it also limits the kinematics for speeding up to meet the desired target velocity (Lewin 1-5). Assuming the kinematics for the robot are correct, the motion profile is accurate.
Now PID and motion profiling isn’t everything in an autonomous routine. They have to be implemented properly. The best way to implement any algorithm in high school VEX robotics is to use three wheel encoder odometry. Odometry, if built and tuned correctly, is an absolute positioning system for skid-steer robots (a chassis where all wheels are parallel) both holonomic and nonholonomic.
Team 5225A, the Pilons, developed and published a paper that shows exactly how to implement odometry with a VEX robot. In this paper they give some suggestions on how to implement using odometry (with a control algorithm like motion profiling or PID) and one of such suggestions is: “[N]o parameters should indicate a value relative to your current position or orientation; they should be field-centric. This means you don’t say turn 25 degrees to the right, you say turn to face 90 degrees [clockwise] from the audience direction,” (5225, 9).
This is something that I believe should be used much more frequently in autonomous routines across the board. If you move or turn relative to the current position, the robot can be in the wrong position, and it will ruin the rest of your routine. Having a way to specify to turn to a direction or move to a certain point. rather than relative to the previous position or angle of the robot. Doing this will help your routine be more consistent, but not necessarily accurate for a given movement.
In addition to odometry making routines more consistent, there is another use of odometry enables the programmer to specify a path and follow it. This is called a path follower. Path followers are robot controllers that use odometry, and a motion algorithm to follow a path. Points are defined relative to the robot’s starting orientation and position, a path generated using a number of interpolation algorithms (Ravankar, 1). The path is then followed by the robot using a path follower.
One such algorithm is called pure pursuit. The algorithm works as, what Coulter says, “a method for calculating the arc necessary to get a robot back onto a path … We tend to think of the vehicle as chasing a point on the path some distance ahead of it - it is pursuing that moving point,” (Coulter, 3). Later in Coulter’s paper, it says that it is often compared to humans driving a car where they look at a point and move based off of that point. Basically, pure pursuit is a path following algorithm that purely follows a point that is specified on the path at a certain distance ahead of the robot’s position.
The first paper on pure pursuit is published by The Robotics Institute at Carnegie Mellon University, and written by Craig Coulter. In this paper, the Coulter says that the pure pursuit algorithm is being used on several professional robots that the author has helped to develop. This means that the pure pursuit algorithm is extremely robust, and has a lot of capability for programmers.
Now, at this point of the paper, I could reference and explain how other path followers such as the ramsete controller (Veness, 1) work; however, according to Elbanhawi’s research, “Pure pursuit outperformed other controllers under sto-chastic actuation condition and discontinuous paths. These kinematic controllers were limited to low speed and small changes in steering as they ignore the dynamics of vehicle, ” (Elbanhawi, 620).
However, this paper is mainly highlighting the PID and motion profiling algorithms. Just realize that, according to Coulter, the biggest disadvantages of pure pursuit are, “A sharp change in curvature can be requested at a high speed, causing the vehicle’s rear end to skid. The vehicle will not close on the path as quickly as desired because of the first order lag in steering,” (Coulter, 15).
However, the skidding has been solved by limiting the maximum speed of the vehicle based on the curvature of the path by a library that Theo Lemay has created from team 7842F, and the lag in steering is caused by the lookahead distance. This problem has been solved by a forum post on chiefdelphi by edf42001 that came up with a way to adjust the lookahead distance based on the distance from the path.
In the tests for this paper, I am going to compare the consistency and accuracy of two different algorithms. One algorithm will be motion profiling and the other algorithm will be using three PID loops together (as implemented in OkapiLib). Two of the algorithms are used for going straight. The first one is distance and the second one is angle. Using both of these algorithms will allow the robot to go in a straight line that is a certain distance away either forward or backward.
The second PID loop is for turning. The other algorithm is a motion profile controller. For this, two different controllers are to be used left, and right, to make turns and go straight. For this controller I am going to basically use the default algorithms from okapi (main.cpp). The implementation for these algorithms are in (main.cpp). I am going to use the ChassisControllerPID (chassisControllerPID.cpp), and two of the AsyncLinearMotionProfileController (asyncLinearMotionProfileController.cpp) class in a SkidSteerModel configuration (main.cpp).
During these tests, the battery will provide consistent voltage to the motors (so that voltage is always greater than 13 volts). For tuning PID, I will use George Gillards commonly used guide by VEX Robotics programmers to learn and tune their PID. I will use the information from the motion profiling pdf from Chuck Lewin on the topic for tuning profiling. Then I will log the kinematics of the robot. Each iteration of tuning will include executing a 24 inch movement or 90 degree turn both forward and back.
When these are both tuned as properly as possible, I will log each algorithm’s movement time, position, velocity, acceleration, jerk, and snap and put it all in a graph (kinematicsLog.cpp). These graphs will help visualize what each algorithm does in it’s process. After tuning each algorithm properly, I will then make and tune a simple three point Tower Takeover autonomous routine with our competition (tray) robot with each of the algorithms.
This will be the test to determine which algorithm is more accurate, and possibly a custom algorithm that is able to control the robot more effectively than either of the algorithms. The test will include five runs of both the motion profile and PID algorithm. If a test fails it will be considered invalid outlier the data from the test will not be used. While performing each test, the robot will be logging the kinematics and position data over time with a delay of 50 milliseconds (main.cpp).
In the process of writing this paper, the motion profile parameters weren’t tuned as well as they could have been implemented. I didn’t tune the motion profile as well as it could have been tuned (PID took ~9.5 seconds, while motion profiling took ~12 seconds for the routine) until it was too late, and a friend (Salmon from the VEX Robotics Unofficial Discord) pointed out to me that the motion profile movement should take about as long as the PID movement. Because of this, we are not going to take the amount of time that each algorithm takes into whether one is better. I might come out with another test where I have tuned the same robot’s motion profile better and modify this paper.
Ex. 1: Motion Profile Graph Beginning (RED: Position, BLUE: Velocity, Green: Acceleration)
Example one and two, of the tests performed, is the start of the autonomous. The robot starts with a cube in the tray, moves forward 12 inches, sets the intake to in full, moving forward 15 inches to grab two cubes, move backwards -5.5 inches, turning ~145 degrees, and sets the intake to out half power for half a second. In the process of the first movement, notice the motion
Ex. 2: PID Graph Start (RED: Position, BLUE: Velocity, Green: Acceleration)
profile’s acceleration. It starts to deviate from one another at the beginning of reaching the maximum velocity. With PID, the inconsistencies start to happen when it starts to slow down. They both need work to be more accurate, but you wouldn’t be able to tell the deviation unless it is graphed.
The same inconsistencies are minimal in both of the algorithms in the next several movements, however the PID algorithm shows an early start of being more inconsistent from run to run. This is because PID is a closed loop control algorithm, while a motion profile is an open loop control algorithm. A closed loop algorithm will adjust for subtle changes in the environment, while an open loop doesn’t adjust for them and you need to send in the extra variables and logic to make sure that the autonomous is correct.
Ex. 3: Motion Profile Graph Ending (RED: Position, BLUE: Velocity, Green: Acceleration)
In graphs three and four, the second part of the autonomous mode occurs. During this movement, the robot: moves forward 17 inches, sets the tilter for the tray to go up, delays 1.9 seconds, sets the intake to out half power, waits for the tilter to settle, sets the tilter down, and moves backward 17 inches. In the movement, there is a large change in the center of mass in the robot (the tray and cubes), so it is expected that there is more variance in the kinematics.
If you look at the first peak in acceleration, (green) in the second graph, one of the lines settled later compared to the other lines. This tends to happen with settlers because each run is different. Despite that some runs take a little bit longer, the algorithm still works, and the change in real life is so slim that you can’t tell.
The biggest thing that I noticed during the movement after placing a stack is: when the PID is run, it fully throttles the robot backwards. This resulted in vibrations in the tray and robot. While this can be fixed with having less vibration in the robot, the motion profile algorithm was just as smooth as usual with the normal parameters. This is a good example of the biggest flaw with PID: a common way to limit how fast the robot goes is to adjust the voltage.
Ex. 4: PID Graph Ending (RED: Position, BLUE: Velocity, Green: Acceleration)
However adjusting the voltage, with a PID loop, will adjust the gains of the loop enough to mess it up. To do this, lower the max output voltage, and adjust your parameters (retune) when making movements that need to accelerate slower. This process is tedious and annoying. It is common that there are two or three fully tuned PID variables to manage for a routine, and tuning one set of PID variables takes a lot of time.
Example five is a comparison of the position data of the robot during the autonomous routine between PID (purple) and motion profiling (orange). While PID has slight variations in following the path that can and will grow to accumulate error, the motion profiling movement is clearly not meant for precision and will accumulate error over time. This lack of precision is the biggest problem with motion profiling.
It just can’t compete with PID on precision, because it is inherently an open loop movement algorithm. In autonomous modes, this lack of precision is often combated by pushing the robot against a wall of the field to straighten the robot directly
Ex. 5: Motion Profile (orange) vs. PID (purple) Position
before performing an action that needs precision. This is a good way to combat this inaccuracy, but motion profiling just clearly can’t quite be as precise as PID.
In a recent Tower Takeover robot explanation on youtube, the programmer said something that has stuck with me, “We do all motion profiles everywhere, so we’ll run the motion profile and then the last like inch will be a PID loop,” (2114X Tower Takeover Robot Explanation). This is brilliant. They combine the best of both motion profiling and PID to make the movement accurate and kinematically consistent, however is there a better way?
The biggest problem with the default PID algorithm is that there is no way to adjust the initial movement kinematics to the robot’s specifications, so all that needs to be changed is: simply run a profile for the acceleration of the robot, and then start a pid loop that will decelerate the robot effectively. This adjusts the initial movement for PID (which isn’t factored into the algorithm). While this hasn’t been tested, if implemented properly, it would be vastly superior to the default PID algorithm, because it fixes the biggest problem with it.
Ex. 6: PID Forward and Back Movement (Blue is Position, while Red is Velocity)
The biggest problem with only using the motion profile algorithm is the fact that it can’t adjust to correct for disturbances (due to it being open loop). This can be solved by what PID does with the integral variable (ex. 6). It has an output for when the robot is at the very least voltage or velocity that it can be while still providing movement.
This can be implemented with the open loop motion profile algorithm by running the movement. After running the motion profile, it has a simple closed proportional loop that outputs the minimum moving velocity (either positive or negative depending on the error) with an exit loop settler. This will enable the robot to get to the desired position (be accurate) without running a PID loop.
In finale, it doesn’t matter which algorithm you use, because motion profiling and PID are arbitrary algorithms that get close to the right kinematics, but they aren’t perfect in all scenarios. Therefore, the implementation in OkapiLib could allow for much more accurate control of the robot. While odometry on top of these algorithms makes it much easier to consistently control, the target needs to be able to be reached accurately, and then check for disturbances with odometry to move based from the real time position feedback.
5225, Team. “Introduction to Position Tracking.” Technical Resources, The Pilons, 7 Oct. 2018, thepilons.ca/technical-resources/.
Alouache, Ali, and Qinghe Wu. "Fuzzy Logic PD Controller for Trajectory Tracking of an Autonomous Differential Drive Mobile Robot (i.e. Quanser Qbot)." The Industrial Robot, vol. 45, no. 1, 2018, pp. 23-33. ProQuest, https://search-proquest-com.ezproxy.uvu.edu/docview/1980917484?accountid=14779, doi:http://dx.doi.org.ezproxy.uvu.edu/10.1108/IR-07-2017-0128.
asyncLinearMotionProfileController.cpp. commit 6f2ad34a06f23b77e91a2cff63939bd7370c7f6b, OkapiLib, 2020. Github, github.com/OkapiLib/OkapiLib/blob/master/src/api/control/async/asyncLinearMotionProfileController.cpp.
chassisControllerPid.cpp. commit 5cec774037b57f2a0e1f8d70c32117990dc8bca3, OkapiLib, 2020. Github, github.com/OkapiLib/OkapiLib/blob/master/src/api/chassis/controller/chassisControllerPid.cpp.
chassis.cpp. revision 1, acetousk, 2020. Github, https://gist.github.com/acetousk/3912bd4ba60f614fe989c5c6e4c1f6d0.
Coulter, R Craig. “Implementation of the Pure Pursuit Path Tracking Algorithm.” The Robotics Institute, Carnegie Mellon University, Jan 1992, www.ri.cmu.edu/pub_files/pub3/coulter_r_craig_1992_1/coulter_r_craig_1992_1.pdf. Pittsburgh, PA. 23 Sep 2019.
edf42001. “Implementation of the Adaptive Pure Pursuit Controller.” Chief Delphi, Team 1712, 18 Aug. 2018, www.chiefdelphi.com/t/paper-implementation-of-the-adaptive-pure-pursuit-controller/166552.
Elbanhawi, M., et al. “Receding Horizon Lateral Vehicle Control for Pure Pursuit Path Tracking.” JOURNAL OF VIBRATION AND CONTROL, vol. 24, no. 3, pp. 619–642. EBSCOhost, doi:10.1177/1077546316646906. Accessed 13 Feb. 2020.
Gillard, George. “An Introduction to PID Controllers.” George Gillard, 22 July 2017, georgegillard.com/documents/2-introduction-to-pid-controllers.
Girbés, Vicent, et al. “Path Following Hybrid Control for Vehicle Stability Applied to Industrial Forklifts.” Robotics and Autonomous Systems, vol. 62, no. 6, June 2014, pp. 910–922. EBSCOhost, doi:10.1016/j.robot.2014.01.004.
kinematicsLogging.cpp. revision 1, acetousk, 2020. Github, https://gist.github.com/acetousk/7e6d64ffaa4ffba743bc7577748123ee.
Lewin, Chuck. “Mathematics of Motion Control Profiles.” Performance Motion Devices, Semantic Scholar, 2007, pdfs.semanticscholar.org/a229/fdba63d8d68abd09f70604d56cc07ee50f7d.pdf.
main.cpp. commit ae14d4bd982df6aff4e85126ac33334add1c959a, Team 914, 2020. Github, https://github.com/team914/muphry/blob/develop/src/main.cpp.
positionLogging.cpp. revision 1, acetousk, 2020. Github, https://gist.github.com/acetousk/7e6d64ffaa4ffba743bc7577748123ee.
Ravankar, Abhijeet, et al. “Path Smoothing Techniques in Robot Navigation: State-of-the-Art, Current and Future Challenges.” SENSORS, vol. 18, no. 9. EBSCOhost, doi:10.3390/s18093170. Accessed 13 Feb. 2020.
Veness, Tyler. “Ramsete Controller¶.” Ramsete Controller - FIRST Robotics Competition Documentation, WPILib Suite, 6 Jan. 2020, docs.wpilib.org/en/latest/docs/software/advanced-control/trajectories/ramsete.html.
“2114X Tower Takeover Robot Explanation.” Youtube, Cobra Kai, 14 Mar. 2020, https://www.youtube.com/watch?v=Vp-0t6fN26Y&feature=youtu.be&t=525.