Navigation method for a metrological device unmanned delivery vehicle

Abstract

The application discloses a navigation method for metering equipment unmanned distribution vehicles, and belongs to the technical field of unmanned vehicle navigation. The method comprises collecting road image data, radio frequency signal strength indication data and real-time pose data, extracting path features and target guide features, using a visual path tracking model based on a pure tracking algorithm to obtain a first control instruction for maintaining the metering equipment unmanned distribution vehicle to travel along a road center line, using a radio frequency signal approach model to calculate target site angle deviation to obtain a second control instruction for guiding the metering equipment unmanned distribution vehicle to move towards a metering equipment site, determining a navigation state according to the distance between the metering equipment unmanned distribution vehicle and the metering equipment site, outputting a final control instruction to an execution end, and constructing an early warning feedback mechanism and generating a real-time trajectory of the metering equipment unmanned distribution vehicle. The application realizes stable control from long-distance cruising to short-distance accurate parking through navigation state switching and dynamic speed regulation, and through the cooperation of visual cruising and radio frequency guidance.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of unmanned vehicle navigation, and particularly relates to a navigation method of an unmanned delivery vehicle for metrological equipment. BACKGROUND

[0002] The unmanned delivery vehicle for metrological equipment is an unmanned vehicle applied to the automatic flow scene of metrological instruments and metrological equipment. In large factories, industrial parks and research institutions, the number of metrological equipment is large and widely distributed. The traditional manual taking and sending method is inefficient and prone to errors. Therefore, as an intelligent logistics carrier, the core task of the unmanned delivery vehicle for metrological equipment is to automatically complete the physical transfer of metrological equipment to be tested and metrological equipment that has been tested between the metrological equipment use site and the centralized test center according to the preset instructions. Through the unmanned delivery vehicle for metrological equipment, the automation and intelligentization of the whole life cycle management of metrological equipment are realized, and the accuracy and safety of the production process are ensured.

[0003] In order to solve the problem of cooperative navigation between long-distance path tracking and accurate parking at the end point of the unmanned delivery vehicle for metrological equipment, the prior art adopts a single navigation mode for full-process processing, such as a method of relying only on visual sensors to drive along road markings and a method of relying only on general environmental features for positioning. However, when the vehicle drives to the vicinity of the target site, the road markings become blurred and missing, and in areas with dense sites, the similarity of general environmental features is high, which makes it difficult for the single navigation mode to accurately identify the specific target parking point, and further causes the vehicle to overshoot and hover due to improper speed control near the target site, which cannot be stably parked, and reduces the overall efficiency and success rate of the automatic transfer process. SUMMARY

[0004] The present application aims to provide a navigation method of an unmanned delivery vehicle for metrological equipment, which realizes stable control from long-distance cruising to short-distance accurate parking through navigation state switching and dynamic speed regulation, and cooperates with visual cruising and radio frequency guidance.

[0005] To solve the above technical problems, the technical solution adopted by the present application is:

[0006] The navigation method of the unmanned delivery vehicle for metrological equipment comprises the following steps:

[0007] S1, collecting and preprocessing road image data, radio frequency signal strength indication data and real-time pose data of the unmanned delivery vehicle for metrological equipment;

[0008] S2, for the preprocessed road image data, using Hough transform to extract the position coordinates of the road center line as the path feature, and extracting the current signal strength value in the preprocessed radio frequency signal strength indication data as the target guidance feature;

[0009] S3, inputting the path feature into a visual path tracking model based on a pure tracking algorithm to obtain a first control instruction for maintaining the metering equipment unmanned delivery vehicle to travel along the road center line;

[0010] S4, inputting the target guide feature into a radio frequency signal approach model to obtain a second control instruction for guiding the metering equipment unmanned delivery vehicle to move towards the metering equipment station;

[0011] S5, setting a close distance threshold and a parking distance threshold according to the distance between the metering equipment unmanned delivery vehicle and the metering equipment station, determining the navigation state of the vehicle, combining the first control instruction and the second control instruction according to the navigation state, and outputting a final control instruction to an execution end, the process being:

[0012] When the distance is greater than or equal to the close distance threshold, it is determined to be a cruising state, the weight of the first control instruction is increased, and the vehicle travels at a preset cruising speed;

[0013] When the distance is less than the close distance threshold but greater than the parking distance threshold, it is determined to be a close state, the weight of the second control instruction is increased, and the travel speed is proportional to the distance;

