A path optimization method and system for a mobile robot

Abstract

The present invention relates to the technical field of robot path planning, and in particular to a path optimization method and system for a mobile robot. The present invention proposes the following scheme: first, obstacle modeling is performed on a workspace through multi-source equipment to obtain a three-dimensional model of the obstacle; secondly, a workspace map is constructed according to the three-dimensional model of the obstacle to generate a global path for the mobile robot; finally, the mobile robot moves from a starting point to an end point according to the global path of the mobile robot, and whether there are dynamic obstacles during the movement is determined; if there are dynamic obstacles, the mobile robot determines a local obstacle avoidance path according to a collision avoidance strategy, and bypasses the dynamic obstacle according to the local obstacle avoidance path.

Description

Technical Field

[0001] The present invention relates to the technical field of robot path planning, and in particular to a path optimization method and system for a mobile robot. Background Art

[0002] Path planning is one of the important contents in the field of robotics research, especially path planning in complex environments is a difficult point in the field of robotics research. How to enable the robot to quickly and accurately plan a collision-free, smooth path that meets its motion constraints in a known or unknown environment is a challenging research topic. In various fields that require robots to perform tasks, such as national security, search and rescue in hazardous environments, and medical services, path planning and optimization have great potential application value. The main problem at this stage is that in complex environments, the curvature of the path planned by the traditional path planning algorithm is too large, and the non-smooth path will increase the complexity of the path tracking algorithm of the mobile robot with incomplete constraints, making it difficult to be stably implemented by the robot.

[0003] For example, the patent with publication number CN115167474A discloses a mobile robot path planning optimization method, the steps are as follows: obtain a static grid map, each node of the grid map represents the current position as an obstacle or a passable area, and determine the starting point and the end point; add node distance information to the cost function of the traditional path planning algorithm, and perform global path planning of the improved path planning algorithm on the grid map; use the key point extraction strategy to extract key points from the above global path to obtain a set containing only the starting point, the end point and the key points; connect the nodes in the above set in order to obtain a smooth global path. This invention improves the efficiency of path search by introducing node distance information; extracts key points from the obtained global path to obtain a global path that is more in line with the actual movement of the robot, and achieves significant improvements in path search time, number of inflection points, number of traversed nodes and path length.

[0004] The above patents all have the problems raised by this background technology: the working environment of the robot is complex and changeable. There are not only static obstacles, but also some dynamic obstacles with unknown operating status. Traditional path planning algorithms have shortcomings such as low efficiency, poor stability and adaptability when facing complex environments, and it is difficult to meet actual needs. At present, real-time surveys of the robot's working environment are carried out based on single-view modeling algorithms, which are mainly based on voxel expressions. The modeling quality is low, information is seriously missing, and the scene details cannot be accurately portrayed, which makes it difficult to meet the requirements of robot interaction. In order to solve the above problems, this application designs a path optimization method and system for a mobile robot. Summary of the invention

[0005] The technical problem to be solved by the present invention is to address the deficiencies in the prior art and provide a path optimization method and system for a mobile robot. First, obstacle modeling is performed on the workspace through multi-source devices to obtain a three-dimensional model of the obstacle. Secondly, a workspace map is constructed according to the three-dimensional model of the obstacle to generate a global path for the mobile robot. Finally, the mobile robot moves from a starting point to an end point according to the global path of the mobile robot. During the movement, it is determined whether there are dynamic obstacles. If there are dynamic obstacles, the mobile robot determines a local obstacle avoidance path according to a collision avoidance strategy and bypasses the dynamic obstacles according to the local obstacle avoidance path.

[0006] To achieve the above object, the present invention provides the following technical solutions:

[0007] A path optimization method for a mobile robot comprises the following steps:

[0008] S1: Determine the working space of the mobile robot according to the starting and ending positions of the mobile robot, perform obstacle modeling on the working space through multi-source equipment, and obtain a three-dimensional model of the obstacles;

[0009] S2: Construct a workspace map based on the three-dimensional model of obstacles and generate a global path for the mobile robot based on the global path selection strategy;

[0010] S3: The mobile robot moves from the starting point to the end point according to the global path of the mobile robot, and senses whether there are dynamic obstacles through the sensor component;

[0011] S4: If there is a dynamic obstacle, the mobile robot determines a local obstacle avoidance path according to the collision avoidance strategy, bypasses the dynamic obstacle according to the local obstacle avoidance path, and repeats steps S3 and S4 until the mobile robot reaches the end position;

[0012] The multi-source device includes a laser radar and a drone tilt camera. The specific steps of performing obstacle modeling on the workspace through the multi-source device are as follows:

[0013] S1.1: Scan the workspace of the mobile robot by using a laser radar to obtain a three-dimensional point cloud dataset of the workspace, and extract point cloud features of suspected obstacles based on the three-dimensional point cloud dataset;

[0014] S1.2: Jointly calibrate the laser radar and the UAV tilt camera, obtain the UAV tilt photography image of the coordinate position of the suspected obstacle point cloud feature through the UAV tilt camera, and obtain the suspected obstacle entity features based on the UAV tilt photography image;

[0015] S1.3: constructing a feature fusion network, taking the suspected obstacle point cloud features and the suspected obstacle entity features as input parameters of the feature fusion network, training the input parameters through the feature fusion network, and outputting obstacle fusion point cloud data;

[0016] S1.4: According to the obstacle fusion point cloud data, import it into the point cloud processing software for processing, and output the three-dimensional model of the obstacle in the workspace;

