A robot path planning method and device for semi-structured environment

Abstract

The application provides a robot path planning method and device for a semi-structured environment. The method provided by the application comprises: in the process of locally optimizing a global path, for each speed pair of the robot in the next time window without collision, calculating a first distance between the robot and a target point, a second distance between the robot and a static obstacle, and a third distance between the robot and a dynamic obstacle; calculating a target guidance score, an environment safety score, a collision risk score, and a speed score; determining a first weight of the target guidance score; determining a second weight of the environment safety score; determining a third weight of the collision risk score; calculating a comprehensive score of the speed pair; and selecting the speed pair with the highest comprehensive score as a next motion instruction. The robot path planning method and device for a semi-structured environment provided by the application can effectively avoid obstacles.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of path planning, in particular to a robot path planning method and device for semi-structured environment. BACKGROUND

[0002] Mobile robots play an important role in various scenarios such as warehousing, logistics, rescue, etc., and path planning, as a core component of mobile robot technology, plays a crucial role in improving the autonomous navigation ability and work efficiency of the robot.

[0003] Traditional path planning methods mainly include global path planning and local path planning. Global path planning plans a collision-free path from the starting point to the ending point for the robot based on known environmental map information. However, when facing complex dynamic environments, the traditional method has defects such as inflexible obstacle avoidance strategy, and cannot effectively avoid obstacles. SUMMARY

[0004] Therefore, the present application provides a robot path planning method and device for semi-structured environment, which can effectively avoid collision problems during robot operation.

[0005] Specifically, the present application is realized by the following technical solutions:

[0006] The first aspect of the present application provides a robot path planning method for semi-structured environment, which comprises:

[0007] In the process of locally optimizing the global path, the first distance between the robot and the target point, the second distance between the robot and the static obstacle, and the third distance between the robot and the dynamic obstacle are calculated for each speed pair of the robot in the next time window without collision; wherein the third distance between the robot and the dynamic obstacle is determined based on the position of the robot and the future position of the dynamic obstacle after a preset number of time steps in the future;

[0008] According to the first distance, a target guidance score is calculated to measure whether the robot is advancing towards the target; according to the second distance, an environmental safety score is calculated to measure the safety of the trajectory; according to the third distance, a collision risk score is calculated to measure the obstacle avoidance ability of the robot; and according to the speed pair, a speed score is calculated to measure the speed of the robot;

[0009] determine a first weight of the target guidance score according to the position of the robot, the position of the target point and the first distance; determine a second weight of the environment safety score according to the position of the robot and the environment safety score; determine a third weight of the collision risk score according to the position of the robot and the collision risk score; wherein the first weight is negatively correlated with the first distance;

[0010] calculate a comprehensive score of the speed pair according to the target guidance score, the environment safety score, the collision risk score, the speed score, the first weight, the second weight, the third weight and a fourth weight preset for the speed score;

[0011] select a speed pair with the highest comprehensive score as the next motion instruction according to the comprehensive scores of the speed pairs.

[0012] The second aspect of the application provides a robot path planning device for a semi-structured environment, the device comprising a calculation module, a determination module, a scoring module and a processing module; wherein,

[0013] The calculation module is configured to, in the process of locally optimizing the global path, calculate, for each speed pair of the robot in the next time window without collision, a first distance between the robot and the target point, a second distance between the robot and the static obstacle, and a third distance between the robot and the dynamic obstacle; wherein the third distance between the robot and the dynamic obstacle is determined based on the position of the robot and the future position of the dynamic obstacle after a preset number of time steps in the future;

[0014] The calculation module is further configured to calculate, according to the first distance, a target guidance score for measuring whether the robot is advancing towards the target; calculate, according to the second distance, an environment safety score for measuring the safety of the trajectory; calculate, according to the third distance, a collision risk score for measuring the obstacle avoidance ability of the robot; and calculate, according to the speed pair, a speed score for measuring the speed of the robot;

[0015] The determination module is configured to determine a first weight of the target guidance score according to the position of the robot, the position of the target point and the first distance; determine a second weight of the environment safety score according to the position of the robot and the environment safety score; and determine a third weight of the collision risk score according to the position of the robot and the collision risk score; wherein the first weight is negatively correlated with the first distance;

[0016] The scoring module is configured to calculate a comprehensive score of the speed pair according to the target guidance score, the environment safety score, the collision risk score, the speed score, the first weight, the second weight, the third weight, and a fourth weight preset for the speed score.

[0017] The processing module is configured to select, according to the comprehensive scores of the speed pairs, a speed pair with the highest comprehensive score as a next motion instruction.

