Robot outdoor large-scene autonomous navigation method based on open digital map

Abstract

The invention particularly relates to a robot outdoor large-scale scene autonomous navigation method based on an open digital map, which comprises the following steps: extracting a road network structure, constructing a road network topological graph, and modeling a road network space by using an R tree; associating a starting point with a target point according to the road network topological graph of the R-tree structure, and performing global path planning based on a Dijkstra algorithm to obtain path points from the starting point to the target point; acquiring laser radar sensor point clouds, and generating a local voxel map by using the point clouds after the ground points are filtered out; performing local target point mapping by combining the global path line segment and a local voxel map to generate a local target point; performing local front-end path search based on a JPS algorithm to obtain a local front-end path from the current position of the robot to a local target point; a safety corridor is constructed according to the local front-end path, and trajectory optimization is carried out; and tracking the optimized trajectory, and planning the speed by considering the nonlinear model of the robot and the actual motion state. The method provided by the invention can realize outdoor large-range long-distance autonomous navigation.

Description

Technical Field

[0001] The present invention relates to the technical field of robot navigation, and particularly relates to a method for autonomous navigation of a robot in a large outdoor scene based on an open digital map. Background Art

[0002] Autonomous navigation is a key technology for realizing autonomous decision-making and control of robots. Its main goal is to enable robots to independently complete path planning, obstacle avoidance, and dynamic adaptation from a starting point to a target point based on perception information in an unknown or dynamic environment. The autonomous navigation technology enables robots to flexibly perceive, make autonomous decisions, and plan paths in complex and changing environments, thereby achieving efficient, safe, and accurate movement. With the continuous expansion of application fields, autonomous navigation has brought revolutionary progress and development to industrial automation. As an important application field, large-scale outdoor structured scenes have shown important value and strategic status in intelligent logistics, search and rescue patrols, and military strike scenarios.

[0003] Thanks to the rapid development of scene reconstruction technology, the autonomous navigation scheme of building a map first and then planning has been widely applied. Existing navigation methods usually construct a global static metric map based on SLAM (Simultaneous Localization and Mapping) to obtain prior information of unknown areas, and then perform relocalization and path planning on this map to complete the navigation task. The navigation scheme based on SLAM has been widely applied due to its advantages of flexibility, efficiency, and reliability. However, there are still many problems in the face of large-scale complex outdoor scenes, which are specifically manifested in the following aspects:

[0004] 1. The map-based navigation technology requires robots to pre-construct a high-precision environmental map. However, limited by the characteristics of scene scale, drift error, and environmental dynamic changes, there are serious challenges in constructing a high-precision map in a large-scale scene. For example, as the mapping scale expands, how to ensure the consistency of the map; in an environment with scarce information, how to ensure the reliability of data association; when the environment changes drastically, how to cope with the invalidation of the original map, etc. These problems make the navigation framework based on SLAM inapplicable to large-scale long-term navigation scenarios.

[0005] 2. Although existing planning methods can handle general obstacle avoidance problems, they have significant limitations when facing complex structured scenarios. First, existing methods mainly rely on static local cost maps, and their response to dynamically changing environments or fast-moving obstacles is relatively laggy, making it difficult to ensure the real-time performance and safety of path planning. Second, local planning algorithms are prone to falling into local optimal solutions, especially in structured areas with complex environmental features and dense obstacles, resulting in non-optimal planned paths. Therefore, it is urgently necessary to improve the stability and safety of the planned path, overcome the defects of traditional local planning strategies in complex structured scenarios, and achieve a stable, accurate, efficient, and safe navigation system.

[0006] Autonomous navigation in large-scale structured scenarios is an important direction in the application field of mobile robots. In response to the challenges faced by traditional navigation schemes that first build a map and then plan in outdoor large-scale long-distance navigation tasks, developing an autonomous navigation system suitable for large-scale structured scenarios, effectively addressing the problems existing in existing methods, and providing technical support for mobile robots to efficiently and autonomously execute various tasks have profound significance for expanding the application scenarios of mobile robots in civilian and military fields and promoting the intelligent development of unmanned systems.

[0007] It should be noted that the information disclosed in the above background art section is only used to enhance the understanding of the background of the present invention, and thus may include information that does not constitute the prior art known to those of ordinary skill in the art. Summary of the Invention

[0008] The present invention provides a method for autonomous navigation of a robot in a large outdoor scene based on an open digital map, which can achieve autonomous navigation over a large outdoor range at a long distance, and thus can overcome the defects existing in the prior art to a certain extent.

[0009] Other features and advantages of the present invention will become apparent through the following detailed description, or will be partially learned through the practice of the present invention.

