最优控制与轨迹规划学习笔记：包含倒立摆控制、路径规划优化及离散点参考线优化案例

最优控制和轨迹规划学习笔记 包含多个实际案例 倒立摆上翻控制 满足车辆运动学约束的路径规划 离散点参考线优化 lattice横向距离规划 这段代码包含了三个程序，我们将分别对它们进行详细的分析。 1. 最速降线问题求解 这个程序的主要功能是通过优化算法求解最速降线问题。它应用于物理学、工程学和数学等领域，用于确定两个给定点之间的最速下降路径。 程序的主要思路是通过将路径分成多个小段，然后通过优化算法找到每个小段的最佳下降路径。程序首先定义起点和终点的坐标，然后根据给定的分段数目将路径分成多个小段。接下来，它使用fminunc函数和CostTime函数来进行优化，找到最佳的路径。最后，程序使用plot函数绘制出找到的路径和解析解的路径。 CostTime函数是目标函数，它计算路径的总时间。它通过计算每个小段的长度和速度来计算总时间。程序中的for循环用于计算每个小段的长度，并将其加到总时间中。 2. 车辆路径规划 这个程序的主要功能是进行车辆路径规划，以避开给定的障碍物。它应用于自动驾驶、机器人导航和交通控制等领域，用于确定车辆的最佳路径，以避开障碍物并到达目标位置。 程序的主要思路是使用优化算法来找到车辆的最佳路径。程序首先定义车辆的初始位置和障碍物的位置和尺寸。然后，它使用循环来计算横向偏移边界，以确保车辆不会碰到障碍物。接下来，程序使用casadi库来进行优化，定义决策变量和目标函数，并添加约束条件。最后，程序使用plot函数绘制出找到的路径和障碍物。 3. 参考点路径规划 这个程序的主要功能是进行参考点路径规划，以使车辆按照给定的参考点行驶。它应用于自动驾驶、机器人导航和路径规划等领域，用于确定车辆的最佳路径，以便它按照给定的参考点行驶。 程序的主要思路是使用优化算法来找到车辆的最佳路径。程序首先定义参考点的坐标和车辆的初始状态和终点状态。然后，它使用casadi库来进行优化，定义决策变量和目标函数，并添加约束条件。最后，程序使用plot函数绘制出找到的路径和参考点。 以上是对给定代码的详细分析和解释。这些程序涉及到的知识点包括优化算法、数值计算、路径规划和绘图等。希望这些解释对你有帮助！

一、最速降线问题求解模块（Brachistochrone_curve.m）

1.1 功能定位

该模块旨在数值求解经典的最速降线问题，即寻找一条从指定起点到终点的曲线，使小球在重力作用下沿此曲线滑动的时间最短。同时，通过与解析解对比，验证数值解法的准确性。

1.2 核心逻辑

• 问题离散化：将整个路径划分为N+2个节点（N为分段数目，默认200），以相邻节点间的直线段近似曲线，通过优化中间节点的纵坐标，逼近最优曲线。
• 初始解设定：以起点到终点的直线作为初始解，为后续优化提供基础起点。
• 时间成本计算：基于物理公式，计算小球在每段直线上的滑动时间，累加得到总时间作为优化目标函数。核心是利用重力场中速度与下落高度的关系，结合线段长度，推导每段时间的计算方式。
• 优化求解：采用拟牛顿算法（quasi-newton）对目标函数进行最小化求解，得到最优的节点纵坐标序列。
• 结果可视化：同时绘制数值解（以星号标记）和解析解（摆线），直观对比两种解法的差异，验证数值解的合理性。

1.3 关键参数与输出

• 输入参数：起点坐标（xy0，默认[0,0]）、终点坐标（xyf，默认[10,10]）、分段数目（N，默认200）。
• 输出结果：最优曲线的横纵坐标序列（xs, ys）、数值解与解析解的对比图像。

二、倒立摆上翻控制模块（CartPoleSwingUp目录）

2.1 功能定位

该模块实现倒立摆系统从初始下垂状态（摆角为0）到目标上翻状态（摆角为π）的最优控制，通过施加水平力控制小车运动，使摆杆稳定到达竖直向上位置，同时最小化控制能量消耗。

2.2 核心组件与功能

2.2.1 系统参数定义（CartPoleSwingUp.m）

设定倒立摆的物理参数，包括摆长（l）、小车质量（m1）、摆球质量（m2）、重力加速度（g）；定义初始状态（init，小车位置、摆角、小车速度、摆角速度均为0）和目标状态（ref，小车位置为0、摆角为π、速度均为0）；设置约束条件（小车位置范围、控制 force 范围）及控制时间（T，默认2秒）。

