利⽤matlab来进⾏路径规划,matlab路径规划系列
前⾔：本博客将结合常⽤的路径规划算法进⾏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如何用matlab将已知点连线
此时⽤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.计算机器⼈此时与终点的距离和⽅向
公式很简单，⼀个平⽅和开根号，⼀个⽤反正切。