Autonomous navigation method and equipment for man-machine cooperation and medium

Abstract

The invention discloses an autonomous navigation method and device for man-machine cooperation and a medium. The method comprises the steps that an unscented Kalman filter is used for obtaining the current position and the velocity vector direction of a tracked person; using a point cloud matching algorithm to classify the point cloud data within a first preset range of the current position of the tracked person, and determining a target current position of the tracked person; determining a final velocity vector direction of the tracked person according to an included angle between the velocity vector direction of the tracked person and the current orientation of the vehicle and a preset included angle; determining three initial navigation points in a figure coordinate system based on the target current position and the final velocity vector direction of the tracked person; the three initial navigation points are scored respectively, and the cooperation mode of the vehicle at the next moment is determined based on the scoring result; and according to the cooperation mode of the vehicle at the next moment, the dynamic characteristics of the tracked person and a TEB planner, a more reasonable and smoother navigation track in a following mode and an accompanying mode is finally obtained.

Description

Technical Field

[0001] The present application relates to the field of artificial intelligence technology, and in particular to an autonomous navigation method, device and medium for human-machine collaboration. Background Art

[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 achieve fully autonomous unmanned farms at this stage. Human-machine collaboration provides a potential solution to achieve a certain degree of automation in agricultural production while improving operating efficiency. Human-vehicle following is an important aspect of this human-machine collaboration, which is applicable to different agricultural scenarios, including the transportation of goods between trees and orchards. With the improvement of orchard management standards, well-maintained orchards will have wider and flatter roads, allowing vehicles to have more collaborative mode options according to the actual environment and operating requirements, which is also what farmers expect. When conditions permit, farmers want vehicles to accompany them to ensure safety and improve the interaction with users during operation; on narrow roads, farmers prefer vehicles to follow them to improve the passing rate on narrow roads. In addition, this mode conversion should occur automatically without human intervention, thereby improving convenience. However, in the traditional autonomous navigation method, the human-machine collaboration in agricultural applications has a single collaborative mode, which makes it impossible to realize the human-vehicle following mode or human-vehicle accompanying mode involved in the real scene of human-machine collaboration. In addition, the dynamic characteristics of the person being tracked are not taken into account, which ultimately leads to unreasonable trajectories of the vehicle during the following and accompanying process. Summary of the invention

[0003] The purpose of this application is to provide an autonomous navigation method, device and medium for human-machine collaboration, which can solve the problem in the prior art that the vehicle's collaboration mode is single and does not take into account the dynamic characteristics of the tracked person, thereby causing unreasonable navigation trajectory of the vehicle.

[0004] To achieve the above objectives, this application provides the following solutions:

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

[0006] 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 to perform human-machine collaboration;

[0007] 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;

[0008] 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 determining the target current position of the tracked person;

[0009] Determine the final velocity vector direction of the tracked person according to 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;

[0010] Based on the target current position and the final velocity 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 velocity vector direction as the x-axis, and the y-axis determined by the right-hand rectangular coordinate system;

[0011] Scoring the three initial navigation points respectively to determine scoring results;

[0012] Based on the scoring result, determining the cooperation mode of the vehicle at the next moment; the cooperation mode includes a following mode and an accompanying mode;

[0013] The navigation trajectory of the vehicle is determined according to the cooperation mode of the vehicle at the next moment, the 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 bypass it and continue to move forward.

[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 collaboration described above.

[0015] In a third aspect, the present application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the autonomous navigation method for human-machine collaboration described above.

[0016] According to the specific embodiments provided in this application, this application discloses the following technical effects:

[0017] The present application provides an autonomous navigation method, device and medium for human-machine collaboration, which uses 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. Then, the point cloud matching algorithm is used to accurately determine the current position of the target of the tracked person; and the final velocity vector direction of the tracked person is accurately determined according to 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. Further, according to the current position of the target of the tracked person and the final velocity vector direction of the person, the three initial navigation points in the person coordinate system are accurately determined; the three initial navigation points are scored respectively to determine whether the collaboration mode of the vehicle at the next moment is a following mode or a companion mode, avoiding the unreasonable navigation trajectory of the vehicle caused by the only single collaboration mode in the prior art. Finally, according to the collaboration mode of the vehicle at the next moment, and specifically considering the dynamic characteristics of the tracked person, combined with the preset parking conditions, and the TEB planner, a more reasonable and smooth navigation trajectory of the vehicle in the following mode or the accompanying mode is obtained. Finally, the overall following performance or accompanying performance of human-machine collaboration is improved, and the human-machine collaboration performance is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0019] Figure 1 A schematic diagram of a flow chart of an autonomous navigation method for human-machine collaboration provided in an embodiment of the present application;

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

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

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

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

[0024] Figure 6An elastic following strategy diagram of 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 schematic diagram of a person turning toward a vehicle and Figure 8 (b) 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 the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0028] In order to make the above-mentioned objects, features and advantages of the present application more obvious and easy to understand, the present application is further described in detail below in conjunction with 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 filter estimation algorithm. UKF is based on the UT transform, abandons the traditional practice of linearizing nonlinear functions, adopts the Kalman linear filter framework, and uses the unscented transform (UT) to process the nonlinear transmission of the mean and covariance for the one-step prediction equation, which becomes the UKF algorithm. UKF approximates the probability density distribution of nonlinear functions, using a series of determined samples to approximate the posterior probability density of the state, rather than approximating the nonlinear function, and does not require the derivation of the Jacobian matrix. UKF does not linearize and ignores high-order terms, so the calculation accuracy of nonlinear distribution statistics is higher.

