• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

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

人工智能 zhangrelay 1820次浏览 0个评论

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

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

  找了一些案例都是kinetic,Gazebo7及以前的版本适用。为了能使melodic和noetic都可适用,做了适当的修改。   当然urdf基本是通用的。   丑萌模式:   ROS自平衡车案例学习(机器人操作系统+现代控制理论融合)   唉,不忍直视!!!   然后更改了配色:   ROS自平衡车案例学习(机器人操作系统+现代控制理论融合)   拖车模式–机器人拖着摆漫游   ROS自平衡车案例学习(机器人操作系统+现代控制理论融合)   平衡模式–摆垂直   这是一个学习ROS机器人和现代控制理论最高效的案例:


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

Gazebo 9 API文档:

Ignition Math文档:


如果直接编译Github上面的源码,画风是这样的!!!   ROS自平衡车案例学习(机器人操作系统+现代控制理论融合) ROS自平衡车案例学习(机器人操作系统+现代控制理论融合) ROS自平衡车案例学习(机器人操作系统+现代控制理论融合)   基本可以断定与Gazebo版本升级有关,这也是为何博客以Igniton Gazebo更新为主的原因。  

简要分析并调试

ROS自平衡车案例学习(机器人操作系统+现代控制理论融合)   对于GetSimTime这一类报错,查看API文档,已经更新为SimTime,在源码中替换错误接口即可。   ROS自平衡车案例学习(机器人操作系统+现代控制理论融合)   对于math无法找到的原因,查看上图,已经更新为ignition::math::Vector3d。对于修改即可。   ROS自平衡车案例学习(机器人操作系统+现代控制理论融合)   Pose同理,这些都更新到ignition::math。   ROS自平衡车案例学习(机器人操作系统+现代控制理论融合)   恩,问题变了……那么继续吧。依据上述方式修改如Rot(),Pos()。   ROS自平衡车案例学习(机器人操作系统+现代控制理论融合)   经过漫长的一点点修正,最后见到曙光:  

:~/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了,在运行时还有如下错误。   ROS自平衡车案例学习(机器人操作系统+现代控制理论融合)   ROS自平衡车案例学习(机器人操作系统+现代控制理论融合)   安装:

  • 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工具:   ROS自平衡车案例学习(机器人操作系统+现代控制理论融合) ROS自平衡车案例学习(机器人操作系统+现代控制理论融合)


最后,上传一些和现代控制理论密切相关的代码吧。   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 }};

  -Fin-


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明ROS自平衡车案例学习(机器人操作系统+现代控制理论融合)
喜欢 (0)

您必须 登录 才能发表评论!

加载中……