python解决控制问题系例之一：解决微分方程-状态方程求解作图问题

python解决控制问题系例之一：解决微分方程-状态方程求解作图问题

文章目录

• python解决控制问题系例之一：解决微分方程-状态方程求解作图问题
• • 1. 利用scipy.integrate功能包的solve_ivp求解方案
  • 2. 利用python-control功能包的input_output_response求解方案
  • 3. 利用python-control功能包的interconnect函数构造闭环非线性系统轨迹数值解求解方案
  • 4. LQR下的微分控制系统数值解求解方案

1. 利用scipy.integrate功能包的solve_ivp求解方案

以求解lotka-Volterra 系统为例来演示如何使用slove_ivp。lotka-Volterra系统的动态模型可以描述为：
x ˙ = a x − b x y , y ˙ = − c y + d x y dot{x} = ax - bxy, \ dot{y} = -cy + dxy x˙=ax−bxy,y˙​=−cy+dxy
在使用solve_ivp前，需要先建立系统模型函数lotkavolterra，函数的输入为自变量时间t、输入状态向量z=(x, y)以及系统的参数a,b,c,d。函数的返回值为一个列表，列表中每一项为一行微分方程的更新值。该部分具体的代码为：

# 引入必要的功能包
from scipy.integrate import solve_ivp
import numpy as np
import matplot.pyplot as plt

def lotkavolterra(t, z, a, b, c, d):
    x, y = z
    return [a * x - b * x * y, -c * y + d * x * y]

调用solve_ivp来生成微分方程的数值解，包含的参数包括：系统模型函数lotkavolterra, 时间t的范围，状态初值，以及参数a、b、c、d的具体值。具体代码为：

sol = solve_ivp(lotkavolterra, [0, 15], [10, 5], args=(1.5, 1, 3, 1), dense_output=True)
t = np.linspace(0, 15, 300)
z = sol.sol(t)

最后使用matplotlib功能包中的plot功能，画出系统状态关于时间t的轨迹。

plt.plot(t, z.T)
plt.legend(['x', 'y'], loc='best', shadow=True)
plt.xlabel('t')
plt.title('Lotka-Volterra System')
plt.show()

最终效果图：

2. 利用python-control功能包的input_output_response求解方案

此方案中需要用到control功能包。control功能包是包含了很多控制系统中常用的建模、分析和类matlab函数使用方法，对于求解微分方程构成的系统轨迹非常方便。

以功能包中的案例 predator-prey为例，讲解微分方程数值解求解方法。首先引入必要的功能包，包括numpy、control和matplotlib.pyplot, 代码为：

import control as ct
import numpy as np
import matplotlib.pyplot as plt

构建predator_prey的模型，其微分方程为：
H ˙ = ( r + u ) H 1 − H / K − a H L c + H , L ˙ = b a H L c + H − d L dot{H} = frac{(r + u)H}{1-H/K} - frac{aHL}{c + H}, \ dot{L} = bfrac{aHL}{c + H} - dL H˙=1−H/K(r+u)H​−c+HaHL​,L˙=bc+HaHL​−dL
在上述公式中，H代表猎兔的数量，L代表猎狐的数量，u为投给猎兔的食物量，r、a、b、c、d为相关的参数。通过predprey_rhs函数实现微分方程的建模，具体代码为：

#  construct the dynamics of predator-prey ecosystem 
def predprey_rhs(t, x, u, params={}):
    # Parameter Setup
    a = params.get('a', 3.2)
    b = params.get('b', 0.6)
    c = params.get('c', 50.)
    d = params.get('d', 0.56)
    k = params.get('k', 125)
    r = params.get('r', 1.6)
    
    # Map the states into local variable names
    H = x[0]
    L = x[1]
    
    # Compute the control action(only allow addition of food)
    u_0 = u if u > 0 else 0
    
    # Computer the dynamics
    dH = (r + u_0) * H * (1 - H/k) - (a * H * L)/(c + H)
    dL = b * (a * H *  L)/(c + H) - d * L

    return [dH, dL]

当构建完成微分方程的函数后，通过control包中的NonlinearIOSystem函数将微分方程构建成I/O系统对象，方面后续调用control包中input_output_response进行数值求解。具体代码为：

# I/O nonlinear system setup
io_predprey = ct.NonlinearIOSystem(
    predprey_rhs, None, inputs='u', outputs=('H', 'L'), states=('H', 'L'),
    name='predprey')

在该代码中，第一个参数代表微分方程，第二个参数为输出方程（此处无），系统输入定义为’u’, 系统输出为系统状态，即为’H’和’L’，系统名称定义为‘predprey’。

接下来定义系统的状态初值X0，仿真求解时间T, 调用input_output_response求数值解，考虑开环控制，即u为0时系统的数值解。代码为：

# Open-loop dynamics(initial condition response without control)
X0 = [25, 20]                      #Initial H, L
T = np.linspace(0, 70, 500)        #Simulation 70 years of time

#Simulate the system
t, y = ct.input_output_response(io_predprey, T, 0, X0)

