AGV visual navigation method and system, electronic equipment and storage medium

Abstract

The invention provides an AGV visual navigation method and system, electronic equipment and a storage medium. The method comprises the following steps: scanning a two-dimensional code matrix through a vehicle-mounted camera; obtaining a two-dimensional code scanned by a vehicle-mounted camera, and calculating the relative pose of the center of the two-dimensional code matrix and the camera according to the relative pose of the center of the two-dimensional code and the camera and the relative pose of the center of the two-dimensional code matrix and the center of the two-dimensional code matrix; calculating the relative pose of the vehicle body and the center of the two-dimensional code matrix according to the relative pose of the center of the two-dimensional code matrix and the camera and the calibrated relative pose between the camera and the vehicle body; and calculating the current pose of the vehicle according to the pose of the center of the two-dimensional code matrix in the world coordinate system and the relative pose of the vehicle body and the center of the two-dimensional code matrix. According to the scheme, vehicle positioning can be realized under the condition that the vehicle slightly deviates from the pre-estimated path or part of the two-dimensional code is shielded, and the vehicle positioning accuracy and robustness are guaranteed.

Description

Technical Field

[0001] The present invention belongs to the field of navigation technology, and in particular relates to an AGV visual navigation method, system, electronic equipment and storage medium. Background Art

[0002] QR code navigation is one of the most commonly used and basic navigation methods in the AGV industry. In actual operation, the AGV uses its own visual sensor to scan and identify the QR code pre-posted in the environment to achieve positioning and navigation functions.

[0003] At present, the industry generally uses a single QR code to achieve positioning. When the vehicle deviates slightly from the operating path or the single QR code is affected by stains, non-human occlusion and other factors, the visual sensor will not be able to scan the QR code or read the QR code information, which will cause the vehicle to lose its positioning. Summary of the invention

[0004] In view of this, an embodiment of the present invention provides an AGV visual navigation method, system, electronic device and storage medium, which are used to solve the problem of vehicle losing positioning when the QR code is blocked.

[0005] In a first aspect of an embodiment of the present invention, an AGV visual navigation method is provided, comprising: Scan the QR code matrix through the on-board camera; The two-dimensional code matrix is ​​at least a 2×2 matrix, the two-dimensional codes in the matrix are of the same size, and the two-dimensional codes are arranged equidistantly; Obtain the QR code scanned by the vehicle-mounted camera, and calculate the relative pose between the center of the QR code matrix and the camera based on the relative pose between the center of the QR code and the camera and the relative pose between the center of the QR code and the center of the QR code matrix; According to the relative pose between the center of the QR code matrix and the camera and the calibrated relative pose between the camera and the vehicle body, the relative pose between the vehicle body and the center of the QR code matrix is ​​calculated; The current posture of the vehicle is calculated based on the posture of the center of the QR code matrix in the world coordinate system and the relative posture of the vehicle body and the center of the QR code matrix.

[0006] In a second aspect of an embodiment of the present invention, an AGV visual navigation system is provided, comprising: Data acquisition module, used to scan the QR code matrix through the vehicle-mounted camera; The two-dimensional code matrix is ​​at least a 2×2 matrix, the two-dimensional codes in the matrix are of the same size, and the two-dimensional codes are arranged equidistantly; A relative pose calculation module is used to obtain the QR code scanned by the vehicle-mounted camera, and calculate the relative pose between the center of the QR code matrix and the camera according to the relative pose between the center of the QR code and the camera and the relative pose between the center of the QR code and the center of the QR code matrix; and According to the relative pose between the center of the QR code matrix and the camera and the calibrated relative pose between the camera and the vehicle body, the relative pose between the vehicle body and the center of the QR code matrix is ​​calculated; The vehicle positioning module is used to calculate the current posture of the vehicle based on the posture of the center of the QR code matrix in the world coordinate system and the relative posture of the vehicle body and the center of the QR code matrix.

[0007] In a third aspect of an embodiment of the present invention, an electronic device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the method described in the first aspect of the embodiment of the present invention when executing the computer program.

[0008] In a fourth aspect of an embodiment of the present invention, a computer-readable storage medium is provided, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the method provided in the first aspect of the embodiment of the present invention are implemented.

[0009] In an embodiment of the present invention, visual navigation based on a multi-code matrix can still achieve vehicle positioning when the vehicle deviates slightly from the estimated path or part of the QR code is contaminated or blocked, avoiding the problem of vehicle positioning loss caused by traditional single QR code being blocked, and ensuring the accuracy and robustness of vehicle positioning. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0011] Figure 1 A schematic diagram of a flow chart of an AGV visual navigation method provided by one embodiment of the present invention; Figure 2 A schematic diagram of the installation position of a vehicle-mounted camera provided by an embodiment of the present invention; Figure 3 A schematic diagram of a two-dimensional code matrix provided by an embodiment of the present invention; Figure 4 A schematic diagram of the structure of an AGV visual navigation system provided by one embodiment of the present invention; Figure 5A schematic structural diagram of an electronic device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0012] In order to make the purpose, features and advantages of the present invention more obvious and easy to understand, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the embodiments described below are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0013] It should be understood that the term "including" and other similar expressions in the specification or claims of the present invention and the above-mentioned drawings are intended to cover non-exclusive inclusions, such as a process, method, system, or device including a series of steps or units is not limited to the listed steps or units. In addition, "first" and "second" are used to distinguish different objects, not to describe a specific order.

[0014] See also Figure 1 , a flow chart of an AGV visual navigation method provided by an embodiment of the present invention includes: S101, scanning the QR code matrix through the vehicle-mounted camera; The two-dimensional code matrix is ​​at least a 2×2 matrix, the two-dimensional codes in the matrix are of the same size, and the two-dimensional codes are arranged equidistantly; The vehicle-mounted camera, also known as the visual sensor, is located at the bottom of the vehicle, with its viewing angle facing the ground, and is generally installed at the center of the length and width of the vehicle body. Figure 2 The internal and external parameters of the vehicle-mounted camera are calibrated values. The position of the camera in the vehicle is fixed, and its posture relationship with the vehicle body is a calibrated value, and the distance from the ground is also a calibrated value.

[0015] The two-dimensional code matrix is ​​at least a 2×2 matrix, for example, it can be a 2×3, 4×3, 4×4 matrix, etc., each two-dimensional code has the same size, and the distance between adjacent two-dimensional codes is the same. At the same time, each two-dimensional code corresponds to a number ID. Figure 3 As shown, the figure shows a 4×4 two-dimensional code matrix, in which the length and width of a single two-dimensional code are both 15 mm, and the spacing between adjacent two-dimensional codes is 5 mm.

[0016] In this embodiment, the vehicle generally operates in specific areas such as factories, ports or indoors, and can be a two-wheeled vehicle or a four-wheeled vehicle, and the specific vehicle type is not further limited.

[0017] S102, obtaining the QR code scanned by the vehicle-mounted camera, and calculating the relative pose between the center of the QR code matrix and the camera according to the relative pose between the center of the QR code and the camera and the relative pose between the center of the QR code and the center of the QR code matrix; In some embodiments, the QR code matrix is ​​scanned by a vehicle-mounted camera, and the relative position of the center of the QR code with respect to the center of the camera can be automatically calculated for the QR code scanned by the camera, and the ID number of the QR code can be obtained.

[0018] In the QR code matrix, since the size and corner positions of the QR code are known, and the intrinsic and extrinsic parameters of the camera are known, the relative pose of the recognizable QR code center relative to the camera can be calculated, as well as the relative pose of the QR code center relative to the center of the QR code matrix. The QR code matrix center refers to the center of the entire QR code matrix pattern.

[0019] Specifically, the relative position of the center of the QR code with respect to the camera is calculated according to the corner point information of the QR code, the distance between the vehicle-mounted camera and the ground, and the internal and external parameters of the vehicle-mounted camera.

[0020] Among them, the position and posture of the center of each QR code relative to the center of the QR code matrix are calculated based on the QR code ID number, QR code size and distance information between QR codes scanned by the camera.

[0021] Since the size, spacing and coding number of each QR code in the QR code matrix are known, the position of the QR code relative to the center of the matrix can be determined according to the QR code number, and then the relative position of the QR code center relative to the center of the matrix can be calculated according to the QR code size and QR code spacing information.

[0022] For example, there are three QR codes scanned by the camera, with IDs 1, 2, and 3. Since the arrangement order and size information of the QR codes are known (for example, the length and width of the QR codes are both 15 mm, and the distance between adjacent QR codes is 5 mm), the position (x iToCenter ,y iToCenter ,th iToCenter ),as follows: ; ; th iToCenter =0, In the formula, SingleID is the scanned QR code ID, MultiID is the QR code matrix number, and the unit is millimeter. Therefore, we can get (x 1ToCenter , y 1ToCenter ) = (-30,30), (x 1ToCenter , y 1ToCenter ) = (-30,30), (x2ToCenter ,y 2ToCenter ) = (-10,30), (x 3ToCenter , y 3ToCenter ) = (10,30).

[0023] The center of the QR code refers to the center of the QR code scanned by the vehicle-mounted camera, that is, the center of the unobstructed and recognizable QR code. The relative pose between the recognizable QR code center and the camera and the relative pose between the QR code center and the QR code matrix center can be calculated.

[0024] Specifically, the relative position of the center of the QR code matrix and the camera is calculated according to the formula: ; ; th iCenterToCam = th iToCam -th iToCenter ; Among them, the position of the center of the QR code scanned by the camera relative to the center of the QR code matrix is ​​(x iToCenter ,y iToCenter , th iToCenter ), the position of the center of the QR code scanned by the camera relative to the camera is expressed as (x iToCam ,y iToCam , th iToCam ), the relative position of the center of the QR code matrix and the camera is expressed as (x iCenterToCam ,y iCenterToCam , th iCenterToCam ), x, y represent the horizontal and vertical coordinates of the plane, and th represents the attitude angle.

[0025] Preferably, the relative position (x iCenterToCam ,y iCenterToCam , th iCenterToCam ) to obtain the average value of the relative position of the QR code matrix center and the camera (x CenterToCam ,y CenterToCam , th CenterToCam ).

[0026] Based on the QR code scanned by the camera, the relative poses of the QR code matrix center and the camera can be calculated respectively. The average of these relative poses can be used to obtain the average relative pose of the QR code matrix center and the camera, which has higher accuracy than a single QR code.

[0027] S103, calculating the relative posture of the vehicle body and the center of the QR code matrix according to the relative posture of the center of the QR code matrix and the camera and the calibrated relative posture between the camera and the vehicle body; Since the position of the camera in the vehicle is fixed, the relative pose between the camera and the vehicle is determined by the camera's installation position and installation posture in the vehicle, such as the camera is located at the front or the center of the vehicle, the angle between the camera lens and the horizontal direction of the vehicle, etc. The relative pose between the two is generally a calibration value. The relative pose between the vehicle and the center of the QR code matrix can be obtained based on the calibrated relative pose between the camera and the vehicle, combined with the relative pose between the center of the QR code matrix and the camera.

[0028] S104, calculating the current posture of the vehicle according to the posture of the center of the two-dimensional code matrix in the world coordinate system and the relative posture of the vehicle body and the center of the two-dimensional code matrix.

[0029] It can be understood that since the QR code matrix pattern is posted on the ground, the center position of the matrix is ​​fixed and known. When a vehicle passes over the QR code matrix pattern, the vehicle posture can be calculated based on the center position of the QR code matrix and the relative posture of the vehicle body and the center of the QR code matrix.

[0030] In this embodiment, visual navigation based on the multi-code matrix can still achieve vehicle positioning when the vehicle deviates slightly from the estimated path or part of the QR code is contaminated or blocked, and ensure the accuracy and robustness of vehicle positioning.

[0031] It should be understood that the serial numbers of the steps in the above embodiments do not imply a sequence of execution, and the execution sequence of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0032] Figure 4 A schematic diagram of the structure of an AGV visual navigation system provided by an embodiment of the present invention, the system includes: The data acquisition module 410 is used to scan the QR code matrix through the vehicle-mounted camera; The two-dimensional code matrix is ​​at least a 2×2 matrix, the two-dimensional codes in the matrix are of the same size, and the two-dimensional codes are arranged equidistantly; The relative pose calculation module 420 is used to obtain the two-dimensional code scanned by the vehicle-mounted camera, and calculate the relative pose of the center of the two-dimensional code matrix and the camera according to the relative pose of the center of the two-dimensional code and the camera and the relative pose of the center of the two-dimensional code and the center of the two-dimensional code matrix; and According to the relative pose between the center of the QR code matrix and the camera and the calibrated relative pose between the camera and the vehicle body, the relative pose between the vehicle body and the center of the QR code matrix is ​​calculated; Wherein, the relative posture calculation module 420 includes: The first relative posture calculation unit is used to calculate the relative posture of the center of the QR code and the camera according to the corner point information of the QR code, the distance between the vehicle-mounted camera and the ground, and the internal and external parameters of the vehicle-mounted camera.

[0033] Wherein, the relative posture calculation module 420 also includes: The second relative posture calculation unit is used to calculate the posture of the center of the two-dimensional code relative to the center of the two-dimensional code matrix according to the two-dimensional code ID number, the size of the two-dimensional code and the distance information between the two-dimensional codes scanned by the camera.

[0034] Specifically, the relative position of the center of the QR code matrix and the camera is calculated according to the formula: ; ; th iCenterToCam = th iToCam -th iToCenter ; Among them, the position of the center of the QR code scanned by the camera relative to the center of the QR code matrix is ​​(x iToCenter ,y iToCenter , th iToCenter ), the position of the center of the QR code scanned by the camera relative to the camera is expressed as (x iToCam ,y iToCam , th iToCam ), the relative position of the center of the QR code matrix and the camera is expressed as (x iCenterToCam ,y iCenterToCam , th iCenterToCam ), x, y represent the horizontal and vertical coordinates of the plane, and th represents the attitude angle.

[0035] Preferably, the step of calculating the relative position of the center of the two-dimensional code matrix and the camera according to the formula further includes: The relative position (x) of the QR code matrix center and the camera corresponding to all QR codes scanned by the camera iCenterToCam ,y iCenterToCam , th iCenterToCam ) to obtain the average value of the relative position of the QR code matrix center and the camera (x CenterToCam ,y CenterToCam , th CenterToCam ).

[0036] The vehicle positioning module 430 is used to calculate the current posture of the vehicle according to the posture of the center of the two-dimensional code matrix in the world coordinate system and the relative posture of the vehicle body and the center of the two-dimensional code matrix.

[0037] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the above-described systems and modules can refer to the corresponding processes in the aforementioned method embodiments, and will not be repeated here.

[0038] Figure 51 is a schematic diagram of the structure of an electronic device provided by an embodiment of the present invention. The electronic device is used for vehicle AGV visual positioning. Figure 5 As shown, the electronic device 5 of this embodiment includes: a memory 510, a processor 520 and a system bus 530, wherein the memory 510 includes an executable program 5101 stored thereon, and those skilled in the art can understand that Figure 5 The electronic device structure shown in the figure does not constitute a limitation of the electronic device, and may include more or less components than shown in the figure, or combine certain components, or arrange the components differently.

