{"id":362,"date":"2020-03-26T19:15:10","date_gmt":"2020-03-26T23:15:10","guid":{"rendered":"http:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/?page_id=362"},"modified":"2021-09-08T18:26:53","modified_gmt":"2021-09-08T22:26:53","slug":"dynamics-and-controls-subsystem","status":"publish","type":"page","link":"https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/implementation\/dynamics-and-controls-subsystem\/","title":{"rendered":"Dynamics and Controls Subsystem"},"content":{"rendered":"<h1>Dynamics and Controls Subsystem Progress<\/h1>\n<p>The Dynamics and Control subsystem is where vehicle simulator development, vehicle dynamics and tire modeling, online model learning, and vehicle controls are designed and implemented. The initial vehicle model that has been implemented as a baseline is a dynamic bicycle&nbsp;model with parameters that map to our test vehicle platform.&nbsp;The next iteration would be to implement a neural network or model based reinforcement learning technique for representing the vehicle dynamics by taking in state feedback from simulation or on-board sensors and training the model to learn edge weights that allow accurate predictions of the vehicles future states.<\/p>\n<h3>January 6th, 2020<\/h3>\n<p>Initialized BitBucket repository that will be used for revision control and documentation of ongoing software development within this subsystem.<\/p>\n<h3>January 24th, 2020<\/h3>\n<p>After doing immense amounts of research on different environments that could be used for performing vehicle simulation, our team decided to move forward with developing our simulation within MATLAB\/Simulink. This had to do with the fact that compared to CARLA or Gazebo, we had more freedom for design choices, and had strong documentation and resources available from MathWorks, whereas the other two mentioned environments would limit our ability to model the nonlinear dynamics of our vehicle.<\/p>\n<h3>February 7th, 2020<\/h3>\n<p>A pipeline diagram that lays out the overall structure and flow of data transfer throughout the simulator and subsequent robot was created, which we believe will be useful to refer to when constructing our software. This allows us to make more informed decisions about inputs and outputs from various functions, and helps determine what data and methods should be encased in class objects.<\/p>\n<p><img loading=\"lazy\" decoding=\"async\" class=\"size-large wp-image-382 aligncenter\" src=\"http:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/03\/Software-Block-Diagrams-Simulation-1-683x1024.png\" alt=\"\" width=\"640\" height=\"960\" srcset=\"https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/03\/Software-Block-Diagrams-Simulation-1-683x1024.png 683w, https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/03\/Software-Block-Diagrams-Simulation-1-200x300.png 200w, https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/03\/Software-Block-Diagrams-Simulation-1-768x1152.png 768w\" sizes=\"auto, (max-width: 640px) 100vw, 640px\" \/><\/p>\n<p>&nbsp;<\/p>\n<h3>February 21st, 2020<\/h3>\n<p>First iteration of vehicle simulation was successfully implemented within MATLAB, with the first demonstration consisting of a simple PI longitudinal controller. A visualization of this can be seen below, where the blue rectangle represents the vehicle and the dotted red line represents the straight path:<\/p>\n<p><img loading=\"lazy\" decoding=\"async\" class=\"alignnone size-medium wp-image-383 aligncenter\" src=\"http:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/03\/PI-Controller-300x248.png\" alt=\"\" width=\"300\" height=\"248\" srcset=\"https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/03\/PI-Controller-300x248.png 300w, https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/03\/PI-Controller.png 664w\" sizes=\"auto, (max-width: 300px) 100vw, 300px\" \/><\/p>\n<p>The visualization script uses homogeneous transformations to plot how the vehicle travels through space.<\/p>\n<h3>March 6th, 2020<\/h3>\n<p>We have been prototyping various control algorithms for the lateral control of our robotic system. We are looking at methods that we can start off by implementing simplified versions, and then add in more complex features over time. We will be starting off with linearized tire dynamics, and fixed parameter vehicle model, and moving forward we will be implementing a fiala brush tire model based on data gathered during testing, along with implementing a learning-based approach to update model parameters on the vehicle as the system traverses a trajectory.<\/p>\n<h3>March 24th, 2020<\/h3>\n<p>So far for lateral control we have been prototyping a Stanley controller, which has provided stable control, but has also produced a fairly large cross-track error, as can be seen in the image below:<\/p>\n<p><img loading=\"lazy\" decoding=\"async\" class=\"size-medium wp-image-384 aligncenter\" src=\"http:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/03\/stanley_controller-300x247.png\" alt=\"\" width=\"300\" height=\"247\" srcset=\"https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/03\/stanley_controller-300x247.png 300w, https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/03\/stanley_controller.png 452w\" sizes=\"auto, (max-width: 300px) 100vw, 300px\" \/><\/p>\n<p>&nbsp;<\/p>\n<p>This method is currently being refined.<\/p>\n<h3><\/h3>\n<h3><strong>March 28th, 2020<\/strong><\/h3>\n<p>Today, we successfully produced a prototyping circuit for the low-level vehicle controls. We are currently using a PWM board from Adafruit, that we communicate with over I2C from our Jetson Xavier, to send control signals to our vehicle&#8217;s electronic speed controller (ESC). With this circuit, we have been able to adjust the steering angle and the drive motor speed. Going forward, this will be used as a way to prototype onboard vehicle controls, once they are transferred from simulation.<\/p>\n<p><img loading=\"lazy\" decoding=\"async\" class=\"size-large wp-image-423 aligncenter\" src=\"http:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/03\/new_test_circuit-768x1024.jpg\" alt=\"\" width=\"640\" height=\"853\" srcset=\"https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/03\/new_test_circuit-768x1024.jpg 768w, https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/03\/new_test_circuit-225x300.jpg 225w, https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/03\/new_test_circuit.jpg 800w\" sizes=\"auto, (max-width: 640px) 100vw, 640px\" \/><\/p>\n<p>&nbsp;<\/p>\n<h3>April 1st, 2020<\/h3>\n<p>Today, the a simplified version of the Stanley controller that has been worked on within simulation has been tuned and is now providing considerably better tracking performance. A test trajectory and the vehicle&#8217;s subsequent tracking performance can be seen below:<\/p>\n<div style=\"width: 640px;\" class=\"wp-video\"><video class=\"wp-video-shortcode\" id=\"video-362-1\" width=\"640\" height=\"480\" loop autoplay preload=\"metadata\" controls=\"controls\"><source type=\"video\/mp4\" src=\"http:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/04\/simulation_test_comp.mp4?_=1\" \/><a href=\"http:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/04\/simulation_test_comp.mp4\">http:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/04\/simulation_test_comp.mp4<\/a><\/video><\/div>\n<h3>April 10th, 2020<\/h3>\n<p>We have been able to make significant progress on getting a full version of the Stanley lateral control algorithm implemented properly, and then in turn testing and tuning control gains on various trajectories. This implementation is specifically based on the steering control law for a dynamic bicycle model, which can be seen below:<\/p>\n<p><img loading=\"lazy\" decoding=\"async\" class=\"wp-image-640 aligncenter\" src=\"http:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/05\/stan_control_law.png\" alt=\"\" width=\"339\" height=\"43\" srcset=\"https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/05\/stan_control_law.png 923w, https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/05\/stan_control_law-300x38.png 300w, https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/05\/stan_control_law-768x98.png 768w\" sizes=\"auto, (max-width: 339px) 100vw, 339px\" \/><\/p>\n<p>The main upgrade in the controller comes from adding in the k_d,steer gain terms into the control law. These terms allow the commanded steering angle to be damped and help prevent chatter in the control effort, along with assisting with stability in the control law.<\/p>\n<h3>April 17th, 2020<\/h3>\n<p>Significant time was spent converting the control algorithms that were tested and tuned in simulation into code that could be used onboard our robot. The simulation code is all written in MATLAB, while our onboard software is built within Python. All of the necessary code to control the robot has been written and committed to our BitBucket repository, which includes ROS callback functions that take messages from the motion planning and localization pipelines and pass them to the control pipeline.<\/p>\n<h3>April 20th, 2020<\/h3>\n<p>Over the past few days, the focus has been on integrating the Dynamics and Controls and Motion Planning pipelines within our simulation, which has ultimately been successful. This allows us to simultaneously prototype control algorithms, tune gains, and test motion planning algorithms all within one environment, which has immense utility for our project, since we can see how robust each subsystem is when changes are made to others, which will cut out a lot of time wasted during integration on the physical robot.&nbsp; This has allowed for more complex maps to be used for generating the simulation videos as well, with the videos representing our spring validation demonstrations #2 and #3 being shown below, respectively:<\/p>\n<div style=\"text-align: center\">https:\/\/youtu.be\/z3pD4KP2dxw<\/div>\n<div style=\"text-align: center\">https:\/\/youtu.be\/q2AJTw-yrGA<\/div>\n<p>&nbsp;<\/p>\n<h3>April 25th, 2020<\/h3>\n<p>Significant progress has been on the onboard software pipeline that is deployed on the physical robot. To show that the vehicle is capable of being controlled solely by the onboard computer, an open loop control test was performed. Here is the video:<\/p>\n<div style=\"text-align: center\">https:\/\/youtu.be\/6_oYPcoN3eM<\/div>\n<h3>April 27th, 2020<\/h3>\n<p>This video displays the final capabilities of the integrated dynamics, controls, planning, and localization pipelines within simulation for the spring semester. A complicated map with many varying radius turns are present, and the vehicle is able to track with great performance.<\/p>\n<div style=\"text-align: center\">\n<div style=\"width: 640px;\" class=\"wp-video\"><video class=\"wp-video-shortcode\" id=\"video-362-2\" width=\"640\" height=\"207\" loop autoplay preload=\"metadata\" controls=\"controls\"><source type=\"video\/mp4\" src=\"http:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/05\/MRSD_trajectory_20s.mp4?_=2\" \/><a href=\"http:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/05\/MRSD_trajectory_20s.mp4\">http:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/05\/MRSD_trajectory_20s.mp4<\/a><\/video><\/div>\n<\/div>\n\n\n<h3 class=\"wp-block-heading\">August 26th, 2020<\/h3>\n\n\n\n<p>The team has discussed what the system development priorities are for the upcoming semester, and have decided to focus on minor refinements to improve the controller code tunability and efficiency, but not on major overhauls. In the event that the rest of the system is brought to a coherent and functional state, more advanced controls techniques may be implemented.<\/p>\n\n\n\n<h3 class=\"wp-block-heading\">September 23rd, 2020<\/h3>\n\n\n\n<p>During a code review of the control code repository, it was found that the conversion of the throttle and steering command calculations to PWM signal values was resulting in a positive feedback loop that would cause the controller to consistently go unstable. These bugs were fixed and committed.<\/p>\n\n\n\n<h3 class=\"wp-block-heading\">October 9th, 2020<\/h3>\n\n\n\n<p>Feature added to the Interface and the vehicle controller, where steering and throttle commands are able to be sent over ROS to control the vehicle. This was implemented due to a hardware issue with the RC receiver, which prevented our team from being able to manual control the robot, whether <\/p>\n\n\n\n<h3 class=\"wp-block-heading\">October 15th, 2020<\/h3>\n\n\n\n<p>The team began focusing on testing the controller on hardware by starting with longitudinal control testing. This consisted of pre-generated trajectories that have the vehicle speed up, slow down, and arrive at various waypoints at various distances.<\/p>\n\n\n\n<h3 class=\"wp-block-heading\">November 4th<\/h3>\n\n\n\n<p>Recently, the team has been doing a significant amount of field testing in which the longitudinal and lateral controller gains have been being tuned. While we have seen improvement in path tracking capabilities, we have also noticed that the vehicle appears to track with dissimilar performance when tracking a left vs. right turn<\/p>\n\n\n\n<h3 class=\"wp-block-heading\">November 10th<\/h3>\n\n\n\n<p>Full vehicle tracking pipeline functioning well together. A video and a few examples plots of tracked trajectory performance can be seen below:<\/p>\n\n\n\n<figure class=\"wp-block-embed-youtube wp-block-embed is-type-video is-provider-youtube wp-embed-aspect-16-9 wp-has-aspect-ratio\"><div class=\"wp-block-embed__wrapper\">\nhttps:\/\/www.youtube.com\/watch?v=WhzDipRFfMg&#038;feature=youtu.be\n<\/div><\/figure>\n\n\n\n<h2 class=\"wp-block-heading\"><img loading=\"lazy\" decoding=\"async\" width=\"506\" height=\"406\" class=\"wp-image-1045\" style=\"width: 500px\" src=\"http:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/12\/image-6.png\" alt=\"\" srcset=\"https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/12\/image-6.png 506w, https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-content\/uploads\/sites\/41\/2020\/12\/image-6-300x241.png 300w\" sizes=\"auto, (max-width: 506px) 100vw, 506px\" \/><\/h2>\n","protected":false},"excerpt":{"rendered":"<p>Dynamics and Controls Subsystem Progress The Dynamics and Control subsystem is where vehicle simulator development, vehicle dynamics and tire modeling, online model learning, and vehicle controls are designed and implemented. The initial vehicle model that has been implemented as a baseline is a dynamic bicycle&nbsp;model with parameters that map to our test vehicle platform.&nbsp;The next <a href=\"https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/implementation\/dynamics-and-controls-subsystem\/\" rel=\"nofollow\"><span class=\"sr-only\">Read more about Dynamics and Controls Subsystem<\/span>[&hellip;]<\/a><\/p>\n","protected":false},"author":187,"featured_media":0,"parent":33,"menu_order":0,"comment_status":"closed","ping_status":"closed","template":"","meta":{"footnotes":""},"class_list":["post-362","page","type-page","status-publish","hentry"],"_links":{"self":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-json\/wp\/v2\/pages\/362","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-json\/wp\/v2\/pages"}],"about":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-json\/wp\/v2\/types\/page"}],"author":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-json\/wp\/v2\/users\/187"}],"replies":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-json\/wp\/v2\/comments?post=362"}],"version-history":[{"count":19,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-json\/wp\/v2\/pages\/362\/revisions"}],"predecessor-version":[{"id":1048,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-json\/wp\/v2\/pages\/362\/revisions\/1048"}],"up":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-json\/wp\/v2\/pages\/33"}],"wp:attachment":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2020teamb\/wp-json\/wp\/v2\/media?parent=362"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}