#plot the response
plt.figure(1)
plt.plot(t, y[0], 'b', label='Hares')
plt.plot(t, y[1], 'g--', label='Lynxes')
plt.legend(loc='best')
plt.axis([0, 70, 0, 100])
plt.xlabel('Time [years]')
plt.ylabel('Population')
plt.title('Open-loop control response')
plt.show()

3. 利用python-control功能包的interconnect函数构造闭环非线性系统轨迹数值解求解方案

如果面对更加复杂的微分系统，即要求设计反馈控制器，构造一个闭环的非线性系统，那么此时就需要利用control功能包中interconnect函数将控制对象，控制器，参考信号等模块进行输入输出端耦合，实现前馈、反馈链接。本节以vehicle steering 简化模型实现跟踪轨迹的应用为例，说明如何通过构建反馈闭环系统，求解轨迹数值解并作图。

vehicle steering 简化运动学模型为：
x ˙ = v cos ⁡ ( θ ) , y ˙ = v sin ⁡ ( θ ) , θ ˙ = v tan ⁡ ( ϕ ) l dot{x} = v cos(theta), \ dot{y} = v sin(theta) ,\ dot{theta} = frac{v tan(phi)}{l} x˙=vcos(θ),y˙​=vsin(θ),θ˙=lvtan(ϕ)​
其中，位置x,y, 车身与水平方向夹角theta为状态，速度v和转向轮与车身运动方向夹角phi为输入，l为车身距离，系统输出为全状态。
利用control包中的NonlinearIOSystem建立输入输出系统对象vehicle 。具体代码为：

# This example illustrate how to establish the closed-loop system then find state trajectories
import numpy as np
import matplotlib.pyplot as plt
import control as ct
from cmath import sqrt

# Step1 construct vehicle steering dynamics
def vehicle_update(t, x, u, params={}):
    
    l = params.get('wheelbase', 3.)
    phimax = params.get('maxsteer', 0.5) 
    
    phi = np.clip(u[1], -phimax, phimax) # saturation of phi
    
    dx = u[0] * np.cos(x[2])
    dy = u[0] * np.sin(x[2])
    dtheta = u[0] / l * np.tan(phi)
    
    return np.array([dx, dy, dtheta])

def vehicle_output(t, x, u, params={}):
    return x

vehicle = ct.NonlinearIOSystem(
    vehicle_update, vehicle_output, states=3, name='vehicle',
    inputs=('v', 'phi'), outputs=('x', 'y', 'theta'))

利用极点配置，构造一个反馈控制器，构成出控制器的I/O输入输出对象controller。控制器模型的输入为误差状态ex、ey、etheta 和跟踪速度vd、跟踪方向phid, 控制器模型的输出为实际速度v和控制方向phi。代码为：

# Step2 construct feedback controller 
# this example using gain scheduled 
def control_output(t, x, u, params):
    # Get the controller parameters
    longpole = params.get('longpole', -2.)
    latpole1 = params.get('latpole1', -1/2 + sqrt(-7)/2)
    latpole2 = params.get('latpole2', -1/2 - sqrt(-7)/2)
    l = params.get('wheelbase', 3)
    
    # Extract the system inputs
    ex, ey, etheta, vd, phid = u
 
    # Determine the controller gains
    alpha1 = -np.real(latpole1 + latpole2)
    alpha2 = np.real(latpole1 * latpole2)
 
    # Compute and return the control law
    v = -longpole * ex          # Note: no feedfwd (to make plot interesting)
    if vd != 0:
        phi = phid + (alpha1 * l) / vd * ey + (alpha2 * l) / vd * etheta
    else:
        # We aren't moving, so don't turn the steering wheel
        phi = phid
     
    return  np.array([v, phi])

controller = ct.NonlinearIOSystem(
    None, control_output, name='controller',
    inputs=('ex', 'ey', 'etheta', 'vd', 'phid'), outputs=('v', 'phi'))

构造参考轨迹生成模型，该模型中，输入为参考速度vref和参考纵向唯一yref，输出为：
x d = v r e f ∗ t y d = y r e f θ d = 0 v d = v r e f ϕ d = 0 x_d = v_{ref} * t\ y_d = y_{ref}\ theta_d = 0\ v_d = v_ref\ phi_d = 0 xd​=vref​∗tyd​=yref​θd​=0vd​=vr​efϕd​=0
具体代码为：

# Step3 construct trajector_gen 
def trajecter_output(t, x, u, params={}):
    vref, yref = u
    return np.array([vref * t, yref, 0, vref, 0])

trajector = ct.NonlinearIOSystem(
    None, trajecter_output, name='trajector',
    inputs=('vref', 'yref'), outputs=('xd', 'yd', 'thetad', 'vd', 'phid'))

接下来，采用interconnect函数实现三个模型的端口链接，具体结构如下图：

其中vehicle的状态负反馈回控制器controller的输入端，与轨迹生成模块的输出xd,yd,thetad做差形成偏差信号ex,ey,etheta。反馈闭环系统的构造代码如下：

# Final step: We construct the system using the InterconnectedSystem constructor and using
# signal labels to keep track of everything.