[0035] Step 103: using a point cloud matching algorithm to classify the point cloud data within a first preset range of the current position of the tracked person, and determine the target current position of the tracked person.

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

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

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

[0039] Among them, in the process of using UKF to track the point cloud data of the tracked person, since the clustering parameters are fixed, but the distance of the tracked person relative to the vehicle will change, it may cause UKF to track only part of the point cloud belonging to the tracked person during the tracking process. In order to calculate the accuracy of the position of the tracked person, the point cloud data tracked by UKF is re-matched using the 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] The scattered point clouds near the point cloud that is being tracked and matched are all attributed to the point cloud data of the tracked object:

[0041]

[0042] Where, ID cluster It means that the DBSCAN clustering algorithm gives the original ID of each cluster, and ID person is the cluster ID of the person tracked by UKF.person represents the location of the cluster center of the person tracked by UKF, P cluster represents the center position of all clusters, P i represents the Cartesian coordinates of each point in the human cluster tracked by UKF, d cluster Defined as the length of the cluster, (x 1 ,y 1 ) is the first cloud point in the cluster, (x n ,y n ) is the last cloud point of clustering.

[0043] After the rematching is completed, the average position of all cluster points belonging to the tracked person is recalculated as the current position of the tracked person's target.

[0044] Step 104: Determine the final velocity vector direction of the tracked person according to 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.

[0045] In some embodiments, step 104 specifically includes steps 301 to 303:

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

[0047] Step 302: If the target result is yes, use the velocity vector recorded in the rolling window as the final velocity vector.

[0048] Step 303: If the target result is no, the velocity vector direction recorded by the unscented Kalman filter is used as the final velocity vector direction.

[0049] In practical applications, accurately obtaining the velocity vector of the tracked person is crucial for 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 UKF to changes in the velocity vector stems from its use of multiple sigma points in a nonlinear system to propagate state information. Unreasonable parameters will amplify the impact of noise or mutations on the final estimate. In addition, adjusting UKF parameters relies on experience and varies from sensor to sensor, which makes the adjustment process extremely challenging. Considering the variations in sensor accuracy and type, as well as the difficulty of parameter tuning, additional methods are needed to obtain a more accurate velocity vector.

[0050] In some embodiments, step 302 specifically includes steps 401 to 403:

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

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

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

