技术领域Technical Field
本发明属于移动机器人导航技术领域，具体涉及一种融合优先探索和定时器机制的三维激光雷达导航方法。The present invention belongs to the technical field of mobile robot navigation, and in particular relates to a three-dimensional laser radar navigation method integrating priority exploration and timer mechanism.
背景技术Background technique
在未知环境下的自主导航可显著提高移动机器人的工作效率。该领域包括在线定位，建图和规划。其中，度量地图和拓扑地图作为导航地图中的重要表达形式，在环境表示中起着至关重要的作用。然而，拓扑地图相比度量地图更适合在大范围场景下应用，其计算成本更低。然而，目前很少有研究基于可见性图的导航方法，尤其是在复杂环境中。其次，现有的导航方法在未知环境下所产生的导航路径容易冗余，使得移动机器人探索不必要的空间，因而降低了导航效率。因此，急需减少冗余路径搜索的策略来提高导航系统的高效性。Autonomous navigation in unknown environments can significantly improve the efficiency of mobile robots. This field includes online positioning, mapping, and planning. Among them, metric maps and topological maps, as important forms of expression in navigation maps, play a vital role in environmental representation. However, compared with metric maps, topological maps are more suitable for application in large-scale scenarios and have lower computational costs. However, there are currently few studies on navigation methods based on visibility graphs, especially in complex environments. Secondly, the navigation paths generated by existing navigation methods in unknown environments are prone to redundancy, which makes mobile robots explore unnecessary spaces, thereby reducing navigation efficiency. Therefore, there is an urgent need for strategies to reduce redundant path searches to improve the efficiency of navigation systems.
专利CN112904855A公开了一种基于改进动态窗口的跟随机器人局部路径规划方法，该方法使得路径规划的反应速度更快，一定程度上提高了路径规划的导航效率，但是在一些未知环境下依然会产生冗余路径而探索冗余空间；Patent CN112904855A discloses a local path planning method for a following robot based on an improved dynamic window. This method makes the path planning response faster and improves the navigation efficiency of the path planning to a certain extent. However, in some unknown environments, redundant paths will still be generated and redundant spaces will be explored.
专利CN117111597A公开了一种复杂未知环境下的无人车组合路径规划方法,该方法针对复杂未知环境下无人车路径规划效率低和可靠性差等问题，提出了改进了跳点搜索算法的跳跃规则和强制邻居筛选规则，但是在一些未知环境下该方法会容易陷入局部死角，而导致导航任务失败。Patent CN117111597A discloses a combined path planning method for unmanned vehicles in complex unknown environments. This method addresses the problems of low efficiency and poor reliability of unmanned vehicle path planning in complex unknown environments, and proposes improved jumping rules and forced neighbor screening rules for the jump point search algorithm. However, in some unknown environments, this method may easily fall into local blind spots, resulting in failure of the navigation task.
发明内容Summary of the invention
针对现有技术存在的缺陷和不足，本发明提出了一种融合优先探索和定时器机制的三维激光雷达导航方法，该方法用于提高移动机器人在未知环境下的导航效率，减少对冗余空间的探索，旨在进一步提高导航系统的鲁棒性和高效性。In view of the defects and shortcomings of the prior art, the present invention proposes a three-dimensional lidar navigation method that integrates priority exploration and timer mechanism. The method is used to improve the navigation efficiency of mobile robots in unknown environments and reduce the exploration of redundant spaces, aiming to further improve the robustness and efficiency of the navigation system.
本发明的技术方案如下：一种融合优先探索和定时器机制的三维激光雷达导航方法，具体实施步骤为：The technical solution of the present invention is as follows: a three-dimensional laser radar navigation method integrating priority exploration and timer mechanism, and the specific implementation steps are as follows:
步骤一：通过激光雷达获取原始点云扫描，得到若干点云帧；Step 1: Obtain the original point cloud scan through the laser radar to obtain several point cloud frames;
步骤二：从若干点云帧中进行障碍物轮廓顶点提取；Step 2: Extract obstacle contour vertices from several point cloud frames;
步骤三：基于几何特征方法，对提取后的障碍物轮廓顶点进行优化，剔除冗余障碍物顶点；Step 3: Based on the geometric feature method, the extracted obstacle contour vertices are optimized and redundant obstacle vertices are removed;
步骤四：更新障碍物顶点并注册到局部可视图中，经过局部可视图合并构建全局可视图；Step 4: Update the obstacle vertices and register them in the local visible graph, and then construct the global visible graph by merging the local visible graphs;
步骤五：基于全局可视图采用具有优先探索和定时器机制的路径搜索方法生成全局路径，并形成全局路径点；Step 5: Generate a global path based on the global visibility graph using a path search method with priority exploration and timer mechanism, and form a global path point;
步骤六：通过全局路径点并结合局部地面区域的可通行性，对采样的离散点进行正向模拟并规划出机器人的候选路径，通过碰撞检测对候选路径进行过滤，并结合路径和角度的计算成本从中选取代价值最小的局部路径；Step 6: Through the global path points and the passability of the local ground area, the sampled discrete points are forward simulated and the candidate paths of the robot are planned. The candidate paths are filtered through collision detection, and the local path with the minimum replacement value is selected based on the calculation cost of the path and angle.
步骤七：在局部路径生成后，采用具有动态视距的路径跟踪方法输出最终的移动机器人转向角和线速度。Step 7: After the local path is generated, the path tracking method with dynamic sight distance is used to output the final steering angle and linear speed of the mobile robot.
本发明的特点在于：The present invention is characterized in that:
步骤三中，所提出的几何特征进行障碍物轮廓优化，具体按照以下步骤实施：In step 3, the proposed geometric features are used to optimize the obstacle contour, which is implemented in the following steps:
对于给定轮廓上任意三个连续顶点和/>两个顶点之间的距离(Vi-1,Vi)和(Vi,Vi+1)定义如下：For any three consecutive vertices on a given contour and/> The distance between two vertices (Vi-1 ,Vi ) and (Vi ,Vi+1 ) is defined as follows:
因此，两条相邻边之间的角度定义如下：Therefore, the angle between two adjacent sides is defined as follows:
此外，定义了两个新变量dmax和θmax，其中，dmax表示障碍物多边形的两个相邻顶点之间的最长距离，θmax定义如下：In addition, two new variables dmax and θmax are defined, where dmax represents the longest distance between two adjacent vertices of the obstacle polygon and θmax is defined as follows:
其中表示每个障碍物多边形中三个最长边的平均距离；in represents the average distance of the three longest sides in each obstacle polygon;
基于此，有两种情况需要进行障碍物轮廓优化：如果d(Vi-1,Vi)和d(Vi,Vi+1)都小于η×dmax，则意味着障碍物具有无效顶点，在这种情况下，删除顶点Vi并破坏其连接边(Vi-1,Vi)和边(Vi,Vi+1)；如果d(Vi-1,Vi)和d(Vi,Vi+1)大于η×dmax但小于2η×dmax，角度θ大于θmax，也删除顶点Vi并从可视图中删除其连接边(Vi-1,Vi)和边(Vi,Vi+1)；Based on this, there are two cases where obstacle contour optimization is required: if d(Vi-1 ,Vi ) and d(Vi ,Vi+1 ) are both less than η×dmax , it means that the obstacle has an invalid vertex. In this case, delete vertex Vi and destroy its connected edge (Vi-1 ,Vi ) and edge (Vi ,Vi+1 ); if d(Vi-1 ,Vi ) and d(Vi ,Vi+1 ) are greater than η×dmax but less than 2η×dmax , and the angle θ is greater than θmax , also delete vertex Vi and delete its connected edge (Vi-1 ,Vi ) and edge (Vi ,Vi+1 ) from the visible graph;
步骤五中，基于全局可视图采用具有优先探索和定时器机制的路径搜索方法生成全局路径，具体按照以下步骤实施：In step 5, a path search method with priority exploration and timer mechanism is used to generate a global path based on the global visibility graph, which is implemented in the following steps:
在移动机器人行使过程中，会存在伪障碍物点，即会随着机器人视野变化的障碍物轮廓节点，伪障碍物点定义如下：During the operation of the mobile robot, there will be pseudo obstacle points, that is, obstacle contour nodes that change with the robot's field of view. The pseudo obstacle points are defined as follows:
式中，xT和yT分别为T时刻观测到障碍物顶点A的X和Y坐标值，xT-1和yT-1分别为T-1时刻观测到障碍物顶点A的X和Y坐标值，dT-1,T为移动移动机器人从时间T-1到T的距离，kd代表较小的距离常数，如果判别式成立，则可以将A点判断为伪障碍点；通过优先探索算法搜索出若干可通行全局路径，对于存在伪障碍物点的全局路径进行滤除，选择距离成本最小的全局路径作为最优全局路径，按以上方法获得的全局路径避免了对冗余空间的探索；WherexT andyT are the X and Y coordinate values of the obstacle vertex A observed at time T, xT-1 and yT-1 are the X and Y coordinate values of the obstacle vertex A observed at time T-1,dT-1,T is the distance of the mobile robot from time T-1 to T,kd represents a smaller distance constant. If the discriminant holds, point A can be judged as a pseudo obstacle point. Several traversable global paths are searched out through the priority exploration algorithm, and the global paths with pseudo obstacle points are filtered out. The global path with the smallest distance cost is selected as the optimal global path. The global path obtained by the above method avoids the exploration of redundant space.
此外，为防止移动机器人陷入局部死角，本发明提出定时器机制用于脱困，定义定时器Tm如下：In addition, in order to prevent the mobile robot from falling into a local blind spot, the present invention proposes a timer mechanism for escaping from the dead end, and the timerTm is defined as follows:
其中Cp代表封闭环境的周长，V代表移动机器人的速度；Where Cp represents the perimeter of the closed environment, and V represents the speed of the mobile robot;
若在某一条规划的全局路径中移动机器人所处的时间已经达到了Tm，且在移动机器人进入该支路后的0.8Tm，0.9Tm和Tm时刻与目标点之间的距离均大于刚进入该支路时与目标点的距离，即移动机器人在该支路中探索时无向目标点靠近的趋势，则可向移动机器人发送指令使其原路返回并重新选择路径；若在移动机器人进入该支路后的0.8Tm，0.9Tm和Tm时刻与目标点之间的距离中至少有一个小于刚进入该支路时与目标点的距离，则将坚持定时器清零重新计时，并重复上述操作，直至抵达目标点；If the time of the mobile robot in a planned global path has reached Tm , and the distance between the mobile robot and the target point at 0.8Tm , 0.9Tm and Tm after entering the branch is greater than the distance between the mobile robot and the target point when it just enters the branch, that is, the mobile robot has no tendency to approach the target point when exploring the branch, then a command can be sent to the mobile robot to make it return to the original path and reselect a path; if at least one of the distance between the mobile robot and the target point at 0.8Tm , 0.9Tm and Tm after entering the branch is less than the distance between the mobile robot and the target point when it just enters the branch, then the persistence timer will be reset and re-timed, and the above operation will be repeated until the target point is reached;
步骤六中，结合路径和角度的计算成本从中选取代价值最小的局部路径，具体按以下步骤实施：In step 6, the local path with the smallest replacement value is selected based on the calculation cost of the path and angle, which is implemented in the following steps:
通过对采样的离散点进行正向模拟，规划出机器人的候选路径PATHi(i＝1,..,729)，记PATHi的末端路径点为Pi，目标点为PT，移动机器人当前位姿Pr，定义每条候选路径与目标点的成本Costi为：By forward simulation of the sampled discrete points, the candidate path PATHi (i＝1,..,729) of the robot is planned. The end path point of PATHi isPi , the target point is PT , the current position of the mobile robot is Pr , and the cost Costi of each candidate path and the target point is defined as:
式中，θ为当前移动机器人方位角，可通过陀螺仪获取，该成本充分考虑了每条路径与目标点的角度差以及路径方向与当前车辆方位的角度差，随后选取成本最低的候选路径作为待跟踪轨迹；In the formula,θ is the current azimuth of the mobile robot, which can be obtained through the gyroscope. The cost fully considers the angle difference between each path and the target point and the angle difference between the path direction and the current vehicle orientation. Then, the candidate path with the lowest cost is selected as the trajectory to be tracked;
步骤七中，采用具有动态视距的路径跟踪方法输出最终的移动机器人转向角和线速度，具体按以下步骤实施：In step 7, a path tracking method with dynamic sight distance is used to output the final steering angle and linear speed of the mobile robot, which is specifically implemented in the following steps:
将阿克曼底盘模型转换为单车模型，可得如下式：Converting the Ackerman chassis model to a bicycle model yields the following equation:
δ表示转向角，L表示轴距，R代表转弯半径；δ represents the steering angle, L represents the wheelbase, and R represents the turning radius;
进一步地，根据单车模型几何关系，可得如下式：Furthermore, according to the geometric relationship of the bicycle model, the following formula can be obtained:
e(t)＝ld·sin(α(t)e(t)＝ld ·sin(α(t)
式中，δ(t)表示t时刻的转向角，ld表示视距，ɑ(t)表示t时刻的偏航角，e(t)表示水平误差；Where, δ(t) represents the steering angle at time t, ld represents the sight distance, ɑ(t) represents the yaw angle at time t, and e(t) represents the horizontal error;
基于几何模型的纯跟踪方法过于依赖视距的选择，因此提出基于速度V和最小转弯半径C的动态视距计算方法，如下式：The pure tracking method based on the geometric model is too dependent on the selection of sight distance, so a dynamic sight distance calculation method based on speed V and minimum turning radius C is proposed, as shown in the following formula:
ld＝k·V+Cld = k·V+C
式中，k表示速度系数；Where k represents the speed coefficient;
对于速度V控制，在设定恒定速度后使用PID算法进行输出，如下式：For speed V control, the PID algorithm is used to output after setting a constant speed, as shown below:
VO＝Kp(Vc-VT)VO =Kp (Vc -VT )
式中，Kp表示速度比例系数，Vo表示输出速度，Vc表示获取的实际速度，VT表示设定速度。WhereKp represents the speed proportional coefficient,Vo represents the output speed,Vc represents the actual speed obtained, andVT represents the set speed.
本发明的有益效果为：The beneficial effects of the present invention are:
1.本发明提供了一种基于几何方法的障碍物边界轮廓优化方法，有效地减少了障碍物边界顶点，因而减少了地图更新时间成本；1. The present invention provides an obstacle boundary contour optimization method based on a geometric method, which effectively reduces the obstacle boundary vertices, thereby reducing the time cost of map update;
2.本发明提供了一种优先探索机制，有效地减少了冗余空间的探索，使得导航路径短；2. The present invention provides a priority exploration mechanism, which effectively reduces the exploration of redundant space and makes the navigation path short;
3.本发明所提供的定时器机制有效防止机器人陷入局部死角，提高了导航效率；3. The timer mechanism provided by the present invention effectively prevents the robot from falling into a local blind spot, thereby improving navigation efficiency;
4.本发明所提供的基于动态视距地路径跟踪方法有效减少了导航路径误差，使得规划的导航路径更接近于设定跟踪路径。4. The path tracking method based on dynamic line of sight provided by the present invention effectively reduces the navigation path error, making the planned navigation path closer to the set tracking path.
综上，与现有技术相比，本发明在未知环境下能够实现更高效地导航，具体地说减少了冗余空间探索和防止陷入局部死角，以及实现高精度的路径跟踪。In summary, compared with the prior art, the present invention can achieve more efficient navigation in an unknown environment, specifically reducing redundant space exploration and preventing local blind spots, and achieving high-precision path tracking.
附图说明BRIEF DESCRIPTION OF THE DRAWINGS
图1是本发明提供的一种融合优先探索和定时器机制的三维激光雷达导航方法的系统框图；FIG1 is a system block diagram of a three-dimensional laser radar navigation method integrating priority exploration and timer mechanism provided by the present invention;
图2(a)是本发明在CMU室内长走廊仿真环境下构建的可视图与其他主流方法构建的可视图；FIG2( a ) is a visualization diagram constructed by the present invention in a CMU indoor long corridor simulation environment and a visualization diagram constructed by other mainstream methods;
图2(b)是本发明在CMU室内长走廊仿真环境下构建的可视图中障碍物顶点数与其他主流方法构建的可视图中障碍物顶点数对比结果图；FIG2( b ) is a comparison result of the number of obstacle vertices in the visible graph constructed by the present invention in the CMU indoor long corridor simulation environment and the number of obstacle vertices in the visible graph constructed by other mainstream methods;
图2(c)是本发明在CMU室内长走廊仿真环境下构建的可视图更新时间与其他主流方法构建的可视图更新时间对比结果图；FIG2(c) is a graph comparing the update time of the visible graph constructed by the present invention in a CMU indoor long corridor simulation environment with the update time of the visible graph constructed by other mainstream methods;
图3(a)是室外测试实景图；Figure 3(a) is a real scene of outdoor testing;
图3(b)是本发明在室外环境下构建的可视图与其他主流方法构建的可视图对比结果图；FIG3( b ) is a comparison diagram of the visual graph constructed by the present invention in an outdoor environment and the visual graph constructed by other mainstream methods;
图3(c)是本发明在室外环境下导航路线与其他主流方法导航路线对比结果图；FIG3( c ) is a diagram showing a comparison of the navigation route of the present invention and the navigation route of other mainstream methods in an outdoor environment;
图4是本发明在CMU室内长走廊仿真环境下导航路线与其他主流方法导航路线结果图；FIG4 is a diagram showing the navigation route of the present invention in a CMU indoor long corridor simulation environment and the navigation route results of other mainstream methods;
图5(a)是本发明在CMU长隧道仿真环境下导航路线与其他主流方法导航路线结果图；FIG5( a ) is a diagram showing the navigation route of the present invention and the navigation route results of other mainstream methods in a CMU long tunnel simulation environment;
图5(b)是本发明在CMU长隧道仿真环境下各导航点路径长度与其他主流方法各导航点路径长度对比结果图；FIG5( b ) is a graph showing the comparison of the path lengths of each navigation point of the present invention and the path lengths of each navigation point of other mainstream methods in a CMU long tunnel simulation environment;
图5(c)是本发明在CMU长隧道仿真环境下各导航点时间成本与其他主流方法各导航点时间成本对比结果图；FIG5( c ) is a graph showing a comparison of the time cost of each navigation point of the present invention and the time cost of each navigation point of other mainstream methods in a CMU long tunnel simulation environment;
图6(a)是本发明在室内长走廊环境下轨迹跟踪与其他主流方法轨迹跟踪对比结果图；FIG6( a ) is a diagram showing a comparison of trajectory tracking results of the present invention and other mainstream methods in an indoor long corridor environment;
图6(b)是本发明在室内长走廊环境下水平方向和垂直方向的轨迹跟踪误差结果图；FIG6( b ) is a diagram showing the trajectory tracking error results in the horizontal and vertical directions of the present invention in an indoor long corridor environment;
图6(c)是其他主流方法在室内长走廊环境下水平方向和垂直方向的轨迹跟踪误差结果图。Figure 6(c) shows the trajectory tracking error results of other mainstream methods in the horizontal and vertical directions in an indoor long corridor environment.
具体实施方式Detailed ways
为了使本发明目的、技术方案及优点更加清楚明白，下面结合附图及实施例对本发明作进一步描述。In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described below in conjunction with the accompanying drawings and embodiments.
本发明公开了一种融合优先探索和定时器机制的三维激光雷达导航方法，涉及移动机器人导航技术领域。该方法建立在可视图的基础上，采用三维激光雷达并融合优先探索和定时器机制的路径搜索实现在未知环境下的自主导航。与现有技术相比，本发明在未知环境下能够实现更高效地导航，具体地说减少了冗余空间探索和防止陷入局部死角，以及实现高精度的路径跟踪。The present invention discloses a three-dimensional laser radar navigation method integrating priority exploration and timer mechanism, and relates to the field of mobile robot navigation technology. The method is based on a visual graph, and uses a three-dimensional laser radar and integrates path search of priority exploration and timer mechanism to realize autonomous navigation in an unknown environment. Compared with the prior art, the present invention can realize more efficient navigation in an unknown environment, specifically reducing redundant space exploration and preventing falling into local blind spots, and realizing high-precision path tracking.
本发明提供的一种融合优先探索和定时器机制的三维激光雷达导航方法的系统框图如图1所示，按照以下具体实施步骤：The system block diagram of a three-dimensional laser radar navigation method integrating priority exploration and timer mechanism provided by the present invention is shown in FIG1 , and the specific implementation steps are as follows:
步骤一：通过激光雷达获取原始点云扫描，得到若干点云帧；Step 1: Obtain the original point cloud scan through the laser radar to obtain several point cloud frames;
步骤二：从若干点云帧中进行障碍物轮廓顶点提取；Step 2: Extract obstacle contour vertices from several point cloud frames;
步骤三：基于几何特征方法，对提取后的障碍物轮廓顶点进行优化，剔除冗余障碍物顶点；Step 3: Based on the geometric feature method, the extracted obstacle contour vertices are optimized and redundant obstacle vertices are removed;
步骤四：更新障碍物顶点并注册到局部可视图中，经过局部可视图合并构建全局可视图；Step 4: Update the obstacle vertices and register them in the local visible graph, and then construct the global visible graph by merging the local visible graphs;
步骤五：基于全局可视图采用具有优先探索和定时器机制的路径搜索方法生成全局路径，并形成全局路径点；Step 5: Generate a global path based on the global visibility graph using a path search method with priority exploration and timer mechanism, and form a global path point;
步骤六：通过全局路径点并结合局部地面区域的可通行性，对采样的离散点进行正向模拟并规划出机器人的候选路径，通过碰撞检测对候选路径进行过滤，并结合路径和角度的计算成本从中选取代价值最小的局部路径；Step 6: Through the global path points and the passability of the local ground area, the sampled discrete points are forward simulated and the candidate paths of the robot are planned. The candidate paths are filtered through collision detection, and the local path with the minimum replacement value is selected based on the calculation cost of the path and angle.
步骤七：在局部路径生成后，采用具有动态视距的路径跟踪方法输出最终的移动机器人转向角和线速度。Step 7: After the local path is generated, the path tracking method with dynamic sight distance is used to output the final steering angle and linear speed of the mobile robot.
本发明的特点在于：The present invention is characterized in that:
步骤三中，所提出的几何特征进行障碍物轮廓优化，具体按照以下步骤实施：In step 3, the proposed geometric features are used to optimize the obstacle contour, which is implemented in the following steps:
对于给定轮廓上任意三个连续顶点和/>两个顶点之间的距离(Vi-1,Vi)和(Vi,Vi+1)定义如下：For any three consecutive vertices on a given contour and/> The distance between two vertices (Vi-1 ,Vi ) and (Vi ,Vi+1 ) is defined as follows:
因此，两条相邻边之间的角度定义如下：Therefore, the angle between two adjacent sides is defined as follows:
此外，定义了两个新变量dmax和θmax，其中，dmax表示障碍物多边形的两个相邻顶点之间的最长距离，θmax定义如下：In addition, two new variables dmax and θmax are defined, where dmax represents the longest distance between two adjacent vertices of the obstacle polygon and θmax is defined as follows:
其中表示每个障碍物多边形中三个最长边的平均距离；in represents the average distance of the three longest sides in each obstacle polygon;
基于此，有两种情况需要进行障碍物轮廓优化：如果d(Vi-1,Vi)和d(Vi,Vi+1)都小于η×dmax，则意味着障碍物具有无效顶点，在这种情况下，删除顶点Vi并破坏其连接边(Vi-1,Vi)和边(Vi,Vi+1)；如果d(Vi-1,Vi)和d(Vi,Vi+1)大于η×dmax但小于2η×dmax，角度θ大于θmax，也删除顶点Vi并从可视图中删除其连接边(Vi-1,Vi)和边(Vi,Vi+1)；Based on this, there are two cases where obstacle contour optimization is required: if d(Vi-1 ,Vi ) and d(Vi ,Vi+1 ) are both less than η×dmax , it means that the obstacle has an invalid vertex. In this case, delete vertex Vi and destroy its connected edge (Vi-1 ,Vi ) and edge (Vi ,Vi+1 ); if d(Vi-1 ,Vi ) and d(Vi ,Vi+1 ) are greater than η×dmax but less than 2η×dmax , and the angle θ is greater than θmax , also delete vertex Vi and delete its connected edge (Vi-1 ,Vi ) and edge (Vi ,Vi+1 ) from the visible graph;
步骤五中，基于全局可视图采用具有优先探索和定时器机制的路径搜索方法生成全局路径，具体按照以下步骤实施：In step 5, a path search method with priority exploration and timer mechanism is used to generate a global path based on the global visibility graph, which is implemented in the following steps:
在移动机器人行使过程中，会存在伪障碍物点，即会随着机器人视野变化的障碍物轮廓节点，伪障碍物点定义如下：During the operation of the mobile robot, there will be pseudo obstacle points, that is, obstacle contour nodes that change with the robot's field of view. The pseudo obstacle points are defined as follows:
式中，xT和yT分别为T时刻观测到障碍物顶点A的X和Y坐标值，xT-1和yT-1分别为T-1时刻观测到障碍物顶点A的X和Y坐标值，dT-1,T为移动移动机器人从时间T-1到T的距离，kd代表较小的距离常数，如果判别式成立，则可以将A点判断为伪障碍点；通过优先探索算法搜索出若干可通行全局路径，对于存在伪障碍物点的全局路径进行滤除，选择距离成本最小的全局路径作为最优全局路径，按以上方法获得的全局路径避免了对冗余空间的探索；WherexT andyT are the X and Y coordinate values of the obstacle vertex A observed at time T, xT-1 and yT-1 are the X and Y coordinate values of the obstacle vertex A observed at time T-1,dT-1,T is the distance of the mobile robot from time T-1 to T,kd represents a smaller distance constant. If the discriminant holds, point A can be judged as a pseudo obstacle point. Several traversable global paths are searched out through the priority exploration algorithm, and the global paths with pseudo obstacle points are filtered out. The global path with the smallest distance cost is selected as the optimal global path. The global path obtained by the above method avoids the exploration of redundant space.
由上述分析可知，伪障碍物点是由于遮挡而产生的，伪障碍物点并非为障碍物轮廓边缘上的真实边界点，若生成的路径经过伪障碍物点，那么该路径一定是穿越障碍物的，基于该结论本文引入优先探索机制，当移动机器人的探索路径集内存在多条通向目标点的疑似路径时，优先选择不经过伪顶点并不断有向目标点靠近趋势的路径；From the above analysis, we can see that pseudo obstacle points are generated due to occlusion. Pseudo obstacle points are not real boundary points on the edge of the obstacle contour. If the generated path passes through a pseudo obstacle point, then the path must pass through the obstacle. Based on this conclusion, this paper introduces a priority exploration mechanism. When there are multiple suspected paths leading to the target point in the exploration path set of the mobile robot, the path that does not pass through the pseudo vertex and has a trend of constantly approaching the target point is preferentially selected;
此外，为防止移动机器人陷入局部死角，本发明提出定时器机制用于脱困，定义定时器Tm如下：In addition, in order to prevent the mobile robot from falling into a local blind spot, the present invention proposes a timer mechanism for escaping from the dead end, and the timerTm is defined as follows:
其中Cp代表封闭环境的周长，V代表移动机器人的速度；Where Cp represents the perimeter of the closed environment, and V represents the speed of the mobile robot;
若在某一条规划的全局路径中移动机器人所处的时间已经达到了Tm，且在移动机器人进入该支路后的0.8Tm，0.9Tm和Tm时刻与目标点之间的距离均大于刚进入该支路时与目标点的距离，即移动机器人在该支路中探索时无向目标点靠近的趋势，则可向移动机器人发送指令使其原路返回并重新选择路径；若在移动机器人进入该支路后的0.8Tm，0.9Tm和Tm时刻与目标点之间的距离中至少有一个小于刚进入该支路时与目标点的距离，则将坚持定时器清零重新计时，并重复上述操作，直至抵达目标点；If the time of the mobile robot in a planned global path has reached Tm , and the distance between the mobile robot and the target point at 0.8Tm , 0.9Tm and Tm after entering the branch is greater than the distance between the mobile robot and the target point when it just enters the branch, that is, the mobile robot has no tendency to approach the target point when exploring the branch, then a command can be sent to the mobile robot to make it return to the original path and reselect a path; if at least one of the distance between the mobile robot and the target point at 0.8Tm , 0.9Tm and Tm after entering the branch is less than the distance between the mobile robot and the target point when it just enters the branch, then the persistence timer will be reset and re-timed, and the above operation will be repeated until the target point is reached;
步骤六中，结合路径和角度的计算成本从中选取代价值最小的局部路径，具体按以下步骤实施：In step 6, the local path with the smallest replacement value is selected based on the calculation cost of the path and angle, which is implemented in the following steps:
通过对采样的离散点进行正向模拟，规划出机器人的候选路径PATHi(i＝1,..,729)，记PATHi的末端路径点为Pi，目标点为PT，移动机器人当前位姿Pr，定义每条候选路径与目标点的成本Costi为：By forward simulation of the sampled discrete points, the candidate path PATHi (i＝1,..,729) of the robot is planned. The end path point of PATHi isPi , the target point is PT , the current position of the mobile robot is Pr , and the cost Costi of each candidate path and the target point is defined as:
式中，θ为当前移动机器人方位角，可通过陀螺仪获取，该成本充分考虑了每条路径与目标点的角度差以及路径方向与当前车辆方位的角度差，随后选取成本最低的候选路径作为待跟踪轨迹；In the formula,θ is the current azimuth of the mobile robot, which can be obtained through the gyroscope. The cost fully considers the angle difference between each path and the target point and the angle difference between the path direction and the current vehicle orientation. Then, the candidate path with the lowest cost is selected as the trajectory to be tracked;
步骤七中，采用具有动态视距的路径跟踪方法输出最终的移动机器人转向角和线速度，具体按以下步骤实施：In step 7, a path tracking method with dynamic sight distance is used to output the final steering angle and linear speed of the mobile robot, which is specifically implemented in the following steps:
将阿克曼底盘模型转换为单车模型，可得如下式：Converting the Ackerman chassis model to a bicycle model yields the following equation:
δ表示转向角，L表示轴距，R代表转弯半径；δ represents the steering angle, L represents the wheelbase, and R represents the turning radius;
进一步地，根据单车模型几何关系，可得如下式：Furthermore, according to the geometric relationship of the bicycle model, the following formula can be obtained:
e(t)＝ld·sin(α(t)e(t)＝ld ·sin(α(t)
式中，δ(t)表示t时刻的转向角，ld表示视距，ɑ(t)表示t时刻的偏航角，e(t)表示水平误差；Where, δ(t) represents the steering angle at time t, ld represents the sight distance, ɑ(t) represents the yaw angle at time t, and e(t) represents the horizontal error;
基于几何模型的纯跟踪方法过于依赖视距的选择，因此提出基于速度V和最小转弯半径C的动态视距计算方法，如下式：The pure tracking method based on the geometric model is too dependent on the selection of sight distance, so a dynamic sight distance calculation method based on speed V and minimum turning radius C is proposed, as shown in the following formula:
ld＝k·V+Cld = k·V+C
式中，k表示速度系数；Where k represents the speed coefficient;
对于速度V控制，在设定恒定速度后使用PID算法进行输出，如下式：For speed V control, the PID algorithm is used to output after setting a constant speed, as shown below:
VO＝Kp(Vc-VT)VO =Kp (Vc -VT )
式中，Kp表示速度比例系数，Vo表示输出速度，Vc表示获取的实际速度，VT表示设定速度。WhereKp represents the speed proportional coefficient,Vo represents the output speed,Vc represents the actual speed obtained, andVT represents the set speed.
图2(a)是本发明在CMU室内长走廊仿真环境下构建的可视图与其他主流方法构建的可视图，由图可知，本发明所提出的障碍物边界优化算法能够减少障碍物冗余顶点。FIG2( a ) is a visualization diagram constructed by the present invention in a CMU indoor long corridor simulation environment and a visualization diagram constructed by other mainstream methods. It can be seen from the figure that the obstacle boundary optimization algorithm proposed by the present invention can reduce redundant vertices of obstacles.
图2(b)是本发明在CMU室内长走廊仿真环境下构建的可视图中障碍物顶点数与其他主流方法构建的可视图中障碍物顶点数对比结果图，图中横坐标是建图过程中的时间，纵坐标是障碍物的顶点个数，E-Planner是本发明所提出的方法，FAR Planner是主流先进三维导航方法，由图可知，本发明所提出的方法能够有效减少障碍物顶点个数，最终构建的可视图中障碍物总顶点数减少了13.5％。图2(c)是本发明在CMU室内长走廊仿真环境下构建的可视图更新时间与其他主流方法构建的可视图更新时间对比结果图，图中横坐标是建图过程中的时间，纵坐标是地图更新时间，由图可知，通过优化可视图后，地图更新时间相比FAR Planner方法减少了14.5％。FIG2(b) is a comparison result of the number of obstacle vertices in the visible graph constructed by the present invention in the CMU indoor long corridor simulation environment and the number of obstacle vertices in the visible graph constructed by other mainstream methods. In the figure, the horizontal axis is the time in the mapping process, and the vertical axis is the number of obstacle vertices. E-Planner is the method proposed by the present invention, and FAR Planner is the mainstream advanced three-dimensional navigation method. It can be seen from the figure that the method proposed by the present invention can effectively reduce the number of obstacle vertices, and the total number of obstacle vertices in the final constructed visible graph is reduced by 13.5%. FIG2(c) is a comparison result of the update time of the visible graph constructed by the present invention in the CMU indoor long corridor simulation environment and the update time of the visible graph constructed by other mainstream methods. In the figure, the horizontal axis is the time in the mapping process, and the vertical axis is the map update time. It can be seen from the figure that after optimizing the visible graph, the map update time is reduced by 14.5% compared with the FAR Planner method.
图3(a)是室外测试实景图，在该场景下通过自主搭建的移动机器人平台，开展目标点导航实验，起始点在图中左下方，目标点在图中右上方。图3(b)是本发明在该环境下构建的可视图与其他主流方法构建的可视图对比结果图，由图可知，本发明构建的可视图中障碍物冗余顶点明显较少。图3(c)是本发明在室外环境下导航路线与其他主流方法导航路线对比结果图，由图中白框区域可知，本发明的优先探索机制减少了冗余空间的探索，路径长度明显减少，说明了本发明相比其他导航技术具有明显的进步性；Figure 3(a) is a real-life picture of an outdoor test. In this scenario, a target point navigation experiment was carried out through a self-built mobile robot platform. The starting point is in the lower left of the figure, and the target point is in the upper right of the figure. Figure 3(b) is a comparison result of the visual graph constructed by the present invention in this environment with the visual graph constructed by other mainstream methods. It can be seen from the figure that there are significantly fewer redundant vertices of obstacles in the visual graph constructed by the present invention. Figure 3(c) is a comparison result of the navigation route of the present invention in an outdoor environment with the navigation route of other mainstream methods. It can be seen from the white box area in the figure that the priority exploration mechanism of the present invention reduces the exploration of redundant space and significantly reduces the path length, which shows that the present invention is significantly more advanced than other navigation technologies.
图4是本发明在CMU室内长走廊仿真环境下导航路线与其他主流方法导航路线结果图，由图上方区域可知，本发明所提出的定时器机制有效地防止机器人陷入局部死角，减少了冗余空间的探索。FIG4 is a diagram showing the navigation route of the present invention and the navigation route results of other mainstream methods in the CMU indoor long corridor simulation environment. As can be seen from the upper area of the figure, the timer mechanism proposed by the present invention effectively prevents the robot from falling into a local blind spot and reduces the exploration of redundant space.
图5(a)是本发明在CMU长隧道仿真环境下导航路线与其他主流方法导航路线结果图，对于隧道环境，几乎没有死角，机器人可以从任何方向到达目标点。因此，机器人在导航过程中很少需要往回行驶。然而，在从点2到点3的导航过程中的一些路口，与FAR Planner和传统的D*Lite相比，本发明中的优先探索机制，可以让机器人避免探索无效空间，并以更短的距离到达导航点。图5(b)是本发明在CMU长隧道仿真环境下各导航点路径长度与其他主流方法各导航点路径长度对比结果图，本发明所提出的方法相比其他主流方法具有更多的导航路径，相比FAR Planner规划器的导航总路径减少了17.9％，相比D*Lite规划器的导航总路径较少了23.9％。图5(c)是本发明在CMU长隧道仿真环境下各导航点时间成本与其他主流方法各导航点时间成本对比结果图，相比FAR Planner规划器的时间总成本减少了18.7％，相比D*Lite规划器的导航总路径较少了26.2％，进一步验证了本发明的导航方法的先进性。FIG5(a) is a result diagram of the navigation route of the present invention in the CMU long tunnel simulation environment and the navigation route of other mainstream methods. For the tunnel environment, there are almost no dead ends, and the robot can reach the target point from any direction. Therefore, the robot rarely needs to drive back during the navigation process. However, at some intersections in the navigation process from point 2 to point 3, compared with FAR Planner and traditional D*Lite, the priority exploration mechanism in the present invention allows the robot to avoid exploring invalid space and reach the navigation point at a shorter distance. FIG5(b) is a result diagram of the comparison of the path length of each navigation point of the present invention and the path length of each navigation point of other mainstream methods in the CMU long tunnel simulation environment. The method proposed by the present invention has more navigation paths than other mainstream methods, and the total navigation path of the FAR Planner planner is reduced by 17.9%, and the total navigation path of the D*Lite planner is reduced by 23.9%. FIG5(c) is a comparison result diagram of the time cost of each navigation point of the present invention and the time cost of each navigation point of other mainstream methods in the CMU long tunnel simulation environment. Compared with the FAR Planner planner, the total time cost is reduced by 18.7%, and compared with the D*Lite planner, the total navigation path is reduced by 26.2%, which further verifies the advanced nature of the navigation method of the present invention.
图6(a)是本发明在室内长走廊环境下轨迹跟踪与其他主流方法轨迹跟踪对比结果图，由图可知，本发明提出的基于动态视距的路径跟踪方法规划出的路径更加贴合于真实路径。图6(b)是本发明在室内长走廊环境下水平方向和垂直方向的轨迹跟踪误差结果图，图6(c)是其他主流方法在室内长走廊环境下水平方向和垂直方向的轨迹跟踪误差结果图，对比以上可知，本发明提出的路径跟踪方法在X和Y方向的位置误差上分别比方法其他主流方法减少19.2％和35.1％。Figure 6(a) is a comparison result of the trajectory tracking of the present invention in an indoor long corridor environment with that of other mainstream methods. It can be seen from the figure that the path planned by the path tracking method based on dynamic line of sight proposed by the present invention is more in line with the real path. Figure 6(b) is a result of the trajectory tracking error in the horizontal and vertical directions of the present invention in an indoor long corridor environment, and Figure 6(c) is a result of the trajectory tracking error in the horizontal and vertical directions of other mainstream methods in an indoor long corridor environment. By comparison, it can be seen that the path tracking method proposed by the present invention reduces the position error in the X and Y directions by 19.2% and 35.1% respectively compared with other mainstream methods.
综上所述，结合本发明技术及相关实验结果可知，本发明提出的一种融合优先探索和定时器机制的三维激光雷达导航方法高效地在未知环境下完成了导航任务，减少了冗余空间探索和减少了地图更新时间，具有更低的运算成本。In summary, combined with the technology of the present invention and related experimental results, it can be seen that the three-dimensional lidar navigation method proposed in the present invention that integrates priority exploration and timer mechanism efficiently completes the navigation task in an unknown environment, reduces redundant space exploration and reduces map update time, and has lower computing costs.
上面结合附图对本发明的实例进行了描述，虽然本发明已以较佳实施例公开如上，但其并非用以限定本发明，任何熟悉此技术的人，在不脱离本发明的精神和范围内，都可做各种的改动与修饰，因此本发明的保护范围应该以权利要求书所界定的为准。The examples of the present invention are described above in conjunction with the accompanying drawings. Although the present invention has been disclosed as above in the form of a preferred embodiment, it is not intended to limit the present invention. Anyone familiar with this technology can make various changes and modifications without departing from the spirit and scope of the present invention. Therefore, the scope of protection of the present invention should be based on the definition of the claims.
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410205484.7ACN118068367A (en) | 2024-02-26 | 2024-02-26 | Three-dimensional laser radar navigation method integrating preferential exploration and timer mechanism |
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410205484.7ACN118068367A (en) | 2024-02-26 | 2024-02-26 | Three-dimensional laser radar navigation method integrating preferential exploration and timer mechanism |
| Publication Number | Publication Date |
|---|---|
| CN118068367Atrue CN118068367A (en) | 2024-05-24 |
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410205484.7APendingCN118068367A (en) | 2024-02-26 | 2024-02-26 | Three-dimensional laser radar navigation method integrating preferential exploration and timer mechanism |
| Country | Link |
|---|---|
| CN (1) | CN118068367A (en) |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118466517A (en)* | 2024-07-09 | 2024-08-09 | 南京师范大学 | A robot path planning method based on visibility graph |
| CN119200601A (en)* | 2024-09-20 | 2024-12-27 | 中南大学 | A mobile robot autonomous exploration method based on deep reinforcement learning |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118466517A (en)* | 2024-07-09 | 2024-08-09 | 南京师范大学 | A robot path planning method based on visibility graph |
| CN119200601A (en)* | 2024-09-20 | 2024-12-27 | 中南大学 | A mobile robot autonomous exploration method based on deep reinforcement learning |
| CN119200601B (en)* | 2024-09-20 | 2025-03-28 | 中南大学 | A mobile robot autonomous exploration method based on deep reinforcement learning |
| Publication | Publication Date | Title |
|---|---|---|
| CN110333714B (en) | A method and device for path planning of an unmanned vehicle | |
| Li et al. | Efficient laser-based 3D SLAM for coal mine rescue robots | |
| CN110471426B (en) | Automatic collision avoidance method for unmanned intelligent vehicles based on quantum wolf pack algorithm | |
| CN118068367A (en) | Three-dimensional laser radar navigation method integrating preferential exploration and timer mechanism | |
| CN110231824B (en) | Intelligent agent path planning method based on straight line deviation method | |
| CN115167433B (en) | Methods and systems for robots that integrate global vision to autonomously explore environmental information | |
| CN117249842A (en) | Unmanned vehicle mixed track planning method based on track smooth optimization | |
| CN115639823A (en) | Terrain sensing and movement control method and system for robot under rugged and undulating terrain | |
| CN118915716B (en) | A fusion path planning method for mobile robots based on Voronoi skeleton | |
| CN115371662B (en) | Static map construction method for removing dynamic objects based on probability grids | |
| CN117870653B (en) | A method for establishing and updating two-dimensional differential Euclidean signed distance field map | |
| CN114998276A (en) | Robot dynamic obstacle real-time detection method based on three-dimensional point cloud | |
| CN114740882A (en) | A Trajectory Generation Method for Flexible Target Tracking with UAV Guaranteed Visibility | |
| Si et al. | TOM-Odometry: A generalized localization framework based on topological map and odometry | |
| CN116817913B (en) | New path planning method utilizing turning penalty factors and twin road network improvement | |
| CN116429145B (en) | Automatic docking navigation method and system for unmanned vehicle and garbage can under complex scene | |
| CN117970928A (en) | Path planning method for multiple mobile robots based on ROS system | |
| CN118393518A (en) | Laser radar repositioning system and method based on fusion semantic information descriptor | |
| CN117232545A (en) | Path planning method based on deep learning road environment perception | |
| Li et al. | RF-LOAM: Robust and fast LiDAR odometry and mapping in urban dynamic environment | |
| CN112904901A (en) | Path planning method based on binocular vision slam and fusion algorithm | |
| Dong | Improved A* algorithm for intelligent navigation path planning | |
| CN118466517B (en) | A robot path planning method based on visibility graph | |
| CN118655885A (en) | Path planning system and method for cooperative operation of special vehicles in dynamic environment | |
| CN115220448B (en) | A robot fast path planning method based on sparse visibility graph |
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |