Navigation positioning method, device, system and storage medium

Abstract

The embodiment of the present invention proposes a navigation and positioning method, device, system and storage medium. The method is applied to the control equipment of a loader. The loader is equipped with an industrial camera and a laser radar. The laser transmitter cooperating with the control equipment is used to irradiate a laser line to the floor of a truck. The method includes: acquiring a set of collected data in real time, obtaining a first deflection angle and an offset of the loader according to point cloud data in the collected data, driving the loader according to the first deflection angle and the offset, obtaining a second deflection angle of the loader according to an image of the floor of the truck, and after the loader arrives at the loading position, fitting a deflection angle estimation value according to the first deflection angle and the second deflection angle obtained historically, so as to adjust the loading angle according to the deflection angle estimation value, so that the deflection angle estimation value based on the historical deflection angle data is more accurate, thereby improving the accuracy of navigation and positioning.

Description

Technical Field

[0001] The present invention relates to the field of data processing technology, and in particular to a navigation and positioning method, device, system and storage medium. Background Art

[0002] The intelligent loading system is a fully automatic material loading system that integrates visual inspection subsystem, stack design subsystem, path planning subsystem and robot motion control. The intelligent loading system can realize unmanned, informationized and intelligent loading process, and can effectively solve the problems of low labor efficiency, high safety risks and high labor costs in the process of cargo handling and loading of production enterprises.

[0003] The navigation and positioning of the loader in the truck compartment is an important part of the intelligent loading system, which determines the quality of cargo stacking. At present, the commonly used navigation technologies in the intelligent loading system include marker guidance and environmental natural guidance. However, the intelligent loading system using these navigation technologies has the problem of low navigation and positioning accuracy. Summary of the invention

[0004] In view of this, an object of the present invention is to provide a navigation positioning method, device, system and storage medium, which can improve the problem of low navigation positioning accuracy existing in the current intelligent loading system.

[0005] In order to achieve the above objectives, the technical solutions adopted by the embodiments of the present invention are as follows.

[0006] In a first aspect, an embodiment of the present invention provides a navigation and positioning method, which adopts the following technical solution.

[0007] A navigation and positioning method is applied to a control device of a loader, wherein the loader is equipped with an industrial camera and a plurality of laser radars, a laser transmitter coordinated with the control device is used to irradiate a laser line onto a floor of the truck compartment, and the control device is communicatively connected with the laser radar and the industrial camera, and the method comprises:

[0008] Acquire a set of collected data in real time, the collected data including point cloud data of the truck compartment area facing each laser radar at the same position and time, and the image of the truck compartment floor collected by the industrial camera;

[0009] Obtaining the distance between each of the laser radars and the edge of the truck compartment according to the point cloud data, and calculating the first deflection angle between the loader and the center axis of the truck, and the offset of the loader in the truck compartment according to all the distances;

[0010] Controlling the movement of the loader in the truck compartment in real time according to the first deflection angle and the offset;

[0011] Extracting laser line data from the carriage floor image, calculating a second deflection angle between the loader and the laser line according to the laser line data, and recording the first deflection angle and the second deflection angle obtained from the same set of collected data as a set of deflection angle data;

[0012] After the loader arrives at the loading position, an estimated deflection angle is fitted according to the multiple groups of deflection angle data, and the loading angle of the loader is adjusted according to the estimated deflection angle.

[0013] Furthermore, there are three laser radars, including a first radar, a second radar and a third radar, the first radar is installed on one side of the loader, the second radar is installed on the other side of the loader, and the connecting line of the first radar and the second radar is parallel to the width direction of the loader, and the third radar and the second radar are located on the same side of the loader;

[0014] The step of calculating the first deviation angle between the loader and the center axis of the truck and the offset of the loader in the truck compartment according to all the distances comprises:

[0015] Calculate a first deflection angle between the loader and the center axis of the truck based on the distances between the second radar and the third radar and the edge of the truck compartment, and the installation distance between the second radar and the third radar;

[0016] The offset of the loader in the truck compartment is calculated based on the first deflection angle, the distance between the first radar and the edge of the truck compartment, and the distance between the second radar and the edge of the truck compartment.

[0017] Furthermore, the step of calculating the first deflection angle between the loader and the center axis of the truck includes:

[0018] Calculate the first deflection angle between the loader and the center axis of the truck using a first deflection angle calculation formula;

[0019] Wherein, the first deflection angle calculation formula includes: θ ladar represents the first deflection angle, FR represents the distance between the second radar and the edge of the truck compartment, BR represents the distance between the third radar and the edge of the truck compartment, and L represents the installation distance between the second radar and the third radar;

[0020] The step of calculating the offset of the loader in the truck compartment comprises:

[0021] Using the offset calculation formula, the offset of the loader in the truck compartment is obtained;

[0022] The offset calculation formula includes: FL represents the distance between the first radar and the edge of the compartment of the truck, and shfit represents the offset.

[0023] Furthermore, the step of extracting laser line data from the carriage floor image and calculating a second deflection angle between the loader and the laser line according to the laser line data comprises:

[0024] Pixels in the carriage floor image whose pixel values ​​are greater than a grayscale threshold are taken as target pixels, and the coordinates of each target pixel are extracted as laser line data;

[0025] Based on the coordinates, fitting straight lines about the target pixels are obtained;

