An autonomous navigation system and method applied to a following unmanned cleaning vehicle, storage medium and computer program product

Abstract

The application discloses an autonomous navigation system applied to a following type unmanned cleaning vehicle, and comprises a local target point decision module, a trajectory planning module and a motion control module.The local target point decision module decides a local target point position and a heading angle to which the vehicle should arrive in each control period in real time according to perception information and a grid map.The trajectory planning module plans a smooth trajectory from a current position to the local target point by using an improved space-time hybrid A* algorithm on the basis of considering kinematics of the vehicle and dynamic environment constraints.The motion control module calculates a front wheel rotation angle and an acceleration control amount according to the planned trajectory based on a linear quadratic regulator control algorithm, and drives the vehicle to track the trajectory.The application has the advantages that a "person-vehicle-road" spatial relationship model is constructed, and the intelligent level and operation efficiency of the following type cleaning operation are effectively improved.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation for unmanned vehicles, and particularly to an autonomous navigation system and method, storage medium and computer program product for use in follow-type unmanned cleaning vehicles. Background Technology

[0002] With the development of smart cities and autonomous driving technology, low-speed unmanned cleaning vehicles are increasingly being used for urban road cleaning tasks. Existing cleaning vehicles mostly rely on predefined paths or remote control operation, lacking the ability to flexibly respond to the real-time location and operational status of cleaning personnel, making it difficult to achieve efficient human-machine collaborative operations. Especially in open urban road environments, the walking paths of cleaning personnel are random and dynamic, making it difficult for existing unmanned vehicle navigation systems to track cleaning personnel in real time and dynamically plan their travel trajectories while ensuring safety, resulting in low operational efficiency and poor collaboration.

[0003] In existing technologies, there has been some exploration of autonomous navigation for unmanned cleaning or following vehicles. Literature CN111596671A proposes a trajectory tracking control method for unmanned intelligent sweepers, mainly by tracking and controlling a pre-generated or given reference trajectory to enable the vehicle to travel along a designated path. However, such methods typically assume that the reference trajectory is known and continuous, focusing on trajectory tracking control itself. They struggle to cope with dynamic changes in the position of cleaning personnel, real-time generation of local work target points, and frequent changes in environmental obstacles during cleaning operations, lacking effective support for autonomous decision-making for dynamic targets and online planning of local trajectories. Literature CN116755446A proposes a trajectory tracking method for unmanned sweepers based on model predictive control. Although this scheme achieves static obstacle avoidance and dynamic obstacle following braking through an MPC controller combined with local path replanning, its strategy is relatively simple: it only adopts a braking and following strategy when facing dynamic targets, and does not consider the special process requirements of cleaning operations, such as edge cleaning. In actual operation, cleaning vehicles need to maintain a specific distance from the roadside while following personnel, and dynamically adjust the target point when personnel change direction. The aforementioned patents are mainly for driving on structured roads and lack a collaborative decision-making mechanism based on the spatial relationship between "people-vehicle-road". This leads to problems such as frequent starts and stops, losing track of personnel, or being unable to stay close to the edge in complex dynamic operation scenarios.

[0004] Specifically, the existing technology has the following problems: 1) Existing technologies mostly focus on tracking and controlling predetermined trajectories, lacking the ability to autonomously follow and make real-time decisions for dynamic operational objectives; 2) Traditional trajectory planning methods decouple three-dimensional trajectory planning into two-dimensional path planning and one-dimensional velocity planning, which makes it easy to fall into trajectory suboptimal in complex dynamic scenarios and cannot flexibly cope with complex dynamic scenarios with multiple objectives. 3) The existing system lacks a dedicated decision-making layer for the "follow-up operation" scenario, and cannot handle the complex spatial coupling relationship between cleaning personnel, roadside and vehicles, resulting in low human-machine collaboration efficiency and insufficient operation coverage.

[0005] There is an urgent need for an autonomous navigation system that can perceive the location of cleaning personnel, roadside information, and drivable areas in real time, and on this basis, realize dynamic target point decision-making, trajectory planning, and motion control to improve the intelligence level and operational efficiency of cleaning vehicles. This is the area that this application needs to focus on improving. Summary of the Invention

