增广拉格朗日iLQR时空联合规划代码简介与再开发3-iLQR目录

智能驾驶破壁人

于 2024-10-11 16:59:56 发布

阅读量656

点赞数 24

文章标签：算法

本文链接：https://blog.csdn.net/weixin_46006849/article/details/142825032

版权

增广iLQR-时空联合规划算法代码简介与再开发-前言_时空联合优化器-CSDN博客文章浏览阅读294次，点赞6次，收藏11次。简单来说就是同时求解路径与速度曲线。时空联合规划本质上是求解最优化问题，将路径和速度曲线作为优化问题的变量，同时得到二者在可行范围内的最优解。前言介绍LQR和iLQR介绍增广拉格朗日函数法配置开源代码，编译和plot展示正式开始介绍代码，先介绍iLQR再介绍增广拉格朗日部分iLQR（预计10月底完成iLQR更新）iLQR类和问题构建系统模型DiscreteDynamics 和ContinuousDynamics类连续模型离散化过程。_时空联合优化器https://blog.csdn.net/weixin_46006849/article/details/142681970?spm=1001.2014.3001.5501

介绍

iLQR开源代码链接：https://github.com/Grandereohb/ALTRO-iLQR。

后续代码介绍和再开发都基于此branch，建议申请一个账号建立自己的branch进行开发溯源,代码运行和编译会单独再开一篇。

本篇主要介绍iLQR代码目录，Eigen库，以及模板类。Eigen库和模板类的使用贯穿整个代码,有cpp基础的同学可以跳过。为了方便理解代码主要介绍Eigen库和模板类的基本使用，细节自行了解。

目录简介

根目录

根目录列表如上图所示，包含若干文件夹，CMakeLists.txt,README.md和run.sh.

CMakeLists.txt
- cmake使用CMakeLists.txt文件描述编译过程，当前阶段无需改动CMakeLists文件，后续再发开需要添加新的文件夹才需要修改该文件。有兴趣的可以自行了解。

README.md

介绍编译以及安装步骤，编译的宏定义的使用。


sudo apt-get install cmake libeigen3-dev libfmt-dev doxygen# 安装依赖


cd altro-cpp         # Change directory into directory root.
mkdir build          # Create a build directory.
cmake ..             # Run the CMake configuration step. 
cmake --build .      # Build all CMake targets

run.sh
- 可以自行编写,比如通过bash run.sh 可以执行文件编译并打开可执行test。
- ```
cd build
make --build .
cd bin
./ilqr_test
```

altro

augmented_lagrangian
- 增广拉格朗日函数法可以求解约束最优化问题。iLQR本身是没有约束条件的，通过引入拉格朗日乘数和参数，将约束条件融入到iLQR目标函数构造新的目标函数，从而简化了问题的求解过程；
- 包含增广拉格朗日问题的构建以及求解；
- 包含添加约束条件的增广拉个朗日函数目标函数。
common
- 包含iLQR 求解器的状态类和iLQR参数类；
- 包含线程安全相关类；
- l包含log类可以在求解问题的过程中记录数据；
- 定义iLQR 每次迭代的序列点信息。
constraints
- 为增广拉格朗日函数提供约束条件参数矩阵，包括拉格朗日乘子，惩罚函数矩阵等；
- 定义等式和不等式类型；
- 计算等式和不等式约束条件Jacobian，Hessian和投影锥矩阵。
ilqr
- 包含iLQR问题求解器，问题的构建不在该目录下；
- 包含系统状态方程的离散化和线性化；
- 计算状态和控制序列向量的目标成本函数。
problem
- 构造问题，包含初始化状态，约束条件，状态转移方程和目标成本函数矩阵；
- 包含连续状态转移方程离散化方法，通过重写可以选择合适的离散化方式。
utils
- 包含Assert_msg显示信息；
eigentypes.cpp
- 使用 Eigen 库定义的矩阵类型的别名,方便内部使用。

定义了一个标准化的方式来导出CMake库目标，便于其他CMake项目可以导入和使用。可以自定义设置包含目录、安装路径以及将其添加到指定的导出集中。

docs

算法描述文档。

examples

创建自行车，卡车以及三阶积分器三种自车运动模型，并根据这三种模型构造三种不同的问题类；
包含障碍物约束条件；

perf

生成问题求解器的迭代数据，可以自行设置。

test

使用Gtest 定义测试用例，可以设置launch.json 进行单步调试，详见4配置开源代码，编译篇。

Eigen库

Eigen库是一个高级的C++库，它主要用于线性代数、矩阵和向量运算、数值分析及其相关算法。在使用之前需要包含其头文件#include <eigen3/Eigen/Dense>。

Eigen::Dynamic
1. 矩阵或向量的尺寸在编译时是未知的，动态使用；
2. Eigen::Matrix<double,Eigen::Dynamic,1> 表示一个元素类型为double、大小未知的动态向量。
Eigen::Matrix<T,n,m>
1. 模板类，template <int n, int m,class T = double>,n,m 在编译的时候是确定的；
2. Eigen:Matrix<double,3,2> 表示一个元素类型为double，3行2列的矩阵。
Eigen::VectorXd;
1. Eigen::VectorXd是一个模板特化，等于Eigen::Matrix<double, Eigen::Dynamic, 1>用于表示动态大小的列向量。
Eigen::Ref<const Eigen::VectorXd>
1. Eigen::Ref<const Eigen::VectorXd>是一个模板类Eigen::Ref的特化，用于创建一个对Eigen::VectorXd类型的常量引用,避免复制。
Eigen::MatrixXd;
1. Eigen::MatrixXd是一个模板特化，等于Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>，矩阵的行数和列数都是double类型并且未知。
矩阵或向量的计算，AB为矩阵或者向量
1. A * B 矩阵相乘
2. A.dot(B) 矩阵的点积运算
3. A.transpose() 转置矩阵

模板类

模板类允许在定义类的时候类的某些部分在实例化的时候才确定，比如成员变量的类型或者实现。这样可以创造出功能相同但是数据类型不同的类实例，实现代码的重用和泛型编程。

需要实例化才确定的部分通过模板参数来制定，template <typename T>来声明模板参数并且在类定义中使用这个占位符T来制定需要延迟确定的部分。

对于状态和控制量的维度可以通过模板类来进行设置，比如自车的状态量可以包含3个维度（x,y,heading）,或者5个维度（x,y,heading,yawrate,a）。通过模板类可以实现不同状态量和控制量维度的使用，针对不同的规划场景可以灵活运用。

//实例化，6为状态向量维度，2为控制向量维度
altro::ilqr::iLQR<6, 2> solver = MakeSolver<6, 2>();

// 类定义
template<int n = Eigen::Dynamic, int m = Eigen::Dynamic>
class iLQR