Navigation positioning method and automatic driving device

Abstract

The present application relates to a navigation positioning method and an automatic driving device. The method comprises: obtaining T i The first positioning coordinate and the second positioning coordinate at the moment T are obtained by performing fusion calculation according to the first positioning coordinate and the second positioning coordinate. i The first position coordinate at the moment; the fusion calculation includes: performing fusion calculation through the first correction value, the second correction value and the fusion function to obtain the output value; using the horizontal coordinate in the first positioning coordinate as the first correction value and the horizontal coordinate in the second positioning coordinate as the second correction value for fusion calculation, and obtaining the output value as the horizontal coordinate value of the first position coordinate; using the vertical coordinate in the first positioning coordinate as the first correction value and the vertical coordinate in the second positioning coordinate as the second correction value for fusion calculation, and obtaining the output value as the vertical coordinate value of the first position coordinate. The first position coordinate can more accurately describe the coordinate position of the AGV.

Description

Technical Field

[0001] The present application relates to the field of autonomous driving technology, and in particular to a navigation and positioning method and an autonomous driving device. Background Art

[0002] In the prior art, an automatic guided vehicle (AGV) is an unmanned vehicle that is usually equipped with a variety of sensors for sensing the driving environment and can travel to a designated location under the monitoring of a computer.

[0003] In an AGV equipped with a laser radar, the laser radar performs three-dimensional laser ranging in front of the AGV. The host computer constructs a point cloud image based on the distance set between the front object and the AGV. The point cloud image reflects the road conditions in front of the AGV in real time. The host computer plans the next path for the AGV based on the characteristics of the point cloud image.

[0004] In actual applications, AGVs equipped with LiDAR are usually used as automatic sweeping robots for home use or unmanned logistics vehicles in factories. During the operation of automatic sweeping robots or unmanned logistics vehicles, they are easily affected by external factors, causing the AGV to fail to drive to the designated location along the planned path. For example, when the automatic sweeping robot is operating, a pet at home collides with the automatic sweeping robot, causing the automatic sweeping robot to deviate from the original driving path and unable to reach the preset target location. When the automatic sweeping robot sends an offset, the robot needs to be positioned.

[0005] To achieve laser positioning, the laser radar in the AGV emits laser signals to the surroundings and receives laser signals reflected by the surrounding environment. It constructs a point cloud image through the received laser signals to achieve positioning. However, in real life, laser signals are affected by external factors such as air quality and obstacles. In the existing technology, the laser radar installed in AGV is not enough to meet the needs of precise positioning.

[0006] Therefore, in order to improve the positioning accuracy of AGV, it is urgent to design a navigation and positioning method that integrates lidar, wireless carrier and inertial measurement unit. Summary of the invention

[0007] In order to overcome the problems existing in the related art, the first aspect of the present application provides a navigation positioning method, comprising the following steps:

[0008] Get T i The first positioning coordinates and the second positioning coordinates at the moment;

[0009] The T is obtained by performing a fusion calculation based on the first positioning coordinate and the second positioning coordinate. iThe first position coordinates at the moment;

[0010] The fusion calculation includes:

[0011] The output value is obtained by performing fusion calculation through the first correction value, the second correction value and the fusion function; the fusion function is: The z1 is the first correction value, the z2 is the second correction value, and the z is the output value;

[0012] The horizontal coordinate in the first positioning coordinate is used as a first correction value, and the horizontal coordinate in the second positioning coordinate is used as a second correction value for fusion calculation, and the output value is obtained as the horizontal coordinate value of the first position coordinate;

[0013] The vertical coordinate in the first positioning coordinate is used as a first correction value, and the vertical coordinate in the second positioning coordinate is used as a second correction value for fusion calculation, and the output value is obtained as the vertical coordinate value of the first position coordinate.

[0014] In one embodiment, the obtaining of T i Before the first positioning coordinates and the second positioning coordinates at the moment, including:

[0015] Get T i The calibration signal sent by the point to be detected at the time;

[0016] Determine whether a deviation occurs according to the reception time difference of the calibration signal. If the reception time difference of the point to be detected is equal to the standard time difference, execute the step: obtain the calibration signal sent by the point to be detected at the next moment; if the reception time difference of the offset point is not equal to the standard time difference, determine that the point to be detected is an offset point and execute the step: obtain T i The first positioning coordinates and the second positioning coordinates at the moment.

[0017] In one implementation, determining that the point to be detected is an offset point includes:

[0018] Get the T i a wireless carrier signal at a certain moment, and determining a first positioning coordinate according to the wireless carrier signal;

[0019] Get the T i A laser ranging signal at a certain moment, and determining a second positioning coordinate and a second direction angle according to the laser ranging signal.

[0020] In one embodiment, the T i After the first position coordinate of the moment, including;

[0021] Get the T i an attitude angle signal at a certain moment, and determining a first direction angle according to the attitude angle signal;

[0022] The T is obtained by performing a fusion calculation based on the first direction angle and the second direction angle. i The first position information at the moment;

[0023] According to the T i The first position coordinates at the time and the first posture information determine the corrected steering angle and the corrected displacement distance of the offset point.

[0024] In one embodiment, the obtaining of the T i Laser ranging signals at all times, including:

[0025] Get the timestamp of the host computer, and get the T according to the timestamp i The laser ranging signal at the time T i The attitude angle signal at the moment includes: obtaining the timestamp of the host computer, obtaining the T according to the timestamp i Attitude angle signal at the moment.

[0026] In one embodiment, the obtaining of the T i The method further comprises: determining a first positioning coordinate according to the wireless carrier signal at a certain time.

[0027] Get the offset point at T i The first phase difference and the second phase difference at the moment; the first phase difference is the phase detection value of the first receiving module at the first static receiving point to the offset point; the second phase difference is the phase detection value of the second receiving module at the second static receiving point to the offset point;

[0028] Acquire a first angle of arrival and a second angle of arrival of the offset point respectively according to the first phase difference and the second phase difference;

[0029] The offset point at T is obtained according to the first arrival angle and the second arrival angle i The first positioning coordinates at the moment.

[0030] In one embodiment, respectively acquiring the first arrival angle and the second arrival angle of the offset point according to the first phase difference and the second phase difference includes:

[0031] Obtain a first arrival angle θ1 between the first static receiving point and the offset point, and a second arrival angle θ2 between the second static receiving point and the offset point according to the first phase difference, the second phase difference and an arrival angle calculation formula;

[0032] The arrival angle calculation formula is: θ=arccos((ψλ) / 2πd); θ is the arrival angle, ψ is the phase difference, λ is the wavelength, and d is the preset distance between the two sets of Bluetooth antennas at the static receiving point.

[0033] In one embodiment, the offset point is obtained at T according to the first arrival angle and the second arrival angle. i The first positioning coordinates at the moment include:

[0034] The offset point is obtained at T according to the first arrival angle, the second arrival angle and the triangulation positioning formula i The detection position coordinates at the moment; the triangulation positioning formula is:

[0035] x1=((β2-α2tanθ2)-(β1-α1tanθ1)) / (tanθ2-tanθ1);

[0036] y1=((β2-α2cotθ2)-(β1-α1cotθ1)) / (cotθ2-cotθ1);

[0037] (α1, β1) are the coordinates of the first static receiving point; (α2, β2) are the coordinates of the second static receiving point; (x1, y1) are the coordinates of the offset point at T i The first positioning coordinates at the moment.

[0038] In one implementation, determining the second positioning coordinates according to the laser ranging signal includes:

[0039] Get T i A laser ranging signal at a certain moment, and constructing a point cloud image according to the laser ranging signal;

[0040] Sampling the point cloud image and removing outliers;

[0041] Performing point cloud triangulation on the point cloud image after removing outliers to obtain an image to be positioned;

[0042] The second positioning coordinates and the second direction angle are determined according to the image to be positioned and the original map.

[0043] In one embodiment, the T i Before determining the corrected steering angle and the corrected displacement distance of the offset point based on the first position coordinates and the first posture information at the time instant, the method includes:

[0044] Get the T i-1 The first position coordinates at time T i The first position coordinates at the moment;

[0045] According to the T i-1 The first position coordinate at time T i The first position coordinate at the moment and the filter function are filtered to obtain the second position coordinate;

[0046] The T i-1 Time is T i The moment before the moment;

[0047] The filtering function is τ=τ1+ρ(τ2-τ1), where The τ1 is the first filtering value, the τ2 is the second filtering value, and the τ is the third filtering value;

[0048] The filtering process includes:

[0049] The T i-1 The horizontal coordinate of the first position coordinate at the moment is used as the first filtering value, and the T i The abscissa of the first position coordinate at the time is input into the filtering function as the second filtering value, and the obtained third filtering value is used as the abscissa of the second position coordinate;

[0050] The vertical coordinate of the second position coordinate and the second posture information are obtained respectively according to the filtering process.

[0051] In one embodiment, the T i The first position coordinates and the first posture information at the time instant determine the corrected steering angle and the corrected displacement distance of the offset point, including:

[0052] Determine the offset distance e1 and the offset angle a1 of the offset point according to the first position coordinates;

[0053] Determine the corrected displacement distance of the offset point according to the offset distance e1 and the offset angle a1;

[0054] A corrected rotation angle of the offset point is determined according to the first posture information and the corrected displacement distance.

[0055] In one embodiment, determining the corrected displacement distance of the offset point according to the offset distance e1 and the offset angle a1 includes:

[0056] Determine the corrected displacement distance of the offset point according to the offset distance e1, the offset angle a1 and the distance correction formula;

[0057] The distance correction formula is: Among them, U ae is the angle membership, U de is the distance membership, K Uis the output scaling factor; rule is the fuzzy rule value;

[0058] The distance membership U de Determined according to the offset distance e1, the angle membership degree U ae Determined according to the offset angle a1.

[0059] The second aspect of the present application further provides an automatic driving device, based on the navigation and positioning method according to any one of the first aspects of the present application, comprising:

[0060] A wireless carrier signal module 1, an inertial measurement module 2, a laser radar module 3, and a processing module 4 are provided in the autonomous driving device; a positioning module 5 is provided at the first static receiving point and the second static receiving point; and a calibration module 6;

[0061] The calibration module is arranged near the automatic driving device and is used to send positioning information and calibration information to the processing module;

[0062] The wireless carrier signal module is used to send the wireless carrier signal to the positioning module and send the calibration signal to the calibration module;

