An autonomous navigation method, device and medium for human-robot collaboration

Abstract

The application discloses a kind of for human-computer cooperation autonomous navigation method, equipment and medium, the method using unscented Kalman filter obtains the current position and velocity vector direction of the person being tracked;Using point cloud matching algorithm, the point cloud data in the first preset range of the current position of the person being tracked is classified, to determine the target current position of the person being tracked;According to the included angle of the velocity vector direction of the person being tracked and the current orientation of vehicle and preset included angle, determine the final velocity vector direction of the person being tracked;Based on the target current position and final velocity vector direction of the person being tracked, determine three initial navigation points in the person coordinate system;Three initial navigation points are scored respectively, based on the scoring result, determine the cooperation mode of vehicle next time;According to the cooperation mode of vehicle next time, the dynamic characteristic of the person being tracked and TEB planner, finally obtain more reasonable smooth navigation trajectory under follow mode and accompany mode.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of artificial intelligence, in particular to an autonomous navigation method for human-robot collaboration, a device and a medium. BACKGROUND

[0002] The application of artificial intelligence technology in agriculture is becoming more and more common. However, due to the complexity and dynamics of the agricultural environment, it is still a major challenge to realize a fully autonomous unmanned farm at the present stage. Human-robot collaboration provides a potential solution, which improves work efficiency while achieving a certain degree of agricultural production automation. Human-vehicle following is an important aspect of such human-robot collaboration, which is suitable for different agricultural scenarios, including cargo transportation between forests and orchards. With the improvement of orchard management standards, well-maintained orchards will have wider and flatter roads, allowing vehicles to have more collaboration mode options according to the actual environment and operation requirements, which is also expected by farmers. In conditions permitting, farmers want vehicles to accompany them to ensure safety and improve interaction with users during work; on narrow roads, farmers prefer vehicles to follow them to improve the pass rate on narrow roads. In addition, such mode switching should occur automatically without human intervention, thereby improving convenience. However, in traditional autonomous navigation methods, human-robot collaboration in agricultural applications has a single collaboration mode, which results in the inability to realize the following mode of the vehicle or the accompanying mode of the vehicle involved in real scenarios of human-robot collaboration. In addition, the dynamic characteristics of the tracked person are not considered, and for the above reasons, the vehicle eventually has unreasonable trajectories during following and accompanying. SUMMARY

[0003] The purpose of the present application is to provide an autonomous navigation method for human-robot collaboration, a device and a medium, which can solve the problem of a single collaboration mode of the vehicle and the lack of consideration of the dynamic characteristics of the tracked person in the prior art, thereby resulting in unreasonable navigation trajectories of the vehicle.

[0004] To achieve the above-mentioned purpose, the present application provides the following solutions:

[0005] In a first aspect, the present application provides an autonomous navigation method for human-robot collaboration, comprising:

[0006] acquiring point cloud data of the vehicle at the current time; the point cloud data includes tracked person point cloud data and obstacle point cloud data; the vehicle is used for autonomous accompanying or following the tracked person for human-robot collaboration;

[0007] tracking the tracked person point cloud data using an unscented Kalman filter to obtain the current position and velocity vector direction of the tracked person;

[0008] The point cloud matching algorithm is used to classify the point cloud data in a first preset range of the current position of the tracked person, and determine the target current position of the tracked person.

[0009] According to the included angle between the speed vector direction recorded by the unscented Kalman filter and the speed vector direction recorded by the rolling window, and a preset included angle, a final speed vector direction of the tracked person is determined.

[0010] Based on the target current position and the final speed vector direction of the tracked person, three initial navigation points in a character coordinate system are determined; the character coordinate system is established with the target current position of the tracked person as the origin, the final speed vector direction as the x-axis, and the Y-axis determined according to the right-handed rectangular coordinate system.

[0011] The three initial navigation points are scored respectively to determine a scoring result.

[0012] Based on the scoring result, a cooperation mode of the vehicle at the next moment is determined; the cooperation mode includes a following mode and a companion mode.

[0013] According to the cooperation mode of the vehicle at the next moment, a preset parking condition, dynamic characteristics of the tracked person, and a TEB planner, a navigation trajectory of the vehicle is determined; the dynamic characteristics of the tracked person include a situation in which the tracked person turns and a situation in which the tracked person needs to bypass and continue to move forward when encountering an obstacle in front.

[0014] In a second aspect, the present application provides a computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the autonomous navigation method for human-machine cooperation described in the above.

[0015] In a third aspect, the present application provides a computer readable storage medium having a computer program stored thereon, wherein the computer program is executed by a processor to implement the autonomous navigation method for human-machine cooperation described in the above.

[0016] According to the specific embodiments provided by the present application, the following technical effects are disclosed:

[0017] The application provides a human-machine cooperation autonomous navigation method, device and medium, adopts an unscented Kalman filter to track point cloud data of a tracked person, obtains a current position and a speed vector direction of the tracked person, then accurately determines a target current position of the tracked person by using a point cloud matching algorithm, and accurately determines a final speed vector direction of the tracked person according to an angle between the speed vector direction recorded by the unscented Kalman filter and the speed vector direction recorded by a rolling window and a preset angle. Further, three initial navigation points in a person coordinate system are accurately determined according to the target current position of the tracked person and the final speed vector direction of the person, the three initial navigation points are scored respectively to determine whether a cooperation mode of a vehicle at a next moment is a following mode or a companion mode, and the unreasonable navigation track of the vehicle caused by only a single cooperation mode in the prior art is avoided. Finally, a more reasonable and smooth navigation track of the vehicle in the following mode or the companion mode is obtained according to the cooperation mode of the vehicle at the next moment, in combination with a preset parking condition and a TEB planner, and considering dynamic characteristics of the tracked person. The overall following performance or companion performance of human-machine cooperation is finally improved, and the human-machine cooperation performance is improved. BRIEF DESCRIPTION OF DRAWINGS

[0018] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed to be used in the embodiments will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without any creative effort on the basis of these drawings.

[0019] Figure 1 A flowchart of a human-machine cooperation autonomous navigation method provided in an embodiment of the present application;

[0020] Figure 2 An ANP-TEB framework diagram of a human-machine cooperation autonomous navigation method provided in an embodiment of the present application;

[0021] Figure 3 A rolling window speed vector calculation method schematic diagram of a human-machine cooperation autonomous navigation method provided in an embodiment of the present application;

[0022] Figure 4 A flowchart of a human-machine cooperation autonomous navigation method provided in an embodiment of the present application, and a conversion relationship diagram between coordinate systems in a following mode and a companion mode of a vehicle;

[0023] Figure 5 An obstacle scoring schematic diagram of a human-machine cooperation autonomous navigation method provided in an embodiment of the present application;

[0024] Figure 6A flexible following strategy diagram for an autonomous navigation method for human-machine collaboration provided in an embodiment of the present application;

[0025] Figure 7 A parking strategy diagram for an autonomous navigation method for human-machine collaboration provided in an embodiment of the present application; wherein, Figure 7 (a) is a schematic diagram of parking in a right-angle turn; Figure 7 (b) is a schematic diagram of parking when the accompanying mode encounters an obstacle;

[0026] Figure 8 Schematic diagram of a turning situation of an autonomous navigation method for human-machine collaboration provided in an embodiment of the present application; wherein, Figure 8 (a) is a diagram of a person turning toward a vehicle and Figure 8 (b) in the figure is a schematic diagram of a person turning away from a vehicle. DETAILED DESCRIPTION

[0027] The following will be combined with the drawings in the embodiments of this application to clearly and completely describe the technical solutions in the embodiments of this application. Obviously, the embodiments described are only part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of this application.

[0028] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below with reference to the accompanying drawings and specific implementation methods.

[0029] like Figure 1 and Figure 2 As shown, the present application provides an autonomous navigation method for human-machine collaboration, comprising:

[0030] Step 101: Collecting point cloud data of the vehicle at the current moment; the cloud point data includes point cloud data of the tracked person and point cloud data of obstacles; the vehicle is used to autonomously accompany or follow the tracked person for human-machine collaboration.

[0031] The tracked person point cloud data includes the point cloud of the person being followed; the obstacle point cloud data includes the point cloud of obstacles (fruit trees, other people, etc.).

[0032] Furthermore, the DBSCAN clustering method is used to cluster all point cloud data and filter out noisy point clouds.

[0033] Step 102: Use an unscented Kalman filter to track the point cloud data of the tracked person to obtain the current position and velocity vector direction of the tracked person.

[0034] Among them, the unscented Kalman filter (UKF) is a new type of filtering estimation algorithm. UKF is based on UT transformation, which abandons the traditional linearization of nonlinear functions, and uses the Kalman linear filtering framework. For one-step prediction equation, UKF uses unscented transform (UT) to process the nonlinear transmission of mean and covariance, which becomes the UKF algorithm. UKF is an approximation of the probability density distribution of nonlinear functions, which uses a series of deterministic samples to approximate the posterior probability density of the state, rather than approximating nonlinear functions, without the need for derivative calculation of Jacobian matrix. UKF does not linearize and ignore high-order terms, so the calculation accuracy of nonlinear distribution statistics is higher.

[0035] Step 103: using a point cloud matching algorithm, classifying the point cloud data within a first preset range of the current position of the tracked object, and determining the target current position of the tracked object.

[0036] In some embodiments, step 103 specifically includes steps 203-204:

[0037] Step 203: using a point cloud matching algorithm, classifying the point cloud data within a first preset range of the current position of the tracked object, and classifying the point cloud data within the first preset range with the tracked object point cloud data to obtain updated tracked object point cloud data.

