An adaptive obstacle avoidance path planning method and system

Abstract

The application discloses a kind of self-adapting obstacle avoidance path planning method and system, method includes the following steps: obtaining map information and the position information of starting point and end point on map;Using hybrid A* algorithm, according to the position information of starting point and end point and map information Generation the driving path of least number of turns, according to driving path travel;Real-time acquisition of surrounding pedestrian information, according to the driving speed on driving path of surrounding pedestrian information to avoid pedestrians;If not reach the end point, real-time update map information, the current position is as new starting point, and obtain the position information of starting point and end point on new map, then execute the step of using hybrid A* algorithm, according to the position information of starting point and end point and map information Generation the driving path of least number of turns.The application can generate walking path suitable for closed scene, and can effectively avoid surrounding pedestrians.

Description

Technical Field

[0001] This invention relates to path planning technology, and more particularly to an adaptive obstacle avoidance path planning method and system. Background Technology

[0002] Obstacle avoidance in robots refers to the process by which a mobile robot, while moving, uses sensors to detect static or dynamic obstacles along its planned path, updates its path in real time according to a specific algorithm, avoids the obstacles, and finally reaches its target point.

[0003] Robot path planning refers to planning a shortest accessible route based on the starting position, ending position, and map. The goal of robot path planning is to find a feasible optimal path through environmental perception and active obstacle avoidance. This path should not intersect with any obstacles in the given workspace from the starting point to the ending point. At the same time, it is necessary to optimize the robot's movement path to make it as short and smooth as possible.

[0004] Currently, path planning technology has been widely applied to some functional robots, such as robot vacuum cleaners and food delivery robots. These robots generally have the ability to walk according to the planned path and avoid obstacles in the path.

[0005] In enclosed spaces such as warehouses, restaurants, and shopping malls, where space is limited, there are many people coming and going, and the environment is prone to change, the path planning of robots is challenged. Summary of the Invention

[0006] The technical problem to be solved by the present invention is: in view of the technical problems existing in the prior art, the present invention provides an adaptive obstacle avoidance path planning method and system, which can generate a walking path suitable for closed scenes and effectively avoid surrounding pedestrians.

[0007] To solve the above-mentioned technical problems, the technical solution proposed by this invention is as follows:

[0008] An adaptive obstacle avoidance path planning method includes the following steps:

[0009] Obtain map information, including the location information of the starting point and the destination on the map;

[0010] Using a hybrid A* algorithm, a driving path with the fewest turns is generated based on map information and the location information of the starting and ending points, and the driving is carried out according to the driving path.

[0011] It can obtain real-time information on surrounding pedestrians and adjust its driving speed along the route to avoid pedestrians based on this information.

[0012] If the destination is not reached, the map information is updated in real time, the current location is used as the new starting point, and the location information of the starting point and the destination on the new map is obtained. Then, the hybrid A* algorithm is used to generate the driving path with the fewest turns based on the map information and the location information of the starting point and the destination.

[0013] Furthermore, when generating the driving path with the fewest turns, the process specifically includes: searching from the parent node to the child node, selecting the child node whose direction of movement does not change as the next parent node; if there is no child node whose direction of movement does not change, selecting the child node with the smallest change in movement angle among all child nodes as the next parent node.

[0014] Furthermore, when selecting a child node whose direction of motion does not change as the next parent node, the specific steps include: setting a cost function for the optimal control problem and setting constraints, including the vehicle kinematic equations, the allowable range of state / control variables corresponding to the vehicle's inherent mechanical characteristics, collision avoidance constraints, and state and control constraints of the robot's initial and final positions. The cost function and constraints are solved simultaneously to obtain the optimal driving speed and angle.

[0015] Furthermore, when adjusting driving speed along the route based on information about surrounding pedestrians to avoid them, this specifically includes:

[0016] If a pedestrian is detected, the pedestrian's trajectory is predicted using the pedestrian's social force model.

[0017] If the pedestrian's predicted trajectory does not intersect with the driving path, then the driving speed is maintained within the driving path;

