倒立摆建模仿真实验

好叭，这是篇课程作业，哈哈哈哈哈水一篇。

一、研究背景

倒立摆是控制理论、机器人学和自动化领域中的一个经典研究模型。倒立摆的动力学原理与双足机器人、平衡车、无人机，以及火箭发射时的姿态控制、导弹制导等平衡控制场景高度相关。研究倒立摆有助于解决这类系统的实时稳定控制问题。

二、问题描述

关于倒立摆的问题描述如下：

image-20250528205623453

本例中取小车质量 $M=2kg$，摆杆质量 $m=0.1kg$。后面建模里 $l$ 按“转动支点到摆杆质心的距离”理解，并代入 $l=0.5m$。现需控制小车的作用力 $F$，使得摆杆保持竖直状态。

三、数学建模

对于图中模型，现做以下标注：

image-20250528211816213

牛顿欧拉方法

从传统的力和角度的分析，根据牛顿第二定律可写出摆杆在水平方向和竖直方向的力平衡方程：

根据摆杆质心力矩平衡，可列出方程：

最后，写出小车的力平衡方程：

综合以上四式，得：

化简消去中间变量 $H$、$V$ 得：

在平衡状态周围，$\theta$ 较小，所以有 $\sin\theta \approx \theta$、$\cos\theta \approx 1$，进而得：

选取状态变量 $x_1=\theta$、$x_2=\dot{\theta}$、$x_3=y$、$x_4=\dot{y}$，得到状态方程为：

其中摆杆绕质心的转动惯量为：

将参数 $M=2$、$m=0.1$、$l=0.5$、$g=9.8$ 带入状态方程得：

一般性推广，本例中旋转件为摆杆。若旋转件换做其他形状的刚体，则只需计算出零件对于其质心的转动惯量 $I$，以及转动支点距离摆杆质心的距离 $l$，其对应的状态方程如下：

拉格朗日法

直接列拉格朗日方程，其中取 $q_1=y$、$q_2=\theta$：

接下来计算 $\frac{\partial L}{\partial \dot{q_1}}$、$\frac{\partial L}{\partial \dot{q_2}}$，也就是 $\frac{\partial L}{\partial \dot{y}}$、$\frac{\partial L}{\partial \dot{\theta}}$：

然后再带入拉格朗日方程，会发现结果跟之前牛顿欧拉方法求出来的一毛一样，实在是懒得一步一步敲公式了。

tips

转动惯量计算：

转动惯量是刚体抵抗角加速度能力的量度，离散情况下可以先记成 $I = m r^2$，连续刚体则写成：

其中 $dm$ 是微小质量单元。

本问题中摆杆绕质心的转动惯量为：

平行轴定理：

其中 $d$ 是两轴之间的距离，$I_{cm}$ 是物体绕质心的转动惯量。

四、稳定性分析

计算状态矩阵特征方程：

即：

特征值为 $\lambda_1\approx3.9$、$\lambda_2\approx-3.9$、$\lambda_3=0$、$\lambda_4=0$。

因此系统存在正实部特征值，即存在右半平面极点，系统不稳定，由此可见倒立摆在无外界输入的情况下无法在竖直向上保持稳定。

后续可通过状态反馈控制，配置系统极点均在左半平面内，使系统变得稳定。

五、能控性和能观性分析

能控性矩阵表示为：

计算能控性矩阵结果为：

即 $\operatorname{rank}(Q_c)=4$，系统可控。

系统的能观性与传感器测量量密切相关。若传感器仅测量转角，则系统输出矩阵为：

若仅测量小车位移，则输出矩阵为：

同理，若同时测量转角与小车位移，则输出矩阵为：

三种情况下系统的能观性矩阵由下式计算：

计算得：

因此系统在仅有小车位移传感器、以及同时具有小车位移传感器和摆杆角度传感器这两种情况下可观测；在仅有角度传感器的情况下不可观。

六、控制器设计（极点配置状态反馈控制）

不妨设极点为 $p_1=-0.5$、$p_2=-1$、$p_3=-1.5$、$p_4=-2$。

此时系统极点位于左半平面，满足稳定性要求。

由上述说明可知，系统完全能控，化原系统状态矩阵为能控标准型：

原系统的特征多项式为：

构造变换矩阵：

则存在线性变换使得：

即：

目标特征多项式为：

则有：

即

则状态反馈矩阵 $K$：

考虑到手动计算具有一定的不准确性，而且在后期仿真验证中，手动计算所得反馈矩阵效果不如 Matlab 计算所得结果。原因可能是手动计算的显式求导和小数点后位数省略使误差增大。最终实验仿真以 Matlab 计算所得的增益矩阵为主：

1
2
3
4
5
6
7
A = [0, 1, 0, 0; 
	15.24, 0, 0, 0;
    0, 0, 0, 1; 
    -0.363, 0, 0, 0];