[0039] Combine the following Figure 5 A detailed introduction to the various components of electronic equipment: The memory 510 can be used to store software programs and modules. The processor 520 executes various functional applications and data processing of the electronic device by running the software programs and modules stored in the memory 510. The memory 510 may mainly include a program storage area and a data storage area, wherein the program storage area may store an operating system, an application required for at least one function (such as a sound playback function, an image playback function, etc.), etc.; the data storage area may store data created according to the use of the electronic device (such as cache data), etc. In addition, the memory 510 may include a high-speed random access memory, and may also include a non-volatile memory, such as at least one disk storage device, a flash memory device, or other volatile solid-state storage devices.

[0040] The memory 510 includes an executable program 5101 of a network request method, and the executable program 5101 can be divided into one or more modules / units, which are stored in the memory 510 and executed by the processor 520 to realize abnormal detection and positioning of charging piles, etc. The one or more modules / units can be a series of computer program instruction segments that can complete specific functions, and the instruction segments are used to describe the execution process of the computer program 5101 in the electronic device 5. For example, the computer program 5101 can be divided into functional modules such as a data acquisition module, a relative posture calculation module, and a vehicle positioning module.

[0041] The processor 520 is the control center of the electronic device. It uses various interfaces and lines to connect various parts of the entire electronic device. By running or executing software programs and / or modules stored in the memory 510, and calling data stored in the memory 510, it performs various functions of the electronic device and processes data, thereby monitoring the overall status of the electronic device. Optionally, the processor 520 may include one or more processing units; preferably, the processor 520 may integrate an application processor and a modem processor, wherein the application processor mainly processes the operating system, application programs, etc., and the modem processor mainly processes wireless communications. It is understandable that the above-mentioned modem processor may not be integrated into the processor 520.

[0042] The system bus 530 is used to connect the various functional components inside the computer, and can transmit data information, address information, and control information. Its types can be, for example, PCI bus, ISA bus, CAN bus, etc. The instructions of the processor 520 are transmitted to the memory 510 through the bus, and the memory 310 feeds back data to the processor 520. The system bus 530 is responsible for the data and instruction exchange between the processor 520 and the memory 510. Of course, the system bus 530 can also be connected to other devices, such as network interfaces, display devices, etc.

[0043] In the embodiment of the present invention, the executable program executed by the processor 520 included in the electronic device includes: Scan the QR code matrix through the on-board camera; The two-dimensional code matrix is ​​at least a 2×2 matrix, the two-dimensional codes in the matrix are of the same size, and the two-dimensional codes are arranged equidistantly; Obtain the QR code scanned by the vehicle-mounted camera, and calculate the relative pose between the center of the QR code matrix and the camera based on the relative pose between the center of the QR code and the camera and the relative pose between the center of the QR code and the center of the QR code matrix; According to the relative pose between the center of the QR code matrix and the camera and the calibrated relative pose between the camera and the vehicle body, the relative pose between the vehicle body and the center of the QR code matrix is ​​calculated; The current posture of the vehicle is calculated based on the posture of the center of the QR code matrix in the world coordinate system and the relative posture of the vehicle body and the center of the QR code matrix.

[0044] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and modules described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0045] In the above embodiments, the description of each embodiment has its own emphasis. For parts that are not described or recorded in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0046] As described above, the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features thereof may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. An AGV visual navigation method, characterized in that: include: Scan the QR code matrix through the on-board camera; The two-dimensional code matrix is ​​at least a 2×2 matrix, the two-dimensional codes in the matrix are of the same size, and the two-dimensional codes are arranged equidistantly; Obtain the QR code scanned by the vehicle-mounted camera, and calculate the relative pose between the center of the QR code matrix and the camera based on the relative pose between the center of the QR code and the camera and the relative pose between the center of the QR code and the center of the QR code matrix; According to the relative pose between the center of the QR code matrix and the camera and the calibrated relative pose between the camera and the vehicle body, the relative pose between the vehicle body and the center of the QR code matrix is ​​calculated; The current posture of the vehicle is calculated based on the posture of the center of the QR code matrix in the world coordinate system and the relative posture of the vehicle body and the center of the QR code matrix.

