技术领域Technical Field
本发明涉及自主导航路径规划技术领域。The invention relates to the technical field of autonomous navigation path planning.
背景技术Background technique
机器人的巡线导航技术是一种使机器人沿着预定的路线自动巡逻并执行任务的技术。传统的巡线机器人通过对感知数据进行分析和处理，可以构建精确的环境地图，描述周围环境的几何和语义信息，包括障碍物位置、地形高度、房间结构等，在获得准确的环境地图后，机器人需要规划一条安全且高效的路径从起始点到目标点。机器人自主导航系统收到其他ROS节点发出的巡线节点队列(仅有位姿信息)后，使用规定好的加速度和最大速度生成路径进行巡线路径规划，利用避障节点进行避障，从而实现巡线导航。The robot's line patrol navigation technology is a technology that enables the robot to automatically patrol and perform tasks along a predetermined route. Traditional line patrol robots can build accurate environmental maps by analyzing and processing perception data, describing the geometric and semantic information of the surrounding environment, including obstacle locations, terrain heights, room structures, etc. After obtaining an accurate environmental map, the robot needs to plan a safe and efficient path from the starting point to the target point. After the robot's autonomous navigation system receives the line patrol node queue (only posture information) sent by other ROS nodes, it uses the specified acceleration and maximum speed to generate a path for line patrol path planning, and uses obstacle avoidance nodes to avoid obstacles, thereby realizing line patrol navigation.
上述传统的巡线导航规划无法进行特定路径下的避障切换，无法规划不同路径下的加速度大小、最大速度，当机器人需要过弯道时，无法进行复杂的曲线规划，且当输入的点位为错误点位时，也会进行路径规划。The above-mentioned traditional line patrol navigation planning cannot switch to obstacle avoidance under a specific path, cannot plan the acceleration size and maximum speed under different paths, cannot perform complex curve planning when the robot needs to cross a curve, and will also perform path planning when the input point is an incorrect point.
发明内容Summary of the invention
本发明的目的在于提供一种能够提供更为合适安全的规划的一种基于图结构的自主导航路径规划方法，该方法以图结构进行路径规划，能够为机器人提供不同路径上的不同避障信息。The purpose of the present invention is to provide an autonomous navigation path planning method based on a graph structure that can provide more appropriate and safe planning. The method uses a graph structure to perform path planning and can provide the robot with different obstacle avoidance information on different paths.
为实现上述目的，本发明的技术方案是：一种基于图结构的自主导航路径规划方法，其特征在于，路径规划方法包括预设置方法步骤和运行方法步骤，To achieve the above object, the technical solution of the present invention is: an autonomous navigation path planning method based on a graph structure, characterized in that the path planning method includes a presetting method step and an operating method step,
所述预设置方法步骤包括如下：The pre-setting method steps include the following:
S11、预收集机器人巡线路径上的特殊点位，在系统中设置到达特殊点位所需的角度和旋转时的避障信息；S11, pre-collect special points on the robot's patrol path, and set the angle required to reach the special points and the obstacle avoidance information during rotation in the system;
S12、在系统中将特殊点位进行连接，得到边，并设置每条边的避障信息和速度信息；S12, connecting the special points in the system to obtain edges, and setting obstacle avoidance information and speed information for each edge;
S13、测试完成步骤S12后每一条边的运行情况，并进行相应调整，生成完整的图结构；S13, testing the operation of each edge after completing step S12, and making corresponding adjustments to generate a complete graph structure;
所述运行方法步骤包括如下：The operation method steps include the following:
S21、在图结构中搜索系统收到的点列中所有的点，确定图结构中里包含点列中所有的点，并且点列中所有的点具有连通性，点列记为A{A1，A2，…An}；S21, searching for all points in the point sequence received by the system in the graph structure, determining that the graph structure contains all points in the point sequence, and all points in the point sequence have connectivity, and the point sequence is recorded as A{A1, A2, ...An};
S22、对收到的点列中每两个点进行最短路径算法，并对该两个点不是相邻的进行补充点，保证点列中的点都是在图结构中相邻的，得到n-1条路径后叠加成完整路径记为B{B1，B2，…Bm}；S22, perform the shortest path algorithm on every two points in the received point sequence, and add points if the two points are not adjacent, to ensure that the points in the point sequence are adjacent in the graph structure, and then superimpose n-1 paths to form a complete path, which is recorded as B{B1, B2, ...Bm};
S23、利用补充后的点列寻找对应的边，读取边中的避障信息和速度信息规划生成完整的队列。S23, using the supplemented point sequence to find the corresponding edge, read the obstacle avoidance information and speed information in the edge to plan and generate a complete queue.
步骤S23的具体方法步骤如下：The specific method steps of step S23 are as follows:
S31、首先遍历根据路径B整个边集合，路径B整个边集合记为En，得到所需的边集合记为E{E1，E2，…Em-1}，S31, first traverse the entire edge set of path B, the entire edge set of path B is recorded as En, and the required edge set is recorded as E{E1, E2, ...Em-1},
S32、读取Bi和Bi+1的边Ei计算出加速长度所需长度记为L1和减速所需长度记为L3，其中i表示第i个，在根据边的权重减去L1、L3得到匀速距离记为L2，表达式如下：S32, read the edge Ei of Bi and Bi+1 to calculate the required length for acceleration, recorded as L1, and the required length for deceleration, recorded as L3, where i represents the i-th, and subtract L1 and L3 according to the weight of the edge to obtain the uniform speed distance, recorded as L2, the expression is as follows:
S33、根据得到的加减速长度和匀速运动长度进行点列生成，表达式如下：S33, generating a point sequence according to the obtained acceleration and deceleration length and uniform motion length, the expression is as follows:
其中a表示的是该边的加速度，d表示的是每两个规划点之间的距离，Vmax表示最大速度，V1、V2分别表示在两点的规划速度、W表示该边的长度、dx、dy分别表示边对应的两个顶点之间的位姿x、y的数值差；Where a represents the acceleration of the edge, d represents the distance between each two planning points, Vmax represents the maximum speed, V1 and V2 represent the planning speeds at the two points, W represents the length of the edge, dx and dy represent the numerical differences in the position x and y between the two vertices corresponding to the edge, respectively.
按照以上方法生成对应边中所有点的规划，再读取避障参数，由每条不同的边生成的队列拼接生成完整的队列后发送给控制节点控制机器人进行运动。Generate the plan for all points in the corresponding edge according to the above method, then read the obstacle avoidance parameters, and concatenate the queues generated by each different edge to generate a complete queue, which is then sent to the control node to control the robot to move.
所述步骤S23的具体方法步骤中，当边为曲线时，点列中包含贝塞尔曲线生成的控制点，生成队列的公式如下：In the specific method steps of step S23, when the edge is a curve, the point sequence includes control points generated by the Bezier curve, and the formula for generating the sequence is as follows:
xc和yc表示控制点的坐标，t为每个生成点的比例参数，当机器人进行曲线运动时，令机器人始终保持匀速运动，确保机器人运动的稳定性。xc and yc represent the coordinates of the control points, and t is the scale parameter of each generated point. When the robot performs curved motion, the robot always maintains a uniform speed to ensure the stability of the robot's motion.
所述步骤S1中特殊点位包括机器人工作点和转弯点，和/或，所述速度信息包括最大速度和加速度。The special points in step S1 include the robot working point and turning point, and/or the speed information includes the maximum speed and acceleration.
所述步骤S2中如果边为曲线，则利用贝塞尔曲线设置，通过调整控制点，改变曲线的弧度和偏移量。In step S2, if the edge is a curve, a Bezier curve is used to set the edge and the curvature and offset of the curve are changed by adjusting the control points.
通过采用上述技术方案，本发明的有益效果是：本发明的方法将一些特殊的点位以顶点的形式存储于图结构中，对于相邻的点之间进行连接，并将其属性存储于边当中，其中点的属性有位姿、速度、以及避障信息，而边的属性有加速度、最大速度、权重、以及避障信息。该方法可以对任何一段路径设置不同的避障，也可以根据不同的点设置避障状态，在一些特殊的路段可以设置不同的速度和加速度。在实际的应用场景当中，机器人经常会遇到各种复杂的工作环境。该方法为机器人提供不同路径上的的不同避障信息，从而提供合适安全的规划，实现本发明的目的。By adopting the above technical solution, the beneficial effects of the present invention are as follows: the method of the present invention stores some special points in the form of vertices in the graph structure, connects adjacent points, and stores their attributes in the edges, wherein the attributes of the points include posture, speed, and obstacle avoidance information, and the attributes of the edges include acceleration, maximum speed, weight, and obstacle avoidance information. The method can set different obstacle avoidance for any section of the path, and can also set obstacle avoidance states according to different points, and can set different speeds and accelerations in some special sections. In actual application scenarios, robots often encounter various complex working environments. The method provides the robot with different obstacle avoidance information on different paths, thereby providing appropriate and safe planning to achieve the purpose of the present invention.
附图说明BRIEF DESCRIPTION OF THE DRAWINGS
图1是实施例涉及的一种基于图结构的自主导航路径规划方法的原理框图。FIG1 is a principle block diagram of an autonomous navigation path planning method based on a graph structure according to an embodiment.
具体实施方式Detailed ways
为了进一步解释本发明的技术方案，下面通过具体实施例来对本发明进行详细阐述。In order to further explain the technical solution of the present invention, the present invention is described in detail below through specific embodiments.
本实施例公开的一种基于图结构的自主导航路径规划方法，路径规划方法包括预设置方法步骤和运行方法步骤。This embodiment discloses an autonomous navigation path planning method based on a graph structure, and the path planning method includes a presetting method step and an operating method step.
所述预设置方法步骤包括如下：The pre-setting method steps include the following:
S11、为了使机器人按规划的进行运动，因此需要预收集机器人巡线路径上的特殊点位，特殊点位包括机器人工作点和转弯点等，可人工收集设置，在系统中设置到达特殊点位所需的角度和旋转时的避障信息，；S11. In order to make the robot move according to the plan, it is necessary to pre-collect special points on the robot's patrol path. Special points include robot working points and turning points, etc. They can be manually collected and set. The angle required to reach the special points and the obstacle avoidance information during rotation are set in the system;
S12、在系统中将特殊点位进行连接，得到边，并设置每条边的避障信息和速度信息(包括最大速度、加速度等)，本步骤中如果边为曲线，则利用贝塞尔曲线设置，通过调整控制点，改变曲线的弧度和偏移量；S12, connecting the special points in the system to obtain edges, and setting the obstacle avoidance information and speed information (including maximum speed, acceleration, etc.) of each edge. In this step, if the edge is a curve, the Bezier curve is used to set it, and the curvature and offset of the curve are changed by adjusting the control points;
S13、测试完成步骤S12后每一条边的运行情况，并进行相应调整，生成完整的图结构；S13, testing the operation of each edge after completing step S12, and making corresponding adjustments to generate a complete graph structure;
所述运行方法步骤包括如下：The operation method steps include the following:
S21、在图结构中搜索系统收到的点列中所有的点，确定图结构中里包含点列中所有的点，并且点列中所有的点具有连通性，点列记为A{A1，A2，…An}；S21, searching for all points in the point sequence received by the system in the graph structure, determining that the graph structure contains all points in the point sequence, and all points in the point sequence have connectivity, and the point sequence is recorded as A{A1, A2, ...An};
S22、对收到的点列中每两个点进行最短路径算法，并对该两个点不是相邻的进行补充点，保证点列中的点都是在图结构中相邻的，得到n-1条路径后叠加成完整路径记为B{B1，B2，…Bm}；S22, perform the shortest path algorithm on every two points in the received point sequence, and add points if the two points are not adjacent, to ensure that the points in the point sequence are adjacent in the graph structure, and then superimpose n-1 paths to form a complete path, which is recorded as B{B1, B2, ...Bm};
S23、利用补充后的点列寻找对应的边，读取边中的避障信息和速度信息规划生成完整的队列。S23, using the supplemented point sequence to find the corresponding edge, read the obstacle avoidance information and speed information in the edge to plan and generate a complete queue.
步骤S23的具体方法步骤如下：The specific method steps of step S23 are as follows:
S31、首先遍历根据路径B整个边集合，路径B整个边集合记为En，得到所需的边集合记为E{E1，E2，…Em-1}，S31, first traverse the entire edge set of path B, the entire edge set of path B is recorded as En, and the required edge set is recorded as E{E1, E2, ...Em-1},
S32、读取Bi和Bi+1的边Ei计算出加速长度所需长度记为L1和减速所需长度记为L3，其中i表示第i个，在根据边的权重减去L1、L3得到匀速距离记为L2，表达式如下：S32, read the edge Ei of Bi and Bi+1 to calculate the required length for acceleration, recorded as L1, and the required length for deceleration, recorded as L3, where i represents the i-th, and subtract L1 and L3 according to the weight of the edge to obtain the uniform speed distance, recorded as L2, the expression is as follows:
S33、根据得到的加减速长度和匀速运动长度进行点列生成，表达式如下：S33, generating a point sequence according to the obtained acceleration and deceleration length and uniform motion length, the expression is as follows:
其中a表示的是该边的加速度，d表示的是每两个规划点之间的距离，Vmax表示最大速度，V1、V2分别表示在两点的规划速度、W表示该边的长度、dx、dy分别表示边对应的两个顶点之间的位姿x、y的数值差；Where a represents the acceleration of the edge, d represents the distance between each two planning points, Vmax represents the maximum speed, V1 and V2 represent the planning speeds at the two points, W represents the length of the edge, dx and dy represent the numerical differences in the position x and y between the two vertices corresponding to the edge, respectively.
按照以上方法生成对应边中所有点的规划，再读取避障参数，由每条不同的边生成的队列拼接生成完整的队列后发送给控制节点控制机器人进行运动。Generate the plan for all points in the corresponding edge according to the above method, then read the obstacle avoidance parameters, and concatenate the queues generated by each different edge to generate a complete queue, which is then sent to the control node to control the robot to move.
所述步骤S23的具体方法步骤中，当边为曲线时，点列中包含贝塞尔曲线生成的控制点，生成队列的公式如下：In the specific method steps of step S23, when the edge is a curve, the point sequence includes control points generated by the Bezier curve, and the formula for generating the sequence is as follows:
xc和yc表示控制点的坐标，t为每个生成点的比例参数，其他字母符号表示同上述表达式的相同字母符号，当机器人进行曲线运动时，令机器人始终保持匀速运动，确保机器人运动的稳定性。xc andyc represent the coordinates of the control points, t is the scale parameter of each generated point, and other letter symbols represent the same letter symbols as the above expression. When the robot performs curved motion, the robot always maintains a uniform speed to ensure the stability of the robot's motion.
本发明以图结构的方法进行机器人的路径规划，通过图中的边属性设置直线避障，通过点的属性来进行设置旋转避障，在边上设置机器人运动的加速度和最大速度，该方法可以对任何一段路径设置不同的避障，也可以根据不同的点设置避障状态，在一些特殊的路段可以设置不同的速度和加速度，在实际的应用场景当中，机器人经常会遇到各种复杂的工作环境，该方法可以为机器人提供不同路径上的的不同避障信息，从而提供合适安全的规划，The present invention uses a graph structure method to plan the path of the robot. Straight-line obstacle avoidance is set by edge attributes in the graph, rotation obstacle avoidance is set by point attributes, and the acceleration and maximum speed of the robot movement are set on the edge. This method can set different obstacle avoidances for any section of the path, and can also set obstacle avoidance states according to different points. Different speeds and accelerations can be set in some special sections. In actual application scenarios, robots often encounter various complex working environments. This method can provide different obstacle avoidance information on different paths for the robot, thereby providing appropriate and safe planning.
本发明的方法提出路网的概念，对于已有的点位、路径进行抽象化，对这些抽象化的点位赋值，机器人通过读取图结构当中的数据进行运动，从而做到有规划、稳定地运行，解决了传统的巡线导航规划无法进行特定路径下的避障切换，无法规划不同路径下的加速度大小、最大速度的问题，以及解决了当机器人需要过弯道时无法进行复杂的曲线规划和当输入的点位为错误点位时也会进行路径规划问题。The method of the present invention proposes the concept of a road network, abstracts existing points and paths, assigns values to these abstract points, and the robot moves by reading data in the graph structure, thereby achieving planned and stable operation. The method solves the problems that traditional line patrol navigation planning cannot perform obstacle avoidance switching under a specific path, cannot plan the acceleration size and maximum speed under different paths, and solves the problems that the robot cannot perform complex curve planning when it needs to cross a curve and that path planning will also be performed when the input point is an incorrect point.
上述实施例和图式并非限定本发明的产品形态和式样，任何所属技术领域的普通技术人员对其所做的适当变化或修饰，皆应视为不脱离本发明的专利范畴。The above embodiments and drawings do not limit the product form and style of the present invention. Any appropriate changes or modifications made thereto by ordinary technicians in the relevant technical field should be deemed to be within the patent scope of the present invention.
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410255372.2ACN118031969A (en) | 2024-03-06 | 2024-03-06 | A method for autonomous navigation path planning based on graph structure |
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410255372.2ACN118031969A (en) | 2024-03-06 | 2024-03-06 | A method for autonomous navigation path planning based on graph structure |
| Publication Number | Publication Date |
|---|---|
| CN118031969Atrue CN118031969A (en) | 2024-05-14 |
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410255372.2APendingCN118031969A (en) | 2024-03-06 | 2024-03-06 | A method for autonomous navigation path planning based on graph structure |
| Country | Link |
|---|---|
| CN (1) | CN118031969A (en) |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113205601A (en)* | 2021-05-27 | 2021-08-03 | 北京有竹居网络技术有限公司 | Roaming path generation method and device, storage medium and electronic equipment |
| CN114509065A (en)* | 2022-02-16 | 2022-05-17 | 北京易航远智科技有限公司 | Map construction method, map construction system, vehicle terminal, server side and storage medium |
| US20220371619A1 (en)* | 2021-05-24 | 2022-11-24 | Yandex Self Driving Group Llc | Method and device for operating a self-driving car |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20220371619A1 (en)* | 2021-05-24 | 2022-11-24 | Yandex Self Driving Group Llc | Method and device for operating a self-driving car |
| CN113205601A (en)* | 2021-05-27 | 2021-08-03 | 北京有竹居网络技术有限公司 | Roaming path generation method and device, storage medium and electronic equipment |
| CN114509065A (en)* | 2022-02-16 | 2022-05-17 | 北京易航远智科技有限公司 | Map construction method, map construction system, vehicle terminal, server side and storage medium |
| Title |
|---|
| 江欣恺: "智能家居机器人导航技术研究", 《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》, 15 March 2022 (2022-03-15), pages 038 - 2075* |
| Publication | Publication Date | Title |
|---|---|---|
| CN113085850B (en) | Vehicle obstacle avoidance method and device, electronic equipment and storage medium | |
| CN108664024B (en) | Motion planning and cooperative positioning method and device for unmanned vehicle network formation | |
| Murali et al. | Perception-aware trajectory generation for aggressive quadrotor flight using differential flatness | |
| Raja et al. | PFIN: An efficient particle filter-based indoor navigation framework for UAVs | |
| Moore et al. | A generalized extended kalman filter implementation for the robot operating system | |
| CN113470089B (en) | A method and system for cross-domain co-location and mapping based on 3D point cloud | |
| Hüppi et al. | T-prm: Temporal probabilistic roadmap for path planning in dynamic environments | |
| CN101943916B (en) | Kalman filter prediction-based robot obstacle avoidance method | |
| CN114510057A (en) | A ROS-based autonomous navigation method for mobile robots in indoor environments | |
| US20200160732A1 (en) | Area evaluation system, method, and program | |
| CN111260751A (en) | Mapping method based on multi-sensor mobile robot | |
| WO2021248798A1 (en) | Path following method, system and apparatus, and computer-readable storage medium | |
| CN115435781A (en) | Robot indoor and outdoor seamless positioning method and system based on multi-sensor fusion | |
| Abbenseth et al. | Cloud-based cooperative navigation for mobile service robots in dynamic industrial environments | |
| CN109857134A (en) | Unmanned plane tracking control system and method based on A*/minimum_snap algorithm | |
| CN112859110A (en) | Positioning and navigation method based on three-dimensional laser radar | |
| Kong et al. | Embodied AI in mobile robots: Coverage path planning with large language models | |
| Oleynikova et al. | A complete system for vision-based micro-aerial vehicle mapping, planning, and flight in cluttered environments | |
| CN118466496A (en) | Vehicle track determining method, vehicle track determining device and electronic device | |
| CN118031969A (en) | A method for autonomous navigation path planning based on graph structure | |
| CN117516573A (en) | Multi-target waypoint planning method and system | |
| CN117707186A (en) | Indoor robot positioning and path planning system | |
| Meyer et al. | Top-uav: Open-source time-optimal trajectory planner for point-masses under acceleration and velocity constraints | |
| Du et al. | Interactive sensing and path-planning with incremental 3D path repair for a quadrotor UAV in cluttered and partially known environments | |
| Daass et al. | Design of multi-sensor fusion architectures based on the covariance intersection algorithm—estimating calculation burdens |
| 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 |