应用
 
用于一维、二维、三维轨迹平滑（对需要平滑的轴使用该函数即可）
 
公式
 
以5点MA平滑为例：
 $$\begin{array}{l}{y}_s(i)=\frac{1}{2N+1}(y(i+N)+y(i+N-1)+\dots +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}$$
 
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;
 

 
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

 

地图轨迹平滑算法
 
引子：
 
之前看到网上有大神写过一个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 根据需要得到矩阵，然后就可以编程了！

人工势场算法简介
 
 
  人工势场法路径规划是由Khatib提出的一种虚拟力法（Oussama Khatib，Real-Time obstacle Avoidance for Manipulators and Mobile Robots. Proc of The 1994 IEEE.）。它的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。应用势场法规划出来的路径一般是比较平滑并且安全，但是这种方法存在局部最优点问题。 
   该算法在网上已经有较多的详细解释，在这里不做具体解释，主要讲解下Matlab下代码的编写。想了解算法原理的可以看最后的参考资料。
 
 
人工势场算法缺陷
 
当物体离目标点比较远时，引力将变的特别大，相对较小的斥力在甚至可以忽略的情况下，物体路径上可能会碰到障碍物。
当物体离目标点比较远时，引力将变的特别大，相对较小的斥力在甚至可以忽略的情况下，物体路径上可能会碰到障碍物。
在某个点，引力和斥力刚好大小相等，方向想反，则物体容易陷入局部最优解或震荡。
 
 
缺陷解决方法
 
缺陷一：对于可能会碰到障碍物的问题，可以通过修正引力函数来解决，避免由于离目标点太远导致引力过大。
 
（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
角度分量
b
null
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.机器人路径规划_人工势场法