[0063] The inertial measurement module is used to send the attitude angle signal to the processing module;

[0064] The laser radar module is used to send the laser ranging signal to the processing module;

[0065] The positioning module is used to receive the wireless carrier signal and send positioning information to the processing module;

[0066] The processing module is used to execute the navigation and positioning method in each application embodiment.

[0067] The technical solution provided by this application may have the following beneficial effects:

[0068] In order to prevent the positioning error of the navigation and positioning device on the AGV from being too large during the automatic driving of the AGV, the present application provides a navigation and positioning method, in which a laser radar and a wireless carrier device are arranged on the AGV. i When the time is offset, the wireless carrier signal and the laser ranging signal at the current time are respectively obtained, and the first positioning coordinates are determined according to the wireless carrier signal; at the same time, according to T i The laser ranging signal at the moment is used to obtain the second positioning coordinates; the second positioning coordinates and the first positioning coordinates are fused and calculated to determine T i The first position coordinates of the AGV at the moment can more accurately reflect the real-time position of the AGV and reduce positioning errors.

[0069] It should be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] The above and other objects, features and advantages of the present application will become more apparent through a more detailed description of exemplary embodiments of the present application in conjunction with the accompanying drawings, wherein the same reference numerals generally represent the same components in the exemplary embodiments of the present application.

[0071] Figure 1 is a first flow chart of a navigation and positioning method shown in an embodiment of the present application;

[0072] Figure 2 is a second flow chart of a navigation and positioning method shown in an embodiment of the present application;

[0073] Figure 3 is a third flow chart of a navigation and positioning method shown in an embodiment of the present application;

[0074] Figure 4 It is a flowchart of a wireless carrier signal positioning method shown in an embodiment of the present application;

[0075] Figure 5 It is a schematic diagram of a flow chart of a laser ranging and positioning method shown in an embodiment of the present application;

[0076] Figure 6 is a flow chart of a posture angle fusion calculation method shown in an embodiment of the present application;

[0077] Figure 7 It is a flowchart of a filtering processing method for navigation and positioning shown in an embodiment of the present application;

[0078] Figure 8 It is a flow chart of an AGV trajectory correction method for navigation and positioning shown in an embodiment of the present application;

[0079] Fig. 9 It is a schematic diagram of the logical structure of an automatic navigation device shown in an embodiment of the present application. DETAILED DESCRIPTION

[0080] The preferred embodiments of the present application will be described in more detail below with reference to the accompanying drawings. Although the preferred embodiments of the present application are shown in the accompanying drawings, it should be understood that the present application can be implemented in various forms and should not be limited by the embodiments described herein. On the contrary, these embodiments are provided to make the present application more thorough and complete, and to fully convey the scope of the present application to those skilled in the art.

[0081] The terms used in this application are for the purpose of describing specific embodiments only and are not intended to limit this application. The singular forms of "a", "said" and "the" used in this application and the appended claims are also intended to include plural forms unless the context clearly indicates other meanings. It should also be understood that the term "and / or" used herein refers to and includes any or all possible combinations of one or more associated listed items.

[0082] It should be understood that although the terms "first", "second", "third", etc. may be used in this application to describe various information, this information should not be limited to these terms. These terms are only used to distinguish the same type of information from each other. For example, without departing from the scope of this application, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Thus, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of this application, the meaning of "multiple" is two or more, unless otherwise clearly and specifically defined.

[0083] Embodiment 1

[0084] In an AGV equipped with only one positioning device, when the positioning coordinate deviation of the positioning device of the AGV is too large, the AGV cannot determine the actual position based on the positioning coordinate. Therefore, in order to reduce the positioning error of the AGV, the AGV is usually equipped with two or more positioning devices. Two positioning coordinate data are obtained through detection by the two positioning devices. The two positioning coordinate data need to be fused and calculated to determine a more accurate positioning coordinate.

[0085] In order to improve the fusion accuracy of positioning coordinate data, the present application embodiment provides a navigation positioning method, see Figure 1 , including the following steps:

[0086] 10. Get T i The first positioning coordinates and the second positioning coordinates at the moment;

[0087] 20. Perform fusion calculation based on the first positioning coordinates and the second positioning coordinates to obtain the T i The first position coordinates at the moment.

[0088] Specifically, the fusion calculation includes:

[0089] The output value is obtained by performing fusion calculation through the first correction value, the second correction value and the fusion function; the fusion function is: The z1 is the first correction value, the z2 is the second correction value, and the z is the output value;

[0090] In the embodiment of the present application, the first positioning coordinates and the second positioning coordinates are respectively obtained by different positioning devices, and the first positioning coordinates and the second positioning coordinates are fused and calculated to determine the first position coordinates.

[0091] See also Figure 2 , the calculation steps of the first position coordinates are as follows:

[0092] 201. Perform fusion calculation by taking the abscissa in the first positioning coordinate as a first correction value and the abscissa in the second positioning coordinate as a second correction value, and obtain the output value as the abscissa value of the first position coordinate;

[0093] In step 201, the calculation formula of the horizontal coordinate x of the first position coordinate is:

[0094] 202. Perform fusion calculation using the ordinate in the first positioning coordinate as a first correction value and the ordinate in the second positioning coordinate as a second correction value to obtain the output value as the ordinate value of the first position coordinate.

