HybridA*与RS联合,生成的路径虽然可以满足车辆的运动特性,但却并不能筛查路径是否无碰撞。

一、碰撞检测的思路

1、通过检测障碍物栅格来进行碰撞检测

这是A*中应用的方法,通过扩展时检查栅格地图的0、1标记,将障碍物栅格的代价置为inf,这样可以避免路径碰撞,HybridA*的扩展过程也可以用此方法检测,但是要做区别。

A*中物体仅表示一个点,只占据一个栅格,而车辆的车身是有面积的,在栅格地图中可能占据多个栅格,因此,碰撞检测不仅要保证路径不碰撞,也要保证车身覆盖区域不碰撞。最直接的方法,就是计算路径上每个时刻车身覆盖的栅格区域,检测覆盖的栅格有没有置1,若有,则说明碰撞。

这样一来就产生两个问题,

a、计算复杂,路径点较多时,每个路径点都要计算一遍车身区域,计算量会很大;解决办法比较粗暴,就是每隔一段距离检测一次,不要处处检测。

b、车身覆盖的栅格怎么确定?毕竟车身是可以歪斜的,如下图,车身占据的栅格并不好判断。

针对第二个问题,可以采用包围盒的办法,比如AABB包围盒,OBB包围盒,球包围盒。

AABB包围盒是轴对齐包围盒,简单来说就是就是用一个与栅格地图平行的方框,罩住对象物体。

OBB包围盒是有向包围盒,OBB比AABB更加接近于物体的真实形状,占据面积小,但是计算复杂,不太适用于动态物体。

球包围盒是用圆覆盖物体,matlab中帮助文件搜索vehicleCostmap,可以看到球包围盒的应用。

通过对比,显然AABB包围盒更容易计算,知道车身四个顶点的栅格坐标后,直接利用栅格坐标就可以覆盖出AABB包围盒,但对空间要求大,狭窄空间很容易碰撞。

2、线检测碰撞

相较于利用栅格占据的检测方法,线检测在特定场景下要更为简单,比如在设定好的泊车区域中,障碍物是固定的,泊车场景的障碍物可以简化成障碍线,通过障碍线圈出泊车区域,这时,只要判定车身的边框线与障碍线有没有交点,就可以检测出是否碰撞。

二、碰撞检测代码

碰撞检测的代码比较简单,泊车算法中应用的是线检测的方法,障碍线在entrypoint文件中已经定义,核心思路就是按照生成的路径,每隔一段距离就计算一次车身的边框,并检测四条边框线与障碍线有无交点。

需要注意的地方为:

1、代码中为了计算车辆边框,需要知道车辆四个顶点的位置,这四个顶点是先在车辆坐标系下计算的,然后坐标变换至世界坐标系;

2、检测车身边框与障碍线是否相交,用到了polyxpoly函数。

matlab代码如下:

function isCollision = VehicleCollisionCheck(pVec,ObstLine,Vehicle)
    //车位坐标系下计算车辆四个顶点的位置
    W = Vehicle.W;
    LF = Vehicle.LF;
    LB = Vehicle.LB;
    Cornerfl = [LF, W/2];
    Cornerfr = [LF, -W/2];
    Cornerrl = [-LB, W/2];
    Cornerrr = [-LB, -W/2];
    //车辆坐标系转换至世界坐标系
    Pos = pVec(1:2);
    theta = pVec(3);
    dcm = angle2dcm(-theta, 0, 0); // 方向余弦矩阵,负号是 把坐标转换为基坐标
    //FL点转换
    tvec = dcm*[Cornerfl';0];  // 旋转变换
    tvec = tvec';
    Cornerfl = tvec(1:2)+Pos;  // 平移变换
     //FR点转换
    tvec = dcm*[Cornerfr';0];
    tvec = tvec';
    Cornerfr = tvec(1:2)+Pos;
     //RL点转换
    tvec = dcm*[Cornerrl';0];
    tvec = tvec';
    Cornerrl = tvec(1:2)+Pos;
     //RR点转换
    tvec = dcm*[Cornerrr';0];
    tvec = tvec';
    Cornerrr = tvec(1:2)+Pos;   
     // 记录构成车辆模型的四条直线的起止坐标   
    Rect = [];                             //  _ _ _ _ _
    Rect(end+1,:) = [Cornerfl, Cornerfr];  // |    ^    |
    Rect(end+1,:) = [Cornerfr, Cornerrr];  // |    ^    |
    Rect(end+1,:) = [Cornerrr, Cornerrl];  // |    ^    |
    Rect(end+1,:) = [Cornerrl, Cornerfl];  // | _ _^ _ _|
    obs_self_define=[-10, 20; 10, 20; 10, 3.5; 3, 3.5; 3, 0; -3, 0; -3, 3.5; -10, 3.5; -10, 20];  // 手动根据地图修改障碍物线段,地图变换时需要修改此数据
    isCollision = false;
     //检测车身四条边框线与每条障碍线是否存在交点,存在则碰撞,不存在则不碰撞
    for i = 1:length(ObstLine)
        [xi,yi] = polyxpoly([Rect(:,1);Rect(1,1)],[Rect(:,2);Rect(1,2)],obs_self_define(:,1),obs_self_define(:,2));  // 检测车身是否与边界相交
        if isempty(xi)==0 
            isCollision = true;
        end
        if isCollision == true
            return
        end
    end
end

三、HybridA*代码部分

清楚A*、RS曲线、碰撞检测、以及HybridA*的思路后,HybridA*剩下的问题就是具体实现,这里还涉及到一个车辆动力学模型,泊车算法中用到的车辆动力学模型很简单,参考以下模型,典型应用就是tan(θ)= L / R;

HybridA*部分涉及内部函数较多,整体思想与A*一致,函数调用关系及HybridA*代码大致思路如下:

至此,自动泊车的路径规划就完成了,剩下的就是生成动画部分,这部分并不重要,Astar及HybridA*的全部matlab代码在以下连接中,与文章中提供的是一致的。

链接:https://pan.baidu.com/s/1LxUl4YdCvNMcwDLSMU_whQ 
提取码:g9n8 

其中HybridA*算法的matlab代码详细注释如下:

function [x,y,th,D,delta] = HybridAStar(Start,End,Vehicle,Configure)  
    veh = Vehicle;
    cfg = Configure;
    mres = cfg.MOTION_RESOLUTION; // motino resolution 
    
    // 把起始的位姿(x,y,theta)转换为grid上的栅格索引
    [isok,xidx,yidx,thidx] = CalcIdx(Start(1),Start(2),Start(3),cfg);
    if isok // 把位姿栅格定义为一个结点,形成链表结构
        tnode = Node(xidx,yidx,thidx,0,0,Start(1),Start(2),Start(3),[xidx,yidx,thidx],0);
    end
    Open = [tnode]; // hybrid A*的Open集合
    Close = [];
    x = [];
    y = [];
    th = [];
    D = [];
    delta = [];
    while ~isempty(Open)
        // pop the least cost node from open to close
        [wknode,Open] = PopNode(Open,cfg);
        [isok,idx] = inNodes(wknode,Close); // 判断是否在Close集合内
        
       
        if isok
            Close(idx) = wknode;//父节点在close中
        else
            Close = [Close, wknode];//加入close中
        end

        // 以wknode为根节点生成搜索树,使用Reeds-Shepp方法基于车辆单轨模型进行运动学解析拓展子结点
        [isok,path] = AnalysticExpantion([wknode.x,wknode.y,wknode.theta],End,veh,cfg);
        if  isok
            //把wknode从idx移到Close集合最后面
            Close(end+1) = wknode;
//             Close(idx) = [];//此句错误,删除了起始点。
            [x,y,th,D,delta] = getFinalPath(path,Close,veh,cfg);
            break // 如果能直接得到RS曲线,则跳出while循环 
        end
        [Open,Close] = Update(wknode,Open,Close,veh,cfg); // 使用
    end
//     [isok,path] = AnalysticExpantion(Start,End,Vehicle,Configure);
end

function [x,y,th,D,delta] = getFinalPath(path,Close,veh,cfg)
    wknode = Close(end); // RS曲线中最后一个元素是目标点
    Close(end) = [];
    nodes = [wknode];
    // 找目标点wknode的parent,回溯,直到Close集合为空
    while ~isempty(Close)
        n = length(Close);
        parent = wknode.parent;
        // 计算从目标返回到起始点的路径点序列,放入nodes中
        for i = n:-1:1
            flag = 0; // 只有赋值,没有使用
            tnode = Close(i);
            if tnode.xidx == parent(1)...
                    && tnode.yidx == parent(2)...
                    && tnode.yawidx == parent(3)
                nodes(end+1) = tnode;
                wknode = tnode;
                flag = 1;
                break
            end
        end
        Close(i) = [];        
    end
    rmin = veh.MIN_CIRCLE;
    smax = veh.MAX_STEER;
    mres = cfg.MOTION_RESOLUTION;
    gres = cfg.XY_GRID_RESOLUTION;
    // decrease one step, caz node origin is consider in
    nlist = floor(gres*1.5/cfg.MOTION_RESOLUTION)+1; // 固定值31,和栅格的索引是选取线的交点还是选取栅格中心有关,floor是朝负无穷大四舍五入     // 每条轨迹大概是2米的长度。这里乘以1.5是确保下一个末端状态肯定在另一个栅格中,不会还在一个栅格中!在地图栅格中子结点拓展。比如对角线长度是1.4,此时还是在同一个栅格中
    x = [];
    y = [];
    th = [];
    D = [];
    delta = [];
    flag = 0;
    // 路径要么是纯RS路径,要么是由RS路径和混合A*组合一起来的路径,先处理混合A*的结点,最后处理RS路径,肯定有RS路径
    if length(nodes) >= 2
        // 不是纯的RS路径,而是由RS路径和混合A*组合一起来的路径,>=2这些节点都是混合A*搜出来的,这里生成A*的步长路径,每个步长的车辆状态
        for i = length(nodes):-1:2
            tnode = nodes(i);
            ttnode = nodes(i-1); // parent of i
            // initial point
            px = tnode.x;
            py = tnode.y;
            pth = tnode.theta;
            x = [x, px];
            y = [y, py];
            th = [th, pth];
            D = [D, ttnode.D];
            delta = [delta, ttnode.delta];
            for idx = 1:nlist
                [px,py,pth] = VehicleDynamic(px,py,pth,ttnode.D,ttnode.delta,veh.WB);
                x = [x, px];
                y = [y, py];
                th = [th, pth];
                D = [D, ttnode.D];
                delta = [delta, ttnode.delta];
            end
            // 删除点,因为最后一个步长点还要作为起点,为RS路径做准备
            if i ~= 2//等于2时,nodes中还有最后一个点,这个点就是RS的起点,由于生成罗辑是上一个点到下一个点的路径,所以此时生成的是第2个点到第一个点的路径,已经到了RS起点位置,因此不再删除最后一个点
                x(end) = [];
                y(end) = [];
                th(end) = [];
                D(end) = [];
                delta(end) = [];
            end
        end
    else
        // 最后一个结点是纯的RS路径终点
        flag = 1;
        tnode = nodes(1);
        px = tnode.x;
        py = tnode.y;
        pth = tnode.theta;
        x = [x, px];
        y = [y, py];
        th = [th, pth];
    end
    // 此时已经搜完了全部路径,最后肯定有一段是RS路径
    types = path.type;
    t = rmin*path.t;
    u = rmin*path.u;
    v = rmin*path.v;
    w = rmin*path.w;
    segs = [t,u,v,w,rmin*path.x;];// avoid duplicate of x
    for i = 1:5
        if segs(i) == 0
            continue
        end
        s = sign(segs(i)); // 前进或后退
        
        if types(i) == 'S'          
            tdelta = 0;
        elseif types(i) == 'L'
            tdelta = smax;
        elseif types(i) == 'R'
            tdelta = -smax;
        else
            // do nothing
        end
        if flag == 1
            // initialization if only rs path
            D = [D, s*mres];
            delta = [delta, tdelta];
            flag = 0;
        end
        // 根据RS曲线路径的输入,基于运动学公式计算RS曲线上每个路径点的状态x,y,th
        for idx = 1:round(abs(segs(i))/mres) // 四舍五入为最近的小数或整数
           	[px,py,pth] = VehicleDynamic(px,py,pth,s*mres,tdelta,veh.WB); // s*mres中s代表前进和后退
            x = [x, px];
            y = [y, py];
            th = [th, pth];
            D = [D, s*mres];
            delta = [delta, tdelta];         
        end
    end
end

function [Open,Close] = Update(wknode,Open,Close,veh,cfg)
    mres = cfg.MOTION_RESOLUTION; // motino resolution    
    smax = veh.MAX_STEER; // 0.6[rad],maximum steering angle
    sres = smax/cfg.N_STEER; // 20,steering resolution  
    // all possible control input,
    for D = [-mres,mres] // D是0.1m,正负代表前进或后退,车辆当前位置的后轴中心与下一个位置的后轴中心之间的直线距离,有2个子结点
        for delta = [-smax:sres:-sres,0,sres:sres:smax] // delta是转向角,分辨率是0.03[rad],[-0.6,0.6],有21个子结点(包含0[rads])
            [isok,tnode] = CalcNextNode(wknode,D,delta,veh,cfg); // 计算wknode的所有子结点,一共2*21=42个,此函数是根据固定的D和delta计算wknode沿着一条路径的所有子结点,tnode是此条路径的末端点
            if isok == false // 子结点不可行
                continue
            end
            [isok,~] = inNodes(tnode,Close);// 在Close集合中
            if isok
                continue
            end 
            // 拓展的节点如果在Open中比较f值;若不在则添加到Open中
            [isok,idx] = inNodes(tnode,Open);
            if isok
                // 与之前的cost比较,进行更新
                tcost = TotalCost(tnode,cfg);
                ttnode = Open(idx);
                ttcost = TotalCost(ttnode,cfg);
                if tcost < ttcost
                    Open(idx) = tnode;
                end
            else
                Open(end+1) = tnode;
            end           
        end
    end  
end


function [isok,idx] = inNodes(node,nodes)
    for i = 1:length(nodes)
        tnode = nodes(i);
        if node.xidx == tnode.xidx...
                && node.yidx == tnode.yidx...
                && node.yawidx == tnode.yawidx
            idx = i;
            isok = true;
            return
        end
    end
    idx = 1;
    isok = false;
end

// 根据D和delta计算wknode沿着一条路径的所有子结点
function [isok,tnode] = CalcNextNode(wknode,D,delta,veh,cfg)
    px = wknode.x;
    py = wknode.y;
    pth = wknode.theta;
    gres = cfg.XY_GRID_RESOLUTION;
    obstline = cfg.ObstLine;
    // 每条轨迹大概是2米的长度。这里乘以1.5是确保下一个末端状态肯定在另一个栅格中,不会还在一个栅格中!在地图栅格中子结点拓展。比如对角线长度是1.4,此时还是在同一个栅格中
    nlist = floor(gres*1.5/cfg.MOTION_RESOLUTION)+1; //此处是定值31, 计算给定D和delta下,沿着一条路径上wknode的子结点的数目,以便填充数据     // 每条轨迹大概是2米的长度。这里乘以1.5是确保下一个末端状态肯定在另一个栅格中,不会还在一个栅格中!在地图栅格中子结点拓展。比如对角线长度是1.4,此时还是在同一个栅格中
    x = zeros(1,nlist+1);
    y = x;
    th = x;
    x(1) = px;
    y(1) = py;
    th(1) = pth;
    for idx = 1:nlist // 根据当前的状态和给定的控制,计算此条路径上的连着的车辆状态,根据上一时刻计算下一时刻
        [px,py,pth] = VehicleDynamic(px,py,pth,D,delta,veh.WB);
        x(idx+1) = px; // x,y,th储存了数据,但是没用到变量
        y(idx+1) = py;
        th(idx+1) = pth;
        if rem(idx,5) == 0 // 每隔5个点进行一次碰撞检测
            tvec = [px,py,pth];
            isCollision = VehicleCollisionCheck(tvec,obstline,veh);
            if isCollision
                break
            end
        end
    end
    tnode = wknode;
    if isCollision
        isok = false;
        return
    else
//         plot(x,y);
        [isok,xidx,yidx,thidx] = CalcIdx(px,py,pth,cfg); // 把路径末端点的实际坐标转换为栅格坐标
        if isok == false
            return
        else
            cost = wknode.cost;
            if D > 0 // 前进
                cost = cost + gres*1.5; // 每条轨迹大概是2米的长度。这里乘以1.5是确保下一个末端状态肯定在另一个栅格中,不会还在一个栅格中!在地图栅格中子结点拓展。比如对角线长度是1.4,此时还是在同一个栅格中
            else // 后退
                cost = cost + cfg.BACK_COST*gres*1.5;
            end
            if D ~= wknode.D
                cost = cost + cfg.SB_COST;
            end
            cost = cost + cfg.STEER_COST*abs(delta);
            cost = cost + cfg.STEER_CHANGE_COST*abs(delta-wknode.delta);
            tnode = Node(xidx,yidx,thidx,D,delta,px,py,pth,...
                [wknode.xidx,wknode.yidx,wknode.yawidx],cost); // tnode是路径的末端点,cost为到当前状态到此路径末端状态的成本
        end         
    end
end

function [wknode,nodes] = PopNode(nodes,cfg)
//用于筛选出总代价最小的父节点
    mincost = inf;
//     minidx = 1;
    gres = cfg.XY_GRID_RESOLUTION;
    for idx = 1:length(nodes)
        tnode = nodes(idx);
        // x in the col y in row
        tcost = TotalCost(tnode,cfg);
        if tcost < mincost
            mincost = tcost;
            minidx = idx;
        end
    end
    wknode = nodes(minidx);
    nodes(minidx) = [];
end

function cost = TotalCost(wknode,cfg)
    gres = cfg.XY_GRID_RESOLUTION;
    costmap = cfg.ObstMap;
    // 从栅格中心到目标
    cost = cfg.H_COST*costmap(wknode.yidx,wknode.xidx); // 无障碍碰撞地图上的成本,用A*搜出来的,二维地图,没用航向
    // 从当前结点到栅格中心
    xshift = wknode.x - (gres*(wknode.xidx-0.5)+cfg.MINX); // 栅格的index是线的交点,而不是栅格的中心,在求坐标时所以会有减0.5
    yshift = wknode.y - (gres*(wknode.yidx-0.5)+cfg.MINY);
    cost = cost+cfg.H_COST*norm([xshift,yshift]);
    // f = g + h
    cost = wknode.cost + cost;
end

function [isok,path] = AnalysticExpantion(Start,End,Vehicle,Configure)
    isok = true;
    isCollision = false;
    
    // 将起点转换到原点计算轨迹,变换坐标系了
    pvec = End-Start;
    x = pvec(1);
    y = pvec(2);
    phi = Start(3);
//     phi = mod2pi(phi);
    dcm = angle2dcm(phi, 0, 0); // 起点start坐标系在基坐标系下的方向余弦矩阵
    // dcm*x 表示将基坐标中的x表示到旋转后的坐标系中,即计算坐标旋转后各向量在新坐标中的表示
    tvec = dcm * [x; y ; 0]; // 计算坐标旋转后各向量在起点start坐标中的表示
    x = tvec(1);
    y = tvec(2);
    veh = Vehicle;
    cfg = Configure;
    rmin = veh.MIN_CIRCLE;
    smax = veh.MAX_STEER;
    mres = cfg.MOTION_RESOLUTION;
    obstline = cfg.ObstLine;
    
    // 看是否从当前点到目标点存在无碰撞的Reeds-Shepp轨迹,前面pvec=End-Start;的意义就在这里,注意!这里的x,y,prev(3)是把起点转换成以start为原点坐标系下的坐标
    path = FindRSPath(x,y,pvec(3),veh);

    // 以下是根据路径点和车辆运动学模型计算位置,检测是否会产生碰撞,返回isok的值。对每段路径从起点到终点按顺序进行处理,这一个线段的终点pvec是下一个线段的起点px,py,pth,  
    types = path.type;
    t = rmin*path.t;
    u = rmin*path.u;
    v = rmin*path.v;
    w = rmin*path.w;
    x = rmin*path.x;
    segs = [t,u,v,w,x];
    pvec = Start;
    for i = 1:5
        if segs(i) ==0
            continue
        end
        px =pvec(1);
        py = pvec(2);
        pth = pvec(3);
        s = sign(segs(i)); // 符号函数,判断此段运动方向是前进还是后退
        
        // 根据车辆的2*3种运动类型(前后2种,转向3种),设置D和delta
        D = s*mres; // 分辨率的正负
        if types(i) == 'S'
            delta = 0;
        elseif types(i) == 'L'
            delta = smax;
        elseif types(i) == 'R'
            delta = -smax;
        else
            // do nothing
        end
        
        // 把此段的路径离散成为路点,即栅格索引,然后为路点,然后检测是否存在障碍物碰撞问题
        for idx = 1:round(abs(segs(i))/mres) // round()四舍五入
            // D和delta是固定,说明转弯的时候是按固定半径的圆转弯
           	[px,py,pth] = VehicleDynamic(px,py,pth,D,delta,veh.WB);
            if rem(idx,5) == 0 // rem(a,b),返回用 a/b后的余数,每5个点,即0.5m检查下是否碰撞
                tvec = [px,py,pth];
                isCollision = VehicleCollisionCheck(tvec,obstline,veh);
                if isCollision
                    break
                end
            end
         end
        if isCollision
            isok = false;
            break // 如果路径存在碰撞则舍弃此条Reeds-Shepp路径
        end
        pvec = [px,py,pth];
    end
    // 终点位姿小于期望阈值也舍弃
    if (mod2pi(pth) - End(3)) > deg2rad(5)
        isok = false;
    end
end

// 根据当前位姿和输入,计算下一位置的位姿
function [x,y,theta] = VehicleDynamic(x,y,theta,D,delta,L)
    x = x+D*cos(theta); // 运动学公式: x_dot = v_x * cos(theta); x_dot * t = v_x * t * cos(theta),在采样时间t内,则有x = x + v_x * t * cos(theta),其中v_x * t=D
    y = y+D*sin(theta); // 运动学公式
    theta = theta+D/L*tan(delta); // L是轴距,航向变化,theta_dot=v/R,R=L/tan(delta)
    theta = mod2pi(theta);
end

// 把位姿(x,y,theta)转换为grid上的栅格索引,如果不符合实际则isok=false
function [isok,xidx,yidx,thidx] = CalcIdx(x,y,theta,cfg)
    gres = cfg.XY_GRID_RESOLUTION;
    yawres = cfg.YAW_GRID_RESOLUTION;
    xidx = ceil((x-cfg.MINX)/gres);
    yidx = ceil((y-cfg.MINY)/gres);
    theta = mod2pi(theta); // 控制theta范围在[-pi,pi]区间
    thidx = ceil((theta-cfg.MINYAW)/yawres);
    isok = true;
    //若点超出地图边界,则返回false
    if xidx <=0 || xidx > ceil((cfg.MAXX-cfg.MINX)/gres)
        isok = false;
        return
    elseif yidx <=0 || yidx > ceil((cfg.MAXY-cfg.MINY)/gres)
        isok = false;
        return
    end
    costmap = cfg.ObstMap;
    if costmap(yidx,xidx) == inf
        isok = false;
    end
end

// 把弧度x控制在[-pi,pi]
function v = mod2pi(x)
    v = rem(x,2*pi); // 求整除x/2pi的余数
    if v < -pi
        v = v+2*pi;
    elseif v > pi
        v = v-2*pi;
    end
end