【MATLAB Navigation Toolbox之轨迹优化】

本篇文章基于MATLAB导航工具箱的官方文档，详情参考Create optimization options for optimizePath function - MATLAB - MathWorks 中国

目录

1 optimizePathOptions

1.1 轨迹参数

1.2 障碍物参数

1.3 求解器参数

1.4 权重

2 举例

3 optimizePath

---

1 optimizePathOptions

这是为优化路径函数创建优化选项

options = optimizePathOptions

上面的代码是创建默认优化选项，下面将详细介绍具体的参数调整

1.1 轨迹参数

很多参数有最大值限制，因为受到实际车辆或机器人运动学的限制

MaxPathStates：路径中允许的最大姿势数，默认是200

ReferenceDeltaTime：两个连续姿势之间的行程时间，默认0.3

MinTurningRadius：最小转弯半径，默认是1

MaxVelocity：最大速度，默认是0.4

MaxAngularVelocity：最大角速度，默认是0.3

MaxAcceleration：最大加速度，默认是0.5

MaxAngularAcceleration：最大角加速度，默认是05

1.2 障碍物参数

ObstacleSafetyMargin：与障碍物的安全距离，这是一个软约束，默认是0.5，计算代价时用到的，离0.5越远且越小，代价越大

ObstacleCutoffDistance：障碍物截断距离，超过此距离的障碍物视为冗余避障约束，忽略这些障碍物，默认是2.5

ObstacleInclusionDistance：障碍物包含距离，一般来说路径优化器考虑最近的障碍物，但在障碍物包含距离内的障碍物超过一个时会考虑包含距离内的所有障碍物，默认为0.75

1.3 求解器参数

NumIteration：求解器调用次数，默认是4

MaxSolverIteration：每个求解器调用的最大次数，默认是15

1.4 权重

WeightTime：时间的成本函数权重。要缩短行程时间，请增大，默认为10

WeightSmoothness：非完整约束运动的成本函数权重，要获得更平滑的路径，请增大，默认是1000

WeightMinturningRadius:要确保转弯半径高于最小转弯半径，请增大,默认是10

WeightObstacles:用于保持与障碍物的安全距离的成本函数权重，要保持与障碍物的安全距离，请增大，默认是50

WeightVelocity、WeightAngularVelocity、WeightAcceleration、WeightAngularAcceleration：都类似，保持在最大值一下，默认值为100、10、10、10

2 举例

map = load("exampleMaps.mat").complexMap;
map = binaryOccupancyMap(map);
stateValidator = validatorOccupancyMap;
stateValidator.Map = map;
stateValidator.ValidationDistance = 0.01;
planner = plannerHybridAStar(stateValidator,MinTurningRadius=2);
start = [6 3 pi/2];
goal = [32 32 0];
path = plan(planner,start,goal);
inpath = path.States;
options = optimizePathOptions
options.MinTurningRadius = 2;
options.MaxPathStates = size(inpath,1) * 3;
options.ObstacleSafetyMargin = 0.75;
optpath = optimizePath(inpath,map,options);
show(map)
hold on
quiver(inpath(:,1),inpath(:,2),cos(inpath(:,3)),sin(inpath(:,3)),0.1);
quiver(optpath(:,1),optpath(:,2),cos(optpath(:,3)),sin(optpath(:,3)),0.1);
legend("Input Path","Optimized Path")

3 optimizePath

optPath = optimizePath(path)
optPath = optimizePath(path,map)
optPath = optimizePath(path,options)
optPath = optimizePath(path,map,options)
[optPath,kineticInfo] = optimizePath(___)
[optPath,kineticInfo,solutionInfo] = optimizePath(___)

path：以矩阵的形式返回路径中的状态

map：地图

options：指定为optimizePathOptions

optpath：返回优化后的路径中的状态,以矩阵的形式返回

kineticInfo：动力学信息，返回一个结构体

TimeStamps	与优化路径中的姿势相对应的时间
Velocity	在两个连续姿势之间行进的速度
AngularVelocity	两个连续姿势之间的角速度

solutionInfo：解决方案，返回一个结构体

NumIterations	执行的迭代次数
Cost	路径的最终成本