简单的最优控制软件。

Igor Ladnik

4.97/5 (94投票s)

2015 年 1 月 9 日

CPOL

15分钟阅读

149715

3238

用于求解二次最优控制问题的简单软件。

简介

在本文中，我想介绍一个紧凑且易于使用的动态系统最优控制软件工具。最优控制是一个非常广泛的主题。其基础由 Norbert Wiener、John Ragazzini、Lotfi Zadeh、Richard Bellman、Lev Pontryagin、Rudolf Kalman 等人在 20 世纪 40-60 年代的工作奠定。最优控制有许多实际应用。在此，我们将重点关注以下经典的最优控制问题。我们的目标是以外部控制输入，将一个给定的动态系统从其初始状态转移到某个最终状态。我们希望在此过程中，系统的参数尽可能地接近其预定的轨迹。这是最优控制中一个非常基本且具有大量实际应用的任务。例如，启动电动机时限制电流的初始跳变到可接受的值；快速抑制电子和机械系统中的不良振荡；以及从航天器控制到导弹拦截的无数复杂应用。

处理这个问题甚至其表述都需要大量的数学知识。我将仅限于描述问题和理解如何使用软件所需的最低限度。一些附加的递归公式在附录中给出，但不提供证明。

问题陈述

让我们形式化我们的问题。一个动态系统由以下一组微分方程描述：

(1)

其中 t 是时间，x 是描述系统行为的参数的状态向量，ẋ 是 x 随时间的一阶导数向量，m 是控制向量。通常，控制向量的维数不超过状态向量的维数。在这个陈述中，向量 m 不依赖于向量 x，可以被视为开环系统的控制。带有输入控制向量 m 和状态向量 x 的开环控制系统如下图所示：

开环控制系统

我们希望在 0 到最终时间 t_f 的时间间隔内获得控制策略 m(t)，以最小化成本函数：

(2)

其中上标 ^T 表示向量或矩阵的转置，r 是期望的状态向量，u 是期望的控制向量，Q 和 Z 分别是状态向量和控制向量偏差的权重矩阵。在大多数情况下，Q 和 Z 是对角矩阵（但不一定是！）。它们对角线上的每个元素定义了在给定时刻，期望值和实际值之间参数平方差的相对权重。这些矩阵可以是时间相关的，例如，通常的做法是增加最终时刻偏差的相对权重，以确保期望的最终状态。这里提出的问题通常被称为“跟踪问题”。

线性动态系统

在给出解决方案之前，让我们讨论动态系统的一个特殊情况。研究最深入的一类系统是线性动态系统，即其动力学由一组线性微分方程描述的系统。对于这样的系统，方程 (1) 可以重写为：

(3)

其中 A(t) 和 B(t) 是矩阵。首先，我们将为这类系统提供一个解，然后将其推广到非线性动态系统。

离散时间

由于我们将提供数值解，我们将处理离散时间点的系统。因此，我们将问题重新表述为离散时间 t = k ⋅ Δt，其中 Δt 是一个很小的时间间隔（采样间隔），k 是给定的时间步，0 <= k < N, t_f = (N-1) ⋅ Δt。现在方程 (1) 可以为离散时间重写为：

(4)

而线性系统定义为：

(5)

其中 F_k 和 H_k 是由 A(t) 和 B(t) 得到的矩阵，如下所示：
F_k = exp(A(k⋅Δt)⋅Δt) ≅ I+A(k⋅Δt)⋅Δt, H_k = A^-1[exp(A(k⋅ Δt)⋅Δt) - I]B ≅ B(k⋅Δt)⋅Δt, 其中 I 是单位矩阵。

离散时间下的成本函数 (2) 将重塑为：

(6)

线性情况下的解

该问题的数值解可以基于 Bellman 最优性原理获得。结果相当冗长，并在附录中给出。为了通用理解，知道 m_k 控制策略是通过两个主要运行计算出来的至关重要。首先，执行一个反向计算运行，从最后一个时间步 N-1 开始，向下到初始时间 0。反向计算运行使用系统矩阵 F 和 H，期望状态 r 和控制 u 向量，以及权重矩阵 Q, Z，会得到一个向量 c_k 和一个矩阵 L_k。然后，执行正向运行，从时间 0 到 N-1，在每个 k 时间步得到向量 m_k，如下所示：

