一种基于图像处理的移动机器人的路径规划与优化方法

1.本发明涉及了一种路径规划与优化方法，具体涉及一种基于图像处理的移动机器人的路径规划与优化方法。


背景技术：

2.地面移动机器人结合了移动平台与操作手臂的优点，能在平面环境自由移动作业，可以有效扩大机器人的作业范围。目前在工业生产、仓储物流、科学探索等领域得到广泛的运用，随着移动机器人的运用环境日益复杂，路径规划的实时性与路径质量有了更高的要求。
3.在移动机器人的作业过程中，需要解决的问题包括环境感知与建模，路径规划与优化。利用各类相机进行摄影是一种经济方便的环境感知方法。利用图片信息，在环境障碍物复杂的情况下，对环境进行快速的感知、建模，进行路径规划与优化，是一个重要的难点问题。
4.通常，移动机器人的路径规划面向点到点的轨迹问题。需要在整体环境已知的情况下，从给定的起始位置出发运动到目标位置，并避开途中所有的障碍物。在目前的路径规划中，以栅格化环境为环境建模基础的a*算法能在该环境中找到一条距离最优的解。该路径与全局路径最优路径较为接近，但受栅格化环境的限制，存在较为尖锐的棱角与更长的路径长度。目前技术中仅提出一种在栅格中任意角度路径规划的方法，优化了a*算法所得的路径，但路径仍受到栅格环境的限制。


技术实现要素：

5.为了解决背景技术中存在的问题，本发明提供一种基于图像处理的移动机器人的路径规划与优化方法。在第一阶段，利用投影变换将图片中的机器人运动空间内的障碍物转换至真实环境图像中，用a*算法寻找一条可行路径。在第二阶段，提出一种障碍物图形识别方法，将环境中障碍物的不规则形状近似为直线，大大简化障碍物约束。以路径节点为变量，构建目标函数对问题进行描述求解，并提出使用交替极小化方法来有效地求解。
6.本发明采用的技术方案是：
7.本发明移动机器人的路径规划与优化方法包含如下步骤：
8.步骤100，拍摄获取移动机器人周围的环境静态图像，对环境静态图像进行二值化预处理，获得二值化环境静态图像，二值化环境静态图像中包括若干障碍物区域。
9.步骤200，使用投影变换法将二值化环境静态图像中的各个障碍物区域投影至投影变换图像，再通过最近邻插值法将投影变换图像中的各个障碍物区域进行插值处理，获得真实环境图像。
10.步骤300，利用栅格化对真实环境图像进行建模获得地图环境模型，在地图环境模型中利用a*算法获取移动机器人的初始路径。
11.步骤400，通过轮廓处理将步骤200中的真实环境图像中的每个障碍物区域的外轮
廓均进行线段拟合，从而获得环境障碍物模型。
12.步骤500，根据步骤300中的移动机器人的初始路径建立目标函数，并通过步骤400中的环境障碍物模型建立目标函数的环境障碍物约束。
13.利用交替最小化方法对目标函数进行优化，获得最终优化路径作为移动机器人的目标路径。
14.所述的步骤100中，二值化预处理具体为通过图像滤波法和灰度阈值法处理环境静态图像，获得二值化环境静态图像，并将二值化环境静态图像中灰度值高于预设灰度阈值的若干相间隔的区域均作为障碍物区域，即获得若干障碍物区域。
15.所述的步骤300中，将真实环境图像进行栅格化获得大小一致的若干栅格，由大小一致的若干栅格构成了地图环境模型；根据移动机器人的初始位置和预设目标位置，结合地图环境模型，利用a*算法，获得移动机器人从初始位置出发至预设目标位置的初始路径p
(0)
，使得初始路径p
(0)
不与地图环境模型中的各个障碍物区域重合，其中，分别为初始路径的第一个、第二个、...第n个初始路径点位置，为移动机器人的初始位置，为移动机器人的预设目标位置，均分别位于地图环境模型中的栅格的顶点。
16.初始路径p
(0)
中任意两个相邻初始路径点位置之间的连线为移动机器人在初始路径p
(0)
中移动的其中一条初始子路径，即初始路径p
(0)
是由n-1条初始子路径依次连接构成的折线。
17.所述的步骤400中，使用连通块处理，将步骤200中的真实环境图像的各个障碍物区域单独分离；针对每个障碍物区域，获取障碍物区域的外轮廓，通过轮廓处理将外轮廓进行线段拟合，具体如下：
18.1)将障碍物区域的外轮廓均匀划分为i个轮廓点，依次为第一个轮廓点g1、第二个轮廓点g2、
…
、第i个轮廓点gi，依次连接i个轮廓点并形成闭环，构成i条轮廓向量线段，依次为第一条轮廓向量线段s1、第二条轮廓向量线段s2、
…
、第i条轮廓向量线段si。
19.2)依次遍历计算第一条轮廓向量线段s1与该条轮廓向量线段之后的若干轮廓向量线段之间的夹角，每条轮廓向量线段计算夹角时以该条轮廓向量线段的连线方向作为轮廓向量线段的方向，夹角计算具体如下：
20.若依次连续遍历计算获得的所有夹角均小于等于预设阈值角度ψ，则不进行操作，直接将第一条轮廓向量线段s1作为第一拟合线段。
21.否则，直至第i次计算时，当次计算获得的第一条轮廓向量线段s1与第i条轮廓向量线段si之间的夹角大于预设阈值角度ψ，则将第一条轮廓向量线段s1至第i条轮廓向量线段si之间的所有轮廓向量线段进行直线线段拟合合并作为第一拟合线段，即将第一个轮廓点g1与第i+1个轮廓点g
i+1
直接连接形成的直线段作为第一拟合线段，停止当前遍历计算，其中，1《i《i。
22.3)依次将第一拟合线段之后的若干轮廓向量线段均进行上述步骤2)中的第一条轮廓向量线段s1的相同的操作，依次获得第二拟合线段、第三拟合线段、
…
、第j拟合线段，其中，1《i《j≤i。
23.4)依次遍历计算第一拟合线段与该条拟合线段之后的若干拟合线段各自的线段长度：
24.若依次遍历计算获得的线段长度均大于预设长度μ，则不进行操作，将第一拟合线段、第二拟合线段、
…
、第j拟合线段分别直接作为第一边界线段l1、第二边界线段l2、
…
、第j边界线段lj，将障碍物区域的外轮廓替换为由第一边界线段l1、第二边界线段l2、
…
、第j边界线段lj围成的轮廓。
25.若直至第k次计算时，当次计算获得的第k拟合线段的线段长度小于等于预设长度μ，第k拟合线段之前的各条拟合线段的线段长度均大于预设长度μ，则将第一拟合线段、第二拟合线段、
…
、第k-1拟合线段分别直接作为第一边界线段l1、第二边界线段l2、
…
、第k-1边界线段l
k-1
，将第k拟合线段与第k+1拟合线段进行线段拟合合并为一条线段作为第k边界线段lk；继续将第k边界线段lk以及第k边界线段lk之后的若干拟合线段均进行与第k拟合线段的相同的操作，依次获得第k+1边界线段l
k+1
、第k+1边界线段l
k+2
、
…
、第l边界线段l
l
，其中，1≤k《l《j≤i；将障碍物区域的外轮廓替换为由第一边界线段l1、第二边界线段l2、
…
、第l边界线段l
l
围成的轮廓。
26.若直至最后次计算时，当前最后次计算获得的第j拟合线段的线段长度小于等于预设长度μ，第j拟合线段之前的各条拟合线段的线段长度均大于预设长度μ，或者第j拟合线段和第一拟合线段共线，则将第j拟合线段和第一拟合线段进行线段拟合合并为一条线段作为第一边界线段l1，将第二拟合线段、第三拟合线段、
…
、第j-1拟合线段分别直接作为第二边界线段l2、第三边界线段l3、
…
、第j-1边界线段l
j-1
；将障碍物区域的外轮廓替换为由第一边界线段l1、第二边界线段l2、
…
、第j-1边界线段l
j-1
围成的轮廓。
27.从而将线段拟合后的真实环境图像构建获得环境障碍物模型。
28.所述的步骤500中，根据步骤300中的移动机器人的初始路径建立的目标函数如下：
[0029][0030]
其中，p为优化路径，p＝[p1，p2，...，pn，...，pn]，p1、p2、...、pn、...、pn分别为优化路径p的第一个、第二个、...、第n个、...、第n个优化路径点位置；和p1均为移动机器人的初始位置，和pn均为移动机器人的预设目标位置；λ为可调的正则化参数。
[0031]
其中，表示优化路径p中第n个优化路径点位置与初始路径p
(0)
中第n个初始路径点位置的距离；优化路径p中任意两个相邻优化路径点位置之间的连线为移动机器人在优化路径p中移动的其中一条优化子路径，即优化路径是由n-1条优化子路径依次连接构成的折线；|p
n+1-pn|表示优化路径p中第n+1个优化路径点位置与第n个优化路径点位置之间的距离，即表示优化路径中第n条优化子路径的长度。
[0032]
目标函数的第一项用于控制优化路径与初始路径之间的距离；目标函数的第二项用于平衡优化路径的长度以及目标函数的运算速度。目标函数输出的优化路径p满足总路径长度最短。
[0033]
通过步骤400中的环境障碍物模型建立的目标函数的环境障碍物约束，具体如下：
[0034]
针对优化路径p中的优化路径点位置pn，设优化路径点位置pn的坐标为[xn，yn]。针对步骤400中获得的由若干边界线段围成的轮廓构成的其中一个障碍物区域，获取障碍物区域的轮廓上距离优化路径点位置pn距离最短的一个点m，设m点的坐标为[xm，ym]，则环境障碍物约束具体为：
[0035][0036]
其中，d为预设路径距离。
[0037]
或者，为方便计算，环境障碍物约束具体为由m点在优化路径点位置pn处使用泰勒展开获得的一阶线性约束作为环境障碍物约束。
[0038]
所述的步骤500中，利用交替最小化方法对目标函数f(p)进行凸优化迭代，具体如下：
[0039]
将初始路径p
(0)
作为优化路径p输入目标函数f(p)中依次进行多次凸优化迭代，直至第u次凸优化迭代和第u-1次凸优化迭代获得的第u优化迭代路径和第u-1优化迭代路径满足||f(p(
u)
)-f(p
(u-1)
)||≤ε，停止凸优化迭代，u次凸优化迭代具体如下：
[0040][0041]
其中，||f(p
(u)
)-f(p
(u-1)
)||表示目标函数在第u次凸优化迭代和第u-1次凸优化迭代的差值，ε为预设阈值，和p
(2)
分别为第一次凸优化迭代获得的第一优化迭代路径以及第二次凸优化迭代获得的第二优化迭代路径；argmin表示凸优化迭代；u为正整数。
[0042]
第u优化迭代路径p
(u)
即为移动机器人的最终优化路径，作为移动机器人的目标路径控制移动机器人进行移动。
[0043]
本发明的有益效果是：
[0044]
本发明公开的一种基于图像处理的移动机器人路径规划与优化方法。能够从简单的图片信息出发，利用投影变换法自动换算真实坐标中障碍物所处的位置，通过自动拟合障碍物完成移动机器人的环境建模，利用a*算法生成初始路径，再用交替极小值方法对路径进行进一步的优化，从而生成一条能与障碍物保持安全距离且路径长度接近最优的移动路径。
附图说明
[0045]
图1为本方法总体流程图；
[0046]
图2为由二值化环境静态图像到真实环境图像的投影变换示意图(黑色部分为障碍物区域)；
[0047]
图2的(a)为二值化环境静态图像的障碍物区域示意图；
[0048]
图2的(b)为经投影变换法转至真实环境图像的障碍物区域示意图；
[0049]
图3为移动机器人行进的初始路径和最终优化路径；
[0050]
图3的(a)为移动机器人沿a*算法所规划的初始路径行进的路线示意图；
[0051]
图3的(b)为移动机器人沿优化后的最终优化路径行进的路线示意图。
具体实施方式
[0052]
下面结合附图及具体实施例对本发明作进一步详细说明。
[0053]
如图1所示，本发明方法包含如下步骤：
[0054]
步骤100，拍摄获取移动机器人周围的环境静态图像，对环境静态图像进行二值化预处理，获得二值化环境静态图像，二值化环境静态图像中包括若干障碍物区域。
[0055]
二值化预处理具体为通过图像滤波法和灰度阈值法处理环境静态图像，获得二值化环境静态图像，并将二值化环境静态图像中灰度值高于预设灰度阈值的若干相间隔的区域均作为障碍物区域，即获得若干障碍物区域。
[0056]
步骤200，使用投影变换法将二值化环境静态图像中的各个障碍物区域投影至投影变换图像，再通过最近邻插值法将投影变换图像中的各个障碍物区域进行插值处理，获得真实环境图像。
[0057]
步骤300，利用栅格化对真实环境图像进行建模获得地图环境模型，在地图环境模型中利用a*算法获取移动机器人的初始路径，如图3的(a)所示。
[0058]
将真实环境图像进行栅格化获得大小一致的若干栅格，由大小一致的若干栅格构成了地图环境模型；根据移动机器人的初始位置和预设目标位置，结合地图环境模型，利用a*算法，获得移动机器人从初始位置出发至预设目标位置的初始路径p
(0)
，使得初始路径p
(0)
不与地图环境模型中的各个障碍物区域重合，其中，分别为初始路径的第一个、第二个、...第n个初始路径点位置，为移动机器人的初始位置，为移动机器人的预设目标位置，均分别位于地图环境模型中的栅格的顶点。
[0059]
初始路径p
(0)
中任意两个相邻初始路径点位置之间的连线为移动机器人在初始路径p
(0)
中移动的其中一条初始子路径，即初始路径p
(0)
是由n-1条初始子路径依次连接构成的折线。
[0060]
步骤400，通过轮廓处理将步骤200中的真实环境图像中的每个障碍物区域的外轮廓均进行线段拟合，从而获得环境障碍物模型。
[0061]
使用连通块处理，将步骤200中的真实环境图像的各个障碍物区域单独分离；针对每个障碍物区域，获取障碍物区域的外轮廓，通过轮廓处理将外轮廓进行线段拟合，具体如下：
[0062]
1)将障碍物区域的外轮廓均匀划分为i个轮廓点，依次为第一个轮廓点g1、第二个轮廓点g2、
…
、第i个轮廓点gi，依次连接i个轮廓点并形成闭环，构成i条轮廓向量线段，依次为第一条轮廓向量线段s1、第二条轮廓向量线段s2、
…
、第i条轮廓向量线段si。
[0063]
2)依次遍历计算第一条轮廓向量线段s1与该条轮廓向量线段之后的若干轮廓向量线段之间的夹角，每条轮廓向量线段计算夹角时以该条轮廓向量线段的连线方向作为轮廓向量线段的方向，夹角计算具体如下：
[0064]
若依次连续遍历计算获得的所有夹角均小于等于预设阈值角度ψ，则不进行操作，直接将第一条轮廓向量线段s1作为第一拟合线段。
[0065]
否则，直至第i次计算时，当次计算获得的第一条轮廓向量线段s1与第i条轮廓向量线段si之间的夹角大于预设阈值角度ψ，则将第一条轮廓向量线段s1至第i条轮廓向量线段si之间的所有轮廓向量线段进行直线线段拟合合并作为第一拟合线段，即将第一个轮廓点g1与第i+1个轮廓点g
i+1
直接连接形成的直线段作为第一拟合线段，停止当前遍历计算，其中，1《i《i。
[0066]
3)依次将第一拟合线段之后的若干轮廓向量线段均进行上述步骤2)中的第一条轮廓向量线段s1的相同的操作，依次获得第二拟合线段、第三拟合线段、
…
、第j拟合线段，其中，1《i《j≤i。
[0067]
4)依次遍历计算第一拟合线段与该条拟合线段之后的若干拟合线段各自的线段长度：
[0068]
若依次遍历计算获得的线段长度均大于预设长度μ，则不进行操作，将第一拟合线段、第二拟合线段、
…
、第j拟合线段分别直接作为第一边界线段l1、第二边界线段l2、
…
、第j边界线段lj，将障碍物区域的外轮廓替换为由第一边界线段l1、第二边界线段l2、
…
、第j边界线段lj围成的轮廓。
[0069]
若直至第k次计算时，当次计算获得的第k拟合线段的线段长度小于等于预设长度μ，第k拟合线段之前的各条拟合线段的线段长度均大于预设长度μ，则将第一拟合线段、第二拟合线段、
…
、第k-1拟合线段分别直接作为第一边界线段l1、第二边界线段l2、
…
、第k-1边界线段l
k-1
，将第k拟合线段与第k+1拟合线段进行线段拟合合并为一条线段作为第k边界线段lk；继续将第k边界线段lk以及第k边界线段lk之后的若干拟合线段均进行与第k拟合线段的相同的操作，依次获得第k+1边界线段l
k+1
、第k+1边界线段l
k+2
、
…
、第l边界线段l
l
，其中，1≤k《l《j≤i；将障碍物区域的外轮廓替换为由第一边界线段l1、第二边界线段l2、
…
、第l边界线段l
l
围成的轮廓。
[0070]
若直至最后次计算时，当前最后次计算获得的第j拟合线段的线段长度小于等于预设长度μ，第j拟合线段之前的各条拟合线段的线段长度均大于预设长度μ，或者第j拟合线段和第一拟合线段共线，则将第j拟合线段和第一拟合线段进行线段拟合合并为一条线段作为第一边界线段l1，将第二拟合线段、第三拟合线段、
…
、第j-1拟合线段分别直接作为第二边界线段l2、第三边界线段l3、
…
、第j-1边界线段l
j-1
；将障碍物区域的外轮廓替换为由第一边界线段l1、第二边界线段l2、
…
、第j-1边界线段l
j-1
围成的轮廓。
[0071]
从而将线段拟合后的真实环境图像构建获得环境障碍物模型。
[0072]
步骤500，根据步骤300中的移动机器人的初始路径建立目标函数，并通过步骤400中的环境障碍物模型建立目标函数的环境障碍物约束；利用交替最小化方法对目标函数进行优化，获得最终优化路径作为移动机器人的目标路径。
[0073]
根据步骤300中的移动机器人的初始路径建立的目标函数如下：
[0074][0075]
其中，p为优化路径，p＝[p1，p2，...，pn，...，pn]，p1、p2、...、pn、...、pn分别为优化
路径p的第一个、第二个、...、第n个、...、第n个优化路径点位置；和p1均为移动机器人的初始位置，和pn均为移动机器人的预设目标位置；λ为可调的正则化参数。
[0076]
其中，表示优化路径p中第n个优化路径点位置与初始路径p
(0)
中第n个初始路径点位置的距离；优化路径p中任意两个相邻优化路径点位置之间的连线为移动机器人在优化路径p中移动的其中一条优化子路径，即优化路径是由n-1条优化子路径依次连接构成的折线；|p
n+1-pn|表示优化路径p中第n+1个优化路径点位置与第n个优化路径点位置之间的距离，即表示优化路径中第n条优化子路径的长度。
[0077]
目标函数的第一项用于控制优化路径与初始路径之间的距离；目标函数的第二项用于平衡优化路径的长度以及目标函数的运算速度。目标函数输出的优化路径p满足总路径长度最短。
[0078]
通过步骤400中的环境障碍物模型建立的目标函数的环境障碍物约束，具体如下：
[0079]
针对优化路径p中的优化路径点位置pn，设优化路径点位置pn的坐标为[xn，yn]。针对步骤400中获得的由若干边界线段围成的轮廓构成的其中一个障碍物区域，获取障碍物区域的轮廓上距离优化路径点位置pn距离最短的一个点m，设m点的坐标为[xm，ym]，则环境障碍物约束具体为：
[0080][0081]
其中，d为预设路径距离。
[0082]
或者，为方便计算，环境障碍物约束具体为由m点在优化路径点位置pn处使用泰勒展开获得的一阶线性约束作为环境障碍物约束。
[0083]
利用交替最小化方法对目标函数f(p)进行凸优化迭代，具体如下：
[0084]
将初始路径p
(0)
作为优化路径p输入目标函数f(p)中依次进行多次凸优化迭代，直至第u次凸优化迭代和第u-1次凸优化迭代获得的第u优化迭代路径和第u-1优化迭代路径满足||f(p
(u)
)-f(p
(u-1)
)||≤ε，停止凸优化迭代，u次凸优化迭代具体如下：
[0085][0086]
其中，||f(p
(u)
)-f(p
(u-1)
)||表示目标函数在第u次凸优化迭代和第u-1次凸优化迭代的差值，ε为预设阈值，和p
(2)
分别为第一次凸优化迭代获得的第一优化迭代路径以及第二次凸优化迭代获得的第二优化迭代路径；argmin表示凸优化迭代；u为正整数。
[0087]
第u优化迭代路径p
(u)
即为移动机器人的最终优化路径，作为移动机器人的目标路径控制移动机器人进行移动，如图3的(b)所示。