[0095] In step 202, the calculation formula of the abscissa value x of the first position coordinate is:

[0096] In the embodiment of the present application, after step 201 and step 202, the first position coordinate (x, y) after fusion calculation is obtained. The first position coordinate is used to express the i The coordinates of the AGV at that moment.

[0097] In this embodiment of the present application, the AGV is obtained at T i The wireless carrier signal and the laser ranging signal at the time are used to determine the first positioning coordinates according to the wireless carrier signal; at the same time, according to T i The laser ranging signal at the moment is used to obtain the second positioning coordinates; the second positioning coordinates and the first positioning coordinates are fused and calculated to determine T i The first position coordinates of the AGV at the moment can more accurately reflect the real-time position of the AGV and reduce positioning errors.

[0098] Embodiment 2

[0099] Before the fusion calculation of the first positioning coordinate and the second positioning coordinate is performed in Example 1, the first positioning coordinate and the second positioning coordinate of the AGV at the current moment need to be calculated. In the prior art, the positioning of the AGV can be achieved in a variety of ways. The embodiment of the present application obtains the first positioning coordinate and the second positioning coordinate respectively by wireless carrier signal positioning (Ultra Wide Band, UWB) and simultaneous positioning and mapping (Simultaneous Localization and Mapping, SLAM) based on laser ranging.

[0100] In the embodiment of the present application, UWB sensors and laser radars are installed on the body of the AGV. In daily production, in order to prevent multiple AGVs from colliding, each AGV needs to run along a preset track. Therefore, when it is detected that the AGV deviates from its preset track, the UWB and SLAM methods are used to locate the deviation to determine the exact position of the AGV when it deviates.

[0101] A navigation positioning method according to an embodiment of the present application, comprising: Figure 3 , specifically including the following steps:

[0102] 101. Get T i The calibration signal sent by the point to be detected at the time;

[0103] In practical applications, in order to timely detect whether the AGV deviates from the preset track, at least two calibration base stations are arranged near the AGV.

[0104] In an embodiment of the present application, the AGV, as a point to be detected, sends a non-calibration signal to the calibration base station in real time, and then calculates in real time the time difference of the calibration signal sent by the AGV arriving at different calibration base stations, and compares the time difference with the standard time difference.

[0105] For example, in T i At time T, the calibration base station obtains the calibration signal sent by the AGV and calculates the calibration signal of the AGV at T i The time difference between the two times, if the AGV is at T i If the time difference at the moment is not equal to the standard time difference corresponding to the AGV on the preset trajectory, the AGV is determined to be an offset point and step 102 is executed; otherwise, T is continued to be obtained. i The calibration signal sent by the point to be detected at time.

[0106] 102. Get the T i a wireless carrier signal at a certain moment, and determining a first positioning coordinate according to the wireless carrier signal;

[0107] Specifically, in order to locate the AGV when it deviates from the preset track, a positioning base station is respectively provided at the first static receiving point and the second static receiving point.

[0108] Specifically, there are two positioning base stations (ie, a first receiving module and a second receiving module).

[0109] When AGV is in T i When the AGV deviates from the preset trajectory at any time, it sends a positioning data packet to the positioning base station through the UWB sensor.

[0110] The present application provides a method for locating a wireless carrier signal. Figure 4 , including the following steps:

[0111] 1021. Obtain the offset point at T i The first phase difference and the second phase difference at the time instant;

[0112] Specifically, each positioning base station is respectively provided with two receiving antennas.

[0113] Specifically, the first receiving module is provided with two receiving antennas, which respectively detect the phase of the positioning data packet, and the difference between the phase detection values ​​of the two receiving antennas is the first phase difference.

[0114] Similar to the first receiving module, the second receiving module is provided with two receiving antennas, which respectively detect the phase of the positioning data packet, and the difference between the phase detection values ​​of the two receiving antennas is the second phase difference.

[0115] 1022. Obtain a first arrival angle θ1 between the first static receiving point and the offset point, and a second arrival angle θ2 between the second static receiving point and the offset point according to the first phase difference, the second phase difference, and an arrival angle calculation formula;

[0116] Specifically, the arrival angle calculation formula is: θ=arccos((ψλ) / 2πd); θ is the arrival angle, ψ is the phase difference; λ is the wavelength; d is the preset distance between the two groups of receiving antennas at the static receiving point.

[0117] 1023. Obtain the offset point at T according to the first arrival angle θ1 and the second arrival angle θ2. i The detection position coordinates at the moment.

[0118] In the embodiment of the present application, the host system obtains the first arrival angle θ1 and the second arrival angle θ2 sent by the static receiving point. The host system obtains the offset point at T according to the first arrival angle, the second arrival angle and the triangulation positioning formula. i The detection position coordinates at the moment; the triangulation positioning formula is:

[0119] x1=((β2-α2tanθ2)-(β1-α1tanθ1)) / (tanθ2-tanθ1);

[0120] y1=((β2-α2cotθ2)-(β1-α1cotθ1)) / (cotθ2-cotθ1);

[0121] (α1, β1) are the coordinates of the first static receiving point; (α2, β2) are the coordinates of the second static receiving point; (x1, y1) are the coordinates of the offset point at T i The first positioning coordinates at the moment.

