优化算法应用（一）路径规划

重点：

1. 如何建立模型：问题—》数学模型—》代码实现。
2. 优化算法如何处理解空间内的无效区域。
3. 问题模型每一维不是一个数值，优化算法如何处理。

一． 问题描述

如题，只给出了路径规划的目的，其余条件全靠脑补。

1. 既然有路径，那么必然存在起点和终点。
2. 既然需要规划路径，那么肯定不是所有位置都能经过，或者经过速度（代价）不一样，那么一定会有障碍物。
3. 既然路径能够规划，那么，地图一定是明确的，不会出现有未知的区域。
   那么现在可以总体描述一下问题：在一个已知的区域内，选择一条最合适的路径，连通起点到终点，使付出的代价最小。

二． 模型建立

根据问题描述可以确定模型中的几个要素：
1. 起点，终点
起点和终点为地图中的两个顶点；在地图中使用绿色区域表示。

2. 障碍物位置，范围
障碍物将分为两种:
a. 可以通过的区域，圆形，但是通过会有一定代价，距离障碍物中心越近，代价越大；在地图中使用红色区域表示。
b. 不可通过的区域，矩形，该区域任何位置都不允许通过；在地图中使用灰色区域表示。

3. 地图范围
为了简化模型，地图范围为一个矩形区域，路径上的所有点必须在该区域内。

4. 路径
连接起点至终点的曲线；在地图中使用蓝色曲线表示。

模型地图如下图所示:

图1

　　为了能使用优化算法对路径进行优化，该模型的输入应是路径，输出应该是一个数值，该数值将表示输入的路径的优劣。
　　为了简化问题，将路径上的关键点作为模型的输入，关键点之间的路径为该两个关键点的连线,路径如图2所示。

图2

　　一般来说，路径是越短越好，故我们求的是该模型的最小值。
　　路径评价函数 = 路径长度+通过障碍的代价。
　　由于矩形障碍是无法通过的，在通过矩形障碍时直接给与一个无穷大的代价即可，所以我们只需要确定如何计算进过圆形障碍的代价。

图3

　　如图，圆形障碍的半径为R,关键点A距圆心O的距离AO=r

　　其中a为该障碍物的代价系数，a>0，a的值越大，其经过该障碍的代价越大。

　　f表示该模型的适应度函数，d为路径的长度，p为路径通过障碍的代价。
　　其中路径的长度d比较好计算，顺序计算两个点之间的距离再累加即可。那么如何计算路径通过障碍的代价呢？
　　线段是由无数个点组成的，所有无法计算每个点的代价在累加。这时需要对路径进行采样，得到数个采样点取计算其代价。
　　如果不使用采样点则可能出现路径穿过障碍物却没有代价。

图4