[0038] Step 204: averaging the updated tracked object point cloud data to determine the target current position of the tracked object.

[0039] In the process of tracking the tracked object point cloud data using UKF, the clustering parameters are fixed, but the distance of the tracked object relative to the vehicle changes, which may cause UKF to only track part of the point cloud belonging to the tracked object during tracking. In order to improve the accuracy of the tracked object position calculation, the point cloud data tracked by UKF is re-matched using a point cloud matching algorithm. Each point will get an ID number after DBSCAN clustering, and points with the same ID number are considered to belong to the same cluster.

[0040] That is, the scattered point cloud near the tracked and matched point cloud is attributed to the tracked object point cloud data:

[0041]

[0042] In the formula, ID cluster is the original ID given by the DBSCAN clustering algorithm for each cluster, and ID person is the cluster ID of the person tracked by UKF. Pperson represents the position of the cluster center tracked by the UKF, P cluster represents the position of the cluster center tracked by the UKF, P i represents the Cartesian coordinates of each point in the cluster tracked by the UKF, d cluster defined as the length of the cluster, (x1, y1) is the first cloud point in the cluster, (x n , y n ) is the last cloud point in the cluster.

[0043] After the re-matching is completed, the average position of all points belonging to the cluster of the tracked person is recalculated as the current position of the target of the tracked person.

[0044] Step 104: determining the final speed vector direction of the tracked person according to the included angle between the speed vector direction recorded by the UKF and the speed vector direction recorded by the rolling window, and a preset included angle.

[0045] In some embodiments, step 104 specifically comprises steps 301-303:

[0046] Step 301: comparing whether the included angle is less than or equal to the preset included angle to obtain a target result.

[0047] Step 302: if the target result is yes, using the speed vector recorded by the rolling window as the final speed vector.

[0048] Step 303: if the target result is no, using the speed vector direction recorded by the UKF as the final speed vector direction.

[0049] In practical applications, accurately obtaining the speed vector of the tracked person is crucial to the ANP system, because it determines the initial position of the navigation point. The position data provided by the vehicle odometer is usually affected by significant noise. The sensitivity of the UKF to the change of the speed vector is derived from its use of multiple sigma points to propagate state information in nonlinear systems. Unreasonable parameters can amplify the impact of noise or mutations on the final estimate. In addition, adjusting the UKF parameters depends on experience and varies with different sensors, which makes the adjustment process extremely challenging. Considering the changes in sensor accuracy and type, and the difficulty of parameter tuning, additional methods are needed to obtain a more accurate speed vector.

[0050] In some embodiments, step 302 specifically comprises steps 401-403:

[0051] Step 401: determining five target position points within a second preset range of the current position of the target of the tracked person based on the current position of the target of the tracked person and the rolling window; wherein the second preset range is within the rolling window.

[0052] Step 402: determining a head point and a tail point of the rolling window according to the five target position points and the rolling window.

[0053] Step 403: connecting the head point and the tail point to obtain the final speed vector.

[0054] Referring to Figure 2 , the present application is applied to an adaptive navigation point (ANP, a technology for dynamically adjusting navigation points) system, and is combined with a Time Elastic Band (TEB, a technology for path planning and trajectory optimization) which performs well on common agricultural vehicles. Compared with traditional autonomous navigation methods such as TEB, the ANP-TEB gives a switching method between a following mode and a companion mode, and uses parking, elastic following and other strategies to give a more reasonable and smooth trajectory of the vehicle in the following or companion process, and can enable the vehicle to maintain the most ideal following distance and the most stable speed change. The ANP system can dynamically generate and adjust navigation points according to real-time environmental data and task requirements, thereby optimizing path planning and obstacle avoidance capability, and the TEB planner is mounted on the ANP to obtain the ANP-TEB model.

[0055] As Figure 3 , the ANP records the position of the person in the world coordinate system with a threshold greater than 0.1 meters during operation. The ANP system introduces a new method for calculating the speed vector, which can provide a more stable speed direction when the person walks along a straight line or a gentle curve. If the angle difference between the UKF speed vector (i.e., the speed vector of the tracked person calculated by the UKF) and the speed vector obtained using the rolling window is less than a preset angle (θ), the ANP will use the vector provided by the rolling window method as the final speed vector of the person, and θ is set to 30°. Conversely, when the angle difference exceeds this threshold, the ANP will use the vector provided by the UKF as the final speed vector of the person, ensuring that the vehicle has sufficient response capability when making a sharp turn.

[0056] Step 105: determining three initial navigation points in the person coordinate system based on the target current position of the tracked person and the direction of the final speed vector; the person coordinate system is established with the target current position of the tracked person as the origin, with the direction of the final speed vector as the x-axis, and with the Y-axis determined in the right-handed rectangular coordinate system.