[0054] Reference Figure 2 , this application is applied to the Adaptive Navigation Point (ANP) system, and combined with the Time Elastic Band (TEB) which performs well on common agricultural vehicles, a technology for path planning and trajectory optimization. Compared with traditional autonomous navigation methods such as TEB, ANP-TEB provides a method for switching between the following mode and the accompanying mode, and uses parking, elastic following and other strategies to give the vehicle a more reasonable and smooth trajectory during the following or accompanying process, while allowing the vehicle to maintain the most ideal following distance and the most stable speed change. Among them, 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 capabilities, and the TEB planner is mounted on the ANP to obtain the ANP-TEB model.

[0055] like Figure 3 During operation, ANP will continuously record the position of a person in the world coordinate system with a threshold greater than 0.1 meters. The ANP system introduces a new method for calculating the velocity vector, which can provide a more stable velocity direction when a person walks along a straight line or a gentle curve. If the angle difference between the UKF velocity vector (i.e., the velocity vector of the tracked person calculated by UKF) and the velocity vector obtained using the rolling window is less than the preset angle (θ), ANP will use the vector provided by the rolling window method as the person's final velocity vector, and θ is set to 30°. On the contrary, when the angle difference exceeds this threshold, ANP will use the vector provided by UKF as the person's final velocity vector to ensure that the vehicle maintains sufficient responsiveness when making fast turns.

[0056] Step 105: Based on the target current position and final velocity vector direction of the tracked person, three initial navigation points in the 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 velocity vector direction as the x-axis, and the Y-axis determined by the right-handed rectangular coordinate system.

[0057] Among them, BEV is a bird's-eye view. A perspective of viewing an object or scene from above, just like a bird looking down at the ground in the air. In the field of autonomous driving and robotics, data obtained by sensors (such as LiDAR and cameras) are often converted into BEV representations to better perform tasks such as object detection and path planning. BEV can simplify complex three-dimensional environments into two-dimensional images, which is especially important for efficient computing 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] like Figure 4 , from the perspective of the bird's eye view (BEV), a world coordinate system is established, wherein the world coordinate system is constructed with the initial vehicle head direction when the vehicle is not started as the X-axis and the plane perpendicular to the initial vehicle head direction as the Y-axis; wherein, (x w ,y w ) represents the fixed Cartesian world coordinate system provided by the vehicle odometer;

[0060] The position of the origin of the character coordinate system in the world coordinate system (x pw ,y pw ) is calculated as:

[0061]

[0062] Among them, the vehicle odometer also provides vehicle posture information, (x c ,y c ) represents the vehicle coordinate system (vehicle radar coordinate system), the positive direction of the vehicle coordinate system is the direction of the front of the vehicle; the person coordinate system is established with the current position of the target being tracked as the center, the direction of the final velocity vector as the X-axis, and the Y-axis determined by the right-hand 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 character coordinate system in the vehicle coordinate system, It is the angle between the positive direction of the vehicle coordinate system and the positive direction of the world coordinate system.

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

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

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

[0066] In some embodiments, according to the formula total_score=β 3 (β 1 *basic_score+β 2 *

[0067] obstacle_score) to determine the scoring result;

[0068] Among them, total_score represents the scoring result; basic_score represents the basic score of the initial navigation point; obstacle_score represents the obstacle score; β1, β2, and β3 represent the basic score weight, obstacle score weight, and mode protection score weight, respectively. The obstacle score in the follow mode is always set to 10. The obstacle score in the accompanying mode is determined as follows:

[0069] Calculate the vertical distance t from the i-th obstacle point to the AB ray i , when t i When it 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 from the i-th obstacle point to the AB ray i for:

[0070] in, (A x , A y ) is the position of the accompanying navigation point A; v is the final velocity vector of the tracked object; (B x , B y ) is any point on AB ray, which is obtained by taking any point B on the ray parallel to v with A as the origin; t i is the vertical distance from the i-th obstacle point to the AB ray; (O x , O y ) is the position of vehicle O; d 1 is the projection length of vehicle O on ray AB, d 2 is the distance of the obstacle ahead of the vehicle detection accompanying navigation point; d i is the i-th obstacle point (P ix , P iy ) on the AB ray; (P ix , P iy ) is the location of the obstacle point; It is the modulus of the vector formed by points A and B.

