如题,只给出了路径规划的目的,其余条件全靠脑补。
根据问题描述可以确定模型中的几个要素:
1. 起点,终点
起点和终点为地图中的两个顶点;在地图中使用绿色区域表示。
2. 障碍物位置,范围
障碍物将分为两种:
a. 可以通过的区域,圆形,但是通过会有一定代价,距离障碍物中心越近,代价越大;在地图中使用红色区域表示。
b. 不可通过的区域,矩形,该区域任何位置都不允许通过;在地图中使用灰色区域表示。
3. 地图范围
为了简化模型,地图范围为一个矩形区域,路径上的所有点必须在该区域内。
4. 路径
连接起点至终点的曲线;在地图中使用蓝色曲线表示。
模型地图如下图所示:
为了能使用优化算法对路径进行优化,该模型的输入应是路径,输出应该是一个数值,该数值将表示输入的路径的优劣。
为了简化问题,将路径上的关键点作为模型的输入,关键点之间的路径为该两个关键点的连线,路径如图2所示。
如图,圆形障碍的半径为R,关键点A距圆心O的距离AO=r<R,那么路径通过点A所需的代价可以使用如下公式计算:
其中a为该障碍物的代价系数,a>0,a的值越大,其经过该障碍的代价越大。
如上图5,由于步长较大,线段中有3个采样点,路径依然穿越了障碍区域,却没有计算代价,实际上,由于路径AB穿过了不可通过的障碍区域,其代价应为无穷大。
对于一个拥有k个圆形障碍的地图,若路径不穿过方形区域,由m个关键点组成,有n个采样点,其最终代价计算如下:
如上图6,在得出路径A-K1-K2-K3-K4-B后,我们可以依次去掉各关键点,计算其代价:即A-K2-K3-K4-B,A-K1-K3-K4-B,A-K1-K2-K4-B,A-K1-K2-K3-B这四条路径的代价,如果其中有一条路径的代价小于原路径,则可以将m-1后重新进行计算。
根据上面的模型,我们可以将模型代码划分为三块:地图以及两只障碍物。
路径是模型的输入,代价是模型的输出。
由于下面需要使用优化算法来对模型进行优化,之前优化算法matlab实现中,适应度函数的输入时一个向量,而这里模型的输入时一个关键点序列,故需要将关键点序列与向量相互转化:
如上公式(4),左边为m个二维点的坐标,右边为模型的输入序列,在计算代价时,需要将右边的序列转化为左边的点去计算,而在优化算法中则使用右边的序列去进行。
代码文件列表如下:
optimization algorithm\application_path_planning为该应用根目录
文件名 | 说明 |
---|---|
Obstacle_Rect.m | 矩形障碍区域。 |
Obstacle_Circle.m | 圆形障碍区域。 |
Map_Model.m | 地图模型。 |
其实现如下:
optimization algorithm\application_path_planning\Obstacle_Rect.m
矩形区域:判断是否有点在该区域内
optimization algorithm\application_path_planning\Obstacle_Circle.m
圆形区域:判断点与圆心的距离,计算代价
optimization algorithm\application_path_planning\Map_Model.m
地图模型:
1.设定障碍区域,地图边界
2.关键点序列与向量转换
3.根据关键点和步长得到采样点
4.根据关键点,采样点计算代价
5.绘制地图和路径
模型建立完了,我给地图中随机添加了几个障碍物,现在来看看地图的样子。
optimization algorithm\application_path_planning\Test.m
测试脚本
这是当前的地图模型,下面将使用优化算法在该地图中寻找一条代价最小的路径连接起点和终点。
这里将使用差分进化算法来求解路径,差分进化算法实现可以看优化算法matlab实现(七)差分进化算法matlab实现。如果想使用其他优化算法,则引入相关的优化算法路径后,实例化即可。
由于不知道需要多少个关键点才能确定路径,故先计算5个关键点的路径,如果有多余的关键点,后面减少关键点后再次计算。5个关键点,每个关键点有x,y两个坐标,对于优化算法来说其维度为10维。
参数 | 值 |
---|---|
维度 | 10(5个关键点) |
种群 | 50 |
最大迭代 | 1000 |
范围 | [(0,0),(100,100)] |
步长 | 0.5 |
实现代码如下:
optimization algorithm\application_path_planning\Test.m
最终结果如下
程序显示有多余的点,接下来会测一测关键点为4,3,2,1时的情况。
关键点数 | 运行时间(秒) | 代价 | 是否有多余点 |
---|---|---|---|
5 | 1106 | 127.428 | 是 |
4 | 928 | 125.98 | 是 |
3 | 783 | 127.7271 | 否 |
2 | 760 | 128.0267 | 否 |
1 | 719 | 133.5343 | 否 |
迭代图像如下:
从最终结果可以看出,上述模型至少需要2个关键点(可以看有4个关键点的动态图,有两个多余的点)但是却在有4个关键点时得到了整个结果中的最优解。毕竟优化算法是概率算法,不保证一定能得到最优解,所以需要多进行几次试验选取最优的结果才行,上面只进行了一次试验,所以是一个正常的结果。
试验的耗时随着维度的增加而增加,但是这个增加并不明显。关键点的数量对计算模型代价所耗时并不直接。模型计算耗时的最关键因素是:关键点数量+采样点数量。所以步长越大,路径越短,耗时也就越少。在步长一定,且当关键点数量较多时,解空间搜索范围较大,出现较长路径的概率很大,所以耗时会相对较多。
下面在确定关键点为3时,测试下步长为0.1, 1.0,2.0时的结果。
最终结果如下
步长 | 运行时间(秒) | 代价 | 是否有多余点 |
---|---|---|---|
2 | 188 | 123.2993 | 否 |
1 | 411 | 123.7047 | 否 |
0.5 | 783 | 127.7271 | 否 |
0.1 | 3188 | 127.7327 | 否 |
从表格可得知,步长越大,所需计算时间越少,但结果却越好,这是因为取样点较少,路径代价计算有一定失真。看图可以看出,当步长为1.0和2.0时,路径其实有掠过不可通过的障碍,但是由于步长较大,没有取样点在该障碍区域内,所以计算时认为该路径有效。
本文主要使用优化算法解决了路径规划问题。这个问题不算太复杂,优化算法也不用修改,只需要将问题建立模型,然后使用优化算法去计算结果即可。也就是说,只要问题模型建立好,使用任何一个优化算法来求解都是相同的步骤。
对于更加复杂的问题模型,可能需要对指定的优化算法进行针对性的修改来提高计算效率和准确度。不过大多数使用通用算法即可,不需要定制。
对于开头的问题,可以参考下面的做法:
1如何建立模型:问题—》数学模型—》代码实现。
因为需要使用优化算法来求解,所以问题模型的核心是一个函数,其输入是所需要的解,其输出是该解的评价函数。只要满足这两点,函数内如何计算,根据实际情况处理即可。
2优化算法如何处理解空间内的无效区域。
当该解(个体位置)到达无效区域时,直接返回一个极差值。本问题中就是返回一个极大值,这样到达该区域的个体将会直接被淘汰。缺点是当无效区域占解空间比例很大时,会很费时。如果要规避,就要对优化算法的算子进行修改了,也比较麻烦。
3问题模型每一维不是一个数值,优化算法如何处理。
将模型的输入转化为一个一维的向量,例如这里一个二维点可以转换为一个2维的向量,5个二维点就是一个10维的向量。虽然这样做维度会比较高,但是处理起来比较简单。直接将5个点传入优化算法进行迭代也不是不行,比较勉强,算法实现需要修改不少,好在matlab这种动态语言没声明变量类型,需要处理的部分不太多,但还是有不少bug需要解决,就不勉强了。