Path re-planning method and device and electronic equipment

Abstract

The embodiment of the invention provides a path re-planning method and device and electronic equipment, relates to the field of intelligent storage, is applied to terminal equipment, and comprises the following steps: determining whether an initial reference path is blocked by an obstacle in a grid cost map; under the condition that an obstacle exists on the initial reference path, sampling the initial reference path to obtain a plurality of first sampling points in the longitudinal direction of the initial reference path and a plurality of second sampling points of each first sampling point in the transverse direction of the initial reference path, the longitudinal direction is an advancing direction which passes through the first sampling point and is tangent to the initial reference path, and the transverse direction is perpendicular to the longitudinal direction; and determining a replanning path according to the plurality of first sampling points, the plurality of second sampling points, the position of the obstacle and the grid cost map. Automatic obstacle avoidance of the terminal equipment can be realized, and the operation efficiency and the operation flexibility of the terminal equipment are improved.

Description

Technical Field

[0001] This disclosure relates to the field of smart warehousing technology, and in particular to a route replanning method, apparatus, and electronic device. Background Technology

[0002] Currently, the path planning capabilities of mobile terminal devices are heavily reliant on human intervention. For example, in the field of smart warehousing, robot movement strategies still primarily rely on pure path tracking control. When obstacles such as falling materials block the path within the warehouse, avoidance is the solution, requiring manual removal of obstacles to resolve robot waiting times and the resulting congestion. This leads to low robot flexibility and poor efficiency. Summary of the Invention

[0003] This disclosure provides a path replanning method, apparatus, and electronic device. The following technical solutions are disclosed in this disclosure:

[0004] A first aspect of this disclosure provides a path replanning method applied to a terminal device, comprising: determining whether an initial reference path is blocked by an obstacle in a grid cost map; if an obstacle exists on the initial reference path, sampling the initial reference path to obtain a plurality of first sampling points in the longitudinal direction of the initial reference path and a plurality of second sampling points in the transverse direction of the initial reference path for each first sampling point, wherein the longitudinal direction is a travel direction that passes through the first sampling points and is tangent to the initial reference path, and the transverse direction is perpendicular to the longitudinal direction; and determining a replanning path based on the plurality of first sampling points, the plurality of second sampling points, the position of the obstacle, and the grid cost map.

[0005] In some embodiments, determining whether there is an obstacle on the initial reference path in the grid cost map includes: obtaining an initial reference path from a management device, wherein the initial reference path is a path planned by the management device for the terminal device from the start point of the task to the end point of the task, and the management device and the terminal device are communicatively connected; obtaining point cloud data from a sensor device in the terminal device and transforming the point cloud data into the grid cost map; projecting the initial reference path into the grid cost map, and determining whether there is an obstacle on the initial reference path based on the occupancy of the grids traversed by the initial reference path; and determining that the initial reference path is blocked by the obstacle if there is an obstacle on the initial reference path.

[0006] In some embodiments, sampling an initial reference path to obtain multiple first sampling points on the initial reference path and multiple second sampling points for each first sampling point in the lateral direction includes: sampling along the initial reference path according to a first step length to obtain multiple first sampling points; for each first sampling point, determining the heading angle of the first sampling point, the heading angle being the angle formed by rotating counterclockwise from a preset reference direction to the longitudinal direction; and determining multiple second sampling points for the first sampling point in the lateral direction based on the coordinates of the first sampling point, the heading angle, the second step length, and the lateral sampling range constraint.

[0007] In some embodiments, determining a replanning path based on a plurality of first sampling points, a plurality of second sampling points, the location of an obstacle, and a grid cost map includes: using the grid cost map to determine the distance between each of the plurality of first sampling points and the plurality of second sampling points and the center point of the obstacle; identifying sampling points whose distance is greater than or equal to a preset distance threshold as valid sampling points; determining the safety cost of each valid sampling point based on the distance between each valid sampling point and the center point of the obstacle; and determining the replanning path based on the distance between each valid sampling point and the center point of the obstacle and the safety cost.

[0008] In some embodiments, determining the safety cost of each valid sampling point based on the distance between each valid sampling point and the center point of the obstacle includes: determining the safety cost based on the distance between each valid sampling point and the center point of the obstacle, a preset safety distance, and a safety cost weight. Specifically, when the distance between the valid sampling point and the center point of the obstacle is greater than or equal to the preset safety distance, the safety cost is zero; when the distance is less than the preset safety distance, the safety cost is the product of the safety cost weight and the safety penalty value, and the safety penalty value is the difference between the preset safety distance and the distance.

[0009] In some embodiments, determining the replanning path based on the distance between each valid sampling point and the center point of the obstacle, and the safety cost, includes: for a first valid sampling point (i, k), determining the deviation cost of the first valid sampling point relative to a second valid sampling point (i+1, j) in the longitudinal direction, where the first valid sampling point is any valid sampling point, the second valid sampling point is the next sampling point in the longitudinal direction of the first valid sampling point, i is the longitudinal index of the first sampling point, k is the lateral index of the first sampling point, i+1 is the longitudinal index of the second valid sampling point, and j is the lateral index of the second valid sampling point; determining the lateral cost of the first valid sampling point based on the lateral deviation between the first valid sampling point and the corresponding first sampling point in the lateral direction, and the lateral deviation weight; determining the total cost of the first valid sampling point based on the deviation cost, the lateral cost, the safety cost, and the total cost of the second valid sampling point; determining the set of first valid sampling points with the minimum total cost function value based on the total cost of the first valid sampling point; and determining the replanning path using a backtracking method based on the set of first valid sampling points.

[0010] In some embodiments, for a first valid sampling point (i, k), determining the deviation of the first valid sampling point relative to a second valid sampling point (i+1, j) in the longitudinal direction includes: based on the coordinates of the first valid sampling point... Coordinates of the second valid sampling point The deviation cost of the first effective sampling point is determined by the reference heading angle of the first sampling point corresponding to the first effective sampling point, the heading change weight between the first effective sampling point and the corresponding first sampling point, the heading angle of the second effective sampling point, and the angle deviation weight between the heading angle of the first effective sampling point and the reference heading angle.

[0011] In some embodiments, determining the total cost of the first effective sampling point based on the deviation cost, lateral cost, security cost, and the total cost of the second effective sampling point includes: when the vertical index i of the first effective sampling point is n-1, determining that the deviation cost is zero, the total cost of the second effective sampling point is zero, and determining that the total cost of the first effective sampling point is the sum of the security cost and the lateral cost, where n is the maximum value of the sampling point in the vertical direction; when the vertical index i of the first effective sampling point is < n-1, determining the minimum value of the sum of the deviation cost and the total cost of the second effective sampling point, and determining that the total cost of the first effective sampling point is the minimum value, the security cost, and the lateral cost.

[0012] In some embodiments, the method further includes: establishing a target cost function and constraints based on the lateral deviation between the replanned path and the initial reference path; and, if the constraints are satisfied, smoothing the replanned path with the objective of minimizing the function value of the target cost function to obtain a smoothed path.

[0013] In some embodiments, establishing the target cost function and constraints based on the lateral deviation between the replanned path and the initial reference path includes: determining cost deviation smoothing based on the lateral deviation between the replanned path and the initial reference path, the number of sampling points on the replanned path, and the cost weight of each lateral deviation; determining first-order smoothing based on the first derivative of the lateral deviation, the number of sampling points on the replanned path, and the first-order cost weight of each lateral deviation; determining second-order smoothing based on the second derivative of the lateral deviation, the number of sampling points on the replanned path, and the second-order cost weight of each lateral deviation; determining third-order smoothing based on the third derivative of the lateral deviation, the number of sampling points on the replanned path, and the third-order cost weight of each lateral deviation; determining obstacle avoidance smoothing based on the obstacle avoidance relaxation factor of each effective sampling point on the replanned path and the obstacle avoidance cost weight corresponding to each obstacle avoidance factor; establishing the target cost function based on cost deviation smoothing, first-order smoothing, second-order smoothing, third-order smoothing, and obstacle avoidance smoothing; and establishing state transition equations, lateral smoothness constraints, hard constraints on the safety range, lateral velocity constraints, lateral acceleration constraints, and soft constraints on the safety range based on the lateral deviation between the replanned path and the initial reference path.

[0014] In some embodiments, establishing state transition equations, lateral smoothness constraints, hard constraints on the safe range, lateral velocity constraints, lateral acceleration constraints, and soft constraints on the safe range based on the lateral deviation between the replanned path and the initial reference path includes: using the lateral deviation between each sampling point on the replanned path and the corresponding sampling point on the initial reference path as variables in the Taylor formula to obtain state transition equations and lateral smoothness constraints; determining the safe passage range for each sampling point on the replanned path, ensuring the lateral deviation between the replanned path and the initial reference path falls within the safe passage range to obtain hard constraints on the safe range; ensuring the first derivative of the lateral deviation falls within a preset first threshold range to obtain lateral velocity constraints; ensuring the second derivative of the lateral deviation falls within a preset second threshold range to obtain lateral acceleration constraints; and ensuring the difference between the lateral deviation and the soft constraint factor falls within the soft constraint range to obtain soft constraints on the safe range, where the lower boundary of the soft constraint range is the difference between the safe lower boundary of the safe passage range and the safe contraction margin, and the upper boundary of the soft constraint range is the difference between the safe upper boundary of the safe passage range and the safe contraction margin.

[0015] In some embodiments, determining the safe passage range for each sampling point on the replanning path includes: determining the safe upper boundary and safe lower boundary of the safe passage range based on the coordinates of each valid sampling point on the replanning path, wherein the safe upper boundary represents the left boundary in the lateral direction that ensures the terminal device can safely pass through the valid sampling point, and the safe lower boundary represents the right boundary in the lateral direction that ensures the terminal device can safely pass through the valid sampling point.

[0016] In some embodiments, determining the safe upper and lower boundaries of the safe passage range based on the coordinates of each valid sampling point on the replanning path includes: for each valid sampling point (i, k), when k equals the lateral sampling range constraint, or the position of the valid sampling point (i, k) is occupied by an obstacle, or the position of the previous sampling point (i, k-1) in the lateral direction is occupied by an obstacle, the safe lower boundary of the valid sampling point (i, k) is determined to be the distance between adjacent sampling points in the lateral direction; otherwise, the safe lower boundary is the safe lower boundary of the previous sampling point (i, k-1); when k equals the lateral sampling range constraint, or the position of the valid sampling point (i, k) is occupied by an obstacle, or the position of the next sampling point (i, k+1) in the lateral direction is occupied by an obstacle, the safe upper boundary of the valid sampling point (i, k) is determined to be the distance between adjacent sampling points in the lateral direction; otherwise, the safe upper boundary is the safe upper boundary of the next sampling point (i, k+1).

[0017] A second aspect of this disclosure provides a path replanning apparatus applied to a terminal device, comprising: a first determining unit, configured to determine whether an initial reference path is blocked by an obstacle in a grid cost map; a sampling unit, configured to sample the initial reference path when an obstacle exists, obtaining a plurality of first sampling points in the longitudinal direction of the initial reference path and a plurality of second sampling points in the transverse direction of the initial reference path for each first sampling point, wherein the longitudinal direction is a travel direction that passes through the first sampling points and is tangent to the initial reference path, and the transverse direction is perpendicular to the longitudinal direction; and a second determining unit, configured to determine a replanning path based on the plurality of first sampling points, the plurality of second sampling points, the position of the obstacle, and the grid cost map.

