一种基于RRT的非规则形状移动机器人的路径规划方法

一种基于rrt的非规则形状移动机器人的路径规划方法
技术领域
1.本发明属于移动机器人路径规划技术领域，尤其涉及一种基于rrt的非规则形状移动机器人的路径规划方法。


背景技术：

2.路径规划是移动机器人研究领域的一个基本问题，主要目标是在工作环境中寻找一条合适的轨迹以有效避开障碍物。随着路径规划研究的深入，为了满足不同工作环境的需求，提出了许多规划算法，如dijkstra、a*、细胞分解法、人工势场法、进化算法等。快速探索随机树(rrt)一种基于采样的路径规划算法，适合解决高维空间和复杂约束下的路径规划问题，其基本思想是以产生随机点的方式通过一个步长向目标点搜索前进，有效躲避障碍物，避免路径陷入局部极小值，显示出了高效率和强大的路径搜索能力。
3.路径优化是rrt研究的一个重要方向，rrt*是对rrt算法的主要改进，它的基本思想是在探索随机树的生长过程中，通过增加生成随机点的数量、优化节点的连接方式(即父节点选择和重新布线)，提高路径的合理性。但是在有运动学约束的情况下，机器人在工作空间的状态包括位置坐标和方向角，随着采样点的增加，重复计算采样点是否满足机器人的运动学约束、并且比较与原路径节点的合理性需要大量的计算，非常耗时，严重制约了路径规划的效率。另一方面，规划的路径是通过采样点连接而成，由于采样点是随机的，这就导致在同样的工作空间，每次规划获得的路径也是随机的，即规划路径不收敛。


技术实现要素：