[0010] According to a first aspect of the present invention, there is provided a method for autonomous navigation of a robot in a large outdoor scene based on an open digital map, the method comprising:

[0011] Extract the road network structure from the OSM raw data, convert the road nodes and their connection relationships in the road network information into the data structure form of a topological graph, construct a road network topological graph, and model the road network space using an R-tree;

[0012] Perform start point and end point association based on the road network topological graph with an R-tree structure, and perform global path planning based on the Dijkstra algorithm to obtain a series of path points from the start point to the end point;

[0013] Obtain the point cloud of the lidar sensor, and generate a local voxel map using the point cloud after filtering the ground points; combine the global path segment and the local voxel map for local target point mapping to generate local target points;

[0014] Based on the JPS algorithm, perform local front-end path search to obtain a series of local front-end paths from the current position of the robot to the local target points;

[0015] Construct a safety corridor according to the local front-end path and perform trajectory optimization;

[0016] Track the optimized trajectory, and plan the speed considering the non-linear model and the actual motion state of the robot.

[0017] In some exemplary embodiments, extracting the road network structure from the OSM raw data, converting the road nodes in the road network information and their connection relationships with each other into the data structure form of a topological graph, constructing a road network topological graph, and modeling the road network space with an R-tree, including:

[0018] Select the target area on the OSM official website and export the map file in XML format;

[0019] Search for the way elements in the XML file, and extract the data whose key value of the tag label in its attributes is "highway". These data are the main road data;

[0020] Convert the road nodes in the road network information and their connection relationships with each other into the data structure form of a topological graph;

[0021] Construct the topological map into an R-tree, establish leaf nodes with each road section as the minimum unit, and recursively push layer by layer. Finally, divide the vector road network with the minimum bounding rectangle and establish the R-tree of the road network over the entire space.

[0022] In some exemplary embodiments, the association of the starting point and the target point according to the road network topological graph with the R-tree structure includes:

[0023] Use the road network space segmentation and modeling method based on the R-tree structure to quickly retrieve candidate road sections within a certain range of the target point;

[0024] Calculate the projection distance from the target point to each candidate road section, and judge whether the projection point is on the extension line of the road section; ignore the road sections projected on the extension line, and finally set the road section with the minimum projection distance as the target road section, and set its projection point as the associated node:

[0025] According to the relationship between the associated node and the two end nodes of the target road section, establish a new connection relationship to update the road network topological graph into a complete navigation topological graph.

[0026] In some exemplary embodiments, obtaining the lidar sensor point cloud and generating a local voxel map using the point cloud after filtering out ground points includes:

[0027] Obtain the lidar sensor point cloud and segment the ground point cloud using the Linefit algorithm;

[0028] Initialize a number of unoccupied voxels around the robot, and then set the corresponding voxels to occupied according to the positions of the non-ground point cloud, thereby constructing a local voxel map.

[0029] In some exemplary embodiments, combining the global path segment and the local voxel map for local target point mapping to generate local target points includes:

[0030] Determine the linear parametric equation according to the coordinates of the next target point and the previous point;

[0031] Substitute the linear parametric equation into the plane equation of the perception boundary around the robot and solve it. Substitute the obtained value into the linear parametric equation to obtain the coordinates of the intersection point, which is the local target point.

[0032] In some exemplary embodiments, constructing a safety corridor based on the local front path and performing trajectory optimization includes:

[0033] Construct a safety corridor jointly based on the path searched by the front end and the local voxel map, and there are no obstacles in the corridor;

[0034] Design an optimization objective function:

[0035]

[0036] where j n represents the smoothing term, and J dis is the safety constraint term; the first part α·∑||j n || 2 dt of the objective function aims to minimize the sum of the squares of the jerk, and the second part (1-α)·J dis is an optimization term related to the distance from the front path. α is the weight coefficient between the smoothing term and the safety term;

[0037] Construct constraints, including start and end point state constraints, safety corridor constraints, state continuity constraints, and state boundary constraints;

[0038] Use the Gurobi optimizer to solve for the coefficients of the piecewise polynomial trajectory, and obtain continuous trajectory points by densely sampling on the piecewise trajectory.

[0039] In some exemplary embodiments, tracking the optimized trajectory and planning the speed by considering the non-linear model and actual motion state of the robot includes:

[0040] Obtain the actual state of the robot, including position, speed, and yaw angle;

[0041] Calculate the lateral error d and the direction offset error η;

[0042] Set the discrete non-linear motion equation of the system:

[0043] s t = f(s t-1 , u t-1 )