[0014] When the distance is less than or equal to the parking distance threshold, it is determined to be a parking state, the second control instruction is used for guidance, and the vehicle travels at a preset crawling speed until the metering equipment unmanned delivery vehicle stops at the metering equipment station;

[0015] S6, when the radio frequency signal strength indication data cannot be continuously collected and the path feature cannot be extracted, a warning mechanism is triggered, and the pose deviation between the final control instruction and the preprocessed real-time pose data is calculated for feedback optimization.

[0016] Preferably, in S1, the specific method of collection and preprocessing is:

[0017] S11, collecting and preprocessing road image data, including:

[0018] S111, collecting a continuous video stream in the direction of travel of the vehicle through a vehicle-mounted image sensor deployed at the front end of the metering equipment unmanned delivery vehicle, and extracting a static image frame therefrom as road image data;

[0019] S112, performing grayscale processing on the road image data, converting the three-channel color image in the road image data into a single-channel grayscale image, and performing convolution operation on the grayscale image using an edge detection operator to obtain preprocessed road image data;

[0020] S12, collecting and preprocessing radio frequency signal strength indication data, including:

[0021] S121, periodically receiving a radio frequency signal transmitted by a radio frequency beacon preset at a metering device site through a radio frequency signal receiver deployed on the metering device unmanned delivery vehicle, and parsing radio frequency signal strength indication data;

[0022] S122, applying a moving average filtering algorithm to the continuously collected radio frequency signal strength indication data sequence for smoothing processing, taking the radio frequency signal strength of the current sampling point and the previous N-1 sampling points of the current sampling point for arithmetic average, and taking the obtained arithmetic average value as the preprocessed radio frequency signal strength indication data at the current time, wherein N is the size of the filtering window;

[0023] S13, collecting and preprocessing real-time pose data, including:

[0024] S131, collecting the absolute position coordinates, angular velocity and acceleration of the vehicle through the global navigation satellite system receiver and the inertial measurement unit deployed on the metering device unmanned delivery vehicle;

[0025] S132, inputting the absolute position coordinates, angular velocity and acceleration into the Kalman filter for data fusion, and outputting the optimal estimation value as the preprocessed real-time pose data.

[0026] Preferably, in S2, the position coordinates of the road center line are extracted by Hough transform as the path feature, and the process of extracting the current signal strength value in the preprocessed radio frequency signal strength indication data as the target guide feature includes:

[0027] S21, mapping each edge point coordinate in the preprocessed road image data from the image coordinate system to the Hough parameter space, the mapping logic being: converting the point in the image coordinate system into a sinusoidal curve in the Hough parameter space, the parameter of the point on the sinusoidal curve corresponding to the straight line passing through the point in the image coordinate system;

[0028] S22, performing peak detection in the Hough parameter space, and identifying the parameter group corresponding to the two detected peak points as the left and right road edge lines in the image coordinate system;

[0029] S23, in the image coordinate system, calculating the midpoints of the left and right road edge lines at the preset horizontal position, and taking the coordinate set of the midpoints as the position coordinates of the road center line, the position coordinates of the road center line constituting the path feature;

[0030] S24, taking the preprocessed radio frequency signal strength indication data at the current time as the current signal strength value, and the current signal strength value constituting the target guide feature.

[0031] Preferably, in S3, the process of deriving the first control instruction for maintaining the metering device unmanned delivery vehicle driving along the road center line includes:

[0032] S31, according to the pre-processed real-time pose data, searching for the path point closest to the rear axle center of the unmanned delivery vehicle from the position coordinates of the road center line in the path feature;

[0033] S32, searching forward from the closest path point along the position coordinates of the road center line to determine the path point with a preset preview distance from the rear axle center of the unmanned delivery vehicle as the preview point;

[0034] S33, calculating the included angle between the current heading of the unmanned delivery vehicle and the direction vector from the rear axle center of the unmanned delivery vehicle to the preview point as the path tracking angle deviation;

[0035] S34, according to the path tracking angle deviation, the preset preview distance, and the front-rear axle distance of the unmanned delivery vehicle, calculating the first control instruction to construct a visual path tracking model, and the first control instruction is the steering angle.

[0036] Preferably, in S4, the process of obtaining the second control instruction for guiding the unmanned delivery vehicle to move towards the metering device station includes:

[0037] S41, according to the pre-processed real-time pose data and the preset position of the metering device station, calculating the direction vector from the current position of the unmanned delivery vehicle to the metering device station;

[0038] S42, calculating the included angle between the current heading of the unmanned delivery vehicle and the direction vector as the target station angle deviation;

[0039] S43, constructing the radio frequency signal approaching model as a proportional controller, and the proportional controller obtains the second control instruction by multiplying the target station angle deviation by a preset proportional gain coefficient.

[0040] Preferably, in S6, when the radio frequency signal strength indication data cannot be continuously collected and the path feature cannot be extracted, the process of triggering the early warning mechanism includes:

[0041] S61, setting a continuous failure counter, when the radio frequency signal strength indication data cannot be successfully collected and the path feature cannot be successfully extracted within a collection period, the continuous failure counter is incremented by one, and when the radio frequency signal strength indication data is successfully collected and the path feature is successfully extracted, the continuous failure counter is cleared;

[0042] S62, judging whether the value of the continuous failure counter reaches a preset count threshold, if the count threshold is reached, triggering the early warning mechanism and sending an early warning signal to the external monitoring system.

[0043] Preferably, in S6, the pose deviation of the final control instruction and the preprocessed real-time pose data is calculated, and the process for feedback optimization comprises:

[0044] S63, according to the final control instruction and the preset vehicle kinematics model, the target pose of the unmanned delivery vehicle of the metering device at the next moment is predicted;

[0045] S64, the difference between the target pose and the preprocessed real-time pose data collected at the next moment in position and direction is extracted as the pose deviation;

[0046] S65, the pose deviation is input into the optimization algorithm composed of the proportional-integral-derivative controller as a new error term, and the adjustment range of the approaching distance threshold, the parking distance threshold, the first control instruction weight and the second control instruction weight is adjusted online, wherein the proportional-integral-derivative controller calculates the adjustment range according to the size, the cumulative amount and the change rate of the pose deviation.

[0047] Preferably, it further comprises: S7, connecting the continuously collected real-time pose data as the real-time trajectory of the unmanned delivery vehicle of the metering device, and associating the radio frequency signal strength indication data with the corresponding real-time pose data to generate a radio frequency signal strength distribution map, and the specific process is:

[0048] S71, the preprocessed real-time pose data collected continuously is stored in time sequence, and the data points in the real-time pose data are connected in turn in the two-dimensional coordinate system to form a real-time trajectory;

[0049] S72, generating a radio frequency signal strength distribution map, comprising:

[0050] S721, the preprocessed radio frequency signal strength indication data at each collection time is bound with the preprocessed real-time pose data at the same collection time to form a data pair containing radio frequency signal strength information and position information;

[0051] S722, in the two-dimensional coordinate system, the data pair is labeled according to the position information, and the position of the data pair is assigned a preset color value according to the size of the radio frequency signal strength information, and the color values of the data pairs together constitute the radio frequency signal strength distribution map.

[0052] The present application has the beneficial effects of:

[0053] The present application sets three navigation states of cruising, approaching and parking, cooperates with pure tracking visual algorithm and radio frequency guidance, solves the stable transition problem of long-distance path tracking and short-distance accurate parking, uses the distance-based state switching mechanism and dynamic speed adjustment, so that the navigation strategy can be adjusted adaptively according to the position relationship with the target, and the applicability of completing accurate docking in complex environment is enhanced.

[0054] The present application effectively avoids overshoot and hovering of the metering device unmanned delivery vehicle near the target station by controlling the navigation mode and driving speed in stages, and significantly improves the success rate of the automatic parking process and the reliability of the system operation. BRIEF DESCRIPTION OF DRAWINGS

[0055] Figure 1 A flowchart of the method of the present application;

[0056] Figure 2 A simulation diagram of the metering device unmanned delivery vehicle in the cruising state during the verification process of the present application;

[0057] Figure 3 A simulation diagram of the metering device unmanned delivery vehicle entering the approaching state during the verification process of the present application;

[0058] Figure 4 A simulation diagram of the metering device unmanned delivery vehicle entering the parking state during the verification process of the present application;

[0059] Figure 5 A simulation diagram of the metering device unmanned delivery vehicle completing parking during the verification process of the present application. DETAILED DESCRIPTION

[0060] The technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the accompanying drawings.

[0061] Embodiment 1: As shown in the figure, the navigation method of the metering device unmanned delivery vehicle includes the following steps: Figure 1