2.2.2 动力学建模（autoGen_cartPoleDynamics.m、cartPoleDynamics.m）

• 动力学方程推导：基于拉格朗日力学推导倒立摆的加速度方程，考虑小车水平力（F）和摆杆扭矩（T，此处设为0）的作用，计算小车加速度（ddx）和摆角加速度（ddq），生成状态向量的导数（dz），即小车速度、摆角速度、小车加速度、摆角加速度。
• 自动代码生成：利用符号计算工具生成动力学方程的代码，确保公式推导的准确性，避免手动计算误差。

2.2.3 运动学计算（autoGen_cartPoleKinematics.m）

根据小车位置（x）、摆角（q）及对应的速度（dx、dq），计算摆球的位置（p）和速度（dp），为后续可视化和动力学约束提供运动学基础。

2.2.4 优化问题构建（CartPoleSwingUp.m）

• 决策变量定义：包括状态变量（X，4维，涵盖小车位置、摆角、速度）、加速度变量（A，2维，小车和摆杆加速度）、控制变量（U，1维，小车水平力）。
• 约束条件施加：
• 边界约束：初始状态和目标状态固定，确保控制过程从起点到终点。
• 动力学约束：通过积分器（getIntegratorDynamics.m）将加速度与状态转移关联，利用梯形积分公式计算下一时刻状态，保证状态变化符合动力学规律。
• 变量范围约束：控制 force 、小车位置在设定范围内。
• 目标函数设定：以控制 force 的平方和作为目标函数，最小化控制能量消耗。

2.2.5 积分器实现（getIntegratorDynamics.m）

定义状态转移的积分函数，基于当前状态（位置、速度）、加速度和时间步长（dT），通过梯形积分计算下一时刻的状态，核心是将连续时间的动力学方程离散化，适用于数值优化。

2.2.6 可视化模块（animate.m、drawCartPole.m、plotTraj.m）

• 实时动画（animate.m、drawCartPole.m）：根据每时刻的状态向量，绘制小车（矩形）、摆杆（线段）和摆球（圆点）的位置，模拟倒立摆的运动过程，支持调整播放速度和绘图窗口编号。
• 轨迹绘制（plotTraj.m）：绘制小车位置、摆角、小车速度、摆角速度及控制 force 随时间的变化曲线，直观展示控制过程中各变量的动态特性。

2.3 求解与输出

采用IPOPT求解器对非线性优化问题进行求解，得到最优的状态轨迹（traj.state）和控制序列（traj.control），输出动画和变量时间历程图，验证控制效果是否满足要求。

三、加农炮轨迹优化模块（cannon_control目录）

3.1 功能定位

该模块解决加农炮炮弹的最优轨迹规划问题，在考虑空气阻力（二次 drag）的情况下，寻找从原点（0,0）到目标点（6,0）的发射轨迹，最小化初始发射速度的平方（即最小化发射能量），同时满足动力学约束。

3.2 核心组件与功能

3.2.1 动力学模型（cannonDynamics.m）

建立炮弹的运动微分方程，考虑水平和竖直方向的空气阻力（与速度平方成正比，系数为c）及重力作用（竖直向下，单位重力）。状态向量包括位置（x、y）和速度（dx、dy），状态导数为速度和加速度（水平加速度由水平 drag 决定，竖直加速度由竖直 drag 和重力共同决定）。

3.2.2 事件检测（groundEvent.m）

定义ODE求解的事件函数，当炮弹竖直位置（y）为0时触发终止条件，用于模拟炮弹落地过程，为优化初始值提供参考轨迹。

3.2.3 参考轨迹生成（simulateCannon.m）

给定初始发射速度（v0）、发射角度（th0）、分段数目（N）和 drag 系数（c），利用ODE求解器（ode45）模拟炮弹飞行过程，生成参考轨迹（位置、速度、时间），作为优化问题的初始猜测值，提高求解收敛速度。

3.2.4 优化问题构建与求解（cannon_control.m）

• 决策变量：包括最终时间（tf）、时间步长（dt=tf/(N-1)）、各节点的位置（x、y）和速度（dx、dy）。
• 目标函数：以初始发射速度的平方（dx(1)² + dy(1)²）为优化目标，最小化发射能量。
• 约束条件：
• 边界约束：起点（x(1)=0,y(1)=0）和终点（x(N)=6,y(N)=0）位置固定。
• 动力学约束：采用梯形积分离散化动力学方程，确保相邻节点的状态转移符合炮弹运动规律。
• 求解与可视化：设置初始猜测值（参考轨迹），调用IPOPT求解器得到最优轨迹，绘制炮弹的空间轨迹（x-y图），验证是否到达目标点且轨迹合理。

