野外移动机器人的导航-导航算法调研

RRT*-一种基于采样的路径规划算法

1，probalilistic road map(概率路图)

probalilistic road map简称为PRM,是用采样点的方式将空间结构进行简化的一种方式，在这个基础上采用A*等完备的算法对路径进行查询。整个过程分为learning phase,query phase两个步骤。
问题：分前后两个阶段，不够efficient。

2，rapidly-exploring random tree

一边撒点，一边对路径进行进行查询。
缺点：路径不够平滑，路径不是最优

3，rapidly-exploring random tree*

相比于RRT，RRT算法新增了两个环节，choose parent 和rewire，这使得RRT具有渐进最优的能力。
3，kinodynamic-RRT*
考虑机器人动力学约束，通过改进steer函数，增强轨迹的平滑性。

4，informed-RRT*

RRT在已经得到一条可行路径之后，可以将采样空间收缩到一个椭圆形区域中，区域之外的点对于缩短规划出的路径长度并没有实际价值。
informed RRT就是的主要思想就是上面这种思想，在获取可行路径之后，将采样空间限制在一个椭圆形区域中，并且随着路径长度的不断缩短，逐渐缩小该椭圆形区域。

实验

RRT 基本算法在matlab中的实现
RRT 沿着物体在matlab中的实现
RRT* 算法基于OMPL库在ROS中的实现
建立坡度斜面，并对其进行无约束的RRT*规划

可能会出现这样的问题

对于规划的路径，建立贴近地面的约束，这样约束变为两个：碰撞检测+是否贴近地面

//判断是否可行
bool RRTstarPreparatory::isFeasible(const double coord_x, const double coord_y, const double coord_z)
{
    bool is_obs_free;
    bool is_close2ground;

    is_obs_free = isObsFree(coord_x,coord_y,coord_z);
    is_close2ground  = isCloseGround(coord_x,coord_y,coord_z);

    return (is_obs_free && is_close2ground);
}

其中

//判断是否靠近地面
bool RRTstarPreparatory::isCloseGround(const double coord_x, const double coord_y, const double coord_z)
{
    Vector3d pt;
    Vector3i idx;
    bool is_close2ground=false;
    bool index_check;
    int size_check = 1;
    pt(0) = coord_x;
    pt(1) = coord_y;
    pt(2) = coord_z;
    idx = coord2gridIndex(pt);
    int idx_x = idx(0);
    int idx_y = idx(1);
    int idx_z = idx(2);
    for(int i=-size_check;i<=size_check;i++){
        for(int j=-size_check;j<=size_check;j++){
            for(int k=-size_check;k<=size_check;k++){
                idx_x = idx(0)+i;
                idx_y = idx(1)+j;
                idx_z = idx(2)+k;
                if((idx_x >= 0 && idx_x < GLX_SIZE && idx_y >= 0 && idx_y < GLY_SIZE && idx_z >= 0 && idx_z < GLZ_SIZE ))
                {
                    idx(0) = idx_x;
                    idx(1) = idx_y;
                    idx(2) = idx_z;
                    pt = gridIndex2coord(idx);
                    if((isObsFree(pt(0),pt(1),pt(2))) == false){
                        is_close2ground = true;
                    }
                }
                //cout<< idx_x <<"  "<<idx_y<<"  "<<idx_z<<"  "<<is_close2ground<<endl;
            }
        }
    }
    return is_close2ground;
}

下一步：
1，解决目标点点找不到的问题
2，研究地图中坡度的数学表达式，来建立静力学约束

参考：
matlab实现3d-rrt
UAV源码
参考代码
3D下使用OMPL进行规划