[0018] The robot path planning method and device for a semi-structured environment provided by the present application have the following advantages. First, in the process of robot path planning, the third distance between the robot and the dynamic obstacle is determined based on the prediction of the position of the dynamic obstacle after a preset number of future time steps, thereby enhancing the adaptability of the robot to the dynamic environment. Second, the target guidance score (measuring whether the robot is moving towards the target), the environment safety score (measuring the safety of the trajectory), the collision risk score (measuring the obstacle avoidance ability), and the speed score (measuring the speed) are calculated, and these scores comprehensively evaluate the pros and cons of the robot motion from different dimensions, thereby providing data support for the intelligent selection of the motion instruction. Third, in order to more flexibly cope with different environmental conditions and motion requirements, a dynamic weight adjustment mechanism is introduced. The first weight of the target guidance score is negatively correlated with the first distance, that is, the more the robot approaches the target point, the more important the target orientation is. Meanwhile, the second and third weights are dynamically adjusted according to the position of the robot and the environment safety score and the collision risk score, thereby ensuring that the robot moves towards the target as efficiently as possible under the premise of safety. Fourth, after the comprehensive scores of the speed pairs are calculated, the speed pair with the highest comprehensive score is intelligently selected as the next motion instruction, and the target orientation, safety, obstacle avoidance ability, and driving speed of the robot are comprehensively considered, thereby ensuring the efficient and safe navigation of the robot in a complex environment. In this way, through the processes of refined calculation and intelligent selection, the static obstacle and the dynamic obstacle are divided into different items for separate calculation, two different adaptive weight values are obtained, the navigation efficiency of the robot in a complex environment is improved, the robot can quickly find the best path to the target, and the motion strategy can be flexibly adjusted when the dynamic obstacle appears, thereby effectively avoiding collision. Meanwhile, through the dynamic weight adjustment mechanism, the navigation performance of the robot under different environmental conditions is further optimized, thereby realizing the navigation efficiency and safety of the robot in a complex environment. BRIEF DESCRIPTION OF DRAWINGS

[0019] Figure 1 The flowchart of the robot path planning method for a semi-structured environment according to Embodiment One of the present application is shown in FIG. 1.

[0020] Figure 2A local optimization schematic diagram shown for an exemplary embodiment of the present application;

[0021] Figure 3 A hardware structure diagram of a robot path planning device for a semi-structured environment according to an embodiment of the present application;

[0022] Figure 4 A structure schematic diagram of a robot path planning device for a semi-structured environment according to an embodiment of the present application. DETAILED DESCRIPTION

[0023] The exemplary embodiments will be described in detail herein below with reference to the drawings. When the following description refers to the drawings, identical or similar components are denoted by the same reference numerals, and repeated description of these components can be omitted. The following exemplary embodiments described in the following exemplary embodiments are not representative of all embodiments consistent with the present application.

[0024] The terminology used in the present application is for the purpose of describing particular embodiments only and is not intended to be limiting of the present application. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It will be further understood that the terms "comprises" and / or "comprising," when used in this specification, specify the presence of stated features, integers, steps, operations, elements, and / or components, but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof.

[0025] It is to be understood that the singular forms "a", "an", and "the" include plural referents unless the context clearly dictates otherwise. It is to be understood that the term "and / or" as used herein encompasses all possible combinations of one or more of the associated listed items and can be abbreviated as "or". It is further noted that the terms "comprise", "comprising", "comprises" and / or "comprising" when used in this specification, specify the presence of stated features, integers, steps, operations, elements, and / or components but do not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof.

[0026] The following specific embodiments are given to further illustrate the technical solutions of the present application.

[0027] Figure 1 A flow chart of a robot path planning method for a semi-structured environment according to an embodiment of the present application. Please refer to Figure 1 The method provided by the embodiment can include:

[0028] S101、In the process of locally optimizing the global path, for each speed pair of the robot in the next time window without collision, the first distance between the robot and the target point, the second distance between the robot and the static obstacle, and the third distance between the robot and the dynamic obstacle are calculated; wherein the third distance between the robot and the dynamic obstacle is determined based on the position of the robot and the future position of the dynamic obstacle after a preset number of time steps in the future.

[0029] Robot path planning includes two processes of global path planning and local optimization of the global path, that is, mainly includes:

[0030] (1) Operating in a predefined global map environment, determining a global path based on a global path planning algorithm.

[0031] (2) Locally optimizing the global path to facilitate the robot to navigate to each sub-target.

[0032] Optionally, in a possible implementation, the process of determining the global path can include:

[0033] (1) determining an initial global path in a predefined global map environment using a neural network-based global path planning algorithm; wherein the constraints include the kinematic constraints of the robot when determining the initial global path.

[0034] Specifically, the predefined global map environment can contain information such as obstacle positions and road topologies.

[0035] In specific implementation, the global map environment can be obtained through map generation techniques such as simultaneous localization and mapping.

[0036] Further, the neural network-based global path planning algorithm can generate a global path from the starting point to the end point by learning the relationship between environmental features and path optimization objectives (such as the shortest path, the lowest energy consumption). Further, the kinematic constraints of the robot are the limitations of the robot's movement ability on path planning. For example, in an embodiment, the kinematic constraints of the robot can include the turning radius of the robot, the maximum speed and maximum acceleration of the robot, and the joint angle limit of the robot.

[0037] By integrating kinematic constraints, the goal is to generate a collision-free path that is not only efficient but also smoother and more suitable for the robot's movement ability. This optimization can ensure that the generated path is consistent with the actual movement characteristics of the robot, thereby improving navigation and reducing the risk of collision.

[0038] In robot path planning, it is crucial to ensure that the robot can move safely and efficiently from the starting point to the end point. To achieve this goal, kinematic constraints are introduced to optimize the path. For example, the distance between the radius of the robot and the path inflection point is introduced to ensure that the robot does not collide with obstacles when passing through the path inflection point.

[0039] In addition to considering the radius of the robot, a safety distance is introduced to ensure that the robot maintains a sufficient distance from obstacles or other potential risks during movement, thereby avoiding any collisions or dangerous situations.

[0040] In implementation, in this step, the topological map can be determined as a predefined global map, and the starting point, target point and static obstacle information are labeled on it, and then the map is input into the neural network for calculation through forward propagation to generate multiple possible initial global paths from the starting point to the target point.