[0057] Wherein, BEV is the bird's eye view perspective. A perspective from which an object or scene is viewed from above, as if looking down upon it from a bird in the air. In the field of autonomous driving and robotics, the data obtained by sensors such as LiDAR and cameras is usually converted into BEV representation in order to better perform object detection, path planning and other tasks. BEV can simplify the complex three-dimensional environment into a two-dimensional image, which is particularly important for efficient calculation in real-time systems.

[0058] In practical applications, the position data required to calculate the vehicle velocity vector is obtained based on the vehicle odometer.

[0059] As Figure 4 , from the bird's eye view (BEV) perspective, a world coordinate system is established, which is constructed with the initial vehicle heading when the vehicle is not started as the X axis and the vertical to the initial vehicle heading in the plane as the Y axis; wherein, (x w , y w ) represents a fixed Cartesian world coordinate system provided by the vehicle odometer;

[0060] The calculation formula of the position (x pw , y pw ) of the origin of the person coordinate system in the world coordinate system is:

[0061]

[0062] Wherein, the vehicle odometer also provides the attitude information of the vehicle, (x c , y c ) represents the vehicle coordinate system (vehicle radar coordinate system), and the positive direction of the vehicle coordinate system is the heading of the vehicle head; the person coordinate system is established with the target current position of the person being tracked as the center, with the final velocity vector direction as the X axis, and with the Y axis determined in the right-handed rectangular coordinate system in the plane; (x, y) represents the position of the origin of the vehicle coordinate system in the world coordinate system; (x pc , y pc ) is the position of the origin of the person coordinate system in the vehicle coordinate system, is the angle between the positive direction of the vehicle coordinate system and the positive direction of the world coordinate system.

[0063] Wherein, the green dots represent three initial navigation points, which are distributed at positions (0, a), (0, -a) and (-b, 0) in the person coordinate system, wherein a represents the set accompanying distance, and b represents the set following distance.

[0064] Wherein, a and -a are located on the left and right sides of the person respectively, that is, the accompanying is divided into left accompanying and right accompanying, and the following is only one kind behind the person.

[0065] Step 106: scoring the three initial navigation points respectively to determine the scoring result.

[0066] In some embodiments, the scoring result is determined according to a formula total_score = β3(β1*basic_score + β2*obstacle_score), wherein total_score represents the scoring result; basic_score represents the initial navigation point basic score; obstacle_score represents the obstacle score; β1, β2, and β3 represent the basic score weight, the obstacle score weight, and the mode protection score weight, respectively; the obstacle score in the following mode is always set to 10; and the determination process of the obstacle score in the accompanying mode is as follows:

[0067]

[0068] wherein total_score represents the scoring result; basic_score represents the initial navigation point basic score; obstacle_score represents the obstacle score; β1, β2, and β3 represent the basic score weight, the obstacle score weight, and the mode protection score weight, respectively; the obstacle score in the following mode is always set to 10; and the determination process of the obstacle score in the accompanying mode is as follows:

[0069] The vertical distance t from the i-th obstacle point to the AB ray is calculated i When t i is less than twice the width of the vehicle, the i-th obstacle point (P ix , P iy ) is classified as a dangerous point; the vertical distance t i from the i-th obstacle point to the AB ray is:

[0070] wherein,

[0071] (A x , A y ) is the position of the accompanying navigation point A; v is the final speed vector of the tracked object; (B x , B y ) is any point on the AB ray, the AB ray being a ray with A as the origin and any point B taken on the ray parallel to v; t i is the vertical distance from the i-th obstacle point to the AB ray; (O x , O y ) is the position of the vehicle O; d1 is the projection length of the vehicle O on the AB ray, d2 is the distance of the vehicle from the obstacle in front of the accompanying navigation point; d i is the projection length of the i-th obstacle point (P ix , P iy ) on the AB ray; (P ix , P iy ) is the position of the obstacle point; is the length of the vector formed by points A and B.

[0072] ​Obtaining the number of dangerous points detected by any of the accompanying sides of the tracked object; comparing whether the number of dangerous points is greater than or equal to a preset number to obtain a first result; if the first result is yes, the obstacle score in the accompanying mode is 0; if the first result is no, determining the obstacle score in the accompanying mode according to the number of dangerous points and the preset obstacle score corresponding to the number of different dangerous points.

[0073] The three initial navigation points include an accompanying navigation point and a following navigation point; the accompanying navigation point includes a first initial accompanying navigation point and a second initial accompanying navigation point; the coordinates of the first initial accompanying navigation point are (0, a1); the coordinates of the second initial accompanying navigation point are (0, -a1); the coordinates of the following navigation point are (-b, 0); a1 and b are the preset accompanying distance and following distance respectively.

[0074] As Figure 5 In the character coordinate system, three initial navigation points are generated at coordinates (0, a1), (0, -a1) and (-b, 0). For these three initial navigation points, the score includes three factors: basic score, obstacle score and mode protection score. The basic score gives priority to the accompanying mode or the following mode, which can be adjusted according to personal preference. The obstacle score is used for the vehicle to judge the feasibility of the navigation position, and the mode protection score weight can ensure that the vehicle will not switch modes too frequently.