[0006] The technical problem to be solved by the present invention is to provide an autonomous navigation system and method, storage medium and computer program product for follow-up unmanned cleaning vehicles, which constructs a "human-vehicle-road" spatial relationship model and effectively improves the intelligence level and work efficiency of follow-up cleaning operations.

[0007] To address the above technical problems, this invention provides an autonomous navigation system for follow-type unmanned cleaning vehicles, comprising: Local target point decision module: Based on perception information and grid map, it makes real-time decisions on the location and heading angle of the local target point that the vehicle should reach in each control cycle; The sensed information includes the location of cleaning personnel and roadside parameters; A spatial relationship model of "human-vehicle-road" was constructed. Based on the positional relationship and distance information of the cleaning personnel and the unmanned vehicle relative to the same side or opposite side of the roadside, a rule-based adaptive strategy was adopted to dynamically switch between three modes: "autonomous following", "reference point following" and "circular search" to generate local target points that take into account safety, passability and efficiency. Trajectory planning module: Employs an improved spatiotemporal hybrid A* algorithm to plan a smooth trajectory from the current position to the local target point, taking into account vehicle kinematics and dynamic environmental constraints. Motion control module: Based on the Linear Quadratic Regulator (LQR) control algorithm, it calculates the front wheel steering angle and acceleration control quantities according to the planned trajectory, and drives the vehicle to follow the trajectory.

[0008] The present invention also provides an autonomous navigation method for use in follow-up unmanned cleaning vehicles, comprising the following steps: Step S1: Acquisition of upper-layer perception information, obtained through the communication mechanism between the vehicle-mounted sensors and ROS: The sensed information includes the location of cleaning personnel and roadside parameters; The position coordinates of the cleaning staff in the vehicle coordinate system ; Roadside parameters, wherein the roadside is represented by a linear model: ; in: These are the slope and intercept of the equation of the roadside line fitted in the vehicle coordinate system, respectively. A local two-dimensional grid map identifies drivable areas and obstacles. The grid map is a mapping of the vehicle's environment, which decomposes the space into interconnected but non-overlapping grids. Each grid is classified as occupied, free, or unknown based on its stored data value. A stored value of 0 represents "free," which is a drivable area without obstacles. A stored value of 100 represents "occupied," which is a non-drivable area with obstacles. A stored value of -1 represents "unknown," which is a non-drivable area with unknown risks.

[0009] Step S2: Local target point decision; Based on the relative positions of cleaning personnel and vehicles, and roadside information, a spatial relationship model of people, vehicles, and roads is constructed. Then, depending on the positional relationship of the cleaning personnel and the unmanned vehicle relative to the roadside (whether on the same side or opposite sides), a rule-based adaptive strategy is used to generate local target points. These local target points include target locations. and target heading angle .

[0010] The logical judgment for local target point decision-making is as follows: 1) When the roadside information is valid, the cleaning personnel and the unmanned vehicle are on the same side of the roadside, and the distance between the cleaning personnel and the roadside exceeds a certain distance, the vehicle enters automatic following mode. Prioritizing the cleaning personnel's position as guidance, the target point is generated using the circular arc method. The circular arc method is based on the vehicle's kinematics model. Given a starting point and an ending point, it generates a circular arc path that meets the vehicle's minimum turning radius constraint. A target point is selected on this path at a preset safe distance from the starting point, with its heading angle aligned with the tangent direction of the circular arc. 2) When the roadside information is valid, the cleaning staff and the unmanned vehicle are on the same side of the roadside but the distance between the cleaning staff and the roadside does not exceed a certain distance, or the cleaning staff and the unmanned vehicle are on opposite sides of the roadside, the reference point following mode is entered. The road structure is given priority, and the reference point is set at a point near the cleaning staff and at a safe distance from the roadside. Then, the target point is generated using the arc method. 3) When the roadside information is invalid, the circular search reference point mode is entered. The search is performed in the grid map with the cleaner as the center and the preset safe distance as the initial radius. If the intersection of the circle and the arc generated by the circular arc method with the cleaner's position as the endpoint is drivable in the grid map, the intersection is set as the reference point, and the target point is generated by the circular arc method. Otherwise, the radius of the search circle is expanded until a feasible reference point is found. After generating the target point, a passability check is performed to ensure that there is no risk of collision in the direction of the heading angle near the target point; if the check fails, the heading angle is finely adjusted within a preset range until it passes.

