CN106970617B - 一种求解三目标机器人路径规划问题的方法 - Google Patents

Abstract

本发明公开了一种求解三目标机器人路径规划问题的方法，包括考虑机器人本身大小而寻找移动路径的方法，路径周边障碍栅格区域的判断方法和求解三目标机器人路径规划问题的算法。本发明由于考虑了机器人本身大小，防止机器人在狭窄路径与障碍栅格发生碰撞，具有更好的实用性，提出了一种依据当前路径的位置和大小来判断当前路径周边障碍栅格的方法，并只考虑当前路径周边的障碍栅格对路径安全性的影响，提高了算法的计算速度；针对三目标问题的特点以及蚁群算法容易陷入局部最优的缺点，构建了判断算法陷入早熟的机制，依据当前解和支配解的分布情况，在已找到的支配解附近做微小的扰动，帮助算法脱离局部最优，找到质量更优的解。

Description

一种求解三目标机器人路径规划问题的方法

技术领域

本发明涉及机器人路径规划技术领域，具体涉及一种求解三目标机器人路径规划问题的方法。

背景技术

目前，机器人路径规划问题除了寻求最短路径为单一目标的路径规划问题外，还会考虑机器人动作难度上的问题，即路径平滑度问题；路径的安全性问题，即机器人与其他物体的碰撞情况。现有技术在对问题建模时，将机器人看作一个质点，忽略了机器人本身的大小，但实际应用中，尤其在碰到较狭窄路径时，机器人本身的大小将会影响到机器人是否会与周边障碍物发生碰撞，从而影响最优路径的选择；在构造基于路径安全性的目标函数f3(x)时，要计算机器人在移动过程中当前路径与所有障碍栅格的最小距离，但实际应用中，距离某段路径较远处的障碍栅格对当前路径的安全性影响不大，因此，计算所有障碍栅格和当前路径的最小距离会额外增加计算的时间；在使用蚁群算法求解该问题时，搜索时间较长，容易陷入局部最优。

发明内容

本发明的目的是为解决上述不足，提供一种求解三目标机器人路径规划问题的方法。

本发明的目的是通过以下技术方案实现的：

一种求解三目标机器人路径规划问题的方法，方法如下：机器人在选择下一个栅格的过程中，除了要避开障碍栅格，还要避免与障碍栅格的碰撞，假定机器人为一半径为r的圆形质点，机器人从栅格0移动到栅格5的移动轨迹，如果考虑到机器人本身的大小，机器人从栅格0移动到栅格5会与障碍栅格2、3发生碰撞，因此，栅格5不能作为栅格0下一步移动的栅格，机器人从栅格0到栅格5移动的轨迹位于a、b、c、d所在区域，机器人在移动过程中需要先判断ad、bc、05三条线段是否会与障碍栅格区域发生相交，机器人需要保证在移动过程中不发生碰撞即要保证图中ad、bc、05三条线段都不都能与任何一个障碍栅格区域发生相交，线段ad、bc、05的数学公式如下：

假设x[i]，y[i]表示出发点i的位置坐标(0号栅格)。i+1表示出发点可能移动的下一个点(5号栅格)，r为机器人的半径；

线段ad公式如下：

其中：

线段bc公式如下：

其中：

线段05公式如下：

(y[i+1]-y[i])·x+(x[i]-x[i+1])·y+x[i+1]·y[i]-x[i]·y[i+1]＝0