2. The method according to claim 1, characterized in that The obtaining of the QR code scanned by the vehicle-mounted camera also includes: The relative position of the QR code center and the camera is calculated based on the QR code corner information, the distance between the vehicle camera and the ground, and the internal and external parameters of the vehicle camera.

3. The method according to claim 1, characterized in that The step of calculating the relative pose between the center of the two-dimensional code matrix and the camera according to the relative pose between the center of the two-dimensional code and the camera and the relative pose between the center of the two-dimensional code and the center of the two-dimensional code matrix comprises: According to the QR code ID number, QR code size and distance information between QR codes scanned by the camera, the position of the QR code center relative to the center of the QR code matrix is ​​calculated.

4. The method according to claim 1, characterized in that: The step of calculating the relative pose between the center of the two-dimensional code matrix and the camera according to the relative pose between the center of the two-dimensional code and the camera and the relative pose between the center of the two-dimensional code and the center of the two-dimensional code matrix comprises: Calculate the relative position of the QR code matrix center and the camera according to the formula: ； ； th iCenterToCam = th iToCam - th iToCenter ; Among them, the position of the center of the QR code scanned by the camera relative to the center of the QR code matrix is ​​(x iToCenter ,y iToCenter , th iToCenter ), the position of the center of the QR code scanned by the camera relative to the camera is expressed as (x iToCam ,y iToCam , th iToCam ), the relative position of the center of the QR code matrix and the camera is expressed as (x iCenterToCam ,y iCenterToCam , th iCenterToCam ), x, y represent the horizontal and vertical coordinates of the plane, and th represents the attitude angle.

5. The method according to claim 4, characterized in that The method of calculating the relative position of the center of the two-dimensional code matrix and the camera according to the formula also includes: The relative position (x) of the QR code matrix center and the camera corresponding to each QR code scanned by the camera iCenterToCam ,y iCenterToCam , th iCenterToCam ) to obtain the average value of the relative pose between the center of the QR code matrix and the camera.

6. An AGV visual navigation system, characterized in that: include: Data acquisition module, used to scan the QR code matrix through the vehicle-mounted camera; The two-dimensional code matrix is ​​at least a 2×2 matrix, the two-dimensional codes in the matrix are of the same size, and the two-dimensional codes are arranged equidistantly; A relative pose calculation module is used to obtain the QR code scanned by the vehicle-mounted camera, and calculate the relative pose between the center of the QR code matrix and the camera according to the relative pose between the center of the QR code and the camera and the relative pose between the center of the QR code and the center of the QR code matrix; and According to the relative pose between the center of the QR code matrix and the camera and the calibrated relative pose between the camera and the vehicle body, the relative pose between the vehicle body and the center of the QR code matrix is ​​calculated; The vehicle positioning module is used to calculate the current posture of the vehicle based on the posture of the center of the QR code matrix in the world coordinate system and the relative posture of the vehicle body and the center of the QR code matrix.

7. The system according to claim 6, characterized in that The relative posture calculation module comprises: The first relative posture calculation unit is used to calculate the relative posture of the center of the QR code and the camera according to the corner point information of the QR code, the distance between the vehicle-mounted camera and the ground, and the internal and external parameters of the vehicle-mounted camera.

8. The system according to claim 6, characterized in that The relative posture calculation module comprises: The second relative posture calculation unit is used to calculate the posture of the center of the two-dimensional code relative to the center of the two-dimensional code matrix according to the two-dimensional code ID number, the size of the two-dimensional code and the distance information between the two-dimensional codes scanned by the camera.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the AGV visual navigation method as described in any one of claims 1 to 5 are implemented.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed, the steps of the AGV visual navigation method as described in any one of claims 1 to 5 are implemented.

Images