(7)

正向和反向计算运行都显示在下图：

计算方案

向量 c_N-k 和矩阵 L_N-k 由反向运行得知，它们依赖于系统矩阵 F 和 H，用户定义的期望向量和权重。它们还依赖于反向运行中涉及的辅助矩阵 P_N-k 和向量 v_N-k，这些矩阵和向量从零初始条件开始。在正向计算运行的每一步，我们根据该时间的状态向量 x_k 获得控制向量 m_k。然后使用方程 (5) 计算下一个状态 x_k+1。此过程重复进行，直到最后一个时间点 N-1，从而得到开环控制策略 m_k 和系统状态 x_k 随时间的变化。陈述 (7) 为我们提供了闭环控制，如下图所示：

闭环控制系统

其中向量 c 构成闭环控制，矩阵 L 提供状态向量 x 的反馈。这些参数允许设计者根据系统的实际状态生成向量 m，从而使系统更稳定、更鲁棒。

对线性情况解的讨论

对于线性系统，成本函数 (6) 的最小控制策略 m_k 仅需一次迭代即可实现，该迭代包括一次反向运行和一次正向运行。如上所述，该策略取决于系统矩阵和用户选择的参数，即期望向量和权重矩阵。这些参数足够直观，可以将控制策略设计简化为对它们的调整。用户选择一组参数，计算控制策略和相应的状态轨迹。如果对结果不满意，则对另一组期望向量和权重矩阵重复计算。还有趣的是，如果系统矩阵 F、H 和权重矩阵 Q 是时不变的，那么反馈矩阵 L 也将是时不变的，这大大简化了系统控制器设计。对于线性情况，系统矩阵 F、H 通常是时不变的。因此，要获得时不变反馈，用户应该选择恒定的权重矩阵 Q，即在最终时刻不严格追求期望最终状态的控制。在现实生活中，通过恰当选择 Q，我们可以在可接受的时间内，甚至在我们预定的最终时间之前，实现期望的最终状态。虽然使用状态向量坐标来实现控制策略很方便，但在现实生活中，通常并非所有这些坐标都可以测量。已经开发了各种技术来克服这个困难。可以使用不同种类的状态坐标观测器，利用可测量坐标的信息来估计未测量坐标。

非线性系统

非线性系统的情况要复杂得多。与线性系统不同，它没有针对所有类型系统的整体解决方案。在这里，我们可以尝试使用拟线性化方法将非线性优化问题简化为线性问题。为了使用相同的数学解，我们必须将方程 (1) 给出的通用动态系统描述以类似于方程 (5) 的线性化形式表示。为了实现这一点，我们应该考虑一个迭代计算过程。我们可以在i-th 轨迹的 x_k 和 m_k 的特定点上将 (1) 线性化为 (5)：

(8)

其中

(9)

线性化后的系统矩阵 F 和 H 是函数 f 在第 i 个轨迹的点 k 处相应的偏导数矩阵。第 (i+1)-th 轨迹上的状态和控制向量可以表示为：

(10)

方程 (8) 可以针对增量 Δx 和 Δm 进行公式化。在这种情况下，成本函数 (6) 中的新向量 r 和 u 对应于：

(11)

分别。这使得我们可以将非线性问题简化为一系列拟线性迭代，其中每一次迭代都如上面计算方案图所示。该算法包含以下步骤：

假设对于迭代 i=0，向量 x_k 和 m_k（通常除了 x_k=0 初始条件外都为零，但选择权在用户）根据 (8) 计算所有线性化后的 F_kⁱ 和 H_kⁱ。
根据 (11) 为第 i 次迭代计算新的 r_kⁱ 和 u_kⁱ。
使用反向和正向运行计算 Δm_kⁱ 和 Δx_kⁱ，如同线性情况一样。
计算第 (i+1) 次迭代的 x_kⁱ⁺¹ 和 m_kⁱ⁺¹。
检查停止迭代的条件。该条件可以基于，例如，与前一次迭代相比的成本函数值差异，或者包含固定数量的迭代。无论如何，如果条件未满足，则应重复步骤 1-5，直到满足 stop 条件。在条件满足后，策略 m_k 和状态 x_k 可以被视为最终解。