[0018] If the pedestrian's predicted trajectory intersects with the driving path, the pedestrian's speed is predicted, and the time advantage is calculated based on the pedestrian's speed, the current driving speed, and the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the distance between the pedestrian and the robot. If the time advantage is greater than a preset time advantage threshold, the driving speed is maintained in the driving path.

[0019] If the time advantage is less than the preset time advantage threshold, and the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the robot is greater than the comfortable braking distance, then the robot will decelerate in the driving path.

[0020] If the time advantage is less than the preset time advantage threshold, and the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the robot is less than the comfortable braking distance but greater than the maximum braking distance, then the robot will stop abruptly in the driving path.

[0021] If the time advantage is less than the preset time advantage threshold, and the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the robot is less than the maximum braking distance, then the robot accelerates along the driving path.

[0022] Furthermore, predicting pedestrian speed includes: if there is only one pedestrian, predicting the pedestrian speed in the single-person state, the expression for the pedestrian speed in the single-person state is:

[0023]

[0024] Where τ is the tangent direction. For pedestrians, For the ideal direction of pedestrians, The repulsive force of the obstacle on the pedestrian. It refers to the interaction force between pedestrians.

[0025] Furthermore, predicting pedestrian speed includes: if there are two or more pedestrians, predicting the pedestrian speed in a group state. The expression for the pedestrian speed in a group state is:

[0026]

[0027] in, Let τ be the repulsive force exerted by the obstacle on the pedestrian, and τ be the tangential direction. For pedestrians, For the ideal speed and direction of pedestrians, β1 represents the interaction force between pedestrians, α is a parameter describing the intensity of communication between members of different groups, and α is a parameter describing the interaction force between pedestrians. i For head rotation angle, The attractiveness of the group's quality center to pedestrian i. It refers to the repulsive force among group members.

[0028] Furthermore, the expression for the time advantage is as follows:

[0029]

[0030] Where, d y v is the distance between the intersection of the pedestrian's predicted trajectory and the travel path and the pedestrian. p v is the pedestrian speed, d is the distance between the intersection of the pedestrian's predicted trajectory and the travel path and the robot, and v is the distance between the pedestrian's predicted trajectory and the travel path and the robot. v This refers to the robot's driving speed.

[0031] Furthermore, when decelerating within the driving path, this includes:

[0032] The desired velocity is calculated using the following expression:

[0033]

[0034] Among them, a cmfThe preset comfortable deceleration acceleration is given by d, where d is the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the robot, d0 is the initial value of the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the robot when decelerating in the driving path, and v0 is the initial value of the robot's driving speed when decelerating in the driving path.

[0035] The expression for deceleration acceleration is as follows:

[0036] a = -a cmf +k p (vv des )

[0037] Among them, a cmf The preset comfortable deceleration acceleration is given by v, where v is the robot's speed. des For the desired speed, k p This is a preset proportional coefficient;

[0038] Deceleration is achieved based on deceleration acceleration until the robot reaches the desired speed.

[0039] Furthermore, during an emergency stop within the driving path, this includes:

[0040] The desired velocity is calculated using the following expression:

[0041]

[0042] Where d is the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the robot, d0 is the initial value of the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the robot when the vehicle decelerates in the driving path, and v0 is the initial value of the robot's driving speed when the vehicle decelerates in the driving path.

[0043] The expression for deceleration acceleration is as follows:

[0044]

[0045] Where d is the distance between the intersection of the pedestrian's predicted trajectory and the travel path and the robot, and v is the robot's travel speed. des For the desired speed, k p This is a preset proportional coefficient;

[0046] Deceleration is achieved based on deceleration acceleration until the robot reaches the desired speed.

[0047] The present invention also proposes an adaptive obstacle avoidance path planning system, including a robot configured to perform any of the adaptive obstacle avoidance path planning methods described herein.

[0048] Compared with the prior art, the advantages of the present invention are as follows:

[0049] This invention uses a hybrid A* algorithm to generate the driving path with the fewest turns based on map information, thereby reducing the probability of the robot colliding with static obstacles during its movement.

[0050] This invention acquires real-time information about surrounding pedestrians during robot operation and adjusts the robot's speed along the path based on this information, thus effectively avoiding pedestrians.