[0041] (2) The initial global path is subjected to three times of Bezier curve smoothing processing to obtain the global path.

[0042] Specifically, the three times of Bezier curve smoothing processing is a method of optimizing the path through mathematical curves. The three times of Bezier curve is defined by four points: a starting point, two control points and an end point. In path planning, the control points can be used to adjust the shape of the path to make it smoother.

[0043] In implementation, the three times of Bezier curve is defined by four control points, which can generate a continuous and curvature-smooth path, eliminating sharp turning points in the initial path and ensuring that the optimized path is smooth and the robot is highly safe.

[0044] In implementation, first, the starting point and the end point are determined, which represent the starting point and the end point of the robot movement, and are usually determined according to task requirements or environmental information; then the control points are set, appropriate control points are placed between the starting point and the end point to adjust the curvature and direction of the path, and the positions of these control points can be automatically calculated by an algorithm or manually set by an operator; then a three times of Bezier curve is generated according to the starting point, the control points and the end point, and this curve is the obtained global path.

[0045] The robot path planning method for semi-structured environment provided in this embodiment is based on the neural network global path planning method, which can handle dynamic environments and nonlinear problems, and is more adaptable than traditional algorithms. The three times of Bezier curve smoothing processing eliminates the polyline turning and curvature mutation problems of the initial path, generates a continuous and smooth curve, improves the stability of the robot movement, reduces unnecessary turns and stops, and thus improves the overall movement efficiency.

[0046] Referring to the foregoing description, after obtaining the global path, the global path is further locally optimized to ensure that the robot can effectively bypass the obstacles by using limited global information.

[0047] It should be noted that the local optimization is a process of real-time adjustment and optimization of the parameters of the robot's travel according to the current position, speed and surrounding environment information of the robot on the basis of the global path.

[0048] Figure 2 A schematic diagram of local optimization shown for an exemplary embodiment of the present application. Please refer to Figure 2 , the path of the robot from the starting point to the target point is the global path, and the robot locally optimizes the optimization area in the circular range with the robot as the center during travel. The global path in the optimization area is the local path. By optimizing the local path, the obstacle avoidance performance of the robot when facing mixed dynamic and static obstacles is improved.

[0049] Further, in the present application, the local optimization is based on the improved dynamic window approximation algorithm. When the improved window approximation algorithm is used for local optimization, the optimal speed pair of each time window can be determined for a future set of time windows, and then the robot action is controlled based on the determined optimal speed pair in each speed window to achieve the purpose of obstacle avoidance.

[0050] Specifically, the size of the time window is set according to actual needs, which is not limited in the present embodiment. For example, the time window can be determined as 0.5 seconds according to the reaction time of the robot. In addition, the speed pair includes the angular speed and linear speed of the robot.

[0051] In specific implementation, for the next time window, a plurality of collision-free speed pairs in the next time window can be determined first, further, each speed pair is scored to obtain a comprehensive score of each speed pair, and then the speed pair with the highest comprehensive score is selected as the optimal speed pair based on the comprehensive score of each speed pair.

[0052] Specifically, the determination method of the plurality of collision-free speed pairs in the next time window can include:

[0053] (1) Based on the limitations of the hardware structure of the robot, the surrounding environment and other conditions, the speed sampling space is determined.

[0054] In specific implementation, the speed boundary limit, acceleration limit and environmental obstacle limit can be determined according to the model of the robot and the generated global path, and then the speed sampling space is generated.

[0055] In an embodiment, the speed sampling space is defined as follows:

[0056] ;

[0057] where Vm is a kinematic constraint of the robot, and vmmin and vmmax are minimum linear velocity and maximum linear velocity of the robot, and vmmin and vmmax are minimum linear velocity and maximum linear velocity of the robot,

[0058] The limits of linear acceleration and angular acceleration of the mobile robot are limited by the motor performance. Assuming that the maximum acceleration and deceleration are the same, the velocity sampling space is defined as follows:

[0059] ;

[0060] where Vm is a kinematic constraint of the robot, avmax and awmax are minimum acceleration and maximum acceleration of the mobile robot, vc and ωc are linear velocity and angular velocity of the mobile robot, and Δt is the length of time of change.

[0061] (2) Uniformly sampling in the velocity sampling space interval to obtain all velocity pairs of the robot.

[0062] Specifically, the DWA algorithm can be used to uniformly sample in the determined velocity sampling space according to a preset sampling interval.

[0063] Further, the specific length of the sampling interval is set according to actual needs, which is not limited in the embodiment. For example, in an embodiment, the sampling interval can be determined according to the running speed of the robot to prevent collision caused by too small sampling interval.

[0064] In specific implementation, after the velocity sampling space is determined, the DWA algorithm is used to uniformly sample in the velocity sampling space at a certain sampling interval, and the sampling set is represented as follows:

[0065] ;

[0066] where n is the sampling set, and vmmin and vmmax are minimum linear velocity and maximum linear velocity of the robot in the velocity sampling space, and vmmin and vmmax are minimum linear velocity and maximum linear velocity of the robot in the velocity sampling space,

[0067] (3) Screening each velocity pair combination to remove the velocity pairs with collision to obtain multiple non-collision velocity pairs.

[0068] Specifically, considering the obstacles around the mobile robot, the constraint to prevent the mobile robot from colliding with the surrounding obstacles at a certain moment is as follows:

[0069] ;

[0070] wherein, and are minimum and maximum linear velocities of the robot, and are minimum and maximum angular velocities of the robot.

[0071] Further, after determining the velocity pair of the robot without collision in the next time window, in this step, for each velocity pair, the first distance between the robot and the target point, the second distance between the robot and the static obstacle, and the third distance between the robot and the dynamic obstacle are calculated.

[0072] Further, the static obstacle refers to an object that is stationary in the map, and the dynamic obstacle refers to an object whose position or velocity changes over time. For example, in an embodiment, the static obstacle can include walls, boxes, etc., and the dynamic obstacle can include pedestrians, vehicles, etc. Specifically, the first distance can be determined based on the current position of the robot and the position of the target point, the second distance can be calculated based on the position of the robot and the position of the static obstacle identified in the global map environment, and the third distance can be determined based on the position of the robot and the future position of the dynamic obstacle after a preset number of time steps in the future. Wherein the position of the robot is sensed in real time based on the sensor mounted on the robot.

[0073] Further, the first distance can measure whether the robot is advancing towards the target point and how far the robot needs to go to reach the target point, the second distance can evaluate the influence of the static obstacle on the robot for evaluating the safety of the trajectory, and the third distance can evaluate the influence of the dynamic obstacle on the robot, which can also evaluate the safety of the trajectory.

[0074] Specifically, in a possible implementation, the determination process of the future position of the dynamic obstacle after a preset number of time steps can include:

[0075] (1) When the dynamic obstacle enters the detection range of the robot, record the positions of the dynamic obstacle at consecutive n time steps, and establish a position matrix of the dynamic obstacle according to the positions of the dynamic obstacle at consecutive n time steps.

[0076] Specifically, when optimizing the local path, the sensor on the robot detects the position of the dynamic obstacle, and when detecting that the dynamic obstacle enters the detection range of the robot, the dynamic obstacle is tracked.

[0077] It should be noted that the detection range involves a specific range which is set according to actual needs, and in this embodiment, no limitation is made thereto. For example, in an embodiment, the detection range can be determined according to the radius of the robot and the preset safety distance, and a circle with the robot as the center and the safety distance as the radius is determined as the detection range.

[0078] Specifically, when tracking the dynamic obstacle, the positions of the dynamic obstacle at consecutive n time steps are recorded, and a position matrix of the dynamic obstacle is established according to the positions of the dynamic obstacle at consecutive n time steps. Specifically, the time step can be 0.1s. In a specific implementation, the robot carries a laser radar, and every 0.1s, the point cloud is clustered to screen out the contour of the dynamic obstacle, and the position coordinates (x, y, z) thereof are recorded.

[0079] Further, the position matrix of the dynamic obstacle is a matrix composed of the position coordinates of the dynamic obstacle at consecutive multiple time steps, and the motion law of the obstacle can be analyzed through the position matrix of the dynamic obstacle.

[0080] In a specific implementation, the position matrix is as follows:

[0081] ;

[0082] wherein, is the abscissa of the dynamic obstacle at the first time step, is the ordinate of the dynamic obstacle at the first time step, and the maximum value of n is equal to the total number of multiple time steps, is the abscissa of the dynamic obstacle at the n th time step, is the ordinate of the dynamic obstacle at the n th time step, and n is the time step.

[0083] (2) According to the position matrix, a polynomial fitting function based on the least square method is used to predict the future position of the dynamic obstacle after a preset number of time steps.

[0084] In a specific implementation, the degree of the polynomial can be selected based on the motion characteristics of the dynamic obstacle. For example, in an embodiment, when the dynamic obstacle moves at a constant speed, a first-order polynomial can be selected; when the dynamic obstacle moves in a curve, a second-order polynomial can be selected.

[0085] Further, the specific value of the preset number is set according to actual needs, and in this embodiment, no limitation is made thereto. For example, in an embodiment, the preset number can be determined as 5 according to the distance between the dynamic obstacle and the robot.

[0086] Further, the position data of the dynamic obstacle in the position matrix is combined to construct the equation set corresponding to the least square method. For example, in an embodiment, for a quadratic polynomial, a polynomial function after fitting is obtained by minimizing the error, and the future position of the dynamic obstacle after a preset number of time steps is calculated based on the polynomial function.

[0087] In a specific implementation, the predicted future position can be as follows:

[0088] ;

[0089] wherein, is the horizontal coordinate of the future position, is the vertical coordinate of the future position.

[0090] It should be noted that due to the irregular motion of the obstacle, a rolling window method can be used, when new position data becomes available, the fitting coefficients are recalculated, and the position coordinate matrix is updated to track the motion trend of the dynamic obstacle in real time.

[0091] The robot path planning method for semi-structured environments provided in this embodiment significantly improves the trajectory prediction accuracy by updating the position matrix in real time to capture the motion state of the dynamic obstacle and using polynomial fitting to adapt to various motion modes. At the same time, due to the low computational complexity of the prediction method of the least square method, fast and efficient prediction can be realized to obtain the accurate future position of the dynamic obstacle.

[0092] S102, according to the first distance, calculate the target guidance score for measuring whether the robot is advancing towards the target; according to the second distance, calculate the environment safety score for measuring the safety of the trajectory; according to the third distance, calculate the collision risk score for measuring the obstacle avoidance ability of the robot; according to the speed pair, calculate the speed score for measuring the speed of the robot.

