From 02a193ddcf5dba9a759b8f760393bb4b3b6391ad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=9D=8E=E6=98=8E=E6=B3=BD?= <1198158393@qq.com> Date: Thu, 6 Jan 2022 11:17:08 +0000 Subject: [PATCH] add project-uav-planning/limingze.md. --- project-uav-planning/limingze.md | 322 +++++++++++++++++++++++++++++++ 1 file changed, 322 insertions(+) create mode 100644 project-uav-planning/limingze.md diff --git a/project-uav-planning/limingze.md b/project-uav-planning/limingze.md new file mode 100644 index 0000000..88804ee --- /dev/null +++ b/project-uav-planning/limingze.md @@ -0,0 +1,322 @@ +目录 +一、研究目标 3 +二、研究开发方案 3 +三、关键技术 3 +四、研究开发技术 4 +五、程序实现 4 + + + + + + + + +一、研究目标 +近年来,随着自主智能体的迅速发展,携带多种传感器的自主智能体广泛应用在农学,地质,森林,海洋,地理测绘,军事侦察,环保等领域。如今,我们在空间和时间这两个维度上,前所未有地精准感知这个世界,快速精确地搜集着各种类型的数据。视觉信息,特别是图像,作为自主智能体获取的较为重要的一种信息,其在环境感知和场景识别中充当了重要的角色。 场景目标检测和变化场景感知是环境感知的两个重要分支。场景目标检测负责空间维度的认知,主要用于识别和定位当前环境中的物体。而变化场景感知主要用于时间维度的感知,主要检测随着时间流逝当前环境所发生的变化。 +场景目标检测是计算机视觉领域的基础研究方向之一,其在视觉场景理解、机器人感知、交通监控等方面具有重要的意义。尤其随着无人驾驶技术成为近几年新的热点,作为无人驾驶技术核心算法之一的目标检测也受到更多的关注。目标检测亦可分成两个子问题:目标定位和目标分类。目标分类问题主要解决从输入图像中判断是否存在有兴趣类别的物体,其输出带有对应置信度的标签以表明兴趣物体出现在图像中某个区域的可能性。目标定位问题负责解决输入图像或某个区域中兴趣物体的位置和大小,其输出物体的外接矩、或物体中心、或物体的轮廓边界等。 +二、研究开发方案 + A* (A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法,也是许多其他问题的常用启发式算法。注意--是最有效的直接搜索算法,之后涌现了很多预处理算法(如ALT,CH,HL等等),在线查询效率是A*算法的数千甚至上万倍。 +公式表示为: f*(n)=g*(n)+h*(n), +其中, f*(n) 是从初始状态经由状态n到目标状态的最小代价估计, +g*(n) 是在状态空间中从初始状态到状态n的最小代价, +h*(n) 是从状态n到目标状态的路径的最小估计代价。 +(对于路径搜索问题,状态就是图中的节点,代价就是距离) +真实h(n)的选取: +保证找到最短路径(最优解的)条件,关键在于估价函数f(n)的选取(或者说h(n)的选取)。 +以h(n)表达状态n到目标状态估计的距离,那么h(n)的选取大致有如下三种情况: +如果h(n)< h*(n),这种情况下,搜索的点数多,搜索范围大,效率低。但能得到最优解。 +如果h(n)=h*(n),此时的搜索效率是最高的。 +如果 h(n)>h*(n),搜索的点数少,搜索范围小,效率高,但不能保证得到最优解。 + + +三、关键技术 +1.建立地图 +将 A*算法用在移动机器人路径规划之前,第一步需要用白色表征可通行区域的栅格,用黑色表征不可通行区域的栅格,即说明该栅格存在障碍物,最后对栅格进行编号设置。格栅地图示例如图 1 所示。在栅格地图中,比较通用的标识措施主要有两种:一种是坐标系法,另一种是编号法。本文中采用的是坐标系法。 +2. A*算法总结 +(1) 把起点加入 open list 。 +(2) 重复如下过程: +a. 遍历 open list ,查找 F 值最小的节点,把它作为当前要处理的节点。 +b. 把这个节点移到 close list 。 +c. 对当前方格的 8 个相邻方格的每一个方格? +◆ 如果它是不可抵达的或者它在 close list 中,忽略它。否则,做如下操作。 +◆ 如果它不在 open list 中,把它加入 open list ,并且把当前方格设置为它的父亲,记录该方格的 F , G 和 H 值。 +◆ 如果它已经在 open list 中,检查这条路径 ( 即经由当前方格到达它那里 ) 是否更好,用 G 值作参考。更小的 G 值表示这是更好的路径。如果是这样,把它的父亲设置为当前方格,并重新计算它 的 G 和 F 值。如果你的 open list 是按 F 值排序的话,改变后你可能需要重新排序。 +d. 停止,当你 +◆ 把终点加入到了 open list 中,此时路径已经找到了,或者 +◆ 查找终点失败,并且 open list 是空的,此时没有路径。 +(3) 保存路径。从终点开始,每个方格沿着父节点移动直至起点,这就是你的路径。 +四、研究开发计划 +1.阅读参考文献,建立对所研究问题的基本认识,学习相关的方法(A*算法)等。(4-5周) +2.下载并学习Qt基本操作(2-3周) +3.整体程序的编写、调试(2-3周) +4.实验报告撰写 (2-4周) +五、程序实现 +1.建立地图 + // draw grid + if( m_mv->getDrawGrid() ) { + painter->setPen(QColor(240,240,240)); + y2 = my; + for(i=0; i<=mx; i++) { + x2 = x1 + i*dx; + painter->drawLine(x2, y1, x2, y2); + } + x2 = mx; + for(i=0; i<=my; i++) { + y2 = y1 + i*dy; + painter->drawLine(x1, y2, x2, y2); + } + } + + // draw cells + for(i=0; igetMapCell(i); + if( v == RMAP_FREE ) continue; + ix = i % mx; + iy = i / my; + x2 = x1 + ix*dx; + y2 = y1 + iy*dy; + switch(v) { + case RMAP_OBSTACLE: + painter->fillRect(x2, y2, dx, dy, Qt::black); + break; + case RMAP_OBSTACLE_UNDESCOVERED: + painter->fillRect(x2, y2, dx, dy, QColor(180, 180, 180)); + break; + case RMAP_OBSTACLE_SCANNED: + painter->fillRect(x2, y2, dx, dy, QColor(255, 0, 0)); + break; + default: + break; + } +} + +2.A*算法: +int AStarData::open_table_push(AStarNode *pn) +{ + int ns, i; +pAStarNode *a; + + // adjust open_table size dynamically + if( open_node_count+1 >= open_table_size ) { + ns = open_table_size*2; + a = new pAStarNode[ns]; + for(i=0; is_x, open_table[i]->s_y, + open_table[i]->s_g, open_table[i]->s_h); + } + printf("\n"); +} + +// 交换两个元素 +void AStarData::swap(int idx1, int idx2) +{ + pAStarNode tmp = open_table[idx1]; + open_table[idx1] = open_table[idx2]; + open_table[idx2] = tmp; +} + +// 堆调整 +void AStarData::adjust_heap(int nIndex) +{ + int curr = nIndex; + int child = curr * 2 + 1; // 得到左孩子idx( 下标从0开始,所有做孩子是curr*2+1 ) + int parent = ( curr - 1 ) / 2; // 得到双亲idx + + if (nIndex < 0 || nIndex >= open_node_count) + return; + + // 往下调整(要比较左右孩子和cuur parent ) + while ( child < open_node_count ) { + // 小根堆是双亲值小于孩子值 + if ( child + 1 < open_node_count && + open_table[child]->s_g + open_table[child]->s_h > + open_table[child+1]->s_g + open_table[child+1]->s_h ) { + ++child; // 判断左右孩子大小 + } +   if (open_table[curr]->s_g + open_table[curr]->s_h <= + open_table[child]->s_g + open_table[child]->s_h) { + break; + } else { + swap(child, curr); // 交换节点 + curr = child; // 再判断当前孩子节点 + child = curr * 2 + 1; // 再判断左孩子 + } + } + if (curr != nIndex) + return; + // 往上调整( 只需要比较curr child和parent ) + while (curr != 0) { + if (open_table[curr]->s_g + open_table[curr]->s_h >= + open_table[parent]->s_g + open_table[parent]->s_h) { + break; + } else { + swap(curr, parent); + curr = parent; + parent = (curr-1)/2; + } + } +} + +// insert neighbor nodes to open_table +int AStarData::insert_to_opentable( + int x, int y, + pAStarNode curr_node, + pAStarNode end_node, + int w ) +{ + int i; + pAStarNode cn; + + // 检查范围 + if( x < 0 || x >= nx || y < 0 || y >= ny ) + return -1; + + //检查是否通过障碍 + { + int dx, dy, ix1, iy1, ix2, iy2; + uint32_t idx1, idx2; + + dx = x - curr_node->s_x; + dy = y - curr_node->s_y; + + if( abs(dx) == 1 || abs(dy) == 1 ) { + ix1 = x; + iy1 = curr_node->s_y; + idx1 = iy1*nx + ix1; + + ix2 = curr_node->s_x; + iy2 = y; + idx2 = iy2*nx + ix2; + + // 如果两个都是障碍物则返回 + if( map_maze[idx1].s_style == 1 && map_maze[idx2].s_style == 1 ) + return -1; + } + } + + cn = &(map_maze[y*nx + x]); + + if( cn->s_style != 1 ) { // 不是障碍物 + if ( !cn->in_closetable ) { // 不在闭表中 + if ( cn->in_opentable ) { + // 在open表中 + // 需要判断是否是一条更优化的路径 + if ( cn->s_g > curr_node->s_g + w ) { // 如果更优化 + cn->s_g = curr_node->s_g + w; + cn->s_parent = curr_node; + + for ( i = 0; i < open_node_count; ++i ) { + if ( open_table[i]->s_x == cn->s_x && + open_table[i]->s_y == cn->s_y ) { + break; + } + } + + adjust_heap( i ); // 下面调整点 + } + } else { + // 不在open中 + cn->s_g = curr_node->s_g + w; + cn->s_h = abs(end_node->s_x - x ) + abs(end_node->s_y - y); + cn->s_parent = curr_node; + + cn->in_opentable = 1; + open_table_push(cn); + } + } + } + + return 0; +} + +// 添加相邻八个节点 +void AStarData::add_neighbors(pAStarNode curr_node, pAStarNode end_node) +{ + int x = curr_node->s_x; + int y = curr_node->s_y; + + insert_to_opentable( x+1, y, curr_node, end_node, 10 ); + insert_to_opentable( x-1, y, curr_node, end_node, 10 ); + insert_to_opentable( x, y+1, curr_node, end_node, 10 ); + insert_to_opentable( x, y-1, curr_node, end_node, 10 ); + insert_to_opentable( x+1, y+1, curr_node, end_node, 14 ); + insert_to_opentable( x+1, y-1, curr_node, end_node, 14 ); + insert_to_opentable( x-1, y+1, curr_node, end_node, 14 ); + insert_to_opentable( x-1, y-1, curr_node, end_node, 14 ); +} + +int AStarData::findRoute(void) +{ + int is_found; +open_node_count = 0; + + // push start point to open_table + start_node->in_opentable = 1; + open_table_push(start_node); + start_node->s_g = 0; + start_node->s_h = abs(end_node->s_x - start_node->s_x) + + abs(end_node->s_y - start_node->s_y); + start_node->s_parent = NULL; + is_found = 0; + + // if start point is end point then return + if ( start_node->s_x == end_node->s_x && start_node->s_y == end_node->s_y ) { + printf("ERR: start == end\n"); + return -1; + } + + + while( 1 ) { + curr_node = open_table[0]; // open表的第一个点一定是f值最小的点(通过堆排序得到的) + open_table[0] = open_table[--open_node_count]; // 最后一个点放到第一个点,然后进行堆调整 + adjust_heap( 0 ); // 调整堆 + curr_node->in_closetable = 1; // 已经在close表中了 + if ( curr_node->s_x == end_node->s_x && + curr_node->s_y == end_node->s_y ) { // 终点在close中,结束 + is_found = 1; + break; + } + add_neighbors(curr_node, end_node); // 添加相邻节点 + if ( open_node_count == 0 ) { // 没有路径到达 + is_found = 0; + break; + } + } + + if ( is_found ) { + curr_node = end_node; + + while( curr_node ) { + path_stack.push_back(curr_node); + curr_node = curr_node->s_parent; + } + } else { + printf("ERR: No route can be found!\n"); + return -1; + } + + return 0; +} + +4.结果展示 +(1)画地图,设定起点(红)终点(蓝),见图 +(2)P,B准备开始扫描,N步进,H完整过程,绿色路径为实时规划路径,蓝色为无人机经过路径,最终路径见图 + + + -- Gitee