用Python和NumPy手把手实现线性系统的状态空间建模与仿真（附完整代码）

用Python和NumPy手把手实现线性系统的状态空间建模与仿真附完整代码在控制工程领域状态空间法作为现代控制理论的核心工具已经广泛应用于机器人、自动驾驶、工业自动化等前沿领域。与传统的传递函数方法相比状态空间表示不仅能处理多输入多输出系统还能更直观地揭示系统内部状态的变化规律。本文将带您从零开始用Python实现状态空间建模的关键环节包括系统矩阵定义、状态方程求解、能控性分析以及动态响应仿真。1. 环境配置与基础准备1.1 必需库安装与验证在开始之前我们需要确保Python环境中已安装以下科学计算库pip install numpy scipy matplotlib control slycot注意control库需要slycot作为后端支持若安装失败可尝试conda install -c conda-forge control slycot验证安装是否成功import numpy as np from scipy import linalg import matplotlib.pyplot as plt import control as ct print(NumPy版本:, np.__version__) print(Control库版本:, ct.__version__)1.2 状态空间基础表示线性时不变系统的状态空间标准形式为$$ \begin{cases} \dot{x} Ax Bu \ y Cx Du \end{cases} $$对应的Python矩阵定义方法# 二阶质量-弹簧-阻尼系统示例 A np.array([[0, 1], [-2, -3]]) # 状态矩阵 B np.array([[0], [1]]) # 输入矩阵 C np.array([[1, 0]]) # 输出矩阵 D np.array([[0]]) # 直接传输矩阵 sys ct.ss(A, B, C, D) # 创建状态空间对象2. 状态方程求解与仿真2.1 状态转移矩阵计算状态转移矩阵$\Phi(t) e^{At}$是求解齐次状态方程的关键NumPy提供多种计算方法def state_transition_matrix(A, t): 计算状态转移矩阵的三种方法 # 方法1Scipy矩阵指数 phi_scipy linalg.expm(A * t) # 方法2泰勒级数展开前20项 n 20 phi_taylor np.eye(A.shape[0]) term np.eye(A.shape[0]) for k in range(1, n): term term (A * t) / k phi_taylor term # 方法3特征值分解法 eigvals, eigvecs linalg.eig(A) phi_eig eigvecs np.diag(np.exp(eigvals * t)) linalg.inv(eigvecs) return phi_scipy, phi_taylor, phi_eig2.2 时域响应仿真使用control库进行阶跃响应和脉冲响应仿真# 时域响应设置 t np.linspace(0, 10, 1000) # 阶跃响应 t_step, y_step ct.step_response(sys, Tt) # 脉冲响应 t_impulse, y_impulse ct.impulse_response(sys, Tt) # 绘制结果 plt.figure(figsize(12, 4)) plt.subplot(121) plt.plot(t_step, y_step) plt.title(阶跃响应) plt.subplot(122) plt.plot(t_impulse, y_impulse) plt.title(脉冲响应) plt.tight_layout()3. 系统能控性与能观性分析3.1 能控性矩阵计算判断系统能控性的关键指标是能控性矩阵的秩def controllability_matrix(A, B): 构造能控性矩阵 n A.shape[0] C B for i in range(1, n): C np.hstack((C, np.linalg.matrix_power(A, i) B)) return C Ctrb controllability_matrix(A, B) print(能控性矩阵秩:, np.linalg.matrix_rank(Ctrb))3.2 能观性分析类似地能观性矩阵的秩决定系统状态的可观测性def observability_matrix(A, C): 构造能观性矩阵 n A.shape[0] O C for i in range(1, n): O np.vstack((O, C np.linalg.matrix_power(A, i))) return O Obsv observability_matrix(A, C) print(能观性矩阵秩:, np.linalg.matrix_rank(Obsv))4. 状态反馈控制器设计4.1 极点配置算法通过状态反馈$u -Kx$可以任意配置系统极点假设系统能控def pole_placement(A, B, desired_poles): 阿克曼公式极点配置 n A.shape[0] Ctrb controllability_matrix(A, B) if np.linalg.matrix_rank(Ctrb) ! n: raise ValueError(系统不能控无法任意配置极点) # 计算特征多项式系数 poly np.poly(desired_poles) # 阿克曼公式计算K gamma np.zeros((1, n)) gamma[0, -1] 1 K gamma linalg.inv(Ctrb) linalg.polyvalm(poly, A) return K desired_poles [-21j, -2-1j] # 期望闭环极点 K pole_placement(A, B, desired_poles) print(反馈增益矩阵K:, K)4.2 闭环系统仿真验证极点配置效果A_cl A - B K # 闭环系统矩阵 sys_cl ct.ss(A_cl, B, C, D) # 比较开环与闭环阶跃响应 t, y_open ct.step_response(sys, Tt) _, y_closed ct.step_response(sys_cl, Tt) plt.figure() plt.plot(t, y_open, label开环系统) plt.plot(t, y_closed, label闭环系统) plt.legend() plt.title(极点配置前后响应对比)5. 状态观测器实现5.1 全维观测器设计当系统状态不可直接测量时需要构建状态观测器def observer_gain(A, C, desired_obs_poles): 观测器增益设计 # 利用对偶原理转换为极点配置问题 A_dual A.T B_dual C.T K_dual pole_placement(A_dual, B_dual, desired_obs_poles) L K_dual.T return L obs_poles [-5, -6] # 观测器极点比系统极点快3-5倍 L observer_gain(A, C, obs_poles) print(观测器增益矩阵L:, L)5.2 观测器仿真验证def observer_system(A, B, C, L, u, y, x0_hat, dt0.01): 观测器动态仿真 n_steps len(u) x_hat np.zeros((n_steps, A.shape[0])) x_hat[0] x0_hat for k in range(n_steps-1): dx_hat A x_hat[k] B * u[k] L (y[k] - C x_hat[k]) x_hat[k1] x_hat[k] dx_hat * dt return x_hat # 生成真实系统响应 t, y ct.step_response(sys, Tt) u np.ones_like(t) # 阶跃输入 # 观测器初始状态与真实状态不同 x0_hat np.array([0.5, -0.2]) # 运行观测器 x_hat observer_system(A, B, C, L, u, y, x0_hat) # 绘制状态估计结果 plt.figure(figsize(12, 4)) plt.subplot(121) plt.plot(t, x_hat[:, 0], label估计状态x1) plt.subplot(122) plt.plot(t, x_hat[:, 1], label估计状态x2)6. 完整案例倒立摆控制系统6.1 系统建模考虑经典倒立摆系统的线性化模型# 系统参数 M 1.0 # 小车质量(kg) m 0.1 # 摆杆质量(kg) l 0.5 # 摆杆长度(m) g 9.8 # 重力加速度(m/s^2) # 状态空间矩阵 A np.array([ [0, 1, 0, 0], [0, 0, -m*g/M, 0], [0, 0, 0, 1], [0, 0, (Mm)*g/(M*l), 0] ]) B np.array([[0], [1/M], [0], [-1/(M*l)]]) C np.array([[1, 0, 0, 0], [0, 0, 1, 0]]) D np.zeros((2, 1)) pendulum ct.ss(A, B, C, D)6.2 控制器与观测器联合仿真# 设计控制器稳定摆杆的同时移动小车 desired_poles [-1, -2, -11j, -1-1j] K pole_placement(A, B, desired_poles) # 设计观测器假设只能测量小车位置和摆杆角度 obs_poles [-4, -5, -32j, -3-2j] L observer_gain(A, C, obs_poles) # 闭环系统仿真 def closed_loop_system(x, t, A, B, K, L, C): 带观测器的闭环系统 x_hat x[4:] # 观测器状态 u -K x_hat # 基于估计状态的反馈 # 真实系统动态 dx A x[:4] B * u # 观测器动态 y C x[:4] dx_hat A x_hat B * u L (y - C x_hat) return np.concatenate([dx, dx_hat]) # 初始条件摆杆略微倾斜 x0 np.array([0, 0, 0.1, 0, 0, 0, 0, 0]) t np.linspace(0, 10, 1000) from scipy.integrate import odeint sol odeint(closed_loop_system, x0, t, args(A, B, K, L, C)) # 绘制结果 plt.figure(figsize(12, 6)) plt.subplot(211) plt.plot(t, sol[:, 0], label小车位置) plt.subplot(212) plt.plot(t, sol[:, 2], label摆杆角度(rad)) plt.plot(t, sol[:, 6], --, label估计角度) plt.legend()7. 进阶技巧与性能优化7.1 数值稳定性处理在计算矩阵指数和高阶矩阵运算时需要注意数值稳定性问题def robust_matrix_exp(A, t, eps1e-10): 带缩放-平方的矩阵指数计算 n max(int(np.ceil(np.log2(np.linalg.norm(A)*t/eps))), 0) scaled_A A * t / (2**n) phi linalg.expm(scaled_A) for _ in range(n): phi phi phi return phi7.2 稀疏系统处理对于大规模稀疏系统使用稀疏矩阵运算可显著提升效率from scipy.sparse import csc_matrix, linalg as splinalg def sparse_controllability(A_sparse, B_sparse): 稀疏能控性矩阵计算 n A_sparse.shape[0] C B_sparse for i in range(1, n): C splinalg.hstack([C, A_sparse**i B_sparse]) return C7.3 硬件在环仿真将Python控制器与物理仿真器如Gazebo连接import socket import pickle class HardwareInterface: def __init__(self, ip127.0.0.1, port65432): self.sock socket.socket(socket.AF_INET, socket.SOCK_DGRAM) self.addr (ip, port) def send_control(self, u): data pickle.dumps({control: u.tolist()}) self.sock.sendto(data, self.addr) def receive_state(self): data, _ self.sock.recvfrom(1024) return pickle.loads(data)[state]