[0122] 103. Obtain the T i a laser ranging signal at a certain moment, and determining a second positioning coordinate and a second direction angle according to the laser ranging signal;

[0123] Specifically, when AGV is in T i The AGV uses laser radar to measure the distance of the surrounding environment.

[0124] The present application embodiment provides a laser ranging positioning method, such as Figure 5 As shown, the following steps are included:

[0125] 1031. Obtain the timestamp of the host computer, and obtain the T according to the timestamp. i Laser ranging signal at each moment;

[0126] 1032. Get T i A laser ranging signal at a certain moment, and constructing a point cloud image according to the laser ranging signal;

[0127] 1033. Sampling the point cloud image and removing outliers;

[0128] 1034. Perform point cloud triangulation on the point cloud image after removing outliers to obtain an image to be positioned;

[0129] 1035. Determine the second positioning coordinates (x2, y2) according to the image to be positioned and the original map.

[0130] In the embodiment of the present application, when the AGV is at T i Deviate from the preset trajectory at any time to obtain the first positioning coordinates (x1, y1) and the second positioning coordinates (x2, y2).

[0131] 104. Perform fusion calculation based on the first positioning coordinates and the second positioning coordinates to obtain the T i The first position coordinates at the moment.

[0132] The horizontal coordinate in the first positioning coordinate is used as the first correction value, and the horizontal coordinate in the second positioning coordinate is used as the second correction value for fusion calculation, and the output value is obtained as the horizontal coordinate value of the first position coordinate.

[0133] In step 201, the calculation formula of the abscissa value x of the first position coordinate is:

[0134] In step 202, the calculation formula of the ordinate y of the first position coordinate is:

[0135] In the embodiment of the present application, when it is determined that the AGV is at T i When the preset trajectory is deviated at time T, the upper system determines the first positioning coordinates of the AGV according to the wireless carrier signal sent by the AGV; and obtains the second positioning coordinates of the AGV according to the laser ranging signal sent by the AGV; the second positioning coordinates and the first positioning coordinates are fused and calculated to determine the AGV at T i The first position coordinates at the time can more accurately reflect the position of the AGV at this time, and further determine the distance that the AGV deviates from the preset trajectory.

[0136] Embodiment 3

[0137] The present application locates the AGV after the deviation through the first embodiment and the second embodiment, and determines the coordinates of the AGV. Before issuing an instruction to the AGV to return to the preset track, it is also necessary to determine the AGV at T i The orientation angle at the moment.

[0138] Therefore, an embodiment of the present application also provides a posture angle fusion calculation method for navigation and positioning.

[0139] See also Figure 6 , including the following steps:

[0140] 301. Obtain the T i an attitude angle signal at a certain moment, and determining a first direction angle according to the attitude angle signal;

[0141] In the embodiment of the present application, an inertial sensor is also provided on the body of the AGV. During the driving process of the AGV, the inertial sensor detects the steering angle of the AGV in real time.

[0142] It should be noted that the steering angle of the AGV in the embodiment of the present application is determined relative to the reference coordinate system.

[0143] 302. Obtain the T i a laser ranging signal at a certain moment, and determining a second direction angle according to the laser ranging signal;

[0144] In the embodiment of the present application, the host system obtains T i A laser ranging signal at a certain moment is used, and a point cloud image is constructed according to the laser ranging signal; the point cloud image is sampled and outliers are removed; the point cloud image after the outliers are removed is triangulated to obtain an image to be located; and the second direction angle is determined according to the image to be located and an original map.

[0145] 303. Perform fusion calculation according to the first direction angle and the second direction angle to obtain the T i The first position information at the moment.

[0146] In step 303, the upper system performs fusion calculation using the value of the first direction angle as the first correction value and the value of the second direction angle as the second correction value, and uses the output value obtained by the following attitude angle calculation formula as the first attitude information.

[0147] The attitude angle calculation formula is: in, is the first pose information, is the first direction angle, is the second direction angle.

[0148] In the embodiment of the present application, when it is determined that the AGV is at T i When the preset trajectory is deviated at time T, the upper system determines the first direction angle of the AGV through the inertial sensor; and obtains the second direction angle of the AGV through the laser ranging sensor of the AGV; the second direction angle and the first direction angle are fused and calculated to determine the AGV at T i The first posture information at the moment can more accurately reflect the direction of the AGV at this moment. When the AGV moves, the direction of the AGV at the next moment can be determined based on the second posture information.

[0149] Embodiment 4

[0150] In order to further correct the first position coordinates and the first pose information determined in the above embodiment, the embodiment of the present application also provides a filtering processing method for navigation positioning, see Figure 7 , including the following steps:

[0151] 401. Get the T i-1 The first position coordinates at time T i-1 The first position coordinates at the moment;

[0152] 402. According to the T i-1 The first position coordinate at time T i The first position coordinate at the moment and the filter function are filtered to obtain the second position coordinate;

[0153] Specifically, the T i-1 Time is T i The moment before the moment;

[0154] Specifically, the filtering function is τ=τ1+ρ(τ2-τ1), where: The τ1 is the first filtering value, the τ2 is the second filtering value, and the τ is the third filtering value;

[0155] In step 402, the filtering process includes:

[0156] The T i-1 The abscissa x of the first position coordinate at the moment i-1 as the first filtered value, and the T i The abscissa x of the first position coordinate at the moment i The filter function τ=τ1+ρ(τ2-τ1) is input as the second filter value, and the obtained third filter value is used as the abscissa x' of the second position coordinate. i .

[0157] Similarly, the ordinate y' of the second position coordinate is i Therefore, AGV in T i The second position coordinate at the moment is (x' i , y' i ).

[0158] It should be noted that the second position coordinates are position coordinates after filtering.

[0159] 403. Obtain the second posture information according to the filtering process.

[0160] In step 403, the filtering process includes:

[0161] The T i-1 The first pose information at the moment is used as the first filter value α i-1 , and the T i The first pose information at the moment is used as the second filter value α i Input the filter function τ=τ1+ρ(τ2-τ1), and obtain the third filter value as the second posture information

[0162] In the embodiment of the present application, the second position coordinate (x' i , y' i ) and the second pose information The coordinates and deflection angles of the AGV can be further corrected.

[0163] Embodiment 5

[0164] When the AGV deviates from the preset track, the second position coordinates and second posture information of the AGV are obtained through the fusion positioning and filtering processing of the above embodiment. The embodiment of the present application calculates the displacement distance and direction of the AGV returning to the preset track through the second position coordinates and second posture information of the AGV.

[0165] The present application also provides an AGV trajectory correction method for navigation and positioning, such as Figure 8 As shown, the following steps are included:

[0166] 501. Determine an offset distance e1 and an offset angle a1 of an offset point according to the second position coordinates;

[0167] In the embodiment of the present application, the offset distance e1 is T i The straight-line distance between the AGV and the preset trajectory at the moment; the offset angle a1 is the angle between the deviated trajectory and the preset trajectory.

[0168] Furthermore, the parameters set in the upper system include: angle domain ae max 、Distance domain max and output domain U max ;

[0169] Specifically, the unit of the angle domain is radian (rad); the unit of the distance domain is millimeter (mm); the output domain U max is the angular velocity in radians per minute (rad / min).

[0170] In the actual work of AGV, the angle domain ae max The fuzzy subset is {-3, -2, -1, 0, 1, 2, 3}; the distance domain is de max The fuzzy subsets of are {-3, -2, -1, 0, 1, 2, 3}.

[0171] Among them, the angle scaling factor is calculated according to the above preset parameters Distance scaling factor and the output scaling factor N is the number of subsets.

[0172] Specifically, the value of N is 7.

[0173] In the embodiment of the present application, the input offset distance and offset angle need to be corrected, and the corrected offset distance is e2=K de *e1; the corrected offset angle is a2=K ae *a1.

[0174] 502. Determine a corrected displacement distance of the offset point according to the offset distance;

[0175] In an embodiment of the present application, the corrected displacement distance is determined based on the offset distance and the distance correction formula.

[0176] The distance correction formula is: Among them, U ae is the angle membership, U de is the distance membership, K U is the output scaling factor; rule is the fuzzy rule value.

[0177] Further, the distance membership U is determined according to the corrected offset distance e2. de .

[0178] Specifically, the distance membership U de The calculation formula is:

[0179] U de =|e2-rule[e]| / 1, where rule[e] is the selected distance membership value.

[0180] Further, the angle membership U is determined according to the corrected offset angle a2. ae .

[0181] Specifically, the angle membership U ae The calculation formula is:

[0182] U ae =|a2-rule[a]| / 1, where rule[a] is the selected distance membership value.

[0183] For example, when a2=2.3 and e2=1.6, a2 is in the subsets (1,2,3) and (2,3,3); e2 is in the subsets (0,1,2) and (1,2,3). Therefore, the angle membership values ​​of a2 are 2 and 3, and the distance membership values ​​of e2 are 1 and 2.

[0184] Therefore, according to the above calculation formulas for the angle membership value and the distance membership value, the angle membership value U at the current moment can be calculated: ae [1] and U ae [2], and the distance membership U de [1] and U de [2].

[0185] The specific calculation steps are as follows:

[0186] When the angle membership value is 3, the angle membership is:

[0187] When the angle membership value is 2, the angle membership is:

[0188] When the distance membership value is 1,

[0189] When the distance membership value is 2,

[0190] Furthermore, the fuzzy rule value rule is determined according to the corrected offset distance e2, the offset angle a2 and the mapping table of the fuzzy rule value.

[0191] Furthermore, the mapping table of the fuzzy rule values ​​is as follows:

[0192]

[0193] In the embodiment of the present application, the fuzzy rule value rule includes 4 values, namely:

[0194] rule[1][1], rule[1][2], rule[2][1] and rule[2][2];

[0195] For example, U ae The angle membership value of [1] is 3, U de The distance membership value of [1] is 1, so the mapping table value of rule[1][1] corresponding to the fuzzy rule value is 3.

[0196] Similarly, the specific values ​​of rule[1][2], rule[2][1], and rule[2][2] are obtained respectively.

[0197] In the embodiment of the present application, the corrected displacement distance is calculated based on the angle membership, the distance membership, the fuzzy rule value and the expansion of the distance correction formula.

[0198] The expansion of the distance correction formula is:

[0199]

[0200] The corrected displacement distance of the AGV can be calculated by using the corrected offset distance e2, the offset angle a2 and the distance correction formula. That is, when the AGV returns to the track, the distance it needs to move is specifically the corrected displacement distance.

