【MATLAB Navigation Toolbox之状态空间】

本篇文章基于MATLAB导航工具箱的官方文档，详情参考Motion Planning - MATLAB & Simulink - MathWorks 中国

目录

1 状态空间介绍

2 SE2状态空间

3 SE3 状态空间

4 stateSpaceDubins 状态空间

5 stateSpaceReedsShepp 状态空间

6 举例

---

1 状态空间介绍

描述自动驾驶车辆在某一时刻和某一特定地点的状态的一组属性值被称为该时刻车辆的“状态”，车辆的路径则是由状态空间中的一些点组成的。下面介绍MATLAB导航工具箱中内置的状态空间。详情请参考MATLAB官方文档

2 SE2状态空间

这个状态空间一般用于移动机器人二维运动

space = stateSpaceSE2
space = stateSpaceSE2(bounds)

（1）Name：状态空间的名称，默认是“SE2”

（2）StateBounds：代表 的范围，默认是[-100 100; -100 100; -3.1416 3.1416]

（3）WeightXY、WeightTheta：计算两个状态之间的distance的时候的权重，公式如下

对象函数：

enforceStateBounds：将指定的状态减少到指定状态空间对象的属性中的状态边界

boundedStates = enforceStateBounds(space,states)

interpolate：根据指定的插值比率在指定的开始状态和结束状态之间插值状态，ratio代表两个状态之间的位置，0代表state1，1代表state2

interpStates = interpolate(space,state1,state2,ratio)

interpStates以矩阵的形式3*n或7*n返回包括起点终点以及插值点的状态

可以插值一个点或多个点，如下

states = interpolate(space,state1,state2,0.5)
states = interpolate(space,state1,state2,0:0.02:1)

sampleGaussian：基于具有指定均值和标准差的高斯（正态）分布返回状态空间的样本状态

state = sampleGaussian(space,meanState,stdDev)
state = sampleGaussian(space,meanState,stdDev,numSamples)

 显然meanState为均值，stdDev为标准差，numSamples为样本数量

sampleUniform：均匀分布，与上面的高斯分布同理

3 SE3 状态空间

这个状态空间一般用于机械臂三维运动或无人机飞行的状态空间

大部分属性与对象函数与SE2一样，这里不再赘述。下面讲讲不同之处

状态空间的维数达到七维。空间由 [x， y， z， qw， qx， qy， qz] 表示的状态向量组成。 x、y 和 z 是笛卡尔坐标。qw、qx、qy 和 qz 表示四元数中的方向。distance的计算也不同

4 stateSpaceDubins 状态空间

dubins曲线与reeds-shepp曲线不熟悉的读者请先阅读​​​​​​【自动驾驶轨迹规划之dubins曲线与reeds-shepp曲线】_无意2121的博客-CSDN博客_杜宾斯路径规划

stateSpaceDubins适合用于智能车辆不倒退的情况，一般在结构化道路上

space = stateSpaceDubins
space = stateSpaceDubins(bounds)

大部分的属性与对象函数也是和SE2一样，但是有一个比较重要的不同点是，stateSpaceDubins由最小转弯半径的曲线连接，因为车辆由于自身结构限制，存在转弯最小半径，而两个带航向角约束的状态之间的最短路径是必须由最小转弯半径的圆弧连接。

MinTurningRadius：最小转弯半径默认为1

5 stateSpaceReedsShepp 状态空间

stateSpaceReedsShepp适合于智能车辆可以前进倒退的情况，一般在非结构化道路上

与stateSpaceDubins相比，多了对前进运动或反向运动的惩罚

ForwardCost：默认是1

ReverseCost：默认是1

6 举例

ss = stateSpaceReedsShepp;
sv = validatorOccupancyMap(ss);
load exampleMaps
map = occupancyMap(simpleMap,10);
sv.Map = map;
sv.ValidationDistance = 0.01;
ss.StateBounds = [map.XWorldLimits;map.YWorldLimits; [-pi pi]];
planner = plannerRRT(ss,sv);
planner.MaxConnectionDistance = 0.3;
start = [0.5,0.5,0];
goal = [2.5,0.2,0];
rng(100,'twister'); % repeatable result
[pthObj,solnInfo] = planner.plan(start,goal);
show(map); 
hold on;
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'.-'); % tree expansion
plot(pthObj.States(:,1), pthObj.States(:,2),'r-','LineWidth',2) % draw path