[0026] According to the coefficients of the fitting straight line, a second deflection angle between the loader and the laser line is obtained by using trigonometric functions.

[0027] Furthermore, the step of obtaining a second deflection angle between the loader and the laser line by using a trigonometric function according to the coefficient of the fitting straight line comprises:

[0028] Based on the coefficient of the fitting straight line, a second deflection angle between the loader and the laser line is obtained using a second deflection angle calculation formula;

[0029] The second deflection angle calculation formula includes:

[0030]

[0031] Among them, θ laser represents the second deflection angle, and k represents the coefficient of the fitting straight line.

[0032] Furthermore, the step of fitting the deflection angle estimation value according to the multiple groups of deflection angle data comprises:

[0033] Calculating the absolute deflection difference between the first deflection angle and the second deflection angle in each set of deflection angle data;

[0034] Eliminate the absolute deflection differences greater than the deflection threshold value from the plurality of absolute deflection differences to obtain a plurality of target deflection differences, and fit the plurality of target deflection differences to obtain an estimated deflection difference;

[0035] Acquire a second deflection angle obtained by capturing the carriage floor image at the loading position by the industrial camera;

[0036] The difference between the second deflection angle and the estimated deflection angle difference is used as the deflection angle estimation value.

[0037] Furthermore, the step of fitting the plurality of target deflection differences to obtain an estimated deflection difference comprises:

[0038] Based on the plurality of target deflection differences, a value with the minimum sum of distances from each of the target deflection differences is fitted using the least squares method as the estimated deflection difference.

[0039] In a second aspect, an embodiment of the present invention provides a navigation and positioning system, which adopts the following technical solution.

[0040] A navigation and positioning system comprises a control device of a vehicle loader, an industrial camera and a plurality of laser radars installed on the vehicle loader, and a laser transmitter;

[0041] The laser emitter is used to irradiate a laser line onto the floor of the truck;

[0042] The industrial camera is used to collect the image of the floor of the truck compartment;

[0043] The laser radar is used to collect point cloud data of the truck compartment area that the laser radar is facing;

[0044] The control device is used to implement the navigation and positioning method as described in the first aspect.

[0045] In a third aspect, an embodiment of the present invention provides a navigation and positioning device, which adopts the following technical solution.

[0046] A navigation and positioning device is applied to the control equipment of a loader. The loader is equipped with an industrial camera and multiple laser radars. A laser transmitter matched with the control equipment is used to irradiate a laser line onto the floor of the truck. The control equipment is connected to the laser radar and the industrial camera for communication. The navigation and positioning device includes a data acquisition module, a navigation module and a positioning module:

[0047] The data acquisition module is used to acquire a set of collected data in real time, wherein the collected data includes point cloud data of the truck compartment area facing each of the laser radars at the same position and time, and an image of the truck compartment floor collected by the industrial camera;

[0048] The navigation module is used to obtain the distance between each of the laser radars and the edge of the truck compartment according to the point cloud data, calculate the first deflection angle between the loader and the center axis of the truck, and the offset of the loader in the truck compartment according to all the distances, and control the movement of the loader in the truck compartment in real time according to the first deflection angle and the offset;

[0049] The positioning module is used to extract laser line data from the car floor image, calculate the second deviation angle between the loader and the laser line based on the laser line data, record the first deviation angle and the second deviation angle obtained from the same group of collected data as a group of deviation angle data, and after the loader arrives at the loading position, fit the deviation angle estimation value based on multiple groups of the deviation angle data, and adjust the loading angle of the loader according to the deviation angle estimation value.

[0050] In a fourth aspect, an embodiment of the present invention provides a storage medium, which adopts the following technical solution.

[0051] A storage medium stores a computer program, which, when executed by a processor, implements the navigation and positioning method as described in the first aspect.

[0052] The navigation and positioning method, device, system and storage medium provided by the embodiments of the present invention calculate the first deflection angle and offset in real time through the point cloud data collected by the laser radar during the travel of the loader, so as to control the travel of the loader in the truck compartment according to the first deflection angle and the offset, and obtain the second deflection angle according to the laser line data extracted from the image of the compartment floor, and record the first deflection angle and the second deflection angle obtained from the collected data at the same position and the same time during the travel of the loader as a set of deflection angle data, and after the loader arrives at the loading position, fit the deflection angle estimation value according to the multiple sets of deflection angle data recorded in the historical records, and then adjust the loading angle of the loader according to the deflection angle estimation value, so that the deflection angle estimation value based on the historical deflection angle data is more accurate, thereby improving the accuracy of navigation and positioning.

[0053] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, preferred embodiments are given below and described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0055] Figure 1 A schematic diagram of the structure of a vehicle loading system to which the navigation and positioning method provided in an embodiment of the present invention is applied is shown.

[0056] Figure 2 A schematic flow chart of a navigation and positioning method provided in an embodiment of the present invention is shown.

[0057] Figure 3 Shows Figure 2 Schematic diagram of the process of some sub-steps of step S103.

[0058] Figure 4 Shows Figure 2 Schematic diagram of the process of some sub-steps of step S107.

[0059] Figure 5 Shows Figure 2 Schematic diagram of the process of some sub-steps of step S109.

[0060] Figure 6 A block diagram of a navigation and positioning system provided by an embodiment of the present invention is shown.

[0061] Figure 7 A block diagram of a navigation and positioning device provided by an embodiment of the present invention is shown.

[0062] Figure 8 A block diagram of a control device provided by an embodiment of the present invention is shown.

[0063] Icons: 100-loading system; 110-loading machine; 120-truck; 130-control equipment; 140-industrial camera; 150-lidar; 160-laser transmitter; 170-laser line; 180-navigation and positioning device; 190-data acquisition module; 200-navigation module; 210-positioning module; 220-navigation and positioning system. DETAILED DESCRIPTION

[0064] The following will be combined with the accompanying drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Generally, the components of the embodiments of the present invention described and shown in the drawings here can be arranged and designed in various different configurations.

[0065] Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the invention claimed for protection, but merely represents selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without making creative work are within the scope of protection of the present invention.

[0066] It should be noted that relational terms such as "first" and "second" are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0067] The navigation and positioning of the loader in the truck compartment is an important part of the intelligent loading system, which determines the quality of cargo stacking. Common navigation technologies used in intelligent loading systems include marker guidance and environmental natural guidance. Marker guidance is to lay magnetic tape on the desired path, spray or illuminate markers, and then use sensors to identify and track to achieve navigation. Environmental natural guidance uses the characteristics of the surrounding environment to plan the driving path without the need for additional markers.

[0068] Compared with natural environment guidance, marking guidance is more accurate, but the mobility of trucks is not suitable for marking or laying tapes in the car. Natural environment guidance usually uses laser radar to scan parallel to the horizontal plane to obtain point cloud data on both sides of the car for guidance. However, since there are no fences on both sides of flatbed trucks, the natural environment guidance solution is not suitable for flatbed trucks, and the installation height and position of the radar on the fence truck are also limited.

[0069] Based on the above reasons, the current intelligent loading system has the problem of low navigation and positioning accuracy.

[0070] Based on the above considerations, an embodiment of the present invention provides a navigation positioning solution, which can improve the problem of low navigation positioning accuracy in the current intelligent loading system. The following will introduce the navigation positioning solution provided by the embodiment of the present invention from the perspectives of the navigation positioning method and the navigation positioning system.

[0071] The navigation and positioning method provided by the embodiment of the present invention can be applied to Figure 1In the application environment shown. The navigation and positioning method is applied to a loading system 100, which includes a loading machine 110 and a truck 120. The loading machine 110 is equipped with a control device 130, an industrial camera 140 and multiple laser radars 150. A laser transmitter 160 is installed near the rear of the truck 120 and outside the truck 120, and the laser transmitter 160 is installed on the outside of the truck 120. The control device 130 is connected to the industrial camera 140 and multiple laser radars 150 for communication, and the laser transmitter 160 is used to irradiate a laser line 170 to the floor of the truck 120. Among them, the scanning port of each laser radar 150 faces the floor of the truck 120.

[0072] The control device 130 is used to calculate the first deflection angle between the loader 110 and the central axis of the truck 120, the offset of the loader 110 in the compartment of the truck 120, and the second deflection angle between the loader 110 and the laser line 170 in real time based on the data collected by the laser radar 150 and the industrial camera 140. The loader 110 is controlled to travel in the compartment of the truck 120 based on the first deflection angle and the offset obtained in real time, and after reaching the loading position, the deflection angle estimation value is obtained based on the multiple first deflection angles and second deflection angles recorded in the historical records, and the loading angle of the loader 110 is adjusted based on the deflection angle estimation value.

[0073] It should be noted that, in the embodiment of the present invention, the laser line 170 emitted by the laser emitter 160 is parallel to the surface where the floor of the truck 120 is located, and the laser line 170 can form any angle with the central axis of the truck 120 .

[0074] The control device 130 may be, but is not limited to, various personal computers and computers.

[0075] It should be understood that the laser transmitter 160 is installed outside the truck 120 , that is, not on the truck 120 .

[0076] In one embodiment, Figure 2 As shown, a navigation positioning method is provided. This embodiment mainly applies this method Figure 1 Take the control device 130 in FIG. 1 as an example.

[0077] S101, obtaining a set of collected data in real time.

[0078] The collected data includes point cloud data of the compartment area of ​​the truck 120 that each laser radar 150 is facing at the same position and time, and the image of the compartment floor of the truck 120 collected by the industrial camera 140.

[0079] Each laser radar 150 and industrial camera 140 on the loader 110 will collect data at the same time and position to obtain collected data, that is, the point cloud data and the car floor image in the collected data are collected at the same position and time.

[0080] S103, obtaining the distance between each laser radar and the edge of the truck compartment based on the point cloud data, and calculating the first deviation angle between the loader and the center axis of the truck, as well as the offset of the loader 110 in the truck compartment based on all the distances.

[0081] For each point cloud data collected by the laser radar 150 , the distance between the laser radar 150 that collects the point cloud data and the edge of the compartment of the truck 120 is obtained based on the point cloud data.

[0082] It should be understood that, since the laser radar 150 collects point cloud data of the area of ​​the truck 120 compartment that it is facing, the distance is the distance between the laser radar 150 and the edge of the truck 120 compartment that it is facing.