[0075] The values of β1 and β2 are set to 1. When the mode remains unchanged, β3 decays at a rate of 2%. Conversely, when the mode changes, β3 is reset to its initial value of 2.5, with a minimum value of 1. Assuming that the execution rate of the program is about 10Hz, it is expected that β3 will decay from the initial value to 1 after about 3 seconds.

[0076] Further, according to the formula The three initial navigation points are corrected to obtain corrected navigation points; where a represents the corrected accompanying distance; α is the included angle between the current orientation of the vehicle and the direction of the final speed vector of the tracked object; b represents the preset following distance; the corrected navigation points include the coordinates of the first accompanying navigation point (0, a); the coordinates of the second accompanying navigation point (0, -a); the coordinates of the following navigation point (-b, 0); the corrected navigation points replace the three initial navigation points.

[0077] Referring to Figure 6, the ANP can also dynamically adjust the following distance, i.e., the elastic following strategy, in addition to the necessary parking. The smaller the angle between the orientation of the vehicle and the final velocity vector direction of the person, the more likely the vehicle will maintain a straight trajectory, at which time the set following distance should be maintained. As the angle increases, the vehicle should reduce the following distance to ensure that the person remains stable within the two-dimensional lidar field of view while turning. If the angle between the orientation of the vehicle and the motion direction of the person is α. Where a represents the adjusted following distance, the minimum value of the distance is 1m to ensure safety.

[0078] In practical applications, when t i is less than twice the width of the vehicle, the points (P ix , P iy ) are classified as infeasible dangerous points. In order to reduce the effect of noise, if five dangerous points are detected on either side, the obstacle score of that side is set to 0. Otherwise, it is set to 10.

[0079] Where the obstacle score in the following mode is always set to 10, because the following mode is always a feasible option.

[0080] Step 107: Based on the scoring results, determine the cooperative mode of the vehicle at the next time; the cooperative mode includes the following mode and the accompanying mode.

[0081] In some embodiments, step 107 specifically includes: when the initial cooperative mode executed when the vehicle starts is set to the following mode, for the three initial navigation points, if the accompanying navigation point score is detected to be higher than the following navigation point score, the cooperative mode of the vehicle at the next time is determined to be the accompanying mode; wherein the accompanying navigation point score is the score with the higher score among the first accompanying navigation point score and the first accompanying navigation point score; if the following navigation point score is detected to be higher than the accompanying navigation point score, the cooperative mode of the vehicle at the next time is determined to be the following mode.

[0082] Step 108: According to the cooperative mode of the vehicle at the next time, the preset parking condition, the dynamic characteristics of the tracked person, and the TEB planner, determine the navigation trajectory of the vehicle; wherein the dynamic characteristics of the tracked person include: the case where the tracked person turns and the case where the tracked person encounters an obstacle in front and needs to bypass and continue to move forward.

[0083] Where the TEB planner is a high-efficiency and flexible path planning and trajectory optimization tool, especially suitable for autonomous vehicles, drones and robot navigation in dynamic environments. The TEB planner combines the Time Elastic Band (TEB) algorithm, which can dynamically adjust the path to avoid obstacles while ensuring path smoothness.

[0084] Time Elastic Band (TEB) algorithm is an algorithm for robot path planning, mainly used for optimizing the local motion trajectory of the robot. It optimizes the local motion path of the robot by modifying the global trajectory. TEB algorithm solves the configuration and time problem, i.e. multi-objective optimization problem, by constructing a hyper-graph and using the optimization algorithm in the g2o (generalized graph optimization) framework. In the hyper-graph, the robot state and time interval are nodes, the objective function and constraint function are edges, and the nodes are connected by edges to form a hyper-graph. The objectives of TEB algorithm include the overall path length, trajectory running time, distance to obstacles, passing through intermediate path points, and compliance with robot dynamics, kinematics and geometric constraints, etc.

[0085] In some embodiments, step 108 specifically comprises steps 501-503:

[0086] Step 501: the preset parking condition comprises a first parking trigger condition and a second parking trigger condition; for the case that the tracked object turns, the first parking trigger condition is set, which comprises that the included angle exceeds a second preset included angle for three consecutive times, and the following navigation point and the target current position of the tracked object are in different quadrants of a vehicle coordinate system; when the cooperation mode of the vehicle at the next moment is the following mode, it is detected whether the vehicle meets the first parking trigger condition; if yes, the vehicle is controlled to stop; if not, the following navigation point is sent to the TEB planner, and the vehicle is controlled to perform the following navigation operation corresponding to the following navigation point based on the TEB planner.

