[发明专利]一种基于Harris算法与RRT算法复合的自主路径规划方法

权利要求书

1.一种基于Harris算法与RRT算法复合的自主路径规划方法，其特征在于：包括以下步骤：

步骤1、以目标场地的外接矩形的任意一个顶角作为原点o，以所述原点o相连的两条边分别为x轴和y轴，从而建立二维的直角坐标系oxy；

在所述直角坐标系oxy中，将所述目标场地划分成M×N的二维的栅格地图B_M×N如公式(1)所示

其中，M表示所述栅格地图B_M×N的横向栅格数，N表示栅格地图B_M×N的纵向栅格数；GM_n表示所述栅格地图B_M×N上坐标为(n_x，n_y)的坐标点被占用的状态，M＞n_x＞0，N＞n_y＞0，若GM_n＝0表示所述的坐标点为空闲点，若GM_n＝1表示所述的坐标点为障碍点；

在栅格地图B_M×N中设置坐标为(s_x，s_y)的起始点node_s，以及坐标为(g_x，g_y)的目标点node_g；

步骤2、采用骨架提取算法对步骤1得到的栅格地图B_M×N进行计算，经过骨架提取算法计算后，栅格地图B_M×N中的连通区域分别被细化成细线，由此得到包含栅格地图中所有可行路径的多条细线；

步骤3、采用Harris角点检测算法对细化出来的各个可行路径分别进行计算，提取得到各个可行路径对应的特征点node_c，然后对各个特征点node_c进行自适应阈值处理，得到各自对应的拓扑节点node_t；

步骤4、根据拓扑节点node_t，建立拓扑结构地图map_t；

步骤5、采用随机快速搜索树算法，首先把拓扑节点node_t添加到随机快速搜索树算法的初始树Tree_i中，然后基于随机快速搜索树算法进行初始路径的搜寻与查找；

步骤6、根据自适应阈值公式(3)和公式(4)来对由拓扑节点所构成的初始路径进行局部优化，公式(3)、(4)如下：

Threshold2＝μ*(M+N)/4 (3)

其中：Threshold2是一个自适应阈值，作用是为对由拓扑节点所构成的初始路径进行分段，当拓扑节点之间的距离大于自适应阈值Threshold2时，其下面的拓扑节点就变成下一段路径；

μ是根据栅格地图的长、宽和栅格地图大小的数量级来定义的自适应参数，M和N为栅格地图的长和宽；

d＝min{M，N}为栅格地图中较短的边，b＝max{M，N}为栅格地图中较长的边，ε为栅格地图大小的数量级，当栅格地图较短的边a和较长的边b相等时令μ为1，当栅格地图较短的边a和较长的边b不等时，令μ为公式(4)；

通过Threshold2这个阈值把初始路径分段，这些分段的路径为局部路径；

步骤7、将由拓扑节点所构成的初始路径根据公式(3)和(4)进行自适应阈值处理后对其进行局部优化，每一段局部路径优化时都被限制在了椭圆内；

在每段局部路径规划中调用Informed RRT^*算法，从所构成每段局部路径规划中的第一个拓扑节点node_t1向最后一个拓扑节点站点node_tl进行扩展搜索，利用公式(5)计算各个相邻拓扑节点之间的距离d_i，其中如果通过Informed RRT^*算法扩展到的节点node_e所得到的路径比由拓扑节点所构成的路径短，那么则用node_e代替相应的拓扑节点，公式(5)如下：

公式(5)中，实际路径函数值Ld代表的是相邻两个节点之间的距离，其中每一段优化后的局部路径的长度可由公式(6)表示：

L_i＝Ld₁+Ld₂+…+Ld_e+…+Ld_l (6)

其中，Ld₁代表第一个拓扑节点node_t1和第二个拓扑节点node_t2；Ld_e代表通过InformedRRT^*算法扩展到的节点node_e和相应的拓扑节点之间的距离；Ld_l代表最后一个拓扑节点node_tl和其相邻的拓扑节点之间的距离；

步骤8、将所述每段优化后的路径L_i按照拼接规则进行拼接后形成新的路径{L₁，L₂，L₃，...，L_m，...，L_n-1，L_n}，由此得到优化后的全局路径L_a。