目录
前言
无人驾驶分为环境感知、轨迹规划、运动控制三大部分，随着无人驾驶领域的不断发展，轨迹规划这门技术也越来越重要，在这里分享通过matlab的GPOPSII工具箱进行车辆轨迹规划的学习心得。
提示：以下是本篇文章正文内容，下面案例可供参考
一、GPOPSII是什么？
近年来发展出的求解轨迹优化问题的方法主要分为两类：
①间接法：即借助变分法或者最大值原理，把泛函优化问题转化为两点边值问题求解
②直接法：通过对控制变量或状态变量进行离散，把泛函优化转化为非线性规划问题，然后采用各种非线性规划算法求解。其中伪谱法由于具有快速收敛性和较高优化精度，近年来成为轨迹优化领域常用的数值优化方法。
高斯伪谱法(GPOPS)就是一种直接求解轨迹优化问题的方法，通过伪谱法将问题离散为非线性规划问题后，通过'snopt'或者‘ipopt’等非线性问题求解器进行优化，得到符合目标函数要求的解。
MATLAB中GPOPSII的安装可以参考以下两篇文章：
二、一个基于车辆单轨运动学模型的过弯轨迹规划例子
1.问题介绍
本文先基于一个最简单的车辆模型进行过弯轨迹规划。
该模型为以后轮作为坐标原点的阿克曼车辆纯运动学模型。
该模型数学建模如下：本文为了简便，控制量直接选择速度，但此时应该还要有加速度状态量约束，用于对速度变化率做限制，此处为了简便不做限制。同时该模型仅为运动学模型，即假设车辆无侧滑，与实际车辆模型仍有很大误差，本文先采用该简单模型，对GPOPSII轨迹优化能力进行初步验证。
模型具体推导过程可以参考书籍：龚建伟《无人驾驶车辆模型预测控制》
书中对车辆运动学、动力学以及轮胎模型都进行了详细的推导分析与建模。
本例子中弯道设置如下：红色曲线即为道路的内外侧边缘线。车辆从（0，0）点出发，前往
（15，15）。在GPOPS中，将这段路径分为三部分路径约束，即入弯前,弯道中和出弯后。
表示如下：
目标函数为时间最优，各状态量的边界条件如下：
2.代码分析
①边界条件初始化代码：
可以先将各阶段初值、终值用变量初始化好。
也可以不写好变量，在设置bound时直接将边界条件写入。
%% 01.初始参数设置
clc;clear;close all;
tic;
%-------------------------------------------------------------------------%
%-----------------------根据问题，进行状态量边界条件约束的求解边界 -----------%
%-------------------------------------------------------------------------%
% 设置时间
t0 = 0;
tmax = 30;
% 设置状态量初值
x11_0 =0;x12_0 =5;%x13_0=; xij_0表示状态变量xi在第j阶段的初值，若某阶段初值被注释，则说明该阶段没有初值不确定，根据优化目标自动选取，只给定范围
x11_f=5;%x12_f=;x13_f=; xij_f表示状态变量xi在第j阶段的终值
x21_0 =0.2;x23_0 =10;%x22_0 =;
x22_f =10; x23_f =15;%x21_f =;
x30 =0;x3f= pi/2;
% 设置状态量边界条件
x11_Min = 0;x12_Min = 5;x13_Min = 14;
x11_Max = 5;x12_Max = 16;x13_Max = 16;
x21_Min = -1;x22_Min = -1;x23_Min = 10;
x21_Max = 1;x22_Max = 10;x23_Max = 15;
x3Min = -pi/2;
x3Max = pi/2;
%设置road约束宽度，即与目标路径中心相距的距离限制，本文设计道路宽两米，所以限幅（-1，1）
c10 = 0;
c1Min = -1;
c1Max = 1;
% 设置控制量边界条件
u1Min = -pi*12/180; %车辆前轮转向角限幅 rad 此处选择正负12°是与本例子弯道设计有关，
%该弯道不够急，若转向角限幅过大，车辆转向能力很足，可以直接贴着道路内边缘转向，可能得不到期望的轨迹。
u1Max = pi*12/180;
u2Min = 0.001;%车辆速度限幅 m/s
u2Max = 20;%% 02.边界条件设置
%-------------------------------------------------------------------------%
%------------------------ 将求解边界设置于问题中 -------------------------%
%-------------------------------------------------------------------------%
iphase=1;
bounds.phase(iphase).initialtime.lower = t0;
bounds.phase(iphase).initialtime.upper = t0;
bounds.phase(iphase).finaltime.lower = t0;
bounds.phase(iphase).finaltime.upper = tmax;
bounds.phase(iphase).initialstate.lower = [x11_0 x21_0 x30];
bounds.phase(iphase).initialstate.upper = [x11_0 x21_0 x30];
bounds.phase(iphase).state.lower = [x11_Min x21_Min x3Min];
bounds.phase(iphase).state.upper = [x11_Max x21_Max x3Max];
bounds.phase(iphase).finalstate.lower = [x11_f x21_Min x3Min];
bounds.phase(iphase).finalstate.upper = [x11_f x21_Max x3Max];
bounds.phase(iphase).control.lower = [u1Min u2Min];
bounds.phase(iphase).control.upper = [u1Max u2Max];
bounds.phase(iphase).path.lower = c1Min;
bounds.phase(iphase).path.upper = c1Max;
guess.phase(iphase).time = [t0; tmax];
guess.phase(iphase).state = [[x11_0; x11_Max],[x21_Min; x21_Max],[x3Min; x3Max]];
guess.phase(iphase).control = [[u1Min; u1Max],[u2Min; u2Max]];
iphase=2;
bounds.phase(iphase).initialtime.lower = t0;
bounds.phase(iphase).initialtime.upper = tmax;
bounds.phase(iphase).finaltime.lower = t0;
bounds.phase(iphase).finaltime.upper = tmax;
bounds.phase(iphase).initialstate.lower = [x12_0 x22_Min x3Min];
bounds.phase(iphase).initialstate.upper = [x12_0 x22_Max x3Max];
bounds.phase(iphase).state.lower = [x12_Min x22_Min x3Min];
bounds.phase(iphase).state.upper = [x12_Max x22_Max x3Max];
bounds.phase(iphase).finalstate.lower = [x12_Min x22_f x3Min];
bounds.phase(iphase).finalstate.upper = [x12_Max x22_f x3Max];
bounds.phase(iphase).control.lower = [u1Min u2Min];
bounds.phase(iphase).control.upper = [u1Max u2Max];
bounds.phase(iphase).path.lower = c1Min;
bounds.phase(iphase).path.upper = c1Max;
guess.phase(iphase).time = [t0; tmax];
guess.phase(iphase).state = [[x12_0; x12_Max],[x21_Min; x22_f],[x3Min; x3Max]];
guess.phase(iphase).control = [[u1Min; u1Max],[u2Min; u2Max]];
iphase=3;
bounds.phase(iphase).initialtime.lower = t0;
bounds.phase(iphase).initialtime.upper = tmax;
bounds.phase(iphase).finaltime.lower = t0;
bounds.phase(iphase).finaltime.upper = tmax;
bounds.phase(iphase).initialstate.lower = [x13_Min x23_0 x3Min];
bounds.phase(iphase).initialstate.upper = [x13_Max x23_0 x3Max];
bounds.phase(iphase).state.lower = [x13_Min x23_Min x3Min];
bounds.phase(iphase).state.upper = [x13_Max x23_Max x3Max];
bounds.phase(iphase).finalstate.lower = [x13_Min x23_f x3f];
bounds.phase(iphase).finalstate.upper = [x13_Max x23_f x3f];
bounds.phase(iphase).control.lower = [u1Min u2Min];
bounds.phase(iphase).control.upper = [u1Max u2Max];
bounds.phase(iphase).path.lower = c1Min;
bounds.phase(iphase).path.upper = c1Max;
guess.phase(iphase).time = [t0; tmax];
guess.phase(iphase).state = [[x13_Min; x13_Max],[x23_0; x23_f],[x3Min; x3Max]];
guess.phase(iphase).control = [[u1Min; u1Max],[u2Min; u2Max]];
如上图所示，在设置bounds边界条件结构体时，每一段的状态量都有初始值、过程值、终值三个约束.每一个约束包含上下界，若上下界一致则为确定值。
以第一阶段为例，起始点是确定的，则状态量x,y的初值就是确定的。过程中y要在道路内，则会有上下界（-1，1），而第一段是入弯前轨迹，所以x会有上下界(0,5)。因此x的终值也确定为5，但y的终值根据轨迹优化的结果，是不确定的，因此y终值在(-1,1)之间，不固定。
在设置好边界条件后，顺便对初值进行猜测，该初值猜测选取边界条件即可。
②设置每段路径之间连接点结构体
%% 03.连接点结构体设置
%-------------------------------------------------------------------------%
%------------- Set up Event Constraints That Link Phases -----------------%
%-------------------------------------------------------------------------%
bounds.eventgroup(1).lower = [zeros(1,3),0];
bounds.eventgroup(1).upper = [zeros(1,3),0];
bounds.eventgroup(2).lower = [zeros(1,3),0];
bounds.eventgroup(2).upper = [zeros(1,3),0];
%eventgroup用与后续存储每段之间连接点的差值，
%每有n段路径，就有n-1个eventgroup。结构里的zeros对于状态量数量，若有n个状态量，写为zeros(1,n)
for i=1:3 %此处1：3对应三段，即有n段 此处i=1:n
meshphase(i).colpoints = 4*ones(1,10);
meshphase(i).fraction = 0.1*ones(1,10);
end
③设置求解器参数
%% 04.求解器参数设置
%-------------------------------------------------------------------------%
%---------------------------- 设置求解器参数 -----------------------------%
%-------------------------------------------------------------------------%
setup.name = 'Vehicle-Conering';
setup.functions.continuous = @vsopcContinuous;
setup.functions.endpoint = @vsopcEndpoint;
setup.bounds = bounds;
setup.guess = guess;
setup.nlp.solver = 'snopt';
setup.derivatives.supplier = 'sparseCD';
setup.derivatives.derivativelevel = 'second';
setup.mesh.method = 'hp1';
setup.mesh.tolerance = 1e-6;
setup.mesh.maxiteration = 10;
setup.mesh.colpointsmax = 4;
setup.mesh.colpointsmin = 10;
setup.mesh.phase = meshphase;
setup.method = 'RPMintegration';
简单介绍结构体中几个重要的参数：
nlp.solver：求解器选择，可以选择'snopt'或者'ipopt'两种求解器。
mesh.tolerance：误差限制，当得到的解满足该误差要求后停止求解。
setup.mesh.maxiteration：最大迭代次数，设置过小时，可能还没找到满足误差要求的解就停止求解，设置过大会花费过多求解时间，根据需求设置合适值即可。
④进行求解和结果图像绘制
%% 05.求解
%-------------------------------------------------------------------------%
%----------------------- 使用 GPOPS2 求解最优控制问题 --------------------%
%-------------------------------------------------------------------------%
output = gpops2(setup);
solution = output.result.solution;
toc;
%% 06.画图
%%绘制道路边缘线(红)以及实际轨迹(绿)
figure('Color',[1,1,1]);
iphase=1;
plot(solution.phase(iphase).state(:,1),solution.phase(iphase).state(:,2),'g');hold on;
x_in0=linspace(0,5,10);
y_in0=x_in0-x_in0+1;
x_in1=linspace(5,14,100);
y_in1=-sqrt(81-(x_in1-5).^2)+10;
x_in=[x_in0 x_in1];
y_in=[y_in0 y_in1];
line([14 14],[10 15],'Color','r');hold on;
plot(x_in, y_in,'r-'); hold on;
iphase=2;
plot(solution.phase(iphase).state(:,1),solution.phase(iphase).state(:,2),'g');
iphase=3;
plot(solution.phase(iphase).state(:,1),solution.phase(iphase).state(:,2),'g');
axis equal;
x_out0=linspace(0,5,10);
y_out0=x_out0-x_out0-1;
x_out1=linspace(5,16,100);
y_out1=-sqrt(121-(x_out1-5).^2)+10;
x_out=[x_out0 x_out1];
y_out=[y_out0 y_out1];
line([16 16],[10 15],'Color','r');hold on;
plot(x_out, y_out,'r-'); hold on;
xlabel('X(m)'); ylabel('Y(m)');
legend('实际轨迹','道路边缘线');
%%根据需要，绘制各状态量随时间的变化曲线
figure('Color',[1,1,1]);
plot(solution.phase(iphase).time,solution.phase(iphase).control(:,2),'g');hold on;
xlabel('t(s)'); ylabel('V(m/s)');
figure('Color',[1,1,1]);
plot(solution.phase(iphase).time,solution.phase(iphase).control(:,1),'g');hold on;
xlabel('t(s)'); ylabel('转向角(m/s)');
⑤根据模型编写状态量微分方程(重要)
%% 07函数模块部分
% ----------------------------------------------------------------------- %
% ------------------------- BEGIN: vsopcContinuous.m -------------------- %
% ----------------------------------------------------------------------- %
function phaseout = vsopcContinuous(input)
iphase=1;
%% 先获取当前状态量的值
%t = input.phase.time;
%x11 = input.phase(iphase).state(:,1);
x21 = input.phase(iphase).state(:,2);
x31 = input.phase(iphase).state(:,3);
u11 = input.phase(iphase).control(:,1);
u21 = input.phase(iphase).control(:,2);
%% 根据模型,写状态量的微分方程,获得状态量变化率
dx11 = cos(x31).*u21;
dx21 = sin(x31).*u21;
dx31 = tan(u11).*u21;
%编写道路约束
%% 将状态量变化率幅值给dynamics结构体变量，用于计算下一周期状态量的值
phaseout(1).dynamics = [dx11,dx21,dx31];
%% 编写该段路径约束，x21及第一段的y坐标值，即在第一阶段限制了y在(-1,1),该限幅大小在设置边界条件时的路径约束限幅中设置
phaseout(1).path=x21;
%%同理编写其他段微分方程即路径约束
iphase=2;
%t = input.phase.time;
x12 = input.phase(iphase).state(:,1);
x22 = input.phase(iphase).state(:,2);
x32 = input.phase(iphase).state(:,3);
u12 = input.phase(iphase).control(:,1);
u22 = input.phase(iphase).control(:,2);
dx12 = cos(x32).*u22;
dx22 = sin(x32).*u22;
dx32 = (tan(u12).*u22)/3;
%编写道路约束
phaseout(2).dynamics = [dx12,dx22,dx32];
phaseout(2).path=sqrt((x22-10).^2+(x12-5).^2)-10;
iphase=3;
%t = input.phase.time;
x13 = input.phase(iphase).state(:,1);
%x23 = input.phase(iphase).state(:,2);
x33 = input.phase(iphase).state(:,3);
u13 = input.phase(iphase).control(:,1);
u23 = input.phase(iphase).control(:,2);
dx13 = cos(x33).*u23;
dx23 = sin(x33).*u23;
dx33 = (tan(u13).*u23);
%编写道路约束
phaseout(3).dynamics = [dx13,dx23,dx33];
phaseout(3).path=x13-15;
end
% ----------------------------------------------------------------------- %
% -------------------------- END: vsopcContinuous.m --------------------- %
% ----------------------------------------------------------------------- %⑥编写目标函数以及进行段与段之间的连接
%% 08目标函数以及段之间连接
% ----------------------------------------------------------------------- %
% -------------------------- BEGIN: vsopcEndpoint.m --------------------- %
% ----------------------------------------------------------------------- %
function output = vsopcEndpoint(input)
%%获取第一段各状态量的终值，第二阶段的初值，终值，第三阶段的初值
% Variables at Start and Terminus of Phase 1
tf1 = input.phase(1).finaltime;
xf1 = input.phase(1).finalstate;
% Variables at Start and Terminus of Phase 2
t02 = input.phase(2).initialtime;
tf2 = input.phase(2).finaltime;
x02 = input.phase(2).initialstate;
xf2 = input.phase(2).finalstate;
% Variables at Start and Terminus of Phase 3
t03 = input.phase(3).initialtime;
x03 = input.phase(3).initialstate;
%%将相邻段的初值、终值进行比较，确保第一段终值和第二阶段的初值相等，从而将不同段之间连接起来
% Event Group 1: Linkage Constraints Between Phases 1 and 2
% 本例子有三个状态量，所以将三个状态量都进行对比即x02(1:3)和xf1(1:3).
output.eventgroup(1).event = [x02(1:3)-xf1(1:3), t02-tf1];
% Event Group 2: Linkage Constraints Between Phases 2 and 3
output.eventgroup(2).event = [x03(1:3)-xf2(1:3), t03-tf2];
% Event Group 3: Linkage Constraints Between Phases 3 and 4
%%目标函数即为第三阶段的结束时间，即优化目标为时间最优
J = input.phase(3).finaltime;
output.objective = J;
end
% ----------------------------------------------------------------------- %
% --------------------------- END: vsopcEndpoint.m ---------------------- %
% ----------------------------------------------------------------------- %
三、结果与分析
通过上下改变初始位置，得到如下结果
图3-1 初始位置（0，-1） 图3-2 初始位置（0，-0.5）
图3-3 初始位置（0，0） 图3-4 初始位置（0，0.5）
图3-5 初始位置（0，0.75） 图3-6 初始位置（0，1）
结果分析：
可以看到，在初始位置接近道路外边缘时，车辆有较充足的转向纵向距离，因此车辆直接向左前方轻微转向，准备进入转弯阶段。当初始位置接近道路内边缘时，转向纵向距离不足，因此车辆会先向右前方转向，留出合适的转向纵向距离之后，再准备进行转向，该结果符合预期。
并且可以观察到，规划出来的转向轨迹会和道路内边缘存在一个相切点，即赛车中专门的一个术语：‘apex’。可以看到随着初始位置靠近道路内边缘，规划出来的转向轨迹与道路内边缘切点越靠前，出弯后的位置越靠近道路外边缘，符合预期。
结论：
通过简易的纯运动学模型，并合理约束转向角度提高近似准确率，利用GPOPSII进行转弯轨迹的规划，可以初步得到车辆最优过弯轨迹的一种趋势。但在该模型中，以前轮转向角和速度作为控制量求解，实际是不准确的，因为车辆为惯性系统，速度无法突变。同时，车辆过弯时的侧滑特性是不可忽略的，因此接下来将结合车辆动力学模型，对车辆的转弯特性、横摆特性进行描述，进一步提高车辆轨迹仿真的近似准确度。
总结
以上就是本文的主要内容，本文仅仅简单通过一个运动学模型介绍了GPOPS在无人驾驶轨迹规划中的使用，之后会基于更准确的车辆动力学模型、轮胎模型并结合CARSIM进行联合仿真验证。
PS：编者大四在读，是无人车辆轨迹规划和运动控制方向的准研究生，先前在网上寻找GPOPS轨迹规划相关教程时，未找到路径约束、分段约束相关的内容。于是根据GPOPS官方文档进行学习，有了一点学习心得，留下该学习笔记，欢迎读者指出错误，一起讨论、学习进步。
367
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