[0062] S1, collecting and pre-processing road image data, radio frequency signal strength indication data and real-time pose data of the metering device unmanned delivery vehicle;

[0063] S2, using Hough transform to extract the position coordinates of the road center line as the path feature for the pre-processed road image data, and extracting the current signal strength value in the pre-processed radio frequency signal strength indication data as the target guiding feature;

[0064] S3, inputting the path feature into a visual path tracking model based on a pure tracking algorithm to obtain a first control instruction for maintaining the metering device unmanned delivery vehicle driving along the road center line;

[0065] S4, inputting the target guiding feature into a radio frequency signal approach model to obtain a second control instruction for guiding the metering device unmanned delivery vehicle to move towards the metering device station;

[0066] ​S5, determine the navigation state of the vehicle according to the distance between the metering equipment unmanned delivery vehicle and the metering equipment site, combine the first control instruction and the second control instruction according to the navigation state, and output the final control instruction to the execution end;

[0067] S6, when continuous radio frequency signal strength indication data cannot be collected and path characteristics cannot be extracted, triggering a warning mechanism, and calculating the pose deviation between the final control instruction and the preprocessed real-time pose data for feedback optimization.

[0068] In S1, the specific method of collection and preprocessing is:

[0069] S11, collecting and preprocessing road image data, including:

[0070] S111, collecting continuous video streams in the driving direction of the vehicle through the vehicle-mounted image sensor deployed at the front end of the metering equipment unmanned delivery vehicle, and extracting static image frames as road image data from the continuous video streams;

[0071] S112, performing grayscale processing on the road image data, converting the three-channel color image in the road image data into a single-channel grayscale image, and performing convolution operation on the grayscale image using an edge detection operator to obtain preprocessed road image data;

[0072] S12, collecting and preprocessing radio frequency signal strength indication data, including:

[0073] S121, periodically receiving radio frequency signals transmitted by radio frequency beacons preset at metering equipment sites through the radio frequency signal receiver deployed on the metering equipment unmanned delivery vehicle, and parsing the radio frequency signal strength indication data;

[0074] S122, applying a moving average filtering algorithm to the sequence of continuously collected radio frequency signal strength indication data for smoothing processing, taking the radio frequency signal strength of the current sampling point and the previous N-1 sampling points for arithmetic average, and taking the obtained arithmetic average value as the preprocessed radio frequency signal strength indication data at the current time, wherein N is the size of the filtering window;

[0075] S13, collecting and preprocessing real-time pose data, including:

[0076] S131, collecting the absolute position coordinates, angular velocity and acceleration of the vehicle through the global navigation satellite system receiver and the inertial measurement unit deployed on the metering equipment unmanned delivery vehicle;

[0077] S132, inputting the absolute position coordinates, angular velocity and acceleration into the Kalman filter for data fusion, and outputting the optimal estimate value as the preprocessed real-time pose data.

[0078] In S2, the position coordinates of the road center line extracted by the Hough transform are used as the path feature, and the current signal strength value in the preprocessed radio signal strength indication data is used as the target guiding feature, and the process includes:

[0079] In S21, each edge point coordinate in the preprocessed road image data is mapped from the image coordinate system to the Hough parameter space, and the mapping logic is that the point in the image coordinate system is converted into a sinusoidal curve in the Hough parameter space, and the parameters of the points on the sinusoidal curve correspond to the straight lines passing through the points in the image coordinate system.

[0080] In S22, peak detection is performed in the Hough parameter space, and the parameter set corresponding to the two peak points detected is identified as the left and right road edge lines in the image coordinate system.

[0081] In S23, in the image coordinate system, the midpoints of the left and right road edge lines at the preset horizontal position are calculated, and the coordinate set of the midpoints is taken as the position coordinates of the road center line, and the position coordinates of the road center line constitute the path feature.

[0082] In S24, the preprocessed radio signal strength indication data at the current time is taken as the current signal strength value, and the current signal strength value constitutes the target guiding feature.

[0083] In S3, the process of deriving the first control instruction for maintaining the metering equipment unmanned delivery vehicle driving along the road center line includes:

[0084] In S31, according to the preprocessed real-time pose data, the nearest path point to the rear axle center of the metering equipment unmanned delivery vehicle is found on the position coordinates of the road center line in the path feature.

[0085] In S32, the nearest path point is found along the position coordinates of the road center line to determine the path point with a preset preview distance from the rear axle center of the metering equipment unmanned delivery vehicle as the preview point.