[0201] 503. Determine a corrected rotation angle of the offset point according to the second posture information and the corrected displacement distance.

[0202] Specifically, the corrected rotation angle is equal to the second posture information plus the rotation angle.

[0203] In the embodiment of the present application, by determining the corrected displacement distance and the corrected rotation angle of the AGV, the AGV returns to the preset trajectory under the instruction of the host system.

[0204] Embodiment 6

[0205] The present application provides an automatic driving device, including:

[0206] A wireless carrier signal module 1, an inertial measurement module 2, a laser radar module 3, and a processing module 4 are provided in the autonomous driving device; a positioning module 5 is provided at the first static receiving point and the second static receiving point; and a calibration module 6;

[0207] The calibration module is arranged near the automatic driving device and is used to send positioning information and calibration information to the processing module;

[0208] The wireless carrier signal module is used to send the wireless carrier signal to the positioning module and send the calibration signal to the calibration module;

[0209] The inertial measurement module is used to send the attitude angle signal to the processing module;

[0210] The laser radar module is used to send the laser ranging signal to the processing module;

[0211] The positioning module is used to receive the wireless carrier signal and send positioning information to the processing module;

[0212] The processing module is used to execute the navigation and positioning method in each application embodiment.

[0213] Regarding the device in the above embodiment, the specific manner in which each module performs operations has been described in detail in the embodiment of the method, and will not be elaborated again here.

[0214] The scheme of the present application has been described in detail above with reference to the accompanying drawings. In the above embodiments, the description of each embodiment has its own emphasis. For the part that is not described in detail in a certain embodiment, refer to the relevant description of other embodiments. Those skilled in the art should also know that the actions and modules involved in the description are not necessarily required for the present application. In addition, it can be understood that the steps in the method of the embodiment of the present application can be adjusted in order, merged and deleted according to actual needs, and the modules in the device of the embodiment of the present application can be merged, divided and deleted according to actual needs.

[0215] In addition, the method according to the present application may also be implemented as a computer program or a computer program product, which includes computer program code instructions for executing some or all of the steps in the above method of the present application.

[0216] Alternatively, the present application can also be implemented as a non-temporary machine-readable storage medium (or computer-readable storage medium, or machine-readable storage medium) on which executable code (or computer program, or computer instruction code) is stored. When the executable code (or computer program, or computer instruction code) is executed by a processor of an electronic device (or electronic device, server, etc.), the processor executes part or all of the steps of the above-mentioned method according to the present application.

[0217] Those skilled in the art will further appreciate that the various illustrative logical blocks, modules, circuits, and algorithm steps described in connection with the application herein may be implemented as electronic hardware, computer software, or combinations of both.

[0218] The flow chart and block diagram in the accompanying drawings show the possible architecture, function and operation of the system and method according to multiple embodiments of the present application. In this regard, each square box in the flow chart or block diagram can represent a part of a module, a program segment or a code, and the part of the module, the program segment or the code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the square box can also occur in a sequence different from that marked in the accompanying drawings. For example, two continuous square boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each square box in the block diagram and / or the flow chart, and the combination of the square boxes in the block diagram and / or the flow chart can be implemented with a dedicated hardware-based system that performs the specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.

[0219] The embodiments of the present application have been described above, and the above description is exemplary, not exhaustive, and is not limited to the disclosed embodiments. Many modifications and changes will be apparent to those of ordinary skill in the art without departing from the scope and spirit of the described embodiments. The selection of terms used herein is intended to best explain the principles of the embodiments, practical applications, or improvements to the technology in the market, or to enable other persons of ordinary skill in the art to understand the embodiments disclosed herein.

Claims

1. A navigation positioning method, characterized in that: The following steps are involved: Get The first positioning coordinates and the second positioning coordinates at the moment; The first positioning coordinates and the second positioning coordinates are combined and calculated to obtain the The first position coordinates at the moment; The fusion calculation includes: The output value is obtained by performing fusion calculation through the first correction value, the second correction value and the fusion function; the fusion function is: ; is the first correction value, is the second correction value, is the output value; The horizontal coordinate in the first positioning coordinate is used as a first correction value, and the horizontal coordinate in the second positioning coordinate is used as a second correction value for fusion calculation, and the output value is obtained as the horizontal coordinate value of the first position coordinate; The ordinate in the first positioning coordinate is used as a first correction value, and the ordinate in the second positioning coordinate is used as a second correction value for fusion calculation, and the output value is obtained as the ordinate value of the first position coordinate; The acquisition Before the first positioning coordinates and the second positioning coordinates at the moment, including: Get The calibration signal sent by the point to be detected at the time; Determine whether a deviation occurs according to the reception time difference of the calibration signal. If the reception time difference of the point to be detected is equal to the standard time difference, execute the step: obtain the calibration signal sent by the point to be detected at the next moment; if the reception time difference of the offset point is not equal to the standard time difference, determine that the point to be detected is an offset point and execute the step: obtain The first positioning coordinates and the second positioning coordinates at the moment; The step of determining that the point to be detected is an offset point includes: Get the a wireless carrier signal at a certain moment, and determining a first positioning coordinate according to the wireless carrier signal; Get the A laser ranging signal at a certain moment, and determining a second positioning coordinate and a second direction angle according to the laser ranging signal.