[0072] Obtain the number of dangerous points detected on any accompanying side of the tracked person; compare 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, determine the obstacle score in the accompanying mode according to the number of dangerous points and preset obstacle scores corresponding to different numbers of dangerous points.

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

[0074] like Figure 5 , the coordinates in the character coordinate system (0, a 1 )、(0,-a 1 ) and (-b, 0), for which three initial navigation points are scored based on three factors: basic score, obstacle score, and mode protection score. The basic score prioritizes the accompanying mode or the following mode, which can be adjusted according to personal preference. The obstacle score is used by the vehicle to determine the feasibility of the navigation position, while the mode protection score weight ensures that the vehicle does not switch modes too frequently.

[0075] Among them, 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 and the minimum value is 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] Furthermore, according to the formula The three initial navigation points are corrected to obtain corrected 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 corrected navigation points include a first accompanying navigation point with coordinates (0, a); a second accompanying navigation point with coordinates (0, -a); and a following navigation point with coordinates (-b, 0); and the three initial navigation points are replaced by the corrected navigation points.

[0077] Reference Figure 6In addition to necessary stops, ANP can also dynamically correct the following distance, i.e., the elastic following strategy. The smaller the angle between the vehicle's orientation and the direction of the person's final velocity vector, the more likely the vehicle is to maintain a straight trajectory, and the set following distance should be maintained at this time. As the angle increases, the vehicle should reduce the following distance to ensure that the person remains stable within the field of view of the two-dimensional lidar when turning. If the angle between the vehicle's orientation and the person's direction of movement is α. Among them, a represents the corrected following distance, and to ensure safety, the minimum value of this distance is 1m.

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

[0079] The obstacle score in follow mode is always set to 10, since follow mode is always a viable option.

[0080] Step 107: Based on the scoring result, determine the cooperation mode of the vehicle at the next moment; the cooperation mode includes a following mode and an accompanying mode.

[0081] In some embodiments, step 107 specifically includes: when the initial collaboration 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, it is determined that the collaboration mode of the vehicle at the next moment is the accompanying mode; wherein the accompanying navigation point score is the higher score between the first accompanying navigation point score and the first accompanying navigation point score; if it is detected that the following navigation point score is higher than the accompanying navigation point score, it is determined that the collaboration mode of the vehicle at the next moment is the following mode.

[0082] Step 108: Determine the navigation trajectory of the vehicle according to the next cooperation mode of the vehicle, preset parking conditions, dynamic characteristics of the tracked person and 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 bypass it and continue to move forward.

[0083] Among them, the TEB planner is an efficient and flexible path planning and trajectory optimization tool, which is particularly 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] The Time Elastic Band (TEB) algorithm is an algorithm for robot path planning, mainly used to optimize the local motion trajectory of the robot. It optimizes the local motion path of the robot by correcting the global trajectory. The TEB algorithm constructs a hyper-graph and uses the optimization algorithm in the g2o (general graph optimization) framework to solve configuration and time problems, that is, multi-objective optimization problems. In the hyper-graph, the robot state and time interval are used as nodes, the objective function and constraint function are used as edges, and each node is connected by edges to form a hyper-graph. The objectives of the TEB algorithm include the overall path length, trajectory running time, distance to obstacles, passing through intermediate path points, and complying with the robot's dynamics, kinematics, and geometric constraints.

[0085] In some embodiments, step 108 specifically includes steps 501 to 503:

[0086] Step 501: The preset parking conditions include a first parking trigger condition and a second parking trigger condition; in response to the situation where the tracked person turns, the first parking trigger condition is set, and the first parking trigger condition includes: the angle exceeds the second preset angle for three consecutive times, 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 collaboration mode of the vehicle is the accompanying mode, detect whether the vehicle meets the first parking trigger condition; if so, control the vehicle to stop; if not, send the accompanying navigation point to the TEB planner, and control the vehicle to perform the accompanying navigation operation corresponding to the accompanying navigation point based on the TEB planner.

