2019-06-08　关于路径规划SBPL

因为项目的原因又去了解了一下路径规划算法，包括Ａ和RRT。
由于项目中使用的全局SBPL算法是基于Ａ的变种，小白稍微了解了一下Ａ*算法，简单来说是一种启发式搜索算法，通过计算损失函数找到最优解，具体如下：https://blog.csdn.net/zhulichen/article/details/78786493。
SBPL中还需要获取机器人的基础运动元，以及相应的代价。通过SBPL给出的Matlab可生成.mprim文件，具体如下：

    resolution = 0.01;％像素
    numberofangles = 8; %360度中离散化成８个方向的运动
    numberofprimsperangle = 16;％每个方向中有１6种运动基础方式

    %不同运动方式的损失函数
    forwardcostmult = 1;
    backwardcostmult = 5;
    forwardandturncostmult = 1;
    sidestepcostmult = 50;
    turninplacecostmult = 50;

　 % 以０度为例
    basemprimendpts0_c(1,:) = [1 0 0 forwardcostmult];
    basemprimendpts0_c(2,:) = [4 0 0 forwardcostmult];
    basemprimendpts0_c(3,:) = [8 0 0 forwardcostmult];
    basemprimendpts0_c(4,:) = [6 2 0 sidestepcostmult];
    basemprimendpts0_c(5,:) = [6 -2 0 sidestepcostmult];
    basemprimendpts0_c(6,:) = [2 3 0 sidestepcostmult];
    basemprimendpts0_c(7,:) = [2 -3 0 sidestepcostmult];
    basemprimendpts0_c(8,:) = [-5 0 0 backwardcostmult];
　　basemprimendpts0_c(9,:) = [6 2 1 forwardandturncostmult];
    basemprimendpts0_c(10,:) = [6 -2 -1 forwardandturncostmult];
    %2/32 theta change
    basemprimendpts0_c(11,:) = [4 3 2 forwardandturncostmult];
    basemprimendpts0_c(12,:) = [4 -3 -2 forwardandturncostmult];
    %turn in place
    basemprimendpts0_c(13,:) = [0 0 1 turninplacecostmult];
    basemprimendpts0_c(14,:) = [0 0 -1 turninplacecostmult];
    basemprimendpts0_c(15,:) = [0 0 3 turninplacecostmult];
    basemprimendpts0_c(16,:) = [0 0 -3 turninplacecostmult]; 

其中括号里表示x,y,theta,cost。生成８*16=128中运动方式，考虑到整体的可行性，每个运动元的终点在网格上。(对数据进行四舍五入)有了运动元就容易全局规划了。