ROS self-balancing vehicle case study (integration of robot operating system + modern control theory)

Before, in modern control theory, I have studied some inverted pendulums and self-balancing trolleys, and now I use the ROS+Gazebo environment to try it out.

ROS self-balancing robot simulation (robot operating system + modern control theory fusion case)

I found some cases are kinetic, Gazebo7 and previous versions are applicable. Appropriate modifications were made to make both melodic and noetic applicable.

Of course, urdf is basically universal. Most important URL:

Ugly Mode:

Oh, can't bear to look straight! ! !

Then changed the color scheme:

Trailer Mode--Robot dragging and roaming

Balance Mode - Pendulum Vertical

This is the most efficient case for learning ROS robotics and modern control theory:

If you want to check the previous material: Modern Control Theory Course Column

Tutorial link, but for the kinetic version:

Gazebo 9 API Documentation:

Ignition Math documentation:

If you compile the source code on Github directly, the style of painting is like this! ! !

It can basically be concluded that it is related to the Gazebo version upgrade, which is why the blog focuses on the Igniton Gazebo update.

Brief analysis and debugging

For errors such as GetSimTime, check the API documentation, it has been updated to SimTime, and the error interface can be replaced in the source code.

For the reason why math cannot be found, see the above picture, which has been updated to ignition::math::Vector3d. For modification.

Similarly for Pose, these are updated to ignition::math.

Well, the question has changed... so go ahead. Modify according to the above method such as Rot(), Pos().

After a long and little correction, we finally saw the light of day:

:~/RobTool/rsv_balance_simulator$ catkin_make
Base path: /home/relaybot/RobTool/rsv_balance_simulator
Source space: /home/relaybot/RobTool/rsv_balance_simulator/src
Build space: /home/relaybot/RobTool/rsv_balance_simulator/build
Devel space: /home/relaybot/RobTool/rsv_balance_simulator/devel
Install space: /home/relaybot/RobTool/rsv_balance_simulator/install
####
#### Running command: "make cmake_check_build_system" in "/home/relaybot/RobTool/rsv_balance_simulator/build"
####
####
#### Running command: "make -j8 -l8" in "/home/relaybot/RobTool/rsv_balance_simulator/build"
####
[  0%] Built target std_msgs_generate_messages_nodejs
[  0%] Built target std_msgs_generate_messages_lisp
[ 13%] Built target rsv_balance_gazebo_control
[ 13%] Built target std_msgs_generate_messages_cpp
[ 13%] Built target std_msgs_generate_messages_py
Scanning dependencies of target gazebo_rsv_balance
[ 13%] Built target std_msgs_generate_messages_eus
[ 13%] Built target _rsv_balance_msgs_generate_messages_check_deps_SetMode
[ 13%] Built target _rsv_balance_msgs_generate_messages_check_deps_SetInput
[ 13%] Built target _rsv_balance_msgs_generate_messages_check_deps_State
[ 39%] Built target rsv_balance_msgs_generate_messages_nodejs
[ 39%] Built target rsv_balance_msgs_generate_messages_lisp
[ 56%] Built target rsv_balance_msgs_generate_messages_eus
[ 82%] Built target rsv_balance_msgs_generate_messages_cpp
[ 91%] Built target rsv_balance_msgs_generate_messages_py
[ 91%] Built target rsv_balance_msgs_generate_messages
[ 95%] Building CXX object rsv_balance_gazebo/CMakeFiles/gazebo_rsv_balance.dir/src/gazebo_rsv_balance.cpp.o
[100%] Linking CXX shared library /home/relaybot/RobTool/rsv_balance_simulator/devel/lib/libgazebo_rsv_balance.so
[100%] Built target gazebo_rsv_balance

But this is not ok, there are the following errors at runtime.

Install:

  • sudo apt install ros-melodic-python-qt-binding

and modify

/home/relaybot/RobTool/rsv_balance_simulator/src/rsv_balance_rqt/src/rsv_balance_mode/balance_mode_widget.py:

#from python_qt_binding.QtGui import QWidget
from python_qt_binding.QtWidgets import QWidget

Start the balance car case:

:~/RobTool/rsv_balance_simulator$ roslaunch rsv_balance_gazebo simulation_terrain.launch 
... logging to /home/relaybot/.ros/log/613e7c6e-fb51-11ea-9752-ba9c53c9fb50/roslaunch-TPS2-30059.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://TPS2:45073/

SUMMARY
========

PARAMETERS
 * /gazebo/enable_ros_network: True
 * /robot_description: <?xml version="1....
 * /rosdistro: melodic
 * /rosversion: 1.14.7
 * /use_sim_time: True

NODES
  /
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    urdf_spawner (gazebo_ros/spawn_model)