[0018] A third aspect of this disclosure provides an electronic device, including: a processor and a memory; the memory for storing computer-executable instructions; and the processor for reading instructions from the memory and executing the instructions to implement the method in any of the implementations of the first aspect.

[0019] A fourth aspect of this disclosure provides a computer-readable storage medium storing computer instructions configured to cause the computer to perform the method in any implementation of the first aspect.

[0020] A fifth aspect of this disclosure provides a computer program product including a computing program stored on a computer-readable storage medium. The computer program includes program instructions that, when executed by a computer, cause the computer to perform the method in any implementation of the first aspect described above.

[0021] The path replanning method, apparatus, and electronic device provided in this disclosure can achieve automatic obstacle avoidance of terminal devices by sampling the initial reference path when there are obstacles in the initial reference filter, and determining the replanning path using a grid cost map based on the sampling points and obstacle positions, thereby improving the terminal's operating efficiency and flexibility. Attached Figure Description

[0022] Figure 1 A schematic diagram illustrating an application scenario provided by an embodiment of this disclosure;

[0023] Figure 2 This is a flowchart of a path replanning method proposed in an embodiment of this disclosure;

[0024] Figure 3 This is a schematic diagram of sampling points shown in an embodiment of the present disclosure;

[0025] Figure 4 This is a schematic diagram of a replanned path obtained according to the path replanning method provided in an embodiment of this disclosure;

[0026] Figure 5 This is a flowchart illustrating a path replanning method proposed in an embodiment of this disclosure;

[0027] Figure 6 This is a flowchart illustrating a path replanning method proposed in an embodiment of this disclosure;

[0028] Figure 7 This is a schematic diagram of a smoothed path obtained by smoothing a replanning path according to an embodiment of this disclosure;

[0029] Figure 8 This is a flowchart illustrating a path replanning method proposed in an embodiment of this disclosure;

[0030] Figure 9 This is a flowchart illustrating a method for generating a smooth path for autonomous obstacle avoidance according to an embodiment of this disclosure;

[0031] Figure 10 This is a flowchart illustrating a path replanning method proposed in an embodiment of this disclosure;

[0032] Figure 11 This is a schematic diagram of a path replanning device 1100 according to an embodiment of the present disclosure;

[0033] Figure 12 This is a schematic diagram of the structure of an electronic device 1200 for implementing the above method, as shown in an embodiment of this disclosure. Detailed Implementation

[0034] Numerous specific details are set forth in the following description to provide a full understanding of this disclosure. However, this disclosure can be implemented in many other ways than those described herein, and those skilled in the art can make similar extensions without departing from the spirit of this disclosure. Therefore, this disclosure is not limited to the specific implementations disclosed below.

[0035] The terminology used in one or more embodiments of this disclosure is for the purpose of describing particular embodiments only and is not intended to be limiting of the one or more embodiments of this disclosure. The singular forms “a,” “the,” and “the” as used in one or more embodiments of this disclosure and the appended claims are also intended to include the plural forms unless the context clearly indicates otherwise. It should also be understood that the term “and / or” as used in one or more embodiments of this disclosure refers to and includes any or all possible combinations of one or more associated listed items.

[0036] It should be understood that although the terms first, second, etc., may be used to describe various information in one or more embodiments of this disclosure, such information should not be limited to these terms. These terms are only used to distinguish information of the same type from one another. For example, first may also be referred to as second without departing from the scope of one or more embodiments of this disclosure, and similarly, second may also be referred to as first. Depending on the context, the word “if” as used herein may be interpreted as “when”, “in response to a determination”, or “when…”.

[0037] This disclosure provides a path replanning method, and also relates to a path replanning apparatus, an electronic device, a computer program product, and a computer-readable storage medium, which will be described in detail in the following embodiments.

[0038] The following describes the solutions of the embodiments of this disclosure with reference to examples.

[0039] In related technologies, the path planning capabilities of mobile terminal devices are highly dependent on human intervention, with warehousing being a typical scenario. For example, picking and handling equipment (such as robots) in warehouses are used to pick and move goods. A robot management system can manage the robots in the warehouse, for instance, by planning travel paths for the robots based on different handling tasks. The robots can then move the target goods to the workstation for picking according to the planned path.

[0040] For example, a robot can track its path using laid tracks or QR codes to reach a designated location and move goods. With the continuous iteration of robot products, the market share of simultaneous localization and mapping (SLAM) navigation robots is increasing year by year. SLAM navigation robots solve the problem of laying tracks or QR codes, significantly reducing project implementation costs. Compared with other navigation robots, SLAM navigation robots are more flexible, code-free navigation mobile robots.

[0041] Although robot navigation flexibility has improved, current robot movement strategies still primarily rely on pure path tracking control. This means that current robots simply execute tasks according to the path instructed by the system, lacking the ability to autonomously plan their own paths, resulting in low robot flexibility. Taking industrial picking robots as an example, current solutions for situations such as materials falling and obstacles blocking paths in warehouses involve the robot stopping after detecting the obstacle. This requires manual removal of the obstacle to resolve the robot's stopping and waiting, leading to significant robot congestion and greatly impacting efficiency.

[0042] Figure 1 This is one application scenario of the present disclosure. For example... Figure 1 As shown, the terminal device 101 may encounter obstacles 102 while moving along the planned path. In related technologies, when a terminal device encounters an obstacle, it avoids it by stopping and waiting for manual removal. The solution proposed in this disclosure enables the terminal device 101 to autonomously replan its path to avoid obstacles.

[0043] In this embodiment of the disclosure, the terminal device 101 may be a device with the requirements for planning paths and avoiding obstacles, such as picking and handling equipment in a warehousing system (e.g., an industrial picking robot). Optionally, the terminal device 101 may also be an Internet of Things (IoT) device such as a vehicle in the field of autonomous driving or a home appliance in the field of smart homes, and this disclosure does not limit it.

[0044] In some embodiments, the terminal device 101 may be a robot that handles boxes and / or goods, such as a RoboShuttle (RS) robot. The RS robot moves the target goods and / or boxes at the target location on the fixed shelf to other locations, or moves the target goods and / or boxes at other locations to the target location on the fixed shelf.

[0045] In some embodiments, the terminal device 101 may be a robot for moving shelves, such as a stealthy robot, such as a P robot. The P robot is used to move mobile shelves (also known as portable shelves). For example, the P robot can move the mobile shelves to the picking area or to a location below a fixed shelf or other locations according to the moving task sent by the server.

[0046] In some embodiments, the terminal device 101 may be a four-way robot, such as an X-series robot, which is a storage and retrieval robot that can move horizontally in four directions along a track within a three-dimensional shelving unit.

[0047] In some embodiments, the terminal device 101 may be a sorting robot, such as an S-series robot, for sorting according to different types of boxes / goods.

[0048] In some embodiments, the terminal device 101 may be a forklift robot, such as an F-series robot; or it may be a mobile handling robot, such as an M-series robot.

[0049] In some embodiments, different functional modules can be deployed on the terminal device 101 according to different types of robots, such as a communication module for communication, a power supply module for power supply, a camera module for recognizing road conditions and recognizing QR codes on cargo boxes / shelves / landmarks, a PNC module for planning and controlling maps, a clock module, etc.

[0050] In some alternative embodiments, the path replanning method provided in this disclosure can be used in a warehousing system, for example, for terminal devices within the warehousing system. This warehousing system may also include electronic equipment (not shown in the figures), which may have the function of planning and issuing paths to the terminal devices. For example, this electronic equipment may be deployed with a Robot Management System (RMS) or an Autonomous Mobile Robot (AMR) control system, etc.

[0051] For example, the electronic device can be a terminal or a server. For instance, when the electronic device is a terminal, it can include various personal computers, laptops, smartphones, tablets, and portable wearable devices. When the electronic device is a server, it can be a standalone server or a server cluster consisting of multiple servers; this disclosure does not limit the scope of the embodiments.

[0052] In this disclosure, the electronic equipment and terminal 101 in the warehousing system can communicate with each other. The electronic equipment can control or manage terminal 101, plan and issue paths for terminal 101, and terminal 101 can provide feedback to the electronic equipment on its real-time location, task execution status, and abnormal situations during movement along the issued path. Terminal 101 can have a path replanning function, and after replanning, terminal 101 can report the replanned path and location information to the electronic equipment. This disclosure does not impose any limitations on this aspect.

[0053] Figure 2 This is a flowchart illustrating a path replanning method proposed in an embodiment of this disclosure, applied to a terminal device. For example, the terminal device can... Figure 1 The terminal device 101 described. (e.g.) Figure 2 As shown, the method includes the following steps:

[0054] Step 201: Determine whether the initial reference path is blocked by obstacles in the grid cost map.

[0055] In some embodiments, a grid cost map can be a rasterized data structure generated by a terminal device or obtained from other devices, which can be used for robot navigation and path planning. A grid cost map divides the environment into a series of grid cells, each assigned a cost value to represent information such as the accessibility, safety, or feasibility of that location.

[0056] For example, the sensors of the terminal device can be used to perceive the surrounding environment of the terminal device. For instance, the sensor device of the terminal device can be a radar sensor device. The terminal device can obtain radar point cloud data from the radar sensor device of the terminal device and transform the radar point cloud data into a grid cost map, thereby assisting the robot in identifying obstacles.

[0057] In some embodiments, the initial reference path may be an initially planned path obtained by the terminal device from other devices. For example, the initial reference path may be a path planned and issued to the terminal device by the management device in the warehousing system.

[0058] In the above embodiments, the management device may be Figure 1The electronic equipment described in the warehousing system. Specifically, the management device can be a device that manages the terminal devices. For example, in a warehousing scenario, the terminal devices can be devices used for handling goods, such as industrial picking robots. The management device can be a device with robot management functions, such as a device deployed with an RMS. In other words, the RMS can manage the terminal devices. In a warehousing scenario, the RMS can instruct the robot to take an initial reference path. For example, the RMS can plan a path for the robot based on the handling task, and the robot can reach the designated location to handle goods according to the indicated initial reference path.

[0059] Optionally, the initial reference path can also be a path planned and generated by the terminal device itself. For example, the terminal device can receive task instructions from the management device, such as a handling task and its pickup and destination locations. The terminal device can then plan and generate a path based on the pickup and destination locations. This disclosure does not restrict the source of the initial reference path.

[0060] In some embodiments, the terminal device may project an initial reference path onto a grid cost map to determine whether there are obstacles on the initial reference path. Obstacles may be static obstacles such as goods that have fallen from a warehouse, or dynamic obstacles such as workers, other terminal devices, etc., and this disclosure does not limit them.

[0061] In some optional embodiments, the process by which the terminal device determines whether there is an obstacle on the initial reference path in the grid cost map includes: obtaining the initial reference path from the management device, wherein the initial reference path is a path planned by the management device for the terminal device from the task start point to the task end point, and the management device and the terminal device are communicatively connected; obtaining point cloud data from the sensor devices in the terminal device and transforming the point cloud data into the grid cost map; projecting the initial reference path into the grid cost map, and determining whether there is an obstacle on the initial reference path based on the occupancy of the grids traversed by the initial reference path; and determining that the initial reference path is blocked by the obstacle if there is an obstacle on the initial reference path.