[0083] For example, if the laser radar 150 is facing the compartment area on the right side of the truck 120, the distance is the distance between the laser and the right edge of the compartment of the truck 120. Similarly, if the laser radar 150 is facing the compartment area on the left side of the truck 120, the distance is the distance between the laser and the left edge of the compartment of the truck 120.

[0084] S105, controlling the movement of the loader in the truck compartment in real time according to the first deflection angle and the offset.

[0085] The laser radar 150 and the industrial camera 140 collect data in real time, so that during the driving process of the loader 110, a new first deflection angle and offset are continuously obtained in real time, so that the driving of the loader 110 can be controlled according to the real-time first deflection angle and offset.

[0086] S107, extracting laser line data from the carriage floor image, calculating a second deflection angle between the loader and the laser line based on the laser line data, and recording the first deflection angle and the second deflection angle obtained from the same set of collected data as a set of deflection angle data.

[0087] The first deflection angle and the second deflection angle obtained from a set of collected data (point cloud data collected by multiple laser radars 150 and the car floor image collected by the industrial camera 140) are recorded as a set of deflection angle data, thereby generating multiple sets of deflection angle data during the driving process of the loader 110.

[0088] S109, after the loader arrives at the loading position, an estimated deflection angle is fitted according to the multiple sets of deflection angle data, and the loading angle of the loader is adjusted according to the estimated deflection angle.

[0089] In the above-mentioned navigation and positioning method, during the driving process of the loader 110, the first deflection angle and the offset are calculated in real time through the point cloud data collected by the laser radar 150, so that the driving of the loader 110 in the compartment of the truck 120 is controlled according to the first deflection angle and the offset, and the second deflection angle is obtained according to the laser line 170 data extracted from the compartment floor image. During the driving process of the loader 110, the first deflection angle and the second deflection angle obtained from the collected data at the same position and the same time are recorded as a set of deflection angle data, and after the loader 110 arrives at the loading position, the deflection angle estimation value is fitted according to the multiple sets of deflection angle data recorded in the historical records, and then the loading angle of the loader 110 is adjusted according to the deflection angle estimation value, thereby improving the accuracy of the deflection angle estimation value, and thus improving the accuracy of the navigation and positioning.

[0090] It should be noted that since the scanning ports of the laser radar 150 are all facing the floor of the truck 120, that is, scanning vertically and horizontally, it is not affected by whether the truck 120 is a flatbed truck 120. Therefore, the navigation and positioning method of the embodiment of the present invention is applicable to any type of truck 120.

[0091] Due to the complex condition of the truck 120, when the floor of the truck is uneven, the error of the first deflection angle calculated by the point cloud data collected by the laser radar 150 is relatively large. However, since the laser line 170 is emitted by the laser emitter 160 outside the truck 120, it is not affected by the internal condition of the truck 120, so the error of the second deflection angle is relatively small. Therefore, the second deflection angle between the loader 110 and the laser line 170 and the first deflection angle are combined to obtain the deflection angle estimation value of the loader 110 at the loading position, which increases the estimation sample, thereby reducing the error influence of the first deflection angle, and further improving the accuracy of the deflection angle estimation value.

[0092] With respect to S103, in one embodiment, edge point cloud data about the edge of the truck 120 directly facing the laser radar 150 can be determined from the point cloud data collected by the laser radar 150. Since the point cloud data has three-dimensional coordinate attributes, the distance between the laser radar 150 and the edge of the truck 120 approaching can be calculated based on the edge point cloud data.

[0093] The method of extracting edge point cloud data from point cloud data can be flexibly selected. For example, edge point cloud data can be extracted using machine learning models such as trained neural networks, or edge point cloud data can be extracted according to preset rules.

[0094] In the loading system 100, the number and location of the laser radars 150 on the loader 110 can be flexibly selected according to actual needs. In one embodiment, the laser radars 150 on the loader 110 can be set to three, including a first radar, a second radar, and a third radar, the first radar is installed on one side of the loader 110, the second radar is installed on the other side of the loader 110, and the connection line between the first radar and the second radar is parallel to the width direction of the loader 110, and the third radar and the second radar are located on the same side of the loader 110.

[0095] Based on the above-mentioned setting and installation of the laser radar 150, in one embodiment, referring to Figure 3 , which is a flowchart of some sub-steps of the above step S103. The following steps are used to calculate the first deviation angle between the central axis of the loader 110 and the truck 120 and the offset of the loader 110 in the truck 120 based on all distances.

[0096] S103-1, based on the distances between the second radar and the third radar and the edge of the truck compartment, and the installation distance between the second radar and the third radar, calculate a first deviation angle between the loader and the center axis of the truck.

[0097] That is, the first deflection angle is obtained according to the distance between the second radar and the edge of the compartment of the truck 120, the distance between the third radar and the edge of the compartment of the truck 120, and the safety distance between the second radar and the third radar.

[0098] S103-2, calculating the offset of the loader in the truck compartment according to the first deflection angle, the distance between the first radar and the edge of the truck compartment, and the distance between the second radar and the edge of the truck compartment.

[0099] Furthermore, with respect to S103 - 1 , a first deflection angle calculation formula is used to calculate a first deflection angle between the center axes of the loader 110 and the truck 120 .

[0100] The first deflection angle calculation formula includes: Among them, θ ladar represents the first deflection angle, FR represents the distance between the second radar and the edge of the compartment of the truck 120, BR represents the distance between the third radar and the edge of the compartment of the truck 120, and L represents the installation distance between the second radar and the third radar.