[0093] Specifically, the target guidance score is used to measure whether the robot is advancing towards the target. In an embodiment, the first distance itself is directly determined as the target guidance score of the first distance.

[0094] Further, the environment safety score is used to measure the safety of the robot trajectory. In an embodiment, the reciprocal of the second distance is determined as the environment safety score.

[0095] Further, the collision risk score is used to measure the obstacle avoidance ability of the robot. In an embodiment, the collision risk score for measuring the obstacle avoidance ability of the robot according to the third distance comprises:

[0096] The collision risk score is calculated according to the following formula:

[0097] ;

[0098] wherein, the is the collision risk score, the is the third distance; the is a positive attenuation coefficient.

[0099] Further, the speed score is used to measure the speed of the robot, in an embodiment, the speed value is directly determined as the speed score.

[0100] Referring to the foregoing description, in the embodiment, the reciprocal of the second distance is determined as the environmental safety score.

[0101] That is, the environmental safety score can be calculated based on the following formula:

[0102] ;

[0103] wherein, is the environmental safety score, s is the second distance.

[0104] Meanwhile, the collision risk score is calculated according to the following formula:

[0105] ;

[0106] wherein, the is the collision risk score, the is the third distance; the is a positive attenuation coefficient.

[0107] It can be understood that when the second distance is small, that is, the environmental safety score will significantly increase, thereby warning that the current trajectory of the system has a high collision risk and needs to be adjusted in time to avoid potential safety hazards. On the contrary, if the robot maintains a safe distance from the static obstacle, the environmental safety score will be relatively low, indicating that the current trajectory safety is high. Through the use of this reciprocal relationship, the distance change between the robot and the static obstacle can be intuitively and sensitively captured, thereby providing a strong basis for real-time adjustment of path planning and ensuring safe navigation of the robot in a complex environment.

[0108] In addition, when the third distance increases, the exponential function value thereof will decrease rapidly, i.e., the environmental safety score decreases, indicating that the collision risk of the robot with the dynamic obstacle decreases, and the travel trajectory is safer; on the contrary, if the third distance decreases, the environmental safety score will significantly increase, warning that the current trajectory has a higher collision risk, and needs to be adjusted in time. This scoring mechanism based on the exponential function can sensitively capture the change in the distance between the robot and the dynamic obstacle, and accurately reflect the influence of the change on the safety of the robot travel, thereby helping to improve the obstacle avoidance capability and overall safety of the robot in a complex dynamic environment.

[0109] It should be noted that, as the distance increases, the positive attenuation coefficient grows and tends to a fixed value, and this method makes the algorithm more sensitive to dynamic obstacles.

[0110] S103, determining a first weight of the target guidance score according to the position of the robot, the position of the target point and the first distance; determining a second weight of the environmental safety score according to the position of the robot and the environmental safety score; determining a third weight of the collision risk score according to the position of the robot and the collision risk score; wherein the first weight is negatively related to the first distance.

[0111] Specifically, the first weight, the second weight and the third weight are used to represent the importance of each score corresponding to the robot at different times in different positions and different states, in other words, the importance of each score at different times is different (i.e., the first weight, the second weight and the third weight are dynamically changing), and the influence of each score at different times on the driving mode of the robot is measured by the three weights.

[0112] Optionally, in a possible implementation, the method for determining the first weight can include:

[0113] (1) determining a weight reference value according to the position of the robot and the position of the target point.

[0114] In a specific implementation, the position coordinates of the robot and the position coordinates of the target point are obtained. Then, based on these coordinates, an initial weight reference value is calculated by a distance formula or a direction cosine, etc.

[0115] For example, in an embodiment, the weight reference value is determined according to a first formula, and the first formula is:

[0116] ;

[0117] wherein, is the weight reference value, is a preset first initial parameter, is a preset weight of the first heading angle adaptive parameter, a position of the robot, a position of the target point, a preset first distance threshold, a preset first bias parameter.

[0118] Specifically, the specific values of the weight of the first heading angle adaptive parameter, the first distance threshold, and the first bias parameter are set according to actual needs, which are not limited in the embodiment.

[0119] It can be understood that the first formula calculates the dynamic weight reference value by comprehensively considering the distance between the robot and the target point, the heading angle, and a series of preset parameters. The weight reference value can represent the influence of the real-time distance between the robot and the target point on the path planning. Compared with the traditional fixed weight method, the accuracy and reliability of the path planning are ensured.

[0120] (2) When the first distance is greater than or equal to a preset target distance threshold, the weight reference value is determined as the first weight.

[0121] (3) When the first distance is less than the target distance threshold, the product of the weight reference value and a preset proportion coefficient is determined as the first weight; wherein the preset proportion coefficient is greater than 1.

[0122] That is, after the weight reference value is determined, the first weight can be determined based on the following formula:

[0123]

[0124] wherein a(t) is the first weight, is the first distance, is the target distance threshold, is the preset proportion coefficient.

[0125] It should be noted that the specific value of the target distance threshold is set according to actual needs, which is not limited in the embodiment. For example, in an embodiment, the target distance threshold is twice the radius of the robot.

[0126] Further, when the first distance is greater than or equal to the target distance threshold, it indicates that the robot is still relatively far from the target point, and other factors are preferred in path planning. At this time, the weight reference value is directly used as the first weight. Further, when the first distance is less than the target distance threshold, it indicates that the robot is close to the target point, and the speed factor is preferred in path planning. The preset proportion coefficient is introduced to increase the weight, so that the robot is more inclined to reach the target point as soon as possible in the navigation process.