[0017] The specific steps of extracting the point cloud features of suspected obstacles based on the three-dimensional point cloud data set are as follows:

[0018] S1.1.1: Preprocess the three-dimensional point cloud data in the three-dimensional point cloud data set, wherein the preprocessing includes three-dimensional point cloud data compression, three-dimensional point cloud data noise reduction and three-dimensional point cloud data splicing;

[0019] S1.1.2: Randomly sample the preprocessed 3D point cloud data through random consistency sampling to obtain the observed point cloud data, calculate the neighborhood points of the observed point cloud data according to the Lagrange multiplier method, make discrete judgments on the observed point cloud data and the neighborhood points, and calculate the discrete degree of the observed point cloud data. The calculation formula for the discrete degree of the observed point cloud data is:

[0020]

[0021] Among them, D o represents the discrete degree of the observed point cloud data, A represents the covariance matrix of the observed point cloud data, n represents the normal vector of the observed point cloud data, cos -1 (·) represents the arccosine function, j represents a single neighborhood point of the observed point cloud data, M represents the total number of neighborhood points of the observed point cloud data, and n j represents the normal vector of the jth neighborhood point of the observed point cloud data, max(·) represents the maximum value function, r j Represents the Euclidean distance between the observed point cloud data and the jth neighboring point of the observed point cloud data;

[0022] S1.1.3: Traverse the 3D point cloud data set, obtain the discrete degree of all observed point cloud data, and compare the discrete degree with the ground point cloud threshold. If it is less than or equal to the ground point cloud threshold, delete the observed point cloud data and eliminate useless ground point clouds;

[0023] S1.1.4: Perform coplanarity judgment on the remaining observation point cloud data, merge the observation point cloud data that meet the coplanarity conditions and allocate point set space to obtain the point cloud features of suspected obstacles.

[0024] The specific steps of obtaining the entity features of suspected obstacles based on the oblique photography images of the drone are as follows:

[0025] S1.2.1: Preprocess the UAV oblique photography images, where the preprocessing includes regional network joint adjustment and multi-view image matching;

[0026] S1.2.2: Based on the pre-processed UAV oblique photography image, traverse the pixel points of the UAV oblique photography image, calculate the rigid transformation vector and the change amount in the iterative process of two adjacent pixel points, and calculate the entity feature value of the pixel point based on the dynamic adjustment factor, the rigid transformation vector and the change amount;

[0027] S1.2.3: Compare the entity feature value of the pixel with the entity threshold. If it is greater than the entity threshold, mark the pixel as a sampling point and confirm the neighborhood of the sampling point based on the local shape distribution function;

[0028] S1.2.4: The feature description vector of the sampling point is calculated based on the normal vector angle between the sampling point and the pixel points in the neighborhood of the sampling point. The calculation formula of the feature description vector of the sampling point is:

[0029]

[0030] Among them, V f represents the sampling point feature description vector, T c represents the entity feature value of the sampling point, i represents a single pixel in the neighborhood of the sampling point, N represents the total number of pixels in the neighborhood of the sampling point, and θ i Represents the normal vector angle between the sampling point and the i-th pixel in the sampling point neighborhood, C i represents the curvature of the i-th pixel in the neighborhood of the sampling point, 6 represents the normalization factor, e represents the exponential function, a and b represent the horizontal and vertical matrix coordinates of the sampling point, a i and b i represents the horizontal and vertical matrix coordinates of the i-th pixel in the neighborhood of the sampling point, and c represents the convolution factor;

[0031] S1.2.5: Aggregate the feature description vectors through the maximum pooling operation, and then further abstract the aggregated features based on sparse convolution, and obtain the entity features of suspected obstacles through the RPN network;

[0032] The specific steps of the global path selection strategy are as follows:

[0033] S2.1: Initialize the map state space and define the starting point as the root node P init , the end point is defined as the destination node Pgoat, and it is determined whether the root node is near the destination node. If it is near the destination node and there are no obstacles between the two nodes, the global path of the mobile robot is directly generated according to the straight line. If it is not near the destination node or there are obstacles in the middle, the root node is used as the starting point as the initial global path;

[0034] S2.2: Set a random threshold p and generate a random step size q. Determine the size of the random step size and the random threshold. If the random step size is less than or equal to the random threshold, based on the direction of the connection between the root node and the destination node, set the node with a distance of q from the root node as the random node P. rand ;

[0035] S2.3: With the line connecting the random node and the root node as the direction, generate an intermediate node group at a fixed step length, traverse the intermediate node group, and determine whether there is an obstacle. If there is no obstacle, add the intermediate node to the initial global path. If there is an obstacle, repeat S2.2 and S2.3 until no intermediate node group can be generated;

[0036] S2.4: Determine whether there is a random node in the initial global path. If there is no random node, take the latest intermediate node of the initial global path as the root node, find the best direction of the root node through the ant colony algorithm, and determine the next root node according to the best direction. If there is a random node, take the random node as the root node, and repeat S2.2 to S2.4 until P init =P goat , and denoise the initial global path to generate a reference global path;

[0037] S2.5: Repeat S2.2 to S2.4 until ten sets of reference global paths are obtained, the path cost values ​​of the reference global paths are calculated, and the reference global path corresponding to the smallest path cost value is selected as the global path of the movable robot.

[0038] The calculation formula of the path cost value of the reference global path is:

[0039] S v =ω d E d +ω w E w +ω h E h +ω f E f ,