[0087] Step 502: In case that the tracked person encounters an obstacle in front and needs to bypass it and continue to move 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 collaboration mode of the vehicle is the following mode, detect whether the vehicle meets the second parking trigger condition; if so, control the vehicle to stop; if the second parking trigger condition is not met, send the following navigation point to the TEB planner, and control the vehicle to perform the following navigation operation corresponding to the following navigation point based on the TEB planner.

[0088] The vehicle coordinate system is established with the vehicle position as the origin, the vehicle velocity vector direction as the x-axis, and the y-axis determined based on right-hand rectangular coordinates.

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

[0090] Reference Figure 7 ,When the tracked person makes a right angle turn or intends to switch from the accompanying mode to the following mode, the traditional TEB method only considers the point-to-point trajectory planning without considering the dynamic characteristics of the tracked person, which often leads to unreasonable ,trajectories.,In contrast, a more appropriate behavior is to park the vehicle until the person completes the turn or bypasses the obstacle, that is, the corrected following navigation point is in the same quadrant of the vehicle coordinate system as the target person (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 a person is detected to be less than a safe distance, ANP will force the vehicle to enter parking mode, that is, the vehicle stops, which is set to 0.8 meters in this application.

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

[0093] In practice, when the vehicle starts, the ANP is initially set to follow mode, and at each time step, that is, every moment after the vehicle starts, it will continuously score three navigation points. If it detects that a companion navigation point has a higher score than the following navigation point, the ANP will send the position of the companion navigation point to the TEB planner. On the contrary, if the following navigation point has a higher score, the ANP will send the position of the following navigation point.

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

[0095] In actual applications, when the vehicle detects a person turning in the accompanying mode, it will first switch to the following mode. This transition allows the vehicle to be guided by the person being followed, thereby improving the turning performance. In the accompanying process, the scene of a person turning can be divided into the scene of the person turning towards the vehicle and the scene of the person turning away from the vehicle ( Figure 8 ).exist Figure 8In (a), when a person turns and moves toward the vehicle, since the vehicle cannot make direct lateral movements, the distance between the person being tracked and the vehicle decreases rapidly, reaching the 0.8m safety distance setting of ANP, triggering the parking trigger condition. Given that the person usually avoids collision and goes around the vehicle, ANP only needs to keep the vehicle stationary until the safe distance is restored and the corrected follow-up navigation point and the person being tracked are in the same quadrant of the vehicle coordinate system. During this period, ANP will continue to record new velocity vectors. Figure 8 In (b), the only difference is that the person turns to a different direction. In this case, the task can be continued by simply stopping the vehicle briefly so that the corrected following navigation point and the person being tracked are in the same quadrant of the vehicle coordinate system.

[0096] Human-vehicle following and accompanying is an important aspect of human-machine collaboration. When introducing traditional autonomous navigation algorithms into human-vehicle following or accompanying, firstly, the function of autonomous mode conversion is lacking. Secondly, because the dynamic characteristics of the person being followed are not taken into account, traditional autonomous navigation algorithms often give unreasonable or failed trajectories during the following and accompanying process.

[0097] The present application proposes a method of combining ANP and TEB. ANP can realize autonomous switching between following mode and accompanying mode by generating three initial navigation points at different positions and scoring them. At the same time, rolling window velocity vector calculation is adopted to give a more stable velocity vector direction when the person is moving straight. At the same time, ANP takes into account the dynamic characteristics of the person being followed, reasonably adopts parking and elastic following strategies, and gives the most reasonable and smooth trajectory, thereby improving the overall following or accompanying performance and improving the human-machine collaboration performance.

[0098] In an exemplary embodiment, a computer device is further provided, including a memory and a processor, wherein a computer program is stored in the memory, and the above method is implemented when the processor executes the computer program.

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

[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 used for analysis, stored data, displayed data, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of relevant data must comply with relevant regulations.

[0101] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiments can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to the 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 (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRdM), magnetic random access memory (MRdM), ferroelectric random access memory (FRdM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RdM) or external cache memory, etc. By way of illustration and not limitation, RdM may be in various forms, such as static random access memory (SRdM) or dynamic random access memory (DRdM).

[0102] The database involved in each embodiment provided in this application may include at least one of a relational database and a non-relational database. The non-relational database may include a distributed database based on blockchain, etc., but is not limited thereto. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but is not limited thereto.

[0103] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0104] This article uses specific examples to illustrate the principles and implementation methods of this application. The description of the above embodiments is only used to help understand the method and core ideas of this application. At the same time, for those skilled in the art, according to the ideas of this application, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting this application.

Claims

1. An autonomous navigation method for human-machine collaboration, characterized in that: include: Collect the 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 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, and determining the target current position of the tracked person; Determine the final velocity vector direction of the tracked person according to 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; Based on the target current position and the final velocity 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 velocity vector direction as the x-axis, and the y-axis determined by the right-hand rectangular coordinate system; Scoring the three initial navigation points respectively to determine scoring results; Based on the scoring result, determining the cooperation mode of the vehicle at the next moment; The collaboration mode includes a follow mode and an accompanying mode; The navigation trajectory of the vehicle is determined according to the cooperation mode of the vehicle at the next moment, the 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 bypass it and continue to move forward.

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 cooperation mode of the vehicle at the next moment based on the scoring results, the method further includes: According to the formula The three initial navigation points are corrected to obtain corrected navigation points, and the corrected navigation points are used 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 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; the coordinates of the corrected navigation points include the coordinates of the first accompanying navigation point as (0, a); the coordinates of the second accompanying navigation point as (0, -a); and the coordinates of the following navigation point as (-b, 0).

3. The autonomous navigation method for human-machine collaboration according to claim 2, characterized in that: Scoring the three initial navigation points respectively to determine the scoring results, specifically including: Determine the scoring result according to the formula total_score=β3(β1*basic_score+β2*obstacle_score); Among them, total_score represents the scoring result; basic_score represents the basic score of the initial navigation point; obstacle_score represents the obstacle score; β1, β2, and β3 represent the basic score weight, obstacle score weight, and mode protection score weight, respectively. The obstacle score in the follow mode is always set to 10. The obstacle score in the accompanying mode is determined as follows: Calculate the vertical distance t from the i-th obstacle point to the AB ray i , when t i When it 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 from the i-th obstacle point to the AB ray i for: in, (Ax, Ay) is the position of the accompanying navigation point A; v is the final velocity vector of the tracked object; (B x , B y ) is any point on AB ray, which is obtained by taking any point B on the ray parallel to v with A as the origin; is the modulus of the vector formed by points A and B; t i is the vertical distance from the i-th obstacle point to the AB ray; (O x , O y ) is the position of vehicle O; d1 is the projection length of vehicle O on AB ray, d2 is the distance of the obstacle in front of the vehicle detection accompanying navigation point; d i is the i-th obstacle point (P ix , P iy ) on the AB ray; (P ix , P iy ) is the location 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 cooperation mode of the vehicle at the next moment 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, it is determined that the cooperation mode of the vehicle at the next moment is 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 first 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 according to the next moment's cooperation mode of the vehicle, the preset parking conditions, the dynamic characteristics of the tracked person and the TEB planner, specifically including: The preset parking condition includes a first parking trigger condition and a second parking trigger 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 cooperation 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 that the tracked person encounters an obstacle in front and needs to bypass it and continue to move 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 cooperation 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; Based on the following navigation operation and the accompanying navigation operation, a navigation trajectory of the vehicle is determined.

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 position of the tracked person to determine the target current position 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: According to the angle between the velocity vector direction recorded by the unscented Kalman filter and the velocity vector direction recorded by the rolling window and the preset angle, the final velocity vector direction of the tracked person is determined, specifically including: Compare 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 obtain a target result; If the target result is yes, determine the final velocity vector using the velocity vector 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: The velocity vector direction is calculated using a rolling window to determine a final velocity vector, specifically including: Based on the target current position of the tracked person and the rolling window, five target position points within a second preset range of the target current position of the tracked person are determined; wherein the second preset range is within the rolling window; Determine the head point and the 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 as described in 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