steering = ct.interconnect(
    #list of subsystems
    (trajector, controller, vehicle), name='steering',
    # interconnections betweem subsystems
    connections=(
        ['vehicle.v', 'controller.v'],
        ['vehicle.phi', 'controller.phi'],
        ['controller.vd', 'trajector.vd'],
        ['controller.phid', 'trajector.phid'],
        ['controller.ex', 'trajector.xd', '-vehicle.x'],
        ['controller.ey', 'trajector.yd', '-vehicle.y'],
        ['controller.etheta', 'trajector.thetad', '-vehicle.theta']
    ),
    #list of inputs and outputs
    inplist=['trajector.vref', 'trajector.yref'],
    inputs=['vref', 'yref'], 
    outlist=['vehicle.x', 'vehicle.y', 'vehicle.theta', 'controller.v', 'controller.phi'],
    outputs=['x', 'y', 'theta', 'v', 'phi']
)

通过matplotlib作图，得到控制信号的关于时间的轨迹响应图。

# setup the simulation condition and plot
plt.figure(1)

yref = 1
T = np.linspace(0, 5, 100)

plt.plot([0, 5], [yref, yref], 'k--', label='yref')

y_index = steering.find_output('y')
v_index = steering.find_output('v')

for vref in [8, 10, 12]:
    tout,yout = ct.input_output_response(
        steering, T, [vref * np.ones(len(T)), yref * np.ones(len(T))])
    
    plt.plot([0, 5], [vref, vref], 'k--', label='vref')
    
    y_line, = plt.plot(tout, yout[y_index, : ], 'r')
    v_line, = plt.plot(tout, yout[v_index, : ], 'b')
    
plt.xlabel('time (s)')
plt.ylabel('v vel (m/s), y pos (m)')
plt.legend((v_line, y_line), ('v', 'y'), loc='best')
plt.show()

4. LQR下的微分控制系统数值解求解方案

如果由微分方程构成的控制系统，需要考虑最优控制的问题，那么此时就需要调用control包中的optimal.solve_ocp函数来解决最优控制数值解求解问题。下面例子以vehicle steering 问题为基础，求LQR下的轨迹并作图。价值函数采用线性二次型，同时对输入和状态均有边界限制条件。具体代码为：

import numpy as np
import control as ct
import control.optimal as opt
import matplotlib.pyplot as plt

def vehicle_update(t, x, u, params):
    # Get the parameters for the model
    l = params.get('wheelbase', 3.)         # vehicle wheelbase
    phimax = params.get('maxsteer', 0.5)    # max steering angle (rad)

    # Saturate the steering input
    phi = np.clip(u[1], -phimax, phimax)

    # Return the derivative of the state
    return np.array([
        np.cos(x[2]) * u[0],            # xdot = cos(theta) v
        np.sin(x[2]) * u[0],            # ydot = sin(theta) v
        (u[0] / l) * np.tan(phi)        # thdot = v/l tan(phi)
    ])

def vehicle_output(t, x, u, params):
    return x                            # return x, y, theta (full state)

# Define the vehicle steering dynamics as an input/output system
vehicle = ct.NonlinearIOSystem(
    vehicle_update, vehicle_output, states=3, name='vehicle',
    inputs=('v', 'phi'), outputs=('x', 'y', 'theta'))

# # Initial condition setup
x0 = [0., -2., 0.]; u0 = [10., 0.]
xf = [100., 2., 0.]; uf = [10., 0.]
Tf = 10

# Set up the optimal problem
# Step1: Define quadratic cost function and terminal cost function
Q = np.diag([0, 0, 0.1])
R = np.diag([1, 1])
P = np.diag([1000, 1000, 1000])
traj_cost = opt.quadratic_cost(vehicle, Q, R, x0=xf, u0=uf)
term_cost = opt.quadratic_cost(vehicle, P, 0, x0=xf)
# Step2: constraints on inputs setup
constraints = [opt.input_range_constraint(vehicle, [8, -0.1], [12, 0.1])]
# Step3: solve for the optimal inputs
horizon = np.linspace(0, Tf, 3, endpoint=True)
result = opt.solve_ocp(
    vehicle, horizon, x0, traj_cost, constraints, terminal_cost=term_cost, 
    initial_guess=u0)

# Plotting the results
# Simulate the system dynamics (open loop)
resp = ct.input_output_response(
    vehicle, horizon, result.inputs, x0,
    t_eval=np.linspace(0, Tf, 100))
t, y, u = resp.time, resp.outputs, resp.inputs

plt.subplot(3, 1, 1)
plt.plot(y[0], y[1])
plt.plot(x0[0], x0[1], 'ro', xf[0], xf[1], 'ro')
plt.xlabel("x [m]")
plt.ylabel("y [m]")

plt.subplot(3, 1, 2)
plt.plot(t, u[0])
plt.axis([0, 10, 9.9, 10.1])
plt.xlabel("t [sec]")
plt.ylabel("u1 [m/s]")

plt.subplot(3, 1, 3)
plt.plot(t, u[1])
plt.axis([0, 10, -0.01, 0.01])
plt.xlabel("t [sec]")
plt.ylabel("u2 [rad/s]")

plt.suptitle("Lane change manuever")
plt.tight_layout()
plt.show()

结果为：