【智能网联汽车协同决策与规划技术学习笔记】（二）AMPL与Matlab联合仿真求解轨迹规划问题

1 个月前 · 来自专栏智能网联汽车协同决策与规划技术学习笔记

刘红铄

自动驾驶规划控制算法工程师

1.AMPL安装及运行

AMPL语言是一种描述数学规划模型的建模语言，其与IPOPT求解器有特定的接口，方便调用IPOPT求解器求解数学规划问题。下载地址为： https:// ampl.com/try-ampl/downl oad-a-free-demo/ 链接，下载ampl.mswin64.zip即可在windows系统中使用，在 https:// ampl.com/products/solve rs/open-source/ 中下载ipopt-win64.zip，将其解压在AMPL目录中，即可使用AMPL语法调用IPOPT求解器求解。

注：AMPL以及IPOPT均不需要安装，下载压缩包解压后点击EXE文件即可运行

使用AMPL求解器需要.mod类型的模型文件（用AMPL语法进行书写）和.INIVAL类型的初始解文件，注意AMPL的命令行指令末尾需要加冒号

2.轨迹规划问题建模

最优化问题由 目标函数 和约束组成，对轨迹规划问题建模就是针对轨迹规划问题建立相应的目标函数和约束。轨迹规划问题的任务是在车辆的起始时刻与终止时刻运动状态之间计算出符合约束条件的行驶轨迹，因此目标函数的建立要以“轨迹”为核心，约束条件则根据车辆的运行条件进行设置。

优化问题的目标函数包括 末值性能指标 和 积分性能指标 两部分，末值性能指标用于描述对终止时刻车辆运动状态的最小化需求；积分型性能指标用于表征在整个时域上对于某指标积累程度的最小化需求。对于轨迹规划问题，累计指标包括 轨迹平滑程度（由前轮转角和加速度表征）、轨迹远离障碍物程度 。

轨迹规划问题的约束条件包括如下几方面：

系统动态方程约束：即 车辆动力学（运动学）约束 ，以保证求解出的轨迹 是车辆在真实环境中是能够达到的 。
两点边界值约束：两点即 起始点 和 终止点 ，对于无人车轨迹规划问题，通常对终止点的状态值进行设计，以保证车辆以规定的 角度和位置 停车。
流行约束：车辆运动过程中的约束条件，他们能够将状态/控制变量约束在 解空间的某一流行上 ，其包括 车辆运动能力限制 和 外部环境限制 两方面。对于车辆运动能力限制，在系统动态方程约束中考虑的是车辆整体的宏观运动行为，此处则是给出机械层面的限制，如：最大速度、最大转向角度等，伴随车辆整个运动过程，与车辆的微观受力无关，此处建模较为简单。外部环境限制体现为，保证车辆在整个运动过程中 不与任何障碍物发生碰撞 ，此处建模的难点在于如何表征车辆与障碍物的距离（ 在ESDF地图中此信息可以直接读取 ），本书中 通过凸多边形描述障碍物信息，限制车身矩形顶点在障碍物凸多边形外部，同时限制障碍物多边形顶点在车身矩形外部，则二者一定不会发生碰撞 ，则无人车的外部环境限制转化为：某点处于某一凸多边形外部的约束建模问题。

注：最终的轨迹是如何形成的？ 不同的算法有不同的方法，本书的方法是通过求解最优控制问题得到终止时刻t和控制量u（t）(包括加速度和前轮偏转角速度)，通过这两个值可以推导得到全局坐标系下的x，y坐标值，从而得到轨迹。

3.轨迹规划问题的数值求解

求解一般性最优控制问题的方法包括间接法、动态规划法和直接法。间接法通过求解析解的方式求解最优问题，常用方法包括变分法、庞特里亚金极值原理等，难以处理较为复杂的约束。直接法的思想是通过离散化将最优控制问题转化为非线性规划问题（NLP），随后调用成熟的NLP求解器进行求解。

直接法的离散方法包括配点法和打靶法，配点法同时离散状态变量和控制变量，打靶法仅离散控制变量。根据离散目标的不同分为部分离散法和完全离散法，本书采用的全联立正交配置有限元法是一种完全离散方法。

非线性求解器中采用的优化方法（处理约束问题的方法）可以分为有效集方法（active-set-method）和障碍函数法（barrier-function-method）。