[0127] ​Specifically, the specific value of the preset proportion coefficient is set according to actual needs, which is not limited in the embodiment. For example, in an embodiment, the preset proportion coefficient can be set to 3.

[0128] It should be noted that the preset proportion coefficient is a number greater than 1, and by multiplying the preset proportion coefficient and the weight reference value, the first weight can be amplified, so that the importance of the target guiding score corresponding to the first weight is improved.

[0129] The robot path planning method for semi-structured environment provided in the embodiment can directly use the weight reference value as the first weight when the robot is relatively far away from the target point, so as to ensure the safety of path planning and guide the robot to move towards the target more effectively; and when the first distance is less than the target distance threshold, it indicates that the robot has approached the target point, and at this time, the first weight is increased by multiplying the weight reference value by a preset proportion coefficient greater than 1, so that the robot can quickly reach the target point. The dynamic adjustment mechanism of the weight makes the path planning more flexible and intelligent, can optimize the motion strategy of the robot according to the navigation requirements in different stages, and improves the accuracy and efficiency of path planning.

[0130] Further, optionally, in an embodiment, when the second weight of the environment safety score is determined according to the position of the robot and the environment safety score, the second weight can be determined according to a second formula, and the second formula is:

[0131] ;

[0132] wherein, is a preset second initial parameter, is a preset weight of the second heading angle adaptive parameter, is the position of the robot, is the position of the static obstacle, is a preset second distance threshold, is a preset second bias parameter.

[0133] Specifically, the specific values of the weight of the second heading angle adaptive parameter, the second distance threshold and the second bias parameter are set according to actual needs, which are not limited in the embodiment.

[0134] It can be understood that the second formula considers the distance between the robot and the static obstacle as the robot moves, and when the robot approaches the static obstacle, the weight of the environment safety score in the comprehensive score will automatically increase, so as to make the path planning algorithm pay more attention to the safety of the environment and avoid collision with the static obstacle.

[0135] Further, in a possible implementation, the third weight of the collision risk score can be determined according to a third formula:

[0136] ;

[0137] wherein, is a preset third initial parameter, is a preset weight of the third heading angle adaptive parameter, is the position of the robot, is the position of the dynamic obstacle, is a preset third distance threshold, is a preset third bias parameter.

[0138] Specifically, the specific values of the weight of the third heading angle adaptive parameter, the third distance threshold, and the third bias parameter are set according to actual needs, which are not limited in the embodiment.

[0139] It can be understood that, since the dynamic obstacle cannot be considered in the initial path planning, when the distance between the robot and the dynamic obstacle is far, the third weight calculated by the third formula is mainly determined by and , which indicates that the robot is currently in a relatively safe environment, and the weight of the collision risk score can be relatively low; as the robot gradually approaches the dynamic obstacle, the third weight also decreases accordingly, and the importance of the collision risk score in the comprehensive score will automatically increase, prompting the path planning algorithm to pay more attention to the collision risk with the dynamic obstacle, so as to select a safer path.

[0140] S104, according to the target guidance score, the environment safety score, the collision risk score, the speed score, the first weight, the second weight, the third weight, and the fourth weight preset for the speed score, calculate the comprehensive score of the speed pair.

[0141] In specific implementation, the comprehensive score of the speed pair can be calculated based on the following formula:

[0142] ;

[0143] wherein, is a first weight coefficient of the target guidance score, β(t) is a second weight coefficient of the static obstacle distance score, δ(t) is a third weight coefficient of the collision risk score, γ(t) is a fourth weight of the speed score, W1 is the target guidance score, and W2 is the speed score. is the environment safety score, which is defined as the score of the distance to the static obstacle, a collision risk score defined as a score of distance to dynamic obstacles, a comprehensive weight coefficient for regulating the performance of the robot.

[0144] S105, according to the comprehensive scores of each speed pair, selecting the speed pair with the highest comprehensive score as the next motion instruction.

[0145] In a specific implementation, according to the comprehensive scores of each speed pair calculated in real time, the speed pair with the highest comprehensive score is selected as the next motion instruction, which is input into the robot to control the robot to travel at the speed pair.

[0146] The robot path planning method for semi-structured environments provided in this embodiment has the following advantages. First, in the process of robot path planning, the third distance between the robot and the dynamic obstacle is determined based on the prediction of the position of the dynamic obstacle after a preset number of future time steps, which enhances the adaptability of the robot to dynamic environments. Second, the target guidance score (measuring whether the robot is moving towards the target), the environmental safety score (measuring the safety of the trajectory), the collision risk score (measuring the obstacle avoidance ability), and the speed score (measuring the speed) are calculated, which comprehensively evaluates the pros and cons of the robot motion from different dimensions and provides data support for intelligent selection of motion instructions. Third, to more flexibly respond to different environmental conditions and motion requirements, a dynamic weight adjustment mechanism is introduced. The first weight of the target guidance score is negatively related to the first distance, that is, the closer the robot is to the target point, the more important the target orientation is. At the same time, the second and third weights are dynamically adjusted according to the position of the robot and the environmental safety score and the collision risk score, ensuring that the robot moves towards the target as efficiently as possible under the premise of safety. Fourth, after calculating the comprehensive scores of each speed pair, the speed pair with the highest comprehensive score is intelligently selected as the next motion instruction, which comprehensively considers the target orientation, safety, obstacle avoidance ability, and travel speed of the robot, ensuring efficient and safe navigation of the robot in complex environments. In this way, through the process of refined calculation and intelligent selection, static obstacles and dynamic obstacles are divided into different items for separate calculation, obtaining two different adaptive weight values, which improves the navigation efficiency of the robot in complex environments, not only helping the robot to find the best path to the target faster, but also flexibly adjusting the motion strategy when dynamic obstacles appear to effectively avoid collisions. At the same time, through the dynamic weight adjustment mechanism, the navigation performance of the robot in different environmental conditions is further optimized, realizing the navigation efficiency and safety of the robot in complex environments.