B = [0; -0.741; 0; 0.494];
desired_poles = [-0.5, -1, -1.5, -2];  
K = place(A, B, desired_poles);

最终输出结果：

1
K=[ -32.52  -7.32  -0.21  -0.86]

七、Gazebo仿真

为了验证可行性，并展示整体控制效果，基于 ROS 和 Gazebo 设计仿真实验。

控制器代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
#include "ros/ros.h"
#include <std_msgs/Float64.h>
#include <sensor_msgs/JointState.h>

// 定义状态反馈增益矩阵 K（需根据系统调整）
const double K[] = { -32.51, -7.32, -0.207, -0.861}; // 示例增益：K = [k1, k2, k3, k4]

//全局状态变量、小车的基本参数
double theta = 0.0;      // x1: 摆杆角度（弧度）
double theta_dot = 0.0;  // x2: 角速度
double x = 0.0;          // x3: 小车位移（米）
double x_dot = 0.0;      // x4: 小车速度

double F =0;
double limit_f = 40;   
double limit_change = 0.8;    //控制器由摆荡控制转为状态反馈控制的角度界限
      

double m_ = 0.1;
double g_ = 9.8;
double l_ = 0.5;

//由于坐标系选择的原因，这里需要将ros中传回的关节角度进行标准化，变为-3.14到3.14之间
double normalizeTo0To2Pi(double angle) {
    // 1. 对2π取模，得到[-2π, 2π]范围内的值
    angle = std::fmod(angle+3.14, 2 * M_PI);
    
    // 2. 将超出[-π, π]的值调整到范围内
    if (angle > M_PI) {
        angle -= 2 * M_PI;  // 例如：4π/3 → 4π/3 - 2π = -2π/3
    } else if (angle < -M_PI) {
        angle += 2 * M_PI;  // 例如：-5π/4 → -5π/4 + 2π = 3π/4
    }
    
    return angle;  // 结果在[-π, π]之间
}
//摆荡控制函数，控制策略为能量注入，当然也可以直接设置一个左右晃动的固定值
double swingUpControl(double theta, double theta_dot) {
        double E_target = m_ * g_ * l_;

        double E_current = 0.5 * m_ * l_ * l_ * theta_dot * theta_dot + m_ * g_ * l_ * cos(theta);
        if(E_target<E_current) //判断当前能量和最高点平衡时整体能量的大小关系，如果超过，则不再注入
        {
            return 0;
        }
        double F = 20.0 * (E_target-E_current) * ((theta_dot * cos(theta) > 0) ? -1 : 1);//力的方向注意根据坐标系来定
        return F;  
}
    
//接受传感器话题后的回调函数
void jointStateCallback(const sensor_msgs::JointState::ConstPtr& msg) {
	//更新状态变量
    theta = normalizeTo0To2Pi(msg->position[0]);      // x1
    theta_dot = msg->velocity[0];  // x2
    x = msg->position[1];          // x3
    x_dot = msg->velocity[1];       // x4

}

int main(int argc, char *argv[])

{

    setlocale(LC_ALL,"");
    ros::init(argc,argv,"cp_ctrl");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/cartpole/joint_states", 10, jointStateCallback); //订阅传感器消息
    ros::Publisher pub = nh.advertise<std_msgs::Float64>("/cartpole/cart_effort_controller/command", 10);  //发布话题command
    ros::Rate rate(500); // 控制频率500Hz（与YAML中publish_rate一致）

    while (ros::ok()) {
        //判断当前状态
        //角度接近平衡点时，计算控制力 F = -K*x （状态反馈）
        if(theta>=-limit_change&&theta<limit_change)
        {
            F = -(K[0] * theta + K[1] * theta_dot + K[2] * x + K[3] * x_dot);
            
        }
        else{//角度差得还远的话用摆荡
            F= swingUpControl(theta,theta_dot);

        }
        //ROS_INFO("当前角度: %.2f", theta);
        //ROS_INFO(" %.2f", theta_dot);
        F = (F > limit_f) ? limit_f : ((F < -limit_f) ? -limit_f : F);
        // 发布控制指令
        std_msgs::Float64 effort_msg;
        effort_msg.data = F;
        pub.publish(effort_msg);

        ros::spinOnce();
        
        rate.sleep();
    }
    return 0;
}

整体流程如下：

初始状态时，由于没有控制器输入，倒立摆头朝下处于稳定状态，如图所示：

image-20250531134217271

此时，对滑块施加左右变化的力，对摆杆注入一定动能，让摆杆摆动起来，如图：

image-20250531134656544

当摆动至平衡点附近，控制器切换为状态反馈控制，控制施加在滑块上的力，使摆杆保持竖直朝上，如图：

image-20250531134912515

同时，当在杆上施加一定的外力干扰时，也可以及时回正，其角度、位移等参数曲线如下右图所示：

角度与位移曲线 外力干扰后的响应曲线

实验效果见演示视频