[0011] Step S3: Improved Spatiotemporal Hybrid A* Trajectory Planning; In the SLT 3D spatiotemporal grid map, an improved spatiotemporal hybrid A* algorithm is used for coarse trajectory search, unifying path and velocity planning. Specifically, this includes: node state expansion based on the vehicle kinematics model, considering front wheel steering angle and acceleration control; designing a heuristic function that comprehensively considers spatiotemporal Euclidean distance and desired vehicle speed; constructing a risk field model to assess dynamic obstacle collision risk; for the initial coarse trajectory obtained from the search, constructing its corresponding drivable spatiotemporal corridor, transforming complex obstacle avoidance constraints into linear constraints within a convex space; based on this, constructing a quadratic programming problem with trajectory smoothness, comfort, and safety as objectives, and solving it to obtain the final smooth trajectory used for tracking. .

[0012] Step S4: LQR-based motion control; based on the smooth trajectory planned in step S3. The LQR algorithm is used to calculate the motion control quantity required to track the trajectory, namely the front wheel steering angle. and acceleration Specifically, this includes: establishing a state error model encompassing vehicle lateral position deviation, heading angle deviation, lateral velocity deviation, and longitudinal velocity deviation; designing a cost function that considers both the integral of the state error and the rate of change of the control input, aiming to achieve the fastest error convergence with the minimum control input; and obtaining the optimal state feedback gain matrix by solving the algebraic Riccati equation. Based on the current state error vector Calculate the optimal control quantity: Ultimately, trajectory tracking is achieved.

[0013] Step S5: Control command issuance and vehicle execution. The calculated front wheel steering angle and acceleration commands are sent to the vehicle's underlying actuators via the CAN bus and ROS interface to control the vehicle to travel along the planned trajectory.

[0014] In addition, the present invention provides a storage medium, which is a computer-readable storage medium, on which a computer program is stored, which, when executed by a processor, implements the steps of the autonomous navigation method described above.

[0015] In addition, the present invention also provides a computer program product, including a computer program that, when executed by a processor, implements the steps of the autonomous navigation method as described above.

[0016] The one or more technical solutions proposed in this invention have at least the following beneficial effects: 1) Through a three-level collaborative mechanism, a safe and reasonable local target point can be determined even without complete upper-level information; 2) An improved spatiotemporal hybrid A* algorithm is employed for unified spatiotemporal planning, taking into account the collision risk of dynamic obstacles. Based on this, a spatiotemporal corridor is constructed to transform complex non-convex obstacle avoidance constraints into linear constraints within a convex space. Quadratic programming is then used for trajectory smoothing optimization, achieving a two-layer planning architecture from "feasible" to "smooth," effectively ensuring trajectory quality in complex dynamic scenarios. Compared to traditional spatiotemporal decoupled planning methods, this approach offers higher flexibility, security, and more reasonable planning results. 3) Through actual testing, the trajectory planning module of this invention has an average calculation time of less than 100ms, which meets the real-time control requirements of 10Hz, and the safe distance between the vehicle and the road edge is stably maintained at 30cm, which effectively improves the intelligence level and work efficiency of the following cleaning operation. Attached Figure Description

[0017] The accompanying drawings, which form part of this application, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an undue limitation of the invention. In the drawings: Figure 1 This is a flowchart illustrating a specific embodiment of the present invention; Figure 2 This is a schematic diagram of the decision logic of the local target point decision module in a specific embodiment of the present invention; Figure 3 This is a schematic diagram of the local target point pose decision-making module under different human-vehicle-road relationships in a specific embodiment of the present invention; Figure 4 A schematic diagram illustrating the node expansion method of the improved spatiotemporal hybrid A* algorithm according to a specific embodiment of the present invention; Figure 5 This is a schematic diagram of the trajectory planning result of the trajectory planning module in a specific embodiment of the present invention. Detailed Implementation