有效集方法的思想如下：如果先验地知道在最优解处哪些不等式约束有效，则可以将其余不等式约束条件剔除，降低求解难度。有效集法的注意工作是 通过不断检验来推测最优解处的有效约束是什么（有效约束的定义：在解向量处约束条件取等号的约束条件） 。有效集法一般不会考虑全部约束条件，在迭代过程中占用内存资源小，擅长处理初始解质量较高的简单问题，序贯二次规划（SQP）是目前最常用的有效集法。

障碍函数法将NLP问题中的不等式约束条件作为惩罚项补入目标函数中，并在不同的惩罚因子条件下进行牛顿迭代，直到惩罚因子收敛到$0^{+}$的某一充分小邻域并且对应的NLP问题求解成功为止（这一过程是双层嵌套计算，在外层计算合适的惩罚因子，在内层计算各个惩罚因子对应的子问题）。障碍函数法始终考虑全部约束条件，适合处理大规模，复杂约束条件的NLP问题，内点法（IPM）是典型的障碍函数法。

【2023年3月1日更新】

推荐两篇文章，第一篇文章对对该部分内容进行了系统的介绍

第二篇文章进行了形象的描述并提供了实现代码

4.用AMPL语言描述轨迹规划问题

问题描述包括两部分， 最优控制问题建模 和 最优控制问题离散化为非线性规划问题 。

创建变量的语法如下，AMPL语言中将参数类型的变量定义为param，将集合定义为set，将变量定义为var。

param K_radau := 3;     //参数向量，必须有明确的赋值
set I := {1..Nfe};      //一维向量，命名为I，从1到Nfe的整数集合
set I1:= {1..Nfe-1};
set J := {1..K_radau};
set K := {0..K_radau};
param tau{j in K};            //一维参数向量，用于离散化
param dljtauk{j in K,k in K}; //二维参数向量
var x{i in I, j in K};        //二维向量，命名为x，ij分别为下标
var y{i in I, j in K};
var theta{i in I, j in K};
var v{i in I, j in K};
var phy{i in I, j in K};
var a{i in I, j in J};
var w{i in I, j in J};
var tf >= 0;
var hi = tf / Nfe;

构建最优化问题的典型语法如下：

//目标函数，第二项是积分型性能指标，sum{i in I, j in J}是离散形式的积分表示
//该表达形式由离散方法决定
minimize cost_function:
10 * tf + sum{i in I, j in J}(w[i,j]^2);
//动态方程约束，整个set的x都要符合该约束，x = vcos(theta)
//DIFF_dxdt：约束句柄名称
//{i in I, k in J}：约束作用范围
s.t. DIFF_dxdt {i in I, k in J}:
sum{j in K}(dljtauk[j,k]*x[i,j]) = hi * v[i,k] * cos(theta[i,k]);
//变量在有限元边界处满足连续性条件，与全联立离散化方法相关
s.t. EQ_diffx {i in I1}:
x[i+1,0] = sum{j in K}((prod{k in K:k<>j}((1-tau[k])/(tau[j]-tau[k])))*x[i,j]);
//变量初值约束
s.t. EQ_init_x:
x[1,0] = 1.03;
//变量终值约束
s.t. EQ_terminal_x:
x[Nfe,K_radau] = 5.31;
//变量流形约束
s.t. Bonds_v {i in I,j in K}:
-v_max <= v[i,j] <= v_max;

5.AMPL与Matlab联合仿真

将AMPL官方API接口文件夹与AMPL.exe文件和IPOPT.exe文件一起，作为MATLAB工作的“当前文件夹”即可通过MATLAB调用AMPL文件，调用方法如下(省略部分见随书代码)：

close all;
% 添加AMPL API路径
addpath(genpath('amplapi'));
setupOnce;
% 轨迹规划任务相关参数设置
param.Nfe = 10;
% 声明一个ampl API对象
ampl_ = AMPL;
% 设置NLP问题中的参数
ampl_.eval(['param Nfe := ',num2str(param.Nfe),';']);
% 从文件中添加NLP模型非可变部分
ampl_.read('planning_fixed_part.mod');
% 初始化(注：此处只是介绍初始化赋值语句写法，实际初始化方法将在2.4节介绍)
% 以x变量为例，将其中元素设置为0.01*(1+行数+列数)
x = ampl_.getVariable('x');
for ii = 1 : param.Nfe
    for jj = 0 : param.K
        instance = x.get(ii,jj);
        instance.setValue(0.01 * (1 + ii + jj));
% 指定数学规划问题求解器为IPOPT
ampl_.setOption('solver','ipopt');
% 开始求解NLP问题
ampl_.solve;
% 在完成求解后导出优化得到的决策变量x、y等