Python-基于反步法和四阶龙格库塔的非线性控制系统仿真（牛顿第二定律）

一、问题描述

题目及下文推导过程笔记借用自微信公众号：王崇卫，感谢大佬整理的笔记！orz。

二、推导过程

控制律推导过程详见：链接: 【Advanced控制理论】15_Nonlinear Backstepping Control_反馈线性化控制_Feedback Linearization

三、Python仿真源码

3.1 参数的声明 (params)

import math
import numpy as np

# 期望轨迹的设定
def trajectory(t):
    x1d = math.cos(t)
    x2d = -math.sin(t)
    ddx1d_dt = -math.cos(t)
    dx1d_dt = -math.sin(t)
    return x1d,x2d,ddx1d_dt,dx1d_dt

# 参数设置
x1_0 = 0.0  # 初始位移
x2_0 = 0.0  # 初始速度
t0 = 0.0  # 初始时间
t_end = 20.0  # 终止时间
dt = 0.01  # 时间步长

alpha = 1.0 # 模型参数
m = 1.0  # 质量

k1 = 1 # 控制律参数1
k2 = 2 # 控制律参数2

""" 本仿真中状态变量和控制律都是一维的，所以没用到n """
# n = 1 # 状态变量的维数

# 定义存数据的列表，画图象用
x1 = list()
x2 = list()
control_value = list()

t = np.arange(t0, t_end + dt, dt)
# 定义理想轨迹函数
def f1(t):
    return np.cos(t)
# 定义理想速度函数
def f2(t):
    return -np.sin(t)

# 计算期望轨迹和期望速度（画图象用）
desire_x1 = f1(t)
desire_x2 = f2(t)
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

3.2 状态空间方程模型与控制律的求解 (controller)

from params import *

def model_equation(t, x1, x2, u):
    """
    牛顿第二定律方程：m*a + αx**3 = F

    参数:
        t (float): 时间
        x1 (float): 位移
        x2 (float): 速度
        u (float): 控制量（时变的值，此处为外力 F）

    返回:
        dx1_dt (float): 位移的导数(速度)
        dx2_dt (float): 速度的导数(加速度)
        返回值用于四阶龙格库塔求解被控轨迹
    """

    dx1_dt = x2
    dx2_dt = (-alpha * x1**3 + u) / m

    return [dx1_dt, dx2_dt]

def control_law(t):
    """
    计算时变的控制量

    参数:
        t (float): 时间

    返回:
        u (float): 控制律（时变量）
    """
    x1d,x2d,ddx1d_dt,dx1d_dt = trajectory(t)

    e = x1d - x1[-1]
    delta = x2d - x2[-1]
    u = m*e + m* ddx1d_dt + m*k1*dx1d_dt - m*k1*x2[-1] + alpha*x1[-1]**3 + m*k2*delta

    return u
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

3.3 四阶龙格库塔法(runge_kutta4)

import numpy as np
from params import *

def runge_kutta4(f, x1_0, x2_0, dt, param_func):
    """
    :param f: 模型的状态空间方程
    :param x_0: 模型的状态变量的初值
        :param x1_0: 模型的第一个状态变量的初值
        :param x2_0: 模型的第二个状态变量的初值
    :param dt: 采样步长
    :param param_func: 求取的控制量
    :return: 返回值
        :param t: 时间序列
        :param x1: 路径序列
        :param x2: 速度序列
    """

    x1.append(x1_0)
    x2.append(x2_0)
    control_value.append(0)

    for i in range(len(t) - 1):
        p = param_func(t[i])  # 获取当前时间步长的控制量
        k1 = f(t[i], x1[i], x2[i], p)
        k2 = f(t[i] + dt / 2, x1[i] + dt * k1[0] / 2, x2[i] + dt * k1[1] / 2, param_func(t[i] + dt / 2))
        k3 = f(t[i] + dt / 2, x1[i] + dt * k2[0] / 2, x2[i] + dt * k2[1] / 2, param_func(t[i] + dt / 2))
        k4 = f(t[i] + dt, x1[i] + dt * k3[0], x2[i] + dt * k3[1], param_func(t[i] + dt))
        # 因为控制量u是时变的量，所以每次都要进行获取，这是和龙格库塔法求解参数时不变微分方程的不同之处

        # kn 的维数等于状态变量的个数
        x1.append(x1[i] + dt * (k1[0] + 2 * k2[0] + 2 * k3[0] + k4[0]) / 6)
        x2.append(x2[i] + dt * (k1[1] + 2 * k2[1] + 2 * k3[1] + k4[1]) / 6)
        control_value.append(param_func(t[i]))

    return t, x1, x2
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

3.4 main函数

from params import *
from runge_kutta4 import *
from controller import *
import matplotlib.pyplot as plt

# 调用四阶龙格库塔法进行被控轨迹的求解
t, x1, x2 = runge_kutta4(model_equation, x1_0, x2_0, dt, control_law)

# 绘制图象进行对比
plt.plot(t, x1, label='Position')
plt.plot(t, desire_x1, label = "Desired_Position")
# plt.plot(t, control_value, label='Control_Value')
plt.xlabel('t')
plt.ylabel('Position')
plt.title('Position Plot')
plt.legend()
plt.grid(True)
plt.show()

plt.plot(t, x2, label='Velocity')
plt.plot(t, desire_x2, label = "Desired_Velocity")
# plt.plot(t, control_value, label='Control_Value')
plt.xlabel('t')
plt.ylabel('Velocity')
plt.title('Velocity Plot')
plt.legend()
plt.grid(True)
plt.show()
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

四、仿真结果

完结撒花！