[0018] The embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Specific embodiments of the present invention will be described in detail. Those skilled in the art should understand that these descriptions are merely exemplary and not intended to limit the scope of protection of the present invention.

[0019] This invention provides an autonomous navigation system for follow-type unmanned cleaning vehicles, comprising: Local target point decision module: Based on perception information and grid map, it makes real-time decisions on the location and heading angle of the local target point that the vehicle should reach in each control cycle; The sensed information includes the location of cleaning personnel and roadside parameters; A spatial relationship model of "human-vehicle-road" was constructed. Based on the positional relationship and distance information of the cleaning personnel and the unmanned vehicle relative to the same side or opposite side of the roadside, a rule-based adaptive strategy was adopted to dynamically switch between three modes: "autonomous following", "reference point following" and "circular search" to generate local target points that take into account safety, passability and efficiency. Trajectory planning module: Employs an improved spatiotemporal hybrid A* algorithm to plan a smooth trajectory from the current position to the local target point, taking into account vehicle kinematics and dynamic environmental constraints. Motion control module: Lateral control uses a linear quadratic regulator (LQR), and longitudinal control uses a PID controller. The front wheel steering angle and acceleration control quantities are calculated based on the planned trajectory to drive the vehicle to follow the trajectory.

[0020] like Figure 1 As shown, the present invention provides an autonomous navigation method for a following unmanned cleaning vehicle, comprising the following steps: Step S0: Initialize system parameters. Start the autonomous navigation system of the unmanned cleaning vehicle, load preset parameters for each sensor, computing unit, and communication link, and initialize the valid flag bits of each module to false. Set the planning time domain T to 8s, the discrete time interval ∆t to 0.1s, the vehicle wheelbase L to 1300mm, the minimum turning radius to 3.31m, the front wheel angle range to [-22°, 22°], the speed range to [0, 10]m / s, the acceleration range to [-3, 2]m / s², the execution frequency of the local target point decision module and the trajectory planning module to 10Hz, and the execution frequency of the motion control module to 100Hz.

[0021] Step S1: Obtain upper-layer information. Obtain local 2D raster map information and cleaning personnel coordinates through the ROS topic communication mechanism. With roadside parameters When a valid message is received, the corresponding flag is set to true.

[0022] Step S2: Local Target Point Decision. First, check if a valid local 2D grid map and cleaning personnel location information have been received. If the flag for either item is false, return to step S1. The formulas for calculating and determining the distance from the cleaning staff to the roadside and the positional relationship between the cleaning staff and the unmanned vehicle relative to the roadside are as follows: ; ; Based on the above judgment results, such as Figure 2 As shown, the corresponding target point generation strategy is executed. Figure 3 This further illustrates schematic diagrams of local target point pose decision-making under different human-vehicle-road relationships in specific embodiments of the present invention. Specifically, the autonomous navigation system determines the pose based on the same-side or opposite-side relationship between the cleaning personnel and the vehicle relative to the roadside, as well as a distance threshold. Figure 3 The diagram illustrates the dynamic generation of target point positions and heading angles for different scenarios: "same side and far distance (autonomous following)" and "same side and close distance / opposite side (reference point following)". Taking the "autonomous following" mode as an example, the starting point is the center of the vehicle's rear axle, and the ending point is the cleaning personnel's position. An arc method is used, and the formula is as follows: ; in: It is the vehicle's minimum turning radius. and It is the distance difference between a given endpoint and a given starting point in the vehicle coordinate system. It is the set safe following distance. and As an intermediate variable, , and This outputs the target point's position and heading.

[0023] Step S3: Improved Spatiotemporal Hybrid A* Trajectory Planning. Upon receiving the local target point published in Step S1, the improved spatiotemporal hybrid A* planner is initiated. This planner employs a hierarchical planning strategy: First, a coarse search is performed in the spatiotemporal domain using the improved spatiotemporal hybrid A* algorithm to obtain an initial trajectory; subsequently, the initial trajectory is smoothed and post-processed by constructing a spatiotemporal corridor and using numerical optimization methods.