[0086] In S33, the included angle between the current heading of the metering equipment unmanned delivery vehicle and the direction vector from the rear axle center of the metering equipment unmanned delivery vehicle to the preview point is calculated as the path tracking angle deviation.

[0087] In S34, according to the path tracking angle deviation , the preset preview distance , and the front and rear axle distance L of the metering equipment unmanned delivery vehicle, the first control instruction is calculated to construct a visual path tracking model, and the first control instruction is the steering angle , and the specific calculation formula is as follows:

[0088] ;

[0089] ;

[0090] wherein, is the steering curvature.

[0091] In S4, the process of deriving the second control instruction for guiding the metering equipment unmanned delivery vehicle to move towards the metering equipment station includes:

[0092] S41, according to the pre-processed real-time pose data and the preset position of the metering equipment station, a direction vector from the current position of the metering equipment unmanned delivery vehicle to the metering equipment station is calculated;

[0093] S42, the included angle between the current heading of the metering equipment unmanned delivery vehicle and the direction vector is calculated, and the included angle is taken as the target station angle deviation;

[0094] S43, the radio frequency signal approach model is constructed as a proportional controller, and the proportional controller derives the second control instruction by multiplying the target station angle deviation by a preset proportional gain coefficient.

[0095] In S5, the navigation state includes a cruising state, an approaching state and a parking state, and the process of determining the navigation state of the vehicle includes:

[0096] S51, setting an approaching distance threshold and a parking distance threshold;

[0097] S52, when the distance between the metering equipment unmanned delivery vehicle and the metering equipment station is greater than or equal to the approaching distance threshold, the cruising state is determined;

[0098] S53, when the distance between the metering equipment unmanned delivery vehicle and the metering equipment station is less than the approaching distance threshold but greater than the parking distance threshold, the approaching state is determined;

[0099] S54, when the distance between the metering equipment unmanned delivery vehicle and the metering equipment station is less than or equal to the parking distance threshold, the parking state is determined.

[0100] According to the navigation state, the process of combining the first control instruction and the second control instruction to output the final control instruction to the execution end includes:

[0101] S521, when the navigation state is the cruising state, the weight of the first control instruction is increased, and the vehicle is driven at a preset cruising speed;

[0102] S522, when the navigation state is the approaching state, the weight of the second control instruction is increased, and the driving speed and the distance between the metering equipment unmanned delivery vehicle and the metering equipment station are proportional, so as to realize dynamic reduction of the driving speed;

[0103] S523, when the navigation state is the parking state, the second control instruction is used for guidance, and the metering equipment unmanned delivery vehicle travels at a preset crawling speed until the metering equipment unmanned delivery vehicle stops at the metering equipment station.

[0104] In S6, when the radio frequency signal strength indication data cannot be continuously collected and the path feature cannot be extracted, the process of triggering the early warning mechanism includes:

[0105] In S61, a continuous failure counter is set, and when the radio frequency signal strength indication data cannot be successfully collected and the path feature cannot be successfully extracted in a collection period, the continuous failure counter is incremented by one, and when the radio frequency signal strength indication data is successfully collected and the path feature is successfully extracted, the continuous failure counter is cleared.

[0106] In S62, it is determined whether the value of the continuous failure counter reaches a preset count threshold, and if the count threshold is reached, the early warning mechanism is triggered, and a warning signal is sent to the external monitoring system.

[0107] The process of calculating the pose deviation of the final control instruction and the preprocessed real-time pose data for feedback optimization includes:

[0108] In S63, according to the final control instruction and the preset vehicle kinematics model, the target pose of the metering equipment unmanned delivery vehicle at the next moment is predicted.

[0109] In S64, the difference between the target pose and the preprocessed real-time pose data collected at the next moment in position and direction is extracted as the pose deviation.

[0110] In S65, the pose deviation is input as a new error term into the optimization algorithm composed of a proportional-integral-derivative controller, and the adjustment range of the approaching distance threshold, the parking distance threshold, the first control instruction weight and the second control instruction weight is adjusted online, wherein the proportional-integral-derivative controller calculates the adjustment range according to the size, cumulative amount and change rate of the pose deviation.

[0111] In order to verify the effectiveness of the method of the embodiment, a simulation experiment is carried out.

