还是要进一步说明，Pyomo不仅可以用于经典优化模型的求解，还可以用于最优控制、仿真等领域，本文介绍的简易车辆轨迹规划旨在说明pyomo在最优控制方面的应用。
已知一辆小车（车辆有形状，不是一个质心）初始的位置，如何控制它从A点移动到B点呢？根据牛顿第二定律，我们只需要动态地对车辆的加速度和转角进行调整即可，但是为了让车里的人更加舒适，应当保证车辆移动过程中加速度尽可能地小同时行驶距离也要短，因此这里就涉及到最优控制。
对于任意时刻，车辆的位置、速度、加速度关系通过如下公式刻画
公式中x,y表示车辆坐标，v表示车辆的速度，theta表示车轴和水平方向的夹角，fai表示前方向盘相对于车轴的角度，L为车身长度，a_v为车辆的加速度。车辆移动控制的输入变量为a_v和fai。假定车辆的初始朝向为水平方向，为使得车辆移动距离最短，需要保证每个时刻水平方向的加速度也要尽可能地小，车辆水平方向的加速度可通过如下公式计算。
小车的控制问题可以表述为，在给定车辆限定速度、限定加速度和限定转角前提下，如何对控制时段内（0<t<t_f）内小车的加速度和转角进行调整使得车辆由初始状态(x(0),y(0),thta(0),v(0))移动到指定的状态(x(f),y(f),thta(f),v(f))，同时要保证下述的目标函数最小化。
假定车辆初始状态为(0,0,0,0)，最终需转变成的状态为(0,4L,0,0)，车辆长度L为5，控制时段为40个时间步长，限定速度、加速度、转角在模型中给出，那么该最优控制模型建模求解如下。
from pyomo.environ import *from pyomo.dae import *# 参数ar_max = 2.8av_max = 2.8phi_max = 0.7v_max = 30v_min = -4# 车辆长度和控制时段L = 5tf = 40# 模型定义m = ConcreteModel()# 集合定义m.t = ContinuousSet(bounds=(0, tf))# 定义决策变量m.av = Var(m.t)m.phi = Var(m.t, bounds=(-phi_max, phi_max))# 定义状态变量m.x = Var(m.t, domain=NonNegativeReals)m.y = Var(m.t, domain=NonNegativeReals)m.a = Var(m.t)m.v = Var(m.t)# 定义一阶导数m.x_dot = DerivativeVar(m.x)m.y_dot = DerivativeVar(m.y)m.a_dot = DerivativeVar(m.a)m.v_dot = DerivativeVar(m.v)# 定义位移、速度、加速度的关系表达式，将其作为约束def ode_x(m, t):return m.x_dot[t] == m.v[t]*cos(m.a[t])def ode_y(m, t):return m.y_dot[t] == m.v[t]*sin(m.a[t])def ode_a(m, t):return m.a_dot[t] == m.v[t]*tan(m.phi[t])/Ldef ode_v(m, t):return m.v_dot[t] == m.av[t]# 车辆移动过程中的约束m.path_v1 = Constraint(m.t, rule=lambda m, t: m.v[t] <= v_max)m.path_v2 = Constraint(m.t, rule=lambda m, t: m.v[t] >= v_min)m.path_a1 = Constraint(m.t, rule=lambda m, t: m.av[t] <= av_max)m.path_a2 = Constraint(m.t, rule=lambda m, t: m.av[t] >= -av_max)m.path_a3 = Constraint(m.t, rule=lambda m, t: m.v[t]**2*sin(m.phi[t])/L <= ar_max)m.path_a4 = Constraint(m.t, rule=lambda m, t: m.v[t]**2*sin(m.phi[t])/L >= -ar_max)#初始状态约束m.pc = ConstraintList()m.pc.add(m.x[0]==0)m.pc.add(m.y[0]==0)m.pc.add(m.a[0]==0)m.pc.add(m.v[0]==0)# 最终状态约束m.pc.add(m.x[tf]==0)m.pc.add(m.y[tf]==4*L)m.pc.add(m.a[tf]==0)m.pc.add(m.v[tf]==0)m.pc.add(m.av[tf]==0)m.pc.add(m.phi[tf]==0)# 定义目标函数m.integral = Integral(m.t, wrt=m.t, rule=lambda m, t: m.av[t]**2 + (m.v[t]**2*sin(m.phi[t])/L)**2)m.obj = Objective(expr=m.integral)# 模型求解TransformationFactory('dae.finite_difference').apply_to(m, wrt=m.t, nfe=30)SolverFactory('ipopt').solve(m).write()
# ==========================================================# = Solver Results =# ==========================================================# ----------------------------------------------------------# Problem Information# ----------------------------------------------------------Problem:Lower bound: -infUpper bound: infNumber of objectives: 1Number of constraints: 440Number of variables: 310Sense: unknown# ----------------------------------------------------------# Solver Information# ----------------------------------------------------------Solver:Status: okMessage: Ipopt 3.13.4\x3a Optimal Solution FoundTermination condition: optimalId: 0Error rc: 0Time: 3.7143239974975586# ----------------------------------------------------------# Solution Information# ----------------------------------------------------------Solution:number of solutions: 0number of solutions displayed: 0
查看模型求解结果
import numpy as npimport matplotlib.pyplot as plt# access the resultst = np.array([t for t in m.t])av = np.array([m.av[t]() for t in m.t])ar = np.array([(m.v[t]()**2 * np.sin(m.phi[t]()))/L for t in m.t])phi = np.array([m.phi[t]() for t in m.t])x = np.array([m.x[t]() for t in m.t])y = np.array([m.y[t]() for t in m.t])a = np.array([m.a[t]() for t in m.t])v = np.array([m.v[t]() for t in m.t])def plot_results(t, x, y, a, v, av, phi):fig, ax = plt.subplots(3,1, figsize=(10,8))ax[0].plot(t, av, t, v**2*np.sin(phi)/L)ax[0].legend(['Acceleration','Lateral Acceleration'])ax[1].plot(t, phi, t, a)ax[1].legend(['Wheel Position','Car Direction'])ax[2].plot(t, v)ax[2].legend(['Velocity'])ax[2].set_ylabel('m/s')for axes in ax:axes.grid(True)plot_results(t, x, y, a, v, av, phi)
对车辆轨迹进行可视化
scl=0.3def draw_car(x=0, y=0, a=0, phi=0):R = np.array([[np.cos(a), -np.sin(a)], [np.sin(a), np.cos(a)]])car = np.array([[0.2, 0.5], [-0.2, 0.5], [0, 0.5], [0, -0.5],[], [-0.2, -0.5], [0, -0.5], [0, 0], [L, 0], [L, 0.5],[],[], [L, 0.5],[L, -0.5],[],[]])carz = scl*R.dot(car.T)plt.plot(x + carz[0], y + carz[1], 'k', lw=2)plt.plot(x, y, 'k.', ms=10)plt.figure(figsize=(10,10))for xs,ys,ts,ps in zip(x, y, a, phi):draw_car(xs, ys, ts, scl*ps)plt.plot(x, y, 'r--', lw=0.8)plt.axis('square')plt.grid(True)
在模型1中进行车辆轨迹的控制时，固定了控制时段，现将控制时段也作为优化项，考虑进模型里，那么模型中的约束将转变为如下形式
目标函数定义为
引入缩放系数，对模型进一步整理
目标函数变为
对该模型进行建模和求解
# 参数ar_max = 2.8av_max = 2.8phi_max = 0.7v_max = 30v_min = -4# 车辆长度L = 5# 模型构建m = ConcreteModel()# 集合定义m.tf = Var(domain=NonNegativeReals)m.t = ContinuousSet(bounds=(0, 1))# 定义决策变量m.av = Var(m.t)m.phi = Var(m.t, bounds=(-phi_max, phi_max))# 定义状态变量m.x = Var(m.t, domain=NonNegativeReals)m.y = Var(m.t, domain=NonNegativeReals)m.a = Var(m.t)m.v = Var(m.t)# 定义一阶导数m.x_dot = DerivativeVar(m.x)m.y_dot = DerivativeVar(m.y)m.a_dot = DerivativeVar(m.a)m.v_dot = DerivativeVar(m.v)# 约束定义def ode_x(m, t):return m.x_dot[t] == m.v[t]*cos(m.a[t])def ode_y(m, t):return m.y_dot[t] == m.v[t]*sin(m.a[t])def ode_a(m, t):return m.a_dot[t] == m.v[t]*tan(m.phi[t])/Ldef ode_v(m, t):return m.v_dot[t] == m.av[t]# 车辆移动过程中的约束m.path_v1 = Constraint(m.t, rule=lambda m, t: m.v[t] <= m.tf*v_max/L)m.path_v2 = Constraint(m.t, rule=lambda m, t: m.v[t] >= m.tf*v_min/L)m.path_a1 = Constraint(m.t, rule=lambda m, t: m.av[t] <= m.tf**2*av_max/L)m.path_a2 = Constraint(m.t, rule=lambda m, t: m.av[t] >= -m.tf**2*av_max/L)m.path_a3 = Constraint(m.t, rule=lambda m, t: m.v[t]**2*sin(m.phi[t]) <= m.tf**2*ar_max/L)m.path_a4 = Constraint(m.t, rule=lambda m, t: m.v[t]**2*sin(m.phi[t]) >= -m.tf**2*ar_max/L)# 初始状态约束m.x[0].fix(0)m.y[0].fix(0)m.a[0].fix(0)m.v[0].fix(0)# 最终状态约束m.x[1].fix(0)m.y[1].fix(4)m.a[1].fix(0)m.v[1].fix(0)m.av[1].fix(0)m.phi[1].fix(0)#定义目标函数m.integral = Integral(m.t, wrt=m.t, rule=lambda m, t: m.av[t]**2 + (m.v[t]**2*sin(m.phi[t]))**2)m.obj = Objective(expr= m.tf + L**2*m.integral/m.tf**3)# 模型求解TransformationFactory('dae.finite_difference').apply_to(m, wrt=m.t, nfe=30)SolverFactory('ipopt').solve(m).write()
# ==========================================================# = Solver Results =# ==========================================================# ----------------------------------------------------------# Problem Information# ----------------------------------------------------------Problem:Lower bound: -infUpper bound: infNumber of objectives: 1Number of constraints: 430Number of variables: 301Sense: unknown# ----------------------------------------------------------# Solver Information# ----------------------------------------------------------Solver:Status: okMessage: Ipopt 3.13.4\x3a Optimal Solution FoundTermination condition: optimalId: 0Error rc: 0Time: 1.3755340576171875# ----------------------------------------------------------# Solution Information# ----------------------------------------------------------Solution:number of solutions: 0number of solutions displayed: 0
查看模型求解结果
# access the resultst = np.array([t*m.tf() for t in m.t])av = np.array([m.av[t]()*L/(m.tf()**2) for t in m.t])phi = np.array([m.phi[t]() for t in m.t])x = np.array([m.x[t]()*L for t in m.t])y = np.array([m.y[t]()*L for t in m.t])a = np.array([m.a[t]() for t in m.t])v = np.array([m.v[t]()*L/m.tf() for t in m.t])ar = v**2 * np.sin(phi)/Lplot_results(t, x, y, a, v, av, phi)
车辆轨迹可视化
scl=0.2plt.figure(figsize=(10,10))for xs,ys,ts,ps in zip(x, y, a, phi):draw_car(xs, ys, ts, scl*ps)plt.plot(x, y, 'r--', lw=0.8)plt.axis('square')plt.grid(True)
微信公众号后台回复
加群：加入全球华人OR|AI|DS社区硕博微信学术群
资料：免费获得大量运筹学相关学习资料
人才库：加入运筹精英人才库，获得独家职位推荐
电子书：免费获取平台小编独家创作的优化理论、运筹实践和数据科学电子书，持续更新中ing...
加入我们：加入「运筹OR帷幄」，参与内容创作平台运营
知识星球：加入「运筹OR帷幄」数据算法社区，免费参与每周「领读计划」、「行业inTalk」、「OR会客厅」等直播活动，与数百位签约大V进行在线交流
文章须知
微信编辑：疑疑
关注我们
FOLLOW US