[0044] where u t-1 = (a t-1 , ω t-1 ), a t-1 represents the control quantity of the robot in the forward direction, and ω t-1 is the rotation control quantity of the robot;

[0045] Construct an optimization problem, including a loss function and constraint conditions;

[0046]

[0047] s.t.

[0048] x0 = x(t)

[0049]

[0050] where represents the reference speed; N represents the model prediction step size; λ d , λ v , λ a , λ ω , respectively represent the individual proportion states of lateral offset, angular offset, acceleration control, rotation control, acceleration control change amount, and rotation control change amount in the loss function; x min , x max and u min , u max respectively represent the upper and lower limits of the state variables and control quantities during the optimization process;

[0051] Solve the above optimization problem, and calculate the desired speed command according to the optimization variable u t and the current speed:

[0052] v x = v0 + a0 × dt

[0053] ω z = ω0

[0054] wherein, v x and ω z are respectively the linear velocity and angular velocity driving amounts within this period, and a0 and ω0 are respectively the optimized control amounts at the initial moment.

[0055] According to a second aspect of the present invention, there is provided a storage medium having a computer program stored thereon, and when the computer program is executed by a processor, it implements the method for autonomous navigation of a robot in a large outdoor scene based on an open digital map described in the first aspect above.

[0056] According to a third aspect of the present invention, there is provided a computer program product having a computer program stored thereon, and when the computer program is executed by a processor, it implements the method for autonomous navigation of a robot in a large outdoor scene based on an open digital map described in the first aspect above.

[0057] According to a fourth aspect of the present invention, there is provided an electronic device, comprising:

[0058] a processor; and

[0059] a memory for storing executable instructions of the processor;

[0060] wherein, the processor is configured to implement the method for autonomous navigation of a robot in a large outdoor scene based on an open digital map described in the first aspect above when executing the executable instructions.

[0061] The design method of an autonomous navigation system for a large-scale complex outdoor scene based on an open-source digital map and the design method of a robot autonomous navigation system based on an open-source digital map provided by the embodiments of the present invention can get rid of the dependence on high-precision maps. After a navigation target point is given, the present invention can use the open-source digital map to plan a global path, and then continuously plan a safe and smooth trajectory for the robot to move forward according to the actual perceived environmental data, and plan an accurate and stable speed. The following effects can be achieved:

[0062] (1) The environmental modeling efficiency is improved

[0063] The present invention constructs a road network topology structure by using open-source map data (such as OSM), replacing the dependence on high-precision environmental maps in traditional SLAM technology, thereby avoiding the efficiency bottleneck caused by limited map construction in large-scale scenes. Through the screening of road network data and the modeling of topological graphs, the present invention realizes the rapid abstraction and accurate modeling of the navigation area, effectively reducing the computational burden of map generation and update in complex environments, and at the same time providing reliable prior information support for subsequent path planning. Compared with traditional methods, this method significantly improves the modeling efficiency and has strong practicability and adaptability.

[0064] (2) Enhances trajectory safety

[0065] In view of the characteristics of dense obstacles and significant dynamic changes in complex scenarios, the present invention proposes a method for constructing a safety corridor based on hard constraints, and combines an innovative front-end path projection-guided trajectory optimization algorithm to refine and optimize the initial path. During the planning process, by leaving a sufficient safety margin between the desired trajectory and obstacles, the risk of collision between the trajectory and obstacles is significantly reduced. At the same time, the optimization algorithm takes into account the smoothness and safety of the trajectory, enabling the robot to stably and efficiently complete the navigation task in various complex dynamic environments and ensuring the safety during the task execution process.

[0066] (3) Provides a complete navigation framework

[0067] The present invention adopts a modular design and constructs a complete navigation framework integrating road network topology generation, path planning, and motion planning. The road network topology generation module efficiently extracts road information from the open-source map and establishes topological relationships. The path planning module realizes the combination of global and local, and avoids dynamic obstacles under the guidance of the global path. The motion planning module further optimizes the path to meet the kinematic and dynamic constraints of the robot. This framework has high flexibility and scalability, and can not only meet the diverse navigation needs in large-scale outdoor scenarios, but also provide a systematic solution for mobile robots in complex dynamic environments, greatly enhancing the practicality and reliability of their autonomous navigation

[0068] It should be understood that the above general description and the following detailed description are only exemplary and explanatory, and cannot limit the present invention. Brief Description of the Drawings

[0069] The drawings herein are incorporated into the specification and constitute a part of this specification, showing embodiments consistent with the present invention, and are used together with the specification to explain the principles of the present invention. Obviously, the drawings in the following description are only some embodiments of the present invention, and those of ordinary skill in the art can obtain other drawings based on these drawings without creative efforts.