[0024] Step S31: Construct a spatiotemporal grid map. In the Frenet coordinate system, using the line connecting the start and end points as a reference, construct an sl coordinate system, where the s-axis represents the longitudinal displacement along the line, and the l-axis represents the lateral displacement perpendicular to the line. Transform the coordinates of the vehicle, local target points, and obstacles in the two-dimensional grid map to the aforementioned sl coordinate system. Set the planning time domain T and discrete time resolution Δt, and expand the sl map along the discrete time axis t into a three-dimensional slt spatiotemporal grid map. In this map, stationary objects are represented as cylindrical bodies occupying the same sl region along the time axis t, while dynamic obstacles are mapped to occupying grids in slt space that vary with t, appearing as tilted tubular bodies. Therefore, compared to a two-dimensional grid map, using a spatiotemporal grid map clearly describes the positional relationship and relative motion between the vehicle and obstacles over a future period.

[0025] Step S32: Spatiotemporal node expansion. The state space of the spatiotemporal node is represented as follows: The first three terms are consistent with the traditional A* algorithm, where s and l represent the vehicle's longitudinal and lateral positions, respectively. Let v represent the heading angle, v represent the velocity state variable, and t represent the time relative to the current node of the vehicle's position. ; The front wheel steering angle is determined based on vehicle kinematic constraints. exist The internal components are uniformly discretized to obtain a discrete set of front wheel steering angles. as follows: ; in: Similarly, determine the upper and lower limits of vehicle acceleration. Then, the vehicle acceleration is discretized non-uniformly using a variable step size method. ,exist The discrete acceleration set A is obtained by sampling accelerations at intervals that increase progressively from 0 to the left and right within the range, as follows: ; For each parent node to be expanded Initially, based on the aforementioned discretized front wheel steering angle and acceleration Expand child nodes. Figure 4 This diagram illustrates the node expansion method of the improved spatiotemporal hybrid A* algorithm according to a specific embodiment of the present invention. Figure 4 In this algorithm, the improved spatiotemporal hybrid A* algorithm generates a series of child nodes in the SLT spatiotemporal grid at the next time step, based on the current state of the parent node and by applying different discrete front wheel steering angles and acceleration control values. The spatiotemporal expansion process is represented as follows: ; Where: L is the vehicle wheelbase, and dt is the time step between adjacent nodes. The node state is updated using equation (7), and collision detection is performed on each new child node based on the spatiotemporal grid map to ensure that no collision occurs between the child node and the obstacle. If a collision occurs, the extended node is discarded; otherwise, the child node is considered a feasible node and added to the set to be explored. The discrete front wheel angle set is traversed. Given a discrete acceleration set A, a series of child nodes can be obtained by expanding based on the parent node.

[0026] Step S33: Design of Heuristic and Cost Functions. The spatiotemporal Euclidean distance is used as the heuristic function for any node. to the target point The heuristic function is: ; in, These are weighting coefficients used to guide the search towards the target region, avoiding wasting computational resources in hopeless directions; from the starting node to any node. The actual cost Cost from parent node The sum of the transition costs is as follows: ; in, As a cost for target deviation, penalize the current node's speed. With expected speed The deviation encourages vehicles to travel at efficient speeds. As a penalty for centerline offset, the horizontal position of the current node is penalized. With reference line The offset encourages the vehicle to travel in the reference direction. To account for safety costs, based on the risk field model, obstacles are considered as risk sources. The closer a node is to a risk source, the higher its safety cost. The risk field of dynamic obstacles is directional, with the risk distribution being stronger in the direction of movement. These are the corresponding weight coefficients. The three cost functions mentioned above are expressed as follows: ; ; ; in: and These represent the coordinates of the center locations of the node and the obstacle risk source, respectively. and These represent the distribution factors of the obstacle in the s and l directions, respectively. The distribution factors are related to the outline of the obstacle and its acceleration in the s and l directions. Let be the position vector of the node relative to the center of the risk source. For vectors Speed ​​of obstacles The angle between them, since the risk field of a static obstacle is isotropic, therefore its It is always 0.

[0027] Step S34: Spatiotemporal Hybrid A* Algorithm Flow. Based on the aforementioned definition, the current location of the vehicle is taken as the starting node for the search. Spatiotemporal node expansion is performed based on equation (7). Node collision detection is considered during the expansion process, and node cost evaluation is performed based on equations (8) and (9) to find the optimal expansion node. This process continues until the target node is found, completing the spatiotemporal hybrid A* search process and obtaining the result from discrete nodes. The initial coarse trajectory is represented by the spatiotemporal hybrid A* algorithm, whose input values ​​are: initial state, target state, grid map, vehicle parameters, and cost function weights; the output value is: coarse trajectory.

[0028] Step S35: Trajectory Optimization. This invention employs a numerical optimization method for post-processing smoothing. First, a spatiotemporal corridor is constructed, and a safe convex feasible region is opened for each node along the initial trajectory. Specifically, in each time slice... , with trajectory points Centered on, along and The direction expands until it encounters the nearest obstacle or boundary, forming a rectangular feasible region at that moment. The feasible regions of all time slices are chained together to form a continuous spatiotemporal corridor, which transforms complex non-convex obstacle constraints into a series of convex linear constraints. Then, a quadratic programming problem is constructed and solved within this corridor, with the objective function defined as: ; in: and Let x represent the horizontal and vertical positions of the initial trajectory point. , and , These are the lateral and longitudinal velocities and accelerations, respectively. to The weighting coefficients are used to minimize the deviation of the trajectory from the initial solution, lateral sway, acceleration variation, and deviation from the desired velocity; the constraints include: 1) All trajectory points must be located within the spacetime corridor, i.e. and ; 2) Velocity, acceleration, and jerk must meet the upper limit of vehicle dynamics; 3) Adjacent trajectory points must meet higher-order continuity constraints to ensure trajectory smoothness, where the lateral direction of the trajectory points... Must meet: ; in: By solving this quadratic programming problem, an optimized trajectory is finally output that can be directly tracked by the control module.

[0029] Step S4: Motion control strategy with lateral and longitudinal decoupling; lateral control employs a linear quadratic regulator (LQR). First, an error dynamics model for vehicle path tracking is established, with its state error vector defined as: ; in: This is the lateral position error. This represents the rate of change of the lateral error. For heading angle error, This represents the rate of change of heading angle error; Secondly, a quadratic cost function is designed based on a linearized vehicle model at the current vehicle speed: ; Where: Q is the semi-positive definite state weight matrix, R is the positive definite control weight matrix, and u is the control input, representing the front wheel steering angle. ; Then, the optimal feedback gain matrix K is obtained by solving the standard Riccati equation; Finally, in each control cycle, the optimal control quantity is calculated based on the real-time state error. ; Longitudinal control uses a PID controller, based on speed error. Calculate acceleration command : ; in, It is the reference vehicle speed at the reference trajectory point. That is the actual vehicle speed. These are the proportional coefficient, integral coefficient, and differential coefficient, respectively.

[0030] Step S5: Issuance of control commands. This involves issuing control commands to the specified quantities. and After amplitude limiting, the signal is encapsulated and periodically sent to the chassis controller via the vehicle CAN bus protocol and ROS communication mechanism to drive the vehicle to execute.

[0031] By performing the above steps, this invention enables unmanned cleaning vehicles to automatically follow cleaning personnel during urban road cleaning operations, ensuring the safe and efficient completion of cleaning tasks. Actual testing shows that, in the Tongji University campus road environment, the target decision module's average calculation time is less than 1ms. The selection of target points ensures the safety of personnel and vehicles, current accessibility, and subsequent drivability. Furthermore, the following distance is adjustable, with the minimum distance between the vehicle and the road edge maintained at 30cm. Figure 5The diagram shows the measured results of the trajectory planning module described in this invention. The blue straight line represents the schematic line connecting the starting point and the ending point, and the green curve represents the final trajectory generated, conforming to vehicle kinematics and successfully avoiding obstacles. The average calculation time of the planning module is less than 100ms, meeting the real-time control requirement of 10Hz. The motion control module calculates the corresponding lateral and longitudinal control quantities based on the trajectory information and sends them to the underlying actuators to control the vehicle to travel along the planned trajectory.