获得函数 f 的偏导数的精确公式可能是一项繁琐的工作，当函数 f 是数值定义的（例如，作为表格）时，则变得不可能。在这些情况下，可以通过给偏导数的状态向量坐标一个小的增量并计算 f 函数的结果值，来使用简单的数值近似。代码示例提供了相应的例子。

方法限制

与任何数值方法一样，此方法也有其局限性。一个局限性相当明显，并且应在线性和非线性情况中予以考虑：时间采样间隔应足够小，以保证计算的收敛性。r、u 的期望值和权重因子 Q 和 Z 应该合理，以实现直观的最佳系统行为。对于非线性系统，我们应该清楚地认识到这是一个“没有严格规则的游戏”，因此不能保证该技术适用于特定情况。该方法对线性化系统矩阵的计算精度很敏感。更复杂的方法可用于估计该方法对特定非线性系统的适用性，但对于工程师来说，尝试失败的方法似乎更实用。同样，在某种程度上，处理非线性系统仍然是“艺术而非科学”。

恒定反馈

与线性情况一样，也可以考虑非线性系统的恒定反馈控制。要获得非线性情况下的恒定反馈，权重矩阵 Q、Z、状态 x 和控制 m 的初始轨迹应为时不变（除了初始状态 x₀），并且应在单次迭代中获得可接受的结果（因为根据 (9) 计算的系统矩阵 F 和 H 依赖于前一次迭代轨迹）。通常，工程师在系统优化中采用一种相当非正式的方法，即最小化成本函数不是最终目标，而是实现可接受系统行为的一种手段。因此，在某些情况下，为了反馈的简便性而牺牲成本函数的进一步最小化可能是明智的。也有可能在某些情况下，恒定反馈可以在一定程度上补偿非线性。当然，就像非线性系统中的所有事物一样，它并非在所有情况下都可能。下面将讨论一个合适的数值示例。

软件使用

.NET

该软件用纯 C# 编写，不依赖任何附加库。在上次更新中，它已转换为 .NET 6（DLL 文件是 .NET Standard 2.0）。.NET 演示可以通过命令在 Windows 和 Linux 上运行：

dotnet Test.dll

项目 MatrixCalc 提供向量和矩阵的算术运算，包括转置和求逆。项目 SequentialQuadratic 支持系统优化和模拟的计算。项目 Test 使用 static 方法和接口 SequentialQuadratic 来解决特定问题。DLL SequentialQuadratic 包含几个内部类型，负责不同的优化问题（线性，带有恒定权重和期望值；线性，带有随时间变化的权重和期望值；以及相应的非线性）。这些类型的实例通过重载的 static 方法 SQ.Create() 创建，并通过一组相应的接口 IBase、IBaseLQExt、ITrackingLQ 和 INonLinearQ 暴露给调用者。为了执行优化过程，用户应调用适当的方法，提供输入并获取所需的接口。然后应调用 RunOptimization() 方法来实际执行优化计算，之后调用 Output2Csv() 方法将结果以逗号分隔的格式写入 Excel 兼容的 *.csv 文件。要使用给定的控制策略模拟系统，请调用 Simulate() 方法。下面是一个非线性系统（范德波尔振荡器）优化代码的示例：

