精华内容
下载资源
问答
  • 二维轨迹规划
    千次阅读
    2020-11-26 21:59:59

    博客推荐

    Q-Learning

    二维避障轨迹规划

    目前我写的代码不够成熟,就不贴了。Agent的reward机制如下:走出图像reward为一个较大负数,撞上障碍也为一个较大负数,若不是前两者,则reward为当前点位与目标点位距离的反比函数(越近奖赏越大)。
    state状态的划分是根据像素区域来的,等同于把图像分割成了若干像素块,这样可以减小state的数量,否则训练困难。Action根据移动方向的角度来分,比如10°划分的话一个state就有36个actions,这样Q表就是 s t a t e s × a c t i o n s states\times actions states×actions尺寸的矩阵。

    结果

    在这里插入图片描述
    结果基本全图收敛。

    代码

    注:这只是个学习程序,实用价值不高。

    clc;clf;clear;
    %% 加载图片二值化(方形图片)
    map=imbinarize(imread('test2.bmp')); % input map read from a bmp file. for new maps write the file name here
    map = map(:,:,1);        %三通道取前两个通道
    %% 参数初始化
    statePixSize = 100;   %像素方格视为一个state
    stateNumRow = size(map,1)/statePixSize; 
    stateNum = stateNumRow^2;
    source = [192,263]; %初始点
    goal = [757,779]; %目标点
    iter1 = 100;   %学习迭代次数 
    iter2 = 3000;   %每次迭代内循环次数
    state0 = calculateState(source(1),source(2),stateNumRow,statePixSize); %起始点的状态
    stateGoal = calculateState(goal(1),goal(2),stateNumRow,statePixSize); %goal的状态
    angle = 10;  %一个action度数
    actionNum = 360/angle;
    Q = zeros(stateNum,actionNum);
    e = 0.3;    %解决探索和开发
    gamma = 0.9; alpha = 0.5;
    stepSize = 100;
    threshold = 200;
    
    
    %% 学习循环
    for i = 1:iter1
    %     randXY = randperm(size(map,1),2);
    %     x = randXY(1); y = randXY(2);
    %     state = calculateState(x,y,stateNumRow,statePixSize);
        state = state0;
        x = source(1); y = source(2);
        for j = 1:iter2
            % 当前state下做出action决策
            randNum = rand;
            if(rand>e)
                [~,action] = max(Q(state,:));
            else
                action = randperm(actionNum,1);
            end
            % 判断在该决策下到达的下一个state
            xNext = x + stepSize * sin((action-1)*angle/180*pi);
            yNext = y - stepSize * cos((action-1)*angle/180*pi);
            if(xNext<=0 || xNext >= size(map,1) || yNext<=0 || yNext >=size(map,2))
                Q(state,action) = -10000;
                continue;
            end
            newState = calculateState(xNext,yNext,stateNumRow,statePixSize);
            % 判断碰撞与否以及是否到达goal 并observe reward
            if checkPath([x,y],[xNext,yNext],map)
                reward = 2000/(1+distanceCost([xNext,yNext],goal));
                if newState == stateGoal
                    reward = 1000;
                end
                Q(state,action) = Q(state,action) + alpha * (reward + gamma * max(Q(newState,:)) - Q(state,action));
            else
                reward = -5000;
                Q(state,action) = Q(state,action) + alpha * (reward + gamma * max(Q(newState,:)) - Q(state,action));
            end        
            state = newState;
            x = xNext; y = yNext;
        end
        if ~mod(i,10)
            str = sprintf('Q-Learning学习进度:%d%%',vpa(i/iter1*100,2));
            disp(str);
        end
    end
    
    %% 验证
    imshow(map);
    rectangle('position',[1 1 size(map)-1],'edgecolor','k'); hold on;
    scatter(goal(1),goal(2),100,"filled","b");
    testPoint = [200 100;200 200;200 300;200 400;200 500;200 600;200 700;200 800;200 900];
    for i = 1 : size(testPoint,1)
        scatter(testPoint(i,1),testPoint(i,2),100,"filled","g");
        
        x = testPoint(i,1); y = testPoint(i,2);
        state = calculateState(x,y,stateNumRow,statePixSize);
        path = [x,y];
        try
            while distanceCost([x,y],goal) > threshold
                [~,action] = max(Q(state,:));
                xNext = x + stepSize * sin((action-1)*10/180*pi);
                yNext = y - stepSize * cos((action-1)*10/180*pi);
                newState = calculateState(xNext,yNext,stateNumRow,statePixSize);
                if ~checkPath([x,y],[xNext,yNext],map)
                    break;
                end
                state = newState;
                x = xNext; y = yNext;
                path = [path;x,y];
            end
            plot(path(:,1),path(:,2),[path(end,1),goal(1)],[path(end,2),goal(2)],'LineWidth',2);
        catch
            disp("error!")
        end
    end
    
    
    % for i = 1:50
    %     [~,action] = max(Q(state,:));
    %     xNext = x + stepSize * sin((action-1)*10/180*pi);
    %     yNext = y - stepSize * cos((action-1)*10/180*pi);
    %     newState = calculateState(xNext,yNext,stateNumRow,statePixSize);
    %     state = newState;
    %     x = xNext; y = yNext;
    %     path = [path;x,y];
    % end
    % plot(path(:,1),path(:,2));
    

    calculateState.m

    function state = calculateState(x,y,stateNumRow,statePixSize)
        rowNum = ceil(y/statePixSize);
        colNum = ceil(x/statePixSize);
        state = (rowNum-1) * stateNumRow + colNum;
    end
    

    checkPath.m

    %% checkPath.m	
    function feasible=checkPath(n,newPos,map)
    feasible=true;
    dir=atan2(newPos(2)-n(2),newPos(1)-n(1));
    for r=0:0.5:sqrt(sum((n-newPos).^2))
        posCheck=n+r.*[cos(dir) sin(dir)];
        if ~(feasiblePoint(ceil(posCheck),map) && feasiblePoint(floor(posCheck),map) && ... 
                feasiblePoint([ceil(posCheck(1)) floor(posCheck(2))],map) && feasiblePoint([floor(posCheck(1)) ceil(posCheck(2))],map))
            feasible=false;break;
        end
        if ~feasiblePoint(floor(newPos),map), feasible=false; end
    end
    end
    

    distanceCost.m

    function h=distanceCost(a,b)         %% distanceCost.m
    	h = sqrt(sum((a-b).^2, 2));
    end
    

    feasiblePoint.m

    %% feasiblePoint.m
    function feasible=feasiblePoint(point,map)
    feasible=true;
    % check if collission-free spot and inside maps
    if ~(point(1)>=1 && point(1)<=size(map,1) && point(2)>=1 && point(2)<=size(map,2) && map(point(1),point(2))==1)
        feasible=false;
    end
    end
    

    test2.bmp
    在这里插入图片描述

    更多相关内容
  • 2.内容:基于kalman滤波的二维数据轨迹跟踪matlab仿真+matlab操作视频 3.用处:用于kalman滤波算法编程学习 4.指向人群:本硕博等教研学习使用 5.运行注意事项: 使用matlab2021a或者更高版本测试,运行里面的...
  • Q学习在二维障碍环境下的路径规划,打开文件直接运行即可,傻瓜式操作。Q学习作为强化学习的入门算法在路径规划方面有着一定的效果。
  • RRT算法简单介绍 1. RRT算法定义 RRT(Rapidly-Exploring...本文通过matlab实现RRT算法,解决二维平面的路径规划问题。 2. RRT算法基本步骤 1)确定起点start和终止点goal; 2)在空间中随机生成新的点r(50%为随.

    RRT算法简单介绍

    1. RRT算法定义

    RRT(Rapidly-Exploring Random Tree)算法是一种基于采样的路径规划算法,常用于移动机器人路径规划,适合解决高维空间和复杂约束下的路径规划问题。基本思想是以产生随机点的方式通过一个步长向目标点搜索前进,有效躲避障碍物,避免路径陷入局部极小值,收敛速度快。本文通过matlab实现RRT算法,解决二维平面的路径规划问题。
     

    2. RRT算法基本步骤

    1)确定起点start和终止点goal;

    2)在空间中随机生成新的点r(50%为随机点,50%为目标点,目的是增强RRT向goal点生成的导向性);

    3)判断点r与轨迹树中哪一个节点的欧氏距离最小,记该点为最近父节点closetNode;

    4)沿生长向量方向(最近父节点closetNode指向随机点r的方向)按照步长stepSize生成新的子节点newNode;

    5)碰撞检测:检测closetNode到newNode的连线是否会与障碍物发生碰撞。若是,返回步骤2,重新生成随机点;若否,则将newNode添加到轨迹树中。

    6)检测是否到达goal附近。若是,结束搜索;若否,返回步骤2继续搜索。

    三维轨迹Matlab仿真效果:

    障碍物我们只设置了球形障碍物,也可以设置成立方体、圆柱体等等。

    实施效果1:

         

     左:RRT随机树                                                 右:贪心算法优化后的随机树

     优化前,RRT树有80个节点,并且路径并不平滑。优化后,随机树只有三个点,路径只有两条直线组成。当然还可以继续对路径进行平滑,本文没有再继续做这些工作了。

     实施效果2:

     

    以下为RRT算法的Matlab实现代码:

    mian函数:

    %created by MW
    %date: 2022/5/25
    %func: RRT avoiding obstacle
    
    clc;
    clear;
    
    %% 创建并绘制障碍物
    circleCenter = [100 200 100;
                          200 700 100;
                          200 500 500;
                          700 700 300;
                          900 200 100];
    radius = [100;100;200;200;300];
    %绘制球形障碍物
    figure(1);
    [x, y, z] = sphere;      %创建一个坐标在原点,半径为1的标准圆,用于绘制自定义的圆
    for i = 1: length(radius)
        mesh(radius(i)*x + circleCenter(i,1), radius(i)*y + circleCenter(i,2), radius(i)*z + circleCenter(i,3));
        hold on;
    end
    axis equal;
    
    %% 创建初始位置和目标位置并绘制
    start = [0 0 0];
    goal = [700 800 1000];
    hold on;
    scatter3(start(1),start(2),start(3),'filled','g');
    scatter3(goal(1),goal(2),goal(3),'filled','b');
    
    %% 图片标注
    text(start(1),start(2),start(3),'起点');
    text(goal(1),goal(2),goal(3),'终点');
    view(3);
    grid on;
    axis equal;
    axis([0 1000 0 1000 0 1000]);
    xlabel('x');
    ylabel('y');
    zlabel('z');
    
    %% RRT方法生成避障轨迹
    path = RRT(start,goal,radius,circleCenter);
    
    %% 贪心算法优化RRT轨迹
    newPath = GreedyOptimize(path,radius,circleCenter);
    
    figure(2);
    %绘制球形障碍物
    [x, y, z] = sphere;      %创建一个坐标在原点,半径为1的标准圆,用于绘制自定义的圆
    for i = 1: length(radius)
        mesh(radius(i)*x + circleCenter(i,1), radius(i)*y + circleCenter(i,2), radius(i)*z + circleCenter(i,3));
        hold on;
    end
    axis equal;
    
    %绘制原RRT树
    plot3(path(:,1), path(:,2), path(:,3),'LineWidth',1,'Color','r');
    
    %绘制优化后的RRT树
    for k1 =1: length(newPath)
        point = newPath(k1,:);
        scatter3(point(1),point(2),point(3),'filled','k');   
    end
    plot3(newPath(:,1), newPath(:,2), newPath(:,3),'LineWidth',2,'Color','y');
    
    %图片标注
    text(start(1),start(2),start(3),'起点');
    text(goal(1),goal(2),goal(3),'终点');
    view(3);
    grid on;
    axis equal;
    axis([0 1000 0 1000 0 1000]);
    xlabel('x');
    ylabel('y');
    zlabel('z');
    

    RRT算法:

    function path = RRT(start,goal,radius,circleCenter)
    %% 定义RRT参数
    stepSize = 20;                           %步长
    maxIterTimes = 5000;                     %最大迭代步数
    iterTime = 0;                            %当前迭代次数
    threshold = 20;                          %阈值
    searchSize = [1000 1000 1000];           %空间尺寸
    RRTree = double([start -1]);             %创建RRT树,共4列。前3列为节点坐标,第4列为当前节点的父节点的索引。初始点的索引为-1
    
    %多个点到某一点欧式距离计算方法
    calcDis = @(a,b) sqrt((b(:,1)-a(1,1)).^2 + (b(:,2)-a(1,2)).^2 + (b(:,3)-a(1,3)).^2);
    
    %% 寻找RRT路径
    tic                       % tic-toc函数,用于计时,记录完成整个RRT树的运行时间
    pathFound = false;        %标志物,记录是否正确找到避障路径
    while iterTime <= maxIterTimes
        iterTime = iterTime +1;
        %step1 - 生成随机点
        %为了提高RRT扩展的导向性,以50%的概率在空间中随机生成生成新的随机点,50%的概率以目标点为新的随机点
        if rand < 0.5   
            sample = rand(1,3) .* searchSize + start;
        else
            sample = goal;
        end
        
        %step2 - 寻找树上最近父节点
        [val,nearIndex] = min(calcDis(sample, RRTree(:,1:3)),[],1);       %计算树上每个节点到随机点的欧氏距离,并返回最短距离的值和index
        closestNode = RRTree(nearIndex,1:3);
        
        %step3 - 沿生长向量方向按照步长生成新的子节点
        growVec = sample - closestNode;
        growVec = growVec/sqrt(sum(growVec.^2));
        newPoint = closestNode + growVec*stepSize;
        
        %step4 - 碰撞检测
        feasible = collisionDetec(newPoint,closestNode,radius,circleCenter);
        if ~feasible
            continue;         %如果发生碰撞,则重新寻找随机点      
        end
        
        %为树添加新节点
        RRTree = [RRTree; 
                       newPoint nearIndex];
        plot3([closestNode(1) newPoint(1)],[closestNode(2) newPoint(2)],[closestNode(3) newPoint(3)],'LineWidth',1,'Color','b');
        pause(0.01);
        
        %检测是否到达goal附近
        if sqrt(sum((newPoint - goal).^2)) <= threshold
            pathFound = true;
            break;           %如果节点已到达goal附近,则结束搜寻
        end
       
    end
    
    %搜索结束后如果搜索失败,显示错误信息
    if ~pathFound
        disp('no path found. maximum attempts reached');
    end
    
    toc
    
    %% 绘制回溯路径
    path = goal;
    lastNode = nearIndex;           %父节点索引,这里的nearIndex是goal的父节点索引
    while lastNode >= 0
        path =[RRTree(lastNode,1:3); path];         %将当前节点的上一个节点添加进回溯路径中
        lastNode = RRTree(lastNode, 4);             %更新父节点索引
    end
    plot3(path(:,1), path(:,2), path(:,3),'LineWidth',1,'Color','r');
    
    end
    

    碰撞检测算法:

    线段检测:

    %新的生长向量是否发生碰撞
    function feasible = collisionDetec(newPoint,closestNode,radius,circleCenter)
    feasible = true;
    checkVec = newPoint - closestNode;    %检测向量
    %将检测向量以0.5为步长等比均分为无数个检测点,检测每个检测点是否与球发生碰撞
    for i = 0:0.5:sqrt(sum(checkVec.^2))
        checkPoint = closestNode + i.*(checkVec/sqrt(sum(checkVec.^2)));     %生成检测点
        checkPointFeasible = pointCollisionCheck(checkPoint,radius,circleCenter);
        if ~checkPointFeasible
            feasible = false;
            break;
        end 
    end
    end
    
    

    点检测:

    %监测点是否发生碰撞
    function pointFeasible = pointCollisionCheck(checkPoint,radius,circleCenter)
    pointFeasible = true;
    for s = 1:length(radius)
        if sqrt(sum((checkPoint - circleCenter(s,:)).^2)) <= radius(s)
            pointFeasible = false;
            break;
        end   
    end
    end
    
    

    贪心算法简单介绍

    1. 贪心算法定义:

    贪心算法,是指在对问题求解时,总是做出再当前看来是最好的选择。也就是说,不从整体最优上加以考虑,他所做出的仅是某种意义上的局部最优解。

    贪心算法没有固定算法框架,算法设计的关键是贪心策略的选择。必须注意的是,贪心算法不是对所有问题都能得到整体最优解,选择的贪心策略必须具备无后效性,即某个状态以后的过程不会影响以前的状态,只与当前状态有关。

    2. 贪心算法基本步骤:

    步骤1:从某个初始解出发;
    步骤2:采用迭代的过程,当可以向目标前进一步时,就根据局部最优策略,得到一部分解,缩小问题规模;
    步骤3:将所有解综合起来。

    本问题下的贪心算法:

    1)连接目标点和初始点。

    2)检测是否发生碰撞。

    3)若是,表明从初始点到该点中间不能被省略,沿目标点往上回溯到上一级父节点(上一级父节点更新为新的目标点),回到步骤1;若否,表明从初始点到该点中间的所有节点均可被省略,将该点添加到新的随机树中,并将该点更新为新的初始点,目标点复位到最后一个节点,回到步骤1.

    4)检测直到start更新至goal结束。

    function newPath = GreedyOptimize(path,radius,circleCenter)
    
    startIndex = 1;
    goalIndex = length(path(:,1));
    detectTimes = length(path(:,1)) - 1;          %检测次数
    newPath = [path(startIndex,:)];               %添加初始位置
    
    while detectTimes >0;
        detectTimes = detectTimes-1;
        start = path(startIndex,:);
        goal = path(goalIndex,:);
        
        %碰撞检测
        feasible = collisionDetec(goal,start,radius,circleCenter);
        if ~feasible
            goalIndex = goalIndex - 1;            %若碰撞,index减1,继续向上探索父节点
            continue;                            
        else
            newPath = [newPath; goal];            %若未发生碰撞,表示找到一个最优节点,则从该节点往上的所有父节点均可省略,将该节点添加至路径中
            detectTimes = length(path(:,1)) - goalIndex;        %检测次数更位
            startIndex = goalIndex;               %将当前节点索引更新为新的树的起点
            goalIndex = length(path(:,1));        %将终点索引复位
    
        end
       
    end
    
    newPath = [newPath; path(end,:)];             %添加目标位置
    
    end
    
    

    展开全文
  • 该程序用于计算光场中一点的偏振光轨迹二维
  • 轨迹生成(2D).zip,Path_generation1_circle_3arm.m,Path_generation_circle_2arm.m,Path_generation_rectangular_3arm.m
  • RRT算法在三维轨迹规划中的MATLAB仿真

    千次阅读 多人点赞 2020-11-25 12:07:18
    RRT二维轨迹规划 关于RRT二维轨迹规划以及matlab实现参考博客: RRT RRT算法简单思路分析 生成根节点,并给出终止节点。 随机在空间中找到一点rrr(50%随机点,50%为目标点),判断rrr与轨迹树中哪一个节点的欧氏距离...

    RRT二维轨迹规划

    关于RRT二维轨迹规划以及matlab实现参考博客:
    RRT

    RRT算法简单思路分析

    1. 生成根节点,并给出终止节点。
    2. 随机在空间中找到一点 r r r(50%随机点,50%为目标点),判断 r r r与轨迹树中哪一个节点的欧氏距离最小,记该节点为 n o d e b e s t node_{best} nodebest
    3. n o d e b e s t 向 node_{best}向 nodebest r r r延伸步长 s t e p S i z e stepSize stepSize个单位(用向量实现)。
    4. 判断线段 n o d e b e s t − > r node_{best}->r nodebest>r是否触碰到障碍物。若是,放弃该 r r r,回到2。
    5. 判断 r r r是否临近目标点,若是,退出循环,否则将 r r r添加到轨迹树,回到2。

    算法还可以引入最大错误次数,若在一个迭代过程中超过最大错误次数依旧没有找到不发生碰撞的新子节点,则退出循环,并error一个“无法找到合理轨迹”。

    RRT优势与劣势

    通过对状态空间中的采样点进行碰撞检测,避免了对空间的建模,能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间,通过状态空间的随机采样点,把搜索导向空白区域,从而寻找到一条从起始点到目标点的规划路径,适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。与PRM类似,该方法是概率完备且不最优的。

    三维轨迹规划MATLAB仿真

    二维的见上面给出的博客。
    我们将三维障碍物设定为球,这样方便碰撞检测和绘图。

    %% 绘制障碍物(以球为例,主要是方便计算)
    x0=100; y0=100; z0=100;%球心
    circleCenter = [100,100,100;50,50,50;100,40,60;150,100,100;60,130,50];
    r=[50;20;20;15;15];%半径
    %下面开始画
    figure(1);
    [x,y,z]=sphere;
    for i = 1:length(circleCenter(:,1))
        mesh(r(i)*x+circleCenter(i,1),r(i)*y+circleCenter(i,2),r(i)*z+circleCenter(i,3));hold on;
    end
    axis equal
    

    把球心和对应的半径填入 circleCenter 和 r 矩阵即可。

    给定仿真参数:
    goal为目标
    source为起始

    %% 参数
    source=[10 10 10];
    goal=[150 150 150];
    stepsize = 10;
    threshold = 10;
    maxFailedAttempts = 10000;
    display = true;
    searchSize = [250 250 250];      %探索空间六面体
    

    这里的探索空间六面体为随机点所在的空间。

    绘制起始点和终止点:

    %% 绘制起点和终点
    hold on;
    scatter3(source(1),source(2),source(3),"filled","g");
    scatter3(goal(1),goal(2),goal(3),"filled","b");
    

    在这里插入图片描述
    主循环:
    failedAttempts为最大错误次数,pathFound为是否找到规划路径,RRTree为生成树。

    tic;  % tic-toc: Functions for Elapsed Time
    RRTree = double([source -1]);
    failedAttempts = 0;
    pathFound = false;
    
    %% 循环
    while failedAttempts <= maxFailedAttempts  % loop to grow RRTs
        %% chooses a random configuration
        if rand < 0.5
            sample = rand(1,3) .* searchSize;   % random sample
        else
            sample = goal; % sample taken as goal to bias tree generation to goal
        end
        %% selects the node in the RRT tree that is closest to qrand
        [A, I] = min( distanceCost(RRTree(:,1:3),sample) ,[],1); % find the minimum value of each column
        closestNode = RRTree(I(1),1:3);
        %% moving from qnearest an incremental distance in the direction of qrand
        movingVec = [sample(1)-closestNode(1),sample(2)-closestNode(2),sample(3)-closestNode(3)];
        movingVec = movingVec/sqrt(sum(movingVec.^2));  %单位化
        newPoint = closestNode + stepsize * movingVec;
        if ~checkPath3(closestNode, newPoint, circleCenter,r) % if extension of closest node in tree to the new point is feasible
            failedAttempts = failedAttempts + 1;
            continue;
        end
        
        if distanceCost(newPoint,goal) < threshold, pathFound = true; break; end % goal reached
        [A, I2] = min( distanceCost(RRTree(:,1:3),newPoint) ,[],1); % check if new node is not already pre-existing in the tree
        if distanceCost(newPoint,RRTree(I2(1),1:3)) < threshold, failedAttempts = failedAttempts + 1; continue; end 
        
        RRTree = [RRTree; newPoint I(1)]; % add node
        failedAttempts = 0;
        if display, plot3([closestNode(1);newPoint(1)],[closestNode(2);newPoint(2)],[closestNode(3);newPoint(3)],'LineWidth',1); end
        pause(0.05);
    end
    
    if display && pathFound, plot3([closestNode(1);goal(1)],[closestNode(2);goal(2)],[closestNode(3);goal(3)]); end
    
    if display, disp('click/press any key'); waitforbuttonpress; end
    if ~pathFound, error('no path found. maximum attempts reached'); end
    

    回溯规划轨迹:

    %% retrieve path from parent information
    path = goal;
    prev = I(1);
    while prev > 0
        path = [RRTree(prev,1:3); path];
        prev = RRTree(prev,4);
    end
    
    pathLength = 0;
    for i=1:length(path(:,1))-1, pathLength = pathLength + distanceCost(path(i,1:3),path(i+1,1:3)); end % calculate path length
    fprintf('processing time=%d \nPath Length=%d \n\n', toc, pathLength); 
    figure(2)
    for i = 1:length(circleCenter(:,1))
        mesh(r(i)*x+circleCenter(i,1),r(i)*y+circleCenter(i,2),r(i)*z+circleCenter(i,3));hold on;
    end
    axis equal
    hold on;
    scatter3(source(1),source(2),source(3),"filled","g");
    scatter3(goal(1),goal(2),goal(3),"filled","b");
    plot3(path(:,1),path(:,2),path(:,3),'LineWidth',2,'color','r');
    

    运行(还有三个碰撞检测函数的代码放在后面给出):
    在这里插入图片描述
    GIF:
    在这里插入图片描述
    命令行打印出时间和路径长:
    在这里插入图片描述

    碰撞检测函数代码

    %% checkPath3.m	
    function feasible=checkPath3(n,newPos,circleCenter,r)
    feasible=true;
    movingVec = [newPos(1)-n(1),newPos(2)-n(2),newPos(3)-n(3)];
    movingVec = movingVec/sqrt(sum(movingVec.^2)); %单位化
    for R=0:0.5:sqrt(sum((n-newPos).^2))
        posCheck=n + R .* movingVec;
        if ~(feasiblePoint3(ceil(posCheck),circleCenter,r) && feasiblePoint3(floor(posCheck),circleCenter,r))
            feasible=false;break;
        end
    end
    if ~feasiblePoint3(newPos,circleCenter,r), feasible=false; end
    end
    
    function h=distanceCost3(a,b)         %% distanceCost.m
    	h = sqrt(sum((a-b).^2, 2));
    end
    
    %% feasiblePoint3.m
    function feasible=feasiblePoint3(point,circleCenter,r)
    feasible=true;
    % check if collission-free spot and inside maps
    for row = 1:length(circleCenter(:,1))
        if sqrt(sum((circleCenter(row,:)-point).^2)) <= r(row)
            feasible = false;break;
        end
    end
    end
    

    二维规划的补充

    利用visio绘制几张障碍图,二值化,使用推荐的博客代码运行:
    在这里插入图片描述

    在这里插入图片描述
    可以看到RRT算法得到的轨迹不够平滑,可以改进,采用多项式拟合技术,这里就不再深入了,有兴趣的可以与作者讨论。

    展开全文
  • 假定船舶在二维平面上运动,初始位置为(-100m,200m),水平运动速度为 2m/s, 垂直方向运动速度为 20m/s,GPS 接收机的扫描周期为 T = 1s,观测噪声的均值 为 1,方差为 100。 采用卡尔曼滤波跟踪船舶航行轨迹。...
  • 平流层飞艇的二维轨迹优化
  • 受此启发,将这一思想拓展到UAV或者是任何背景的三维或者是二维轨迹规划问题,即可得到本文所讲述的流体扰动原理规划算法。 这一算法实际上属于人工势场算法的一种变形,在没有障碍物的前提下,三维空间中存在一个...

    思想

    在一条河边仔细观察会发现水流流过河中凸起的石头时会从石头旁边平滑地流过,这是流场的作用。受此启发,将这一思想拓展到UAV或者是任何背景的三维或者是二维轨迹规划问题,即可得到本文所讲述的流体扰动原理规划算法。

    这一算法实际上属于人工势场算法的一种变形,在没有障碍物的前提下,三维空间中存在一个初始的流场,这个流场指向目标点,使轨迹向目标点汇流。当空间中存在障碍物时,障碍物产生一个扰动流场,扰动流场与初始流场同时作用形成合成流场,合成流场的流线即为规划轨迹。可以将扰动流场算法中的初始流场看做人工势场算法中的引力场,扰动场看做斥力场,两者异曲同工。

    初始流场的建立

    以汇流作为初始流场,其流速 u ( P ) u(\mathbf{P} ) u(P)定义为:
    u ( P ) = − [ C ( x − x d ) d ( P , P d ) , C ( y − y d ) d ( P , P d ) , C ( z − z d ) d ( P , P d ) ] T \boldsymbol{u}(\boldsymbol{P})=-\left[\frac{C\left(x-x_{\mathrm{d}}\right)}{d\left(\boldsymbol{P}, \boldsymbol{P}_{\mathrm{d}}\right)} ,\frac{C\left(y-y_{\mathrm{d}}\right)}{d\left(\boldsymbol{P}, \boldsymbol{P}_{\mathrm{d}}\right)}, \frac{C\left(z-z_{\mathrm{d}}\right)}{d\left(\boldsymbol{P}, \boldsymbol{P}_{\mathrm{d}}\right)}\right]^{\mathrm{T}} u(P)=[d(P,Pd)C(xxd),d(P,Pd)C(yyd),d(P,Pd)C(zzd)]T
    其中 C C C为汇流速度, d d d为无人机位置 P \mathbf{P} P与目标位置 P d \mathbf{P_d} Pd的距离:
    d ( P , P d ) = ( x − x d ) 2 + g ( y ) − y d ) 2 + ( z − z d ) 2 d\left(\boldsymbol{P}, \boldsymbol{P}_{\mathrm{d}}\right)=\sqrt{\left.\left(x-x_{\mathrm{d}}\right)^{2}+g(y)-y_{\mathrm{d}}\right)^{2}+\left(z-z_{\mathrm{d}}\right)^{2}} d(P,Pd)=(xxd)2+g(y)yd)2+(zzd)2

    初始流场示意图如下(红色曲线为流场线):
    在这里插入图片描述
    在这里插入图片描述

    扰动流场的建立

    障碍物方程

    假设1:运动目标假定为质点,所有障碍物和威胁等均用圆球、圆锥、圆柱、长方体等标准的凸多面体等效:
    Γ ( P ) = ( x − x 0 a ) 2 p + ( y − y 0 b ) 2 q + ( z − z 0 c ) 2 r \Gamma(\boldsymbol{P})=\left(\frac{x-x_{0}}{a}\right)^{2 p}+\left(\frac{y-y_{0}}{b}\right)^{2 q}+\left(\frac{z-z_{0}}{c}\right)^{2 r} Γ(P)=(axx0)2p+(byy0)2q+(czz0)2r
    式中:常数 a 、 b 、 c a、b、c abc p 、 q 、 r p、q、r pqr分别决定凸面体的大小与形状, ( x 0 , y 0 , z 0 ) (x_0,y_0,z_0) (x0,y0,z0)表示凸面体的中心, Γ ( P ) = 1 \Gamma(\boldsymbol{P})=1 Γ(P)=1即为障碍物表面方程。

    扰动流场

    假设环境中有 W W W个障碍物,将障碍物对初始流场的扰动用扰动矩阵 M ‾ ( P ) \overline{\boldsymbol{M}}(\boldsymbol{P}) M(P)量化表示:
    M ‾ ( P ) = ∏ w = 1 W M ω ( P ) \overline{\boldsymbol{M}}(\boldsymbol{P})=\prod_{w=1}^{W} \boldsymbol{M}_{\boldsymbol{\omega}}(\boldsymbol{P}) M(P)=w=1WMω(P)
    式中: M ‾ ( P ) \overline{\boldsymbol{M}}(\boldsymbol{P}) M(P)为第 ω \omega ω个障碍物的扰动矩阵:
    M w ( P ) = I − ω w ( P ) n w ( P ) ( n w ( P ) ) T ∣ Γ ( P ) ) ∣ 1 ρ w ( n w ( P ) ) T n w ( P ) M_{w}(P)=I - \frac{\omega_{w}(\boldsymbol{P}) \boldsymbol{n}_{w}(\boldsymbol{P})\left(\boldsymbol{n}_{w}(\boldsymbol{P})\right)^{\mathrm{T}}}{\left|\Gamma(\boldsymbol{P}))\right|^{\frac{1}{\rho_{w}}}\left(\boldsymbol{n}_{w}(\boldsymbol{P})\right)^{\mathrm{T}} \boldsymbol{n}_{w}(\boldsymbol{P})} Mw(P)=IΓ(P))ρw1(nw(P))Tnw(P)ωw(P)nw(P)(nw(P))T
    式中: I I I为单位矩阵; ρ w \rho_w ρw为第 w w w个障碍物的反应系数,而 ω w ( P ) , n w ( P ) \omega_{w}(\boldsymbol{P}),\boldsymbol{n}_{w}(\boldsymbol{P}) ωw(P),nw(P)按如下公式计算:
    ω w ( P ) = 1 W = 1 ∏ i = 1 W Γ i ( P ) − 1 ( Γ i ( P ) − 1 ) + ( Γ w ( P ) − 1 ) W ≠ 1 \begin{array}{cc} \omega_{w}(P)= & \\ 1 & W=1 \\ \prod_{i=1}^{W} \frac{\Gamma_{i}(P)-1}{\left(\Gamma_{i}(P)-1\right)+\left(\Gamma_{w}(P)-1\right)} & W \neq 1 \end{array} ωw(P)=1i=1W(Γi(P)1)+(Γw(P)1)Γi(P)1W=1W=1
    n w ( P ) = [ δ Γ w ( P ) δ x δ Γ w ( P ) δ y δ Γ w ( P ) δ z ] T \boldsymbol{n}_{w}(\boldsymbol{P})=\left[\frac{\delta \Gamma_{w}(\boldsymbol{P})}{\delta x} \quad \frac{\delta \Gamma_{w}(\boldsymbol{P})}{\delta y} \quad \frac{\delta \Gamma_{w}(\boldsymbol{P})}{\delta z}\right]^{\mathrm{T}} nw(P)=[δxδΓw(P)δyδΓw(P)δzδΓw(P)]T
    至此,我们可以计算出扰动矩阵 M ‾ ( P ) \overline{\boldsymbol{M}}(\boldsymbol{P}) M(P),利用它修正初始流场流速,得到合成流场:
    u ‾ ( P ) = M ‾ ( P ) u ( P ) \overline{\boldsymbol{u}}(\boldsymbol{P})=\overline{\boldsymbol{M}}(\boldsymbol{P}) \boldsymbol{u}(\boldsymbol{P}) u(P)=M(P)u(P)
    通过积分 u ‾ ( P ) \overline{\boldsymbol{u}}(\boldsymbol{P}) u(P),可以获得轨迹下一点位:
    P next  = P + u ‾ ( P ) Δ t \boldsymbol{P}_{\text {next }}=\boldsymbol{P}+\overline{\boldsymbol{u}}(\boldsymbol{P}) \Delta t Pnext =P+u(P)Δt
    式中: Δ t \Delta t Δt为计算步长,如此迭代求解得到一系列离散点,将其连线即形成扰动流线,即规划路线。

    仿真结果

    目标点: g o a l = [ 2 , 2 , 10 ] ; goal = [2,2,10]; goal=[2,2,10];
    参数: C = 1 ; r o w = 1.2 ; C = 1; row = 1.2; C=1;row=1.2;
    障碍物坐标以及半径:
    o b s t a c l e M a t r i x = [ 4 , 6 , 10 ; 6 , 4 , 10 ; 8 , 8 , 10 ] ; o b s t a c l e R M a t r i x = [ 1 , 1 , 1.2 ] ; obstacleMatrix = [4,6,10;6,4,10;8,8,10];\\ obstacleRMatrix = [1,1,1.2]; obstacleMatrix=[4,6,10;6,4,10;8,8,10];obstacleRMatrix=[1,1,1.2];
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    其中红线为初始流场线,蓝色为合成流场线(规划路线),可以看到规划路线平滑地绕开球形障碍物到达目标点(理论上在没有其他约束的情况下不会与障碍物有相交的情况),在众多规划算法中可以说达到SOTA效果。

    动态障碍

    动态障碍轨迹规划的实现需要考虑障碍的速度,其基本思想就是在某一时刻按照静态流场计算出下一点位(相对位置)后加上障碍物的速度乘以时间步长(绝对位置)。
    u ˉ ( P ) = M ˉ ( P ) ( u ( P ) − v ( P ) ) + v ( P ) P n e x t = P + u ‾ ( P ) Δ t \bar{u}(P)=\bar{M}(P)(u(P)-v(P))+v(P)\\ \boldsymbol{P}_{\mathrm{next}}=\boldsymbol{P}+\overline{\boldsymbol{u}}(\boldsymbol{P}) \Delta t uˉ(P)=Mˉ(P)(u(P)v(P))+v(P)Pnext=P+u(P)Δt
    v ( P ) v(P) v(P)是真实障碍物速度 v w ( P ) ) \boldsymbol{v}_{w}(\boldsymbol{P})) vw(P))的加权,让轨迹靠近障碍物时更多地考虑障碍物的速度,而原理障碍物时不太考虑障碍物的速度:
    v ( P ) = ∑ w = 1 W ω w ( P ) e − Γ w ( P ) λ v w ( P ) \boldsymbol{v}(\boldsymbol{P})=\sum_{w=1}^{W} \omega_{w}(\boldsymbol{P}) \mathrm{e}^{\frac{-\Gamma_{w}(P)}{\lambda}} \boldsymbol{v}_{w}(\boldsymbol{P}) v(P)=w=1Wωw(P)eλΓw(P)vw(P)

    另外,流体法还可以应用于目标点也在运动的情况,这时初始流场需要考虑目标点的运动速度:
    u ( P ) = k 1 u 1 ( P ) + k 2 u 2 ( P ) k 1 = ( d ( P P d ) d ( P 0 P d 0 ) ) m k 2 = 1 − k 1 \begin{array}{c} \boldsymbol{u}(\boldsymbol{P})=k_{1} \boldsymbol{u}_{1}(\boldsymbol{P})+k_{2} \boldsymbol{u}_{2}(\boldsymbol{P}) \\ k_{1}=\left(\frac{d\left(\boldsymbol{P} \boldsymbol{P}_{\mathrm{d}}\right)}{d\left(\boldsymbol{P}_{0} \boldsymbol{P}_{\mathrm{d} 0}\right)}\right)^{m} \\ k_{2}=1-k_{1} \end{array} u(P)=k1u1(P)+k2u2(P)k1=(d(P0Pd0)d(PPd))mk2=1k1
    Pd是目标点坐标,P是当前轨迹坐标,P0是初始点坐标。

    动态障碍规划效果:
    在这里插入图片描述
    在这里插入图片描述
    这个算法的动态避障效果较绝大多数算法都要好。

    一种更优秀的改良算法

    IIFDS

    缺点

    对于形状复杂的障碍物由于其 Γ ( P ) \Gamma(\boldsymbol{P}) Γ(P)复杂甚至不能用 Γ ( P ) \Gamma(\boldsymbol{P}) Γ(P)的形式表示,因此在含有这种障碍物的环境下使用流场法会出现一定的困难,而人工势场法则没有这种问题。另外,当起始点、障碍物中心点、目标点在共线时会出现无解现象,但是这种情况发生条件较为苛刻,可以忽略。

    参考文章

    [1]. 王宏伦, 姚鹏, 梁宵, 等.基于流水避石原理的无人机三维航路规划[J].电光与控制, 2015, 22(10):1-6.

    展开全文
  • 、基于隐函数的变换 2.1 法1:平面上等高线模型 给出一个隐函数如下: 对于这样的隐函数方程,很容易写成函数: z是平面x-0-y上的高度函数,那么,求出平面上任意点(x,y)的高度,将高度为0的点取出,就是原方程...
  • 12小时内进行卫星轨迹仿真,可画出三维轨迹
  • MATLAB源码集锦-fmincon所有约束加二维自变量轨迹
  • 轨迹跟踪——二维轨迹跟踪

    万次阅读 2016-04-25 11:30:03
    其中鱼类的轨迹提取我已做了两部分工作,二维视频跟踪,提取鱼类的轨迹;另一部分工作是重建三维鱼类游动轨迹。鱼类微特征提取还没有动工(惭愧)。 ---------- 二维的视频跟踪在做这项工作之前我们花费了很大的力气...
  • 提出了二维近似熵的概念,并用于表征轴心轨迹的复杂性,定量地评价了回转机械的运行状态。该方法具有计算所需数据短、抗噪及抗野点能力强、对确定性信号和随机信号都适用等特点。应用二维近似熵,对某机组关键轴瓦的...
  • 今天小编就为大家分享一篇python 画二维、三维点之间的线段实现方法,具有很好的参考价值,希望对大家有所帮助。一起跟随小编过来看看吧
  • qt二维OPENGL绘图源码
  • matlab_二维机器人上楼梯运动的步态规划运动轨迹,以及稳定性分析,各关节的运动轨迹都可以直观的表现出来
  • 在具体实施任务时仅靠操作员手中的遥控器控制无人飞行器执行相应的工作,可能会对操作员心理以及技术提出极高的要求,为了避免个人操作失误,进而造成飞行器损坏的危险,一种解决问题的方法就是对飞行器进行航迹规划
  • 针对水下机器人轨迹跟踪控制的速度跳变问题,提出了一种基于生物启发神经动力学模型的自治水下机器人(AUV)三...将该方法对“海筝号”水下机器人进行三维轨迹跟踪控制的仿真实验,仿真结果表明了所提控制方法的有效性。
  • 涵盖二维目标跟踪的基本模型(包括CV,CA,CT),使用卡尔曼滤波实现信息融合,对目标跟踪和卡尔曼的初学者有很大帮助
  • 基于二维模型预测控制的无人水下航行器轨迹跟踪
  • 卡尔曼滤波UKF实现2D目标跟踪 代码肯定能运行且有结果,可开发性强, 如果有问题可联系WX:ZB...仿真结果:二维跟踪轨迹,位置RMSE,速度RMSE(均方根误差) 各维度跟踪轨迹,跟踪误差误差,各个维度跟踪误差)。
  • 二维超声椭圆振动轨迹跟踪控制算法,李文妹,姜晨,针对非球面光学元件二维超声振动辅助磨削加工需要,研究二维椭圆超声振动轨迹的控制算法。为了确保振动轨迹与加工点的法向相对姿
  • 机械臂建模,轨迹规划,避障路径规划(介绍+代码)()前言工作空间获取轨迹规划及绘制关节空间轨迹规划笛卡尔空间轨迹规划正逆运动学程序下载: 前言 最近写的一个作业,记录分享一下,主要是对机械臂在MATLAB中...
  • 基于二维隐马模型的车行轨迹分类,周珊,王小捷,本文利用二维隐马可夫模型对车行轨迹分类。针对车行轨迹的独特性,提出了适用的二维特征提取方案,与以往轨迹分类中所采用的特征
  • 无碳小车三维建模、二维图纸,还有计算方案,matlab仿真轨迹程序
  • 仿真结果:二维跟踪轨迹,各维度跟踪轨迹,跟踪误差误差,各个维度跟踪误差)。 仿真参数设置:见下面链接的里面又给 仿真结果可以先看下面链接博客, 对应的仿真模型及参数设置见无迹卡尔曼滤波 对应的理论分析和...
  • 前言 问题由来 最近遇到了一类图,非常好看,并且在其他论文中也多次遇到。比如,在 Trajectory data-based traffic flow... 每条轨迹线均表示一辆车的行驶路径变化,而线条的颜色则表示瞬时速度值 因此,如果想要绘.

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 22,722
精华内容 9,088
热门标签
关键字:

二维轨迹规划