[0147] Corresponding to the foregoing embodiment of the robot path planning method for semi-structured environments, the present application also provides an embodiment of a robot path planning device for semi-structured environments.

[0148] An embodiment of the robot path planning device for semi-structured environment provided in the present application can be applied to a robot path planning device for semi-structured environment. The device embodiment can be implemented by software, or by hardware or a combination of software and hardware. Taking the software implementation as an example, as a logical device, it is formed by reading the corresponding computer program instructions in the non-volatile memory into the memory for running by the processor of the robot path planning device for semi-structured environment where the device is located. From the hardware layer, as shown in the figure, it is a hardware structure diagram of the robot path planning device for semi-structured environment where the robot path planning device for semi-structured environment provided in the present application is located. In addition to the processor, the memory, the network interface, and the non-volatile memory shown in the figure, the robot path planning device for semi-structured environment where the device is located in the embodiment can also include other hardware according to the actual functions of the robot path planning device for semi-structured environment, and details are not described herein. Figure 3 Figure 3

[0149] Figure 4 The structure schematic diagram of the robot path planning device for semi-structured environment provided in the present application is shown in the figure. Please refer to the figure, the device provided in the embodiment includes a calculation module 410, a determination module 420, a scoring module 430, and a processing module 440; wherein, Figure 4

[0150] The calculation module 410 is configured to, in the process of locally optimizing the global path, calculate a first distance between the robot and the target point, a second distance between the robot and the static obstacle, and a third distance between the robot and the dynamic obstacle for each speed pair of the robot in the next time window without collision; wherein the third distance between the robot and the dynamic obstacle is determined based on the position of the robot and the future position of the dynamic obstacle after a preset number of time steps in the future;

[0151] The calculation module 410 is further configured to calculate a target guiding score for measuring whether the robot is advancing towards the target according to the first distance, calculate an environment safety score for measuring the safety of the trajectory according to the second distance, calculate a collision risk score for measuring the obstacle avoidance ability of the robot according to the third distance, and calculate a speed score for measuring the speed of the robot according to the speed pair;

[0152] ​​​The determining module 420 is configured to determine a first weight of the target guidance score according to the position of the robot, the position of the target point and the first distance; determine a second weight of the environment safety score according to the position of the robot and the environment safety score; and determine a third weight of the collision risk score according to the position of the robot and the collision risk score; wherein the first weight is negatively correlated with the first distance.

[0153] The scoring module 430 is configured to calculate a comprehensive score of the speed pair according to the target guidance score, the environment safety score, the collision risk score, the speed score, the first weight, the second weight, the third weight and a fourth weight pre-set for the speed score.

[0154] The processing module 440 is configured to select a speed pair with the highest comprehensive score as a next motion instruction according to the comprehensive scores of the speed pairs.

[0155] The device of the embodiment can be used to execute the steps of the method embodiment, and the specific implementation principles and implementation processes are similar, which will not be repeated here. Figure 1 The specific implementation principles and implementation processes of the steps of the method embodiment are similar, which will not be repeated here.

[0156] Please continue to refer to Figure 3 The application further provides a robot path planning device for a semi-structured environment, which comprises a memory, a processor and a computer program stored in the memory and executable on the processor, and the processor implements the steps of any method provided in the first aspect of the application when executing the program.

[0157] The application further provides a computer readable storage medium, which stores a computer program, and the program is executable on the processor to implement the steps of any method provided in the application.

[0158] The implementation processes of the functions and roles of the units in the device are specifically described in the implementation processes of the corresponding steps in the above method, which will not be repeated here.

[0159] For the device embodiment, since it basically corresponds to the method embodiment, the related parts can be referred to the part of the method embodiment. The device embodiments described above are only illustrative, and the units described as separate components can be or can not be physically separated, and the components displayed as units can be or can not be physical units, that is, they can be located in one place or distributed on multiple network units. According to actual needs, part or all of the modules can be selected to achieve the purpose of the application scheme. Those skilled in the art can understand and implement without creative labor.

[0160] The above only is the preferred embodiment of the present application, and is not used to limit the present application, any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the protection scope of the present application.

Claims