[0087] Step 502: for the case that the tracked object encounters an obstacle in front and needs to bypass and continue to move forward, the second parking trigger condition is set; the second parking trigger condition comprises that the following navigation point and the target current position of the tracked object are in different quadrants of a vehicle coordinate system; when the cooperation mode of the vehicle at the next moment is the following mode, it is detected whether the vehicle meets the second parking trigger condition; if yes, the vehicle is controlled to stop; if not, the following navigation point is sent to the TEB planner, and the vehicle is controlled to perform the following navigation operation corresponding to the following navigation point based on the TEB planner.

[0088] Wherein, the vehicle coordinate system is established based on the right-handed rectangular coordinate with the vehicle position as the origin and the vehicle velocity vector direction as the x-axis.

[0089] Step 503: based on the following navigation operation and the accompanying navigation operation, the navigation trajectory of the vehicle is determined.

[0090] Referring to Figure 7 When the follower makes a right-angle turn or intends to switch from the follow mode to the accompany mode, the traditional TEB method only considers the point-to-point trajectory planning without considering the dynamic characteristics of the follower, which often leads to unreasonable trajectories. In contrast, a more appropriate behavior is to park until the person completes the turn or bypasses the obstacle, i.e., the revised follow navigation point is in the same quadrant of the vehicle coordinate system as the target person (the tracked person), and then continue to issue a new navigation task to the TEB planner.

[0091] It is worth noting that when the distance of the person is detected to be less than a safety distance, the ANP will be forced to enter the parking mode, i.e., the vehicle stops, which is set to 0.8 meters in this application.

[0092] When the ANP continuously (more than 3 frames to ensure that it is not caused by noise) detects that the angle between the orientation of the vehicle and the moving direction of the follower exceeds 60° (the second preset angle), and the follow navigation point and the target person are in different quadrants of the vehicle coordinate system, such as (a) in Figure 7 , or when the vehicle switches from the accompany mode to the follow mode, such as (b) in Figure 7 , and the follow navigation point and the target person are in different quadrants of the vehicle coordinate system, the ANP will not directly send the navigation point to the TEB. Instead, it selects the parking strategy until the follow navigation point and the target person are in the same quadrant of the vehicle coordinate system, at which time a new navigation task is issued to the TEB planner.

[0093] In practical applications, when the vehicle starts, the ANP is initially set to the follow mode, and at each time step, i.e., at each time after the vehicle starts, it continuously scores three navigation points. If it is detected that the accompany navigation point has a higher score than the follow navigation point, the ANP will send the position of the accompany navigation point to the TEB planner. Conversely, if the follow navigation point has a higher score, the ANP will send the position of the follow navigation point.

[0094] It is worth noting that the ANP only supports switching from the follow mode to the accompany mode or from the accompany mode to the follow mode, and it does not allow direct switching between two accompany positions, because such a sudden change in lateral position is unreasonable.

[0095] In practical applications, when the vehicle detects a person turning in the accompany mode, it will first switch to the follow mode. This transition allows the vehicle to be guided by the tracked person, thereby improving the turning performance. During the accompany process, a person turning scenario can be divided into a person turning towards the vehicle and a person turning away from the vehicle Figure 8 ). In Figure 8In (a) of FIG. 1, when a person turns and moves towards the vehicle, the distance between the tracked person and the vehicle rapidly decreases due to the fact that the vehicle cannot make a direct lateral movement, thus reaching the 0.8m safety distance setting of the ANP, triggering the parking trigger condition, given that the person will usually avoid collision and bypass the vehicle, the ANP only needs to keep the vehicle stationary until the safety distance is restored and the modified follow navigation point and the tracked person are in the same quadrant of the vehicle coordinate system, during which the ANP will still record new speed vectors constantly. In Figure 8 In (b) of FIG. 1, the only difference is that the person turns in a different direction, in which case a short parking is sufficient to continue the task so that the modified follow navigation point and the tracked person are in the same quadrant of the vehicle coordinate system.

[0096] Human-vehicle following and accompanying are important aspects of human-machine cooperation. When traditional autonomous navigation algorithms are introduced into human-vehicle following or accompanying, firstly, the function of autonomous switching of the mode is missing, and secondly, the traditional autonomous navigation algorithm often gives unreasonable or failed trajectories in the process of following and accompanying without considering the dynamic characteristics of the followed person.

[0097] The present application proposes a method of combining ANP and TEB. ANP can realize autonomous switching of the following mode and the accompanying mode by generating three initial navigation points at different positions and scoring them, and can give a more stable speed vector direction when the person is moving straight by using a rolling window speed vector calculation. Meanwhile, ANP considers the dynamic characteristics of the followed person, reasonably adopts the parking and elastic following strategy, and gives the most reasonable and smooth trajectory, thereby improving the overall following or accompanying performance and improving the human-machine cooperation performance.

[0098] In an exemplary embodiment, a computer device is also provided, including a memory and a processor, the memory storing a computer program, and the processor implementing the above method when executing the computer program.