四、车辆轨迹规划模块

4.1 车辆运动学约束轨迹规划（vehicle_Path.m）

4.1.1 功能定位

针对车辆运动学模型（ Ackermann 转向模型），规划从初始状态（位置、航向角、速度）到目标状态的最优路径，满足车辆运动学约束（如转向角限制、加速度限制），同时最小化时间成本和速度变化量。

4.1.2 核心逻辑

• 车辆参数与状态定义：设定车辆轴距（L，默认2.8）；定义初始状态（x0=0,y0=0,theta0=0,v0=0）和目标状态（x1=-3,y1=3,theta1=π/2）；决策变量包括位置（x、y）、航向角（theta）、前轮转向角（delta）、时间步长（dt）、速度（v）。
• 目标函数：以时间步长的平方（加权）和速度变化量的平方和为目标，平衡运动时间和行驶平稳性。
• 约束条件：
• 边界约束：初始和目标状态固定，速度初始值为0。
• 运动学约束：基于 Ackermann 模型，建立位置变化、航向角变化与速度、转向角、时间步长的关系，确保车辆运动符合物理规律。
• 变量范围约束：时间步长、速度、转向角、速度变化率（加速度）在合理范围内。
• 求解与可视化：调用IPOPT求解器得到最优路径，绘制车辆位置轨迹，并用箭头标记初始和目标航向角，验证路径是否满足运动学约束。

4.2 离散参考线平滑（ref_point_smooth.m）

4.2.1 功能定位

对原始离散参考点进行优化调整，在保证参考线与原始点偏差较小、曲线总长度合理的前提下，使参考线更平滑（曲率满足约束），为车辆路径跟踪提供高质量的参考轨迹。

4.2.2 核心逻辑

• 原始参考点定义：输入一系列离散的参考点坐标（waypoints），计算相邻点的距离，确定平均步长（deltaS）。
• 目标函数构建：
• 参考点偏差项：最小化优化后点与原始参考点的位置偏差，确保轨迹贴近原始路径。
• 平滑性项（FEM 位置偏差）：最小化相邻三点的二阶差分平方和，降低曲线曲率变化，提升平滑性。
• 长度约束项：最小化相邻点距离的平方和，控制轨迹总长度，避免过度拉伸或压缩。
• 约束条件：
• 位置偏差约束：优化后点相对于原始点的偏移量在设定范围内（xu、xl、yu、yl，默认1）。
• 曲率约束：基于离散点曲率公式，通过限制相邻三点二阶差分的平方和，确保曲线曲率不超过最大允许值（由最小转弯半径R决定）。
• 求解与可视化：调用IPOPT求解器得到平滑后的参考线，绘制原始点（圆圈标记）和平滑点（星号标记），对比展示平滑效果。

4.3 Lattice 横向距离规划（lattice_plan.m）

4.3.1 功能定位

在Lattice轨迹规划框架中，实现横向方向的最优位置规划。根据障碍物位置确定横向偏移的边界范围，结合车辆横向运动的动力学约束，生成一条偏离参考线最小且无碰撞的横向轨迹。

4.3.2 核心逻辑

• 环境与参数定义：设定车辆初始位置（vehpos）、障碍物列表（oblist）及尺寸（obw、obl）；定义纵向步长（ds，默认1），将纵向路径划分为60个节点，计算每个纵向节点对应的横向偏移上下边界（ll、lu），确保避开障碍物。
• 决策变量：包括横向偏移（l）、横向速度（dl）、横向加速度（ddl）、横向加加速度（dddl），全面描述横向运动状态。
• 目标函数：以横向偏移的平方和为目标，最小化轨迹与参考线（横向偏移为0）的偏离程度，保证行驶稳定性。
• 约束条件：
• 边界约束：每个纵向节点的横向偏移在计算出的上下边界内，避免碰撞。
• 动力学约束：基于横向运动的积分关系，建立横向偏移、速度、加速度之间的状态转移约束，确保运动连续平滑；限制横向加加速度的范围，控制加速度变化率，提升舒适性。
• 求解与可视化：调用IPOPT求解器得到最优横向轨迹，绘制参考线、横向边界（红色）、最优轨迹（绿色）及障碍物（蓝色矩形）、车辆初始位置（红色矩形），验证轨迹的无碰撞性和合理性。

五、共性技术与工具

5.1 优化框架与求解器