[0032] This invention is a system-level integration and collaboration for specific scenarios. It seamlessly integrates the above-mentioned multimodal decision-making, spatiotemporal unified planning and high-precision motion control, LQR lateral control and PID longitudinal control into a complete system for low-speed operation scenarios of "following unmanned cleaning vehicles". Through parameter configuration and collaborative design, the system achieves overall real-time performance, safety and comfort.

[0033] The present invention also provides a computer-readable storage medium having computer-readable program instructions stored thereon, the computer-readable program instructions being used to execute the autonomous navigation method in the above embodiments.

[0034] The computer-readable storage medium provided in this application is, for example, a USB flash drive, but is not limited to electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices, or any combination thereof. More specific examples of computer-readable storage media include, but are not limited to: electrical connections having one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof. In this embodiment, the computer-readable storage medium is any tangible medium that contains or stores a program that is used by or in conjunction with an instruction execution system, system, or device. The program code contained on the computer-readable storage medium is transmitted using any suitable medium, including but not limited to: wires, optical fibers, radio frequency (RF), etc., or any suitable combination thereof.

[0035] The present invention also provides a computer program product, including a computer program that, when executed by a processor, implements the steps of the autonomous navigation method as described above.

[0036] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. An autonomous navigation system for use in follow-type unmanned cleaning vehicles, characterized in that: include: Local target point decision module: Based on perception information and grid map, it makes real-time decisions on the location and heading angle of the local target point that the vehicle should reach in each control cycle; Trajectory planning module: Employs an improved spatiotemporal hybrid A* algorithm to plan a smooth trajectory from the current position to the local target point, taking into account vehicle kinematics and dynamic environmental constraints. Motion control module: Based on the linear quadratic regulator control algorithm, it calculates the front wheel steering angle and acceleration control quantities according to the planned trajectory, and drives the vehicle to follow the trajectory.

2. The autonomous navigation system for a following unmanned cleaning vehicle according to claim 1, characterized in that: The local target point decision module constructs a "human-vehicle-road" spatial relationship model. Based on the positional relationship and distance information of the cleaning personnel and the unmanned vehicle relative to the same or opposite side of the roadside, it adopts a rule-based adaptive strategy to dynamically switch between three modes: "autonomous following", "reference point following" and "circular search" to generate local target points that take into account safety, passability and efficiency.

3. The autonomous navigation system for a following unmanned cleaning vehicle according to claim 1, characterized in that: The sensed information includes the location of cleaning staff and roadside parameters.

4. An autonomous navigation method applied to a following unmanned cleaning vehicle, characterized in that: The steps include the following: Step S1: Acquisition of upper-layer perception information, obtained through the communication mechanism between the vehicle-mounted sensors and ROS: The sensed information includes the location of cleaning personnel and roadside parameters; The position coordinates of the cleaning staff in the vehicle coordinate system ; Roadside parameters, wherein the roadside is represented by a linear model: ; in: These are the slope and intercept of the equation of the roadside line fitted in the vehicle coordinate system, respectively. A local 2D grid map identifies drivable areas and obstacles; Step S2: Local target point decision; Based on the relative positional relationship between the cleaning personnel and the vehicle and the roadside information, a human-vehicle-road spatial relationship model is constructed, and based on the positional relationship between the cleaning personnel and the unmanned vehicle relative to the same side or opposite side of the roadside, a rule-based adaptive strategy is used to generate local target points; After generating the target points, a passability detection is performed to ensure that there is no collision risk in the direction of the heading angle near the target point; If the detection fails, the heading angle is finely adjusted within a preset range until it passes. Step S3: Improved spatiotemporal hybrid A* trajectory planning; In the SLT 3D spatiotemporal grid map, the improved spatiotemporal hybrid A* algorithm is used for coarse trajectory search, and path and velocity planning are processed in a unified manner; Step S4: LQR-based motion control; based on the smooth trajectory planned in step S3. The LQR algorithm is used to calculate the motion control quantity required to track the trajectory, namely the front wheel steering angle. and acceleration ; Step S5: The calculated front wheel steering angle and acceleration commands are sent to the vehicle's underlying actuators via the CAN bus and ROS interface to control the vehicle to travel along the planned trajectory.