[0099] In an exemplary embodiment, a computer readable storage medium is provided, storing a computer program, which is executed by a processor to implement the above method.

[0100] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data for analysis, stored data, displayed data, etc.) involved in the present application are all information and data authorized by the user or authorized by all parties, and the collection, use and processing of related data need to comply with relevant regulations.

[0101] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a non-volatile computer readable storage medium, and when the computer program is executed, the processes of the above-mentioned embodiments can be included. Any reference to memory, database or other medium used in the embodiments provided in the present application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ReadOnly Memory, ROM), magnetic tape, floppy disk, flash memory, optical storage, high-density embedded non-volatile memory, resistive memory (ReRdM), magnetoresistive memory (MRdM), ferroelectric memory (FRdM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RdM) or external cache memory, etc. As an illustration but not limitation, RdM can be in various forms, such as static random access memory (SRdM) or dynamic random access memory (DRdM), etc.

[0102] The database involved in the embodiments provided in the present application can include at least one of a relational database and a non-relational database. The non-relational database can include a distributed database based on a block chain, etc., without being limited thereto. The processor involved in the embodiments provided in the present application can be a general-purpose processor, a central processing unit, a graphics processing unit, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., without being limited thereto.

[0103] The technical features of the above embodiments can be combined arbitrarily. In order to make the description concise, all possible combinations of the technical features in the above embodiments are not described, but as long as the combinations of the technical features do not exist, they should be considered as the scope of the present application.

[0104] The principles and implementation modes of the present application are described by applying specific examples herein. The above description of the embodiments is only used to help understand the method and its core idea of the present application; meanwhile, for those skilled in the art, according to the idea of the present application, the specific implementation mode and application range can be changed. In conclusion, the content of the present application should not be understood as a limitation.

Claims

1. An autonomous navigation method for human-machine collaboration, characterized in that: include: Collecting point cloud data of the vehicle at the current moment; the point cloud data includes point cloud data of the tracked object and point cloud data of obstacles; The vehicle is used to autonomously accompany or follow the tracked person to perform human-machine collaboration; Tracking the point cloud data of the tracked person using an unscented Kalman filter to obtain the current position and velocity vector direction of the tracked person; Using a point cloud matching algorithm, classifying the point cloud data within a first preset range of the current position of the tracked person to determine the target current position of the tracked person; Determine the final velocity vector direction of the tracked person based on the angle between the velocity vector direction recorded by the unscented Kalman filter and the velocity vector direction recorded by the rolling window, as well as the preset angle; Determining three initial navigation points in a person coordinate system based on the target current position and the final velocity vector direction of the tracked person; the person coordinate system is established with the target current position of the tracked person as the origin, the final velocity vector direction as the x-axis, and the y-axis determined by a right-handed rectangular coordinate system; Scoring the three initial navigation points respectively to determine scoring results; the three initial navigation points include accompanying navigation points and following navigation points; the accompanying navigation points include a first accompanying navigation point and a second accompanying navigation point; Determining the next cooperation mode of the vehicles based on the scoring results; the cooperation modes include a follow mode and an accompanying mode; Determine the navigation trajectory of the vehicle based on the next moment's cooperative mode, preset parking conditions, the dynamic characteristics of the tracked person, and the TEB planner; wherein the dynamic characteristics of the tracked person include: the situation where the tracked person turns and the situation where the tracked person encounters an obstacle in front and needs to circumvent it and continue forward; The three initial navigation points are scored respectively to determine the scoring results, specifically including: According to the formula total_ ) determining the scoring result; Among them, total_ Indicates the scoring result; Basic score of initial navigation point; represents the obstacle score; β1, β2, and β3 represent the basic score weight, obstacle score weight, and mode protection score weight, respectively.