[0062] In some embodiments, the terminal device can determine whether there are obstacles on the initial reference path through the grid cost map. Specifically, the determination method may be: by utilizing the characteristics of the grid cost map, the initial reference path is projected onto the grid cost map to determine the occupied status (e.g., occupied or unoccupied) of a certain grid passed by the initial reference path. If it is occupied, it means that the grid is blocked by an obstacle.

[0063] For example, the occupancy status can be represented using binary values ​​0 and 1, such as 0 indicating unoccupied and 1 indicating occupied; it can also be represented using probability, such as using a probability value between 0 and 1 to represent the likelihood of the grid being occupied, where the grid is occupied if the probability falls within a certain range, otherwise it is marked as unoccupied. This disclosure does not limit the method for determining the occupancy status.

[0064] In some embodiments, when the terminal device determines that there are no obstacles on the initial reference path, the terminal device can perform pure path tracking along the reference path. For example, the terminal device can calculate and generate control commands based on the initial reference path and send them to the execution controller, thereby continuing pure path tracking along the initial reference path. In other words, when there are no obstacles on the initial reference path, or within a segment of the initial reference path where there are no obstacles, the terminal can continue to move along the initial reference path.

[0065] Step 202: If there are obstacles on the initial reference path, sample the initial reference path.

[0066] In some embodiments, when there are obstacles in the initial reference path, the terminal device can sample the initial reference path to obtain a plurality of first sampling points in the longitudinal direction of the initial reference path and a plurality of second sampling points in the transverse direction of the initial reference path for each first sampling point. The longitudinal direction is the travel direction that passes through the first sampling points and is tangent to the initial reference path, and the transverse direction is perpendicular to the longitudinal direction.

[0067] In some embodiments, sampling the initial reference path means scattering the initial reference path and its surrounding environment at certain sampling intervals to form a set of sampling points with a certain order and distribution pattern. Optionally, the sampling direction of the initial reference path can be multiple directions. For example, sampling can be performed in the longitudinal direction to form a set of sampling points with a number of points less than the original number of points of the initial reference path; sampling can also be performed in the transverse direction, where each sampling point in the longitudinal direction has a corresponding transverse sampling point within a certain range in the transverse direction; sampling can also be performed in an inclined direction, where the inclined direction can be at a certain angle (e.g., an acute angle) with the transverse or longitudinal direction, and this disclosure does not limit this.

[0068] In other words, the sampling points on the initial reference path are the first sampling points, and the sampling points in the horizontal direction of the initial reference path are the second sampling points. By using the initial reference path as a reference benchmark, sampling can be performed in the vertical and horizontal directions according to a certain step size. This allows the terminal device to sample the initial reference path and its surrounding environment. The terminal device does not need to replan the path according to the destination. Instead, it can replan the path after sampling in the horizontal and vertical directions of the initial reference path, thereby achieving more accurate, more efficient, and lower computational cost path replanning.

[0069] In this embodiment of the disclosure, sampling can be performed in the lateral direction according to a preset number of sampling points. The sampling range can be determined according to the application scenario or specific handling task. For example, when the robot is in the shelf area, the number of lateral sampling points can be less. When the robot is in the empty area of ​​the cargo area or when there are few mobile robots around, the number of lateral sampling points can be more. This disclosure does not limit this.

[0070] In this embodiment of the disclosure, the lateral direction may include a lateral left direction and a lateral right direction. Sampling in the lateral direction can mean sampling in both the lateral left and lateral right directions. Optionally, the number of sampling points in the lateral left and lateral right directions may be the same or different, and this disclosure does not limit this.

[0071] For example, in some embodiments, the terminal device samples an initial reference path to obtain a plurality of first sampling points on the initial reference path and a plurality of second sampling points for each first sampling point in the lateral direction, which may include: sampling along the initial reference path according to a first step length to obtain a plurality of first sampling points;

[0072] For each first sampling point, the heading angle of the first sampling point is determined. The heading angle is the angle formed by rotating counterclockwise from the preset reference direction to the longitudinal direction.

[0073] Based on the coordinate heading angle of the first sampling point, the second step size, and the lateral sampling range constraint, multiple second sampling points in the lateral direction of the first sampling point are determined.

[0074] In the above embodiments, the preset reference direction can be, for example, the vertical direction, the positive X-axis direction, etc. For example, in this disclosure, the heading angle of a first sampling point can be determined as follows: A planar coordinate system is established with the first sampling point as the origin, where the vertical direction is the X-axis, the upward direction is the positive vertical direction, the horizontal direction perpendicular to the X-axis is the Y-axis, and the horizontal direction to the left is the positive Y-axis direction; the angle formed by rotating counterclockwise from the positive vertical direction to the direction of travel passing through the first sampling point (i.e., the longitudinal direction passing through the first sampling point) is the heading angle of the first sampling point. Optionally, the heading angle can be... This indicates that the heading angle can be an obtuse angle.

[0075] In the above embodiments, the first step length can be the distance between two adjacent first sampling points on the initial reference path, that is, the first step length can be the longitudinal sampling interval of the first sampling points. For example, the first step length can be represented by Δs, then the sampling interval between two adjacent first sampling points is Δs.

[0076] In the above embodiments, the second step size can be the distance between the first sampling point and the horizontally adjacent second sampling point, or the distance between two adjacent second sampling points. That is, the second step size can be the horizontal sampling interval of the second sampling points. For example, the second step size can be represented by Δ, then the sampling interval between two adjacent second sampling points is Δ. If the first sampling point is taken as the reference, the interval between the first sampling point and the first second sampling point adjacent to its left is Δ, the interval between the first sampling point and the second second sampling point to its left is 2Δ, the interval between the first sampling point and the third second sampling point to its left is 3Δ, and so on. The same applies to the right side, which will not be elaborated here.

[0077] In the above embodiments, the lateral sampling range constraint can be the maximum range of sampling in the lateral direction at each first sampling point. The lateral sampling constraint can be l limit This indicates that the value can be determined based on the specific handling task or the robot's environment. The interval between any second sampling point in the lateral direction and the corresponding first sampling point can be in increments of l. k It means that l k ∈[Δ, 2·Δ, ..., l limit ].

[0078] In other words, after obtaining the first sampling point by sampling the initial reference path, sampling can be performed in the lateral direction of each first sampling point according to the coordinate heading angle, second step size, and lateral sampling range constraint of the first sampling point, to obtain multiple second sampling points in the lateral direction for each first sampling point. Based on the above first sampling points, the coordinates of the second sampling points obtained by the coordinate heading angle, second step size, and lateral sampling range constraint of the first sampling points can be determined according to the following formula 1:

[0079]

[0080] in, The coordinates of the first sampling point are... The second sampling point (i.e., the horizontal sampling point) is generated based on the first sampling point. Let l be the heading angle corresponding to the first sampling point. k ∈[Δ, 2·Δ, ..., l limit ] represents the horizontal sampling interval (or the interval between any second sampling point and its corresponding first sampling point in the horizontal direction), l limit This is a constraint on the horizontal sampling range.

[0081] For example, in a smart warehousing scenario, the terminal equipment is the picking and handling equipment in the warehousing system, such as... Figure 3As shown, the initial planned path received by the picking and handling equipment from the RMS system of the management equipment is the black path. After longitudinal sampling, longitudinal sampling points are obtained (i.e., black dots in the figure). For each longitudinal sampling point, sampling is performed in the transverse direction to obtain transverse sampling points (i.e., gray dots in the figure).

[0082] Step 203: Determine the replanning path based on multiple first sampling points, multiple second sampling points, the location of obstacles, and the grid cost map.

[0083] In some embodiments, a replanning path can be determined based on the location of the first sampling point, the second sampling point, the obstacle, and the grid cost map. For example, the accessibility of each sampling point and the cost of reaching each sampling point can be determined using the grid cost map, and a replanning path with the minimum cost and safe passage can be determined by combining the results.

[0084] Optionally, the terminal device can select sampling points from the sampled point set (first sampling point, second sampling point) to form a new planned path. Optionally, the terminal device can refer to the position of obstacles to achieve obstacle avoidance. Optionally, the terminal device can use a grid cost map to determine the path with the minimum cost.

[0085] Optionally, the terminal device can preprocess the sampling point set before path replanning, such as filtering, removing sampling points that are blocked by obstacles or too close to obstacles.

[0086] Optionally, the terminal device can determine the location of each sampling point and the distance to the obstacle, and use the cost grid map to determine the cost incurred by the terminal device in bypassing the obstacle by passing through each sampling point, thereby finally determining the new path with the minimum cost.

[0087] Optionally, the terminal device can utilize a grid cost map to determine the cost of each sampling point as a point on a new path. This cost can be multi-dimensional, such as a safety dimension or a accessibility dimension. The safety dimension and accessibility dimension can be related to the distance between each sampling point and the obstacle, and this disclosure does not limit this. The calculation method for the evaluation values ​​of the safety dimension and accessibility dimension is not limited in this disclosure; different penalty values ​​can be set according to different distances from the obstacle.

[0088] Optionally, the terminal device can determine the replanning path with the lowest cost through a cost function. The parameters of the cost function may include the coordinates of the sampling point, the heading angle of the sampling point, the deviation between the heading angle of the sampling point and the heading angle of the corresponding sampling point on the initial planned path, etc. Through the cost function, the terminal device can determine the replanning path that deviates from the initial planned path the least and can achieve obstacle avoidance.

[0089] For example, in a smart warehousing scenario, the terminal equipment is the picking and handling equipment in the warehousing system, such as... Figure 4 As shown, the picking and handling equipment performs path replanning in the cost grid map based on the black first sampling point, the gray second sampling point, and the location of the obstacle, resulting in a gray replanned path. In summary, the above embodiments of this disclosure, by sampling the initial reference path and its lateral direction when obstacles are determined to exist, determine the environment surrounding the reference path. By determining the passability and cost of the sampling points, a replanned path with lower cost and safe passage is determined, achieving active obstacle avoidance and preventing efficiency issues caused by obstacles blocking the terminal device. This disclosure uses the initial planned path as a reference benchmark to sample path points in multiple directions and, based on the obstacle location and the cost grid map, replans a path with minimal deviation from the original path and lower replanning cost. This enables obstacle avoidance when the terminal device encounters obstacles, avoiding lag and waiting when the terminal device performs tasks according to the initial reference path.

[0090] Figure 5 This is a flowchart illustrating a path replanning method proposed in an embodiment of this disclosure, applied to a terminal device. Based on Figure 2 The embodiment shown, Figure 5 right Figure 2 Step 203 will be further described, such as... Figure 5 As shown, determining the replanning path based on multiple first sampling points, multiple second sampling points, the location of obstacles, and the grid cost map includes the following steps:

[0091] Step 301: Using the grid cost map, determine the distance between each of the multiple first sampling points and multiple second sampling points and the center point of the obstacle.

[0092] In some embodiments, the distance from each sampling point to the center point of the obstacle can be a straight-line distance, an Euclidean distance, a Manhattan distance, etc., and this disclosure does not limit this.

[0093] In some embodiments, a grid cost map can be used to determine the distance from each sampling point to the center point of the obstacle location in order to determine whether the terminal device can pass through each sampling point.

[0094] In some embodiments, the terminal device can determine the location of the obstacle's center point on the grid cost map, such as the coordinates of the grid it belongs to, based on the grid cost map. After determining the sampling point, the terminal device can project the sampling point onto the grid cost map and determine the location of the sampling point on the grid cost map, such as the coordinates of the grid it belongs to, based on the grid cost map. Then, the terminal device can determine the distance between the sampling point and the obstacle's center based on the specific locations of the sampling point and the obstacle's center on the grid map.

