一、问题描述
题目及下文推导过程笔记借用自微信公众号:王崇卫,感谢大佬整理的笔记!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)
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
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
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()
四、仿真结果
完结撒花!