x∈[x[i]，x[i+1]] y∈[y[i]，y[i+1]；

机器人选择下一个栅格移动的过程中，只要满足其移动路径不与障碍栅格发生碰撞的任意非障碍栅格都可作为备选栅格，机器人所在的每一段路径与该段路径附近障碍点的距离直接决定该路径的安全性，以路径中相邻两个栅格的中点(即每一段路径的中点)为原点，半径为R的圆作为该段路径的附近区域(R＝dist_ij/2+r，dist_ij表示该段路径的长度，r为机器人的半径)，如果在该区域内不存在障碍栅格，则表明该段路径安全性最佳，安全距离safedist可设置为一个较大的数，如果在该区域存在一个或多个障碍栅格，则计算各障碍栅格中心点到该段路径的距离，选取其中最小的值(如果只有一个障碍栅格，最小值即为该障碍栅格中心点到该段路径的距离)，并将该值设置为安全距离safedist。

一种求解三目标机器人路径规划问题的方法，具体算法如下：

(1)初始化：初始化参数α、β、γ，Q，初始化启发信息矩阵η，d_k，l表示栅格k和栅格l之间的距离；

初始化信息素矩阵τ，该矩阵中各元素初始值为1，构造外部档案集E，E初始时内部没有任何解，档案集大小为M，迭代最大次数N_max，当前迭代次数N＝0；

(2)构建解：N＝N+1，蚂蚁在搜索过程中，会根据小组信息素τ、蚂蚁的启发信息η，蚂蚁访问该栅格的次数V(V_l表示蚂蚁访问l点的次数，V_l越大，表示l栅格作为下个目标点的概率越小，目的是指导蚂蚁优先寻找新的路径以避免陷入局部最优)来计算状态转移概率，p_k，l表示蚂蚁从栅格k转移到栅格l的转移概率；

allowed表示蚂蚁i下一步允许选择的所有栅格的集合，允许选择的栅格为不与蚂蚁当前路径发生碰撞的所有非障碍栅格的集合；

根据该状态转移概率，蚂蚁构建新的解，如果该解被档案集中的解支配，则该解不能进入档案集，把表示当前解没有进入档案集次数的参数T(初值为0)加1，转步骤4)；如果档案集E中的解的个数小于N，同时该解没有被档案集中的解支配，将该解放入档案集E，T＝0，转步骤3)；如果档案集E中的解的个数等于N，且该解没有被档案集中的解支配，转步骤5)；

(3)更新信息素

分别表示当前解路径的路径长度，夹角大小和安全距离的倒数，K,l表示当前解路径上相邻的2个栅格编号，转步骤2)，直到达到最大迭代次数N_max结束，输出档案集中的解；

(4)判断T是否大于常数Tmax(Tmax为预先设定好的值)，如果T>Tmax，则说明算法多次计算没有找到支配解，算法陷入早熟和局部最优，计算档案集中各支配解个体之间的D(i)值，D(i)用于衡量archives(档案集)中解的分布情况；

K＝|archives|/2

表示解i到archives中第k个邻近个体(中心个体)之间的距离，需要计算个体i到archives中所有个体之间的距离，并按照増序排列，第k个个体为中心个体，D(i)值越小，意味着该个体分布在相对分散的区域，选取archives中D(i)值最小的支配解，按照(3)中公式，依据该支配解路径调整信息素，算法会在解集分散的区域做局部搜索，防止算法陷入局部最优，保证了解的均匀分布，如果T<Tmax，转步骤3)，依据当前解信息调整信息素。

(5)计算current solution(档案集中解和当前解的并集)中各解的D(i)值；

K＝|current solution|/2

表示解i到current solution中第k个邻近个体之间的距离，需要计算个体i到current solution中所有个体之间的距离，并按照増序排列，第k个个体为中心个体；

删除掉D(i)值最大的那个解，如果删除的解不是当前解，则转步骤3)，依据当前解的信息调整信息素，如果删除的解是当前解，转步骤3)，依据current solution中D(i)值最小的支配解路径调整信息素。

判断线段是否与障碍栅格区域发生相交的方法为：

(1)求出栅格i的中点(x_i，y_i)到栅格j的中点(x_j，y_j)的直线方程

(2)求该直线与任一障碍栅格的四条边所在的直线的交点；如果没有交点，则该路径不经过该障碍栅格；如果有交点，转步骤3)；

(3)如果该交点位于点(x_i，y_i)到(x_j，y_j)的线段上，同时该交点也位于该障碍栅格的四条边上，则栅格i到栅格j的路径经过该障碍栅格。

本发明具有如下有益的效果：

1)本发明由于考虑了机器人本身大小，防止机器人在狭窄路径与障碍栅格发生碰撞，具有更好的实用性；

2)本发明提出了一种依据当前路径的位置和大小来判断当前路径周边障碍栅格的方法，并只考虑当前路径周边的障碍栅格对路径安全性的影响，提高了算法的计算速度；

3)本发明针对三目标问题的特点以及蚁群算法容易陷入局部最优的缺点，构建了判断算法陷入早熟的机制，依据当前解和支配解的分布情况，在已找到的支配解附近做微小的扰动，帮助算法脱离局部最优，找到质量更优的解。

附图说明

图1为本发明的机器人与障碍物的碰撞情况示意图；

图2为本发明的路径0-1附近无障碍栅格示意图；

图3为本发明的路径2-46附近的障碍栅格示意图；

具体实施方式

下面结合附图对本发明作进一步的说明：