　　如图4，A和B为关键点，如果只计算关键点，则AB连线即为路径，但实际上AB穿过了不可以通过的障碍物，该路径无效。
　　对线段采样的方式很多，这里我选择使用指定步长来对线段进行采样。如，若AB长为11，步长为2，则将AB等分为6段(11/2后向上取整），其中共有6个采样点，我们计算这6个采样点和AB两个关键点的代价即可。
　　步长越小，结果越接近真实，但是计算的复杂程度越高；步长越大，计算起来越快，但是会有一定程度失真。

图5

　　如上图5，由于步长较大，线段中有3个采样点，路径依然穿越了障碍区域，却没有计算代价，实际上，由于路径AB穿过了不可通过的障碍区域，其代价应为无穷大。
　　对于一个拥有k个圆形障碍的地图，若路径不穿过方形区域，由m个关键点组成，有n个采样点，其最终代价计算如下：

　　其中d为路径的长度，p由公式（2）计算得出，r为采样点距圆心的距离。
　　同时，由于计算路径时需要先知道其关键点数量。故计算之前需要先确定m的值。但由于m的值越大，路径的计算越复杂也越慢，而我们无法得知m的最小值。故在计算完后我们需要计算路径中是否有多余的关键点。这个步骤其实可以省略，但是有多余的点时计算量会比较大，算法需要更长时间才能得出结果。

图6

　　如上图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
矩形区域：判断是否有点在该区域内

% 障碍区域：矩形，不可通过
classdef Obstacle_Rect    
    properties
        % 数值较大的顶点，类似于左下顶点
        point_low;
        % 数值较大的顶点，类似于右上顶点
        point_up;

    end
    
    methods
        % 构造函数
        function self = Obstacle_Rect(point_low,point_up)
           self.point_low = point_low;
           self.point_up = point_up;
        end
        
        % 惩罚函数
        function value = get_penalty(self,path_point)
            
            % 如果>成立，I，J的对应维度值为1，否则为0
            I = self.point_up > path_point;
            J = path_point > self.point_low;
            % 如果I,J的所有维值均为1，则说明路点在该矩形内
            if sum(I.*J) == length(path_point)
                % 由于该障碍无法通过，故惩罚数值为最大
                value = realmax('double');
            else
                value = 0;
            end
        end
    end
    
end

optimization algorithm\application_path_planning\Obstacle_Circle.m
圆形区域：判断点与圆心的距离，计算代价

% 障碍区域：圆形，可以通过，但会受到一定惩罚
classdef Obstacle_Circle
    
    properties
        % 中心点的坐标，2维或者3维的点(长度为2或者3的向量)
        point;
        % 区域半径，数值
        radius;
        % 系数，用于计算惩罚函数，数值，默认为1
        coefficient = 1;
    end
    
    methods
        % 构造函数
        function self = Obstacle_Circle(point,radius,coefficient)
           self.point = point;
           self.radius = radius;
           self.coefficient = coefficient;
        end
        
        % 惩罚函数
        function value = get_penalty(self,path_point)
            % 计算路点到障碍中心的距离
            dist = sqrt(sum((path_point - self.point).^2));
            if dist < self.radius
                % 将路点距中心的距离系数作为惩罚系数
                % 即距离中越近，惩罚系数越大
                value = self.coefficient*(1-dist/self.radius);
            else
                value = 0;
            end
        end
    end
        
end

optimization algorithm\application_path_planning\Map_Model.m
地图模型：
1.设定障碍区域，地图边界
2.关键点序列与向量转换
3.根据关键点和步长得到采样点
4.根据关键点，采样点计算代价
5.绘制地图和路径

% 原文链接：https://www.jianshu.com/p/70252c42b37d
% 地图模型
classdef Map_Model

    properties
        
        % 地图维度，2，表示为平面地图
        dim = 2;
        
        % 插值步长
        step_size = 0.5;
        
        % 地图上界
        bound_up = [100,100];
        % 地图下界
        bound_low = [0,0];
        
        % 起点x=5,y=90
        point_start = [5,90];
        % 终点x=90,y=5
        point_end =  [90,5];
        
        % 不可通过的障碍列表
        obstacle_rect_list=[Obstacle_Rect([5,20],[20,30]),Obstacle_Rect([30,45],[50,60])];
    
        % 惩罚性质的障碍列表
        % 中心为（55,70），半径为8，惩罚系数为5
        % 中心为（65,40），半径为10，惩罚系数为5
        obstacle_circle_list=[Obstacle_Circle([55,70],8,5),Obstacle_Circle([65,45],10,5),Obstacle_Circle([20,55],7,5),Obstacle_Circle([40,30],8,5)];;
    end
    
    methods
        % 构造函数
        function self = Map_Model()
        end
        
        % 输入为向量，输出为适应度值
        function value = fit_function(self,x) 
            % 将向量x,转化为数个关键点坐标
            key_point_list = self.get_point_from_vector(x);
            value = self.get_path_value(key_point_list);
        end
        
        % 从输入的向量中获取点的坐标
        function key_point_list = get_point_from_vector(self,x)
            % 加入起点
            key_point_list = [self.point_start];
            for i = 1:(length(x)/self.dim)
                % x中相邻数个值组成一个点
                path_point = x((i-1)*self.dim+1:i*self.dim);
                % 将该点加入列表
                key_point_list = [key_point_list;path_point];
            end
            % 加入终点
            key_point_list = [key_point_list;self.point_end];
        end
        
        % 输入路径关键点，计算其评价值
        function value = get_path_value(self,key_point_list)
            value = 0;
            for i = 2:length(key_point_list)
                cur_point = key_point_list(i,:);
                last_point = key_point_list(i-1,:);
                % 计算两点之间的距离
                dist = sqrt(sum((last_point-cur_point).^2));
                % 累加两点之间的距离，作为适应度函数的一部分
                value = value + dist;
            end
            % 获取采样点
            sampling_point_list = self.get_sampling_point_list(key_point_list);
            
            % 遍历每个点采样点，计算惩罚系数
            for i = 1:size(sampling_point_list,1)
                % 计算长方形区域的惩罚
                for j = 1:length(self.obstacle_rect_list)
                    value = value + self.obstacle_rect_list(j).get_penalty(sampling_point_list(i,:));
                end
                
                % 计算圆形区域的惩罚
                for j = 1:length(self.obstacle_circle_list)
                    penalty = self.obstacle_circle_list(j).get_penalty(sampling_point_list(i,:));
                    value = value + penalty;
                end
            end
        end
        
        % 计算路径中的采样点
        function sampling_point_list = get_sampling_point_list(self,key_point_list)
            % 加入起点
            sampling_point_list = [key_point_list(1,:)];
            for i = 2:length(key_point_list)
                % 获取当前点
                path_point = key_point_list(i,:);
                % 计算该点和上一个点之间的中间点
                middle_point_list = self.get_middle_point(sampling_point_list(end,:),path_point);
                % 将中间点加入列表
                sampling_point_list = [sampling_point_list;middle_point_list];
                % 将该点加入列表
                sampling_point_list = [sampling_point_list;path_point];
            end
        end
        
        % 计算两个关键点之间的插值点
        function middle_point_list = get_middle_point(self,path_point_start,path_point_end)
            % step_size 为插值步长，步长越小，插入的中间点越多
            % 计算两点之间的距离
            dist = sqrt(sum((path_point_start-path_point_end).^2));
            % 计算插值点数
            middle_num = floor(dist/self.step_size)+1;
            middle_point_list = [];
            if middle_num > 0
                % 如果插值点数大于0
                for i = 1:middle_num
                    middle_point = (i*path_point_end+(middle_num+1-i)*path_point_start)/(middle_num+1);
                    middle_point_list = [middle_point_list;middle_point];
                end
            end
        end
        
        % 判断是否有多余的点
        function has_surplus = get_surplus_point_num(self,x)
            has_surplus = 0;
            % 将向量x,转化为数个关键点坐标
            key_point_list = self.get_point_from_vector(x);
            % 计算当前值
            value = self.get_path_value(key_point_list);
            
            for i = 2:(length(key_point_list)-1)
                % 去除一个关键点计算其值
                temp_point_list = [key_point_list(1:i-1,:);key_point_list(i+1:end,:)];
                % 计算去除一个点后的适应度值
                new_value = get_path_value(self,temp_point_list);
                % 如果去除一个点的值更好则直接返回1
                if new_value < value
                    has_surplus = 1;
                    break;
                end
            end
            
        end
        
        % 绘制地图
        function draw_map(self,input,num)
            % 绘制图像

            for i = 1:length(self.obstacle_circle_list)
                %圆心坐标为point，半径为radius,轮廓颜色为红色
                x = self.obstacle_circle_list(i).point(1);
                y = self.obstacle_circle_list(i).point(2);
                r = self.obstacle_circle_list(i).radius;
                rectangle('Position',[x-r,y-r,2*r,2*r],'Curvature',[1,1],'edgecolor',[1 0.2 0.2],'FaceColor',[1 0.2 0.2]);
            end
            
            for i = 1:length(self.obstacle_rect_list)
                %rectangle('Position',[x,y,w,h],'PropertyName',propertyvalue)
                xy = self.obstacle_rect_list(i).point_low;
                wh = self.obstacle_rect_list(i).point_up - self.obstacle_rect_list(i).point_low;
                rectangle('Position',[xy,wh],'Linewidth',1,'LineStyle','-','EdgeColor',[0.5 0.5 0.5],'FaceColor',[0.5 0.5 0.5])
            end
            
            % 绘制起点
            rectangle('Position',[self.point_start-3,6,6],'Linewidth',1,'LineStyle','-','EdgeColor',[0.5 1 0.5],'FaceColor',[0.5 1 0.5]);
            % 绘制终点
            rectangle('Position',[self.point_end-3,6,6],'Linewidth',1,'LineStyle','-','EdgeColor',[0.5 1 0.5],'FaceColor',[0.5 1 0.5]);

            % 如果没有有输入关键点则返回，否则绘制路径
            if isempty(x)
                return
            end
            hold on ;
            
            % 绘制路径
            % 加入起点
            path_point_list = [self.point_start];
            for i = 1:(length(input)/self.dim)
                % x中相邻数个值组成一个点
                path_point = input((i-1)*self.dim+1:i*self.dim);
                x = [path_point(1),path_point_list(end,1)];
                y = [path_point(2),path_point_list(end,2)];
                scatter(path_point(1),path_point(2),20,'b','filled');
                % 当前点连接到上一个点
                plot(x,y,'b','linewidth',1.5);
                hold on ;
                path_point_list = [path_point_list;path_point];
            end
            hold on ;
            % 终点连接到上一个点
            x = [path_point_list(end,1),self.point_end(1)];
            y = [path_point_list(end,2),self.point_end(2)];
            plot(x,y,'b','linewidth',1.5);
            hold on;
            % 加入终点
            path_point_list = [path_point_list;self.point_end];
            text(90,100,num2str(num),'FontSize',20);
            
            % 绘制显示区域
            axis([self.bound_low(1)-10 self.bound_up(1)+10 self.bound_low(2)-10 self.bound_up(2)+10]);
            
            axis equal;
            
            % 固定横纵坐标轴
            set(gca,'XLim',[self.bound_low(1)-10 self.bound_up(1)+10]);
            set(gca,'YLim',[self.bound_low(2)-10 self.bound_up(2)+10])
        end
        
    end
    
end

模型建立完了，我给地图中随机添加了几个障碍物，现在来看看地图的样子。
optimization algorithm\application_path_planning\Test.m
测试脚本

%% 清理之前的数据
% 清除所有数据
clear all;
close all;
% 清除窗口输出
clc;

% 初始化模型
model = Map_Model();
% 绘制无路径地图，无编号
% 第一个参数为算法输出的结果，第二个参数为编号，绘制动态图用
model.draw_map([],'');

图7

　　这是当前的地图模型，下面将使用优化算法在该地图中寻找一条代价最小的路径连接起点和终点。

四． 优化算法计算路径

这里将使用差分进化算法来求解路径，差分进化算法实现可以看优化算法matlab实现（七）差分进化算法matlab实现。如果想使用其他优化算法，则引入相关的优化算法路径后，实例化即可。
　　由于不知道需要多少个关键点才能确定路径，故先计算5个关键点的路径，如果有多余的关键点，后面减少关键点后再次计算。5个关键点，每个关键点有x,y两个坐标，对于优化算法来说其维度为10维。

参数	值
维度	10（5个关键点）
种群	50
最大迭代	1000
范围	[(0,0),(100,100)]
步长	0.5

实现代码如下：
optimization algorithm\application_path_planning\Test.m

%% 清理之前的数据
% 清除所有数据
clear all;
close all;
% 清除窗口输出
clc;

%% 添加目录
% 将上级目录中的frame文件夹加入路径
addpath('../frame')
% 引入差分进化算法
addpath('../algorithm_differential_evolution')

% 初始化模型
model = Map_Model();
% 预设关键点数
point_num = 5;
% 重新计算每一维的取值范围
% 将（100,200）转化为（100,200,100,200,100,200,100,200....）
range_max = repmat(model.bound_up, 1, point_num);
range_max = reshape(range_max, 1, numel(range_max));
range_min = repmat(model.bound_low, 1, point_num);
range_min = reshape(range_min, 1, numel(range_min));

%% 算法实例
dim = model.dim*point_num;
% 种群数量
size = 50;
% 最大迭代次数
iter_max = 1000;
% 取值范围上界
range_max_list = range_max;
% 取值范围下界
range_min_list = range_min;

% 实例化差分进化算法类
base = DE_Impl(dim,size,iter_max,range_min_list,range_max_list);
base.is_cal_max = false;
% 确定适应度函数
base.fitfunction = @model.fit_function;
% 运行
base.run();
disp(['复杂度',num2str(base.cal_fit_num)]);

%% 下面绘制动态图
% 绘制每一代的路径
for i = 1:length(base.position_best_history)
    model.draw_map(base.position_best_history(i,:),i);
    % 每0.01绘制一次
    pause = 0.01;
    %下面是保存为GIF的程序
    frame=getframe(gcf);
    % 返回单帧颜色图像
    imind=frame2im(frame);
    % 颜色转换
    [imind,cm] = rgb2ind(imind,256);
    filename = ['path_planning_',num2str(point_num),'.gif'];
    if i==1
         imwrite(imind,cm,filename,'gif', 'Loopcount',inf,'DelayTime',1e-4);
    else
         imwrite(imind,cm,filename,'gif','WriteMode','append','DelayTime',pause);
    end

    if i 

最终结果如下
程序显示有多余的点，接下来会测一测关键点为4,3,2,1时的情况。

关键点数	运行时间（秒）	代价	是否有多余点
5	1106	127.428	是
4	928	125.98	是
3	783	127.7271	否
2	760	128.0267	否
1	719	133.5343	否

迭代图像如下：

↑关键点5

↑关键点4

↑关键点3

↑关键点2

↑关键点1

　　从最终结果可以看出，上述模型至少需要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	否

↑关键点3，步长2.0

↑关键点3，步长1.0

↑关键点3，步长0.1

从表格可得知，步长越大，所需计算时间越少，但结果却越好，这是因为取样点较少，路径代价计算有一定失真。看图可以看出，当步长为1.0和2.0时，路径其实有掠过不可通过的障碍，但是由于步长较大，没有取样点在该障碍区域内，所以计算时认为该路径有效。

五． 总结

本文主要使用优化算法解决了路径规划问题。这个问题不算太复杂，优化算法也不用修改，只需要将问题建立模型，然后使用优化算法去计算结果即可。也就是说，只要问题模型建立好，使用任何一个优化算法来求解都是相同的步骤。
　　对于更加复杂的问题模型，可能需要对指定的优化算法进行针对性的修改来提高计算效率和准确度。不过大多数使用通用算法即可，不需要定制。
对于开头的问题，可以参考下面的做法：
　　1如何建立模型：问题—》数学模型—》代码实现。
　　因为需要使用优化算法来求解，所以问题模型的核心是一个函数，其输入是所需要的解，其输出是该解的评价函数。只要满足这两点，函数内如何计算，根据实际情况处理即可。
　　2优化算法如何处理解空间内的无效区域。　　
　　当该解（个体位置）到达无效区域时，直接返回一个极差值。本问题中就是返回一个极大值，这样到达该区域的个体将会直接被淘汰。缺点是当无效区域占解空间比例很大时，会很费时。如果要规避，就要对优化算法的算子进行修改了，也比较麻烦。
　　3问题模型每一维不是一个数值，优化算法如何处理。
　　将模型的输入转化为一个一维的向量，例如这里一个二维点可以转换为一个2维的向量，5个二维点就是一个10维的向量。虽然这样做维度会比较高，但是处理起来比较简单。直接将5个点传入优化算法进行迭代也不是不行，比较勉强，算法实现需要修改不少，好在matlab这种动态语言没声明变量类型，需要处理的部分不太多，但还是有不少bug需要解决，就不勉强了。