[0040] Among them, S v represents the path cost value of the reference global path, E d represents the mobile robot's moving energy consumption value, ω d represents the mobile robot's moving energy consumption weight, E w represents the turning energy consumption of the mobile robot, ω w represents the turning energy consumption weight of the mobile robot, E h represents the energy consumption of the mobile robot climbing, ω h represents the energy consumption weight of the mobile robot climbing, E f represents the friction energy consumption of the mobile robot, ωf Represents the friction energy consumption weight of the mobile robot.

[0041] The collision avoidance strategy includes a straight-line motion avoidance strategy and an irregular motion avoidance strategy;

[0042] The specific steps of the linear motion avoidance strategy are as follows:

[0043] S4.1.1: The mobile robot predicts the motion trajectory of the dynamic obstacle based on the motion direction and speed of the dynamic obstacle transmitted by the sensor component, and calculates the collision node and collision direction in combination with the moving speed of the mobile robot, where the collision direction includes frontal collision and side collision;

[0044] S4.1.2: If the collision direction is a side collision, the mobile robot remains in a waiting state before the collision node, and calculates the waiting time according to the length and speed of the dynamic obstacle. After the waiting time, the mobile robot continues to move along the global path of the mobile robot;

[0045] S4.1.3: If the collision direction is a frontal collision, the collision node is regarded as a static obstacle, the node closest to the static obstacle in the global path of the mobile robot is regarded as the target node, the current position of the mobile robot is regarded as the root node, and a temporary path is selected through the local planning algorithm. The mobile robot moves according to the temporary path, and determines whether a collision will occur through the sensor component during the process. If a collision occurs, repeat S4.1.1 to S4.1.3 until the target node is reached;

[0046] The specific steps of the irregular motion avoidance strategy are as follows:

[0047] S4.2.1: The motion trajectory of the dynamic obstacle is sampled by the rolling window method, and the motion trajectory with the highest evaluation function value is determined as the moving path of the dynamic obstacle according to the evaluation function;

[0048] S4.2.2: Compare the dynamic obstacle movement path with the global path of the mobile robot, treat the overlapping nodes as static obstacles, use the current position of the mobile robot as the root node, and define the end point as the destination node. Replan a local obstacle avoidance path and replace the current global path of the mobile robot.

[0049] A path optimization system for a mobile robot, the system comprising a workspace modeling module, a global path planning module and a local obstacle avoidance planning module;

[0050] The workspace modeling module is used to determine the workspace of the mobile robot according to the starting and ending positions of the mobile robot, perform obstacle modeling on the workspace through multi-source equipment, and obtain a three-dimensional model of the obstacles;

[0051] The global path planning module is used to construct a workspace map according to the three-dimensional model of obstacles, and select a reference global path corresponding to the minimum path cost value as the global path of the mobile robot;

[0052] The local obstacle avoidance planning module is used to sense whether there are dynamic obstacles through the sensor component during the movement of the mobile robot. If there are dynamic obstacles, the mobile robot determines a local obstacle avoidance path according to the collision avoidance strategy and bypasses the dynamic obstacles according to the local obstacle avoidance path;

[0053] The workspace modeling module includes:

[0054] A suspected obstacle point cloud feature extraction unit is used to scan the working space of the mobile robot through a laser radar, obtain a three-dimensional point cloud data set of the working space, and extract the suspected obstacle point cloud features according to the three-dimensional point cloud data set;

[0055] The suspected obstacle entity feature extraction unit is used to obtain the UAV oblique photography image of the coordinate position of the suspected obstacle point cloud feature through the UAV oblique camera, and obtain the suspected obstacle entity feature according to the UAV oblique photography image;

[0056] A feature fusion unit is used to construct a feature fusion network, take the suspected obstacle point cloud features and the suspected obstacle entity features as input parameters of the feature fusion network, train the input parameters through the feature fusion network, and output obstacle fusion point cloud data;

[0057] The obstacle model building unit is used to fuse point cloud data according to obstacles, import them into point cloud processing software for processing, and output a three-dimensional model of obstacles in the workspace;

[0058] The local obstacle avoidance planning module includes:

[0059] The linear motion avoidance unit is used for the mobile robot to predict the motion trajectory of the dynamic obstacle according to the motion direction and speed of the dynamic obstacle sent back by the sensor component, calculate the collision node and collision direction in combination with the moving speed of the mobile robot, and determine the collision avoidance strategy;

[0060] The irregular motion avoidance unit is used to sample the motion trajectory of the dynamic obstacle through the rolling window method, determine the motion trajectory with the highest evaluation function value as the dynamic obstacle moving path according to the evaluation function, re-plan a local obstacle avoidance path, and replace the current global path of the movable robot.

[0061] Compared with the prior art, the present invention has the following beneficial effects:

[0062] 1. The present invention uses multi-source information to perform three-dimensional modeling of the mobile robot's workspace, obtains obstacle information in the workspace, achieves high-precision scene reproduction, and improves the environmental perception ability and intelligence level of existing robots;

[0063] 2. Aiming at the needs and characteristics of path planning in dynamic environments, the present invention proposes a two-stage dynamic path planning method. In the first stage, a global optimal path is planned based on global environmental information, and the path is used as a reference path for the second stage. The second stage is the path tracking and local replanning stage. When the robot walks along the optimal path, the local map within its field of view is updated in real time to perform collision prediction and collision avoidance. Path tracking and obstacle avoidance are completed according to the collision avoidance strategy, and the path trajectory of the mobile robot is optimized to improve efficiency and accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:

[0065] Figure 1 This is a schematic flow chart of a path optimization method for a mobile robot according to Embodiment 1 of the present invention;

[0066] Figure 2 This is a feature fusion network structure diagram of Example 1 of the present invention;

[0067] Figure 3 This is a flow chart of the global path selection strategy in Example 1 of the present invention;

[0068] Figure 4 This is a module diagram of a path optimization system for a mobile robot according to Example 2 of the present invention. DETAILED DESCRIPTION

[0069] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments.

[0070] Embodiment 1:

[0071] See also Figure 1 , an embodiment of the present invention provides: a path optimization method for a mobile robot, comprising the following steps:

[0072] S1: Determine the working space of the mobile robot according to the starting and ending positions of the mobile robot, perform obstacle modeling on the working space through multi-source equipment, and obtain a three-dimensional model of the obstacles;

[0073] S2: Construct a workspace map based on the three-dimensional model of obstacles and generate a global path for the mobile robot based on the global path selection strategy;

[0074] S3: The mobile robot moves from the starting point to the end point according to the global path of the mobile robot, and senses whether there are dynamic obstacles through the sensor component;

[0075] S4: If there is a dynamic obstacle, the mobile robot determines a local obstacle avoidance path according to the collision avoidance strategy, bypasses the dynamic obstacle according to the local obstacle avoidance path, and repeats steps S3 and S4 until the mobile robot reaches the end position;

[0076] The multi-source device includes a laser radar and a drone tilt camera. The specific steps of performing obstacle modeling on the workspace through the multi-source device are as follows:

[0077] S1.1: Scan the workspace of the mobile robot by using a laser radar to obtain a three-dimensional point cloud dataset of the workspace, and extract point cloud features of suspected obstacles based on the three-dimensional point cloud dataset;

[0078] S1.2: Jointly calibrate the laser radar and the UAV tilt camera, obtain the UAV tilt photography image of the coordinate position of the suspected obstacle point cloud feature through the UAV tilt camera, and obtain the suspected obstacle entity features based on the UAV tilt photography image;

[0079] S1.3: constructing a feature fusion network, taking the suspected obstacle point cloud features and the suspected obstacle entity features as input parameters of the feature fusion network, training the input parameters through the feature fusion network, and outputting obstacle fusion point cloud data;

[0080] S1.4: According to the obstacle fusion point cloud data, import it into the point cloud processing software for processing, and output the three-dimensional model of the obstacle in the workspace;

[0081] The joint calibration of the laser radar and the tilt camera of the unmanned aerial vehicle includes the calibration of the camera internal parameters and the solution of the coordinate transformation matrix. The camera internal parameter calibration obtains the image of each frame by shooting the calibration plate, and then identifies the outer corner points of the black and white chess squares in the image to obtain the position of the chessboard. Finally, the inner corner points are extracted according to the size of the chessboard and combined with the homography matrix to obtain the camera's internal parameters. The solution of the coordinate transformation matrix is ​​used to solve the conversion relationship between the laser radar coordinate system and the camera coordinate system according to the camera internal parameters and the laser radar internal parameters;

[0082] See also Figure 2 , a structural diagram of a feature fusion network according to an embodiment of the present invention, wherein the feature fusion network comprises an input layer, a feature extraction layer and a feature fusion layer;

[0083] The input layer is used to establish a feature sequence according to the input parameters, wherein the feature sequence includes a suspected obstacle laser point cloud feature sequence and a suspected obstacle entity point cloud feature sequence;

[0084] The feature extraction layer is used to extract feature vectors of feature sequences. The feature extraction layer includes a geometric feature extraction module and a spatial feature extraction module. The feature vectors include spatial features and geometric features.

[0085] The feature fusion layer is used to fuse spatial features and geometric features to generate obstacle fusion point cloud data;

[0086] The input layer is used to accumulate input parameters, establish a grey differential equation, calculate the least squares parameters of the grey differential equation, calculate the fitting value of the feature sequence by discretizing the least squares parameter solution, and obtain the suspected obstacle laser point cloud feature sequence and the suspected obstacle entity point cloud feature sequence;

[0087] The feature extraction layer includes a geometric feature extraction module and a spatial feature extraction module;

[0088] The geometric feature extraction module is used to obtain the center point position of the suspected obstacle laser point cloud feature sequence and the suspected obstacle entity point cloud feature sequence through the attention mechanism, calculate the neighboring points of the center point according to the K nearest neighbor algorithm, use the center point and the neighboring points as the source of the Gaussian kernel function, output the local geometric features of the center point through the Gaussian kernel function, and finally map all local geometric features to a high-dimensional space through a residual connection network, perform a pooling operation, and output the geometric features;

[0089] The spatial feature extraction module includes a multi-layer perceptron network, each layer first performs symmetrical operations on the suspected obstacle laser point cloud feature sequence and the suspected obstacle entity point cloud feature sequence through average pooling, then spatially describes the suspected obstacle laser point cloud feature sequence and the suspected obstacle entity point cloud feature sequence through one-dimensional convolution and normalization, and finally activates according to the ReLU activation function to obtain spatial features;