[0101] For S103 - 2 , the offset calculation company is used to obtain the offset of the loader 110 in the compartment of the truck 120 .

[0102] The offset calculation formula includes: Wherein, FL represents the distance between the first radar and the edge of the compartment of the truck 120, and shfit represents the offset.

[0103] It should be understood that the control device 130 is pre-loaded with a control algorithm, which is used to control the loader 110 to move to the loading position of the compartment of the truck 120. On this basis, for S105, the control device 130 actually controls the movement of the loader 110 in the compartment of the truck 120 based on the first deflection angle and offset obtained in real time and using a preset control algorithm.

[0104] The control algorithm may be a PID-based control algorithm. For example, in one embodiment, the rotation of the drive motor of the loader 110 is controlled by the control algorithm.

[0105] In one embodiment, referring to Figure 4 , which is a flowchart of some sub-steps of the above step S107. The following steps are used to extract the laser line 170 data from the vehicle floor image, and calculate the second deflection angle between the loader 110 and the laser line 170 according to the laser line 170 data.

[0106] S107-1, taking the pixel points in the carriage floor image whose pixel values ​​are greater than the grayscale threshold as target pixel points, and extracting the coordinates of each target pixel point as laser line data.

[0107] Due to the presence of the laser line 170, the brightness of the pixels in the area where the laser line 170 is located in the vehicle floor image is brighter. Therefore, according to the pre-set grayscale threshold, multiple target pixels about the laser line 170 can be extracted from the vehicle floor image. Multiple target pixels constitute the laser line 170 image.

[0108] It should be understood that extracting target pixels according to grayscale thresholds is only one implementation method, and the method of extracting target pixels is not limited to extracting according to grayscale thresholds, and multiple methods can be flexibly selected. For example, target pixels can be extracted according to a pre-trained machine learning model, or according to preset rules.

[0109] S107-2, based on each coordinate, fit a fitting straight line about each target pixel point.

[0110] In one implementation, based on each coordinate, a fitting straight line about each target pixel point can be fitted using the least square method.

[0111] The obtained fitting line is in the form of y=kx+b, where y and x represent the coordinates of the target pixel point, k is the coefficient of the fitting line, and b represents the parameter of the fitting line.

[0112] The fitted straight line is substantially a fitted straight line about the laser line 170 , and quantifies the position of the laser line 170 .

[0113] S107-3, using trigonometric functions according to the coefficients of the fitted straight line, obtain a second deflection angle between the loader and the laser line.

[0114] Further, for S107 - 3 , based on the coefficient of the fitting straight line, a second deflection angle calculation formula is used to obtain a second deflection angle between the loader 110 and the laser line 170 .

[0115] The second deflection angle calculation formula includes:

[0116]

[0117] Among them, θ laser represents the second deflection angle, and k represents the coefficient of the fitting straight line.

[0118] Through the above S107 - 1 to S107 - 3 , the second deflection angle between the loader 110 and the laser line 170 can be calculated, which helps to obtain the actual deflection angle of the loader 110 at the loading position.

[0119] Further, in one embodiment, referring to Figure 5 , is a flowchart of some sub-steps of the above step S109, and the following steps are used to fit the deflection estimation value according to multiple sets of deflection data.

[0120] S109-1, calculating the absolute deflection difference between the first deflection angle and the second deflection angle in each set of deflection angle data.

[0121] S109-2, eliminating the absolute deflection differences greater than the deflection threshold from the multiple absolute deflection differences to obtain multiple target deflection differences, and fitting the multiple target deflection differences to obtain estimated deflection differences.

[0122] The deflection angle threshold is a preset value, which may be set according to the setting angle of the laser line 170 , or may be a value obtained according to historical experience data.

[0123] Among them, the method of fitting multiple target deflection differences to obtain the estimated deflection difference can be flexibly selected. For example, a neural network can be used for fitting, or a preset rule can be used for fitting. In one embodiment, it is achieved in the following way: based on multiple target deflection differences, the least squares method is used to fit the value with the minimum sum of distances to each target deflection difference as the estimated deflection difference.

[0124] The estimated deflection angle difference obtained by the following fitting method is the optimal estimated value based on multiple target deflection angle differences, which helps to improve the accuracy of the deflection angle estimation value.

[0125] S109-3, obtaining a second deflection angle obtained by capturing the carriage floor image at the loading position by the industrial camera.

[0126] Among them, the second deflection angle in S109-3 can be the second deflection angle obtained by the image of the car floor captured by the industrial camera 140 after the loader 110 arrives at and stops at the loading position, or it can be the second deflection angle obtained by the image of the car floor captured by the industrial camera 140 after the loader 110 arrives at the loading position.

[0127] S109-4, taking the difference between the second deflection angle and the estimated deflection angle difference as the deflection angle estimation value.

[0128] Use θ truck Characterizing the estimated deflection angle, it can be known from steps S109-1 to S109-3 that the estimated deflection angle is: θ truck =θ laser -θ ladar +ω. Among them, θ laser is the second deflection angle, θ ladar is the first deflection angle, and ω represents the error term caused by the vehicle condition in the compartment of the truck 120.