5. The autonomous navigation method for a following unmanned cleaning vehicle according to claim 4, characterized in that: The grid map in step S1 is a mapping of the vehicle environment. It decomposes the space into interconnected but non-overlapping grids. Each grid is classified into occupied, free, or unknown states according to its stored data value. A storage value of 0 represents "free", which is a drivable area without obstacles. A storage value of 100 represents "occupied", which is a drivable area with obstacles. A storage value of -1 represents "unknown", which is a drivable area with unknown risks.

6. The autonomous navigation method for a following unmanned cleaning vehicle according to claim 4, characterized in that: The local target point includes the target location. and target heading angle ; The logical judgment for local target point decision-making is as follows: 1) When the roadside information is valid, the cleaning personnel and the unmanned vehicle are on the same side of the roadside and the distance between the cleaning personnel and the roadside exceeds a certain distance, the automatic following mode is entered. The cleaning personnel's position is given priority as the guide, and the target point is generated by the circular arc method. The circular arc method is based on the vehicle kinematics model. Given the starting point and the ending point, a circular arc path that meets the minimum turning radius constraint of the vehicle is generated, and the target point is selected at a preset safe distance from the starting point on the path. Its heading angle is consistent with the direction of the circular arc tangent. 2) When the roadside information is valid, the cleaning staff and the unmanned vehicle are on the same side of the roadside but the distance between the cleaning staff and the roadside does not exceed a certain distance, or the cleaning staff and the unmanned vehicle are on opposite sides of the roadside, the reference point following mode is entered. The road structure is given priority, and the reference point is set at a point near the cleaning staff and at a safe distance from the roadside. Then, the target point is generated using the arc method. 3) When the roadside information is invalid, the circular search reference point mode is entered. The search is performed in the grid map with the cleaner as the center and the preset safe distance as the initial radius. If the intersection of the circle and the arc generated by the circular arc method with the cleaner's position as the endpoint is drivable in the grid map, the intersection is set as the reference point, and the target point is generated by the circular arc method. Otherwise, the radius of the search circle is expanded until a feasible reference point is found.

7. The autonomous navigation method for a following unmanned cleaning vehicle according to claim 4, characterized in that: Step S3 specifically includes: expanding the node state based on the vehicle kinematics model, considering the front wheel steering angle and acceleration control; designing a heuristic function that comprehensively considers the spatiotemporal Euclidean distance and the desired vehicle speed; constructing a risk field model to assess the dynamic obstacle collision risk; constructing a corresponding drivable spatiotemporal corridor for the initial coarse trajectory obtained from the search, transforming the complex obstacle avoidance constraints into linear constraints within a convex space; and on this basis, constructing a quadratic programming problem with trajectory smoothness, comfort, and safety as objectives, and solving it to obtain the final smooth trajectory used for tracking. .

8. The autonomous navigation method for a following unmanned cleaning vehicle according to claim 4, characterized in that: Step S4 specifically includes: establishing a state error model including vehicle lateral position deviation, heading angle deviation, lateral velocity deviation, and longitudinal velocity deviation; designing a cost function that simultaneously considers the integral of the state error and the rate of change of the control quantity, aiming to achieve the fastest error convergence with the minimum control quantity; and obtaining the optimal state feedback gain matrix by solving the algebraic Riccati equation. Based on the current state error vector Calculate the optimal control quantity: Ultimately, trajectory tracking is achieved.

9. A storage medium, characterized in that: The storage medium is a computer-readable storage medium, and a computer program is stored on the storage medium. When the computer program is executed by a processor, it implements the steps of the autonomous navigation method according to any one of claims 4 to 8.

10. A computer program product, characterized in that: It includes a computer program that, when executed by a processor, implements the steps of the autonomous navigation method according to any one of claims 4 to 8.

Images