[0070] Figure 1 is the overall flowchart for the present invention to achieve navigation in large-scale complex scenarios;

[0071] Figure 2 is the OSM road network screening process of the present invention: (a) showing the attribute information of the way element; (b) showing the road network extraction result;

[0072] Figure 3 is the visualization effect diagram of the topological graph of the present invention;

[0073] Figure 4It is the global association result diagram of the present invention: (a) represents the starting point association; (b) represents the target point association

[0074] Figure 5 It is the global planning effect diagram of the present invention;

[0075] Figure 6 It is the schematic diagram of the global target point mapping of the present invention;

[0076] Figure 7 It is the local path planning effect diagram of the present invention: (a) is the experimental scene; (b) is the result of the planned path;

[0077] Figure 8 It is the schematic diagram of the principle of trajectory optimization improvement of the present invention;

[0078] Figure 9 It is the comparison diagram of the effects of the trajectory optimization algorithms of the present invention: (a1)(a2)(a3) are the optimized trajectory results of the existing algorithms, and (b1)(b2)(b3) are the optimized trajectory results of the present invention.

[0079] Figure 10 It is the trajectory tracking effect diagram of the present invention: (a) shows the tracking process; (b) shows the planned speed curve graph;

[0080] Figure 11 It is the method flow chart of the embodiment of the present invention. Detailed implementation manners

[0081] Now, the example embodiments will be described more fully with reference to the accompanying drawings. However, the example embodiments can be implemented in various forms and should not be construed as limited to the examples set forth herein; rather, these embodiments are provided so that this invention will be more complete and comprehensive, and will fully convey the concept of the example embodiments to those skilled in the art. The described features, structures, or characteristics can be combined in any suitable manner in one or more embodiments.

[0082] In addition, the accompanying drawings are only schematic diagrams of the present invention and are not necessarily drawn to scale. The same reference numerals in the drawings denote the same or similar parts, and thus their repeated description will be omitted. Some of the block diagrams shown in the drawings are functional entities and do not necessarily correspond to physically or logically independent entities. These functional entities can be implemented in software form, or in one or more hardware modules or integrated circuits, or in different networks and / or processor devices and / or microcontroller devices.

[0083] Aiming at the disadvantages and deficiencies of the prior art, an autonomous navigation method for robots in large outdoor scenarios based on open digital maps is provided in this exemplary embodiment. When a target point is given, the robot can utilize the road network data in the open-source digital map and combine it with the real-time perceived environmental data to plan a safe and smooth motion trajectory and drive the robot to approach the target point steadily.

[0084] The method of the present invention involves three modules: road network topology generation, path planning, and motion planning. The overall framework is as Figure 1 shown.

[0085] The road network topology generation module is responsible for screening the road network data using the key-value pairs of tags from the open-source map OSM (Open Street Map). After obtaining the road network structure, following the theoretical framework of graph theory, it converts the road nodes and their connection relationships in the road network information into the data structure form of a topological graph, thereby generating an undirected topological connection graph of the navigation area and providing prior information for the path planning module.

[0086] The path planning module is responsible for generating a feasible local front-end path, which includes two parts: global planning and local planning. The global planning is based on the undirected topological connection graph. After associating the starting point and the target point to the topological graph, it generates a series of global path points for guiding the robot towards the navigation target with the shortest path as the goal. In local planning, a local voxel map is constructed centered on the robot, and the radar perception information is mapped onto the local map to model the discrete perception space. The global path points are used to map the target points for local front-end planning within the robot's perception range. Finally, under the guidance of the global path points, a feasible front-end path considering obstacle information is planned to guide the robot to continuously approach the target point on the main road.

[0087] The motion planning module further optimizes the reference path, considering kinematic and dynamic constraints, and performs smoothing and refinement processing on the initial path. Aiming at the problem of many obstacles and high safety requirements in complex outdoor scenarios, a safety corridor is constructed in a hard-constraint manner to impose additional linear inequality constraints on the backend trajectory optimization. At the same time, in order to leave enough safety margins between the desired trajectory and the obstacles, an innovative method of front-end initial path segment projection-guided optimization is proposed, which considers both the trajectory smoothing quality and the safety factor in the optimization objective to construct a smooth and safe high-quality trajectory. The trajectory tracking module uses the non-linear kinematic model of the robot to track the backend target trajectory, sends the generated speed signal to the chassis, and resolves the speed signal into left and right wheel drive signals according to the inverse kinematic model of the robot, thereby realizing the overall navigation control of the system.