2. A navigation and positioning method according to claim 1, characterized in that: Said to get said After the first position coordinate of the moment, including; Get the an attitude angle signal at a certain moment, and determining a first direction angle according to the attitude angle signal; The first direction angle and the second direction angle are combined and calculated to obtain the The first position information at the moment; According to the The first position coordinates at the time and the first posture information determine the corrected steering angle and the corrected displacement distance of the offset point.

3. A navigation and positioning method according to claim 2, characterized in that: The acquisition of Laser ranging signals at all times, including: Get the timestamp of the host computer, and get the Laser ranging signal at each moment; The acquisition of Attitude angle signals at all times, including: Get the timestamp of the host computer, and get the Attitude angle signal at the moment.

4. A navigation and positioning method according to claim 1, characterized in that: The acquisition of The method further comprises: determining a first positioning coordinate according to the wireless carrier signal at a certain time. Get the offset point at The first phase difference and the second phase difference at the moment; the first phase difference is the phase detection value of the first receiving module at the first static receiving point to the offset point; the second phase difference is the phase detection value of the second receiving module at the second static receiving point to the offset point; Acquire a first angle of arrival and a second angle of arrival of the offset point respectively according to the first phase difference and the second phase difference; The offset point is obtained according to the first arrival angle and the second arrival angle. The first positioning coordinates at the moment.

5. A navigation and positioning method according to claim 4, characterized in that: The acquiring the first arrival angle and the second arrival angle of the offset point respectively according to the first phase difference and the second phase difference comprises: The first arrival angle of the first static receiving point and the offset point is obtained according to the first phase difference, the second phase difference and the arrival angle calculation formula , the second static receiving point and the second arrival angle of the offset point ; The angle of arrival calculation formula is: ; is the arrival angle, is the phase difference; is the wavelength; It is the preset distance between the two groups of Bluetooth antennas at the static receiving point.

6. A navigation and positioning method according to claim 5, characterized in that: The offset point is obtained according to the first arrival angle and the second arrival angle. The first positioning coordinates at the moment include: The offset point is obtained according to the first arrival angle, the second arrival angle and the triangulation positioning formula. The detection position coordinates at the moment; the triangulation positioning formula is: ； is the coordinate of the first static receiving point; is the coordinate of the second static receiving point; For the offset point The first positioning coordinates at the moment.

7. A navigation and positioning method according to claim 1, characterized in that: The determining of the second positioning coordinate according to the laser ranging signal comprises: Get A laser ranging signal at a certain moment, and constructing a point cloud image according to the laser ranging signal; Sampling the point cloud image and removing outliers; Performing point cloud triangulation on the point cloud image after removing outliers to obtain an image to be positioned; The second positioning coordinates and the second direction angle are determined according to the image to be positioned and the original map.

8. A navigation and positioning method according to claim 2, characterized in that: According to the Before determining the corrected steering angle and the corrected displacement distance of the offset point based on the first position coordinates and the first posture information at the time instant, the method includes: Get the The first position coordinates at the moment and the The first position coordinates at the moment; According to the The first position coordinates at the time, The first position coordinate at the moment and the filter function are filtered to obtain the second position coordinate; Said Time is The moment before the moment; The filter function is ,in, ; is the first filtering value, is the second filtering value, is the third filtering value; The filtering process includes: The The horizontal coordinate of the first position coordinate at the moment is used as the first filtering value, and the The abscissa of the first position coordinate at the time is input into the filtering function as the second filtering value, and the obtained third filtering value is used as the abscissa of the second position coordinate; The vertical coordinate of the second position coordinate and the second posture information are obtained respectively according to the filtering process.

9. A navigation and positioning method according to claim 2, characterized in that: According to the The first position coordinates and the first posture information at the time instant determine the corrected steering angle and the corrected displacement distance of the offset point, including: Determine the offset distance and the offset angle of the offset point according to the first position coordinates; Determine a corrected displacement distance of the offset point according to the offset distance and the offset angle; A corrected rotation angle of the offset point is determined according to the first posture information and the corrected displacement distance.

10. A navigation and positioning method according to claim 9, characterized in that: The determining the corrected displacement distance of the offset point according to the offset distance and the offset angle comprises: Determine the corrected displacement distance of the offset point according to the offset distance, the offset angle and the distance correction formula; The distance correction formula is: ,in, is the angle membership, is the distance membership, is the output scaling factor; is the fuzzy rule value; The distance membership Determined according to the offset distance, the angle membership Determined according to the offset angle.

11. An automatic driving device, characterized in that: include: A wireless carrier signal module (1), an inertial measurement module (2), a laser radar module (3), and a processing module (4) are arranged in the autonomous driving device; a positioning module is arranged at a first static receiving point and a second static receiving point; and a calibration module (5); The calibration module is arranged near the automatic driving device and is used to send positioning information and calibration information to the processing module; The wireless carrier signal module is used to send the wireless carrier signal to the positioning module and send a calibration signal to the calibration module; The inertial measurement module is used to send an attitude angle signal to the processing module; The laser radar module is used to send the laser ranging signal to the processing module; The positioning module is used to receive the wireless carrier signal and send positioning information to the processing module; The processing module is used to execute the navigation and positioning method as described in any one of claims 1-10.

Images