首页
学习
活动
专区
工具
TVP
发布
社区首页 >专栏 >ROS自平衡车案例学习(机器人操作系统+现代控制理论融合)

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

作者头像
zhangrelay
发布2020-09-22 11:18:16
1.1K0
发布2020-09-22 11:18:16
举报

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

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

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

当然urdf基本是通用的。

丑萌模式:

唉,不忍直视!!!

然后更改了配色:

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

平衡模式--摆垂直


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

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


教程链接,但是为kinetic版本:

Gazebo 9 API文档:

Ignition Math文档:


如果直接编译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 删除

本文分享自 作者个人站点/博客 前往查看

如有侵权,请联系 cloudcommunity@tencent.com 删除。

本文参与 腾讯云自媒体分享计划  ,欢迎热爱写作的你一起参与!

评论
登录后参与评论
0 条评论
热度
最新
推荐阅读
目录
  • 简要分析并调试
领券
问题归档专栏文章快讯文章归档关键词归档开发者手册归档开发者手册 Section 归档