[0088] Refer to Figure 11 shown. The autonomous navigation method for robots in large outdoor scenarios based on open digital maps can specifically include the following steps:

[0089] Step S1, preprocess the open-source digital map. Extract the road network structure from the OSM raw data, construct a road network topology graph, and model the road network space using an R-tree;

[0090] Step S2, associate the starting point and the target point according to the constructed road network topology graph with spatial information, perform global path planning based on the Dijkstra algorithm, and obtain a series of path points from the starting point to the target point;

[0091] Step S3, obtain the lidar sensor point cloud, and generate a local voxel map using the point cloud after filtering the ground points. Combine the global path segment and the local voxel map for local target point mapping, thereby generating local target points. Then, perform local front-end path search based on the JPS algorithm to obtain a series of discrete path points from the current position of the robot to the local target points;

[0092] Step S4, construct a safety corridor according to the local front-end path and perform trajectory optimization. Consider both trajectory smoothness and safety in the optimization objective, thereby optimizing a smooth and safe high-quality trajectory;

[0093] Step S5, track the optimized trajectory, consider the non-linear model and the actual motion state of the robot, and thereby plan a precise and stable speed.

[0094] Next, each step in the example implementation manner will be described in more detail in conjunction with the accompanying drawings and embodiments.

[0095] In step S1, extract the road network structure from the OSM raw data, follow the theoretical framework of graph theory, convert the road nodes and their connection relationships in the road network information into the data structure form of a topology graph, thereby generating an undirected topological connection graph of the navigation area, and fully consider the spatial relationship of the road network vectors. Use a spatial R-tree (Rectangular Tree) to model the road network space. Specifically, it includes the following sub-steps:

[0096] Step 1-1: Select the target area on the OSM official website and export the map file in XML format. In the map file, a series of geographical tag information is included. Among them, the node element represents the landmark nodes of the map, and the "lon" and "lat" parameters are the geographical longitude and latitude coordinates of the landmark nodes respectively.

[0097] Step 1-2: Search for the way element in the XML file and extract the data whose key value of the tag label in its attributes is "highway". These data are the main road data, and the screening of the road network data is completed.

[0098] Step 1-3: Convert the road nodes in the road network information and their connection relationships into the data structure form of a topological graph. Consider the endpoints of each road segment as nodes, denote the node set as N, consider the road segments as edges, denote the edge set as M, set the weight of the edge as the length of the edge, and define the prior map as:

[0099] G = (N, M) (1)

[0100] Through road network screening, the visualization effect of the constructed topological graph is as shown in Figure 3 Figure. The red dots represent the road network nodes, and the black lines represent the road segment edges.

[0101] Step 1-4: Construct the topological map into an R-tree. Establish leaf nodes with each road segment as the minimum unit, and recursively build up. Finally, divide the vector road network with the minimum bounding rectangle and establish the R-tree of the road network in the entire space.

[0102] In step S2, based on the constructed topological graph with spatial information, perform start point and end point association, and conduct global path planning based on the Dijkstra algorithm to obtain a series of path points from the start point to the end point. Specifically, it includes the following sub-steps:

[0103] Step 2-1: Quickly retrieve candidate road segments. Use the road network space segmentation modeling method based on the R-tree structure to quickly retrieve candidate road segments within a certain range of the end point.

[0104] Step 2-2: Traverse the selected candidate road segments, calculate the projection distance from the end point to each road segment, and determine whether the projection point is on the extension line of the road segment. Ignore the road segments projected on the extension line. Finally, set the road segment with the minimum projection distance as the target road segment, and set its projection point as the associated node.

[0105]

[0106] Step 2-3: According to the relationship between the associated node and the two end nodes of the target road segment, as shown in Equation (2), establish a new connection relationship, and update the road network topological graph G = (N, M) to a complete navigation topological graph:

[0107] G = (V, E) (3)

[0108] where, V = N + n s + n t , E = M - e s - e t + e s1 + e s2 + e t1 + e t2 . The association effect is as shown in Figure 4 Figure.

[0109] Step 2-4: Use the Dijkstra shortest path search algorithm to plan a series of global path points (n s , n i , n j , …, n t ) in the navigation topology graph G = (V, E), where n s and n t are the nodes on the topology graph associated with the current point and the navigation target point of the robot respectively. The global planning effect is as shown in Figure 5 .

[0110] In step S3, obtain the lidar sensor point cloud. During the preprocessing process, segment the ground points in the original point cloud, and use the point cloud after filtering the ground points to generate a local voxel map. Combine the global path segment and the local voxel map for local target point mapping, thereby generating local target points. Then, based on the JPS algorithm, perform local front-end path search to obtain a series of discrete path points from the current position of the robot to the local target points. Specifically, it includes the following sub-steps:

[0111] Step 3-1: Obtain the lidar sensor point cloud and use the Linefit algorithm to segment the ground point cloud.

[0112] Step 3-2: Initialize several non-occupied voxels around the robot, and then set the corresponding voxels to occupied according to the positions of the non-ground point clouds, thereby constructing a local voxel map.

[0113] Step 3-3: Determine the local target points by solving the intersection points of the global path and the local voxel map, and the principle is as shown in Figure 6 .

[0114] Step 3-3-1: According to the coordinates of the next target point P Gk-1 and the previous point P Gk , determine the topological straight line equation, which is expressed in parametric equation form as follows:

[0115] r(t) = P Gk + t(P Gk+1 - P Gk ) (4)

[0116] where r(t) is any point on the straight line, t is the straight line parameter, the target point P Gk-1 = (x Gk-1 , y Gk-1 , z Gk-1 ), P Gk = (x Gk , y Gk , z Gk ).

[0117] Step 3-3-2: Assume that the planes where the surrounding perception boundaries of the robot are located are Π1, Π2, Π3, and Π4 respectively. Using the vertex coordinates of the cube, the general equations of each plane can be expressed as:

[0118] A i x + By i + Cz i + D = 0 (5)

[0119] where A i , B i , C i , D i are the coefficients of the plane equation respectively.

[0120] Step 3-3-3: To find the intersection points of the topological line and the perception boundary plane, we substitute the parametric equation of the line into each plane equation, that is:

[0121]

[0122] Solve the equation for the value of t, and we get:

[0123]

[0124] Substitute the obtained value of t i into the line parametric equation, and the coordinates of the intersection point r(t i ) can be obtained.

[0125] Step 3-4: According to the JPS algorithm, quickly plan a local front path p = (p1, p2,..., p n ) that includes both target guidance and is safe and collision-free, so as to provide discrete reference information for the subsequent smoothing and optimization of the trajectory, as Figure 7 shown.

[0126] In step S4, a safety corridor is constructed and the trajectory is optimized. Considering both the smoothness and safety of the trajectory in the optimization objective, a smooth and safe high-quality trajectory is optimized. Specifically, it includes the following sub-steps:

[0127] Step 4-1: Construct a safety corridor based on the path searched by the front end and the local voxel map, and there are no obstacles in the corridor.

[0128] Step 4-2: Design an optimization objective function. Since the inside of the safety corridor is equivalent everywhere, the optimized trajectory will be close to the corridor boundary at the turning points. When the control accuracy cannot be guaranteed, there is still a risk of collision for the robot. Therefore, to balance the smoothness and safety of the trajectory, the present invention defines the following objective function:

[0129]

[0130] where jn represents the smoothing term, J dis is the safety constraint term. The first part of the objective function α·∑||j n || 2 dt aims to minimize the sum of the squares of the snap to ensure the smoothness of the trajectory; the second part (1-α)·J dis is an optimization term related to the distance from the front-end path, which is used to make the optimized trajectory close to the front-end path, thereby reducing the collision risk. α is the weight coefficient between the smoothness term and the safety term, which is used to balance the priorities between these two terms.

[0131] Considering that the discrete space is inflated during the front-end JPS search, the searched path is close to the center of the drivable area and is far from the obstacles on both sides. Therefore, we introduce an optimization term related to the distance from the front-end path in the optimization problem of the existing algorithm to make the optimized trajectory close to the front-end path without changing the range of the back-end solution space. The improvement principle is as Figure 8 shown.

[0132] Define the projection optimization term as:

[0133]

[0134] where r n,j (t) represents the j-th control point at the time interval t, is the point on the line segment and the adjacent line segment where the control point is projected into the polyhedron it belongs to. The formula for calculating the projection point is:

[0135]

[0136] where the line segment endpoints e n+k and e n+k+1 represent the two endpoints of the corresponding line segment in the polyhedron, and r n,j is the control point. The inner product and norm are used to determine the projection ratio of the point r n,j on the corresponding line segment.

[0137] Step 4-3: Construct constraints, including start and end point state constraints, safety corridor constraints, state continuity constraints, and state boundary constraints.

[0138] Step 4-4: Use the Gurobi optimizer to solve for the coefficients of the piecewise polynomial trajectory, and obtain continuous trajectory points by densely sampling on the piecewise trajectory:

[0139] traj=(s1,s2,s3,…)(11)