[0129] Combined with S109-4, θ is used to represent the estimated value of the deflection angle, then θ = θ laser -θ truck =θ ladar +ω. This indicates that the deflection angle estimation value θ obtained by adopting steps S109-1 to S109-4 takes into account the error caused by the vehicle condition in the compartment of the truck 120. Therefore, the accuracy of the deflection angle estimation value is greatly improved, which in turn helps to improve the accuracy of navigation positioning of the loader 110.

[0130] The navigation and positioning method provided by the embodiment of the present invention is applicable to any type of vehicle, such as a fence-type truck 120, a flatbed truck 120, and a container truck 120. In addition, by combining multiple sets of deflection angle data consisting of a first deflection angle between the loader 110 and the central axis of the truck 120 and a second deflection angle between the loader 110 and the laser line 170, an estimated deflection angle value of the loader 110 at the loading position is obtained. Combining the second deflection angle can reduce the error influence of the vehicle condition in the vehicle compartment on the first deflection angle, thereby improving the accuracy of the deflection angle estimation value, and further improving the accuracy of navigation and positioning.

[0131] Based on the inventive concept of the above navigation and positioning method, in one embodiment, a navigation and positioning system 220 is provided, referring to Figure 6 The navigation and positioning system 220 includes a control device 130 of the loader 110 , an industrial camera 140 and a plurality of laser radars 150 installed on the loader 110 , and a laser transmitter 160 .

[0132] The laser radar 150 is used to collect point cloud data of the compartment area of ​​the truck 120 that it is facing.

[0133] The scanning port of each laser radar 150 is oriented toward the floor of the truck 120 , and collects point cloud data of the area of ​​the truck 120 that it is facing.

[0134] The laser emitter 160 is used to irradiate a laser line 170 onto the floor of the truck 120 .

[0135] The laser emitter 160 may be installed outside the truck 120 , the laser line 170 is parallel to the plane where the floor of the truck 120 is located, and the angle between the laser line 170 and the central axis of the truck 120 may be any.

[0136] The industrial camera 140 is used to collect images of the floor of the truck 120 .

[0137] The control device 130 is used to implement the navigation and positioning method provided in the above embodiment.

[0138] The navigation and positioning system 220 provided in the embodiment of the present invention is applicable to the navigation and positioning of any type of vehicle such as a fence-type truck 120, a flatbed truck 120, and a container truck 120. In addition, the navigation and positioning system 220 combines multiple sets of deflection angle data consisting of a first deflection angle between the loader 110 and the central axis of the truck 120 and a second deflection angle between the loader 110 and the laser line 170 to obtain an estimated deflection angle of the loader 110 at the loading position. The combination of the second deflection angle can reduce the error influence of the vehicle condition in the vehicle compartment on the first deflection angle, thereby improving the accuracy of the deflection angle estimation value, and further improving the accuracy of navigation and positioning.

[0139] In one embodiment, Figure 7 As shown, a navigation and positioning device 180 is provided. The navigation and positioning device 180 is applied to the control device 130 of the loader 110 in the above-mentioned loading system 100 or the navigation and positioning system 220. The navigation and positioning device 180 includes a data acquisition module 190, a navigation module 200 and a positioning module 210.

[0140] The data acquisition module 190 is used to acquire a set of collected data in real time.

[0141] The collected data includes point cloud data of the compartment area of ​​the truck 120 that each laser radar 150 is facing at the same position and time, and the image of the compartment floor of the truck 120 collected by the industrial camera 140.

[0142] The navigation module 200 is used to obtain the distance between each laser radar 150 and the edge of the truck 120 according to the point cloud data, and calculate the first deviation angle between the loader 110 and the central axis of the truck 120 and the offset of the loader 110 in the truck 120 according to all the distances. It is also used to control the movement of the loader 110 in the truck 120 in real time according to the first deviation angle and the offset.

[0143] The positioning module 210 is used to extract the laser line 170 data from the car floor image, calculate the second deflection angle between the loader 110 and the laser line 170 based on the laser line 170 data, record the first deflection angle and the second deflection angle obtained from the same set of collected data as a set of deflection angle data, and after the loader 110 arrives at the loading position, fit the deflection angle estimation value based on multiple sets of deflection angle data, and adjust the loading angle of the loader 110 based on the deflection angle estimation value.

[0144] For the specific definition of the navigation and positioning device 180, please refer to the definition of the navigation and positioning method above, which will not be repeated here. Each module in the above-mentioned navigation and positioning device 180 can be implemented in whole or in part by software, hardware and a combination thereof. The above-mentioned modules can be embedded in or independent of the processor in the control device 130 in the form of hardware, or can be stored in the memory in the control device 130 in the form of software, so that the processor can call and execute the operations corresponding to the above modules.

[0145] It should be understood that the control device 130 is a computer device.

[0146] In one embodiment, a control device 130 is provided. The control device 130 may be a terminal, and its internal structure diagram may be as follows: Figure 8 As shown. The control device 130 includes a processor, a memory, a communication interface, a display screen and an input device connected through a system bus. Among them, the processor of the control device 130 is used to provide computing and control capabilities. The memory of the control device 130 includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The communication interface of the control device 130 is used to communicate with an external terminal in a wired or wireless manner, and the wireless manner can be implemented through WIFI, an operator network, near field communication (NFC) or other technologies. When the computer program is executed by the processor, a navigation positioning method is implemented. The display screen of the control device 130 can be a liquid crystal display screen or an electronic ink display screen, and the input device of the control device 130 can be a touch layer covered on the display screen, or a button, a trackball or a touch pad set on the housing of the control device 130, or an external keyboard, touch pad or mouse, etc.