2. The autonomous navigation method for human-machine collaboration according to claim 1, characterized in that: Scoring the three initial navigation points respectively and determining the next cooperation mode of the vehicles based on the scoring results, further comprising: According to the formula , correct the three initial navigation points to obtain corrected navigation points, and use the corrected navigation points as the three initial navigation points; wherein, a represents the corrected accompanying distance; α is the angle between the current orientation of the vehicle and the final velocity vector direction of the tracked person; b represents the set following distance; the coordinates of the corrected navigation point include the coordinates of the first accompanying navigation point (0, a The coordinates of the second accompanying navigation point are (0, - a ); The coordinates of the following navigation point are (-b, 0).

3. The autonomous navigation method for human-machine collaboration according to claim 2, characterized in that: The determination process is: Calculate the vertical distance from the i-th obstacle point to the AB ray ,when When it is less than twice the width of the vehicle, the i-th obstacle point ( , ) is classified as a dangerous point; the vertical distance from the i-th obstacle point to the AB line for: ;in, ; ; ; ( , ) is the position of the accompanying navigation point A; v is the final velocity vector of the tracked object; ( , ) is any point on AB ray, It is obtained by taking A as the origin and taking any point B on the ray parallel to v; is the modulus of the vector formed by points A and B; is the vertical distance from the i-th obstacle point to the AB ray; ( , ) is the position of vehicle O; is the projection length of vehicle O on ray AB, is the distance of the obstacle in front of the vehicle detection accompanying the navigation point; is the i-th obstacle point ( , ) Projection length on AB ray; ( , ) is the position of the obstacle point; Obtain the number of dangerous points detected on any accompanying side of the tracked person; Comparing whether the number of the dangerous points is greater than or equal to a preset number, and obtaining a first result; If the first result is yes, the obstacle score in the companion mode is 0; If the first result is no, the obstacle score in the accompanying mode is determined according to the number of the danger points and the preset obstacle scores corresponding to the numbers of different danger points.

4. The autonomous navigation method for human-machine collaboration according to claim 1, characterized in that: Based on the scoring results, the next vehicle cooperation mode is determined, specifically including: When the initial cooperation mode executed when the vehicle is started is set to the following mode, for the three initial navigation points, if it is detected that the accompanying navigation point score is higher than the following navigation point score, the cooperation mode of the vehicle at the next moment is determined to be the accompanying mode; wherein the scoring result includes the first accompanying navigation point score, the second accompanying navigation point score, and the following navigation point score; the accompanying navigation point score is the higher score between the first accompanying navigation point score and the second accompanying navigation point score; If it is detected that the score of the following navigation point is higher than the score of the accompanying navigation point, it is determined that the cooperation mode of the vehicle at the next moment is the following mode.

5. The autonomous navigation method for human-machine collaboration according to claim 1, characterized in that: Determine the navigation trajectory of the vehicle based on the next moment's cooperative mode, preset parking conditions, the dynamic characteristics of the tracked person, and the TEB planner, specifically including: The preset parking conditions include a first parking triggering condition and a second parking triggering condition; In response to the tracked person turning, the first parking trigger condition is set; the first parking trigger condition includes: the angle exceeds the second preset angle three times in a row, and the following navigation point and the target current position of the tracked person are in different quadrants of the vehicle coordinate system; When the next cooperative mode of the vehicle is the accompanying mode, detecting whether the vehicle meets the first parking trigger condition; if so, controlling the vehicle to park; if not, sending the accompanying navigation point to the TEB planner, and controlling the vehicle to perform the accompanying navigation operation corresponding to the accompanying navigation point based on the TEB planner; In case the tracked person encounters an obstacle ahead and needs to circumvent it and continue forward, the second parking trigger condition is set; the second parking trigger condition includes: the following navigation point and the target current position of the tracked person are in different quadrants of the vehicle coordinate system; When the next cooperative mode of the vehicle is the following mode, detecting whether the vehicle meets the second parking trigger condition; if so, controlling the vehicle to stop; if not, sending the following navigation point to the TEB planner, and controlling the vehicle to perform the following navigation operation corresponding to the following navigation point based on the TEB planner; A navigation trajectory of the vehicle is determined based on the following navigation operation and the accompanying navigation operation.

6. The autonomous navigation method for human-machine collaboration according to claim 1, characterized in that: Using a point cloud matching algorithm, classifying the point cloud data within a first preset range of the current location of the tracked person to determine the target current location of the tracked person, specifically including: Using a point cloud matching algorithm, classifying the point cloud data within a first preset range of the current position of the tracked person, and classifying the point cloud data within the first preset range into the point cloud data of the tracked person, to obtain updated point cloud data of the tracked person; The updated point cloud data of the tracked person is averaged to determine the target current position of the tracked person.

7. The autonomous navigation method for human-machine collaboration according to claim 1, characterized in that: The final velocity vector direction of the tracked object is determined based on the angle between the velocity vector direction recorded by the unscented Kalman filter and the velocity vector direction recorded by the rolling window, as well as the preset angle. Specifically, the following steps are performed: Comparing whether the angle between the velocity vector direction recorded by the unscented Kalman filter and the velocity vector direction recorded by the rolling window is less than or equal to the preset angle, and obtaining a target result; If the target result is yes, determining a final velocity vector using the velocity vectors recorded in the rolling window; If the target result is no, the velocity vector recorded by the unscented Kalman filter is used as the final velocity vector.

8. The autonomous navigation method for human-machine collaboration according to claim 7, characterized in that: Calculating the velocity vector direction using a rolling window to determine a final velocity vector specifically includes: Based on the target current position of the tracked person and the scrolling window, determining five target position points within a second preset range of the target current position of the tracked person; wherein the second preset range is within the scrolling window; Determining a head point and a tail point of the rolling window according to the five target position points and the rolling window; The head point and the tail point are connected to obtain the final velocity vector.

9. A computer device comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the autonomous navigation method for human-machine collaboration according to any one of claims 1 to 8.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the autonomous navigation method for human-machine collaboration described in any one of claims 1 to 8 is implemented.

Images