[0090] The feature fusion layer maps the spatial features and geometric features into a tuple sequence [(Q1, K1), (Q2, K2), ..., (Q m , K m )], where Q m represents the corresponding input of the mth feature in the geometric feature, K mRepresent the corresponding input of the mth feature in the spatial feature, perform dot product on the corresponding input features in each candidate sequence in the tuple sequence, calculate the correlation score after the dot product of the candidate sequence by the Pearson correlation coefficient, compress the correlation score according to the sigmoid function, and compare it with the correlation threshold. If it is greater than the correlation threshold, retain the candidate sequence. If it is less than or equal to the correlation threshold, filter the candidate sequence, collect the local features of the candidate sequence retained in the tuple sequence, and enhance them. Splice the enhanced candidate sequence along the head sequence of the tuple sequence, fuse the local features through residual connection, and perform pooling operation on the feature vector after residual connection fusion to obtain obstacle fusion point cloud data;

[0091] The specific steps of extracting the point cloud features of suspected obstacles based on the three-dimensional point cloud data set are as follows:

[0092] S1.1.1: Preprocess the three-dimensional point cloud data in the three-dimensional point cloud data set, wherein the preprocessing includes three-dimensional point cloud data compression, three-dimensional point cloud data noise reduction and three-dimensional point cloud data splicing;

[0093] S1.1.2: Randomly sample the preprocessed 3D point cloud data through random consistency sampling to obtain the observed point cloud data, calculate the neighborhood points of the observed point cloud data according to the Lagrange multiplier method, make discrete judgments on the observed point cloud data and the neighborhood points, and calculate the discrete degree of the observed point cloud data. The calculation formula for the discrete degree of the observed point cloud data is:

[0094]

[0095] Among them, D o represents the discrete degree of the observed point cloud data, A represents the covariance matrix of the observed point cloud data, n represents the normal vector of the observed point cloud data, cos -1 (·) represents the arccosine function, j represents a single neighborhood point of the observed point cloud data, M represents the total number of neighborhood points of the observed point cloud data, and n j represents the normal vector of the jth neighborhood point of the observed point cloud data, max(·) represents the maximum value function, r j Represents the Euclidean distance between the observed point cloud data and the jth neighboring point of the observed point cloud data;

[0096] S1.1.3: traverse the three-dimensional point cloud data set, obtain the discrete degree of all observed point cloud data, and compare the discrete degree with the ground point cloud threshold. If the discrete degree is less than or equal to the ground point cloud threshold, delete the observed point cloud data and eliminate useless ground point clouds. The ground point cloud threshold is determined by those skilled in the art based on a large number of experiments;

[0097] S1.1.4: Perform coplanarity judgment on the remaining observation point cloud data, merge the observation point cloud data that meet the coplanarity condition and allocate the point set space to obtain the point cloud features of suspected obstacles;

[0098] The specific steps of obtaining the entity features of suspected obstacles based on the oblique photography images of the drone are as follows:

[0099] S1.2.1: Preprocess the UAV oblique photography images, where the preprocessing includes regional network joint adjustment and multi-view image matching;

[0100] S1.2.2: Based on the pre-processed UAV oblique photography image, traverse the pixel points of the UAV oblique photography image, calculate the rigid transformation vector and the change amount in the iterative process of two adjacent pixel points, and calculate the entity feature value of the pixel point based on the dynamic adjustment factor, the rigid transformation vector and the change amount;

[0101] S1.2.3: Compare the entity feature value of the pixel point with the entity threshold. If it is greater than the entity threshold, mark the pixel point as a sampling point, and confirm the neighborhood of the sampling point according to the local shape distribution function. The entity threshold is determined by a person skilled in the art based on a large number of experiments;

[0102] S1.2.4: The feature description vector of the sampling point is calculated based on the normal vector angle between the sampling point and the pixel points in the neighborhood of the sampling point. The calculation formula of the feature description vector of the sampling point is:

[0103]

[0104] Among them, V f represents the sampling point feature description vector, T c represents the entity feature value of the sampling point, i represents a single pixel in the neighborhood of the sampling point, N represents the total number of pixels in the neighborhood of the sampling point, and θ i Represents the normal vector angle between the sampling point and the i-th pixel in the sampling point neighborhood, C i represents the curvature of the i-th pixel in the neighborhood of the sampling point, 6 represents the normalization factor, e represents the exponential function, a and b represent the horizontal and vertical matrix coordinates of the sampling point, a i and b i represents the horizontal and vertical matrix coordinates of the i-th pixel in the neighborhood of the sampling point, and c represents the convolution factor;

[0105] S1.2.5: Aggregate the feature description vectors through the maximum pooling operation, and then further abstract the aggregated features based on sparse convolution, and obtain the entity features of suspected obstacles through the RPN network;

[0106] See also Figure 3 , a flow chart of the global path selection strategy according to an embodiment of the present invention, wherein the specific steps of the global path selection strategy are as follows:

[0107] S2.1: Initialize the map state space and define the starting point as the root node P init , the end point is defined as the destination node P goat , and determine whether the root node is near the destination node. If it is near the destination node and there are no obstacles between the two nodes, the global path of the mobile robot is directly generated according to the straight line. If it is not near the destination node or there are obstacles in the middle, the root node is used as the starting point as the initial global path;

[0108] S2.2: Set a random threshold p and generate a random step size q. Determine the size of the random step size and the random threshold. If the random step size is less than or equal to the random threshold, based on the direction of the connection between the root node and the destination node, set the node with a distance of q from the root node as the random node P. rand ;

[0109] S2.3: With the line connecting the random node and the root node as the direction, generate an intermediate node group at a fixed step length, traverse the intermediate node group, and determine whether there is an obstacle. If there is no obstacle, add the intermediate node to the initial global path. If there is an obstacle, repeat S2.2 and S2.3 until no intermediate node group can be generated;