[0112] The simulation environment is built based on MATLAB software, a preset path including a straight road and a curved road is set in the scene, and stations are set beside the preset path, one of which is a target station (solid five-pointed star) equipped with a radio frequency beacon, and the others are interference stations (hollow five-pointed star) without radio frequency beacons.

[0113] The simulation target is to verify whether the metering equipment unmanned delivery vehicle (solid rectangle) can start from the starting point, stably travel along the preset path, and accurately identify and park at the only target station among the stations.

[0114] Figures 2-5 The running state screenshots of the four different stages of the simulation process of the embodiment method, X and Y represent the position coordinates in the plane coordinate system, in meters, are used to show the spatial position of the metering device unmanned delivery vehicle at different time points, and the cruise path of the metering device unmanned delivery vehicle is between the two black dotted lines. The target station is located on one side of the cruise path.

[0115] Figure 2 The scene in which the metering device unmanned delivery vehicle is in the cruising state is shown. In the initial stage of the simulation, the metering device unmanned delivery vehicle is far away from the target station. At this time, the navigation state of the metering device unmanned delivery vehicle is determined as the cruising state. In the cruising state, the metering device unmanned delivery vehicle mainly relies on the visual path tracking model based on the pure tracking algorithm to stably travel along the road center line at a preset cruising speed. The state of the metering device unmanned delivery vehicle at this time is visual cruising. The travel trajectory (black solid line) is highly consistent with the road center line, verifying the stability and accuracy of long-distance cruising.

[0116] Figure 3 The scene in which the metering device unmanned delivery vehicle enters the approaching state is shown. When the metering device unmanned delivery vehicle travels to the bend and enters the preset approaching range of the target station, the navigation state of the metering device unmanned delivery vehicle switches to the approaching state. In the approaching state, the control weight gradually deviates to the radio frequency signal approach model. The metering device unmanned delivery vehicle begins to dynamically reduce the travel speed according to the distance from the target station. The state of the metering device unmanned delivery vehicle switches to radio frequency guidance, indicating that the control core has turned to the target station, verifying the smooth transition ability of the embodiment method from path tracking to target guidance.

[0117] Figure 4 The scene in which the metering device unmanned delivery vehicle enters the parking state is shown. When the distance between the metering device unmanned delivery vehicle and the target station is less than the preset parking distance threshold, the navigation state of the metering device unmanned delivery vehicle switches to the parking state. In the parking state, the metering device unmanned delivery vehicle completely adopts the radio frequency signal approach model for accurate attitude alignment, and moves to the target station at a preset crawling speed. The current state of the metering device unmanned delivery vehicle is parking. The metering device unmanned delivery vehicle is performing the final alignment at an extremely slow speed, verifying the accurate control ability of the embodiment method in the terminal guidance stage.

[0118] Figure 5 The scene in which the metering device unmanned delivery vehicle completes parking is shown. When the distance between the metering device unmanned delivery vehicle and the target station is less than the final stop distance threshold, the speed of the metering device unmanned delivery vehicle is set to zero, and the navigation state is updated to parking completion. Figure 5 As can be seen from the figure, the metering device unmanned delivery vehicle accurately parks beside the target station, and the travel trajectory (black solid line) is smooth, without spinning and overshooting.

[0119] The simulation experiment results show that the navigation method provided in the embodiment can effectively guide the stable driving of the metering equipment unmanned distribution vehicle under a complex path by coordinating the visual cruise and the radio frequency guidance based on the distance-based state machine and combining the dynamic speed control strategy, and finally realize the high-precision stable parking, solve the problem of improper control near the terminal in the prior art, and easily produce overshoot or spiral, and has good feasibility and practicality.

[0120] Embodiment 2: on the basis of embodiment 1, further comprising: S7, connecting the continuously collected real-time pose data as the real-time trajectory of the metering equipment unmanned distribution vehicle, and associating the radio frequency signal strength indication data with the corresponding real-time pose data to generate a radio frequency signal strength distribution map, and the specific process is:

[0121] S71, store the continuously collected and preprocessed real-time pose data in time sequence, and connect the data points in the real-time pose data in a two-dimensional coordinate system in turn to form a real-time trajectory;

[0122] S72, generate a radio frequency signal strength distribution map, including:

[0123] S721, bind the preprocessed radio frequency signal strength indication data at each collection time with the preprocessed real-time pose data at the same collection time to form a data pair containing radio frequency signal strength information and position information;

