精华内容
下载资源
问答
  • 用于一维、二维、三维轨迹平滑(对需要平滑的轴使用该函数即可) 公式 以5点MA平滑为例: ys(i)=12N+1(y(i+N)+y(i+N−1)+…+y(i−N))ys(1)=y(1)ys(2)=(y(1)+y(2)+y(3))/3ys(3)=(y(1)+y(2)+y(3)+y(4)+y(5))/5ys(4)=(y...

    应用

    用于一维、二维、三维轨迹平滑(对需要平滑的轴使用该函数即可)

    公式

    以5点MA平滑为例:
    y s ( i ) = 1 2 N + 1 ( y ( i + N ) + y ( i + N − 1 ) + … + y ( i − N ) ) y s ( 1 ) = y ( 1 ) y s ( 2 ) = ( y ( 1 ) + y ( 2 ) + y ( 3 ) ) / 3 y s ( 3 ) = ( y ( 1 ) + y ( 2 ) + y ( 3 ) + y ( 4 ) + y ( 5 ) ) / 5 y s ( 4 ) = ( y ( 2 ) + y ( 3 ) + y ( 4 ) + y ( 5 ) + y ( 6 ) ) / 5 \begin{array}{l} y_{s}(i)=\frac{1}{2 N+1}(y(i+N)+y(i+N-1)+\ldots+y(i-N)) \\ y_{s}(1)=y(1) \\ y_{s}(2)=(y(1)+y(2)+y(3)) / 3 \\ y_{s}(3)=(y(1)+y(2)+y(3)+y(4)+y(5)) / 5 \\ y_{s}(4)=(y(2)+y(3)+y(4)+y(5)+y(6)) / 5 \end{array} ys(i)=2N+11(y(i+N)+y(i+N1)++y(iN))ys(1)=y(1)ys(2)=(y(1)+y(2)+y(3))/3ys(3)=(y(1)+y(2)+y(3)+y(4)+y(5))/5ys(4)=(y(2)+y(3)+y(4)+y(5)+y(6))/5

    MATLAB函数

    function res = MovingAverage(input,N)
    %% input为平滑前序列(列向量和行向量均可);N为平滑点数(奇数);res返回平滑后的序列(默认行向量)。
    sz = max(size(input));
    n = (N-1)/2;
    res = [];
    for i = 1:length(input)
        if i <= n
            res(i) = sum(input(1:2*i-1))/(2*i-1);
        elseif i < length(input)-n+1
            res(i) = sum(input(i-n:i+n))/(2*n+1);
        else
            temp = length(input)-i+1;
            res(i) = sum(input(end-(2*temp-1)+1:end))/(2*temp-1);
        end
    end
    end
    

    input为平滑前序列(列向量和行向量均可);N为平滑点数(奇数);res返回平滑后的序列。

    一个例子

    clear;clc;close all;
    arr = [1,2,4,3,6,5,7,7,9,10];
    plot(1:10,arr,'LineWidth',3,'color','r');hold on;
    arr_MA = MovingAverage(arr, 5);
    plot(1:10,arr_MA,'LineWidth',3,'color','b')
    legend("平滑前","平滑后");grid on;
    

    在这里插入图片描述

    展开全文
  • 轨迹平滑方法(权重渐变)

    千次阅读 2020-03-19 10:41:21
    clear all; close all; path=[0 0; 1 0; 2 0; 3 0 3 1 3 2 3 3 3 4 4 4 5 4 6 4]; figure for i = 0.1:0.1:0.5 for j = 0.05:0.05:0.2 alp...

    在这里插入图片描述

    clear all;
    close all;
    
    path=[0 0;
          1 0;
          2 0;
          3 0
          3 1
          3 2
          3 3
          3 4
          4 4
          5 4
          6 4];
    
    figure
    for i = 0.1:0.1:0.5
        for j = 0.05:0.05:0.2
        alpha = i;
        beta = j;
        optPath=PathSmoothing(path,alpha, beta);
       
        plot(path(:,1),path(:,2),'*b');hold on;
        plot(optPath(:,1),optPath(:,2),'-or');hold on;
        axis([-1 7 -2 6])
        legend('Before','After');
        title('Path Smoothing');
        grid on;
        hold on;
        end
    end
    hold off
    
    
    function optPath=PathSmoothing(path,alpha, beta)
    optPath=path;%
    
    
    
    torelance=0.00001;
    change=torelance;
    while abs(change)>=torelance 
        change=0;
        for ip=2:(length(path(:,1))-1) 
            prePath=optPath(ip,:);
            optPath(ip,:)=optPath(ip,:)-alpha*(optPath(ip,:)-path(ip,:));
            optPath(ip,:)=optPath(ip,:)-beta*(2*optPath(ip,:)-optPath(ip-1,:)-optPath(ip+1,:));
            change=change+norm(optPath(ip,:)-prePath);
        end
    end
    end
    
    

    在这里插入图片描述

    展开全文
  • 针对多目标跟踪中的固定间隔平滑问题,将势概率假设密度(CPHD)滤波器和RTS平滑器相结合,提出了RTS的势概率假设密度滤波平滑算法。考虑到在平滑过程中存在较大的输出延迟问题,采用分段思想,提出了分段RTS的势...
  • 地图轨迹平滑算法

    万次阅读 2016-08-04 00:21:34
    地图轨迹平滑算法,Savitzky-Golay 滤波器

    地图轨迹平滑算法

    引子:

    之前看到网上有大神写过一个demo:
    http://blog.csdn.net/zhoumushui/article/details/41751259
    这里写图片描述
    但是提供的代码不全,一直没有明白这个算法是怎么做的:
    这里写图片描述
    这里写图片描述

    算法说明:

    后来一次偶然的机会,看到这篇文章(注意,这篇文章里的代码,部分算法的参数是错的):
    http://blog.csdn.net/liyuanbhu/article/details/11119081
    这里写图片描述
    其中的5点平滑代码恰恰和之前描述的一样,原来之前的文章只给出了边缘4个点(最前面2个点、最后面2个点),恰恰漏掉了最关键的中间所有点、虽然只是简单的5点平均

    这个算法是Savitzky-Golay 滤波器的“5点1阶”算法。那么,怎么推导出“n点k阶”的系数呢?笔者没有时间去仔细研究算法本身,只想找到最快的方法解决问题,最后发现可以通过MatLab直接的得到这些参数,比如:
    这里写图片描述
    这个矩阵前2行对应前两个点的系数,后2行对应后两个点的系数,中间行对应中间所有点的系数。

    结论:这样就完成了,用MatLab 根据需要得到矩阵,然后就可以编程了!

    展开全文
  • 航迹规划——人工势场算法

    万次阅读 2018-03-21 17:35:21
    人工势场算法简介  人工势场法路径规划是由Khatib提出的一种虚拟力法(Oussama Khatib,Real-Time obstacle Avoidance for Manipulators and Mobile Robots. Proc of The 1994 IEEE.)。它的基本思想是将机器人在...

    人工势场算法简介


      人工势场法路径规划是由Khatib提出的一种虚拟力法(Oussama Khatib,Real-Time obstacle Avoidance for Manipulators and Mobile Robots. Proc of The 1994 IEEE.)。它的基本思想是将机器人在周围环境中的运动,设计成一种抽象的人造引力场中的运动,目标点对移动机器人产生“引力”,障碍物对移动机器人产生“斥力”,最后通过求合力来控制移动机器人的运动。应用势场法规划出来的路径一般是比较平滑并且安全,但是这种方法存在局部最优点问题。
      该算法在网上已经有较多的详细解释,在这里不做具体解释,主要讲解下Matlab下代码的编写。想了解算法原理的可以看最后的参考资料。


    人工势场算法缺陷

    1. 当物体离目标点比较远时,引力将变的特别大,相对较小的斥力在甚至可以忽略的情况下,物体路径上可能会碰到障碍物。
    2. 当物体离目标点比较远时,引力将变的特别大,相对较小的斥力在甚至可以忽略的情况下,物体路径上可能会碰到障碍物。
    3. 在某个点,引力和斥力刚好大小相等,方向想反,则物体容易陷入局部最优解或震荡。

    缺陷解决方法

    缺陷一:对于可能会碰到障碍物的问题,可以通过修正引力函数来解决,避免由于离目标点太远导致引力过大。

    (b)目标点附近有障碍物导致目标不可达的问题,引入一种新的斥力函数。
    这里写图片描述
    缺陷三:局部最优问题是一个人工势场法的一个大问题,这里可以通过加一个随机扰动,让物体跳出局部最优值。类似于梯度下降法局部最优值的解决方案。


    重点代码解析


    计算目标点和障碍物对智能体所在位置的角度


    输入意义
    X起点坐标
    Xsum目标和障碍的坐标向量,是(n+1)*2矩阵
    输出意义
    Y是引力,斥力与x轴的角度向量
    function Y=compute_angle(X,Xsum,n)%Y是引力,斥力与x轴的角度向量,X是起点坐标,Xsum是目标和障碍的坐标向量,是(n+1)*2矩阵
      for i=1:n+1%n是障碍数目
          deltaX(i)=Xsum(i,1)-X(1);
          deltaY(i)=Xsum(i,2)-X(2);
          r(i)=sqrt(deltaX(i)^2+deltaY(i)^2);
          if deltaX(i)>0
              theta=acos(deltaX(i)/r(i));
          else
              theta=pi-acos(deltaX(i)/r(i));
          end
          if i==1%表示是目标
              angle=theta;
          else
              angle=theta;
          end     
          Y(i)=angle;%保存每个角度在Y向量里面,第一个元素是与目标的角度,后面都是与障碍的角度
      end
    end

    目标点对智能体所在位置引力计算函数


    输入意义
    X当前坐标
    Xsum障碍的坐标
    k增益常数
    angle角度分量
    bnull
    Po障碍影响距离,大于Po时影响力为0
    n障碍物数量
    输出意义
    Yatx引力在智能体当前位置的 X 轴分量
    Yaty引力在智能体当前位置的 Y 轴分量
    function [Yatx,Yaty]=compute_Attract(X,Xsum,k,angle,b,Po,n)
    %把路径上的临时点作为每个时刻的Xgoal
    R=(X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2;%路径点和目标的距离平方
    r=sqrt(R);%路径点和目标的距离
    Yatx=k*r*cos(angle);%angle=Y(1)
    Yaty=k*r*sin(angle);
    end

    障碍物对智能体所在位置斥力计算函数


    function [Yrerxx,Yreryy,Yataxx,Yatayy]=compute_repulsion(X,Xsum,m,angle_at,angle_re,n,Po,a)%输入参数为当前坐标,Xsum是目标和障碍的坐标向量,增益常数,障碍,目标方向的角度
    Rat=(X(1)-Xsum(1,1))^2+(X(2)-Xsum(1,2))^2;%路径点和目标的距离平方
    rat=sqrt(Rat);%路径点和目标的距离
    for i=1:n
        Rrei(i)=(X(1)-Xsum(i+1,1))^2+(X(2)-Xsum(i+1,2))^2;%路径点和障碍的距离平方
        rre(i)=sqrt(Rrei(i));%路径点和障碍的距离保存在数组rrei中
        R0=(Xsum(1,1)-Xsum(i+1,1))^2+(Xsum(1,2)-Xsum(i+1,2))^2;
        r0=sqrt(R0);
        if rre(i)>Po%如果每个障碍和路径的距离大于障碍影响距离,斥力令为0
              Yrerx(i)=0;
              Yrery(i)=0;
              Yatax(i)=0;
              Yatay(i)=0;
        else
            %if r0<Po
            if rre(i)<Po/2
           Yrer(i)=m*(1/rre(i)-1/Po)*(1/Rrei(i))*(rat^a);%分解的Fre1向量
           Yata(i)=a*m*((1/rre(i)-1/Po)^2)*(rat^(1-a))/2;%分解的Fre2向量
           Yrerx(i)=(1+0.1)*Yrer(i)*cos(angle_re(i));%angle_re(i)=Y(i+1)
           Yrery(i)=-(1-0.1)*Yrer(i)*sin(angle_re(i));
           Yatax(i)=Yata(i)*cos(angle_at);%angle_at=Y(1)
           Yatay(i)=Yata(i)*sin(angle_at);
            else
           Yrer(i)=m*(1/rre(i)-1/Po)*1/Rrei(i)*Rat;%分解的Fre1向量
           Yata(i)=m*((1/rre(i)-1/Po)^2)*rat;%分解的Fre2向量
           Yrerx(i)=Yrer(i)*cos(angle_re(i));%angle_re(i)=Y(i+1)
           Yrery(i)=Yrer(i)*sin(angle_re(i));
           Yatax(i)=Yata(i)*cos(angle_at);%angle_at=Y(1)
           Yatay(i)=Yata(i)*sin(angle_at);
            end
       end%判断距离是否在障碍影响范围内
    end
       Yrerxx=sum(Yrerx);%叠加斥力的分量
       Yreryy=sum(Yrery);
       Yataxx=sum(Yatax);
       Yatayy=sum(Yatay);
    end

    完整Matlab代码下载链接


    人工势场算法完整“.m”文件下载

    参考资料


    1.路径规划-人工势场法
    2.机器人路径规划_人工势场法

    展开全文
  • 针对现有航迹规划算法缺乏同时具备快速性和最优性的...最后采用 K 航迹法进行航迹平滑处理,使得规划的航迹成为无人机的可飞航路。仿真结果表明,该算法能够在有效提高航迹产生速度的同时,可以得到近似最优的航迹。
  • 卡尔曼滤波与航迹融合

    千次阅读 2020-11-29 21:47:15
    卡尔曼滤波算法.总结 前言 这是我雷达信号处理课的一个作业,有些知识我也是新学的,原理也不是很懂. 所谓的航迹融合,就是将各传感器或者雷达扫描到的目标位置进行融合处理,使目标位置信息更加稳定可靠. 以下是本篇...
  • 参考文章:... python实现代码:以下为7点2次MA算法 实验结果: from matplotlib import pyplot as plt import numpy as np x = np.linspace(1, 30, 30) # X轴数据 # 原始数据 a = ...
  • 轨迹平滑方法

    万次阅读 多人点赞 2016-11-23 20:49:51
    主要介绍一下几种平滑方式,并针对三维轨迹进行平滑处理: 1. 滑动平均平滑(Moving average): 邻域内的数据点做平均代替邻域的中心点值,除了一般滑动平均,还有加权滑动平均和指数滑动平均。 2.Savitzky-...
  • 跟踪雷达航迹处理

    千次阅读 2021-05-20 01:07:17
    航迹起始是雷达航迹处理的重要问题,如果航迹起始都不正确,那就更加没有办法实现航迹的正确跟踪。航迹起始的主要任务是在安静或嘈杂环境下建立目标航迹;安静环境下的航迹起始更加容易完成,嘈杂环境下的航迹起始...
  • 利用拟合的航向信息对航迹平滑,提升海面目标航迹质量。在不同的测量噪声环境下,从插值精度与算法耗时方面仿真对比分析了最小二乘与拉格朗日插值方法的优缺点。推导存在测量噪声的基于AIS的三维空间配准算法,利用...
  • 摘要:针对室内定位的实际应用需求,提出了基于行人航迹推算算法(PDR)的适用于手机采集数据的室内定位方法。不同于传统的数据采集方法,该种定位方法利用手机得到加速度、陀螺仪以及地磁原始数据,通过分析加速度...
  • 路径规划、路径平滑与跟踪

    千次阅读 2020-11-22 15:15:55
    1、建立地图模型,包括障碍物与边界; 2、在自由区域内创建路网; 3、依据路网搜索最短安全路径–橙色...4、对路径进行平滑处理–绿色路径; 4、建立机器人运动学模型,运用purePursuit跟踪算法实现跟踪控制。 ...
  • 诸多核心算法技术的应用,保证了能有效规避低精度GPS定位结果对POS结果的影响。 工程实践表明,后处理POS中,厘米级定位精度的结果占结果总数的比例>95%。 2.姿态精度 实时组合导航:姿态误差一致性差,受实时状态...
  • AIS航迹校正

    2018-08-02 20:03:38
    本文提出利用对卡尔曼滤波算法进行适当的修改,引入系统噪声和测量噪声,利用AIS船舶观测节点数据对系统状态做最小二乘法估计,对船舶轨迹进行平滑和预测处理,能够比较正确地估计出船舶轨迹
  • 首先,提出一种定向A*算法,将无人机最大俯仰角与偏航角作为A*算法搜索约束,从而缩小节点扩展区域,并通过循环寻优规避“死区”点,进而产生平滑可飞的预规划航迹;其次,论述了补偿航程差的变步长多点搜索、三维盘旋机动...
  • 摘要:针对室内定位的实际应用需求,提出了基于行人航迹推算算法(PDR)的适用于手机采集数据的室内定位方法。不同于传统的数据采集方法,该种定位方法利用手机得到加速度、陀螺仪以及地磁原始数据,通过分析加速度...
  • 路径规划/路径平滑涉及方法汇总

    千次阅读 2020-10-12 12:39:41
    Hybrid A* 平滑过程3.1 轨迹平滑目标函数3.3 Voronoi field3.3.1 github repo[2]中使用方法3.3.2 Voronoi field的优化可能 1. 背景与参考文献 路径生成与路径平滑相关问题之前了解过,这次重新捡起来。起因是为了...
  • 目标跟踪算法综述

    千次阅读 2021-11-30 13:49:15
    目标跟踪算法从构建模型的角度可以分为生成式(generative)模型和判别式(discrimination)模型两类;从跟踪目标数量可分为单目标跟踪和多目标跟踪。目标跟踪融合了图像处理、机器学习、最优化等多个领域的理论和算法,...
  • 三、项目演示 [运动规划算法]基于飞行走廊的轨迹优化 参考 [1] F Gao, W Wu, Y Lin, S Shen,Online safe trajectory generation for quadrotors using fast marching method and bernstein basis polynomial
  • 联合检测和跟踪的MOT算法(一)

    千次阅读 2020-07-11 15:42:59
    一、联合检测和跟踪的MOT算法(one-shot) 参考: https://blog.csdn.net/c9Yv2cf9I06K2A9E/article/details/105320746 最近一年里,随着 Tracktor++ 这类集成检测和多目标跟踪算法框架的出现,涌现了很多相关的多...
  • 道格拉斯-普克算法是我们常用的一种轨迹点的抽稀算法,抽稀出来的点可以尽可能的维持原先轨迹点的大体轮廓,剔除一些非必要的点 2. 原理 假设在平面坐标系上有一条由N个坐标点组成的曲线,已设定一个阈值epsilon...
  • 轨迹聚类算法,先划分,后聚类,聚类算法是改进的DB-Scan;linux 编译,里面有Makefile
  • 最近读了很多文献,才知道任务规划包含了“任务分配”和“航迹规划”的,之前用词不当,还望见谅,哈哈 言归正传,目录如下: 目录 任务分配的模型要素和研究方法 航迹规划的模型要素和研究方法 总结 ...
  • k808巡航导弹航迹自动规划软件的设计与实现 航迹规划是指在特定条件下,自动生成巡航导弹从初始点到攻击目标点的最优运动轨迹,航迹规划是实现地形跟随,地形回避,威胁回避(TF/TA)飞行为目的的新一代低空突防...
  • 一、RRT算法简介 1 RRT定义 RRT(Rapidly-Exploring Random Tree)算法是一种基于采样的路径规划算法,常用于移动机器人路径规划,适合解决高维空间和复杂约束下的路径规划问题。基本思想是以产生随机点的方式通过一...
  • ros仿真 无人机航向控制算法优化 当前算法:有输入航向角速度时 就进行扭矩计算 ,当外界干扰时 不进行处理 算法优化思想 根据 航向角速度输入 决定机头是否锁定当前航向角度 锁定算法 思想, 期望角度 与实际...
  • 完整代码已上传我的资源:【三维路径规划】基于matlab RRT_Star算法三维路径规划【含Matlab源码 1571期】 备注: 订阅紫极神光博客付费专栏,可免费获得1份代码(有效期为订阅日起,三天内有效); 二、RRT算法简介 ...
  • 一、RRT算法简介 0 引言 随着现代技术的发展,飞行器种类不断变多,应用也日趋专一化、完善化,如专门用作植保的大疆PS-X625无人机,用作街景拍摄与监控巡察的宝鸡行翼航空科技的X8无人机,以及用作水下救援的白鲨...

空空如也

空空如也

1 2 3 4 5 ... 7
收藏数 130
精华内容 52
关键字:

航迹平滑算法

友情链接: 3.rar