如图1所示，一种求解三目标机器人路径规划问题的方法，方法如下：机器人在选择下一个栅格的过程中，除了要避开障碍栅格，还要避免与障碍栅格的碰撞，假定机器人为一半径为r的圆形质点，机器人从栅格0移动到栅格5的移动轨迹，如果考虑到机器人本身的大小，机器人从栅格0移动到栅格5会与障碍栅格2、3发生碰撞，因此，栅格5不能作为栅格0下一步移动的栅格，机器人从栅格0到栅格5移动的轨迹位于a、b、c、d所在区域，机器人在移动过程中需要先判断ad、bc、05三条线段是否会与障碍栅格区域发生相交，机器人需要保证在移动过程中不发生碰撞即要保证图中ad、bc、05三条线段都不都能与任何一个障碍栅格区域发生相交，线段ad、bc、05的数学公式如下：

假设x[i]，y[i]表示出发点i的位置坐标(0号栅格)。i+1表示出发点可能移动的下一个点(5号栅格)，r为机器人的半径；

线段ad公式如下：

其中：

线段bc公式如下：

其中：

线段05公式如下：

(y[i+1]-y[i])·x+(x[i]-x[i+1])·y+x[i+1]·y[i]-x[i]·y[i+1]＝0

x∈[x[i]，x[i+1]] y∈[y[i]，y[i+1]；

如图2和图3所示，机器人选择下一个栅格移动的过程中，只要满足其移动路径不与障碍栅格发生碰撞的任意非障碍栅格都可作为备选栅格，机器人所在的每一段路径与该段路径附近障碍点的距离直接决定该路径的安全性，以路径中相邻两个栅格的中点(即每一段路径的中点)为原点，半径为R的圆作为该段路径的附近区域(R＝dist_ij/2+r，dist_ij表示该段路径的长度，r为机器人的半径)，如果在该区域内不存在障碍栅格，则表明该段路径安全性最佳，安全距离safedist可设置为一个较大的数，如图2所示，路径0-1周边无障碍栅格。如果在该区域存在一个或多个障碍栅格，则计算各障碍栅格中心点到该段路径的距离，选取其中最小的值(如果只有一个障碍栅格，最小值即为该障碍栅格中心点到该段路径的距离)，并将该值设置为安全距离safedist，如图3所示，路径2-46附近的障碍栅格为栅格11，27，32，42，56。

一种求解三目标机器人路径规划问题的方法，具体算法如下：

(1)初始化：初始化参数α、β、γ，Q，初始化启发信息矩阵η，d_k，l表示栅格k和栅格l之间的距离；

初始化信息素矩阵τ，该矩阵中各元素初始值为1，构造外部档案集E，E初始时内部没有任何解，档案集大小为M，迭代最大次数N_max，当前迭代次数N＝0；

(2)构建解：N＝N+1，蚂蚁在搜索过程中，会根据小组信息素τ、蚂蚁的启发信息η，蚂蚁访问该栅格的次数V(V_l表示蚂蚁访问l点的次数，V_l越大，表示l栅格作为下个目标点的概率越小，目的是指导蚂蚁优先寻找新的路径以避免陷入局部最优)来计算状态转移概率，p_k，l表示蚂蚁从栅格k转移到栅格l的转移概率；

allowed表示蚂蚁i下一步允许选择的所有栅格的集合，允许选择的栅格为不与蚂蚁当前路径发生碰撞的所有非障碍栅格的集合；

根据该状态转移概率，蚂蚁构建新的解，如果该解被档案集中的解支配，则该解不能进入档案集，把表示当前解没有进入档案集次数的参数T(初值为0)加1，转步骤4)；如果档案集E中的解的个数小于N，同时该解没有被档案集中的解支配，将该解放入档案集E，T＝0，转步骤3)；如果档案集E中的解的个数等于N，且该解没有被档案集中的解支配，转步骤5)；

(3)更新信息素

分别表示当前解路径的路径长度，夹角大小和安全距离的倒数，K,l表示当前解路径上相邻的2个栅格编号，转步骤2)，直到达到最大迭代次数N_max结束，输出档案集中的解；

(4)判断T是否大于常数Tmax(Tmax为预先设定好的值)，如果T>Tmax，则说明算法多次计算没有找到支配解，算法陷入早熟和局部最优，计算档案集中各支配解个体之间的D(i)值，D(i)用于衡量archives(档案集)中解的分布情况；

K＝|archives|/2