[0124] S722, in a two-dimensional coordinate system, calibrate the data pair according to the position information, and assign a preset color value to the position of the data pair according to the size of the radio frequency signal strength information, and the color values of the data pairs together constitute a radio frequency signal strength distribution map.

Claims

1. A navigation method for unmanned delivery vehicles carrying metering equipment, characterized in that, Includes the following steps: S1. Collect and preprocess road image data, radio frequency signal strength indication data, and real-time pose data of the unmanned delivery vehicle with metering equipment; S2. For the preprocessed road image data, Hough transform is used to extract the position coordinates of the road centerline as path features, and the current signal strength value in the preprocessed radio frequency signal strength indication data is extracted as target guidance features. S3. Input the path features into the visual path tracking model based on the pure tracking algorithm to obtain the first control command for maintaining the unmanned delivery vehicle of the metering equipment along the center line of the road. S4. Input the target guidance features into the radio frequency signal approach model to obtain the second control command used to guide the unmanned delivery vehicle of the metering equipment toward the metering equipment station; S5. Based on the distance between the unmanned delivery vehicle and the metering equipment station, set the approach distance threshold and the parking distance threshold, determine the vehicle's navigation status, and based on the navigation status, combine the first control command and the second control command, and output the final control command to the execution end. The process is as follows: When the distance is greater than or equal to the proximity threshold, the system is in cruise mode, the weight of the first control command is increased, and the system travels at the preset cruise speed. When the distance is less than the approach distance threshold but greater than the parking distance threshold, it is determined to be an approach state, and the weight of the second control command is increased so that the driving speed is proportional to the distance. When the distance is less than or equal to the stopping distance threshold, it is determined to be in a stopping state. The second control command is used to guide it to travel at a preset creeping speed until the unmanned delivery vehicle of the metering equipment stops at the metering equipment station. S6. When radio frequency signal strength indication data cannot be collected continuously and path features cannot be extracted, an early warning mechanism is triggered. The process includes: S61. Set a continuous failure counter. When radio frequency signal strength indication data is not successfully acquired and path features are not successfully extracted within a acquisition cycle, the continuous failure counter is incremented by one. When radio frequency signal strength indication data is successfully acquired and path features are successfully extracted, the continuous failure counter is cleared to zero. S62. Determine whether the value of the continuous failure counter has reached the preset counting threshold. If the counting threshold is reached, trigger the early warning mechanism and send an early warning signal to the external monitoring system. The pose deviation between the final control command and the preprocessed real-time pose data is calculated and used for feedback optimization. The process includes: S63. Based on the final control command and the preset vehicle kinematics model, predict the target pose of the unmanned delivery vehicle of the metering equipment at the next moment. S64. Extract the difference in position and orientation between the target pose and the preprocessed real-time pose data acquired at the next moment, as the pose deviation; S65. The pose deviation is taken as a new error term and input into the optimization algorithm composed of the proportional-integral-derivative controller. The adjustment range of the approach distance threshold, the docking distance threshold, the weight coefficient of the first control command and the weight coefficient of the second control command are adjusted online. The proportional-integral-derivative controller calculates the adjustment range based on the magnitude, cumulative amount and rate of change of the pose deviation.

2. The navigation method for the unmanned delivery vehicle of metering equipment according to claim 1, characterized in that, In S1, the specific method for data acquisition and preprocessing is as follows: S11. Acquire and preprocess road image data, including: S111. By using the on-board image sensor deployed at the front of the unmanned delivery vehicle of the metering equipment, a continuous video stream of the vehicle's driving direction is collected, and static image frames are extracted from it as road image data. S112. Perform grayscale processing on the road image data, converting the three-channel color image in the road image data into a single-channel grayscale image, and using an edge detection operator to perform convolution operation on the grayscale image to obtain the preprocessed road image data. S12. Acquire and preprocess radio frequency signal strength indication data, including: S121. By using a radio frequency signal receiver deployed on the unmanned delivery vehicle of the metering equipment, the radio frequency signal emitted by the radio frequency beacon preset at the metering equipment site is periodically received, and the radio frequency signal strength indication data is parsed out. S122. Apply the moving average filtering algorithm to smooth the continuously acquired radio frequency signal strength indication data sequence. Take the arithmetic mean of the radio frequency signal strength of the current sampling point and the N-1 sampling points before the current sampling point, and use the obtained arithmetic mean as the preprocessed radio frequency signal strength indication data at the current time, where N is the size of the filtering window. S13. Acquire and preprocess real-time pose data, including: S131. The absolute position coordinates, angular velocity and acceleration of the vehicle are collected by the global navigation satellite system receiver and inertial measurement unit deployed on the unmanned delivery vehicle of the metering equipment, respectively. S132. Input the absolute position coordinates, angular velocity and acceleration into the Kalman filter for data fusion, and output the optimal estimate as the preprocessed real-time pose data.