[0110] S2.4: Determine whether there is a random node in the initial global path. If there is no random node, take the latest intermediate node of the initial global path as the root node, find the best direction of the root node through the ant colony algorithm, and determine the next root node according to the best direction. If there is a random node, take the random node as the root node, and repeat S2.2 to S2.4 until P init =P goat , and denoise the initial global path to generate a reference global path;

[0111] S2.5: Repeat S2.2 to S2.4 until ten sets of reference global paths are obtained, the path cost values ​​of the reference global paths are calculated, and the reference global path corresponding to the smallest path cost value is selected as the global path of the movable robot.

[0112] The calculation formula of the path cost value of the reference global path is:

[0113] S v =ω d E d +ω w E w +ω h E h +ω f E f ,

[0114] Among them, S vrepresents the path cost value of the reference global path, E d represents the mobile robot's moving energy consumption value, ω d represents the mobile robot's moving energy consumption weight, E w represents the turning energy consumption of the mobile robot, ω w represents the turning energy consumption weight of the mobile robot, E h represents the energy consumption of the mobile robot climbing, ω h represents the energy consumption weight of the mobile robot climbing, E f represents the friction energy consumption of the mobile robot, ω f Represents the friction energy consumption weight of the mobile robot.

[0115] The collision avoidance strategy includes a straight-line motion avoidance strategy and an irregular motion avoidance strategy;

[0116] The specific steps of the linear motion avoidance strategy are as follows:

[0117] S4.1.1: The mobile robot predicts the motion trajectory of the dynamic obstacle based on the motion direction and speed of the dynamic obstacle transmitted by the sensor component, and calculates the collision node and collision direction in combination with the moving speed of the mobile robot, where the collision direction includes frontal collision and side collision;

[0118] S4.1.2: If the collision direction is a side collision, the mobile robot remains in a waiting state before the collision node, and calculates the waiting time according to the length and speed of the dynamic obstacle. After the waiting time, the mobile robot continues to move along the global path of the mobile robot;

[0119] S4.1.3: If the collision direction is a frontal collision, the collision node is regarded as a static obstacle, the node closest to the static obstacle in the global path of the mobile robot is regarded as the target node, the current position of the mobile robot is regarded as the root node, and a temporary path is selected through the local planning algorithm. The mobile robot moves according to the temporary path, and determines whether a collision will occur through the sensor component during the process. If a collision occurs, repeat S4.1.1 to S4.1.3 until the target node is reached;

[0120] The specific steps of the irregular motion avoidance strategy are as follows:

[0121] S4.2.1: The motion trajectory of the dynamic obstacle is sampled by the rolling window method, and the motion trajectory with the highest evaluation function value is determined as the moving path of the dynamic obstacle according to the evaluation function;

[0122] S4.2.2: Compare the dynamic obstacle movement path with the global path of the mobile robot, treat the overlapping nodes as static obstacles, use the current position of the mobile robot as the root node, and define the end point as the destination node. Replan a local obstacle avoidance path and replace the current global path of the mobile robot.

[0123] Embodiment 2:

[0124] See also Figure 4 , an embodiment provided by the present invention: a path optimization system for a mobile robot, the system comprising a workspace modeling module, a global path planning module and a local obstacle avoidance planning module;

[0125] The workspace modeling module is used to determine the workspace of the mobile robot according to the starting and ending positions of the mobile robot, perform obstacle modeling on the workspace through multi-source equipment, and obtain a three-dimensional model of the obstacles;

[0126] The global path planning module is used to construct a workspace map according to the three-dimensional model of obstacles, and select a reference global path corresponding to the minimum path cost value as the global path of the mobile robot;

[0127] The local obstacle avoidance planning module is used to sense whether there are dynamic obstacles through the sensor component during the movement of the mobile robot. If there are dynamic obstacles, the mobile robot determines a local obstacle avoidance path according to the collision avoidance strategy and bypasses the dynamic obstacles according to the local obstacle avoidance path;

[0128] The workspace modeling module includes:

[0129] A suspected obstacle point cloud feature extraction unit is used to scan the working space of the mobile robot through a laser radar, obtain a three-dimensional point cloud data set of the working space, and extract the suspected obstacle point cloud features according to the three-dimensional point cloud data set;

[0130] The suspected obstacle entity feature extraction unit is used to obtain the UAV oblique photography image of the coordinate position of the suspected obstacle point cloud feature through the UAV oblique camera, and obtain the suspected obstacle entity feature according to the UAV oblique photography image;

[0131] A feature fusion unit is used to construct a feature fusion network, take the suspected obstacle point cloud features and the suspected obstacle entity features as input parameters of the feature fusion network, train the input parameters through the feature fusion network, and output obstacle fusion point cloud data;

[0132] The obstacle model building unit is used to fuse point cloud data according to obstacles, import them into point cloud processing software for processing, and output a three-dimensional model of obstacles in the workspace;

[0133] The local obstacle avoidance planning module includes:

[0134] The linear motion avoidance unit is used for the mobile robot to predict the motion trajectory of the dynamic obstacle according to the motion direction and speed of the dynamic obstacle sent back by the sensor component, calculate the collision node and collision direction in combination with the moving speed of the mobile robot, and determine the collision avoidance strategy;

