精华内容
下载资源
问答
  • 二维避障轨迹规划 目前我写的代码不够成熟,就不贴了。Agent的reward机制如下:走出图像reward为一个较大负数,撞上障碍也为一个较大负数,若不是前两者,则reward为当前点位与目标点位距离的反比函数(越近奖赏越大...

    博客推荐

    Q-Learning

    二维避障轨迹规划

    目前我写的代码不够成熟,就不贴了。Agent的reward机制如下:走出图像reward为一个较大负数,撞上障碍也为一个较大负数,若不是前两者,则reward为当前点位与目标点位距离的反比函数(越近奖赏越大)。
    state状态的划分是根据像素区域来的,等同于把图像分割成了若干像素块,这样可以减小state的数量,否则训练困难。Action根据移动方向的角度来分,比如10°划分的话一个state就有36个actions,这样Q表就是states×actionsstates\times 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
    在这里插入图片描述

    展开全文
  • RRT算法在三维轨迹规划中的MATLAB仿真

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

    RRT二维轨迹规划

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

    RRT算法简单思路分析

    1. 生成根节点,并给出终止节点。
    2. 随机在空间中找到一点rr(50%随机点,50%为目标点),判断rr与轨迹树中哪一个节点的欧氏距离最小,记该节点为nodebestnode_{best}
    3. nodebestnode_{best}向rr延伸步长stepSizestepSize个单位(用向量实现)。
    4. 判断线段nodebest>rnode_{best}->r是否触碰到障碍物。若是,放弃该rr,回到2。
    5. 判断rr是否临近目标点,若是,退出循环,否则将rr添加到轨迹树,回到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算法得到的轨迹不够平滑,可以改进,采用多项式拟合技术,这里就不再深入了,有兴趣的可以与作者讨论。

    展开全文
  • 轨迹跟踪——二维轨迹跟踪

    千次阅读 2016-04-25 11:30:03
    其中鱼类的轨迹提取我已做了两部分工作,二维视频跟踪,提取鱼类的轨迹;另一部分工作是重建三维鱼类游动轨迹。鱼类微特征提取还没有动工(惭愧)。 ---------- 二维的视频跟踪在做这项工作之前我们花费了很大的力气...

    转载请注明原作者t1234xy4:http://blog.csdn.net/t1234xy4/article/details/51241032

    在读研期间,由于导师与水环研究生水生物有项目交叉,我主要研究视频跟踪技术。用来提取鱼类的轨迹以及鱼类的微动作。其中鱼类的轨迹提取我已做了两部分工作,二维视频跟踪,提取鱼类的轨迹;另一部分工作是重建三维鱼类游动轨迹。鱼类微特征提取还没有动工(惭愧)。
    ----------
    

    二维的视频跟踪

    在做这项工作之前我们花费了很大的力气去获取实验数据。购买了三个汉邦高科的摄像头,水箱,摄像头支架等。搭建好实验装置。(由于主要说视频跟踪,具体与鱼相关的就带过)
    注意:
    1、摄像头要固定住,这样拍摄的视频帧才有固定的摄像机坐标,最后才好转换成统一的现实坐标。
    2、获取的视频尽量减少运动背景的干扰,熟悉前景检测的人都应该知道。
    3、注意光线,不要太暗了,也不要太刺眼,有光圈亮点。
    

    前景跟踪算法过程

    首先都是查看文献来着,大概看了20多篇文献资料。其实起指导作用的文献还是只有那么几篇。我就列出来:
    [1].Zhang Z.A flexible new technique for camera calibration[J].Transactions on Pattern Analysis and Machine Intelligence,2000(11):1330-1334.
    [2]Olivier Barnich, Marc Van Droogenbroeck. ViBe: A universal background subtraction algorithm for video sequences[J]. IEEE Transactions on Image Processing, 20(6):1709-1724, June 2011.
    [3]Robust Fragments-based Tracking using the Integral Histogram.
    还有很多文献,就不一一列出来了。对我用C++来实现这些算法的启蒙文献应该是文献[3]:
    第[3]篇文献:是一种改进的模板匹配方法,把模板和目标都分成多个字块,然后分别匹配,这样可以避免部分被遮挡就丢失目标的情况。(有源码可以查看)
    文献[1]很经典,我是直接用的,用来消除摄像头的扭曲形变。
    文献[2]是我们方法的基础,我直接将它的源码移植过来了。
    

    我的工作

    1、主要是从前景检测的结果中用一种更优的方法找出鱼类所在的位置。能够更加精确、实时的提取鱼类轨迹。
    2、将轨迹做平滑处理,使用了基于平方的方法,效果很不错。
    3、开发二维轨迹跟踪软件,确实只是小软件,我只用了3个星期不到的时间做完,当然还有很多需要完善的地方。其实也有之前做三维鱼类轨迹跟踪系统的基础。很多复用了之前的代码。
    

    二维估计跟踪算法:
    原始帧->去失真->vibe->局部搜索->二维轨迹
    vibe算法不是我的原创,是已经很成熟的算法,故不详细说明。贴出它的源码吧,希望大家可以直接借鉴学习。
    二次封装之后的vibe.hxx:

    #ifndef _VIBE_HXX_
    #define _VIBE_HXX_
    class VIBE 
    {
    public:
        VIBE();
        ~VIBE();
        void initialize();
        void update();
        inline void setCurrentFrame( unsigned char* i ) { _image = i; }
        inline void setSegmentMap( unsigned char* i ) { _segMap = i; }
    
        inline void setFrameWidth( int w ) { _frameWidth = w; }
        inline void setFrameHeight( int h ) { _frameHeight = h; }
        inline void setFrameWidthStrip( int s ) { _frameWidthStrip = s; }
    
        inline bool isInitilized() { return _samples; }
    
    private:
        int getRandomSample();
        int getRandomSubSample();
        int getRandomNeightXCoordinate( int x );
        int getRandomNeightYCoordinate( int y );
    
    private:
        int _frameWidth;
        int _frameHeight;
        int _frameWidthStrip;
    
        int _sphereRadius;
        int _pixelSamples;
        int _backgroundThreshold;
        int _subSampling;
    
        int _borderWidth;
    
        unsigned char* _image;
        unsigned char* _segMap;
        unsigned char** _samples;
    };
    #endif
    #include "stdafx.h"
    #include <assert.h>
    #include<stdlib.h>
    #include <time.h>
    
    #include <math.h>
    #include "VIBE.hxx"
    
    #define N 25
    #define R 15
    #define ZMIN    3
    #define PHI     16
    
    VIBE::VIBE()
    {
        _frameWidth = 0;
        _frameHeight = 0;
        _frameWidthStrip = 0;
    
        _sphereRadius = R;
        _pixelSamples = N;
    
        _backgroundThreshold = ZMIN;
        _subSampling = PHI;
    
        _image = 0;
        _segMap = 0;
    
        _samples = 0;
    
        _borderWidth = 0;
    }
    
    VIBE::~VIBE()
    {
        if( _samples )
        {
            for( int i = 0; i < _pixelSamples; i ++ )
                if( _samples[ i ] )
                {
                    delete [] _samples[ i ];
                    _samples[ i ] = 0;
                }
            delete [] _samples;
            _samples = 0;
        }
    }
    
    void VIBE::initialize()
    {
        assert( (_frameWidth < 1 || _frameHeight < 1 || _frameWidthStrip < 1) || "Please set frame info for initialize...\n");
    
        srand((int)time(0));
    
        _samples = new unsigned char*[ _pixelSamples ];
    
        for( int i = 0; i < _pixelSamples; i ++ )
            _samples[ i ] = new unsigned char[ _frameHeight * _frameWidthStrip ];
    
    
        int tq = sqrtf( _pixelSamples );
        _borderWidth = tq / 2;
    
        for( int y = _borderWidth; y < _frameHeight - _borderWidth; y ++ )
        {
            for( int x = _borderWidth; x < _frameWidth - _borderWidth; x ++ )
            {
                int c = 0; 
                for( int i = -_borderWidth; i <= _borderWidth && c < _pixelSamples ; i ++ )
                {
                    for( int j = -_borderWidth; j <= _borderWidth && c < _pixelSamples; j ++ )
                        if( c < _pixelSamples - _backgroundThreshold )
                            *(_samples[ c++ ] + y * _frameWidthStrip + x) = *(_image + ( y + i ) * _frameWidthStrip + ( x + j ));
                        else
                            *(_samples[ c++ ] + y * _frameWidthStrip + x) = *(_image +  y * _frameWidthStrip + x);
    
                }
            }
        }
    }
    
    void VIBE::update()
    {
        for( int x = 0; x < _frameWidth; x ++ )
        {
            for( int y = 0; y < _frameHeight; y ++ )
            {
                int count = 0, index = 0, dist = 0;
    
                if( y < _borderWidth || x < _borderWidth || x >= _frameWidth - _borderWidth || y >= _frameHeight - _borderWidth )
                {
                    *(_segMap + y * _frameWidthStrip + x ) = 0;
                    continue;
                }
    
                while( count < _backgroundThreshold && index < _pixelSamples )
                {
                    dist = abs( *(_image + y * _frameWidthStrip + x) -
                        *(_samples[ index ] + y * _frameWidthStrip + x ) );
    
                    if( dist < _sphereRadius )
                        count ++;
    
                    index ++;
                }
    
    
                if( count >= _backgroundThreshold )
                {
                    *(_segMap + y * _frameWidthStrip + x ) = 0;
    
                    int rand = getRandomSubSample();
                    //if( rand == 0 )
                  if( rand < 6)
                    {
                        rand = getRandomSample();
                        *(_samples[ rand ] + y * _frameWidthStrip + x ) = *( _image + y * _frameWidthStrip + x );
                    }
    
                    rand = getRandomSubSample();
                    //if( rand == 0 )
                   if( rand < 6 )
                    {
                        int xg, yg;
    
                        xg = getRandomNeightXCoordinate(x);
                        yg = getRandomNeightYCoordinate(y);
    
                        rand = getRandomSample();
                        *(_samples[ rand ] + yg * _frameWidthStrip + xg ) = *( _image + y * _frameWidthStrip + x );
                    }
    
                }
                else
                {
                    *(_segMap  + y * _frameWidthStrip + x ) = 255;
                }
            }
        }
    }
    
    int VIBE::getRandomSample()
    {
        int val = _pixelSamples * 1.0 * rand() / RAND_MAX;
    
        if( val == _pixelSamples )
            return val - 1;
        else
            return val;
    
    }
    
    int VIBE::getRandomSubSample()
    {
        int val = _subSampling * 1.0 * rand() / RAND_MAX;
        if( val == _subSampling )
            return val - 1;
        else
            return val;
    
    }
    
    int VIBE::getRandomNeightXCoordinate( int x )
    {
        int val = 4 * 1.0 * rand() / RAND_MAX - 2;
        if( x + val >= _frameWidth || x + val < 0 )
            return x;
        else
            return x + val;
    }
    
    int VIBE::getRandomNeightYCoordinate( int y )
    {
        int val = 4 * 1.0 * rand() / RAND_MAX - 2;
        if( y + val >= _frameWidth || y + val < 0 )
            return y;
        else
            return y + val;
    }

    调用这个VIBE这个类的方法:

    VIBE vibe;
        vibe.setFrameWidth(image->width);
        vibe.setFrameHeight(image->height);
        vibe.setFrameWidthStrip(segImage->widthStep);
        vibe.setSegmentMap( (unsigned char*)(segImage->imageData));
    
        vibe.setCurrentFrame((unsigned char*)grayImage->imageData);
        vibe.initialize();
        ……
        //更新
    vibe.setCurrentFrame( (unsigned char*)grayImage->imageData );
    vibe.update();

    不会用多看看类的源码吧。“不要怕难,多折腾!”,我导师的名言。

    局部搜索以及平滑处理
    设k为帧数,R为搜索半径,P(x,y)为在 (x,y)处的像素值。设第 帧的坐标为(xk,yk) ,可以得到第k+1 帧的位置为 :
    这里写图片描述
    平滑处理过程:
    设平滑窗口的宽度为W1,窗口中每一帧的权重为:
    原理为每一位局当前位置距离的平方,最远处为1^2,……,当前最大2^k
    这里写图片描述
    平滑结果:
    这里写图片描述

    效果展示:
    VIBE结果:
    这里写图片描述

    局部搜索:
    这里写图片描述

    跟踪结果:
    这里写图片描述

    如有疑问,可以联系我。
    等我的论文录用后,再深入讨论三维轨迹跟踪方法。

    展开全文
  • 不知道怎么保存STK卫星的二维轨迹,openjump不支持.sa格式。
  • 蚁群算法实现三避障轨迹规划(Matlab)

    千次阅读 热门讨论 2021-02-08 14:12:06
    无论是GA、SA、PSO、还是本文讲到的蚁群,应用在三维轨迹规划中都存在一个问题:如何将轨迹表述成一个解以及如何生成一个从起始点到终点的解。在我的A*算法三维规划博客中通过分解空间成类似点云的形式来表述解。而...

    关于蚁群算法

    其实大多数优化算法都是“试错”的过程,不同的是如何利用在“试错”过程中得到的经验。蚁群算法在“试错”的过程中通过留下信息素对未来的试错过程加以提示,从而保证一定的收敛性。

    代码分析

    我写了一份matlab蚁群算法三维避障规划的代码,能保证收敛,但不够优秀,可以作为基础加以改进。

    无论是GA、SA、PSO、还是本文讲到的蚁群,应用在三维轨迹规划中都存在一个问题:如何将轨迹表述成一个解以及如何生成一个从起始点到终点的解。在我的A*算法三维规划博客中通过分解空间成类似点云的形式来表述解。而在蚁群算法三维规划中,我采用的是对空间切片的方式(可能不够好,但是能用)。

    读取参数

    clc; clear; close all;
    %% 参数读取与设置
    obstacleMatrix = csvread("./data_csv/obstacleMatrix.csv");
    RobstacleMatrix = csvread("./data_csv/RobstacleMatrix.csv")';
    cylinderMatrix = csvread("./data_csv/cylinderMatrix.csv");
    cylinderRMatrix = csvread("./data_csv/cylinderRMatrix.csv")';
    cylinderHMatrix = csvread("./data_csv/cylinderHMatrix.csv")';
    start = csvread("./data_csv/start.csv")';
    goal = csvread("./data_csv/goal.csv")';
    

    没有csv文件可以直接写这几个matrix,它们表述的是障碍物的坐标和半径等信息,obstacleMatrix为球障碍物的圆心坐标矩阵(n*3矩阵),RobstacleMatrix为对应半径向量(n*1向量),cylinderMatrix为圆柱体中心坐标(n*2矩阵,没有第三个维度,从z=0开始绘制圆柱体),cylinderRMatrix为对应圆柱体半径(n*1向量),cylinderHMatrix为对应圆柱体的高(n*1向量)。

    设置参数

    popNumber = 50;  % 蚁群个体数量
    rou = 0.1;        % 挥发因子
    bestFitness = []; % 每一代的最佳适应值储存列表
    bestfitness = inf;% 初始化最佳适应值(本案例中越小越好)
    everyIterFitness = [];
    deltaX = 0.2; deltaY = 0.2; deltaZ = 0.2;
    gridXNumber = floor(abs(goal(1) - start(1)) / deltaX);
    gridYNumber = 50; gridZNumber = 50;
    ybegin = start(2) - 20*deltaY; zbegin = start(3) - 20*deltaZ;
    pheromone = ones(gridXNumber, gridYNumber, gridZNumber);
    ycMax = 3; zcMax = 3; % 蚂蚁沿y轴最大变动格子数和沿z轴最大变动格子数
    bestPath = []; 
    iterMax = 150; 
    

    其中deltaX为对x轴的切片步长:
    在这里插入图片描述
    deltaY和deltaZ为每一个切片下对Y轴和Z轴的网格划分步长。ybegin和zbegin为y和z的起始参考值,在信息素矩阵pheromone中利用y和z分别减去ybegin和zbegin再分别处以deltaY和deltaZ就是索引值了。相当于对空间离散坐标进行编码。gridXNumber为横向切片的数量。

    绘制障碍物环境

    %% 绘制障碍环境
    figure(1)
    [n,~] = size(obstacleMatrix);
    for i = 1:n   %绘制静态球障碍物
        [x,y,z] = sphere();
        surfc(RobstacleMatrix(i)*x+obstacleMatrix(i,1),...
            RobstacleMatrix(i)*y+obstacleMatrix(i,2),...
            RobstacleMatrix(i)*z+obstacleMatrix(i,3));
        hold on;
    end
    
    [n,~] = size(cylinderMatrix);
    for i = 1:n   %绘制圆柱体障碍物
        [x,y,z] = cylinder(cylinderRMatrix(i));
        z(2,:) = cylinderHMatrix(i);
        surfc(x + cylinderMatrix(i,1),y + cylinderMatrix(i,2),...
              z,'FaceColor','interp');
        hold on;
    end
    
    bar1 = scatter3(start(1),start(2),start(3),80,"cyan",'filled','o','MarkerEdgeColor','k');hold on
    bar2 = scatter3(goal(1),goal(2),goal(3),80,"magenta",'filled',"o",'MarkerEdgeColor','k');
    text(start(1),start(2),start(3),'   起点'); text(goal(1),goal(2),goal(3),'   终点');
    axis equal
    set(gcf,'unit','centimeters','position',[30 10 20 15]);
    

    主循环

    for iter = 1:iterMax
        fprintf("程序已运行:%.2f%%\n",iter/iterMax*100);
        % 路径搜索
        [path, pheromone] = searchPath(popNumber, pheromone, start, goal, ycMax, zcMax,...
                                       deltaX, deltaY, deltaZ, obstacleMatrix,RobstacleMatrix,...
                                       cylinderMatrix, cylinderRMatrix, cylinderHMatrix,...
                                       ybegin, zbegin, gridYNumber, gridZNumber);
        % 路径适应值计算
        fitness = calFit(path, deltaX, start, goal);
        [newBestFitness, bestIndex] = min(fitness);
        everyIterFitness = [everyIterFitness, newBestFitness];
        if newBestFitness < bestfitness
            bestfitness = newBestFitness;
            bestPath = path(bestIndex, :, :);
        end
        bestFitness = [bestFitness, bestfitness];
        % 更新信息素
        cfit = 100 / bestfitness;
        iterNum = 0;
        for x = start(1) + deltaX : deltaX : goal(1) - 0.001
            iterNum = iterNum + 1;
            pheromone(iterNum, round((bestPath(:,iterNum+1,1)-ybegin)/deltaY), round((bestPath(:,iterNum+1,2)-zbegin)/deltaZ))...
            = (1 - rou) * pheromone(iterNum, round((bestPath(:,iterNum+1,1)-ybegin)/deltaY), round((bestPath(:,iterNum+1,2)-zbegin)/deltaZ)) + cfit;
        end
    end
    

    这里的searchPath为每一个迭代过程中每一只蚂蚁搜索出一条从起点到终点的路径。之后对这些蚂蚁的路径计算适应值(长度衡量),选出适应值最低的和历史最低比较从而更新历史最优解。最后是更新信息素,这里我剑走偏锋只用每一代最优路径来更新信息素而没有将所有蚂蚁的路径用来更新,后者我试了下在这个例子上不太好使容易不收敛。
    信息素更新方式我采用的是:
    I=(1rou)I+c/fitI = (1-rou)I + c/fit
    rou是衰减因子,fit是路径适应值,c是一个可调因子。由于我的fit是路径长度,路径越短,信息素增加得越多。

    路径搜索函数searchPath

    function [path, pheromone] = searchPath(popNumber, pheromone, start, goal, ycMax, zcMax,...
                                            deltaX, deltaY, deltaZ, obstacleMatrix, RobstacleMatrix,...
                                            cylinderMatrix, cylinderRMatrix, cylinderHMatrix,...
                                            ybegin, zbegin, gridYNumber, gridZNumber)
    % 获取从起点到终点的路径函数
    path = []; % 用于记录所有蚂蚁的路径
    for ant = 1:popNumber % 对于每一只蚂蚁
        path(ant, 1, 1:2) = start(2:3); % 只记录y和z轴坐标,x轴每次加deltaX
        nowPoint = start(2:3);
        iterNum = 0;
        for x = start(1) + deltaX : deltaX : goal(1) - 0.001 % 减去一个小数避免x直接取到goal(1)
            iterNum = iterNum + 1;
            nextPoint = [];
            p = [];   
            for y = -ycMax * deltaY : deltaY : ycMax * deltaY
                for z = -zcMax * deltaZ : deltaZ : zcMax * deltaZ
                    nextPoint = [nextPoint; nowPoint + [y, z]];
                    if nextPoint(end,1) > ybegin+0.01 && nextPoint(end,1) < ybegin + gridYNumber*deltaY && ...
                       nextPoint(end,2) > zbegin+0.01 && nextPoint(end,2) < zbegin + gridZNumber*deltaZ  % 判断是否越界(信息素矩阵大小已经定了,避免超出)
                        hValue = calHeuristicValue(nowPoint, nextPoint(end,:), goal, x, deltaX, obstacleMatrix,...
                                                   RobstacleMatrix, cylinderMatrix, cylinderRMatrix, cylinderHMatrix);
    %                     pher = pheromone(iterNum, round((nextPoint(end,1) - ybegin)/deltaY), round((nextPoint(end,2) - zbegin)/deltaZ));
                        try
                            pher = pheromone(iterNum, round((nextPoint(end,1) - ybegin)/deltaY), round((nextPoint(end,2) - zbegin)/deltaZ));
                        catch
                            round((nextPoint(end,1) - ybegin)/deltaY)
                        end
                        p = [p, pher * hValue];
                    else
                        p = [p,0]; %置零在轮盘赌中不可能被选中
                    end
                end
            end
            % 轮盘赌选择下一坐标点
            p1 = p / sum(p); % 归一化
            pc = cumsum(p1);
            targetIndex = find(pc >= rand);
            targetNextPoint = nextPoint(targetIndex(1),:);
            path(ant, iterNum + 1, 1:2) = targetNextPoint;
            nowPoint = targetNextPoint;
        end
        path(ant, iterNum + 2, 1:2) = goal(2:3);
    end
    end
    

    这里的大体思路为:已知切片i上一点的坐标,获取切片i+1上的坐标。而切片i+1上的横坐标已经定了即为第i切片的横坐标加上deltaX,纵坐标y和z坐标允许在第i切片的y和z的基础上做偏移,即正负ycMax*deltaY和正负zcMax*deltaZ。对每一个可行的坐标计算其启发值(使用calHeuristicValue函数),得到hValue并乘以该坐标对应的信息素从而得到一个值,值越大越容易被选中。最后使用轮盘赌的方式选取第i+1切片上的坐标点。这里需要注意的是避障检测在计算启发值函数calHeuristicValue函数中使用,如果坐标与障碍物碰撞则将启发值置为0,那么它乘以信息素也是0,在轮盘赌算法中0是不可能被选取的。因此与障碍物有交点的坐标是不可能被选中的。

    对每一只蚂蚁采用上述思路都能找到一条路径,放入三维矩阵path中返回。path第一个维度表示是哪一只蚂蚁,第二个维度表示这只蚂蚁路径上的第几个切片,第三个维度表示这一切片上的y和z。

    计算启发值函数calHeuristicValue

    function h = calHeuristicValue(now, next, goal, x, deltaX, obstacleMatrix, RobstacleMatrix,...
                                   cylinderMatrix, cylinderRMatrix, cylinderHMatrix)
    % 判断下一个坐标点是否碰撞,若碰撞则将启发值置为0,在后续的轮盘赌点位选择时将不可能被选中
    nextXYZ = [x, next];
    flag = checkCol(nextXYZ, obstacleMatrix, RobstacleMatrix, cylinderMatrix, cylinderRMatrix, cylinderHMatrix);
    % 计算启发值
    d1 = getDist([x - deltaX, now], [x, next]);
    d2 = getDist([x, next], goal);
    D = 50 / (d1 + d2);
    h = flag * D;
    end
    

    其中checkCol为碰撞检测,getDist为获取欧式几何距离函数:

    function flag = checkCol(pos, circleCenter,circleR, cylinderCenter,cylinderR, cylinderH)
    % 碰撞检测函数
    [numberOfSphere, ~] = size(circleCenter);
    [numberOfCylinder, ~] = size(cylinderCenter);
    flag = true;
    for i = 1:numberOfSphere
        if getDist(pos, circleCenter(i,:)) <= circleR(i)
            flag = false;
            break;
        end
    end
    for i = 1:numberOfCylinder
        if getDist(pos(1:2), cylinderCenter(i,:)) <= cylinderR(i) && pos(3) <= cylinderH(i)
            flag = false;
            break;
        end
    end
    if pos(3) <= 0, flag = false; end
    end
    
    function d = getDist(x,y)
    d = sqrt(sum((x - y).^2));
    end
    

    计算适应值函数calFit

    function f = calFit(path, deltaX, start, goal)
    % 计算适应值函数
    [n,m,~] = size(path);
    x = [start(1) : deltaX : goal(1) - 0.001, goal(1)];
    for i = 1:n
        f(i) = 0;
        for j = 1:m-1
            f(i) = f(i) + getDist([x(j), path(i,j,1), path(i,j,2)], [x(j+1), path(i,j+1,1), path(i,j+1,2)]);
        end
    end
    end
    

    采用的是以路径长度作为适应值,即适应值越小越好,返回每个蚂蚁路径的适应值,用于后续搜索最小适应值和最优路径以及更新信息素。

    绘制最佳路径以及适应值变化图

    % 绘制路径
    x = [start(1):deltaX:goal(1)-0.001,goal(1)];
    [~,m] = size(x);
    path_ = [];
    for i = 1:m
        path_ = [path_;bestPath(:,i,1),bestPath(:,i,2)];
    end
    bar3 = plot3(x, path_(:,1), path_(:,2),'LineWidth',2,'MarkerSize',7,'Color','r');
    legend([bar1,bar2,bar3],["起始点","终止点","无人机航迹"])
    % 绘制适应值变化图
    figure(2)
    plot(bestFitness,'LineWidth',2,'Color','r'); hold on;
    plot(everyIterFitness,'LineWidth',2,'Color','b')
    legend('历史最佳个体适应度','每代最佳个体适应度')
    title('适应度变化趋势'); xlabel('迭代次数'); ylabel('适应度值'); grid on;
    

    结果

    在这里插入图片描述

    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述

    the future

    感觉我写的代码效率有点低,运行得比较慢,另外切片的这种方式让轨迹不够平滑。这个代码有很多超参数需要调…不想调了…

    展开全文
  • 机器人轨迹规划

    万次阅读 2016-11-08 20:21:14
    轨迹规划的目的:生成运动控制系统的参考输入,以确保机械手完成规划的轨迹。路径和轨迹
  • Minimum Snap轨迹规划详解(1)轨迹规划入门

    万次阅读 多人点赞 2017-07-25 22:03:32
    1. 轨迹规划是什么?在机器人导航过程中,如何控制机器人从A点移动到B点,通常称之为运动规划。运动规划一般又分为两步: 路径规划:在地图(栅格地图、四\八叉树、RRT地图等)中搜索一条从A点到B点的路径,由一系列...
  • 轨迹规划是什么? 在机器人导航过程中,如何控制机器人从A点移动到B点,通常称之为运动规划。运动规划一般又分为两步: 1、路径规划:在地图(栅格地图、四\八叉树、RRT地图等)中搜索一条从A点到B点的路径,由一...
  • 文章目录一、实验目的和要求1.1 目的1.2 要求、实验手段三、轨迹规划的推导过程(7次多项式)3.1 边界条件3.2 连续条件3.3 七次多项式推导四、仿真结果与比较分析4.1 转移过程点4.2 求解四个位置的关节角4.3 求解...
  • 文章目录1 无人驾驶的动作规划2 轨迹规划2.1 车辆模型2.2 道路定义2.3 候选轨迹生成2.4 根据轨迹点构建有向图并搜索3 速度规划3.1 S-T图算法3.2 设置Cost 1 无人驾驶的动作规划 动作规划的目的:是将上游行为决策...
  • 机器人轨迹规划:三次样条曲线

    千次阅读 多人点赞 2020-05-05 22:47:25
    机器人轨迹规划:三次样条曲线 写在前面 在一些避障的应用场景下,一般都是先在任务空间中对多轴机械臂的末端进行路径规划,得到的是末端的运动路径点数据。这条轨迹只包含位置关系,并没有告诉机器人应该以怎样的...
  • Apollo轨迹规划技术分享 轨迹规划(Trajectories Planning)主要指考虑实际临时或者移动障碍物,考虑速度,动力学约束的情况下,尽量按照规划路径进行轨迹规划轨迹规划的核心就是要解决车辆该怎么走的问题。轨迹...
  • Apollo 轨迹规划技术分享

    千次阅读 2019-06-19 23:16:16
    轨迹规划(Trajectories Planning)主要指考虑实际临时或者移动障碍物,考虑速度,动力学约束的情况下,尽量按照规划路径进行轨迹规划轨迹规划的核心就是要解决车辆该怎么走的问题。轨迹规划的输入包括拓扑地图,...
  • 前言 问题由来 最近遇到了一类图,非常好看,并且在其他论文中也多次遇到。比如,在 Trajectory data-based traffic flow... 每条轨迹线均表示一辆车的行驶路径变化,而线条的颜色则表示瞬时速度值 因此,如果想要绘.
  • 机器人笛卡尔空间坐标系轨迹规划的研究

    万次阅读 多人点赞 2019-08-18 08:15:50
    1引言 随着6R机械人的使用性越来越广,对机械人的轨迹...本文将在对机器人完成建模以及运动学解算后的基础上,对机器人进行笛卡尔空间轨迹规划的研究,主要分为直线轨迹规划与圆弧轨迹规划。 2任务位置规划 ...
  • 多机器人轨迹规划(1) 学习笔记(1) 文章基本是一些文献知识与CSDN中找的一些例子 单个机器人轨迹规划 目前机器人的轨迹规划都是通过“示教-编程-再现”方式来实现的,一般的轨迹分为直线轨迹,圆弧轨迹以及复杂的...
  • 基于四阶贝塞尔曲线的无人驾驶可行轨迹规划 背景 对于实际的无人车系统来说, 轨迹规划需要保证其规划出来的轨迹满足运动学约束、侧滑约束以及执行机构约束。为了生成满足无人车初始状态约束、目标状态约束的局部可行...
  • 2、光滑轨迹生成一、光滑轨迹的生成、使用步骤1.引入库2.读入数据总结 简介 1、为什么需要光滑轨迹? 为了更好地自主移动。 速度不能立即改变。 机器人不应该在转弯时停止。 节省能量,经常地加减速会减少能量...
  • 机械臂建模,轨迹规划,避障路径规划(介绍+代码)() 已经分别介绍完机械臂建模,机械臂轨迹规划两个部分,而这一部分则是介绍机械臂避障路径规划部分,主要分为以下几个部分:使用蚁群算法的路径规划
  • C语言实现三自由度机械臂轨迹规划源程序,输入为空间三坐标,输出为相应电机需要旋转的角度
  • Ceres入门---Ceres优化三维轨迹

    千次阅读 2019-05-22 11:30:03
    一直以来滤波算法被运用于轨迹融合,但是研究表明,基于最小二乘的非线性优化方案,有更好的表现。本文基于谷歌开发的Ceres Solver非线性最小二乘优化包,进行轨迹优化。 2.场景介绍 为了演示的方便,我们抛弃...
  • 轨迹规划属于机器人学中的上层问题,其主要目标是计划机器人从A移动到B并避开所有障碍的路线。 1、轨迹计划的对象  轨迹规划的对象是map,机器人通过SLAM获得地map后,则可在地图中选定任意两点进行轨迹规划。...
  • (一)三次多项式轨迹规划

    千次阅读 2019-09-29 16:37:04
    轨迹规划中的三次多项式 选择满足要求的运动学性质的物理量如,如:位移、速度和加速度其实就完成了一个轨迹的规划。以单轴关节角$x(t)$为例,容易得: x(t)=c_0+c_1t+c_2t^2+c_3t^3 v(t)=c_1t+2c_2t+3c_3t^2 ...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 17,983
精华内容 7,193
关键字:

二维轨迹规划