[0095] Step 302: Sampling points whose distance is greater than or equal to a preset distance threshold are determined as valid sampling points.

[0096] In some embodiments, the preset distance threshold can be determined according to the actual situation. For example, the preset distance threshold can be set to the longest radius of the obstacle, that is, the distance from the center point of the obstacle to the farthest position of the obstacle edge; or the preset distance threshold can be set to be greater than the longest radius of the obstacle to improve the fault tolerance rate of the terminal device during operation. By determining the preset distance threshold, it is possible to avoid the terminal device from colliding with the obstacle when moving and to ensure the safe operation of the terminal device.

[0097] In the above embodiments, before determining the preset threshold, the terminal device can analyze the projection of the obstacle on the grid cost map, determine the edge position that is farthest from the center position of the obstacle, and determine the distance between the edge position and the center position of the obstacle, and determine the distance as the radius of the obstacle.

[0098] Alternatively, for example, the terminal device can determine the preset distance threshold based on empirical values. For instance, in a warehouse, the terminal device can determine the maximum value among historically determined preset distances as the current preset distance threshold; or the preset distance threshold can be a fixed value, etc., which are not limited in this disclosure.

[0099] In some embodiments, a valid sampling point is a sampling point that the terminal device can safely pass through, and the terminal device will not collide with the obstacle when passing through a valid sampling point.

[0100] For example, sampling points with a distance greater than or equal to a preset distance threshold can be identified as valid sampling points, while invalid sampling points that other terminal devices cannot safely pass through can be cleared. This allows for the filtering of sampling points so that a path that terminal devices can safely pass through can be replanned based on the valid sampling points.

[0101] Step 303: Determine the safety cost of each valid sampling point based on the distance between each valid sampling point and the center point of the obstacle.

[0102] In some embodiments, the safety cost of a sampling point can be used to indicate the cost required for a terminal device to safely pass through the sampling point. For example, when the safety cost is 0, it means that the terminal device can pass through safely; when the safety cost is not 0, it means that the terminal device may collide with an obstacle when passing through the sampling point.

[0103] In some embodiments, determining the security cost of each valid sampling point based on the distance between each valid sampling point and the center point of the obstacle includes:

[0104] The safety cost is determined based on the distance between each valid sampling point and the center point of the obstacle, the preset safety distance, and the safety cost weight.

[0105] Specifically, when the distance between a valid sampling point and the center point of an obstacle is greater than or equal to a preset safety distance, the safety cost value is zero. When the distance is less than the preset safety distance, the safety cost value is the product of the safety cost weight and the safety penalty value. The safety penalty value is used to represent the level of safety of a valid sampling point. Sampling points with high safety have low penalty values, and sampling points with low safety have high penalty values. For example, the safety penalty value can be the difference between the preset safety distance and the distance between each valid sampling point and the center point of an obstacle.

[0106] For example, the security cost weight can be the weight corresponding to the security cost; the security penalty value can be used to indicate the security of the current sampling point. For example, the larger the security penalty value, the lower the security of the current sampling point and the greater the corresponding security cost.

[0107] In the above embodiments, the security cost of effective sampling points can be expressed as shown in the following formula 2:

[0108] W safe ·(d safe ≥Δ safe :0? (Δ safe -d safe ))Formula 2

[0109] In the above formula, the preset safety distance is represented by Δ. safe The distance between the effective sampling point and the center point of the obstacle is d. safe When d safe ≥Δ safe When the condition is met (i.e., the distance between the valid sampling point and the center point of the obstacle is greater than or equal to the preset safety distance), the safety cost value is 0 (i.e., the value in the parentheses of the above formula is 0, and the safety cost value is equal to W). safe ·0 is 0), when d safe ≥Δ safeIf the condition is not met (i.e., the distance between the valid sampling point and the center point of the obstacle is less than the preset safe distance), a safety penalty value is set, and the safety penalty value is Δ. safe -d safe At this point, the security cost value is the security cost weight W. safe The product of W and the safety penalty value safe ·(Δ safe -d safe ).

[0110] Step 304: Determine the replanning path based on the distance between each valid sampling point and the center point of the obstacle, and the safety cost.

[0111] In some embodiments, the cost required for the terminal device to bypass obstacles by passing through each valid sampling point can be determined based on the distance between the valid sampling point and the center point of the obstacle, as well as the safety cost, cost weight, and other target cost functions. Multiple valid sampling points with the minimum cost required to bypass obstacles are determined, and a replanning path is established based on the multiple valid sampling points with the minimum cost to achieve autonomous obstacle avoidance of the terminal device.

[0112] For example, in some embodiments, determining the replanning path based on the distance between each valid sampling point and the center point of the obstacle, and the safety cost, includes:

[0113] For the first valid sampling point (i, k), determine the deviation cost cost(k, j) of the first valid sampling point relative to the second valid sampling point (i+1, j) in the vertical direction, where the first valid sampling point is any valid sampling point, the second valid sampling point is the next sampling point of the first valid sampling point in the vertical direction, i is the vertical index of the first sampling point, k is the horizontal index of the first sampling point, i+1 is the vertical index of the second valid sampling point, and j is the horizontal index of the second valid sampling point;

[0114] The lateral cost of the first effective sampling point is determined based on the lateral deviation and lateral deviation weight between the first effective sampling point and the first sampling point corresponding to the first effective sampling point in the lateral direction.

[0115] The total cost of the first effective sampling point is determined based on the bias cost, the lateral cost, the security cost, and the total cost of the second effective sampling point.

[0116] Based on the total cost of the first effective sampling points, determine the set of the first effective sampling points with the minimum total cost function value;

[0117] Using a backtracking approach, a replanning path is determined based on the first set of valid sampling points.

[0118] In some embodiments, the deviation cost can be the cost required for the current valid sampling point to move to the next row of valid sampling points in the vertical direction; the lateral deviation can be the deviation between the first valid sampling point and the first sampling point in the same row in the horizontal direction. For example, when the first valid sampling point is the first sampling point, the lateral deviation is 0; the lateral deviation weight is the weight corresponding to the lateral deviation; the lateral cost can be the cost of moving from the first sampling point to the first valid sampling point; wherein, for the first valid sampling point (i, k), determining the deviation cost of the first valid sampling point relative to the second valid sampling point (i+1, ...j) in the vertical direction includes:

[0119] Based on the coordinates of the first valid sampling point Coordinates of the second valid sampling point The deviation cost cost(k, j) of the first effective sampling point is determined by the reference heading angle of the first sampling point corresponding to the first effective sampling point, the heading change weight between the first effective sampling point and the corresponding first sampling point, the heading angle of the second effective sampling point, and the angle deviation weight between the heading angle of the first effective sampling point and the reference heading angle. The deviation cost is as shown in the following formula 3.

[0120]

[0121] in, Let k be the position coordinates of the first valid sampling point at the horizontal position. Let j be the position coordinates of the second valid sampling point at the horizontal position j. For example, when j = k, the deviation cost between two adjacent sampling points in the vertical direction can be determined according to the above formula. W is the reference heading angle of the first sampling point corresponding to the first valid sampling point. dir The heading change weight between the first valid sampling point and the corresponding first sampling point. W represents the heading angle of the second valid sampling point at the lateral position j. dir-diff The angular deviation weight between the heading angle of the first valid sampling point and the reference heading angle.

[0122] In some embodiments, the lateral cost of the first effective sampling point can be determined based on the lateral deviation between the first effective sampling point and the corresponding first sampling point in the lateral direction, and the lateral deviation weight. The specific formula for the lateral cost is as follows: Formula 4:

[0123] W Δ ·I Δ Formula 4

[0124] Among them, I Δ W is the lateral deviation distance between the first valid sampling point and its corresponding first sampling point in the lateral direction.Δ This represents the weighting of the lateral deviation.

[0125] In some embodiments, the total cost C(i, k) of the first effective sampling point can be determined based on the bias cost, the lateral cost, the security cost, and the total cost of the second effective sampling point.

[0126] In some embodiments, determining the total cost of the first valid sampling point based on the bias cost, lateral cost, security cost, and the total cost of the second valid sampling point includes:

[0127] When the vertical index of the first valid sampling point is i = n-1, the deviation cost is determined to be zero, the total cost of the second valid sampling point is determined to be zero, and the total cost of the first valid sampling point is determined to be the sum of the security cost and the horizontal cost, where n is the maximum value of the sampling point in the vertical direction.

[0128] When the vertical index i of the first valid sampling point is less than n-1, determine the minimum value of the sum of the deviation cost and the total cost of the second valid sampling point, and determine the sum of the minimum value, the security cost, and the horizontal cost of the first valid sampling point.

[0129] In some embodiments, the total cost C(i, k) of the first valid sampling point can be expressed as shown in the following formula 5:

[0130] C(i,k)=min(C(i+1,j)+cost(k,j))+W safe ·(d safe ≥Δ safe :0? (Δ safe -d safe ))+W Δ ·I Δ Formula 5

[0131] In the above formula, C(i+1, j) represents the total cost of the second effective sampling point, cost(k, j) represents the bias cost, and W safe ·(d safe ≥Δ safe :0? (Δ safe -d safe )) represents the security value, W Δ ·I Δ This represents the horizontal cost.

[0132] In the above embodiments, n may be the maximum number of sampling points in the longitudinal direction, that is, there are n sampling points in the longitudinal direction in total. The longitudinal index of the first valid sampling point may be from 0 to n - 1. That is, when the longitudinal index i of the first valid sampling point is equal to n - 1, it indicates that the current point is the sampling point in the last row in the longitudinal direction. At this time, since there is no second valid sampling point, the deviation cost is zero and the total cost of the second valid sampling point is zero. Then, according to the above formula, the total cost of the first valid sampling point can be determined as the sum of the safety cost value and the horizontal cost, which is expressed as the following formula 6:

[0133] C(i, k) = W safe ·(d safe ≥Δ safe : 0?(Δ safe - d safe )) + W Δ ·I Δ Formula 6

[0134] In the above embodiments, when the longitudinal index i of the first valid sampling point is less than n - 1, the deviation cost and the total cost of the second valid sampling point are not zero. At this time, the minimum value of the sum of the deviation cost and the total cost of the second valid sampling point can be determined, and the total cost of the first valid sampling point can be determined as the sum of the minimum value, the safety cost value, and the horizontal cost, which is expressed is expressed as the above formula 5.

[0135] In some embodiments, according to the total cost of the first valid sampling point, a set of first valid sampling points with the minimum total cost function value is determined.

[0136] In the above embodiments, for example, the total cost of the valid sampling points can be calculated from the end point to the start point. For example, the cost values between the end point and multiple sampling points in the (n - 2)-th row can be determined, the cost values required to move from the end point to the above multiple sampling points can be determined, and the sampling point with the minimum cost value in the (n - 2)-th row can be determined as the first valid sampling point with the minimum total cost function value according to the cost values. Then, it can be recursively calculated the cost values between the sampling point with the minimum cost value and multiple sampling points in the (n - 3)-th row until the start point is reached, and a set of first valid sampling points with the minimum total cost function value is obtained.

[0137] In other words, the set of first valid sampling points with the minimum total cost function value includes the sampling points with the minimum cost for safely reaching the end point from the start point.