[0135] The irregular motion avoidance unit is used to sample the motion trajectory of the dynamic obstacle through the rolling window method, determine the motion trajectory with the highest evaluation function value as the dynamic obstacle moving path according to the evaluation function, re-plan a local obstacle avoidance path, and replace the current global path of the movable robot.

[0136] Although the embodiments of the present invention have been shown and described above, it is to be understood that the above embodiments are exemplary and are not to be construed as limitations of the present invention. A person skilled in the art may change, modify, replace and vary the above embodiments within the scope of the present invention.

Claims

1. A path optimization method for a mobile robot, characterized in that: The following steps are involved: S1: Determine the working space of the mobile robot according to the starting and ending positions of the mobile robot, perform obstacle modeling on the working space through multi-source equipment, and obtain a three-dimensional model of the obstacles; S2: Construct a workspace map based on the three-dimensional model of obstacles and generate a global path for the mobile robot based on the global path selection strategy; S3: The mobile robot moves from the starting point to the end point according to the global path of the mobile robot, and senses whether there are dynamic obstacles through the sensor component; S4: If there is a dynamic obstacle, the mobile robot determines a local obstacle avoidance path according to the collision avoidance strategy, bypasses the dynamic obstacle according to the local obstacle avoidance path, and repeats steps S3 and S4 until the mobile robot reaches the end position; The specific steps of the global path selection strategy are as follows: S2.1: Initialize the map state space and define the starting point as the global root node P init , the end point is defined as the global destination node P goat , and determine whether the global root node is near the global destination node. If it is near the global destination node and there are no obstacles between the two nodes, directly generate the global path of the mobile robot based on the straight line. If it is not near the global destination node or there are obstacles in the middle, use the global root node as the starting point as the initial global path; S2.2: Set a random threshold p and generate a random step size q. Determine the size of the random step size and the random threshold. If the random step size is less than or equal to the random threshold, based on the connection direction between the global root node and the global destination node, set the node with a distance of q from the global root node as the random node P. rand ; S2.3: With the line connecting the random node and the global root node as the direction, generate an intermediate node group at a fixed step length, traverse the intermediate node group, and determine whether there is an obstacle. If there is no obstacle, add the intermediate node to the initial global path. If there is an obstacle, repeat S2.2 and S2.3 until no intermediate node group can be generated; S2.4: Determine whether there is a random node in the initial global path. If there is no random node, take the latest intermediate node of the initial global path as the global root node, find the best direction of the global root node through the ant colony algorithm, and determine the next global root node according to the best direction. If there is a random node, take the random node as the global root node, and repeat S2.2 to S2.4 until P init =P goat , and denoise the initial global path to generate a reference global path; S2.5: Repeat S2.2 to S2.4 until ten sets of reference global paths are obtained, the path cost values ​​of the reference global paths are calculated, and the reference global path corresponding to the smallest path cost value is selected as the global path of the movable robot; The calculation formula of the path cost value of the reference global path is: S v =ω d E d +oh w E w +oh h E h +oh f E f , Among them, S v represents the path cost value of the reference global path, E d represents the mobile robot's moving energy consumption value, ω d represents the mobile robot's moving energy consumption weight, E w represents the turning energy consumption of the mobile robot, ω w represents the turning energy consumption weight of the mobile robot, E h represents the energy consumption of the mobile robot climbing, ω h represents the energy consumption weight of the mobile robot climbing, E f represents the friction energy consumption of the mobile robot, ω f Represents the friction energy consumption weight of the mobile robot.

2. A method for optimizing the path of a mobile robot according to claim 1, characterized in that: The multi-source device includes a laser radar and a drone tilt camera. The specific steps of performing obstacle modeling on the workspace through the multi-source device are as follows: S1.1: Scan the workspace of the mobile robot by using a laser radar to obtain a three-dimensional point cloud dataset of the workspace, and extract point cloud features of suspected obstacles based on the three-dimensional point cloud dataset; S1.2: Jointly calibrate the laser radar and the UAV tilt camera, obtain the UAV tilt photography image of the coordinate position of the suspected obstacle point cloud feature through the UAV tilt camera, and obtain the suspected obstacle entity features based on the UAV tilt photography image; S1.3: constructing a feature fusion network, taking the suspected obstacle point cloud features and the suspected obstacle entity features as input parameters of the feature fusion network, training the input parameters through the feature fusion network, and outputting obstacle fusion point cloud data; S1.4: According to the obstacle fusion point cloud data, import it into the point cloud processing software for processing, and output the three-dimensional model of the obstacle in the workspace.

3. A method for optimizing the path of a mobile robot according to claim 2, characterized in that: The specific steps of extracting the point cloud features of suspected obstacles based on the three-dimensional point cloud data set are as follows: S1.1.1: Preprocess the three-dimensional point cloud data in the three-dimensional point cloud data set, wherein the preprocessing includes three-dimensional point cloud data compression, three-dimensional point cloud data noise reduction and three-dimensional point cloud data splicing; S1.1.2: Randomly sample the preprocessed 3D point cloud data through random consistency sampling to obtain the observed point cloud data, calculate the neighborhood points of the observed point cloud data according to the Lagrange multiplier method, make discrete judgments on the observed point cloud data and the neighborhood points, and calculate the discrete degree of the observed point cloud data. The calculation formula for the discrete degree of the observed point cloud data is: Among them, D o represents the discrete degree of the observed point cloud data, A represents the covariance matrix of the observed point cloud data, n represents the normal vector of the observed point cloud data, cos -1 (·) represents the arccosine function, j represents a single neighborhood point of the observed point cloud data, M represents the total number of neighborhood points of the observed point cloud data, and n j represents the normal vector of the jth neighborhood point of the observed point cloud data, max(·) represents the maximum value function, r j Represents the Euclidean distance between the observed point cloud data and the jth neighboring point of the observed point cloud data; S1.1.3: Traverse the 3D point cloud data set, obtain the discrete degree of all observed point cloud data, and compare the discrete degree with the ground point cloud threshold. If it is less than or equal to the ground point cloud threshold, delete the observed point cloud data and eliminate useless ground point clouds; S1.1.4: Perform coplanarity judgment on the remaining observation point cloud data, merge the observation point cloud data that meet the coplanarity conditions and allocate point set space to obtain the point cloud features of suspected obstacles.

