ROS自平衡车案例学习（机器人操作系统+现代控制理论融合）

zhangrelay

发布于 2020-09-22 11:18:16

1.2K0

发布于 2020-09-22 11:18:16

文章被收录于专栏：机器人课程与技术

之前，现代控制理论，研究过一些倒立摆和自平衡小车，现在用ROS+Gazebo环境尝试一下。

ROS自平衡机器人仿真（机器人操作系统+现代控制理论融合案例）

找了一些案例都是kinetic，Gazebo7及以前的版本适用。为了能使melodic和noetic都可适用，做了适当的修改。

当然urdf基本是通用的。

丑萌模式：

唉，不忍直视！！！

然后更改了配色：

拖车模式--机器人拖着摆漫游

平衡模式--摆垂直

这是一个学习ROS机器人和现代控制理论最高效的案例：

如需查阅之前资料：现代控制理论课程专栏

https://blog.csdn.net/zhangrelay/category_6161998.html

教程链接，但是为kinetic版本：

http://wiki.ros.org/RoboSavvy/RoboSavvy-Balance/Tutorials

Gazebo 9 API文档：

http://osrf-distributions.s3.amazonaws.com/gazebo/api/9.0.0/index.html

Ignition Math文档：

https://ignitionrobotics.org/api/math/6.4/index.html

如果直接编译Github上面的源码，画风是这样的！！！

基本可以断定与Gazebo版本升级有关，这也是为何博客以Igniton Gazebo更新为主的原因。

简要分析并调试

对于GetSimTime这一类报错，查看API文档，已经更新为SimTime，在源码中替换错误接口即可。

对于math无法找到的原因，查看上图，已经更新为ignition::math::Vector3d。对于修改即可。

Pose同理，这些都更新到ignition::math。

恩，问题变了……那么继续吧。依据上述方式修改如Rot()，Pos()。

经过漫长的一点点修正，最后见到曙光：

:~/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

但这并非就ok了，在运行时还有如下错误。

安装：

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

并修改

/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

启动平衡车案例：

:~/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

使用rqt工具：

最后，上传一些和现代控制理论密切相关的代码吧。

A, B, C, D（定义）。

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

平衡控制（算法）：

#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

具体模型（参数）：

#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 }};

本文参与腾讯云自媒体同步曝光计划，分享自作者个人站点/博客。

原始发表：2020/09/20 ，如有侵权请联系 cloudcommunity@tencent.com 删除

网站