[0051] This invention acquires new map information in real time during the robot's movement and updates the driving path accordingly, thereby effectively adapting to environmental changes. Attached Figure Description

[0052] Figure 1 This is a flowchart of a method according to an embodiment of the present invention.

[0053] Figure 2 This is a logic diagram of path planning using the hybrid A* algorithm in an embodiment of the present invention.

[0054] Figure 3 This is an example of the results of path planning using the hybrid A* algorithm.

[0055] Figure 4 This illustrates the positional relationship between the robot's edge points and static obstacles on the map in this embodiment of the invention.

[0056] Figure 5 This is an obstacle avoidance strategy designed to avoid pedestrians in an embodiment of the present invention.

[0057] Figure 6 This is a diagram illustrating the positional relationship between the robot and the pedestrian.

[0058] Figure 7 This is a logic block diagram of real-time map updates in an embodiment of the present invention.

[0059] Figure 8 This is an example of map information before and after a map update. Detailed Implementation

[0060] The present invention will be further described below with reference to the accompanying drawings and specific preferred embodiments, but this does not limit the scope of protection of the present invention.

[0061] To generate suitable walking paths within enclosed environments and effectively avoid surrounding pedestrians, this embodiment proposes an adaptive obstacle avoidance path planning method, such as... Figure 1 As shown, it includes the following steps:

[0062] S1) Obtain map information and the location information of the starting point and the ending point on the map;

[0063] S2) Using the hybrid A* algorithm, a driving path with the fewest turns is generated based on map information and the location information of the starting point and the end point, and the driving is carried out according to the driving path;

[0064] S3) Obtain real-time information on surrounding pedestrians and adjust the driving speed on the driving path according to the surrounding pedestrian information to avoid pedestrians;

[0065] S4) If the destination has not been reached, update the map information in real time, take the current position as the new starting point, and obtain the position information of the starting point and the destination on the new map. Then return to step S2. If the destination has been reached, end and exit.

[0066] Through the above steps, this embodiment first uses the hybrid A* algorithm to obtain a path that minimizes the number of turns. Then, based on this path, speed planning is performed. As the robot moves along the path at the specified speed, changes in the environment inevitably occur, causing changes in the original map. At this time, the robot updates the map information with the new information and performs real-time path planning and speed planning updates. Finally, a trajectory suitable for the robot in a closed scene is generated, which can effectively avoid pedestrians while ensuring that the robot reaches its destination.

[0067] The following is a detailed explanation of each step.

[0068] In step S1 of this embodiment, the map information specifically refers to a map constructed based on SLAM technology, which can accurately locate the starting point and the ending point on the map.

[0069] In step S2 of this embodiment, the hybrid A* algorithm is a well-known improved version of the classic A* algorithm, which adds the θ dimension to represent orientation and aims to generate a path that is more in line with vehicle dynamics. Its specific implementation steps are well known to those skilled in the art and are not the focus of this solution, so they will not be described in detail here.

[0070] To adapt to closed scenarios, this embodiment has adjusted the process of searching from parent nodes to child nodes using the hybrid A* algorithm, such as... Figure 2 As shown, step S2 includes:

[0071] S21) In the process of searching from parent node to child node in the hybrid A* algorithm, it tends to select the child node whose direction of movement does not change as the next parent node.

[0072] S22) If there is no child node whose direction of motion does not change, then select the child node whose angle of motion changes the least as the next parent node.

[0073] Therefore, when generating the driving path with the fewest turns, the process specifically includes: searching from the parent node to the child node, selecting the child node whose direction of movement does not change as the next parent node; if there is no child node whose direction of movement does not change, selecting the child node with the smallest change in movement angle among all child nodes as the next parent node.

[0074] like Figure 3 As shown, Figure 3 (a) represents the driving path generated in this embodiment. Figure 3 (b) indicates other driving paths that can be generated by the hybrid A* algorithm. In comparison, the driving path generated in this embodiment has fewer turns, which reduces the probability of the robot encountering static obstacles during driving.

[0075] In this embodiment, in order to select a suitable child node in step S21, we transform the problem of tending to select a child node whose motion direction does not change in step S21 into a penalty problem, which is reflected in the cost function of the optimal control problem. IPM is a typical internal penalty function method. Therefore, when selecting a child node whose motion direction does not change as the next parent node, the specific steps include:

[0076] The cost function for the optimal control problem is defined as follows:

[0077] J μ (a,b)=J(a)-μ IPM ·Ln(b) (1)

[0078] Where a represents the solution vector, b represents the relaxation vector, J(a) represents the cost function, Ln(b) represents the logarithmic function, and μ IPM This represents the barrier factor, whose value approaches 0.

[0079] Then, constraints are set to ensure the robot can move normally on the generated trajectory. Since the robot in this embodiment is also a four-wheel drive robot, its control method is similar to that of a vehicle. Therefore, the vehicle's kinematic equations should be used as constraints in the optimal control problem solution process. The expression for the vehicle's kinematic equations is as follows:

[0080]

[0081] Among them, L W Here, x(t) represents the wheelbase; y(t) and v(t) represent the x and y coordinates of the midpoint of the rear wheel, respectively; v(t) represents the velocity along the longitudinal axis of the vehicle body; a(t) represents the acceleration along the longitudinal axis of the vehicle body; φ(t) represents the front wheel steering angle; ω(t) represents the front wheel yaw rate; and θ(t) represents the vehicle's attitude angle in the coordinate system. For these parameters, the allowable range of action of the vehicle's internal mechanical characteristics corresponding to the state / control variables is as follows:

[0082]

[0083] in, Indicates the maximum steering angle of the front wheels, v min v represents the robot's minimum speed. max a represents the robot's maximum speed. min Let a represent the robot's minimum acceleration. max Ω represents the robot's maximum acceleration. max t represents the maximum angular velocity of the front wheel deflection. f This indicates the moment when the robot reaches its final position.

[0084] In this embodiment, the constraints also include collision avoidance constraints. To enable the robot to effectively avoid static obstacles during its movement, collision avoidance constraints need to be incorporated into the solution of the optimal control problem. Here, we describe the obstacle as a convex polygon, such as... Figure 4 As shown, the necessary and sufficient condition for point P on the robot's edge to avoid collision with the polygonal obstacle is the collision avoidance constraint, expressed as follows:

[0085]

[0086] in, Let point p and Q n The area of ​​the triangle formed by point Q1 and point Q2 Let SQ1Q2...Q be the sum of the areas of the triangles formed by point p and any two adjacent points. n For polygons Q1Q2...Q n The area.

[0087] Finally, the constraints also include the robot's initial and final positions, as well as the control constraints, expressed as follows:

[0088]

[0089] Among them, t f Let denoted as , and let lb_x, lb_y, ub_x, and ub_y be the boundaries of the termination position, respectively.

[0090] By combining equations (1) to (5), the optimal control problem can be solved to obtain the optimal driving speed and angle. The optimal driving speed and angle can be used to drive to the child node that is the next parent node.

[0091] Step S3 of this embodiment proposes a decision-making method that combines a finite state machine with a pedestrian social force model. In step S3, adjusting the driving speed along the path based on surrounding pedestrian information to avoid pedestrians specifically includes:

[0092] S31) If no pedestrian is detected, maintain the driving speed in the driving path; if a pedestrian is detected, use the pedestrian's social force model to predict the pedestrian's trajectory and obtain the predicted trajectory of the pedestrian.

[0093] The social force model expression for pedestrians is as follows:

[0094]

[0095] in, The driving force for pedestrians reflects their motivation to move in a specific direction at a certain speed. The repulsive force of the obstacle on the pedestrian. The interaction force between pedestrians, For pedestrian i's reaction to the other members of the group, specifically:

[0096] Pedestrian driving force This can be expressed by the following equation:

[0097]

[0098] Where vi represents the pedestrian's current speed. This represents the ideal speed for pedestrians. Indicates the ideal direction, v i (t) represents the current velocity relative to time, and τ represents the relaxation time.

[0099] Repulsive force of obstacles on pedestrians This can be expressed by the following equation:

[0100]

[0101] Where, d w This represents the vertical distance between the pedestrian and the wall. The repulsive force intensity corresponding to parameters a=3 and b=0.1 is on the same order of magnitude as the internal acceleration term, and the repulsive force range is about 30cm from the wall boundary.