1. A method for robot path planning in semi-structured environments, characterized in that, The method comprises: In the process of locally optimizing the global path, for each speed pair of the robot in the next time window without collision, a first distance between the robot and the target point, a second distance between the robot and the static obstacle, and a third distance between the robot and the dynamic obstacle are calculated; wherein the third distance between the robot and the dynamic obstacle is determined based on the position of the robot and the future position of the dynamic obstacle after a preset number of future time steps; According to the first distance, a target guidance score for measuring whether the robot is advancing towards the target is calculated; according to the second distance, an environment safety score for measuring the safety of the trajectory is calculated; according to the third distance, a collision risk score for measuring the obstacle avoidance ability of the robot is calculated; and according to the speed pair, a speed score for measuring the speed of the robot is calculated; According to the position of the robot, the position of the target point and the first distance, a first weight of the target guidance score is determined; according to the position of the robot and the environment safety score, a second weight of the environment safety score is determined; and according to the position of the robot and the collision risk score, a third weight of the collision risk score is determined; wherein the first weight is negatively related to the first distance, the second weight is determined according to a second formula, and the second formula is: ； wherein, is a preset second initial parameter, is a preset weight of the second heading angle adaptive parameter, is a position of the robot, is a position of a static obstacle, is a preset second distance threshold, is a preset second bias parameter; a third weight is determined according to a third formula, the third formula being: ； wherein, is a preset third initial parameter, is a preset weight of the third heading angle adaptive parameter, is a position of the robot, is a position of the dynamic obstacle, is a preset third distance threshold, is a preset third bias parameter; According to the target guidance score, the environment safety score, the collision risk score, the speed score, the first weight, the second weight, the third weight, and a fourth weight of the speed score set in advance, a comprehensive score of the speed pair is calculated; According to the comprehensive scores of the speed pairs, the speed pair with the highest comprehensive score is selected as the next motion instruction.

2. The method of claim 1, wherein, According to the position of the robot, the position of the target point and the first distance, a first weight of the target guidance score is determined, comprising: According to the position of the robot and the position of the target point, a weight reference value is determined; When the first distance is greater than or equal to a preset target distance threshold, the weight reference value is determined as the first weight; When the first distance is less than the target distance threshold, the product of the weight reference value and a preset proportion coefficient is determined as the first weight; wherein the preset proportion coefficient is greater than 1.

3. The method of claim 2, wherein, According to the position of the robot and the position of the target point, a weight reference value is determined, comprising: The weight reference value is determined according to a first formula, and the first formula is: ； wherein, is a weight reference value, is a preset first initial parameter, is a preset weight of the first heading angle adaptive parameter, is a position of the robot, is a position of the target point, is a preset first distance threshold value, is a preset first bias parameter.

4. The method of claim 1, wherein, The collision risk score for measuring the obstacle avoidance ability of the robot is calculated according to the third distance, comprising: The collision risk score is calculated according to the following formula: ； wherein the is the collision risk score, the is the third distance; the is a positive attenuation coefficient.

5. The method of claim 1, wherein, The environment safety score for measuring the safety of the trajectory is calculated according to the second distance, comprising: The reciprocal of the second distance is determined as the environment safety score.

6. The method of claim 1, wherein, The method for determining the future position of the dynamic obstacle after a preset number of future time steps comprises: When the dynamic obstacle enters the detection range of the robot, the positions of the dynamic obstacle in consecutive n time steps are recorded, and a position matrix of the dynamic obstacle is established according to the positions of the dynamic obstacle in consecutive n time steps; According to the position matrix, a polynomial fitting function based on least squares predicts a future position of the dynamic obstacle after a preset number of time steps.

7. The method of claim 1, wherein, The method for determining the global path comprises: An initial global path is determined by using a neural network-based global path planning algorithm in a predefined global map environment, wherein the constraint conditions include kinematic constraints of the robot when determining the initial global path. The initial global path is subjected to cubic Bezier curve smoothing processing to obtain the global path.

8. A robot path planning apparatus for semi-structured environments, characterized by, The device comprises a calculation module, a determination module, a scoring module and a processing module, wherein The calculation module is configured to, in the process of locally optimizing the global path, calculate a first distance between the robot and the target point, a second distance between the robot and the static obstacle, and a third distance between the robot and the dynamic obstacle for each speed pair of the robot in the next time window without collision, wherein the third distance between the robot and the dynamic obstacle is determined based on the position of the robot and the future position of the dynamic obstacle after a preset number of time steps in the future; The calculation module is further configured to calculate a target guidance score for measuring whether the robot is advancing towards the target according to the first distance, calculate an environment safety score for measuring the safety of the trajectory according to the second distance, calculate a collision risk score for measuring the obstacle avoidance ability of the robot according to the third distance, and calculate a speed score for measuring the speed of the robot according to the speed pair; The determination module is configured to determine a first weight of the target guidance score according to the position of the robot, the position of the target point and the first distance, determine a second weight of the environment safety score according to the position of the robot and the environment safety score, and determine a third weight of the collision risk score according to the position of the robot and the collision risk score, wherein the first weight is negatively correlated with the first distance, the second weight is determined according to a second formula, and the second formula is: ； wherein, is a preset second initial parameter, is a preset weight of the second heading angle adaptive parameter, is a position of the robot, is a position of a static obstacle, is a preset second distance threshold, is a preset second bias parameter; a third weight is determined according to a third formula, which is: ； wherein, is a preset third initial parameter, is a preset weight of the third heading angle adaptive parameter, is a position of the robot, is a position of the dynamic obstacle, is a preset third distance threshold, is a preset third bias parameter; The scoring module is configured to calculate a comprehensive score of the speed pair according to the target guidance score, the environment safety score, the collision risk score, the speed score, the first weight, the second weight, the third weight, and a fourth weight of the speed score set in advance; The processing module is configured to select the speed pair with the highest comprehensive score as the next motion instruction according to the comprehensive scores of the speed pairs.

Images