[0140] where s i contains the pose, velocity, and acceleration information of each trajectory point. The optimized trajectory is asFigure 9 As shown, (a1)(a2)(a3) are the optimized trajectory results of the existing algorithm, and (b1)(b2)(b3) are the optimized trajectory results of the present invention. It can be seen that the trajectory of the algorithm of the present invention is farther from the obstacle and has higher safety.

[0141] In step S5, the optimized trajectory is tracked, and the non-linear model and actual motion state of the robot are considered to plan a precise and stable speed. Specifically, it includes the following sub-steps:

[0142] Step 5-1: Obtain the actual state of the robot, including the position (x, y), speed v, and yaw angle θ.

[0143] Step 5-2: Calculate the lateral error d and the direction offset error η. The lateral error represents the Euclidean distance between the robot's position and the desired trajectory in the y direction, and the direction error represents the difference between the robot's forward direction and the tangential direction of the desired trajectory.

[0144] Step 5-3: Set the discrete non-linear motion equation of the system:

[0145] s t = f(s t-1 , u t-1 )(12)

[0146] where u t-1 = (a t-1 , ω t-1 ), a t-1 represents the control amount of the robot in the forward direction, and ω t-1 is the rotational control amount of the robot.

[0147] Step 5-4: Construct an optimization problem: The specific loss function is as shown in Equation (13), and the constraints are as shown in Equations (14) to (17).

[0148]

[0149] s.t.

[0150] x0 = x(t) (14)

[0151]

[0152] where represents the reference speed; N represents the model prediction step size; λ d , λ v , λ a , λ ω , respectively represent the individual proportion states of lateral offset, angular offset, acceleration control, rotational control, acceleration control change amount, and rotational control change amount in the loss function; xmin and x max and u min and u max respectively represent the upper and lower limits of the state variables and control variables during the optimization process.

[0153] Step 5-5: Use the nonlinear optimizer IPOPT (Interior Point OPTimizer) to solve the above optimization problem. According to the optimization variable u t and the current speed, the desired speed command is calculated as follows:

[0154] v x = v0 + a0 × dt (18)

[0155] ω z = ω0

[0156] where v x and ω z are the linear velocity and angular velocity drive amounts within this period respectively, and a0 and ω0 are the optimized control amounts at the initial moment. The tracking process is as shown in Figure 10 (a) and the drive amounts are as shown in Figure 10 (b).

[0157] It should be noted that, on the other hand, the present application also provides a storage medium, which may be included in an electronic device; or may exist alone without being assembled into the electronic device. The above storage medium carries one or more programs, and when the above one or more programs are executed by an electronic device, the electronic device realizes the method described in the following embodiments.

[0158] In one embodiment, the present application provides a computer program product, including a computer program, which when executed by a processor realizes the steps in the above method embodiments.

[0159] In addition, the above drawings are only schematic illustrations of the processes included in the method according to the exemplary embodiments of the present invention, rather than for limiting purposes. It is easy to understand that the processes shown in the above drawings do not indicate or limit the chronological order of these processes. Additionally, it is also easy to understand that these processes may be executed synchronously or asynchronously in, for example, multiple modules.

[0160] Those skilled in the art will readily think of other embodiments of the present invention after considering the specification and practicing the invention here. The present application aims to cover any variations, uses, or adaptations of the present invention, which follow the general principles of the present invention and include the common general knowledge or conventional technical means in the technical field not disclosed in the present invention. The specification and embodiments are only regarded as exemplary, and the true scope and spirit of the present invention are pointed out by the claims.

[0161] It should be understood that the present invention is not limited to the exact structures described above and shown in the drawings, and various modifications and changes can be made without departing from its scope. The scope of the present invention is only defined by the appended claims.

Claims

1. A robot outdoor large-scale scene autonomous navigation method based on an open digital map, characterized in that, The method includes: Extract the road network structure from the OSM raw data, convert the road nodes in the road network information and their connection relationships into the data structure form of a topological graph, construct a road network topological graph, and model the road network space with an R-tree; Associate the starting point and the target point according to the road network topological graph with the R-tree structure, perform global path planning based on the Dijkstra algorithm, and obtain a series of path points from the starting point to the target point; Obtain the point cloud of the lidar sensor, generate a local voxel map using the point cloud after filtering the ground points; combine the global path segment and the local voxel map for local target point mapping to generate local target points; Perform local front-end path search based on the JPS algorithm to obtain a series of local front-end paths from the current position of the robot to the local target points; Construct a safety corridor according to the local front-end path and perform trajectory optimization; Track the optimized trajectory and plan the speed considering the non-linear model and the actual motion state of the robot.