[0102] Interaction forces between pedestrians The expression is:

[0103]

[0104] Where A represents the debugging parameter, e () Let represent an exponential function, t represent the horizontal direction, n represent the vertical direction, d represent the distance between pedestrians, θ represent the angle between pedestrians, and B represent the range of interaction.

[0105] Pedestrian i's reaction to the other members of the group The expression is:

[0106]

[0107] in, The force generated by the gaze direction and eye contact among members of a group is specifically expressed as:

[0108]

[0109] Where β1 is a parameter describing the intensity of interaction between members of the group, α i For head rotation angle, Let be the velocity vector of pedestrian i;

[0110] Let the attractiveness of the group's center of mass to pedestrian i be expressed as follows:

[0111]

[0112] Where β2 is the strength coefficient. Let q be the unit vector pointing from pedestrian i to the group centroid. When the distance between pedestrian i and the group centroid exceeds the threshold d0, q A =1, otherwise q A =0. Based on data collected under low-density conditions, the threshold d0 can be approximated as... N represents the number of pedestrians in the crowd.

[0113] The repulsive force among group members, preventing overlap between members, is expressed as:

[0114]

[0115] in Let β be the unit vector pointing from pedestrian i to member k within the group, and β3 be the repulsive force strength parameter. When the distance d between pedestrian i and the group's centroid... ik When q is less than the threshold d0 R =1 otherwise q R =0.

[0116] According to equation (6), the pedestrian's walking trajectory over a future period of time can be calculated, and the pedestrian's real-time status S can be collected. P .

[0117] S32) If the pedestrian's predicted trajectory does not intersect with the driving path, then maintain the driving speed within the driving path;

[0118] If the pedestrian's predicted trajectory intersects with the driving path, the pedestrian's speed is predicted, and the time advantage is calculated based on the pedestrian's speed, the current driving speed, and the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the distance between the pedestrian and the robot. If the time advantage is greater than a preset time advantage threshold, the driving speed is maintained in the driving path.

[0119] If the time advantage is less than the preset time advantage threshold, and the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the robot is greater than the comfortable braking distance, then the robot will decelerate in the driving path.

[0120] If the time advantage is less than the preset time advantage threshold, and the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the robot is less than the comfortable braking distance but greater than the maximum braking distance, then the robot will stop abruptly in the driving path.

[0121] If the time advantage is less than the preset time advantage threshold, and the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the robot is less than the maximum braking distance, then the robot accelerates along the driving path.

[0122] like Figure 5 As shown, this embodiment designs a finite state machine with four modes: maintaining speed, deceleration, emergency stop, and acceleration. In each mode, a different method is used to calculate the required acceleration. The transitions between modes are determined by information about surrounding pedestrians and the robot's current position, speed, acceleration, and other states. Figure 6 The horizontal dashed line represents the robot's travel path, and the vertical dashed line represents the predicted trajectory of the pedestrian. The following section combines... Figure 5 and Figure 6 These four modes will be explained in detail.

[0123] Maintaining driving speed: When the robot does not detect a pedestrian, or detects a pedestrian but the pedestrian's predicted trajectory does not intersect with the robot's driving path, or although the pedestrian's predicted trajectory does not intersect with the robot's driving path, but the time advantage t adv Exceeding the preset time advantage threshold τ max The robot will then continue moving at the required speed, and will calculate the target acceleration using a proportional speed control law, as shown in the following expression:

[0124] a = k p (v v -v des (14)

[0125] Where, k p v represents the preset scaling factor. v v is the robot's current speed. des The desired speed of the robot is the same as the preset maximum speed in the travel path.

[0126] In this embodiment, Boolean variables are used to handle pedestrian information. When a pedestrian is detected on the road, the Boolean variable PD is 1; if PD equals 0, or the time advantage t... adv Exceeding the threshold τ max The robot continues to execute the speed-maintaining mode; otherwise, the finite state machine determines which mode to enter next.

[0127] Deceleration: If the time advantage t adv If the distance is too small, the robot cannot pass directly, and the finite state machine enters deceleration mode. Considering the smoothness of travel, when the distance is sufficient, the robot will decelerate at a preset, comfortable acceleration 'a'. cmf Yielding to pedestrians, that is Figure 6 The distance d between the intersection point o of the pedestrian's predicted trajectory and the driving path and the robot is greater than the comfortable braking distance. In this mode, the robot's desired speed and required deceleration acceleration are respectively:

[0128]

[0129] The above formula represents the desired speed of the robot in deceleration mode, where a cmf The preset comfortable deceleration acceleration is given by d, where d is the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the robot, d0 is the initial value of the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the robot when decelerating in the driving path, and v0 is the initial value of the robot's driving speed when decelerating in the driving path.

[0130] a = -a cmf +k p (vv des (16)

[0131] The above formula represents the deceleration acceleration required in the deceleration mode, where a cmf The preset comfortable deceleration acceleration is given by v, where v is the robot's speed. des The desired velocity calculated by equation (15), k p This is a preset scaling factor.

[0132] After calculating the robot's desired speed and required deceleration acceleration, deceleration is performed according to the deceleration acceleration until the robot's travel speed reaches the desired speed.

[0133] Emergency stop: If Figure 6 The distance d between the intersection point o of the pedestrian's predicted trajectory and the driving path and the robot satisfies d max <d<d cmf , where d max For the maximum braking distance, a maxThe preset maximum deceleration acceleration is used; in this mode, the robot requires a greater deceleration acceleration. The robot's desired speed and required deceleration acceleration are as follows:

[0134]

[0135] Where d is the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the robot, d0 is the initial value of the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the robot when the vehicle decelerates in the driving path, and v0 is the initial value of the robot's driving speed when the vehicle decelerates in the driving path.

[0136]

[0137] Where d is the distance between the intersection of the pedestrian's predicted trajectory and the travel path and the robot, and v is the robot's travel speed. des k is the desired speed for deceleration. p This is a preset scaling factor.

[0138] After calculating the robot's desired speed and required deceleration acceleration, deceleration is performed according to the deceleration acceleration until the robot's travel speed reaches the desired speed.

[0139] Acceleration: The condition for a finite state machine to enter this mode is... Figure 6 The distance d between the intersection point o of the pedestrian's predicted trajectory and the driving path and the robot is greater than the maximum braking distance d. max In this situation, there isn't enough space for the robot to slow down and avoid pedestrians. Accelerating and quickly passing through makes more sense. The acceleration in this mode is set to the robot's own maximum acceleration, 'a'.

[0140] In step S32 of this embodiment, predicting pedestrian speed requires forecasting the elements of surrounding pedestrians based on the pedestrian's social force model. However, the social forces experienced by a pedestrian during walking are not fixed and change in real time with changes in the surrounding environment. To address this issue, this embodiment summarizes the pedestrian's speed v under the social force state. p The expression that changes with time, thus deriving t. adv An expression that changes over time.

[0141] Pedestrian speed v p There are two main forms: the first is the single-pedestrian state, and the second is the group state. Predicting pedestrian speed includes: if the pedestrian is alone, predicting the pedestrian speed in the single-pedestrian state; the expression for the pedestrian speed in the single-pedestrian state is:

[0142]

[0143] in, Let τ be the repulsive force exerted by the obstacle on the pedestrian, and τ be the tangential direction. For pedestrians, For the ideal direction of pedestrians, The interaction force between pedestrians;

[0144] If there are two or more pedestrians, the predicted pedestrian speed in a group is expressed as follows:

[0145]

[0146] in, Let τ be the repulsive force exerted by the obstacle on the pedestrian, and τ be the tangential direction. For pedestrians, For the ideal speed and direction of pedestrians, β1 represents the interaction force between pedestrians, α is a parameter describing the intensity of communication between members of different groups, and α is a parameter describing the interaction force between pedestrians. i For head rotation angle, The attractiveness of the group's quality center to pedestrian i. It refers to the repulsive force among group members.

[0147] Furthermore, the expression for the time advantage is as follows:

[0148]

[0149] Where, d y v is the distance between the intersection of the pedestrian's predicted trajectory and the travel path and the pedestrian. p The pedestrian speed is calculated using equation (19) or equation (20), d is the distance between the intersection of the pedestrian's predicted trajectory and the travel path and the robot, and v is the distance between the robot and the intersection of the pedestrian's predicted trajectory and the travel path. v This refers to the robot's driving speed.

[0150] like Figure 7 As shown, in this embodiment, through step S4, the map information is updated in real time while the robot moves along the driving path. This allows the robot to update its driving path according to the new map information, thereby ensuring that the robot can adapt to changing enclosed indoor scenarios, such as changing the position of shelves in a warehouse, changing the position of tables and chairs in a restaurant, or changing the position of display stands in a shopping mall. Step S4, which involves updating the map information in real time, specifically includes: obtaining the position information of the target object in the scene; if the position information of the target object changes, updating the map information according to the changed position information; if the position information of the target object remains unchanged, no path planning update is needed, and the robot continues to drive along the original driving path, and then proceeds to step S3.

[0151] In this embodiment, since the scenario is indoors, guide lines need to be laid on the floor, QR codes need to be placed on the walls, and wireless positioning devices need to be placed on each shelf or table. The robot's built-in sensors will use these wireless positioning devices and QR codes as target objects and interact with them to allow the robot to update the map in real time and update its own position on the updated map. Figure 8 As shown in the diagram, the squares represent restaurant tables. Figure 8 (a) represents the original map information of the restaurant and the driving path recorded by the robot. According to the original information, after the robot reaches point A, it needs to follow... Figure 8 The solid line in (a) indicates the robot's path to point B. However, after reaching point A, the robot obtains real-time location information of all tables in the restaurant and discovers that the three tables in the first row have been pieced together, thus creating an updated map. Figure 8 As shown in (b), subsequent step S2 is executed to obtain... Figure 8 (b) shows the new driving path to point B as indicated by the solid line, thus enabling real-time updates of map information and driving path.

[0152] This embodiment also proposes an adaptive obstacle avoidance path planning system, including a robot configured to execute the adaptive obstacle avoidance path planning method proposed in this embodiment, comprising the following steps:

[0153] Obtain map information, including the location information of the starting point and the destination on the map;

[0154] Using a hybrid A* algorithm, a driving path with the fewest turns is generated based on map information and the location information of the starting and ending points, and the driving is carried out according to the driving path.

[0155] It can obtain real-time information on surrounding pedestrians and adjust its driving speed along the route to avoid pedestrians based on this information.

[0156] If the destination is not reached, the map information is updated in real time, the current location is used as the new starting point, and the location information of the starting point and the destination on the new map is obtained. Then, the hybrid A* algorithm is used to generate the driving path with the fewest turns based on the map information and the location information of the starting point and the destination.

[0157] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention in any way. Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the invention. Therefore, any simple modifications, equivalent changes, and alterations made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention should fall within the protection scope of the present invention.

Claims

1. An adaptive obstacle avoidance path planning method, characterized in that, Includes the following steps: Obtain map information, including the location information of the starting point and the destination on the map; Using a hybrid A* algorithm, a driving path with the fewest turns is generated based on map information and the location information of the starting and ending points, and the driving is carried out according to the driving path. Real-time acquisition of surrounding pedestrian information, and adjustment of driving speed along the route based on this information to avoid pedestrians, specifically including: If a pedestrian is detected, the pedestrian's trajectory is predicted using the pedestrian's social force model. If the pedestrian's predicted trajectory does not intersect with the driving path, then the driving speed is maintained within the driving path; If the pedestrian's predicted trajectory intersects with the driving path, the pedestrian's speed is predicted, and the time advantage is calculated based on the pedestrian's speed, the current driving speed, and the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the distance between the pedestrian and the robot. If the time advantage is greater than a preset time advantage threshold, the driving speed is maintained in the driving path. If the time advantage is less than the preset time advantage threshold, and the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the robot is greater than the comfortable braking distance, then the robot will decelerate in the driving path. If the time advantage is less than the preset time advantage threshold, and the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the robot is less than the comfortable braking distance but greater than the maximum braking distance, then the robot will stop abruptly in the driving path. If the time advantage is less than the preset time advantage threshold, and the distance between the intersection of the pedestrian's predicted trajectory and the driving path and the robot is less than the maximum braking distance, then the robot accelerates in the driving path. If the destination is not reached, the map information is updated in real time, the current location is used as the new starting point, and the location information of the starting point and the destination on the new map is obtained. Then, the hybrid A* algorithm is used to generate the driving path with the fewest turns based on the map information and the location information of the starting point and the destination.

2. The adaptive obstacle avoidance path planning method according to claim 1, characterized in that, When generating the driving path with the fewest turns, the process includes: searching from the parent node to the child node, selecting the child node whose direction of movement does not change as the next parent node; if there is no child node whose direction of movement does not change, selecting the child node with the smallest change in movement angle among all child nodes as the next parent node.

3. The adaptive obstacle avoidance path planning method according to claim 1, characterized in that, When selecting a child node whose direction of motion does not change as the next parent node, the specific steps include: setting a cost function for the optimal control problem and setting constraints, including the vehicle kinematic equations, the allowable range of state / control variables corresponding to the vehicle's inherent mechanical characteristics, collision avoidance constraints, and state and control constraints of the robot's initial and final positions. The cost function and constraints are solved simultaneously to obtain the optimal driving speed and angle.

4. The adaptive obstacle avoidance path planning method according to claim 1, characterized in that, Predicting pedestrian speed includes: predicting the pedestrian speed in a single-person state, where the expression for the pedestrian speed in a single-person state is: in, Tangent direction For pedestrians, For the ideal direction of pedestrians, The repulsive force of the obstacle on the pedestrian. It refers to the interaction force between pedestrians.

5. The adaptive obstacle avoidance path planning method according to claim 1, characterized in that, Predicting pedestrian speed includes: if there are two or more pedestrians, predicting the pedestrian speed in a group state. The expression for the pedestrian speed in a group state is: in, The repulsive force of the obstacle on the pedestrian. Tangent direction For pedestrians, For the ideal speed and direction of pedestrians, The interaction force between pedestrians, A parameter describing the intensity of interaction between members of a group. For head rotation angle, The attractiveness of the group's quality center to pedestrian i. It refers to the repulsive force among group members.

6. The adaptive obstacle avoidance path planning method according to claim 1, characterized in that, The expression for the time advantage is as follows: in, The distance between the intersection of the pedestrian's predicted trajectory and the travel path and the pedestrian. For pedestrian speed, The distance between the intersection of the pedestrian's predicted trajectory and the travel path and the robot. This refers to the robot's driving speed.

7. The adaptive obstacle avoidance path planning method according to claim 1, characterized in that, When slowing down while driving along the route, this includes: The desired velocity is calculated using the following expression: in, For preset comfortable deceleration and acceleration, The distance between the intersection of the pedestrian's predicted trajectory and the travel path and the robot. The initial value is the distance between the intersection of the pedestrian's predicted trajectory and the path and the robot when the pedestrian decelerates along the driving path. This is the initial value of the robot's speed when decelerating along the travel path; The expression for deceleration acceleration is as follows: in, For preset comfortable deceleration and acceleration, For the robot's driving speed, For the desired speed, This is a preset proportionality coefficient; Deceleration is achieved based on deceleration acceleration until the robot reaches the desired speed.

8. The adaptive obstacle avoidance path planning method according to claim 1, characterized in that, When stopping suddenly in the driving path, including: The desired velocity is calculated using the following expression: in, The distance between the intersection of the pedestrian's predicted trajectory and the travel path and the robot. The initial value is the distance between the intersection of the pedestrian's predicted trajectory and the path and the robot when the pedestrian decelerates along the driving path. This is the initial value of the robot's speed when decelerating along the travel path; The expression for deceleration acceleration is as follows: in, The distance between the intersection of the pedestrian's predicted trajectory and the travel path and the robot. For the robot's driving speed, For the desired speed, This is a preset proportionality coefficient; Deceleration is achieved based on deceleration acceleration until the robot reaches the desired speed.

9. An adaptive obstacle avoidance path planning system, characterized in that, The invention includes a robot configured to perform the adaptive obstacle avoidance path planning method according to any one of claims 1 to 8.

Images