前言：本博客将结合常用的路径规划算法进行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)1 && point(2)
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.计算机器人此时与终点的距离和方向
公式很简单，一个平方和开根号，一个用反正切。
转播