auto-starting new master
process[master]: started with pid [30073]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 613e7c6e-fb51-11ea-9752-ba9c53c9fb50
process[rosout-1]: started with pid [30084]
started core service [/rosout]
process[gazebo-2]: started with pid [30090]
process[gazebo_gui-3]: started with pid [30096]
process[robot_state_publisher-4]: started with pid [30101]
process[urdf_spawner-5]: started with pid [30102]
[ WARN] [1600613754.174422899]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[ INFO] [1600613754.549975972]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1600613754.551410505]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1600613754.597940953]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1600613754.599289678]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[INFO] [1600613755.167975, 0.000000]: Loading model XML from ros parameter robot_description
[INFO] [1600613755.172976, 0.000000]: Waiting for service /gazebo/spawn_urdf_model
[ INFO] [1600613755.599306658]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1600613755.620806608]: Physics dynamic reconfigure ready.
[INFO] [1600613755.777683, 0.000000]: Calling service /gazebo/spawn_urdf_model
[INFO] [1600613756.714908, 0.001000]: Spawn status: SpawnModel: Successfully spawned entity
[ INFO] [1600613756.750089801, 0.001000000]: Starting plugin RsvBalancePlugin(ns = //)
[ WARN] [1600613756.750285869, 0.001000000]: RsvBalancePlugin(ns = //): missing <rosDebugLevel> default is na
[ INFO] [1600613756.751423093, 0.001000000]: RsvBalancePlugin(ns = //): <tf_prefix> = 
[ INFO] [1600613756.755910578, 0.001000000]: RsvBalancePlugin(ns = //): Subscribed to cmd_vel!
[ INFO] [1600613756.759582292, 0.001000000]: RsvBalancePlugin(ns = //): Subscribed to tilt_equilibrium!
[ INFO] [1600613756.760707215, 0.001000000]: RsvBalancePlugin(ns = //): Advertise odom on odom !
[ INFO] [1600613756.761440019, 0.001000000]: RsvBalancePlugin(ns = //): Advertise system state on state !
[ INFO] [1600613756.762112803, 0.001000000]: RsvBalancePlugin(ns = //): Advertise joint_states!
[urdf_spawner-5] process has finished cleanly
log file: /home/relaybot/.ros/log/613e7c6e-fb51-11ea-9752-ba9c53c9fb50/urdf_spawner-5*.log
[ INFO] [1600613764.060165362, 7.205000000]: RsvBalancePlugin(ns = //): Mode: balance
[ INFO] [1600613764.257566606, 7.397000000]: RsvBalancePlugin(ns = //): Mode: balance
[ INFO] [1600613764.442336366, 7.580000000]: RsvBalancePlugin(ns = //): Mode: balance
[ INFO] [1600613764.627132079, 7.762000000]: RsvBalancePlugin(ns = //): Mode: balance
[ INFO] [1600613764.929249870, 8.059000000]: RsvBalancePlugin(ns = //): Mode: balance
[ INFO] [1600613765.106441473, 8.232000000]: RsvBalancePlugin(ns = //): Mode: balance
[ INFO] [1600613765.273507683, 8.396000000]: RsvBalancePlugin(ns = //): Mode: balance
[ INFO] [1600613765.449217113, 8.567000000]: RsvBalancePlugin(ns = //): Mode: balance
[ INFO] [1600613765.601345806, 8.719000000]: RsvBalancePlugin(ns = //): Mode: balance
[ INFO] [1600613765.736628865, 8.852000000]: RsvBalancePlugin(ns = //): Mode: balance
[ INFO] [1600613812.917726058, 55.328000000]: RsvBalancePlugin(ns = //): Mode: balance
[ INFO] [1600613813.102688301, 55.509000000]: RsvBalancePlugin(ns = //): Mode: balance
[ INFO] [1600613813.273348434, 55.677000000]: RsvBalancePlugin(ns = //): Mode: balance
[ INFO] [1600613813.432053232, 55.833000000]: RsvBalancePlugin(ns = //): Mode: balance
[ INFO] [1600613813.722674889, 56.122000000]: RsvBalancePlugin(ns = //): Mode: balance

Use the rqt tool:

Finally, upload some code that is closely related to modern control theory.

A, B, C, D (definitions).

extern const float g_fWheelRadius;
extern const float g_fBaseWidth;    // Half of Base width
extern const float g_fI3;           // I3, horizontal inertia

// System matrices
extern const float A[4][4];          // A matrix
extern const float B[4][2];          // B matrix
extern const float C[4][4];          // C matrix
extern const float L[4][4];          // L matrix
extern const float K[2][4];          // K matrix
extern const float A_BK[4][4];       // A-BK matrix

Balance Control (Algorithm):

#include "rsv_balance_gazebo_control/balance_gazebo_control.h"

namespace balance_control
{

BalanceControl::BalanceControl() {}

/*!
* \brief Resets all state and control variables.
*
* Useful when instantiating and reseting the control.
*/
void BalanceControl::resetControl()
{
  t = 0;
  for (int i=0; i < 4; i++)
  {
    x_hat[i] = 0;
    x_adjust[i] = 0;
    x_r[i] = 0;
  }
  u_output[0] = 0.0;
  u_output[1] = 0.0;
}

/*!
* \brief Integrates control and models.
*
* Integrates control with Euler method.
* \param dt       Step period.
* \param x_desired        Input array[4] for goal state
* \param y_fbk       Array[4] for sensor readings
*/
void BalanceControl::stepControl(double dt, const double (&x_desired)[4], const double (&y_fbk)[4])
{
  t += dt;
  //**************************************************************
  // Set reference state
  //**************************************************************
  x_reference[theta]  = x_desired[theta]  + x_adjust[theta];
  x_reference[dx]     = x_desired[dx]     + x_adjust[dx];
  x_reference[dphi]   = x_desired[dphi]   + x_adjust[dphi];
  x_reference[dtheta] = x_desired[dtheta] + x_adjust[dtheta];

  //**************************************************************
  // State estimation
  //**************************************************************
  // x_hat - x_ref
  float x_hat_x_ref[4];
  x_hat_x_ref[theta]  = x_hat[theta]  - x_reference[theta];
  x_hat_x_ref[dx]     = x_hat[dx]     - x_reference[dx];
  x_hat_x_ref[dphi]   = x_hat[dphi]   - x_reference[dphi];
  x_hat_x_ref[dtheta] = x_hat[dtheta] - x_reference[dtheta];

  // A*(x_hat-x_ref)
  dx_hat[theta]   = A[0][0]*x_hat_x_ref[theta] + A[0][1]*x_hat_x_ref[dx]
                  + A[0][2]*x_hat_x_ref[dphi]  + A[0][3]*x_hat_x_ref[dtheta];
  dx_hat[dx]      = A[1][0]*x_hat_x_ref[theta] + A[1][1]*x_hat_x_ref[dx]
                  + A[1][2]*x_hat_x_ref[dphi]  + A[1][3]*x_hat_x_ref[dtheta];
  dx_hat[dphi]    = A[2][0]*x_hat_x_ref[theta] + A[2][1]*x_hat_x_ref[dx]
                  + A[2][2]*x_hat_x_ref[dphi]  + A[2][3]*x_hat_x_ref[dtheta];
  dx_hat[dtheta]  = A[3][0]*x_hat_x_ref[theta] + A[3][1]*x_hat_x_ref[dx]
                  + A[3][2]*x_hat_x_ref[dphi]  + A[3][3]*x_hat_x_ref[dtheta];

  // + B*u_output
  dx_hat[theta]  += B[0][0]*u_output[tauL] + B[0][1]*u_output[tauR];
  dx_hat[dx]     += B[1][0]*u_output[tauL] + B[1][1]*u_output[tauR];
  dx_hat[dphi]   += B[2][0]*u_output[tauL] + B[2][1]*u_output[tauR];
  dx_hat[dtheta] += B[3][0]*u_output[tauL] + B[3][1]*u_output[tauR];

  // y - C*x_hat - x_desired
  float yC[4];
  yC[0] = y_fbk[theta]  - (C[0][0]*x_hat[theta] + C[0][1]*x_hat[dx] + C[0][2]*x_hat[dphi] + C[0][3]*x_hat[dtheta]);
  yC[1] = y_fbk[dx]     - (C[1][0]*x_hat[theta] + C[1][1]*x_hat[dx] + C[1][2]*x_hat[dphi] + C[1][3]*x_hat[dtheta]);
  yC[2] = y_fbk[dphi]   - (C[2][0]*x_hat[theta] + C[2][1]*x_hat[dx] + C[2][2]*x_hat[dphi] + C[2][3]*x_hat[dtheta]);
  yC[3] = y_fbk[dtheta] - (C[3][0]*x_hat[theta] + C[3][1]*x_hat[dx] + C[3][2]*x_hat[dphi] + C[3][3]*x_hat[dtheta]);

  // L*(y-C*x_hat)
  dx_hat[theta]  += L[0][0]*yC[0] + L[0][1]*yC[1] + L[0][2]*yC[2] + L[0][3]*yC[3];
  dx_hat[dx]     += L[1][0]*yC[0] + L[1][1]*yC[1] + L[1][2]*yC[2] + L[1][3]*yC[3];
  dx_hat[dphi]   += L[2][0]*yC[0] + L[2][1]*yC[1] + L[2][2]*yC[2] + L[2][3]*yC[3];
  dx_hat[dtheta] += L[3][0]*yC[0] + L[3][1]*yC[1] + L[3][2]*yC[2] + L[3][3]*yC[3];

  // Integrate Observer, Euler method:
  x_hat[theta]  += dx_hat[theta]*dt;
  x_hat[dx]     += dx_hat[dx]*dt;
  x_hat[dphi]   += dx_hat[dphi]*dt;
  x_hat[dtheta] += dx_hat[dtheta]*dt;

  //**************************************************************
  // Reference model
  //**************************************************************
  float x_r_x_ref[4];
  x_r_x_ref[theta]  = x_r[theta]  - x_reference[theta];
  x_r_x_ref[dx]     = x_r[dx]     - x_reference[dx];
  x_r_x_ref[dphi]   = x_r[dphi]   - x_reference[dphi];
  x_r_x_ref[dtheta] = x_r[dtheta] - x_reference[dtheta];
  x_r[theta]   += (A_BK[0][0]*x_r_x_ref[theta] + A_BK[0][1]*x_r_x_ref[dx]
                    + A_BK[0][2]*x_r_x_ref[dphi] + A_BK[0][3]*x_r_x_ref[dtheta])*dt;
  x_r[dx]      += (A_BK[1][0]*x_r_x_ref[theta] + A_BK[1][1]*x_r_x_ref[dx]
                    + A_BK[1][2]*x_r_x_ref[dphi] + A_BK[1][3]*x_r_x_ref[dtheta])*dt;
  x_r[dphi]    += (A_BK[2][0]*x_r_x_ref[theta] + A_BK[2][1]*x_r_x_ref[dx]
                    + A_BK[2][2]*x_r_x_ref[dphi] + A_BK[2][3]*x_r_x_ref[dtheta])*dt;
  x_r[dtheta]  += (A_BK[3][0]*x_r_x_ref[theta] + A_BK[3][1]*x_r_x_ref[dx]
                    + A_BK[3][2]*x_r_x_ref[dphi] + A_BK[3][3]*x_r_x_ref[dtheta])*dt;

  // Adjust theta based on reference model
  float e_r_dx;
  // e_r_dx = x_r[dx] - x_hat[dx];
  e_r_dx = x_r[dx] - y_fbk[dx];
  x_adjust[theta] += (.025*e_r_dx)*dt;

  //**************************************************************
  // Feedback control
  //**************************************************************
  u_output[tauL] = -(K[0][0]*(x_hat[theta]-x_reference[theta]) + K[0][1]*(x_hat[dx]-x_reference[dx])
                  + K[0][2]*(x_hat[dphi]-x_reference[dphi]) + K[0][3]*x_hat[dtheta]);
  u_output[tauR] = -(K[1][0]*(x_hat[theta]-x_reference[theta]) + K[1][1]*(x_hat[dx]-x_reference[dx])
                  + K[1][2]*(x_hat[dphi]-x_reference[dphi]) + K[1][3]*x_hat[dtheta]);
}

/*!
* \brief Set up the output array.
*
* Returns the address of the actuator torque array[2]
*/
double *BalanceControl::getControl()
{
  return u_output;
}
}  // namespace balance_control

Specific model (parameters):

#include "rsv_balance_gazebo_control/control.h"

const float g_fWheelRadius = 0.19;
const float g_fBaseWidth   = 0.5;
const float g_fI3 = .85;

const float A[4][4] = {
  { 0, 0, 0, 1 },
  { -1.2540568855640422, 0, 0, 0 },
  { 0, 0, 0, 0 },
  { 6.059985836147613, 0, 0, 0 }};

const float B[4][2] = {
  { 0.0, 0.0 },
  { 0.1635280961687897, 0.1635280961687897 },
  { -0.09719802837298575, 0.09719802837298575 },
  { -0.15573631660299134, -0.15573631660299134 }};

const float C[4][4] = {
  { 1.0, 0.0, 0.0, 0.0 },
  { 0.0, 1.0, 0.0, 0.0 },
  { 0.0, 0.0, 1.0, 0.0 },
  { 0.0, 0.0, 0.0, 1.0 }};

const float L[4][4] = {
  { 28.04011263,  -2.25919745,  -0.        ,  10.92301101 },
  { -2.25919745,  28.93520042,  -0.        ,   5.07012978 },
  { 0.,  0.,  20.,  0. },
  { 10.92301101,   5.07012978,  -0.        ,   5.48639076 }};

const float K[2][4] = {
  {-117.29162704,  -14.14213562,   -7.07106781,  -53.23791934},
  {-117.29162704,  -14.14213562,    7.07106781,  -53.23791934}};

const float A_BK[4][4] = {
  {  0.        ,   0.        ,   0.        ,   1.        },
  { 37.10689605,   4.62527303,   0.        ,  17.41179119},
  { 0.        ,   0.        ,  -1.3745877 ,   0.        },
  {-30.47314609,  -4.40488822,  -0.        , -16.58215492 }};

-Fin-

 

Tags: ROS

Posted by syam92 on Sun, 15 May 2022 20:06:50 +0300