之前，现代控制理论，研究过一些倒立摆和自平衡小车，现在用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 }};
-Fin-
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明ROS自平衡车案例学习（机器人操作系统+现代控制理论融合）！