[0147] Those skilled in the art will understand that Figure 8 The structure shown in the figure is only a block diagram of a part of the structure related to the scheme of the present invention, and does not constitute a limitation on the control device 130 to which the scheme of the present invention is applied. The specific control device 130 may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0148] In one embodiment, the navigation and positioning device 180 provided by the present invention can be implemented in the form of a computer program. The computer program can be used in Figure 8 The computer device shown in the figure is run. The memory of the computer device can store various program modules constituting the navigation and positioning device 180, for example, Figure 7 The data acquisition module 190, navigation module 200 and positioning module 210 are shown. The computer program composed of various program modules enables the processor to execute the steps of the navigation and positioning method of various embodiments of the present invention described in this specification.

[0149] For example, Figure 8 The control device 130 shown can be Figure 7 The data acquisition module 190 in the navigation and positioning device 180 shown in the figure performs step S101. The control device 130 can perform steps S103 and S105 through the navigation module 200. The control device 130 can perform steps S107 and S109 through the positioning module 210.

[0150] In one embodiment, a control device 130 is provided, including a memory and a processor, the memory storing a computer program, and the processor implementing the following steps when executing the computer program: acquiring a set of collected data in real time; obtaining the distance between each laser radar 150 and the edge of the carriage of the truck 120 according to the point cloud data, and calculating the first deflection angle between the loader 110 and the central axis of the truck 120 and the offset of the loader 110 in the carriage of the truck 120 according to all distances; controlling the travel of the loader 110 in the carriage of the truck 120 in real time according to the first deflection angle and the offset; extracting the laser line 170 data from the carriage floor image, calculating the second deflection angle between the loader 110 and the laser line 170 according to the laser line 170 data, and recording the first deflection angle and the second deflection angle obtained from the same set of collected data as a set of deflection angle data; after the loader 110 arrives at the loading position, fitting the deflection angle estimation value according to multiple sets of deflection angle data, and adjusting the loading angle of the loader 110 according to the deflection angle estimation value.

[0151] In other embodiments, when the processor executes the computer program, it further implements other steps in the navigation and positioning method provided above.

[0152] In one embodiment, a storage medium is provided, on which a computer program is stored, and when the computer program is executed by a processor, the following steps are implemented: a set of collected data is acquired in real time; the distance between each laser radar 150 and the edge of the carriage of the truck 120 is obtained based on the point cloud data, and a first deflection angle between the loader 110 and the central axis of the truck 120 and an offset of the loader 110 in the carriage of the truck 120 are calculated based on all distances; the travel of the loader 110 in the carriage of the truck 120 is controlled in real time based on the first deflection angle and the offset; laser line 170 data is extracted from the carriage floor image, and a second deflection angle between the loader 110 and the laser line 170 is calculated based on the laser line 170 data, and the first deflection angle and the second deflection angle obtained from the same set of collected data are recorded as a set of deflection angle data; after the loader 110 arrives at the loading position, a deflection angle estimation value is fitted based on multiple sets of deflection angle data, and the loading angle of the loader 110 is adjusted based on the deflection angle estimation value.

[0153] Similarly, in other embodiments, when the computer program is executed by the processor, other steps in the above navigation and positioning method are also implemented.

[0154] In several embodiments provided by the present invention, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely schematic. For example, the flowcharts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the devices, methods and computer program products according to multiple embodiments of the present invention. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of a code, and the module, a program segment or a part of a code contains one or more executable instructions for implementing the specified logical functions. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive 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 box in the block diagram and / or flowchart, and the combination of boxes in the block diagram and / or flowchart can be implemented with a dedicated hardware-based system that performs a specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

[0155] In addition, the functional modules in the various embodiments of the present invention may be integrated together to form an independent part, or each module may exist independently, or two or more modules may be integrated to form an independent part.

[0156] If the functions are implemented in the form of software function modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium, including several instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods described in each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0157] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A navigation and positioning method, characterized in that: A control device applied to a loader, the loader is equipped with an industrial camera and a plurality of laser radars, a laser transmitter matched with the control device is used to irradiate a laser line onto a floor of a truck compartment, the control device is communicatively connected with the laser radar and the industrial camera, and the method comprises: Acquire a set of collected data in real time, the collected data including point cloud data of the truck compartment area facing each laser radar at the same position and time, and the image of the truck compartment floor collected by the industrial camera; Obtaining the distance between each of the laser radars and the edge of the truck compartment according to the point cloud data, and calculating the first deflection angle between the loader and the center axis of the truck, and the offset of the loader in the truck compartment according to all the distances; Controlling the movement of the loader in the truck compartment in real time according to the first deflection angle and the offset; Extracting laser line data from the carriage floor image, calculating a second deflection angle between the loader and the laser line according to the laser line data, and recording the first deflection angle and the second deflection angle obtained from the same set of collected data as a set of deflection angle data; After the loader arrives at the loading position, fitting a deflection angle estimation value according to the multiple sets of deflection angle data, and adjusting the loading angle of the loader according to the deflection angle estimation value; The step of extracting laser line data from the carriage floor image and calculating a second deflection angle between the loader and the laser line according to the laser line data comprises: Pixels in the carriage floor image whose pixel values ​​are greater than a grayscale threshold are taken as target pixels, and the coordinates of each target pixel are extracted as laser line data; Based on the coordinates, fitting straight lines about the target pixels are obtained; According to the coefficients of the fitting straight line, a second deflection angle between the loader and the laser line is obtained by using trigonometric functions; The step of fitting the deflection angle estimation value according to the multiple groups of deflection angle data comprises: Calculating the absolute deflection difference between the first deflection angle and the second deflection angle in each set of deflection angle data; Eliminate the absolute deflection differences greater than the deflection threshold value from the plurality of absolute deflection differences to obtain a plurality of target deflection differences, and fit the plurality of target deflection differences to obtain an estimated deflection difference; Acquire a second deflection angle obtained by capturing the carriage floor image at the loading position by the industrial camera; The difference between the second deflection angle and the estimated deflection angle difference is used as the deflection angle estimation value.

