技术领域
[0001] 本
发明属于路径规划领域，涉及一种多目标三维路径规划方法，该方法可用于智能导航，无人驾驶等领域的路径规划。
背景技术
[0002] 目前
电池储能十分有限，提高电池的续航能
力是重要的研究课题。相关研究主要包含两方面：提高电池的储
电能力；降低电池工作的单位能耗。由于上坡能耗系数显著大于平路上的能耗系数，这对于多山的城市来说，一条平而适度远的路可能远比相对短却上下坡较多的路要节省
能量，且并没有显著多耗费时间成本。
[0003] 目前提出了诸多的路径规划方法：如人工势力场
算法、神经网络方法、
遗传算法、随机树法、A星算法。它们在多种障碍条件下能取得较好的规划效果。然而它们几乎都是基于二维地图展开研究，对三维地图下的路径规划方法进行研究不多。相对来说，不管是
机器人、太空探测器或是电动
汽车在能耗有限的情形下，研究考虑能量损耗的路径规划方法将是十分有价值的。
发明内容
[0004] 有鉴于此，本发明的主要目的在于在兼顾能耗与路程两个目标的
基础上提出一中多目标三维路径规划方法，用以实现在山地形中找到降低能耗的优化路径，提高智能移动设备的续航里程。
[0005] 一方面，本发明提供一种多目标三维路径规划方法，包括如下步骤：
[0006] 步骤1，构建多目标三维路径规划模型，并建立总估计代价函数F(n)：
[0007] F(n)＝G(n)+H(n) (1)
[0008] 式中，G(n)为当前点到邻点的估计代价函数，H(n)为邻点到终点的估计代价函数，其中：
[0009] G(n)通过当前点和邻点之间的能耗和直线距离得到；
[0010] H(n)包括总能耗和路程，总能耗通过计算邻点与目标
节点之间的能耗得到，路程通过计算邻点与目标节点之间的直线距离和曲线距离得到；
[0011] 步骤2，计算总估计代价函数F(n)的值，找到邻点中总估计代价函数F(n)最小的值，存入路径列表Lane中；
[0012] 步骤3，将步骤2中找到的总估计代价函数F(n)最小的节点作为当前点，重复进行路径规划；
[0013] 步骤4，当路径规划到当前点为终点时，停止规划，此时，Lane中存放的即为多目标三维路径规划模型规划出来的路径。
[0014] 优选地，所述步骤1具体表现为：
[0015] 步骤1.1，初始化地图高程数据：获取每个可行节点的(x,y,z)坐标数据，并存入可行节点列表Path中，其中，坐标(x,y)是经纬度，将坐标(x,y)映射为实际距离坐标；
[0016] 步骤1.2，初始化路径规划参数：定义规划起点和终点
位置，设定搜索半径为factor，用于搜索当前节点周围的邻点，将起点作为当前点进行规划；
[0017] 步骤1.3，利用搜索半径factor找到当前点周围的所有邻点，并计算当前点到邻点的估计代价函数G(n)和邻点到终点的估计代价函数H(n)的值，其中：
[0018] G(n)＝D(Pn,Pn+1)+EG(Pn,Pn+1) (2)
[0019] 式中，EG(Pn,Pn+1)为当前点和邻点之间的能耗，D(Pn,Pn+1)为当前点和邻点之间的欧几里得距离，Pn为当前点，Pn+1为邻点；
[0020] H(n)＝η1*EH+η2*D′H+η3*D″H (3)
[0021] 式中，EH是邻点到目标节点的总能耗，D′H为邻点与目标节点之间的直线距离，D″H为邻点与目标节点之间的曲线距离，η1、η2和η3分别为总能耗EH、直线距离D′H和曲线距离D″H的权重。
[0022] 优选地，所述G(n)中当前点和邻点之间的能耗通过建立坡度能耗模型得到，所述坡度能耗模型建立如下：
[0023]
[0024] 式中，g1(Pn,Pn+1)和g2(Pn,Pn+1)分别为在坡度为 时的单位距离下坡能耗函数和单位距离上坡能耗函数，其中：
[0025]
[0026] 式中，(xn,yn,zn)分别为当前点Pn的坐标数据，(xn+1,yn+1,zn+1)分别为邻点Pn+1(的坐标数据；
[0027]
[0028] 优选地，所述单位距离下坡能耗函数g1(Pn,Pn+1)通过如下方法建立：
[0029] 选取一段平路和多段坡度不同的下坡道路；
[0030] 分别测量同一型号的机器人或汽车行驶相同距离的平路和多段下坡道路所产生的能耗；
[0031] 多次测试分别求得平路和每段下坡道路的能耗平均值；
[0032] 求出不同坡度下坡道路平均能耗与平路平均能耗的比值；
[0033] 采用三次多项式拟合的方式，得到单位距离下坡能耗函数g1(Pn,Pn+1)：
[0034]
[0035] 式中，a1,b1和c1均为待定系数。
[0036] 优选地，所述单位距离上坡能耗函数g2(Pn,Pn+1)通过如下方法建立：
[0037] 选取一段平路和多段坡度不同的上坡道路；
[0038] 分别测量同一型号的机器人或汽车行驶相同距离的平路和多段上坡道路所产生的能耗；
[0039] 多次测试分别求得平路和每段上坡道路的能耗平均值；
[0040] 求出不同坡度上坡道路平均能耗与平路平均能耗的比值；
[0041] 采用三次多项式拟合的方式，得到单位距离上坡能耗函数g2(Pn,Pn+1)：
[0042]
[0043] 式中，a2,b2和c2均为待定系数。
[0044] 优选地，通过相邻节点的能耗相加，使算法模拟出山地的地形，并估计出高山低谷的位置，得到所述相邻节点的能耗通过坡度能耗模型计算得到，如下所示：
[0045]
[0046] 其中，EH是邻点到目标节点的总能耗，Pi和Pi+1为节点到目标节点的中间节点，ωi为第i个节点到第i+1个节点的权重，且ωi由sigmoid函数下降的曲线求得，
[0047]
[0048] 其中，b为曲线倾斜度，c为偏置值，exp(-b*x(i))为e的-b*x(i)次方，x(i)为第i个节点到当前点的距离。
[0049] 优选地，所述H(n)中路程通过计算邻点与目标节点之间的直线距离和曲线距离得到，公式如下：
[0050] D＝D′H+D″H (11)
[0051] 式中，D为路程，其中，D′H通过如下公式计算：
[0052]
[0053] 式中，Pgoal为目标节点，(xgoal,ygoal,zgoal)为Pgoal的坐标数据；
[0054] D″H通过如下公式计算：
[0055]
[0056] 式中，D(Pi,Pi+1)为第i个节点到第i+1个节点的直线距离。
[0057] 优选地，通过多目标混沌
优化算法对H(n)中总能耗和路程两个目标进行优化，求解满足总能耗最低和路程最短两个目标的解，并对其中参数η1、η2、η3和c进行优化求解，得到每个参数的最优值。
[0058] 优选地，所述多目标混沌优化算法的具体步骤如下：
[0059] A)初始化种群：对于上述4个优化变量η1、η2、η3和c，分别赋予微小差异的初值，得到4个混沌变量，并将混沌变量的范围分别放大到相应的优化变量取值范围，同时，对于种群中的pop个解分别用混沌序列的方式进行初始化；
[0061] C)目标函数的计算：通过多目标三维路径规划模型得到路径Lane，得到种群中每个解的目标函数值；
[0062] D)非支配关系与拥挤度排序：对目标函数的解进行非支配关系排序，仅保留第一非支配前沿的解，并对第一支配前沿的解，进行拥挤度排序，选取拥挤度较低的解；
[0063] E)混沌变异生成子代：通过混沌序列的方法对父代进行变异，计算公式为：
[0064] Coffspring＝4*Cparent*(1-Cparent) (18)
[0065] 其中，Cparent是父代种群的解集，Coffspring是子代种群的解集；
[0066] F)循环
迭代：将得到的子代种群继续进行步骤C中目标函数的计算，得到子代种群每个解的目标函数值，进行循环迭代；
[0067] G)终止准则：当算法执行到最大代数或者群体中的目标函数值稳定时，算法终止，其中，第一支配前沿的所有解都是所求的解，且得到参数[η1，η2,η3,c]的解。
[0068] 综上所述，本发明首先，建立总估计代价函数F(n)，F(n)由当前点到邻点的估计代价函数G(n)和邻点到终点的估计代价函数H(n)求和构成，其中：G(n)通过当前点和邻点之间的能耗和直线距离得到；H(n)包含总能耗和路程，总能耗通过计算邻点到目标节点间的能耗得到，路程通过计算邻点到目标之间的直线距离和曲线距离得到；接着，通过计算总估计代价函数F(n)的最小值找到当前点；然后，重复进行路径规划直至当前点为终点时停止，这些使总估计代价函数F(n)最小的节点最终形成规划的路径。上述方法相比
现有技术，能实现兼顾能耗及路程的三维规划，在山地形中找到降低能耗的优化路径，提高智能移动设备的续航里程。
[0069] 在进一步的技术方案中，采用多目标混沌优化算法对H(n)中总能耗和路程两个目标进行优化，求解满足总能耗最低和路程最短两个目标的解，并对其中参数η1、η2、η3和c进行优化求解，得到每个参数的最优值，用以解决所述路径规划方法的多目标优化求解问题。
附图说明
[0070] 图1为本发明一种多目标三维路径规划方法的
流程图；
[0071] 图2为本发明一种多目标三维路径规划模型图；
[0072] 图3为本发明多目标混沌优化算法的流程图；
[0073] 图4为本发明一
实施例某山地城市的路网图；
[0074] 图5为本发明基于图4中路网规划出的路径主视图；
[0075] 图6为本发明基于图4中路网规划出的路径俯视图。
具体实施方式
[0076] 需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。
[0077] 本发明的主要目的在于从路径规划的
角度提供一种节省能量的方案。由于电动汽车上坡能耗系数显著大于平路上的能耗系数，这对于多山的城市来说，一条平而适度远的路可能远比相对短却上下坡较多的路要节省能量，且并没有显著多耗费时间成本。不同于传统单一考虑路程最短的角度设计路径规划方法，本发明在兼顾能耗与路程两个目标的基础上提出了一种多目标三维路径规划方法。
[0078] 参见图1，图1为一种多目标三维路径规划方法的流程图，具体包括如下步骤：
[0079] 步骤1.1，初始化地图高程数据：获取每个可行节点的(x,y,z)坐标数据，并存入可行节点列表Path中，其中，坐标(x,y)是经纬度，将坐标(x,y)映射为实际距离坐标；
[0080] 步骤1.2，初始化路径规划参数：定义规划起点和终点位置，设定搜索半径为factor，用于搜索当前节点周围的邻点，将起点作为当前点进行规划；
[0081] 需要说明的是，邻点定义为在当前点周围一定半径的点；
[0082] 步骤1.3，利用搜索半径factor找到当前点周围的所有邻点，并计算当前点到邻点的估计代价函数G(n)和邻点到终点的估计代价函数H(n)的值，其中：
[0083] 由于上坡能耗系数显著大于平路上的能耗系数，并且下坡能耗系数也不同于上坡能耗系数，采用能耗模型的方式规划路径更具备合理性，此规划出来的路径可以避免连续上坡下坡，又因为
电动机在上坡时处于半堵转状态，让电动机工作
电流增大，使电动机大量发热，危害电动机的寿命，由此，上述当前点到邻点的估计代价函数G(n)的计算方式如下：
[0084] G(n)＝D(Pn,Pn+1)+EG(Pn,Pn+1) (2)
[0085] 式中，EG(Pn,Pn+1)为当前点和邻点之间的能耗且单位为千焦(kJ)，D(Pn,Pn+1)为当前点和邻点之间的欧几里得距离，Pn(为当前点，Pn+1为邻点；
[0086] 同时，本发明中邻点到终点的估计代价函数H(n)建立需考虑能耗及路程两个方面，即估计代价函数H(n)包括总能耗EH和路程D，其中，总能耗EH通过计算邻点与目标节点之间的能耗得到，路程D通过计算邻点与目标节点之间的直线距离D′H和曲线距离D″H得到。这种方法使电动汽车或机器人等在山地地形行驶中在保证路径较短的同时并且节省能量，一般H(n)函数只考虑路程的影响，在复杂山地中规划路径时，会使算法分辨不出山地地形，造成反复的上下坡，使能耗增加，降低电池的续航里程，由此，H(n)函数的建立通过下式建立：
[0087] H(n)H＝η1*EH+η2*D′H+η3*D″H (3)
[0088] 式中，EH是邻点到目标节点的总能耗，D′H为邻点与目标节点之间的直线距离，D″H为邻点与目标节点之间的曲线距离，η1、η2和η3分别为总能耗EH、直线距离D′H和曲线距离D″H的权重。
[0089] 步骤2，计算总估计代价函数F(n)的值，找到邻点中总估计代价函数F(n)最小的值，存入路径列表Lane中，F(n)的计算公式如下：
[0090] F(n)＝G(n)+H(n) (1)；
[0091] 图2为一种多目标三维路径规划模型图。从图2亦可看出，总估计代价函数F(n)包括当前点到邻点的估计代价函数G(n)和邻点到终点的估计代价函数H(n)，且估计代价函数G(n)和估计代价函数H(n)分开建模，这是由于当前点到邻点的估计代价是确定的，而邻点到终点的路径不确定，只能估计邻点到终点的代价。
[0092] 步骤3，将步骤2中找到的总估计代价函数F(n)最小的节点作为当前点，重复进行路径规划。
[0093] 步骤4，当路径规划到当前点为终点时，停止规划，此时，Lane中存放的即为多目标三维路径规划模型规划出来的路径。
[0094] 需要说明的是，上述流程图中步骤1.1至步骤1.4可认为用于构建多目标三维路径规划模型，并建立总估计代价函数F(n)。
[0095] 进一步地，当前点到邻点的估计代价函数G(n)是由坡度能耗模型的计算得到，前述坡度能耗模型建立如下：
[0096]
[0097] 式中，g1(Pn,Pn+1)和g2(Pn,Pn+1)分别为在坡度为 时的单位距离下坡能耗函数和单位距离上坡能耗函数，其中，当前点和邻点之间的欧几里得距离D(Pn,Pn+1)的计算公式如下：
[0098]
[0099] 式中，(xn,yn,zn)分别为当前点Pn的坐标数据，(xn+1,yn+1,zn+1)分别为邻点Pn+1(的坐标数据；
[0100] 同时，坡度的计算方法如下式所示：
[0101]
[0102] 此外，在进一步的技术方案中，上述单位距离下坡能耗函数g1(Pn,Pn+1)通过如下方法建立：
[0103] 选取一段平路和多段坡度不同的下坡道路；
[0104] 分别测量同一型号的机器人或汽车行驶相同距离的平路和多段下坡道路所产生的能耗；
[0105] 多次测试分别求得平路和每段下坡道路的能耗平均值；
[0106] 求出不同坡度下坡道路平均能耗与平路平均能耗的比值；
[0107] 采用三次多项式拟合的方式，得到单位距离下坡能耗函数g1(Pn,Pn+1)：
[0108]
[0109] 式中，a1,b1和c1均为待定系数。
[0110] 以某一种型号的机器人或汽车的汽车为例，分别在坡度α＝[0°,-5°,-10°,-15°,-20°,-25°,-30°,-35°,-40°]下，测量其行驶100米所产生的能耗，通过多次实验求得能耗的平均值[E1,E2,E3,E4,E5,E6,E7,E8,E9],求出不同坡度能耗与平路能耗的比值
通过实验数据分析得出坡度是能耗变化的主要因素，采用
三次多项式拟合的方式，通过上面的实验数据求出待定系数a1,b1和c1的值，即可得到上述单位距离下坡能耗函数。
[0111] 同理，单位距离上坡能耗函数g2(Pn,Pn+1)的建立方法与上述单位距离下坡能耗函数g1(Pn,Pn+1)建立方法类似：
[0112] 选取一段平路和多段坡度不同的下坡道路；
[0113] 分别测量同一型号的机器人或汽车行驶相同距离的平路和多段下坡道路所产生的能耗；
[0114] 多次测试分别求得平路和每段下坡道路的能耗平均值；
[0115] 求出不同坡度下坡道路平均能耗与平路平均能耗的比值；
[0116] 采用三次多项式拟合的方式，得到单位距离下坡能耗函数g1(Pn,Pn+1)：
[0117]
[0118] 式中，a1,b1和c1均为待定系数。
[0119] 同样，以某一种型号的机器人或汽车为例，分别在坡度α＝[0°,5°,10°,15°,20°,25°,30°,35°,40°]下，测量其行驶100米所产生的能耗，通过多次实验求得能耗的平均值[E1′,E2′,E3′,E4′,E5′,E6′,E7′,E8′,E9′]，并求出不同坡度能耗与平路能耗的比值通过实验数据分析得出坡度是能耗变化的主要因素，采用
三次多项式拟合的方式，通过上面的实验数据求出待定系数a2,b2和c2的值，即可得到上述单位距离上坡能耗函数。
[0120] 进一步地，总能耗EH通过坡度能耗模型计算得到，如下所示：
[0121]
[0122] 式中，能耗估计模型EH，通过相邻节点的能耗相加，可以使算法模拟出山地的地形，并估计出高山低谷的位置，让算法更具智能性；Pi和Pi+1为节点到目标节点的中间节点，ωi为第i个节点到第i+1个节点的权重，由于人们的习惯是更在意近处的山峰，在对电动汽车或者山地机器人等进行路径规划时，先考虑近处山峰对路径的影响，于是此权重ωi设置为近处权重大，随着距离的增加，权重逐渐减少，故此，权重ωi可由sigmoid函数(神经元的非线性作用函数)下降的曲线求得，
[0123]
[0124] 其中，b为曲线倾斜度，c为偏置值，exp(-b*x(i))为e的-b*x(i)次方，x(i)为第i个节点到当前点的距离。
[0125] 此外，路程估计模型D用于路径长度估计，由曲线路程估计模型D″H及直线距离模型D′H构成，即：
[0126] D＝D′H+D″H (11)
[0127] 由于直线距离模型D′H加入H(n)函数，使本规划算法更具方向性，不会出现路径迂回的状态。直线距离D′H采用欧几里得计算公式得到，单位为米(m),即
[0128]
[0129] 式中，Pgoal为目标节点，xgoal,ygoal,zgoal为Pgoal的坐标数据；
[0130] 曲线距离估计模型采用下式计算得到，即
[0131]
[0132] 其中，D(Pi,Pi+1)为第i个节点到第i+1个节点的直线距离。这里用权重ωi对曲线距离进行加权，使规划路径不会因为远处的山影响权重过大导致直接跨过近处的山。
[0133] 此外，如前文所述邻点到终点的估计代价函数H(n)的建立，包括能耗估计、直线距离和曲线距离的权重η1、η2和η3，同时所述能耗模型的建立，包括偏置值c的计算；采用多目标的方法对能耗最少和路程最短两个目标进行优化，并上述系数进行优化求解。
[0134] 具体地，能耗和路程两个目标的计算方法，采用A星算法规划出来的路径Lane，计算路径中各个相邻点之间的能耗和路程数值，计算公式如下所示：
[0135]
[0136]
[0137] 其中，n为规划路径的总节点数。
[0138] 所述混沌优化的方法，借助混沌的伪随机全局不重复遍历性，相较于遗传、PSO等优化方法更加能够克服收敛于局部的问题，同时，混沌优化的方法包括两个步骤：
[0139] a、依次考察整个空间中的混沌变量，找到满足性能指标的当前最优点，混沌变量的计算方法为：
[0140] x(k+1)＝η*x(k)*(1-x(k)) (16)
[0141] 其中，η是控制参数，当η＝1时，系统处于混沌状态，输出相当于[0,1]之间的随机数，并且在[0,1]具有遍历性，其中的任何状态都不会重复出现；
[0142] b、在经过若干次搜索后当前最优点的没有变化，则定义当前最优点附近的子空间，再考察整个空间中的混沌变量，找到满足性能指标的最优点。
[0143] 优选地，本发明通过多目标混沌优化算法对总能耗EH和路程D两个目标进行优化求解，采用多目标优化算法，能够使邻点到终点的估计代价函数H(n)的建立更精确，得到所需的期望路径，并同时对优化变量η1、η2、η3和c求解最优值。所述优化方法的约束条件是使以下目标函数的值最小：
[0144]
[0145] 具体地，上述多目标混沌优化算法的详细步骤如下：
[0146] A)初始化种群：对于上述4个优化变量η1、η2、η3和c，分别赋予微小差异的初值，得到4个混沌变量，并将混沌变量的范围分别放大到相应的优化变量取值范围，同时，对于种群中的pop个解分别用混沌序列的方式进行初始化；
[0147] B)选定种群规模；
[0148] 优选地，本实施例设定种群规模为pop＝50，最大迭代次数g＝200；
[0149] C)目标函数的计算：通过多目标三维路径规划模型得到路径Lane，得到种群中每个解的目标函数值ELane和DLane；
[0150] D)非支配关系与拥挤度排序：对目标函数的解进行非支配关系排序，仅保留第一非支配前沿的解，即该前沿的解不被其它任何解支配，并对第一支配前沿的解，进行拥挤度排序，选取拥挤度较低的解，从而使获得的解尽可能的分布均匀且具有多样性；
[0151] E)混沌变异生成子代：通过混沌序列的方法对父代进行变异，计算公式为：
[0152] Coffspring＝4*Cparent*(1-Cparent) (18)
[0153] 其中，Cparent是父代种群的解集，Coffspring是子代种群的解集；
[0154] F)循环迭代：将得到的子代种群继续进行步骤C中目标函数的计算，得到子代种群每个解的目标函数值，进行循环迭代；
[0155] G)终止准则：当算法执行到最大代数或者群体中的目标函数值稳定时，算法终止，其中，第一支配前沿的所有解都是所求的解，且得到参数[η1，η2,η3,c]的解。
[0156] 进一步地，如图3所示，步骤F循环迭代具体表现为：进入下一代数后，混沌变异生成子代且需父子代整合后，再进入步骤C目标函数的计算，接着进入步骤D非支配关系与拥挤度排序，然后，混沌变异生成子代选择新的种群父代，直至进入步骤G。
[0157] 同时，参见图4至图6，图4为某城市的路网图，图5为本发明基于路网规划出的路径主视图，图6为本发明基于路网规划出的路径俯视图。
[0158] 使用某城市的路网图，从中得到高程信息和路网信息，建立适用于本发明的地图模型，在matlab(矩阵实验室)中进行路径规划，得到的路径规划效果如图5所示，可以看出本发明提出的算法可以在山地地形中快速获得规划路径，并且路径可以避开高山和低谷进行规划，符合人们的驾驶习惯。
[0159] 故此，为了获得节省电能能量，提出了一种考虑能量损耗及路程的多目标三维路径规划方法，该方法通过建立总估计代价函数F(n)，F(n)由当前点到邻点的估计代价函数G(n)和邻点到终点的估计代价函数H(n)求和构成，其中：估计代价函数G(n)，通过当前点和邻点之间的能耗和直线距离得到；估计代价函数H(n)，包含总能耗和路程，总能耗通过计算邻点到目标节点间的能耗得到，路程通过计算邻点到目标之间的直线距离和曲线距离得到；最后，采用多目标混沌优化算法来解决所述路径规划方法的多目标优化求解问题，以某城市为例进行实验，证明算法能够求得兼顾能量损耗及时间成本的路径。基于实验及原理可推论，该方法可适用于所有三维地形，可为电动汽车、军用山地侦察机器人、太空星体探测器(玉兔号、火星探测器)等考虑能量损耗的设备提供路径规划方案。
[0160] 以上仅为本发明的优选实施例，并非因此限制本发明的
专利范围，凡是利用本发明
说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。