public static void VanDerPol()
{
	const string TITLE = "Van der Pol";
	Console.WriteLine($"Begin \"{TITLE}\"");

	var lstFormatting = new List<OutputConfig> 
	{
		new OutputConfig { outputType = OutputType.Control, index = 0, title="m0",
		                      scaleFactor=1 },
		new OutputConfig { outputType = OutputType.State, index = 0, title="x0" },
		new OutputConfig { outputType = OutputType.State, index = 1, title="x1" },
	};

	const int dimensionX = 2;
	const int dimensionM = 1;

	const double dt = 0.1;
	const int N = 51;

	const int maxIterations = 17;

	var r = new Vector(dimensionX) { [0] = 0, [1] = 0 };
	var u = new Vector(dimensionM) { [0] = 0 };
	var Q = new Matrix(dimensionX, dimensionX) { [0, 0] = 1, [1, 1] = 1 };
	var Z = new Matrix(dimensionM, dimensionM) { [0, 0] = 1 };

	var xInit = new Vector(dimensionX) { [0] = 1, [1] = 0 };

	var mu = 1;

	var functions = new CalcDelegate[]
	{
		(k, deltaT, m, x) => x[1],
		(k, deltaT, m, x) => mu * (1 - Sq(x[0])) * x[1] - x[0] + m[0]
	};

	// Exact formulas for gradients calculation
	var gradientsA = new CalcDelegate[dimensionX, dimensionX];
	gradientsA[0, 0] = (k, deltaT, m, x) => 0;
	gradientsA[0, 1] = (k, deltaT, m, x) => 1;
	gradientsA[1, 0] = (k, deltaT, m, x) => -(2 * mu * x[0] * x[1] + 1);
	gradientsA[1, 1] = (k, deltaT, m, x) => mu * (1 - Sq(x[0]));

	var gradientsB = new CalcDelegate[dimensionX, dimensionM];
	gradientsB[0, 0] = (k, deltaT, m, x) => 0;
	gradientsB[1, 0] = (k, deltaT, m, x) => 1;

	(SQ.Create(functions, gradientsA, gradientsB, Q, Z, r, u, xInit, dt, N,
				(currCost, prevCost, iteration) => iteration > maxIterations)
		.RunOptimization()
		.Output2Csv("VanDerPol_ExactGrads", lstFormatting) as INonLinearQ)
		.OutputExecutionDetails(isExactGradient: true);

	// Numeric gradients calculation
	var delta = 0.1;
	(SQ.Create(functions, new Vector(dimensionM, delta), new Vector(dimensionX, delta),
						 Q, Z, r, u, xInit, dt, N,
						 (currCost, prevCost, iteration) => iteration > maxIterations)
		.RunOptimization()
		.Output2Csv("VanDerPol_NumGrads", lstFormatting) as INonLinearQ)
		.OutputExecutionDetails(isExactGradient: false);

	Console.WriteLine($"End \"{TITLE}\"{Environment.NewLine}");
}

Python

我提供了一个用 Python3 编写的简化版代码。目前，它包含一个线性示例和一个非线性示例。该代码可以在 Windows 和 Linux（我在 Ubuntu 20.04 上进行了测试）环境中运行。要运行代码，您需要安装最新版本的 Python3 及其 numpy 和 scipy 包，解压缩源代码，然后启动文件 Program.py。

在 Linux 中使用：

python3 Program.py

在 Windows 中使用：

python Program.py

命令。

Python 版本支持瞬态可视化。要激活此功能，请安装 matplotlib 包并删除文件 Program.py 中的所有注释 #1。

数值示例

线性系统

有一个机械系统：质量 M 受力作用。根据牛顿第二定律，力等于质量与加速度（位移的二阶导数）的乘积。令位移为 x₀，速度为 x₁，力为 m₀。系统由以下二阶矩阵微分方程描述：

ẋ = Ax + Bm, 其中

A =

\begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix}

B =

\begin{bmatrix} 0 \\ 1/M \end{bmatrix}

M = 1

x_k=0 =

\begin{bmatrix} 0 \\ 0 \end{bmatrix}

成本函数为 ∫₀⁴⁰[(10 - x₀)² + 3x₁²]dt

Q =

\begin{bmatrix} 1 & 0 \\ 0 & 3 \end{bmatrix}

Z =

\begin{bmatrix} 0 \end{bmatrix}

r =

\begin{bmatrix} 10 \\ 0 \end{bmatrix}

u =

\begin{bmatrix} 0 \end{bmatrix}

Δt = 1, N = 41。

线性系统的瞬态

计算得到的闭环输入和反馈系数为：

输入 4.48，

反馈：来自 x₀ -0.448，来自 x₁ -1.22。

范德波尔振荡器

这是一个非线性系统。我们的目标是平滑快速地抑制持续的振荡。

ẋ₀ = x₁
ẋ₁ = μ⋅x₁⋅(1 - x₀²) - x₀ + m₀, μ=1

x_k=0 =

\begin{bmatrix} 1 \\ 0 \end{bmatrix}

成本函数为 ∫₀⁸(x₀² + x₁² + m₀²)dt

Q =

\begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix}

Z =