3. The navigation method for the unmanned delivery vehicle of metering equipment according to claim 1, characterized in that, In S2, the process of using Hough transform to extract the location coordinates of the road centerline as path features and extracting the current signal strength value from the preprocessed radio frequency signal strength indication data as target guidance features includes: S21. Map the coordinates of each edge point in the preprocessed road image data from the image coordinate system to the Hough parameter space. The mapping logic is: convert the points in the image coordinate system into a sine curve in the Hough parameter space. The parameters of the points on the sine curve correspond to the straight line passing through the points in the image coordinate system. S22. Perform peak detection in the Hough parameter space and identify the parameter sets corresponding to the two detected peak points as the left and right road edge lines in the image coordinate system. S23. In the image coordinate system, calculate the midpoint of the left and right road edge lines at the preset horizontal position, and use the set of coordinates of the midpoint as the position coordinates of the road centerline. The position coordinates of the road centerline constitute the path feature. S24. The preprocessed radio frequency signal strength indication data at the current moment is used as the current signal strength value, and the current signal strength value constitutes the target guidance feature.

4. The navigation method for the unmanned delivery vehicle of the metering equipment according to claim 1, characterized in that, In S3, the process of deriving the first control command to maintain the unmanned delivery vehicle for the metering equipment along the centerline of the road includes: S31. Based on the preprocessed real-time pose data, find the path point closest to the center of the rear axle of the unmanned delivery vehicle of the metering equipment on the position coordinates of the road centerline in the path features. S32. Starting from the nearest path point, search forward along the center line of the road to determine the path point that is at a preset aiming distance from the center of the rear axle of the unmanned delivery vehicle of the metering equipment, and use it as the aiming point. S33. Calculate the angle between the current heading of the unmanned delivery vehicle for metering equipment and the direction vector from the center of the rear axle of the unmanned delivery vehicle for metering equipment to the pre-aiming point, and use it as the path tracking angle deviation. S34. Based on the path tracking angle deviation, the preset aiming distance, and the front and rear wheelbase of the unmanned delivery vehicle of the metering equipment, the first control command is calculated to construct a visual path tracking model. The first control command is the steering angle.

5. The navigation method for the unmanned delivery vehicle of the metering equipment according to claim 1, characterized in that, In S4, the process of deriving the second control command to guide the unmanned delivery vehicle for the metering equipment towards the metering equipment station includes: S41. Based on the preprocessed real-time pose data and the preset position of the metering equipment station, calculate the direction vector from the current position of the unmanned delivery vehicle of the metering equipment to the metering equipment station. S42. Calculate the angle between the current heading of the unmanned delivery vehicle of the metering equipment and the direction vector, and use the angle as the angular deviation of the target station; S43. The radio frequency signal approach model is constructed as a proportional controller. The proportional controller obtains the second control command by multiplying the target station angle deviation by a preset proportional gain coefficient.

6. The navigation method for the unmanned delivery vehicle of metering equipment according to claim 1, characterized in that, Also includes: S7. Connect the continuously collected real-time pose data to the real-time trajectory of the unmanned delivery vehicle of the metering equipment, and associate the radio frequency signal strength indication data with the corresponding real-time pose data to generate a radio frequency signal strength distribution map. The specific process is as follows: S71. Store the continuously acquired preprocessed real-time pose data in chronological order, and connect the data points in the real-time pose data sequentially in a two-dimensional coordinate system to form a real-time trajectory. S72. Generate a radio frequency signal strength distribution map, including: S721. The preprocessed radio frequency signal strength indication data at each acquisition moment is bound to the preprocessed real-time pose data at the same acquisition moment to form a data pair containing radio frequency signal strength information and position information. S722. In a two-dimensional coordinate system, the data pairs are calibrated according to their position information, and a preset color value is assigned to the position of the data pairs according to the magnitude of the radio frequency signal strength information. The color values ​​of each data pair together constitute the radio frequency signal strength distribution map.

Images