4.针对上述技术问题，本发明的一个方式的目的之一提出一种基于rrt的非规则形状移动机器人的路径规划方法，通过两棵探索树仅在机器人的运动学条件下生长，在生长过程中，不考虑路径优化问题，有效提高了初始路径的探索效率；本发明的一个方式的目的之一是在运动学约束条件下，提出一种路径节点状态转移方法，可以将随机路径优化到最短路径；本发明的一个方式的目的之一是路径节点状态转移仅仅针对随机路径上的节点，而不对非路径上的采样节点进行运算，极大的减小了优化计算工作量，优化路径具有很好的收敛性。本发明的一个方式的目的之一通过构建核矩阵描述机器人的形状特征，并采用卷积计算分析机器人形状和周围障碍物的交互作用，所提出的方法可以适用于任意形状的移动机器人路径规划。
5.注意，这些目的的记载并不妨碍其他目的的存在。本发明的一个方式并不需要实现所有上述目的。可以从说明书、附图、权利要求书的记载中抽取上述目的以外的目的。
6.本发明提出一种基于rrt的非规则形状移动机器人的路径规划方法，不同于rrt*算法，是一种快速的随机路径规划算法，并在有运动学约束的条件下对随机路径的节点进行状态转移，获得稳定收敛的最短路径。
7.本发明将工作空间均匀划分成网络结构，采用核矩阵描述机器人的形状特征，并通过核矩阵旋转描述机器人在不同方位角的形状特征，考虑机器人形状和在工作空间内障
碍物形状的交互作用，通过卷积计算得到机器人在工作空间(x，y，θ)的状态是否可行；随机路径生成中，从初始状态和目标状态同时生成两棵探索树，采用双向rrt探索路径节点，新节点只需要满足机器人的运动学特征和(x，y，θ)状态可行两个条件，两颗探索树同时相向生成路径节点，并提出了在严格的运动学约束下进行连接，以最快速度找到到达目标的初始随机路径；针对随机路径上的节点，提出满足运动学约束的节点状态转移方法，通过迭代计算，使得初始随机路径向最短路径逐步收敛。
8.本发明是通过以下技术手段实现上述技术目的的。
9.一种基于rrt的非规则形状移动机器人的路径规划方法，包括以下步骤：
10.步骤s1、随机路径生成：在随机路径过程中，需要构建工作空间网络和移动机器人形状核矩阵，确定移动机器人运动学约束，首先，分别以移动机器人在工作空间的初始状态s0和目标状态sg为初始路径节点，同时生成两棵探索树；然后，采用生成随机采样点，并根据运动学约束、节点状态和路径可行性判断采样点作为路径节点的可行性，通过连续生成随机采样点和路径节点的可行性判断，实现两棵探索树的生长，直到两棵探索树在运动学约束条件下相互连接，得到随机路径；
11.步骤s2、路径优化：基于随机路径上的节点状态转移的路径优化方法，通过节点状态的循环迭代实现路径的优化。
12.上述方案中，所述构建工作空间网络的步骤具体为：
13.将移动机器人的工作空间沿x方向和y方向均匀划分成网络结构，定义为t，网络t采用规则形状划分；
14.网络t的每个节点t(x，y)包括自由空间和障碍物占据两种状态，自由空间的节点函数值设定为t(x，y)＝0，障碍物占据区域的节点函数值设定为t(x，y)＝1；
15.对于非规则工作空间，将工作空间以外的区域全部设定为障碍物t(x，y)＝1，建立规则的工作空间网络t。
16.上述方案中，所述构建机器人形状核矩阵的步骤具体为：
17.将移动机器人所占据区域扩展成规则的矩形或正方形区域，并进行均匀划分，构成核矩阵w，网络的形式与工作空间网络t相同，机器人形状核矩阵w放置于工作空间网络t中，节点位置的一一对应；
18.核矩阵w的每个元素包括自由空间和机器人占据两种状态，自由空间的元素值设定为w
θ
(m，n)＝0，机器人占据的元素值设定为w
θ
(m，n)＝1；
19.当移动机器人处于不同方向角时，通过核矩阵w绕后轮中心坐标(x，y)旋转获得，自由空间的元素值依然设定为w
θ
(m，n)＝0，移动机器人占据的元素值设定为w
θ
(m，n)＝1。
20.上述方案中，所述移动机器人运动学约束的要求具体为：
21.假设移动机器人在工作空间中的状态为s＝(x，y，θ)，根据ackermann转向约束条件，机器人前轮转向角受机械结构约束，因此，机器人的转弯半径ρ不能小于其最小值，即其中l为前后轮距，为最大前轮转向角。
22.上述方案中，所述构建初始随机路径的步骤具体为：
23.从初始状态s0和目标状态sg同时建立两棵探索树，分别定义为ftree和btree，其实质为路径节点的集合，ftree的第一个节点为机器人的初始状态s0＝(x0，y0，θ0)，btree的第一个节点为机器人的目标状态sg＝(xg，yg，θg)；随机路径生成包括三个过程：随机采样点的
生成、路径节点的连接和两棵探索树的连接。
24.进一步的，所述随机采样点的生成的步骤具体为：
25.设定采样概率a和步长l，0《a《100％，l为正常数；
26.ftree的生长以概率a选择目标状态sg＝(xg，yg，θg)作为生长方向，以概率(1-a)朝随机方向生长，定义随机生成点为sr＝(xr，yr，θr)；
27.延伸探索步长为l，设定采样点为ss＝(xs，ys，θs)；
28.btree的生长方式与ftree的不同之处在于：一方面btree以概率a选择朝向初始状态s0＝(x0，y0，θ0)，以概率(1-a)朝随机方向生长；另一方面机器人为反向行驶，即倒车。
29.进一步的，所述路径节点的连接步骤具体为：
30.从ftree已经有的节点中找到距离sr最近的节点s
p
作为父节点，沿着s
p
sr方向延伸探索步长为l，生成采样点为ss＝(xs，ys，θs)，然后进行s
p
→ss
路径的可行性判断，具体包括运动学约束、节点状态判断和路径可行性判断三个方面。
31.进一步的，所述运动学约束判断具体为：若采样点ss在机器人能够到达的区域，则进行下一步判断；否则，ss不满足运动学约束，重新进行采样；
32.所述节点状态判断具体为：根据采样点ss的状态，计算机器人采样点ss的核矩阵w
θs
(xs，ys)，将工作空间网络t与采样点ss的核矩阵w
θs
进行卷积计算，h
(xs，ys，θs)
＝t*w
θs
，其中“*”为卷积计算符，若h
(xs，ys，θs)
＝0，表示采样点ss的状态可行，则进行下一步判断；否则，重新进行采样；
33.所述路径可行性判断具体为：建立s
p
→ss
路径，将其划分为多个子路径，分别计算子路径的节点状态s
11
＝(x
11
，y
11
，θ
11
)、s
12
＝(x
12
，y
12
，θ
12
)，然后，采用节点状态判断的方法，判断s
11
和s
12
是否可行，若均可行，则将采样点ss加入ftree，并且定义ss的父节点为s
p
，否则，重新进行采样。
34.进一步的，所述两棵探索树的连接的步骤具体为：
35.通过连续的采样，ftree和btree会相向生长，即节点的距离相互缩短；
36.定义距离阈值ε0，设定为3～4ρ
min
；
37.计算ftree和btree上最近的节点距离ε，当ε》ε0，则继续采样，若ε《ε0，则进行两棵探索树的连接判断；所述两棵探索树的连接判断方法如下：
38.假设节点s
fm
和s
bn
为ftree和btree上最近的节点，根据s
fm
和s
bn
的状态，分别构建各自的最小转弯半径圆；
39.找到两个最小转弯半径圆的内公切线和外公切线，若公切线存在、且满足s
fm
和s
bn
的方向角，则ftree和btree能够在运动学约束下相互连接，得到一条连接初始状态s0和目标状态sg的随机路径，定义为p
rand
＝{s0,s
f1
,
…
,s
fm
,s
bn
,
…
,s
b1
,sg}，否则继续采样，直至ftree和btree能够相互连接。
40.进一步的，所述基于节点状态转移的路径优化方法的步骤具体为：
41.从初始状态s0至目标状态sg，依次选择连续的三个路径节点进行路径节点转移计算，完成一次路径的优化，得到一条新的优化路径；
42.所述路径节点转移计算具体为：假设初始生成的随机路径p
rand
上有三个连续的节点s1、s2和s3；
43.首先，计算节点s1和s3的两个最小转弯半径圆；
44.其次，根据s1和s3的方向角，计算两个最小转弯半径圆的外公切线或内公切线；
45.然后，找到公切线中心s2ˊ
，采用路径节点的连接方法，判断从s1→
s2ˊ
→
s3路径的可行性，若可行，则将路径节点从s2转移至s2ˊ
，否则，s2保持不变；
46.从初始状态s0至目标状态sg重复进行上述路径优化过程，初始随机路径会向最短路径逐步收敛。
47.与现有技术相比，本发明的有益效果是：根据本发明的一个方式，通过两棵探索树仅在机器人的运动学条件下生长，在生长过程中，不考虑路径优化问题，有效提高了初始路径的探索效率；根据本发明的一个方式，在运动学约束条件下，提出一种路径节点状态转移方法，可以将随机路径优化到最短路径；根据本发明的一个方式，路径节点状态转移仅仅针对随机路径上的节点，而不对非路径上的采样节点进行运算，极大的减小了优化计算工作量，优化路径具有很好的收敛性。根据本发明的一个方式，通过构建核矩阵描述机器人的形状特征，并采用卷积计算分析机器人形状和周围障碍物的交互作用，所提出的方法可以适用于任意形状的移动机器人路径规划。
48.注意，这些效果的记载不妨碍其他效果的存在。本发明的一个方式并不一定必须具有所有上述效果。可以从说明书、附图、权利要求书等的记载显而易见地看出并抽出上述以外的效果。
附图说明
49.图1是本发明一实施方式的机器人的工作空间划分示意图，其中图1(a)为规则工作空间示意图，图1(b)为非规则工作空间示意图。
50.图2是本发明一实施方式的机器人形状核矩阵构建示意图，其中图2(a)为θ＝0
°
方向示意图，图2(b)为θ＝45
°
方向示意图。
51.图3是本发明一实施方式的机器人运动学约束示意图。
52.图4是本发明一实施方式的路径节点的连接可行性示意图，其中图4(a)为路径可行示意图，图4(b)为采样点ss不可行示意图，图4(c)为路径过程中不可行示意图。
53.图5是本发明一实施方式的双向探索树生长示意图。
54.图6是本发明一实施方式的运动学约束下的双向探索树连接示意图，其中图6(a)为内公切线示意图，图6(b)为外公切线示意图。
55.图7是本发明一实施方式的初始随机路径规划方法流程图。
56.图8是本发明一实施方式的路径节点状态转移方法示意图，其中图8(a)为外公切线示意图，图8(b)为内公切线示意图。
57.图9是本发明一实施方式空旷工作环境下的规划路径。
58.图10是本发明一实施方式障碍工作环境下的规划路径。
59.图11是本发明一实施方式微小障碍工作环境下的路径优化。
具体实施方式
60.下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
61.本发明所述基于rrt的非规则形状移动机器人的路径规划方法，包括以下步骤：
62.步骤s1、随机路径生成：在随机路径过程中，需要构建工作空间网络和移动机器人形状核矩阵，确定移动机器人运动学约束，首先，分别以移动机器人在工作空间的初始状态s0和目标状态sg为初始路径节点，同时生成两棵探索树；然后，采用生成随机采样点，并根据运动学约束、节点状态和路径可行性判断采样点作为路径节点的可行性，通过连续生成随机采样点和路径节点的可行性判断，实现两棵探索树的生长，直到两棵探索树在运动学约束条件下相互连接，得到随机路径；
63.步骤s2、路径优化：基于随机路径上的节点状态转移的路径优化方法，通过节点状态的循环迭代实现路径的优化。
64.构建工作空间网络：
65.将移动机器人的工作空间沿x和y方向均匀划分成网络结构，定义为t，网络形式可以采用正方形、三角形等规则形状。
66.网络t的每个节点t(x，y)包括自由空间和障碍物占据两种状态，自由空间的节点函数值设定为t(x，y)＝0，障碍物占据区域的节点函数值设定为t(x，y)＝1。
67.对于非规则工作空间，将工作空间以外的区域全部设定为障碍物t(x，y)＝1，亦可建立规则的工作空间网络t。
68.构建机器人形状核矩阵：
69.将移动机器人所占据区域扩展成规则的矩形或正方形区域，并进行均匀划分，网络的形式应该与工作空间网络t相同，以保证机器人形状核矩阵w放置于工作空间网络t中，节点位置的一一对应。
70.构成核矩阵w，核矩阵w的每个元素包括自由空间和机器人占据两种状态，自由空间的元素值设定为w
θ
(m，n)＝0，机器人占据的元素值设定为w
θ
(m，n)＝1。
71.当机器人处于不同方向角时，可以通过核矩阵绕后轮中心坐标(x，y)旋转获得，自由空间的元素值依然设定为w
θ
(m，n)＝0，机器人占据的元素值设定为w
θ
(m，n)＝1。
72.机器人运动学约束：
73.假设机器人在工作空间中的状态为s＝(x，y，θ)，根据阿克曼ackermann转向约束条件，机器人前轮转向角受机械结构约束，因此，为了满足运动学约束，机器人的转弯半径ρ不能小于其最小值，即其中l为前后轮距，为最大前轮转向角。
74.构建初始随机路径：
75.首先，从初始状态s0和目标状态sg同时建立两棵探索树，分别定义为ftree和btree，其实质为路径节点的集合，ftree的第一个节点为机器人的初始状态s0＝(x0，y0，θ0)，btree的第一个节点为机器人的目标状态sg＝(xg，yg，θg)。
76.随机路径生成包括三个过程：随机采样点的生成、路径节点的连接和两棵探索树的连接。
77.第一步：随机采样点的生成
78.设定采样概率a和步长l，0《a《100％，l为正常数。
79.以ftree的生长为例。以概率a选择目标状态sg＝(xg，yg，θg)作为生长方向，以概率(1-a)朝随机方向生长，定义随机生成点为sr＝(xr，yr，θr)。
80.延伸探索步长为l，设定采样点为ss＝(xs，ys，θs)。
81.第二步：路径节点的连接
82.从ftree已经有的节点中找到距离sr最近的节点s
p
作为父节点，沿着s
p
sr方向延伸探索步长为l，生成采样点为ss＝(xs，ys，θs)，然后进行s
p
→ss
路径的可行性判断，具体包括三个方面。
83.①
运动学约束判断：若采样点ss在机器人可以到达的区域，则进行下一步判断；否则，ss不满足运动学约束，重新进行采样。
84.②
节点状态判断：根据采样点ss的状态，计算机器人采样点ss的核矩阵w
θs
(xs，ys)，将工作空间网络t与核矩阵w
θs
进行卷积计算，h
(xs，ys，θs)
＝t*w
θs
，其中“*”为卷积计算符，若h
(xs，ys，θs)
＝0，表示采样点状态ss可行，则进行下一步判断；否则，重新进行采样。
85.③
路径可行性判断：建立s
p
→ss
路径，将其划分为多个子路径，分别计算子路径的节点状态s
11
＝(x
11
，y
11
，θ
11
)、s
12
＝(x
12
，y
12
，θ
12
)，然后，采用上述节点状态判断的方法，判断s
11
和s
12
是否可行，若均可行，则将采样点ss加入ftree，并且定义ss的父节点为s
p
，否则，重新进行采样。这主要是为了避免路径中有小障碍的干扰。
86.btree的生长方式与ftree相同，不同之处在于两个方面：一方面btree以概率a选择朝向初始状态s0＝(x0，y0，θ0)，以概率(1-a)朝随机方向生长；另一方面机器人为反向行驶，即倒车。
87.第三步：两棵探索树的连接
88.通过连续的采样，ftree和btree会相向生长，即节点的距离相互缩短。
89.定义距离阈值ε0，通常可设定为3～4ρ
min
。
90.计算ftree和btree上最近的节点距离ε，当ε》ε0，则继续采样，若ε《ε0，则进行两棵探索树的连接判断。
91.连接判断方法如下：
92.假设节点s
fm
和s
bn
为ftree和btree上最近的节点，根据s
fm
和s
bn
的状态，分别构建各自的最小转弯半径圆。
93.找到两个最小转弯半径圆的内公切线和外公切线，若公切线存在、且满足s
fm
和s
bn
的方向角，则ftree和btree能够在运动学约束下相互连接，得到一条连接初始状态s0和目标状态sg的随机路径，定义为p
rand
＝{s0,s
f1
,
…
,s
fm
,s
bn
,
…
,s
b1
,sg}，否则继续采样，直至ftree和btree能够相互连接。
94.基于节点状态转移的路径优化方法：
95.从初始状态s0至目标状态sg，依次选择连续的三个路径节点进行路径节点转移计算，完成一次路径的优化，得到一条新的优化路径；
96.从初始状态s0至目标状态sg重复进行上述路径优化过程，初始随机路径会向最短路径逐步收敛。
97.所述路径节点转移计算具体为：假设初始生成的随机路径p
rand
上有三个连续的节点s1、s2和s3。
98.首先，计算节点s1和s3的两个最小转弯半径圆。
99.其次，根据s1和s3的方向角，计算两个最小转弯半径圆的外公切线或内公切线。
100.然后，找到公切线中心s2ˊ
，采用上述路径节点的连接方法，判断从s1→
s2ˊ
→
s3路
径的可行性，若可行，则将路径节点从s2转移至s2ˊ
，否则，s2保持不变。
101.具体实施例：
102.一种基于rrt的非规则形状移动机器人的路径规划方法，包括以下步骤：
103.工作空间网络划分方法，结合图1所示：
104.将移动机器人的工作空间沿x和y方向均匀划分成网络结构，定义为t，网络形式可以采用正方形、三角形等规则形状。
105.如图1(a)所示，根据本实施例，优选的，将工作空间划分为400
×
400的正方形网络结构，网络t的每个节点t(x，y)包括自由空间和障碍物占据两种状态，自由空间(白色)的节点函数值设定为t(x，y)＝0，障碍物占据区域(黑色)的节点函数值设定为t(x，y)＝1。
106.如图1(b)所示，对于非规则工作空间，将工作空间以外的区域全部设定为障碍物t(x，y)＝1，亦可建立规则的工作空间网络t。
107.机器人形状描述方法，结合图2所示：
108.如图2(a)所示，假设机器人的形状为“t”形，(x，y)表示机器人的后轮中心坐标，θ表示方向角，θ＝0
°
。将移动机器人所占据区域扩展成规则的矩形或正方形区域，并进行均匀划分，网络的形式应该与工作空间网络t相同，以保证机器人形状核矩阵w放置于工作空间网络t中，节点位置的一一对应。
109.构成核矩阵w，根据本实施例，优选的，如图2(a)所示，划分为40
×
40的正方形矩阵，矩阵的每个元素包括自由空间和机器人占据两种状态，自由空间的元素值设定为w0(m，n)＝0，机器人占据的元素值设定为w0(m，n)＝1。
110.当机器人处于不同方向角时，可以通过核矩阵绕后轮中心p坐标(x，y)旋转获得，如图2(b)所示，θ＝45
°
，自由空间的元素值依然设定为w
θ
(m，n)＝0，机器人占据的元素值设定为w
θ
(m，n)＝1。
111.机器人运动学约束，结合图3所示：
112.假设机器人在工作空间中的状态为s＝(x，y，θ)，根据ackermann转向约束条件，机器人前轮转向角受机械结构约束，因此，为了满足运动学约束，机器人的转弯半径ρ不能小于其最小值其中l为前后轮距。
113.图3给出了左、右转向的最小转弯半径圆弧，在两条圆弧中间区域为机器人可以行驶到达的区域，即满足运动学约束，两条圆弧以外区域不满足运动学约束。
114.随机路径生成方法，结合图4、图5、图6和图7所示：
115.首先，从初始状态和目标状态同时建立两棵探索树，分别定义为ftree和btree，其实质为路径节点的集合。ftree的第一个节点为机器人的初始状态s0＝(x0，y0，θ0)，btree的第一个节点为机器人的目标状态sg＝(xg，yg，θg)。
116.随机路径生成包括三个过程：随机采样点的生成、路径节点的连接和两棵探索树的连接。
117.第一步：随机采样点的生成
118.设定采样概率a和步长l，0《a《100％，l为正常数。
119.以ftree的生长为例。以概率a选择目标状态sg＝(xg，yg，θg)作为生长方向，以概率(1-a)朝随机方向生长，定义随机生成点为sr＝(xr，yr，θr)。
120.延伸探索步长为l，设定采样点为ss＝(xs，ys，θs)。
121.第二步：路径节点的连接
122.从ftree已经有的节点中找到距离sr最近的节点s
p
作为父节点，沿着s
p
sr方向延伸探索步长为l，生成采样点为ss＝(xs，ys，θs)，然后进行s
p
→ss
路径的可行性判断，具体包括三个方面：
123.①
运动学约束判断：若采样点ss在机器人可以到达的区域，则进行下一步判断；否则，ss不满足运动学约束，重新进行采样。
124.②
节点状态判断：根据采样点ss的状态，计算机器人的采样点ss核矩阵w
θs
(xs，ys)，将工作空间网络t与采样点ss核矩阵w
θs
进行卷积计算，h
(xs，ys，θs)
＝t*w
θs
，其中“*”为卷积计算符，若h
(xs，ys，θs)
＝0，表示采样点状态ss可行，则进行下一步判断；否则，重新进行采样。
125.③
路径可行性判断：建立s
p
→ss
路径，将其划分为多个子路径，如图4所示，优选的，划分为3个子路径，分别计算子路径的节点状态s
11
＝(x
11
，y
11
，θ
11
)、s
12
＝(x
12
，y
12
，θ
12
)，然后，采用上述节点状态判断的方法，判断s
11
和s
12
是否可行，若均可行，则将采样点ss加入ftree，并且定义ss的父节点为s
p
，否则，重新进行采样。这主要是为了避免路径中有小障碍的干扰。
126.如图4所示，图4(a)可以生成s
p
→ss
的路径，图4(b)由于采样点ss不可行，与障碍物碰撞，不能生成s
p
→ss
的路径，图4(c)由于s
p
→ss
路径中间有小障碍物，也不可行。
127.btree的生长方式与ftree相同，不同之处在于两个方面：一方面ftree以概率a选择朝向初始状态s0＝(x0，y0，θ0)，以概率(1-a)朝随机方向生长；另一方面机器人为反向行驶，即倒车。
128.第三步：两棵探索树的连接
129.如图5所示，通过连续的采样，ftree和btree会相向生长，即节点的距离相互缩短。
130.定义距离阈值ε0，通常可设定为3～4ρ
min
。
131.计算ftree和btree上最近的节点距离ε，当ε》ε0，则继续采样，若ε《ε0，则进行两棵探索树的连接判断。
132.连接判断方法如图6所示。
133.假设节点s
fm
和s
bn
为ftree和btree上最近的节点，根据s
fm
和s
bn
的状态，分别构建各自的最小转弯半径圆。
134.找到两个最小转弯半径圆的内公切线如图6(a)所示，外公切线如图6(b)所示，若公切线存在、且满足s
fm
和s
bn
的方向角，则ftree和btree能够在运动学约束下相互连接，得到一条连接初始状态s0和目标状态sg的随机路径，定义为p
rand
＝{s0,s
f1
,
…
,s
fm
,s
bn
,
…
,s
b1
,sg}，否则继续采样，直至ftree和btree能够相互连接。
135.生成随机路径的流程如图7所示。
136.基于节点状态转移的路径优化方法，结合图8所示：
137.假设初始生成的随机路径p
rand
上有三个连续的节点s1、s2和s3。
138.首先，计算节点s1和s3的两个最小转弯半径圆。
139.其次，根据s1和s3的方向角，计算两个最小转弯半径圆的外公切线图8(a)或内公切线图8(b)。
140.然后，找到公切线中心s2ˊ
，采用上述路径节点的连接方法，判断从s1→
s2ˊ
→
s3路径的可行性，若可行，则将路径节点从s2转移至s2ˊ
，否则，s2保持不变。
141.从初始状态s0至目标状态sg，依次选择连续的三个路径节点进行上述路径节点转移计算，完成一次路径的优化，得到一条新的优化路径。
142.从初始状态s0至目标状态sg重复进行上述路径优化过程，初始随机路径会向最短路径逐步收敛。
143.规划路径和优化结果结合，图9、图10和图11所示：
144.空旷工作环境：机器人在工作空间的状态用图2所示的p点表示，两棵探索树ftree(forward tree)和btree(backward tree)分别从初始状态initial和目标状态goal相向生长，得到随机路径。图9给出了两组仿真试验，得到了两条不同的随机路径。采用节点状态转移方法优化后，得到优化后的路径，两条优化路径几乎重合，具有良好的收敛性。收敛路径的基本特征是：机器人从初始状态首先沿着最小转弯半径圆调整方向，然后切线方向直线行驶到目标状态最小转弯半径圆，最后沿着最小转弯半径圆到达目标状态。这是满足机器人运动学约束的最短路径。
145.障碍工作环境：图10给出了两组仿真试验，得到了两条不同的随机路径。采用节点状态转移方法优化后，两条优化路径几乎重合，进一步验证了所提出的方法具有良好的收敛性。收敛路径的基本特征是：机器人从初始状态首先沿着最小转弯半径圆调整方向，沿着最小转弯半径圆依次绕过障碍物的a、b、c三个角，切线方向直线行驶到目标状态最小转弯半径圆，最后沿着最小转弯半径圆到达目标状态，如图10的convergence path所示。
146.微小障碍工作环境下的路径优化：如图11所示，初始生成的路径是随机的，如图11中空心小圆圈连接形成的路径，采用所提出的节点状态转移方法优化，可以有效避开小障碍物的影响，优化得到收敛路径，如图11中实心黑色小圆圈连接形成的路径。
147.本发明主要用于具有ackermann转向约束的任意形状移动机器人的路径规划，该方法包括随机路径探索和路径优化两个过程。首先将机器人的工作空间均匀划分成网络结构，采用核矩阵和核矩阵旋转方法描述任意形状移动机器人在不同方位角下情况下的形状特征，通过工作空间网络和核矩阵的卷积计算判断机器人在工作空间的状态；从初始状态和目标状态同时生成两棵探索树，采用双向rrt(rapidly-exploring random tree)方法探索路径节点，路径节点的生成需要同时满足机器人的转向约束和机器人在工作空间的状态两个条件，直到两棵探索树在严格的运动学约束条件下相互连接，获得初始随机路径；然后采用最小转弯圆切线连接法对初始随机路径上的节点进行状态(位置和方位角)的优化，通过连续的迭代计算，获得稳定收敛的局部最短路径。
148.应当理解，虽然本说明书是按照各个实施例描述的，但并非每个实施例仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。
149.上文所列出的一系列的详细说明仅仅是针对本发明的可行性实施例的具体说明，它们并非用以限制本发明的保护范围，凡未脱离本发明技艺精神所作的等效实施例或变更均应包含在本发明的保护范围之内。