A Method and System for Pedestrian Safety Navigation Based on Spatiotemporal Constrained Velocity Obstacles

Abstract

The present invention relates to the technical field of autonomous navigation of mobile robots and pedestrian collision avoidance, and provides a safe pedestrian navigation method and system based on spatio-temporal constrained velocity obstacles. The method includes: obtaining the motion state of pedestrians according to laser information; constructing a velocity obstacle region in the velocity space according to the motion state of pedestrians; adaptively adjusting the time factor according to the pedestrian density, and adjusting the velocity obstacle region according to the time factor, and performing velocity sampling by using the velocity obstacle method under time constraint according to the velocity obstacle region under time constraint, and judging whether to perform velocity resampling through a velocity resampling criterion; performing integral processing on the sampled velocity to generate a candidate trajectory, and designing an evaluation function to select the optimal trajectory. The present invention uses the spatio-temporal constrained velocity obstacle method to guide the robot to generate a safe velocity, and improves the safety and motion efficiency of the robot navigation by designing an adaptively adjustable time factor and a velocity resampling mechanism.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous navigation of mobile robots and pedestrian collision avoidance, and particularly to a safe navigation method and system for pedestrians based on spatio-temporal constrained velocity obstacles. Background Art

[0002] For indoor service robots, the navigation problem in a pedestrian environment has always been a hot research direction. The main problem that autonomous navigation of robots solves is that the robot autonomously plans a feasible path from the starting point to the target point. During the movement process, it is necessary to satisfy the kinematic and dynamic constraints of the robot itself and ensure that the robot does not collide with obstacles in the environment. The motion planning algorithms in current mobile robot autonomous navigation methods mainly include two categories: sampling-based motion planning algorithms and optimization-based motion planning algorithms. Among them, the sampling-based planning algorithm means sampling from the control space or state space of the robot to obtain a set of candidate velocities or paths, and selecting the optimal velocity or path among these candidate points through designing an evaluation function for tracking. The optimization-based planning algorithm means directly modeling the motion planning problem of the robot as a non-linear optimization problem for solution, obtaining a soft-constrained optimization problem by putting the constraint quantity as a penalty term into the optimization objective; or modeling the planning problem as a constrained non-linear hard optimization problem.

[0003] Although the current navigation methods of mobile robots effectively solve the real-time planning challenges in complex environments, there is still room for improvement in detecting and avoiding dynamic problems. Especially in a complex environment with multiple dynamic pedestrians, the navigation of mobile robots still faces major challenges. How to balance the safety of collision avoidance and the efficiency of motion planning, so as to achieve flexible navigation in a pedestrian-rich environment is a crucial issue.

[0004] Some studies only consider planning a smooth and efficient motion trajectory in a static complex environment, but fail due to lack of foresight in avoiding collisions with dynamic pedestrians. Other studies prospectively avoid dynamic pedestrians by adding parameters affecting the motion state of dynamic obstacles to the trajectory evaluation. However, this method requires manual parameter tuning, is difficult to balance safety and planning efficiency, and has poor effects in crowded pedestrian scenarios. Summary of the Invention

[0005] The present invention aims to solve at least one of the technical problems existing in the related art. For this purpose, the present invention provides a safe navigation method and system for pedestrians based on spatio-temporal constrained velocity obstacles, which realizes that during the navigation process of a mobile robot among pedestrians, the spatio-temporal constrained velocity obstacle method is used to guide the robot to generate a safe velocity, and the safety and motion efficiency of robot navigation are improved by designing an adaptively adjusted time factor and a velocity resampling mechanism.

[0006] The present invention provides a pedestrian safety navigation method based on spatio-temporal constrained velocity obstacles, including:

[0007] S1: Detect the position and size information of pedestrians in the environment according to laser information, and obtain the motion state of the pedestrians;

[0008] S2: Construct a velocity obstacle region in the velocity space according to the motion state of the pedestrians and the position of the robot;

[0009] S3: Adaptively adjust the time factor according to the pedestrian density, and adjust the velocity obstacle region according to the time factor to obtain the velocity obstacle region under time constraint;

[0010] S4: Perform velocity sampling using the velocity obstacle method under time constraint according to the velocity obstacle region under time constraint to obtain the sampled velocity;

[0011] S5: When the pedestrians in the environment are dense, judge whether the robot is in a trapped area through the velocity resampling criterion. If it is in the trapped area, perform velocity resampling using the velocity obstacle method under time constraint to obtain the resampled velocity;

[0012] S6: Integrate the sampled velocity and the resampled velocity to generate a candidate trajectory. Design an evaluation function according to the candidate trajectory, the minimum distance from the pedestrians, and the robot velocity, and select the optimal trajectory according to the evaluation function.

[0013] According to the pedestrian safety navigation method based on spatio-temporal constrained velocity obstacles provided by the present invention, in step S2, the center of the circle of the robot is set as the origin, and the edge of the pedestrian O is extended outward by a radius of distance to obtain the velocity obstacle region. The calculation expression of the velocity obstacle region is:

[0014]

[0015] where, is the velocity obstacle region, is the center of the circle of the robot, is the velocity of the robot, is the velocity of the pedestrian, is the region of the obstacle after expanding according to the radius of the robot, is from along the ray extending in the direction.

[0016] According to the pedestrian safety navigation method based on spatio-temporal constrained velocity obstacles provided by the present invention, in step S3, the calculation expression of the time factor is:

[0017]

[0018] Among them, is the time factor, is the number of dynamic pedestrians within the perception range.

[0019] According to a method for safe navigation among pedestrians based on spatio-temporal constrained velocity obstacles provided by the present invention, it further includes adjusting the velocity obstacle area according to the time factor, and the adjusted velocity obstacle area is:

[0020]

[0021] Among them, is the adjusted velocity obstacle area, is the center of the robot, is the velocity of the robot, is the velocity of the pedestrian, is the area of the obstacle after expanding according to the radius of the robot, is the time factor inside along the ray extending in the direction;

[0022] Translate and adjust the velocity obstacle area along the velocity direction of the pedestrian to obtain the absolute velocity obstacle area caused by the pedestrian within the time factor , and denote the set of all absolute velocity obstacle areas generated by dynamic pedestrians in the environment as the velocity obstacle area under time constraint. The calculation expression is: Among them,

[0023]

[0024] Among them, is the velocity obstacle area under time constraint, is the pedestrian within the time factor the absolute velocity obstacle area caused, is the number of dynamic pedestrians within the perception range.

[0025] According to a method for safe navigation among pedestrians based on spatio-temporal constrained velocity obstacles provided by the present invention, it further includes sampling the velocity range that satisfies the time constraint and is outside the velocity obstacle area, and using the velocity obstacle method under time constraint for velocity sampling. The velocity obstacle method under time constraint is: within the velocity obstacle area under time constraint, combined with the kinematic and dynamic constraints of the robot, velocity sampling is performed within the safe velocity range. Under the maximum acceleration constraint, the sampling velocity is:

[0026]

[0027] Among them, is the sampling speed, is the sampling linear speed, is the sampling angular speed, is the current linear speed of the robot, is the current angular speed of the robot, is the minimum linear acceleration, is the maximum linear acceleration, is the minimum angular acceleration, is the maximum angular acceleration, is the speed sampling period;

[0028] Ensure that the robot can stop with maximum deceleration before collision, and the safe speed is:

[0029]

[0030] where, is the safe speed, is with the sampling linear speed and the sampling angular speed the closest distance to the pedestrian on the corresponding trajectory;

[0031] The set of safe sampling speeds of the robot is:

[0032]

[0033] where, is the set of safe sampling speeds of the robot, is the constraint of the linear speed and angular speed of the robot, is the speed range outside the speed obstacle area under the time constraint.

[0034] According to a method for safe navigation among pedestrians based on spatio-temporal constraint speed obstacle provided by the present invention, it further includes that when pedestrians are dense in the environment, the robot needs to expand the sampling range of the speed for speed resampling, so that the robot can escape from the trapped area with a greater linear speed or angular speed to avoid collision.

[0035] According to a method for safe navigation among pedestrians based on spatio-temporal constraint speed obstacle provided by the present invention, it further includes that the speed resampling criterion is:

[0036]

[0037] where, is the center of the circle of the robot, is the speed of the pedestrian, is the area of the pedestrian after expanding according to the radius of the robot, time factor from within along the ray extending in the direction.

[0038] A method for pedestrian safety navigation based on spatio-temporal constrained velocity obstacles provided by the present invention further includes integrating the sampled velocity to generate a candidate trajectory, and the calculation expression of the candidate trajectory is:

[0039]

[0040] where is the position in the direction of the candidate trajectory is the position in the direction of the candidate trajectory is the candidate trajectory is the position in the direction of the candidate trajectory is the candidate trajectory orientation angle, is the current position of the robot in the direction is the current position of the robot in the direction is the current orientation angle of the robot, is the time, is the sampled linear velocity, is the sampled angular velocity.

[0041] A method for pedestrian safety navigation based on spatio-temporal constrained velocity obstacles provided by the present invention further includes that the evaluation function is:

[0042]

[0043] where is the evaluation function, is the trajectory direction, is the closest distance to the pedestrian on the trajectory corresponding to the sampled linear velocity and the sampled angular velocity is the robot velocity, is the trajectory coefficient, is the distance coefficient, is the velocity coefficient; is the velocity coefficient;

[0044] According to the evaluation function, an optimal trajectory is selected, and the velocity corresponding to the optimal trajectory is the velocity of the robot.

[0045] The present invention also provides a pedestrian safety navigation system based on spatio-temporal constrained velocity obstacles for executing a method for pedestrian safety navigation based on spatio-temporal constrained velocity obstacles as described in any one of the above, including:

[0046] An acquisition module, which detects the position and size information of pedestrians in the environment according to laser information and obtains the motion state of the pedestrians;

[0047] A construction module, which constructs a velocity obstacle region in the velocity space according to the motion state of the pedestrians;

[0048] An adjustment module, which adaptively adjusts a time factor according to the pedestrian density and adjusts a speed obstacle area according to the time factor to obtain a speed obstacle area under time constraint;

[0049] A sampling module, which performs speed sampling using a speed obstacle method under time constraint according to the speed obstacle area under time constraint to obtain a sampled speed;

[0050] A resampling module, which performs speed resampling using a speed obstacle method under time constraint when it is determined by a speed resampling criterion that the robot is in a trapped area;

[0051] A selection module, which performs integral processing on the sampled speed and the resampled speed to generate a candidate trajectory, designs an evaluation function according to the candidate trajectory, the minimum distance from a pedestrian, and the robot speed, and selects an optimal trajectory according to the evaluation function.

[0052] One or more of the above technical solutions in the embodiments of the present invention have at least one of the following technical effects:

[0053] The present invention uses a spatio-temporal constrained speed obstacle method to guide a robot to generate a safe speed, adaptively adjusts a time factor according to the number of pedestrians, and flexibly adjusts the urgency of collision avoidance, meeting the needs of the urgency of collision avoidance in different scenarios; the speed resampling mechanism prevents the robot from being trapped in a speed obstacle area without a safe speed, improving the safety and motion efficiency of robot navigation.

[0054] Additional aspects and advantages of the present invention will be given in part in the following description, become apparent in part from the following description, or be understood through the practice of the present invention. Description of the Drawings

[0055] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the following drawings are some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0056] Figure 1 is a schematic flowchart of a method for safe navigation among pedestrians based on spatio-temporal constrained speed obstacles provided by the present invention.

[0057] Figure 2 is a schematic structural diagram of a system for safe navigation among pedestrians based on spatio-temporal constrained speed obstacles provided by the present invention.

[0058] Reference Signs:

[0059] 101. Acquisition module; 102. Construction module; 103. Adjustment module; 104. Sampling module; 105. Resampling module; 106. Selection module. Detailed implementation manners

[0060] To make the objectives, technical solutions and advantages of the present invention clearer, the technical solutions in the present invention will be clearly and completely described below. Apparently, the described embodiments are some but not all of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art without making creative efforts based on the embodiments in the present invention belong to the scope of protection of the present invention. The following embodiments are used to illustrate the present invention but cannot be used to limit the scope of the present invention.

[0061] In the description of this specification, the descriptions referring to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples", etc. mean that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the embodiments of the present invention. In this specification, the schematic descriptions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described can be combined in a suitable manner in any one or more embodiments or examples. In addition, without conflict, those skilled in the art can combine the different embodiments or examples described in this specification and the features of the different embodiments or examples.

[0062] The following Figures 1 to 2 describes a pedestrian safety navigation method and system based on spatio-temporal constrained velocity obstacles of the present invention.

[0063] As Figure 1 shown, a pedestrian safety navigation method based on spatio-temporal constrained velocity obstacles includes:

[0064] S1: According to laser information, detect the position and size information of pedestrians in the environment, and obtain the motion state of pedestrians;

[0065] The motion state of pedestrians is defined as:

[0066]

[0067] Wherein, is the motion state of pedestrians, is the position of pedestrians in the direction, is the velocity of pedestrians in the direction, is the position of pedestrians in the direction, The speed of the pedestrian in the direction, is the radius of the pedestrian, is the change rate of the pedestrian radius, is the transpose.

[0068] S2: Construct a velocity obstacle region in the velocity space according to the motion state of the pedestrian and the position of the robot;

[0069] Let be the robot located at , with a radius of , be the pedestrian located at , with a radius of ; The sum of the Minkowski distances of the two objects and is:

[0070]

[0071] where is the Minkowski distance of the robot , is the Minkowski distance of the pedestrian,

[0072] Set the center of the robot as the origin, and expand the edge of the pedestrian outward by a radius of distance. Thus, the original dynamic obstacle avoidance scenario of the robot is transformed into a collision avoidance problem of a point mass robot moving towards an obstacle with a radius of . The area of the obstacle after expanding according to the robot radius is:

[0073]

[0074] Define the ray extending from the point along the direction as:

[0075]

[0076] where is the ray extending from the point along the direction, is the position of the robot, is the length coefficient, is the speed of the robot;

[0077] The area swept by the ray is the velocity obstacle region generated by the dynamic pedestrian . The calculation expression of the velocity obstacle region is:

[0078]

[0079] Among them, is the speed obstacle area, is the center of the robot, is the speed of the robot, is the speed of the obstacle, is the area of the obstacle after expanding according to the robot radius, is from along direction extending ray.

[0080] S3: Adaptively adjust the time factor according to the pedestrian density, and adjust the speed obstacle area according to the time factor to obtain the speed obstacle area under time constraint;

[0081] The time factor adaptively adjusted according to the pedestrian density is used to adjust the size of the speed obstacle area, so as to adjust the collision urgency;

[0082] Within the time factor range from the point start along direction extending ray is:

[0083]

[0084] When is small, let and intersecting speed is larger, that is, the potential collision speed of the robot is larger. Therefore, some smaller speeds are actually feasible at the current moment and should not be directly excluded;

[0085] For different complex scenarios, the collision avoidance urgency is also different. The time factor essentially reflects the obstacle avoidance urgency of the robot and is adjusted online according to real-time perception information;

[0086] When there are dense pedestrians around the robot, the probability that pedestrians maintain their current motion state is relatively low. A smaller value relaxes the constraint on speed collision at a distant future moment, while enhancing the urgency of avoiding collision at the current moment; Therefore, the smaller the value, the larger the safe speed allowed by the robot; On the contrary, the larger the value, the lower the urgency of collision avoidance, and the smaller the safe speed allowed by the robot; The higher the dynamic degree of pedestrians in the environment, the more complex and irregular their motion states are. Then, in a scenario with denser pedestrians, a smaller , the calculation expression of the time factor is:

[0087]

[0088] Among them, is the time factor, is the number of dynamic pedestrians within the sensing range;

[0089] When there are no dynamic pedestrians around the robot ( ), the robot will not generate a speed obstacle area. Therefore, the number of dynamic pedestrians is greater than zero, that is ,

[0090] In some embodiments of the present invention, the value range of the time factor is .

[0091] By introducing the time factor, the traditional speed obstacle area can be further reduced. The speed obstacle area is adjusted according to the time factor, and the adjusted speed obstacle area is:

[0092]

[0093] Among them, is the adjusted speed obstacle area, is the center of the circle of the robot, is the speed of the robot, is the speed of the obstacle, is the area of the obstacle expanded according to the radius of the robot, is the time factor inside along direction extending ray;

[0094] Translate the speed obstacle area along the speed direction of the obstacle to obtain the pedestrian in the time factor inside the absolute speed obstacle area , and denote the set of all absolute speed obstacle areas generated by dynamic pedestrians in the environment as the speed obstacle area under time constraint. The calculation expression is:

[0095]

[0096] Among them, is the speed obstacle area under time constraint, is the pedestrian in the time factor inside the absolute speed obstacle area caused by, is the number of dynamic pedestrians within the sensing range;

[0097] The number of dynamic pedestrians within the sensing range is the number of obstacles.

[0098] S4: According to the velocity obstacle region under time constraint, use the velocity obstacle method under time constraint to perform velocity sampling to obtain the sampled velocity;

[0099] Only the velocity range that meets the time constraint and is outside the velocity obstacle region can be sampled, and the velocity obstacle method under time constraint is used for velocity sampling;

[0100] Perform velocity sampling at a safe velocity;

[0101] In the velocity obstacle region under time constraint, combined with the kinematic and dynamic constraints of the robot, perform velocity sampling within a safe velocity range.

[0102] The constraints on the linear velocity and angular velocity of the robot are:

[0103]

[0104] Among them, are the constraints on the linear velocity and angular velocity of the robot, is the linear velocity of the robot, is the minimum linear velocity reached by the robot, is the maximum linear velocity reached by the robot, is the angular velocity of the robot, is the minimum angular velocity reached by the robot, is the maximum angular velocity reached by the robot.

[0105] Under the maximum acceleration constraint, the sampled velocity is:

[0106]

[0107] Among them, is the sampled velocity, is the sampled linear velocity, is the sampled angular velocity, is the current linear velocity of the robot, is the current angular velocity of the robot, is the minimum linear acceleration, is the maximum linear acceleration, is the minimum angular acceleration, is the maximum angular acceleration, is the velocity sampling period;

[0108] Ensure that the robot can stop with maximum deceleration before collision, and the safe velocity is:

[0109]

[0110] Among them, is the safe velocity, at the sampling linear velocity and the sampling angular velocity the closest distance to the pedestrian on the corresponding trajectory;

[0111] Considering that there are dynamic pedestrians in the environment, the speed outside the velocity obstacle region is selected. Therefore, the robot's safe sampling speed set is:

[0112]

[0113] wherein, is the robot's safe sampling speed set, are the linear velocity and angular velocity constraints of the robot, is the speed range outside the velocity obstacle region under the time constraint.

[0114] S5: When pedestrians are dense in the environment, judge whether the robot is in a trapped area through the speed resampling criterion. If it is in a trapped area, use the velocity obstacle method under the time constraint to perform speed resampling to obtain the resampled speed;

[0115] When pedestrians are dense in the environment, the velocity obstacle region in the velocity space will also be larger. At this time, the robot may enter a trapped state because there is no safe speed and cannot avoid pedestrians. Therefore, the robot needs to expand the sampling range of the speed to perform speed resampling, so that the robot can escape from the trapped area at a larger linear velocity or angular velocity and avoid collisions.

[0116] The velocity obstacle region under the time constraint can exclude most unsafe speeds. However, when the robot is trapped in the velocity obstacle region and there is no feasible speed option, some collisions still cannot be avoided.

[0117] To ensure the calculation efficiency and prevent the robot from making sharp turns, the angular velocity of the robot is generally sampled within a small range, , however, when the position of the robot is within the velocity collision region and there is no safe speed sampling, it means that the robot is in a trapped area. Therefore, it is necessary to expand the sampling range of the angular velocity, , perform speed resampling, so that the robot can escape from the trapped state at a larger angular velocity and avoid collisions;

[0118] The speed resampling criterion is:

[0119]

[0120] wherein, is the center of the robot, is the speed of the obstacle, is the region of the obstacle after expanding according to the robot radius, is the time factor Inner from Ray extending along direction.

[0121] S6: Integrate the sampling speed and the resampling speed to generate a candidate trajectory. Design an evaluation function based on the candidate trajectory, the minimum distance from the pedestrian, and the robot speed, and select the optimal trajectory according to the evaluation function;

[0122] Integrate the sampled speed to generate a candidate trajectory. The calculation expression of the candidate trajectory is:

[0123]

[0124] Wherein, is the position in the direction of the candidate trajectory, is the position in the direction of the candidate trajectory, is the orientation angle of the candidate trajectory, is the current position of the robot in the direction, is the current position of the robot in the direction, is the current orientation angle of the robot, is the time,

