前言：本博客将结合常用的路径规划算法进行matlab讲解。
首先我们需要明确路径规划的思想就是：假设有一个机器人，在模拟环境下按照你设定的条件和规划算法进行循迹，直到找到一条可行的路径。此时机器人经过的路线就是你规划的路径。所以下面为什么会有求机器人参数，求机器人顶点坐标等操作。
一、路径规划问题所需操作
1.生成地图
告诉电脑你的当前地图环境。比如一个迷宫。这里面就涉及一些图像处理的内容，以及如何把地图转换为matlab数据
例子：
有一张原始地图：
现在要将这图片导入matlab中，转换为matlab数据：
x=imread('zhousan.png');
此时生成一个3维矩阵x，格式为372 * 494 * 3。其中372 * 494代表这张图每个像素点的坐标，3表示每个像素点的颜色。注意：彩色图为三维矩阵，而黑白图为由0，1构成的二维矩阵，0为黑，1为白色。因此，为方便处理，接下来需要将图片变为黑白，也就是二值化处理。函数为：
map=im2bw(x,0.9); //其中x为读入的三维图像数据，0.9为阈值，调整0.9可以改变黑白的效果。此时生成二维黑白图像数据map，格式为372 * 494
此时用imshow(map)函数展示效果效果为：
注意：imshow();函数用于将图像矩阵数据显示出图片来，里面可以为三维矩阵，也可以为二维矩阵
接下来，我们希望海洋部分为白色，陆地为黑色，因为方便在海上画线，所以我们要将黑白对调。前面我们提过map数据里面为0，1，0为黑，1为白色。所以我们只需将0.1取逻辑反即可，即map=~map;
然后再用imshow(map)函数展示效果效果为：
所以第一步图像处理部分代码为：
clc;
close all;
clear all;
x=imread('zhousan.png');
map=im2bw(x,0.9);
//figure(1);
//imshow(map);
map=~map;
//figure(2);
imshow(map);
2.相关参数设置
设置好地图后，需要设置相关参数：
1.机器人中心的起始点，终点的像素坐标，保存在数组里，坐标可以在figure里面确定
注意：图的x为横起的，y为竖起的，所以对应图的数组，点在图中的坐标（x，y）对应数组中的坐标为（y，x）
source = [50 50];
goal = [450,450];
2.机器人初始方向 //机头朝向和x夹角
3.机器人的尺寸（长宽）
4.机器人运动速度
5.最大速度
6.距离障碍物的最小距离
7.最大加速度
8.最大转弯角度
9.实时位置
10.实时的方向
11.判断机器人是否到达终点的最小距离，即当机器人与终点距离小于某值时我们认为到达该点
。。。。。。。。。。
注意：这些参数需要你根据应用选择那些以及添加哪些
3.可行域判断
这一部分用于判断机器人是否在可行域内，也就是机器人是否在空地内，是否与障碍物外，是否在地图内？所以这就涉及判断一个点是否在可行域内。当然，如果把机器人当作一个点，只需要判断这个点是否在可行域内即可判断机器人是否在可行域内，可是如果考虑了机器人的尺寸，则需要判断机器人每个顶点是否在可行域内来判断机器人是否在可行域内。因此我们接下来需要写一个子函数来判断一个点是否在可行域内，
思路：判断一个点是否在可行域内，只许看该点的横坐标是否在1到map的y_size,该点的纵坐标是否在1到map的x_size,以及该点对应的map坐标的值是否为空地？
下面代码展示如何判断一个点是否在可行域内：
clc;
close all;
clear all;
x=imread('zhousan.png');
map=im2bw(x,0.9);
map=~map;
imshow(map);
point=[108 314];
a=judge(point,map);
if a==1
disp('inside')
end
if a==0
disp('outside')
end
function bool_inside = judge(point,map)
[x_size,y_size]= size(map);
bool_inside = 0;
if point(1)>1 && point(1)<y_size && point(2)>1 && point(2)<x_size && map(point(2),point(1))== 1
bool_inside = 1;
end
end
4.求顶点坐标
前面我们在设置参数时设置了机器人的中心坐标，尺寸（边长），机器人的方向（机头朝向和x夹角），因此我们需要求出四个顶点的坐标以用来判断四个顶点是否在可行域内。
求法见图：
所以下面那个点的横坐标为：x1=x+rsin(pi/4 - sita)
纵坐标为：y1=y+rcos(pi/4 - sita)
同理可求其他几个点坐标
注意：通过上面公式计算出来的坐标可能不为整数，但是带入map矩阵的横纵坐标必须为整数，所以求出的四个点的坐标要取整，即用x1=int16（x1）和y1=int16（y1）
5.画出机器人的轮廓
这里以正方形机器人为例，因为图中的坐标可以不为整数，所以不用像带入map矩阵时要对四个点的坐标要取整，这里直接用原始四个点坐标。画的时候直接用语句
line([x1,x2,x3,x4,x1],[y1,y2,y3,y4,y1])
注意：line用法和plot一样，区别在于如果想在一张图上继续画，plot需要用hold on语句，而line不用，line默认在前一张图上继续画。另外这里由于要画出正方轮廓，所以需要五个点，最后一个点坐标为起始点坐标。因为不管是plot还是line’都是将点按顺序连线，所以要想画闭合图，最后一个点要在起始点位置
例子：
clc;
close all;
clear all;
x=imread('zhousan.png');
map=im2bw(x,0.9);
map=~map;
imshow(map);
point1=[108 314];
point2=[110 314];
point3=[110 316];
point4=[108 316];
line([108 110 110 108 108],[314 314 316 316 314]);
6.计算机器人各个方向距离障碍物的距离
有些算法需要计算这个，方向包括5个：前方、作坊、右方、右前45度，左前45度。求得方法就是：
比如求前方时，先计算前方那个点的坐标，然后朝着该方向每次移动一个单位，看该点是否撞上，没撞上继续迭代，撞上就将运动的距离当为这个方向的距离。求左前时，先计算左前方那个顶点点的坐标，然后朝着该方向每次移动一个单位，看该点是否撞上，没撞上继续迭代，撞上就将运动的距离当为这个方向的距离。同理计算其他方向的。
7.计算机器人此时与终点的距离和方向
公式很简单，一个平方和开根号，一个用反正切。
8.一些与路径规划算法有关的操作
这一部分就跟你所用的算法有关，不同的算法有不同的另外的操作，下面将介绍一些常用的路径规划算法。
二、路径规划常用算法——人工势场法
1.思想
终点会产生一个引力场，障碍物会产生一个斥力场，机器人所在位置的总的势能 = 引力势能 - 排斥势能。我们根据机器人所在位置的总势能确定该点的转弯角（也就是要改变路径方向了，即此时改变的角度为多少）、转弯后的速度等等参数，根据这些参数我们确定下一个路径拐点。到达下一个路径拐点后在重复上面的步骤。
2.如何计算势能
排斥势能与距离平方成反比，引力势能与距离平方成正比。
排斥势能：（1/该方向距离障碍物的距离）^k *放大因子
其中k为次方数，越高越精确，但是会加大计算难度，一般取3左右的数，可以为小数，有时也要去不断去试，开最终的效果来决定最后取多少
总的排斥势能就为5个方向的排斥势能之和，由于势能是矢量，所以计算每个方向的排斥势能时都要分解出x和y方向的，然后再将5个方向的排斥势的x、y方向的值分别相加，这样最后就形成了一个1*2的数组，分别为x方向的和y方向的排斥势能
引力势能：（该点距离终点的距离）^k * 缩小因子
注意：因为引力势能比排斥势能大，所以需要乘一个缩小因子，比如0.000001.所以我们需要将两个势能处于一个量级，这就涉及到放大因子和 缩小因子的调参。同样，引力势能要分解出x和y方向的，这样最后就形成了一个1*2的数组，分别为x方向的和y方向的引力势能
总的势能 = 引力势能 - 排斥势能
同样，总的势能也为1*2的数组
3.如何计算路径拐角
拐角=atan2（机器人此时的速度在y上的分量+y方向总势能 ，机器人此时的速度在x上的分量+x方向总势能 ）- 机器人的此时朝向
注意：计算出来的拐角需要在-pi到+pi之间，所以需要+/-2pi操作。设置完后还要小于最开始设定的最大拐角，如果大了，就设为最大拐角值。即：>最大转角时，取最大拐角，<(-最大转角时)，取(-最大拐角)
所以此时新的方向为：原来的方向+拐角
4.如何计算下一个速度
新的速度 ^2 = （x的速度+x的总势能)^2 + ( y的速度+y的总势能) ^2
再开根号就是新的速度。
需要注意：这里面公式中x方向的速度y方向的速度和上面第三小节中的x方向的速度y方向的速度不一样，虽然速度大小一致，但上面的速度的方向还没有经过改变，而这里的速度方向改变了，所以这里的x方向的速度y方向的速度要用新的方向来算。
但是要注意，最开始我们设定了机器人的最大速度和最大加速度，所以算出来的速度首先要小于最大速度，也就是如果算出来的速度大于最大速度，取最大速度值，如果算出来的速度小于最大速度，取算出来的速度。然后，在处理完速度后，再看处理过的速度值与此时速度的差值的绝对值也就是加速度是否大于规定的最大加速度，如果大了就取加u速度值为最大加u速度值，但要注意正反，然后再算速度。
所以新的位置就为 ：（此时的x位置坐标+算出来的x方向速度 * t ，此时的y位置坐标+算出来的y方向速度*t），时间t设为1.
然后判断新的位置是否在可行域内，如果在就把此事的机器人画出来。
5.程序流程
先载入地图，再设定相关的参数，然后进入一个大循环，大循环的终止条件是机器人与终点距离小于定值。在每一次循环内，计算势能，计算拐角，计算速度，然后求出下一位置，最后判断位置是否在可行域内，在就吧机器人画出来，不在就终止程序。
小提示：如果想要最后得到一个动画效果，即看到小机器人的移动动作，只需要在每一次执行line函数后写上
m(i)=getframe;
i=i+1;
这个函数的意思是把每一次花的图取做一帧保存在m矩阵中，通俗的理解就是把每一次画的图按顺序展现出来，而不是一次性展示完
6.完整代码
由于代码组成太多，已上传csdn，也放在百度云了，自取。
效果：