[0138] In some embodiments, after determining the above set, the backtracking method can be used to determine the replanned path according to the set of first valid sampling points. For example, the path formed by linearly connecting the above sampling points can be used as the replanned path, etc.

[0139] In summary, the above embodiments of this disclosure, by determining effective sampling points based on the distance from the sampling point to the center of the obstacle, can determine the safe passage range of the terminal device. By determining the safety cost of effective sampling points, the total cost of effective sampling points can be determined. Based on the total cost, the set of effective sampling points with the minimum cost is determined, and a replanning path is determined based on the combination. This can achieve the determination of a replanning path with the minimum cost and safe passage for the terminal device, ensuring that the terminal device can adjust in time and operate normally when encountering obstacles, and avoiding impact on efficiency and damage to the terminal device.

[0140] Figure 6 This is a flowchart illustrating a path replanning method proposed in an embodiment of this disclosure, applied to a terminal device. Based on Figure 2 , Figure 5 In the illustrated embodiment, the method further includes the following steps:

[0141] Step 401: Based on the lateral deviation between the replanned path and the initial reference path, establish the objective cost function and constraints.

[0142] In some embodiments, the objective cost function can be used to indicate the cost required for the terminal device to change the initial reference path to a replanned path. The replanned path can be optimized through the objective cost function so that the cost required for the terminal device to change the path is minimized.

[0143] In some embodiments, the objective cost function and constraints can be used to constrain and smooth the replanning path. By limiting the constraints, the operating path of the terminal device can be made suitable for the actual application scenario and meet the requirements of smoothness, thereby reducing the loss of the terminal device.

[0144] In some embodiments, establishing the objective cost function and constraints based on the lateral deviation between the replanned path and the initial reference path includes:

[0145] Cost deviation smoothing is determined based on the lateral deviation between the replanned path and the initial reference path, the number of sampling points on the replanned path, and the cost weight of each lateral deviation.

[0146] First-order smoothing is determined based on the first derivative of the lateral deviation, the number of sampling points on the replanning path, and the first-order cost weight of each lateral deviation.

[0147] Second-order smoothing is determined based on the second derivative of the lateral deviation, the number of sampling points on the replanning path, and the second-order cost weight of each lateral deviation.

[0148] The third-order smoothing is determined based on the third derivative of the lateral deviation, the number of sampling points on the replanning path, and the third-order cost weight of each lateral deviation.

[0149] Obstacle avoidance smoothing is determined based on the obstacle avoidance relaxation factor of each valid sampling point on the replanning path and the obstacle avoidance cost weight corresponding to each obstacle avoidance factor.

[0150] Based on cost deviation smoothing, first-order smoothing, second-order smoothing, third-order smoothing, and obstacle avoidance smoothing, establish the target cost function;

[0151] Based on the lateral deviation between the replanned path and the initial reference path, state transition equations, lateral smoothness constraints, hard constraints on the safety range, lateral velocity constraints, lateral acceleration constraints, and soft constraints on the safety range are established.

[0152] For example, cost deviation smoothing is determined based on the lateral deviation between the replanned path and the initial reference path, the number of sampling points on the replanned path, and the cost weight of each lateral deviation. The specific formula for cost deviation smoothing is shown in Formula 7 below:

[0153]

[0154] The lateral deviation between the replanned path and the initial reference path is l. i The number of sampling points on the replanning path is n, and the cost weight of each lateral deviation is w1;

[0155] For example, first-order smoothing is determined based on the first derivative of the lateral deviation, the number of sampling points on the replanning path, and the first-order cost weight of each lateral deviation. The specific formula for first-order smoothing is shown in Formula 8 below:

[0156]

[0157] The first derivative of the lateral deviation is l′ i The number of sampling points on the replanning path is n, and the first-order cost weight of the lateral deviation is w. l′ ;

[0158] For example, second-order smoothing is determined based on the second derivative of the lateral deviation, the number of sampling points on the replanning path, and the second-order cost weight of each lateral deviation. The specific formula for second-order smoothing is shown in Formula 9 below:

[0159]

[0160] The second derivative of the lateral deviation is l″. i The number of sampling points on the replanning path is n, and the second-order cost weight of the lateral deviation is w. l″ ;

[0161] For example, third-order smoothing is determined based on the third derivative of the lateral deviation, the number of sampling points on the replanning path, and the third-order cost weight of each lateral deviation. The specific formula for third-order smoothing is shown in Formula 10 below:

[0162]

[0163] The third derivative of the lateral deviation is l″′ i The number of sampling points on the replanning path is n, and the third-order cost weight of the lateral deviation is w. l″′ ;

[0164] For example, obstacle avoidance smoothing is determined based on the obstacle avoidance relaxation factor of each valid sampling point on the replanning path and the obstacle avoidance cost weight corresponding to each obstacle avoidance factor. The specific formula for obstacle avoidance smoothing is shown in Formula 11 below:

[0165]

[0166] In the above formula, s represents the relaxation factor for obstacle avoidance, used to balance path smoothness and traffic safety, and w s The obstacle avoidance cost weights corresponding to the obstacle avoidance factors;

[0167] For example, based on cost deviation smoothing, first-order smoothing, second-order smoothing, third-order smoothing, and obstacle avoidance smoothing, a target cost function is established. The specific formula for the target cost function is shown in Formula 12 below:

[0168]

[0169] In some embodiments, constraints can be established based on the lateral deviation between the replanned path and the initial reference path. For example, constraints may include state transition equations, lateral smoothness constraints, hard constraints on safety range, lateral velocity constraints, lateral acceleration constraints, soft constraints on safety range, etc.

[0170] Step 402: Under the condition of satisfying the constraints, with the goal of minimizing the function value of the objective cost function, smooth the replanning path to obtain a smooth path.

[0171] In some embodiments, the replanning path can be constrained according to the above constraints. When the constraints are satisfied, the objective cost function is minimized. The replanned path is then smoothed to obtain a better smooth path.

[0172] In some embodiments, the target cost function can be iterated, and the iteration can be stopped when the function value of the target cost function is less than a preset function value, or the iteration can be stopped when the range of change of the function value of the target cost function is less than a preset range of change, or the iteration can be stopped when the function value of the target cost function does not change, so as to obtain the smooth path corresponding to the current target cost function, etc. This disclosure does not limit this.

[0173] For example, in a smart warehousing scenario, the terminal equipment is the picking and handling equipment in the warehousing system, such as... Figure 7 As shown, the picking and handling equipment smooths the replanning path obtained above according to the objective cost function and constraints, resulting in a gray smooth path.

[0174] In summary, the above embodiments of this disclosure, by establishing an objective cost function and constraints to smooth the replanning path, can obtain a replanning path with minimum cost and smoothness, which can make the terminal device move more smoothly, reduce impact force, and reduce energy loss.

[0175] Figure 8 This is a flowchart illustrating a path replanning method proposed in an embodiment of this disclosure, applied to a terminal device. Figure 8 As shown, it includes the following steps:

[0176] Step 501: The lateral deviation between each sampling point on the replanning path and its corresponding sampling point on the initial reference path is used as a variable in the Taylor formula to obtain the state transition equation and the lateral smoothness constraint. In some embodiments, the relationship between the second and third derivatives of the lateral deviation between each sampling point on the replanning path and its corresponding sampling point on the initial reference path is as shown in the following formula 13:

[0177]

[0178] In some embodiments, Taylor's formula 1 can be expressed as the following formulas 14 and 15:

[0179]

[0180] R n (l)=o[(l-l0) n ] Formula 15