[0125] The evaluation function is:

[0126]

[0127] Wherein, is the evaluation function, is the trajectory direction, at the sampling linear velocity and the sampling angular velocity the closest distance from the pedestrian on the corresponding trajectory, is the robot speed term, is the trajectory coefficient, is the distance coefficient, is the speed coefficient;

[0128] According to the evaluation function, select an optimal trajectory. The speed corresponding to the optimal trajectory is the speed of the robot.

[0129] As Figure 2 shown, a pedestrian safety navigation system based on spatio-temporal constrained velocity obstacles includes:

[0130] The acquisition module 101 detects the position and size information of pedestrians in the environment according to the laser information, and obtains the motion state of the pedestrians;

[0131] The building block 102 constructs a velocity obstacle region in the velocity space according to the motion state of the pedestrian and the position of the robot;

[0132] The adjustment module 103 adaptively adjusts the time factor according to the pedestrian density, and adjusts the velocity obstacle region according to the time factor to obtain the velocity obstacle region under time constraint;

[0133] The sampling module 104 performs velocity sampling using the velocity obstacle method under time constraint according to the velocity obstacle region under time constraint to obtain the sampled velocity;

[0134] When the pedestrians are dense in the environment, the resampling module 105 determines whether the robot is in a trapped area through the velocity resampling criterion. If it is in the trapped area, it performs velocity resampling using the velocity obstacle method under time constraint to obtain the resampled velocity;

[0135] The selection module 106 performs integral processing on the sampled velocity and the resampled velocity to generate candidate trajectories. According to the candidate trajectories, the minimum distance from the pedestrians, and the robot velocity, an evaluation function is designed, and the optimal trajectory is selected according to the evaluation function.

[0136] Through the collaborative work of the above modules, the needs of different scenarios for collision avoidance urgency are met, and the motion planning efficiency of the robot navigation system and the success rate of pedestrian collision avoidance have been greatly improved.

[0137] The present invention uses the spatio-temporal constrained velocity obstacle method to guide the robot to generate a safe velocity. Different from the traditional velocity obstacle method, the present invention flexibly adjusts the collision avoidance urgency according to the complexity of different scenarios. Among them, the purpose of time constraint is to adjust the urgency of collision avoidance by specifying the time range to be considered for collision avoidance, and the purpose of space constraint is to consider the positional relationship between the robot and the velocity obstacle region, and further design a velocity resampling mechanism to prevent the robot from being trapped. The present invention has been greatly improved in terms of motion planning efficiency and the success rate of pedestrian collision avoidance compared with the existing methods.

[0138] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements for some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A pedestrian safety navigation method based on time-space constraint speed obstacles, characterized in that: include: S1: According to the laser information, the position and size information of pedestrians in the environment are detected to obtain the movement state of pedestrians; S2: Construct a speed obstacle area in the speed space according to the motion state of the pedestrian and the position of the robot; S3: adaptively adjust the time factor according to the pedestrian density, and adjust the speed barrier area according to the time factor to obtain the speed barrier area under the time constraint; The calculation expression of the time factor is: in, is the time factor, is the number of dynamic pedestrians within the sensing range; S4: according to the speed obstacle area under the time constraint, the speed obstacle method under the time constraint is used to perform speed sampling to obtain the sampling speed; S5: When the environment is crowded with pedestrians, the speed resampling criterion is used to determine whether the robot is in a trapped area. If it is in a trapped area, the speed obstacle method under time constraints is used to perform speed resampling to obtain the resampled speed; S6: Integrate the sampling speed and the resampling speed to generate candidate trajectories, design an evaluation function based on the candidate trajectories, the minimum distance from the pedestrian, and the robot speed, and select the optimal trajectory based on the evaluation function.

2. According to claim 1, a pedestrian safe navigation method based on time-space constraint speed barrier is characterized in that: In step S2, the center of the robot is Set it as the origin and expand the radius of the edge of pedestrian O outward The speed barrier area is obtained by calculating the distance. The calculation expression of the speed barrier area is: in, The speed barrier area. is the center of the robot, is the speed of the robot, is the speed of pedestrians, is the area of ​​obstacles after the robot radius is expanded, For along Direction of rays extending.