4. The path optimization method of a mobile robot according to claim 2, characterized in that: The specific steps of obtaining the entity features of suspected obstacles based on the oblique photography images of the drone are as follows: S1.2.1: Preprocess the UAV oblique photography images, where the preprocessing includes regional network joint adjustment and multi-view image matching; S1.2.2: Based on the pre-processed UAV oblique photography image, traverse the pixel points of the UAV oblique photography image, calculate the rigid transformation vector and the change amount in the iterative process of two adjacent pixel points, and calculate the entity feature value of the pixel point based on the dynamic adjustment factor, the rigid transformation vector and the change amount; S1.2.3: Compare the entity feature value of the pixel with the entity threshold. If it is greater than the entity threshold, mark the pixel as a sampling point and confirm the neighborhood of the sampling point based on the local shape distribution function; S1.2.4: The feature description vector of the sampling point is calculated based on the normal vector angle between the sampling point and the pixel points in the neighborhood of the sampling point. The calculation formula of the feature description vector of the sampling point is: Among them, V f represents the sampling point feature description vector, T c represents the entity feature value of the sampling point, i represents a single pixel in the neighborhood of the sampling point, N represents the total number of pixels in the neighborhood of the sampling point, and θ i Represents the normal vector angle between the sampling point and the i-th pixel in the sampling point neighborhood, C i represents the curvature of the i-th pixel in the neighborhood of the sampling point, δ represents the normalization factor, e represents the exponential function, a and b represent the horizontal and vertical matrix coordinates of the sampling point, a i and b i represents the horizontal and vertical matrix coordinates of the i-th pixel in the neighborhood of the sampling point, and c represents the convolution factor; S1.2.5: The feature description vectors are aggregated through the maximum pooling operation, and then the aggregated features are further abstracted based on the sparse convolution to obtain the entity features of the suspected obstacle through the RPN network.

5. The path optimization method of a mobile robot according to claim 1, characterized in that: The collision avoidance strategy includes a straight-line motion avoidance strategy and an irregular motion avoidance strategy.

6. A method for optimizing the path of a mobile robot according to claim 5, characterized in that: The specific steps of the linear motion avoidance strategy are as follows: S4.1.1: The mobile robot predicts the motion trajectory of the dynamic obstacle based on the motion direction and speed of the dynamic obstacle transmitted by the sensor component, and calculates the collision node and collision direction in combination with the moving speed of the mobile robot, where the collision direction includes frontal collision and side collision; S4.1.2: If the collision direction is a side collision, the mobile robot remains in a waiting state before the collision node, and calculates the waiting time according to the length and speed of the dynamic obstacle. After the waiting time, the mobile robot continues to move along the global path of the mobile robot; S4.1.3: If the collision direction is a head-on collision, the collision node is regarded as a static obstacle, the node closest to the static obstacle in the global path of the mobile robot is used as the local linear motion target node, the current position of the mobile robot is used as the local linear motion root node, and a temporary path is selected through the local planning algorithm. The mobile robot moves according to the temporary path, and determines whether a collision will occur through the sensor component on the way. If a collision occurs, repeat S4.1.1 to S4.1.3 until the local linear motion target node is reached.

7. A method for optimizing the path of a mobile robot according to claim 5, characterized in that: The specific steps of the irregular motion avoidance strategy are as follows: S4.2.1: The motion trajectory of the dynamic obstacle is sampled by the rolling window method, and the motion trajectory with the highest evaluation function value is determined as the moving path of the dynamic obstacle according to the evaluation function; S4.2.2: Compare the dynamic obstacle movement path with the global path of the mobile robot, treat the overlapping nodes as static obstacles, use the current position of the mobile robot as the root node of the local irregular motion, and define the end point as the destination node of the local irregular motion. Replan a local obstacle avoidance path and replace the current global path of the mobile robot.

8. A path optimization system for a mobile robot, implemented based on a path optimization method for a mobile robot as claimed in any one of claims 1 to 7, characterized in that: The system includes a workspace modeling module, a global path planning module and a local obstacle avoidance planning module; The workspace modeling module is used to determine the workspace of the mobile robot according to the starting and ending positions of the mobile robot, perform obstacle modeling on the workspace through multi-source equipment, and obtain a three-dimensional model of the obstacles; The global path planning module is used to construct a workspace map according to the three-dimensional model of obstacles, and select a reference global path corresponding to the minimum path cost value as the global path of the mobile robot; The local obstacle avoidance planning module is used to sense whether there are dynamic obstacles through the sensor component during the movement of the mobile robot. If there are dynamic obstacles, the mobile robot determines a local obstacle avoidance path according to the collision avoidance strategy and bypasses the dynamic obstacle according to the local obstacle avoidance path.

Images