表示解i到archives中第k个邻近个体(中心个体)之间的距离，需要计算个体i到archives中所有个体之间的距离，并按照増序排列，第k个个体为中心个体，D(i)值越小，意味着该个体分布在相对分散的区域，选取archives中D(i)值最小的支配解，按照(3)中公式，依据该支配解路径调整信息素，算法会在解集分散的区域做局部搜索，防止算法陷入局部最优，保证了解的均匀分布，如果T<Tmax，转步骤3)，依据当前解信息调整信息素。

(5)计算current solution(档案集中解和当前解的并集)中各解的D(i)值；

K＝|current solution|/2

表示解i到current solution中第k个邻近个体之间的距离，需要计算个体i到current solution中所有个体之间的距离，并按照増序排列，第k个个体为中心个体；

删除掉D(i)值最大的那个解，如果删除的解不是当前解，则转步骤3)，依据当前解的信息调整信息素，如果删除的解是当前解，转步骤3)，依据current solution中D(i)值最小的支配解路径调整信息素。

判断线段是否与障碍栅格区域发生相交的方法为：

(1)求出栅格i的中点(x_i，y_i)到栅格j的中点(x_j，y_j)的直线方程

(2)求该直线与任一障碍栅格的四条边所在的直线的交点；如果没有交点，则该路径不经过该障碍栅格；如果有交点，转步骤3)；

(3)如果该交点位于点(x_i，y_i)到(x_j，y_j)的线段上，同时该交点也位于该障碍栅格的四条边上，则栅格i到栅格j的路径经过该障碍栅格。

Claims (1)

1.一种求解三目标机器人路径规划问题的方法，其特征在于：具体算法如下：

(1)初始化：初始化参数α、β、γ，Q，初始化启发信息矩阵η，d_k，l表示栅格k和栅格l之间的距离；

初始化信息素矩阵τ，该矩阵中各元素初始值为l，构造外部档案集E，E初始时内部没有任何解，档案集大小为M，迭代最大次数N_max，当前迭代次数N＝0；

(2)构建解：N＝N+1，蚂蚁在搜索过程中，会根据小组信息素τ、蚂蚁的启发信息η，蚂蚁访问该栅格的次数V来计算状态转移概率，p_k，l表示蚂蚁从栅格k转移到栅格l的转移概率；

V_I表示蚂蚁访问l点的次数，V_I越大，表示l栅格作为下个目标点的概率越小，目的是指导蚂蚁优先寻找新的路径以避免陷入局部最优；allowed表示蚂蚁i下一步允许选择的所有栅格的集合，允许选择的栅格为不与蚂蚁当前路径发生碰撞的所有非障碍栅格的集合；

根据该状态转移概率，蚂蚁构建新的解，如果该解被档案集E中的解支配，则该解不能进入档案集，把表示当前解没有进入档案集次数的参数T加1，其中，T的初值为0，转步骤4)；如果档案集E中的解的个数小于N，同时该解没有被档案集中的解支配，将该解放入档案集E，T＝0，转步骤3)；如果档案集E中的解的个数等于N，且该解没有被档案集中的解支配，转步骤5)；

(3)更新信息素

分别表示当前解路径的路径长度，夹角大小和安全距离的倒数，K,l表示当前解路径上相邻的2个栅格编号，转步骤2)，直到达到最大迭代次数N_max结束，输出档案集中的解；

(4)判断T是否大于常数Tmax，其中，Tmax为预先设定好的值，如果T>Tmax，则说明算法多次计算没有找到支配解，算法陷入早熟和局部最优，计算档案集中各支配解个体之间的D(i)值，D(i)用于衡量档案集archives中解的分布情况；

K＝|archives|/2

表示解i到archives中第k个邻近个体之间的距离，需要计算个体i到archives中所有个体之间的距离，并按照増序排列，第k个个体为中心个体，D(i)值越小，意味着该个体分布在相对分散的区域，选取archives中D(i)值最小的支配解，按照(3)中公式，依据该支配解路径调整信息素，算法会在解集分散的区域做局部搜索，防止算法陷入局部最优，保证了解的均匀分布，如果T<Tmax，转步骤3)，依据当前解信息调整信息素；

(5)计算档案集中解和当前解的并集current solution中各解的D(i)值；

K＝|currentsolution|/2

表示解i到current solution中第k个邻近个体之间的距离，需要计算个体i到current solution中所有个体之间的距离，并按照増序排列，第k个个体为中心个体；

删除掉D(i)值最大的那个解，如果删除的解不是当前解，则转步骤3)，依据当前解的信息调整信息素，如果删除的解是当前解，转步骤3)，依据current solution中D(i)值最小的支配解路径调整信息素。

Images