\begin{bmatrix} 1 \end{bmatrix}

r =

\begin{bmatrix} 0 \\ 0 \end{bmatrix}

u =

\begin{bmatrix} 0 \end{bmatrix}

Δt = 0.1, N = 81。

经过 3 次迭代后受控范德波尔振荡器的瞬态

在上面的案例中，选择了期望状态和控制向量以及权重矩阵，我们观察到了良好的收敛迭代计算。

瑞利方程（带标量控制）

这是另一个非线性振荡系统。我们的目标与前一个案例相同：平滑快速地抑制持续的振荡。

ẋ₀ = x₁
ẋ₁ = -x₀ + (1.4 - 0.14⋅x₁²)⋅x₁ + 4⋅m₀

x_k=0 =

\begin{bmatrix} -5 \\ -5 \end{bmatrix}

成本函数为 ∫₀⁵(x₀² + m₀²)dt

Q =

\begin{bmatrix} 1 & 0 \\ 0 & 0 \end{bmatrix}

Z =

\begin{bmatrix} 1 \end{bmatrix}

r =

\begin{bmatrix} 0 \\ 0 \end{bmatrix}

u =

\begin{bmatrix} 0 \\ \end{bmatrix}

Δt = 0.01, N = 501。

3 次迭代后的瞬态

在这种情况下，即使第一次迭代也提供了可接受的结果。但是，对于选定的优化参数，成本函数的数值并没有稳步降低。

带恒定反馈的非线性系统

这是一个非线性系统，可以通过恒定反馈将其从给定的初始状态有效地转换为零状态。

ẋ₀ = x₁
ẋ₁ = -x₀⋅[π/2 + artan(5⋅x₀)] - 5⋅x₀² / [2⋅(1 + 25⋅x₀²)] + 4⋅x₁ + 3⋅m₀

x_k=0 =

\begin{bmatrix} 3 \\ -2 \end{bmatrix}

成本函数为 ∫₀¹⁰(5x₁² + m₀²)dt

Q =

\begin{bmatrix} 0 & 0 \\ 0 & 5 \end{bmatrix}

Z =

\begin{bmatrix} 1 \end{bmatrix}

r =

\begin{bmatrix} 0 \\ 0 \end{bmatrix}

u =

\begin{bmatrix} 0 \\ \end{bmatrix}

Δt = 0.01, N = 1001。

单次迭代的瞬态

m₀ vs. x₁（单次迭代）

上图显示了根据我们选择的权重矩阵，经过单次迭代后，使用我们的软件计算出的瞬态。最后一张图显示了我们计算中受控状态坐标 x₁ 和控制 m₀ 之间的关系。

双连杆机器人机械手

在此 CodeProject 帖子中介绍了将所述技术应用于双连杆机器人机械手最优控制。实现可在代码中找到。

四旋翼无人机

这篇 CodeProject 文章介绍了四旋翼无人机的最优控制。代码在那里已经更新。

代码样本中提供了更多数值示例。

结论

本文简要介绍了线性与非线性系统顺序二次最优控制问题的数值解。利用拟线性化方法将非线性问题转化为一系列针对线性化系统的线性-二次问题。提供了紧凑易用的软件，并附有使用示例。

附录

在反向运行中计算控制向量 m 的递归方程 (7) 中的参数 c 和 L 可以从动态规划 Bellman 方程获得（本文格式不允许提供相应的数学变换），作为以下递归运算集：

其中 P 和 v 是递归计算的辅助矩阵和向量，k 从 k = N - 1 开始，并且 P₀ = 0, v₀ = 0。

请注意，上述计算需要对依赖于传递矩阵 H、权重矩阵和矩阵 P 的辅助 W 矩阵进行求逆。

历史

2015年1月9日：初始版本
2021年6月9日：更新
2022年6月11日：修复了代码中的一个错误

简单的最优控制软件。

目录

简介

问题陈述

线性动态系统

离散时间

线性情况下的解

对线性情况解的讨论

非线性系统

方法限制

恒定反馈

软件使用

.NET

Python

数值示例

线性系统

范德波尔振荡器

瑞利方程（带标量控制）

带恒定反馈的非线性系统

双连杆机器人机械手

四旋翼无人机

结论

附录

历史