2. The method according to claim 1, wherein The extraction of the road network structure from the OSM raw data, converting the road nodes in the road network information and their connection relationships into the data structure form of a topological graph, constructing a road network topological graph, and modeling the road network space with an R-tree includes: Select the target area on the OSM official website and export the map file in XML format; Search for the way element in the XML file and extract the data whose key value of the tag label in its attributes is "highway", and these data are the main road data; Convert the road nodes in the road network information and their connection relationships into the data structure form of a topological graph; Construct the topological map into an R-tree, establish leaf nodes with each road section as the smallest unit, and recursively build up. Finally, divide the vector road network with the minimum bounding rectangle and establish the R-tree of the road network over the entire space.

3. The method according to claim 2, wherein The association of the starting point and the target point according to the road network topological graph with the R-tree structure includes: Use the road network space segmentation and modeling method based on the R-tree structure to quickly retrieve candidate road sections within a certain range of the target point; Calculate the projection distance from the target point to each candidate road section and determine whether the projection point is on the extension line of the road section; ignore the road sections projected on the extension line, and finally set the road section with the minimum projection distance as the target road section and set its projection point as the associated node: According to the relationship between the associated node and the two end nodes of the target road section, establish a new connection relationship and update the road network topological graph to a complete navigation topological graph.

4. The method according to claim 3, wherein The acquisition of the lidar sensor point cloud and the generation of a local voxel map using the point cloud after filtering the ground points includes: Obtain the lidar sensor point cloud and use the Linefit algorithm to segment the ground point cloud; Initialize several non-occupied voxels around the robot, and then set the corresponding voxels to occupied according to the positions of the non-ground point clouds to construct a local voxel map.

5. The method according to claim 4, wherein The combination of the global path segment and the local voxel map for local target point mapping to generate local target points includes: Determine the linear parameter equation according to the coordinates of the next target point and the previous point; Substitute the parametric equation of the straight line into the plane equation of the robot's surrounding perception boundary and solve it. Substitute the obtained value into the parametric equation of the straight line to get the coordinates of the intersection point, which is the local target point.

6. The method according to claim 5, wherein Constructing a safety corridor based on the local front path and performing trajectory optimization includes: Constructing a safety corridor jointly based on the path searched by the front end and the local voxel map, where there are no obstacles in the corridor; Designing an optimization objective function: Among them, j n represents the smoothing term, and J dis is the safety constraint term; the first part α·∑||j n || 2 dt of the objective function aims to minimize the sum of the squares of the jerk, and the second part (1-α)·J dis is an optimization term related to the distance of the front-end path, and α is the weight coefficient between the smoothing term and the safety term; Constructing constraints, including start and end point state constraints, safety corridor constraints, state continuity constraints, and state boundary constraints; Using the Gurobi optimizer to solve for the coefficients of the piecewise polynomial trajectory, and obtaining continuous trajectory points by densely sampling on the piecewise trajectory.

7. The method according to claim 6, characterized in that, Tracking the optimized trajectory and planning the speed considering the non-linear model and actual motion state of the robot includes: Obtaining the actual state of the robot, including position, speed, and yaw angle; Calculating the lateral error d and the direction offset error η; Setting the discrete non-linear motion equation of the system: s t = f(s t-1 , u t-1 ) Among them, u t-1 =(a t-1 , ω t-1 ), a t-1 represents the control amount of the robot in the forward direction, and ω t-1 is the rotation control amount of the robot; Constructing an optimization problem, including a loss function and constraint conditions; s.t. x0 = x(t) Among them, represents the reference speed; N represents the model prediction step; λ d , λ v , λ a , λ ω , respectively represent the individual proportion states of lateral offset, angular offset, acceleration control, rotation control, acceleration control change amount, and rotation control change amount in the loss function; x min , x max and u min , u max respectively represent the upper and lower limits of the state variables and control variables during the optimization process; Solve the above optimization problem and calculate the desired velocity command according to the optimization variable u t and the current velocity: v x = v0 + a0 × dt ω z = ω0 where, v x and ω z are respectively the linear velocity and angular velocity driving quantities within this period, and a0 and ω0 are respectively the optimized control quantities at the initial moment.

8. A storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the method for autonomous navigation of a robot in a large outdoor scene based on an open digital map according to any one of claims 1 to 7.

9. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by a processor, it implements the method for autonomous navigation of a robot in a large outdoor scene based on an open digital map according to any one of claims 1 to 7.

10. An electronic device, characterized in that, Including: A processor; And A memory for storing executable instructions of the processor; Wherein, the processor is configured to execute the method for autonomous navigation of a robot in a large outdoor scene based on an open digital map according to any one of claims 1 to 7 by executing the executable instructions.

Images