[0181] Among them, R n (l)=o[(l-l0) n [] indicates the remaining sub-item.

[0182] Taylor's formula 2 can be expressed as the following formulas 16 and 17:

[0183]

[0184] R n (l)=o[(l-l0) n ] Formula 17

[0185] Taylor's formula 3 can be expressed as the following formulas 18 and 19:

[0186]

[0187] R n (1)=o[(1-10)n ] Formula 19

[0188] Based on the relationship between the derivatives of the lateral deviations mentioned above, and Taylor's formula 3 (Formulas 18 and 19), we can obtain formula 1 for the lateral smoothness constraint, which is expressed as shown in Formula 20 below:

[0189]

[0190] Based on Taylor's formula 2 (formulas 16 and 17) above, we can derive formula 1, which is expressed as the following formula 21:

[0191]

[0192] Substituting the above lateral smoothness constraint formula 1 (formula 20) into the above derived formula 1 (formula 21) and simplifying, we can obtain lateral smoothness constraint formula 2, which is expressed as follows: Formula 22:

[0193]

[0194] Based on Taylor's formula 1 (formulas 14 and 15) above, we can derive formula 2, expressed as follows:

[0195] Equation 23:

[0196]

[0197] Substituting the above lateral smoothness constraint formula 1 (formula 20) into the above derived formula 2 (formula 23) and simplifying, we can obtain the state transition equation, which is expressed as shown in the following formula 24:

[0198]

[0199] Step 502: Determine the safe passage range for each sampling point on the replanning path, and ensure that the lateral deviation between the replanning path and the initial reference path is within the safe passage range to obtain the hard constraint of the safe range.

[0200] In some embodiments, the safe passage range for each sampling point on the replanning path can be determined by:

[0201] Based on the coordinates of each valid sampling point on the replanning path, determine the upper and lower safe boundaries of the safe passage range. The upper safe boundary represents the left boundary in the lateral direction that ensures the terminal device can safely pass through the valid sampling point, and the lower safe boundary represents the right boundary in the lateral direction that ensures the terminal device can safely pass through the valid sampling point.

[0202] In some embodiments, determining the upper and lower safe boundaries of the safe passage range based on the coordinates of each valid sampling point on the replanning path includes:

[0203] For each valid sampling point (i, k), if k equals the lateral sampling range constraint, or the position of the valid sampling point (i, k) is occupied by an obstacle, or the position of the previous sampling point (i, k-1) in the lateral direction is occupied by an obstacle, the safe lower boundary of the valid sampling point (i, k) is determined to be the distance between adjacent sampling points in the lateral direction; otherwise, the safe lower boundary is the safe lower boundary of the previous sampling point (i, k-1).

[0204] When k equals the lateral sampling range constraint, or the position of the valid sampling point (i, k) is occupied by an obstacle, or the position of the next sampling point (i, k+1) in the lateral direction is occupied by an obstacle, the safe upper boundary of the valid sampling point (i, k) is determined to be the distance between adjacent sampling points in the lateral direction; otherwise, the safe upper boundary is the safe upper boundary of the next sampling point (i, k+1).

[0205] Specifically, the upper and lower safe boundaries of the safe passage range can be represented as shown in Formulas 25 and 26 below:

[0206]

[0207]

[0208] in, The lower boundary represents the safe boundary, and the rightmost valid boundary that can be extended by the k-th sampling step size is represented. This represents the upper safety boundary, and this represents the leftmost valid boundary that can be extended to the Kth sampling step.

[0209] In the above embodiments, when When, it means that the Kth sampling point can extend to the right boundary of the horizontal sampling range, that is, the rightmost boundary within the horizontal constraint range. When, it indicates that the position of the current valid sampling point (i, k) is occupied by an obstacle. When the first adjacent sampling point in the horizontal direction is occupied by an obstacle, the safe range to the right of the Kth sampling point can be determined as the distance between adjacent sampling points in the horizontal direction. For other cases, the safe lower boundary can be determined as the safe lower boundary of the previous sampling point (i, k-1).

[0210] In the above embodiments, when This indicates that the Kth sampling point can extend to the left boundary of the horizontal sampling range, that is, the leftmost boundary within the horizontal constraint range. When, it indicates that the position of the current valid sampling point (i, k) is occupied by an obstacle. When the condition is met, it indicates that the next adjacent sampling point in the horizontal direction is occupied by an obstacle. For both of these cases, the safe range to the left of the Kth sampling point can be determined as the distance between adjacent sampling points in the horizontal direction. For other cases, the safe upper boundary can be determined as the safe upper boundary of the next sampling point (i, k+1).

[0211] In some embodiments, the lateral deviation between the replanned path and the initial reference path is kept within a safe passage range, resulting in a safe range hard constraint, which can be expressed as the following formula 27:

[0212]

[0213] Step 503: The first derivative of the lateral deviation is kept within a preset first threshold range to obtain the lateral velocity constraint.

[0214] In some embodiments, the first derivative of the lateral deviation can be constrained to be within a preset first threshold range, thereby obtaining a lateral velocity constraint, which can be expressed as the following formula 28:

[0215] L′ lower ≤l′ i ≤L′ upper Formula 28

[0216] Step 504: The second derivative of the lateral deviation is kept within the range of a preset second threshold to obtain the lateral acceleration constraint.

[0217] In some embodiments, the second derivative of the lateral deviation can be constrained to be within a preset second threshold range, thereby obtaining a lateral acceleration constraint, which can be expressed as the following formula 29:

[0218] L″ lower ≤l″ i ≤L″ upper Formula 29

[0219] Step 505: The difference between the lateral deviation and the soft constraint factor is within the soft constraint range to obtain the safe range soft constraint.

[0220] In some embodiments, the lower boundary of the soft constraint range can be the difference between the safe lower boundary of the safe passage range and the safe contraction margin, expressed as the following formula 30:

[0221]

[0222] in, This is a safety contraction margin determined based on safety considerations.

[0223] The difference between the upper boundary of the soft constraint range and the safe upper boundary of the safe passage range and the safe contraction margin is expressed as the following formula 31:

[0224]

[0225] The difference between the lateral deviation and the soft constraint factor lies within the soft constraint range, resulting in a safe range soft constraint, which is expressed as follows: Formula 32:

[0226]

[0227] Among them, s i This represents the soft constraint factor added to satisfy the inequality constraints.

[0228] In summary, the above embodiments of this disclosure can determine constraints, which facilitates the constraint of the replanning path to obtain a replanning path with the lowest cost and better smoothness. This enables the terminal device to automatically avoid obstacles when it encounters obstacles, improves the flexibility and intelligent processing of the terminal device, and can improve the working efficiency of the terminal device.

[0229] The following is a specific implementation of a path replanning method provided in this disclosure:

[0230] A method for generating smooth paths that autonomously bypass obstacles, such as Figure 9 As shown, the method involves the design of modules such as reference path input, obstacle-blocked road status monitoring, traffic safety space calculation, path replanning, and path smoothing.

[0231] The specific process of this method is as follows:

[0232] Step 1: The robot receives the task reference path from the starting point to the destination;

[0233] Step 2: The robot acquires lightning point clouds using its onboard radar sensor equipment and transforms the radar point clouds into a grid cost map;

[0234] Step 3: Project the reference path onto the grid cost map and determine whether there are obstacles on the path. If there are obstacles on the path, it means the path is blocked, and proceed to step 5; otherwise, it means the reference path is safe, and proceed to step 4.

[0235] Step 4: The robot performs pure path tracking along the reference path, calculates and generates control commands, and sends them to the execution controller;

[0236] Step 5: Initiate path replanning. Based on the reference path and the grid cost map, generate a new planned path to bypass obstacles.

[0237] Step 6: Smooth the new planned path by adding a series of constraints and output the smoothed path;

[0238] Step 7: Calculate and generate control commands based on the smooth path, and send them to the execution controller.

[0239] Regarding the obstacle detour process described above, path replanning is used to search for safe replanned paths, while path smoothing takes the replanned paths as input and generates smooth paths based on a series of constraints.

[0240] Specifically, the path replanning module uses a given reference path, combined with a grid cost map of obstacle distribution, and employs the Bellman equation of dynamic programming to obtain a path that bypasses obstacles and achieves safe driving through sampling and dynamic calculation. Figure 10 As shown, the specific process is as follows:

[0241] Step 1: The robot receives the reference path;

[0242] Step 2: The robot uses formula (1) to uniformly sample the path along a fixed distance Δs within the lateral constraint range, generating sampling points, such as... Figure 3 As shown, the black dashed lines represent the given discrete reference path points, and the gray dashed lines represent the generated lateral sampling points.

[0243]

[0244] in These are the lateral sampling points generated based on the reference point. As a reference point, For the reference heading corresponding to the reference point, l k ∈[Δ, 2·Δ, ..., l limit [] represents the horizontal sampling interval, Δ represents the fixed unit step size, and l limit This is a constraint on the horizontal sampling range.

[0245] Step 3: Calculate the passability of each lateral sampling point using the grid cost map, define passable points as valid sampling points, and define impassable points as invalid sampling points;

[0246] Step 4: Perform statistical analysis on the effective range of the left and right boundaries of the sampling points at the same horizontal position, as shown in formulas (2) and (3):

[0247]

[0248]

[0249] in, This represents the rightmost valid boundary that can be extended by the k-th sampling step. Let represent the leftmost valid boundary that can be extended by the k-th sampling step, and let obs indicate that the position is occupied by an obstacle. After processing with formulas (2) and (3), the safe passage range of each lateral sampling point can be obtained, denoted as .

[0250] Step 5: As shown in formula (4), define the Bellman equation and use dynamic programming to find the minimum cost path from the robot's current position to the target point.

[0251] C(i,k)=min(C(i+1,j)+cost(k,j))+W safe ·(d safe ≥Δ safe :0? (Δ safe -d safe ))+W Δ ·I Δ (4)

[0252] Where C(i, k) represents the total cost from the lateral position k of the i-th path point to the target point along the reference path direction, and d safe Δ represents the distance between the current position and the obstacle. safe To establish a safe distance, W safe For the safety cost weight, I Δ W is the lateral deviation distance from the reference point. Δ Given its weights, cost(k,j) represents the cost between the horizontal position k of the i-th path point and the horizontal position j of the (i+1)-th path point. It is calculated by weighting and summing multiple different considerations, as shown in formula (5):

[0253]

[0254] in, Let k be the horizontal coordinates of the i-th path point. Let j be the horizontal coordinate of the (i+1)th path point. W is the reference heading corresponding to the reference point. dir The weighting of the heading change between two points. Let W be the heading at the (i+1)th path point at the lateral position j. dir-aiff The weight of the angular deviation between the lateral position k of the i-th path point and the reference heading.

[0255] By using dynamic programming, the optimal connection relationship (minimum cost) between sampling points along the reference line direction is found;

[0256] Step 6: Based on minimum cost calculation, obtain the replanning path from the robot's current position to the target point using backtracking, such as... Figure 4 As shown:

[0257] Since the path obtained from path replanning does not consider smoothness, it cannot be directly sent to the controller for execution. It needs to be processed by the path smoothing module to ensure that the final path meets the requirements of smooth and safe robot movement. Path smoothing adopts a quadratic convex optimization method, as shown in formulas (6) and (7).

[0258]

[0259] 1≤Ax≤u (7)

[0260] Formula (6) is the cost function, and formula (7) is the constraint condition. The goal of the quadratic convex optimization is to find the optimization variable that minimizes the value of the cost function while satisfying the constraint condition.

[0261] Based on formula (6) and considering the actual optimization quantities that the robot needs to consider in path planning, the cost function is defined as shown in formula (8):

[0262]

[0263] Defining the third derivative jerk as a constant, we can obtain:

[0264]

[0265] Substituting formula (9) into formula (8), we can derive formulas (8.1) and (8.2):

[0266]

[0267]

[0268] Among them l i w represents the lateral offset of the i-th smooth path point along the reference path direction. l Its cost weight; l′ i w represents the first derivative of the lateral offset with respect to the reference path (lateral velocity). l′ Its cost weight; l″ i w represents the second derivative of the lateral offset with respect to the reference path (lateral acceleration). l″ The cost weight is s; s represents the relaxation factor for safe obstacle avoidance, used to balance path smoothing and traffic safety, w s Its cost weight.

[0269] As can be seen from the definition of the cost function, the goal of quadratic convex optimization is to find a smooth path with the smallest possible lateral offset from the reference path, while ensuring the safe passage of the robot, given a reference path.

[0270] The generation of a smooth path is based on satisfying the constraints of robot motion. Since the replanning path obtained above is obtained by uniformly sampling the path along a fixed distance Δs on the reference path, the connection relationship between each point and its derivative along the reference path direction is shown in Equation (10):

[0271]

[0272] According to Taylor's formula (11) and formula (12)

[0273]

[0274] R n (l)=o[(l-l0) n (12)

[0275]

[0276] R n (l)=o[(l-l0) n (14)

[0277]

[0278] R n (l)=o[(l-l0) n (16)

[0279] According to Taylor's formula, we can obtain:

[0280] Based on formulas (9) and (15), formula (17) can be derived.

[0281]

[0282] Based on formula (13), formula (18) can be derived.

[0283]

[0284] Based on formula (11), formula (19) can be derived.

[0285]

[0286] Substituting formula (17) into formula (18) and simplifying, we can derive formula (20).

[0287]

[0288] Substituting formula (17) into formula (19) and simplifying, we can derive formula (21).

[0289]

[0290] At the same time, for each lateral offset l i The safe passage range obtained from the aforementioned path replanning is constrained as shown in formula (22):

[0291]

[0292] Meanwhile, constraints are applied to the first and second derivatives of the lateral offset, as shown in formulas (23) and (24):

[0293] L′ lower ≤l′ i ≤L′ upper (twenty three)

[0294] L″ lower ≤l″ i ≤L″ upper (twenty four)

[0295] Among them, L′ lower , L′ upper , L″ lower , L″ upper All are constants.

[0296] Meanwhile, to ensure the safety of generating a smooth path, a soft obstacle avoidance constraint is added to the lateral offset, as shown in formula (25):

[0297]

[0298] in, This represents the shrinkage margin of the safety range at each point, based on safety considerations. i This represents the soft constraint factor added to satisfy the inequality constraints.

[0299] Using formula (21) as the state transition equation, formulas (17), (20), (22), (23), and (24) as constraints, and formula (8.2) as the objective of the quadratic convex optimization, a smooth path is obtained using the OSQP optimization solver, as follows: Figure 7 As shown.

[0300] In summary, the method presented in this example improves the intelligence level of the picking robot, increases the flexibility of robot operation, enables it to actively avoid obstacles, and improves efficiency.

[0301] Figure 11 This is a schematic diagram of the structure of a path replanning device 1100 according to an embodiment of this disclosure, applied to a terminal device, such as... Figure 11 As shown, the device includes:

[0302] The first determining unit 1110 is used to determine whether the initial reference path is blocked by an obstacle in the grid cost map; the sampling unit 1120 is used to sample the initial reference path when there is an obstacle on the initial reference path, to obtain multiple first sampling points in the longitudinal direction of the initial reference path, and multiple second sampling points in the transverse direction of the initial reference path for each first sampling point, wherein the longitudinal direction is the travel direction that passes through the first sampling point and is tangent to the initial reference path, and the transverse direction is perpendicular to the longitudinal direction; the second determining unit 1130 is used to determine the replanning path based on the multiple first sampling points, the multiple second sampling points, the position of the obstacle and the grid cost map.

[0303] In some embodiments, the first determining unit further includes a processing unit, configured to obtain an initial reference path from a management device, wherein the initial reference path is a path planned by the management device for the terminal device from the start point of the task to the end point of the task, and the management device and the terminal device are communicatively connected; obtain point cloud data from a sensor device in the terminal device and transform the point cloud data into a grid cost map; project the initial reference path onto the grid cost map, and determine whether there are obstacles on the initial reference path based on the occupancy of the grids traversed by the initial reference path; and determine that the initial reference path is blocked by obstacles if there are obstacles on the initial reference path.

[0304] In some embodiments, the sampling unit is further configured to sample along the initial reference path according to the first step length to obtain a plurality of first sampling points; for each first sampling point, determine the heading angle of the first sampling point, the heading angle being the angle formed by rotating counterclockwise from the preset reference direction to the longitudinal direction; and determine a plurality of second sampling points of the first sampling point in the lateral direction according to the coordinates of the first sampling point, the heading angle, the second step length, and the lateral sampling range constraint.

[0305] In some embodiments, the second determining unit is further configured to use a grid cost map to determine the distance between each of the plurality of first sampling points and the plurality of second sampling points and the location center point of the obstacle; to determine sampling points whose distance is greater than or equal to a preset distance threshold as valid sampling points; to determine the safety cost of each valid sampling point based on the distance between each valid sampling point and the location center point of the obstacle; and to determine a replanning path based on the distance between each valid sampling point and the location center point of the obstacle and the safety cost.

[0306] In some embodiments, the second determining unit is further configured to determine a safety cost value based on the distance between each valid sampling point and the center point of the obstacle, a preset safety distance, and a safety cost weight. The safety cost value is zero when the distance between the valid sampling point and the center point of the obstacle is greater than or equal to the preset safety distance, and when the distance is less than the preset safety distance, the safety cost value is the product of the safety cost weight and the safety penalty value, and the safety penalty value is the difference between the preset safety distance and the distance.

[0307] In some embodiments, the second determining unit is further configured to, for a first valid sampling point (i, k), determine the deviation cost of the first valid sampling point relative to a second valid sampling point (i+1, j) in the vertical direction, wherein the first valid sampling point is any valid sampling point, the second valid sampling point is the next sampling point of the first valid sampling point in the vertical direction, i is the vertical index of the first sampling point, k is the horizontal index of the first sampling point, i+1 is the vertical index of the second valid sampling point, and j is the horizontal index of the second valid sampling point; determine the horizontal cost of the first valid sampling point based on the horizontal deviation between the first valid sampling point and the corresponding first sampling point in the horizontal direction and the horizontal deviation weight; determine the total cost of the first valid sampling point based on the deviation cost, the horizontal cost, the security cost, and the total cost of the second valid sampling point; determine the set of first valid sampling points with the minimum total cost function value based on the total cost of the first valid sampling point; and determine the replanning path based on the set of first valid sampling points using a backtracking method.

[0308] In some embodiments, the second determining unit is further configured to determine the coordinates of the first valid sampling point. Coordinates of the second valid sampling point The deviation cost of the first effective sampling point is determined by the reference heading angle of the first sampling point corresponding to the first effective sampling point, the heading change weight between the first effective sampling point and the corresponding first sampling point, the heading angle of the second effective sampling point, and the angle deviation weight between the heading angle of the first effective sampling point and the reference heading angle.

[0309] In some embodiments, the second determining unit is further configured to determine that when the longitudinal index i = n-1 of the first valid sampling point, the deviation cost is zero, the total cost of the second valid sampling point is zero, and the total cost of the first valid sampling point is the sum of the security cost and the lateral cost, where n is the maximum value of the sampling point in the longitudinal direction; when the longitudinal index i < n-1 of the first valid sampling point, the unit determines the minimum value of the sum of the deviation cost and the total cost of the second valid sampling point, and the unit determines that the total cost of the first valid sampling point is the minimum value, the security cost, and the lateral cost.

[0310] In some embodiments, the path replanning apparatus further includes a processing unit, configured to establish a target cost function and constraints based on the lateral deviation between the replanned path and the initial reference path; and, if the constraints are satisfied, to perform path smoothing on the replanned path with the objective of minimizing the function value of the target cost function, thereby obtaining a smoothed path.

[0311] In some embodiments, the processing unit is further configured to: determine cost deviation smoothing based on the lateral deviation between the replanned path and the initial reference path, the number of sampling points on the replanned path, and the cost weight of each lateral deviation; determine first-order smoothing based on the first derivative of the lateral deviation, the number of sampling points on the replanned path, and the first-order cost weight of each lateral deviation; determine second-order smoothing based on the second derivative of the lateral deviation, the number of sampling points on the replanned path, and the second-order cost weight of each lateral deviation; determine third-order smoothing based on the third derivative of the lateral deviation, the number of sampling points on the replanned path, and the third-order cost weight of each lateral deviation; determine obstacle avoidance smoothing based on the obstacle avoidance relaxation factor of each effective sampling point on the replanned path and the obstacle avoidance cost weight corresponding to each obstacle avoidance factor; establish a target cost function based on cost deviation smoothing, first-order smoothing, second-order smoothing, third-order smoothing, and obstacle avoidance smoothing; and establish state transition equations, lateral smoothness constraints, hard constraints on the safety range, lateral velocity constraints, lateral acceleration constraints, and soft constraints on the safety range based on the lateral deviation between the replanned path and the initial reference path.

[0312] In some embodiments, the processing unit can also be used to take the lateral deviation between each sampling point on the replanning path and the corresponding sampling point on the initial reference path as a variable of the Taylor formula to obtain the state transition equation and lateral smoothness constraint; determine the safe passage range of each sampling point on the replanning path, and ensure that the lateral deviation between the replanning path and the initial reference path is within the safe passage range to obtain a safe range hard constraint; ensure that the first derivative of the lateral deviation is within a preset first threshold range to obtain a lateral velocity constraint; ensure that the second derivative of the lateral deviation is within a preset second threshold range to obtain a lateral acceleration constraint; and ensure that the difference between the lateral deviation and the soft constraint factor is within the soft constraint range to obtain a safe range soft constraint, wherein the lower boundary of the soft constraint range is the difference between the safe lower boundary of the safe passage range and the safe contraction margin, and the upper boundary of the soft constraint range is the difference between the safe upper boundary of the safe passage range and the safe contraction margin.

[0313] In some embodiments, the processing unit can also be used to determine the upper and lower safe boundaries of the safe passage range based on the coordinates of each valid sampling point on the replanning path. The upper safe boundary represents the left boundary in the lateral direction that ensures the terminal device can safely pass through the valid sampling point, and the lower safe boundary represents the right boundary in the lateral direction that ensures the terminal device can safely pass through the valid sampling point.

[0314] In some embodiments, the processing unit can also be configured to, for each valid sampling point (i, k), determine the safe lower boundary of the valid sampling point (i, k) as the distance between adjacent sampling points in the lateral direction when k equals the lateral sampling range constraint, or the position of the valid sampling point (i, k) is occupied by an obstacle, or the position of the previous sampling point (i, k-1) in the lateral direction is occupied by an obstacle; otherwise, the safe lower boundary is the safe lower boundary of the previous sampling point (i, k-1); and determine the safe upper boundary of the valid sampling point (i, k) as the distance between adjacent sampling points in the lateral direction when k equals the lateral sampling range constraint, or the position of the valid sampling point (i, k) is occupied by an obstacle, or the position of the next sampling point (i, k+1) in the lateral direction is occupied by an obstacle; otherwise, the safe upper boundary is the safe upper boundary of the next sampling point (i, k+1).

[0315] Figure 12 This is a schematic diagram illustrating the structure of an electronic device 1200 for implementing the above-described method according to an exemplary embodiment. The components of the electronic device 1200 include, but are not limited to, a memory 1210 and a processor 1220. The processor 1220 is connected to the memory 1210 via a bus 1230, and a database 1250 is used to store data.

[0316] Electronic device 1200 also includes access device 1240, which enables electronic device 1200 to communicate via one or more networks 1260. Examples of these networks include Public Switched Telephone Network (PSTN), Local Area Network (LAN), Wide Area Network (WAN), Personal Area Network (PAN), or combinations of communication networks such as the Internet. Access device 640 may include one or more of any type of wired or wireless network interface (e.g., network interface card (NIC)), such as an IEEE 802.11 Wireless Local Area Network (WLAN) wireless interface, a Wi-MAX (Worldwide Interoperability for Microwave Access) interface, an Ethernet interface, a Universal Serial Bus (USB) interface, a cellular network interface, a Bluetooth interface, a Near Field Communication (NFC) interface, and so on.

[0317] In one embodiment of this disclosure, the above-described components of the electronic device 1200 and Figure 12 Other components, not shown, can also be connected to each other, for example, via a bus. It should be understood that... Figure 12 The illustrated electronic device block diagram is for illustrative purposes only and is not intended to limit the scope of this disclosure. Those skilled in the art can add or replace other components as needed.

[0318] Electronic device 1200 can be any type of stationary or mobile computing device, including mobile computers or mobile computing devices (e.g., tablet computers, personal digital assistants, laptop computers, notebook computers, netbooks, etc.), mobile phones (e.g., smartphones), wearable computing devices (e.g., smartwatches, smart glasses, etc.) or other types of mobile devices, or stationary computing devices such as desktop computers or personal computers (PCs). Electronic device 1200 can also be a mobile or stationary server.

[0319] The processor 1220 implements the steps of the above method when executing the computer instructions.

[0320] The above is an illustrative scheme of an electronic device according to this embodiment. It should be noted that the technical solution of this electronic device and the technical solution of the above method belong to the same concept. For details not described in detail in the technical solution of the electronic device, please refer to the description of the technical solution of the above method.

[0321] Embodiments of this disclosure also provide a non-transitory computer-readable storage medium storing computer instructions, wherein the computer instructions are used to cause a computer to perform the methods described in the above embodiments of this disclosure.

[0322] Embodiments of this disclosure also provide a computer program product, including a computer program that is executed by a processor using the methods described in the above embodiments of this disclosure.

[0323] The foregoing has described specific embodiments of this disclosure. Other embodiments are within the scope of the appended claims. In some cases, the actions or steps recited in the claims may be performed in a different order than that shown in the embodiments and may still achieve the desired results. Furthermore, the processes depicted in the drawings do not necessarily require the specific or sequential order shown to achieve the desired results. In some embodiments, multitasking and parallel processing are also possible or may be advantageous.

[0324] The computer instructions include computer program code, which may be in the form of source code, object code, executable file, or some intermediate form. The computer-readable medium may include: any entity or device capable of carrying the computer program code, recording media, USB flash drive, portable hard drive, magnetic disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc.

[0325] It should be noted that, for the sake of simplicity, the foregoing method embodiments are all described as a series of actions. However, those skilled in the art should understand that this disclosure is not limited to the described order of actions, because according to this disclosure, some steps can be performed in other orders or simultaneously. Furthermore, those skilled in the art should also understand that the embodiments described in the specification are preferred embodiments, and the actions and modules involved are not necessarily essential to this disclosure.

[0326] In the above embodiments, the descriptions of each embodiment have different focuses. For parts not described in detail in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0327] The preferred embodiments disclosed above are merely illustrative of this disclosure. The optional embodiments do not exhaustively describe all details, nor do they limit the invention to the specific implementations described. Clearly, many modifications and variations can be made based on the content of this disclosure. These embodiments are selected and specifically described in this disclosure to better explain the principles and practical applications of this disclosure, thereby enabling those skilled in the art to better understand and utilize this disclosure. This disclosure is limited only by the claims and their full scope and equivalents.

Claims

1. A path replanning method, characterized in that, Applied to terminal devices, including: Determine whether the initial reference path is blocked by obstacles in the grid cost map; When there are obstacles on the initial reference path, the initial reference path is sampled to obtain multiple first sampling points in the longitudinal direction of the initial reference path, and multiple second sampling points for each first sampling point in the transverse direction of the initial reference path. The longitudinal direction is the travel direction that passes through the first sampling points and is tangent to the initial reference path, and the transverse direction is perpendicular to the longitudinal direction. The replanning path is determined based on the multiple first sampling points, the multiple second sampling points, the location of the obstacles, and the grid cost map.

2. The method according to claim 1, characterized in that, Determining whether there are obstacles on the initial reference path in the grid cost map includes: The initial reference path is obtained from the management device, which is the path from the start point of the task to the end point of the task planned by the management device for the terminal device. The management device and the terminal device are communicatively connected. Point cloud data is acquired from the sensor devices in the terminal device, and the point cloud data is transformed into the grid cost map; The initial reference path is projected onto the grid cost map, and the presence of obstacles on the initial reference path is determined based on the occupancy of the grids traversed by the initial reference path. If an obstacle exists on the initial reference path, it is determined that the initial reference path is blocked by the obstacle.

3. The method according to claim 1, characterized in that, The step of sampling the initial reference path to obtain multiple first sampling points on the initial reference path and multiple second sampling points for each first sampling point in the lateral direction includes: On the initial reference path, sampling is performed according to the first step length to obtain the plurality of first sampling points; For each first sampling point, the heading angle of the first sampling point is determined, and the heading angle is the angle formed by rotating counterclockwise from the preset reference direction to the longitudinal direction; Based on the coordinates of the first sampling point, the heading angle, the second step size, and the lateral sampling range constraint, a plurality of second sampling points of the first sampling point in the lateral direction are determined.

4. The method according to claim 1, characterized in that, The step of determining the replanning path based on the plurality of first sampling points, the plurality of second sampling points, the location of the obstacle, and the grid cost map includes: Using the grid cost map, the distance between each of the plurality of first sampling points and the plurality of second sampling points and the center point of the obstacle is determined; Sampling points whose distance is greater than or equal to a preset distance threshold are determined as valid sampling points; The safety cost of each valid sampling point is determined based on the distance between each valid sampling point and the center point of the obstacle. The replanning path is determined based on the distance between each valid sampling point and the center point of the obstacle, and the safety cost.

5. The method according to claim 4, characterized in that, The determination of the safety cost of each valid sampling point based on the distance between each valid sampling point and the center point of the obstacle includes: The safety cost is determined based on the distance between each valid sampling point and the center point of the obstacle, a preset safety distance, and a safety cost weight. Wherein, when the distance between the effective sampling point and the center point of the obstacle is greater than or equal to the preset safety distance, the safety cost value is zero; when the distance is less than the preset safety distance, the safety cost value is the product of the safety cost weight and the safety penalty value, and the safety penalty value is the difference between the preset safety distance and the distance.

6. The method according to claim 5, characterized in that, The step of determining the replanning path based on the distance between each valid sampling point and the center point of the obstacle, and the safety cost, includes: For a first valid sampling point (i, k), determine the deviation cost of the first valid sampling point relative to the second valid sampling point (i+1, j) in the vertical direction, where the first valid sampling point is any valid sampling point, the second valid sampling point is the next sampling point of the first valid sampling point in the vertical direction, i is the vertical index of the first sampling point, k is the horizontal index of the first sampling point, i+1 is the vertical index of the second valid sampling point, and j is the horizontal index of the second valid sampling point; The lateral cost of the first effective sampling point is determined based on the lateral deviation and lateral deviation weight between the first effective sampling point and the first sampling point corresponding to the first effective sampling point in the lateral direction. The total cost of the first effective sampling point is determined based on the deviation cost, the lateral cost, the security cost, and the total cost of the second effective sampling point. Based on the total cost of the first effective sampling points, determine the set of first effective sampling points with the minimum total cost function value; Using a backtracking method, the replanning path is determined based on the first set of valid sampling points.

7. The method according to claim 6, characterized in that, The cost of determining the deviation of the first effective sampling point (i, k) relative to the second effective sampling point (i+1, j) in the longitudinal direction includes: Based on the coordinates of the first valid sampling point Coordinates of the second valid sampling point The deviation cost of the first effective sampling point is determined by the reference heading angle of the first sampling point corresponding to the first effective sampling point, the heading change weight between the first effective sampling point and the corresponding first sampling point, the heading angle of the second effective sampling point, and the angle deviation weight between the heading angle of the first effective sampling point and the reference heading angle.

8. The method according to claim 6, characterized in that, The step of determining the total cost of the first effective sampling point based on the bias cost, the lateral cost, the security cost, and the total cost of the second effective sampling point includes: When the vertical index i = n-1 of the first effective sampling point, the deviation cost is determined to be zero, the total cost of the second effective sampling point is determined to be zero, and the total cost of the first effective sampling point is determined to be the sum of the security cost and the horizontal cost, where n is the maximum value of the sampling point in the vertical direction; When the vertical index i of the first valid sampling point is less than n-1, the minimum value of the sum of the deviation cost and the total cost of the second valid sampling point is determined, and the total cost of the first valid sampling point is determined to be the sum of the minimum value, the security cost, and the horizontal cost.

9. The method according to claim 1, characterized in that, The method further includes: Based on the lateral deviation between the replanned path and the initial reference path, establish the objective cost function and constraints; Under the condition that the constraints are met, the replanning path is smoothed with the goal of minimizing the function value of the objective cost function, and a smoothed path is obtained.

10. The method according to claim 9, characterized in that, The step of establishing the objective cost function and constraints based on the lateral deviation between the replanned path and the initial reference path includes: Cost deviation smoothing is determined based on the lateral deviation between the replanned path and the initial reference path, the number of sampling points on the replanned path, and the cost weight of each lateral deviation. First-order smoothing is determined based on the first derivative of the lateral deviation, the number of sampling points on the replanning path, and the first-order cost weight of each lateral deviation. Second-order smoothing is determined based on the second derivative of the lateral deviation, the number of sampling points on the replanning path, and the second-order cost weight of each lateral deviation. The third-order smoothing is determined based on the third derivative of the lateral deviation, the number of sampling points on the replanning path, and the third-order cost weight of each lateral deviation. Obstacle avoidance smoothing is determined based on the obstacle avoidance relaxation factor of each valid sampling point on the replanning path and the obstacle avoidance cost weight corresponding to each obstacle avoidance factor. The target cost function is established based on the cost deviation smoothing, the first-order smoothing, the second-order smoothing, the third-order smoothing, and the obstacle avoidance smoothing. Based on the lateral deviation between the replanned path and the initial reference path, state transition equations, lateral smoothness constraints, hard constraints on the safety range, lateral velocity constraints, lateral acceleration constraints, and soft constraints on the safety range are established.

11. The method according to claim 10, characterized in that, The step of establishing state transition equations, lateral smoothness constraints, hard safety range constraints, lateral velocity constraints, lateral acceleration constraints, and soft safety range constraints based on the lateral deviation between the replanned path and the initial reference path includes: The lateral deviation between each sampling point on the replanning path and the corresponding sampling point on the initial reference path is used as a variable in the Taylor formula to obtain the state transition equation and the lateral smoothness constraint. Determine the safe passage range for each sampling point on the replanning path, and ensure that the lateral deviation between the replanning path and the initial reference path is within the safe passage range to obtain the hard constraint of the safe passage range; The lateral velocity constraint is obtained by taking the first derivative of the lateral deviation as a value within a preset first threshold range. The second derivative of the lateral deviation is within a preset second threshold range to obtain the lateral acceleration constraint; The difference between the lateral deviation and the soft constraint factor is within the soft constraint range to obtain the safe range soft constraint. The lower boundary of the soft constraint range is the difference between the safe lower boundary of the safe passage range and the safe contraction margin, and the upper boundary of the soft constraint range is the difference between the safe upper boundary of the safe passage range and the safe contraction margin.

12. The method according to claim 11, characterized in that, Determining the safe passage range for each sampling point on the replanning path includes: Based on the coordinates of each valid sampling point on the replanning path, the upper and lower safe boundaries of the safe passage range are determined. The upper safe boundary represents the left boundary in the lateral direction that ensures the terminal device can safely pass through the valid sampling point, and the lower safe boundary represents the right boundary in the lateral direction that ensures the terminal device can safely pass through the valid sampling point.

13. The method according to claim 12, characterized in that, The step of determining the upper and lower safe boundaries of the safe passage range based on the coordinates of each valid sampling point on the replanning path includes: For each valid sampling point (i, k), if k equals the lateral sampling range constraint, or the position of the valid sampling point (i, k) is occupied by an obstacle, or the position of the previous sampling point (i, k-1) in the lateral direction is occupied by an obstacle, the safe lower boundary of the valid sampling point (i, k) is determined to be the distance between adjacent sampling points in the lateral direction; otherwise, the safe lower boundary is the safe lower boundary of the previous sampling point (i, k-1). When k equals the lateral sampling range constraint, or the position of the effective sampling point (i, k) is occupied by an obstacle, or the position of the next sampling point (i, k+1) in the lateral direction of the effective sampling point is occupied by an obstacle, the safe upper boundary of the effective sampling point (i, k) is determined to be the distance between adjacent sampling points in the lateral direction; otherwise, the safe upper boundary is the safe upper boundary of the next sampling point (i, k+1).

14. A path replanning device, characterized in that, Applied to terminal devices, including: The first determining unit is used to determine whether the initial reference path is blocked by an obstacle in the grid cost map; A sampling unit is used to sample the initial reference path when there are obstacles on the initial reference path, to obtain a plurality of first sampling points in the longitudinal direction of the initial reference path, and a plurality of second sampling points for each first sampling point in the transverse direction of the initial reference path, wherein the longitudinal direction is a travel direction that passes through the first sampling points and is tangent to the initial reference path, and the transverse direction is perpendicular to the longitudinal direction. The second determining unit is used to determine a replanning path based on the plurality of first sampling points, the plurality of second sampling points, the location of the obstacle, and the grid cost map.

15. An electronic device comprising: A processor and a memory, wherein the memory is used to store computer-executable instructions; The processor is configured to read the instructions from the memory and execute the instructions to implement the method as described in any one of claims 1 to 13.

16. A computer-readable storage medium, wherein, The storage medium stores computer program instructions, which, when read by a computer, execute the method as described in any one of claims 1 to 13.

17. A computer program product comprising a computer program, wherein, When the computer program is executed by a processor, it implements the method of any one of claims 1 to 13.

Images