3. According to claim 1, a pedestrian safe navigation method based on time-space constraint speed barrier is characterized in that: The speed barrier area is adjusted according to the time factor. The adjusted speed barrier area is: in, is the speed barrier area after adjustment. is the center of the robot, is the speed of the robot, is the speed of pedestrians, is the area of ​​obstacles after the robot radius is expanded, Time factor Inside along Directionally extending rays; Adjust the speed barrier area along the direction of the pedestrian's speed to obtain the pedestrian In time factor Absolute speed obstacle area caused by , the set of all absolute speed obstacle areas generated by dynamic pedestrians in the environment is recorded as the speed obstacle area under time constraint, and the calculation expression is: in, is the speed obstacle area under time constraint, For pedestrians In time factor The absolute speed obstacle area caused by is the number of dynamic pedestrians within the sensing range.

4. According to claim 1, a pedestrian safe navigation method based on time-space constraint speed barrier is characterized in that: The speed range that meets the time constraint and is outside the speed obstacle area is sampled. The speed obstacle method under the time constraint is used for speed sampling. The speed obstacle method under the time constraint is: in the speed obstacle area under the time constraint, the robot kinematics and dynamic constraints are combined to perform speed sampling within a safe speed range. Under the maximum acceleration constraint, the sampling speed is: in, is the sampling speed, is the sampling linear velocity, is the sampling angular velocity, is the current linear velocity of the robot, is the current angular velocity of the robot, is the minimum linear acceleration, is the maximum linear acceleration, is the minimum angular acceleration, is the maximum angular acceleration, is the speed sampling period; Ensure that the robot can stop at the maximum deceleration before collision. The safe speed is: in, For safe speed, The sampling line speed and the sampling angular velocity The closest distance to the pedestrian on the corresponding trajectory; The robot safety sampling speed set is: in, The robot safety sampling speed set, are the robot linear velocity and angular velocity constraints, It is the speed range outside the speed obstacle area under time constraint.

5. According to claim 1, a pedestrian safe navigation method based on time-space constraint speed barrier is characterized in that: When there are dense pedestrians in the environment, the robot needs to expand the speed sampling range and perform speed resampling so that the robot can escape from the trapped area at a higher linear speed or angular velocity to avoid collision.

6. The pedestrian safe navigation method based on time-space constraint speed barrier according to claim 1, characterized in that: The velocity resampling criterion is: in, is the center of the robot, is the speed of pedestrians, To expand the area of ​​pedestrians according to the robot radius, Time factor Internal along Direction of rays extending.

7. The pedestrian safe navigation method based on time-space constraint speed barrier according to claim 1, characterized in that: The sampled velocity is integrated to generate candidate trajectories. The calculation expression of the candidate trajectory is: in, Candidate trajectory The position of the direction, Candidate trajectory The position of the direction, is the candidate trajectory heading angle, For the robot's current The position of the direction, For the robot's current The position of the direction, is the current orientation angle of the robot, For time, is the sampling linear velocity, is the sampling angular velocity.

8. A pedestrian safety navigation system based on time-space constraint speed barriers, characterized in that: A method for performing a pedestrian safety navigation method based on a time-space constrained speed barrier as claimed in any one of claims 1 to 7, comprising: An acquisition module, wherein the acquisition module detects the position and size information of pedestrians in the environment according to the laser information, and obtains the movement state of the pedestrians; A construction module, wherein the construction module constructs a speed obstacle area in a speed space according to a motion state of the pedestrian and a position of the robot; An adjustment module, wherein the adjustment module adaptively adjusts the time factor according to the pedestrian density, and adjusts the speed barrier area according to the time factor to obtain the speed barrier area under the time constraint; A sampling module, wherein the sampling module performs speed sampling according to a speed obstacle area under time constraints and uses a speed obstacle method under time constraints to obtain a sampling speed; A resampling module, when there are dense pedestrians in the environment, the resampling module determines whether the robot is in a trapped area through a speed resampling criterion. If the robot is in a trapped area, a speed obstacle method under time constraints is used to perform speed resampling to obtain a resampled speed; The selection module integrates the sampling speed and the resampling speed to generate candidate trajectories, designs an evaluation function according to the candidate trajectories, the minimum distance from the pedestrian and the robot speed, and selects the optimal trajectory according to the evaluation function.

Images