所有模块均基于CasADi工具包构建优化问题，CasADi提供了灵活的符号计算和数值优化接口，支持将连续时间的动力学问题转化为离散的非线性规划（NLP）问题。统一采用IPOPT（Interior Point OPTimizer）作为求解器，IPOPT擅长处理带约束的非线性优化问题，能够高效收敛到最优解，适用于最优控制和轨迹规划场景。

5.2 离散化方法

为将连续时间的动力学方程转化为数值可解的离散问题，各模块均采用梯形积分法进行离散化处理。该方法通过计算相邻节点的动力学导数平均值，乘以时间步长得到状态变化量，兼顾计算精度和效率，避免一阶欧拉法精度低或高阶方法计算量大的问题。

5.3 可视化与验证

每个模块均配备完善的可视化功能，通过绘制轨迹、时间历程曲线、动画等形式，直观展示优化结果的合理性。同时，部分模块（如最速降线）通过与解析解对比，或（如加农炮）通过参考轨迹初始化，确保数值解的准确性和可靠性。

六、应用场景与扩展方向

6.1 应用场景

• 最速降线：可用于机械设计中快速输送装置的路径优化，如物料滑道、过山车轨道设计等。
• 倒立摆控制：典型的欠驱动系统控制案例，可应用于机器人平衡控制（如双足机器人、自平衡小车）、卫星姿态控制等领域。
• 加农炮轨迹优化：适用于弹药发射、无人机航迹规划、导弹弹道设计等需要考虑空气阻力的运动轨迹优化场景。
• 车辆轨迹规划：直接应用于自动驾驶领域，包括参考线平滑、横向避障、满足运动学约束的路径生成，为车辆控制提供高质量的参考轨迹。

6.2 扩展方向

• 算法优化：部分模块（如参考线平滑、Lattice横向规划）可进一步转化为二次规划（QP）问题，采用QP求解器（如OSQP）提升求解速度，满足自动驾驶实时性要求。
• 约束扩展：增加更多实际场景约束，如车辆动力学约束（轮胎力、侧偏角）、动态障碍物、交通规则（限速、车道边界）等，提升轨迹的实用性。
• 多目标优化：当前模块多为单目标优化（如最小化控制能量、偏移量），可扩展为多目标优化，平衡行驶时间、舒适性、能耗等多个目标，提供更灵活的轨迹选择。

最优控制和轨迹规划学习笔记 包含多个实际案例 倒立摆上翻控制 满足车辆运动学约束的路径规划 离散点参考线优化 lattice横向距离规划 这段代码包含了三个程序，我们将分别对它们进行详细的分析。 1. 最速降线问题求解 这个程序的主要功能是通过优化算法求解最速降线问题。它应用于物理学、工程学和数学等领域，用于确定两个给定点之间的最速下降路径。 程序的主要思路是通过将路径分成多个小段，然后通过优化算法找到每个小段的最佳下降路径。程序首先定义起点和终点的坐标，然后根据给定的分段数目将路径分成多个小段。接下来，它使用fminunc函数和CostTime函数来进行优化，找到最佳的路径。最后，程序使用plot函数绘制出找到的路径和解析解的路径。 CostTime函数是目标函数，它计算路径的总时间。它通过计算每个小段的长度和速度来计算总时间。程序中的for循环用于计算每个小段的长度，并将其加到总时间中。 2. 车辆路径规划 这个程序的主要功能是进行车辆路径规划，以避开给定的障碍物。它应用于自动驾驶、机器人导航和交通控制等领域，用于确定车辆的最佳路径，以避开障碍物并到达目标位置。 程序的主要思路是使用优化算法来找到车辆的最佳路径。程序首先定义车辆的初始位置和障碍物的位置和尺寸。然后，它使用循环来计算横向偏移边界，以确保车辆不会碰到障碍物。接下来，程序使用casadi库来进行优化，定义决策变量和目标函数，并添加约束条件。最后，程序使用plot函数绘制出找到的路径和障碍物。 3. 参考点路径规划 这个程序的主要功能是进行参考点路径规划，以使车辆按照给定的参考点行驶。它应用于自动驾驶、机器人导航和路径规划等领域，用于确定车辆的最佳路径，以便它按照给定的参考点行驶。 程序的主要思路是使用优化算法来找到车辆的最佳路径。程序首先定义参考点的坐标和车辆的初始状态和终点状态。然后，它使用casadi库来进行优化，定义决策变量和目标函数，并添加约束条件。最后，程序使用plot函数绘制出找到的路径和参考点。 以上是对给定代码的详细分析和解释。这些程序涉及到的知识点包括优化算法、数值计算、路径规划和绘图等。希望这些解释对你有帮助！