2. The navigation and positioning method according to claim 1, characterized in that: There are three laser radars, including a first radar, a second radar and a third radar. The first radar is installed on one side of the loader, and the second radar is installed on the other side of the loader. The connecting line of the first radar and the second radar is parallel to the width direction of the loader. The third radar and the second radar are located on the same side of the loader. The step of calculating the first deviation angle between the loader and the center axis of the truck and the offset of the loader in the truck compartment according to all the distances comprises: Calculate a first deflection angle between the loader and the center axis of the truck based on the distances between the second radar and the third radar and the edge of the truck compartment, and the installation distance between the second radar and the third radar; The offset of the loader in the truck compartment is calculated based on the first deflection angle, the distance between the first radar and the edge of the truck compartment, and the distance between the second radar and the edge of the truck compartment.

3. The navigation and positioning method according to claim 2, characterized in that: The step of calculating the first deflection angle between the loader and the center axis of the truck includes: Calculate the first deflection angle between the loader and the center axis of the truck using a first deflection angle calculation formula; Wherein, the first deflection angle calculation formula includes: , Characterize the first deflection angle, Characterizes the distance between the second radar and the edge of the truck compartment, Characterizes the distance between the third radar and the edge of the truck compartment, Characterize the installation distance between the second radar and the third radar; The step of calculating the offset of the loader in the truck compartment comprises: Using the offset calculation formula, the offset of the loader in the truck compartment is obtained; The offset calculation formula includes: , Characterizes the distance between the first radar and the edge of the truck compartment, Characterize the offset.

4. The navigation and positioning method according to claim 1, characterized in that: The step of obtaining the second deflection angle between the loader and the laser line by using a trigonometric function according to the coefficient of the fitting straight line comprises: Based on the coefficient of the fitting straight line, a second deflection angle between the loader and the laser line is obtained using a second deflection angle calculation formula; The second deflection angle calculation formula includes: in, represents the second deflection angle, and k represents the coefficient of the fitting straight line.

5. The navigation and positioning method according to claim 1, characterized in that: The step of fitting a plurality of target deflection differences to obtain an estimated deflection difference comprises: Based on the plurality of target deflection differences, a value with the minimum sum of distances from each of the target deflection differences is fitted using the least squares method as the estimated deflection difference.

6. A navigation and positioning system, characterized in that: It includes a control device of a vehicle loader, an industrial camera and a plurality of laser radars installed on the vehicle loader, and a laser transmitter; The laser emitter is used to irradiate the laser line onto the floor of the truck compartment; The industrial camera is used to collect the image of the floor of the truck compartment; The laser radar is used to collect point cloud data of the truck compartment area that the laser radar is facing; The control device is used to implement the navigation and positioning method as described in any one of claims 1 to 5.

7. A navigation and positioning device, characterized in that: A control device applied to a loader, the loader is equipped with an industrial camera and multiple laser radars, a laser transmitter coordinated with the control device is used to irradiate a laser line onto the floor of a truck compartment, the control device is communicatively connected with the laser radar and the industrial camera, and the navigation and positioning device includes a data acquisition module, a navigation module and a positioning module to implement the navigation and positioning method according to any one of claims 1 to 5: The data acquisition module is used to acquire a set of collected data in real time, wherein the collected data includes point cloud data of the truck compartment area facing each of the laser radars at the same position and time, and an image of the truck compartment floor collected by the industrial camera; The navigation module is used to obtain the distance between each of the laser radars and the edge of the truck compartment according to the point cloud data, calculate the first deflection angle between the loader and the center axis of the truck, and the offset of the loader in the truck compartment according to all the distances, and control the movement of the loader in the truck compartment in real time according to the first deflection angle and the offset; The positioning module is used to extract laser line data from the car floor image, calculate the second deviation angle between the loader and the laser line based on the laser line data, record the first deviation angle and the second deviation angle obtained from the same group of collected data as a group of deviation angle data, and after the loader arrives at the loading position, fit the deviation angle estimation value based on multiple groups of the deviation angle data, and adjust the loading angle of the loader according to the deviation angle estimation value.

8. A storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the navigation and positioning method according to any one of claims 1 to 5 is implemented.

Images