A double-layer visual servo navigation method based on feature code

Through the dual-layer visual servo navigation method, the top and bottom cameras are used to identify the ground feature code and AprilTag code, combined with kinematic model and Kalman filtering method, the problem of flexibility and accuracy of unmanned vehicle navigation in complex environments is solved, and the problem of unmanned vehicle navigation is realized. High stability and high precision navigation effect.

CN114894192BActive Publication Date: 2024-11-29SOUTH CHINA UNIV OF TECH
View PDF 2 Cites 0 Cited by

Patent Information

Application Number
CN202210395982.3
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-04-15
Publication Date
2024-11-29
Estimated Expiration
2042-04-15

AI Technical Summary

Technical Problem

The existing unmanned vehicle navigation technology is difficult to balance between flexibility and accuracy, especially in complex environments. The traditional bottom code scanning method is unstable in the area without the feature code and is costly.

Method used

The double-layer visual servo navigation method is used to identify the ground feature code and AprilTag code through the top and bottom cameras, and combined with kinematic model and Kalman filtering method, high-precision and high-stability navigation of unmanned vehicles is achieved.

Benefits of technology

It improves the navigation accuracy and stability of unmanned vehicles in complex environments, reduces sensor costs, and achieves high-precision position recognition and control through data fusion of dual cameras.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN114894192B_ABST
    Figure CN114894192B_ABST
Patent Text Reader

Abstract

The present invention discloses a double-layer visual servo navigation method for motion control, comprising the following steps: a camera is installed on the top of an unmanned vehicle at a certain angle to the ground to detect feature codes in advance, so as to predict and correct the trajectory of the unmanned vehicle; a camera is installed on the bottom of the unmanned vehicle perpendicular to the ground and downward to detect feature codes laid on the ground, so as to accurately locate and navigate the unmanned vehicle. The present invention realizes a double-layer visual servo navigation method by integrating dual cameras for coordinated cooperation. When the top camera passes the detection and the unmanned vehicle has not reached the feature code, the navigation of the unmanned vehicle is completed through visual feedback and PID control. When the unmanned vehicle reaches the feature code, the bottom camera recognizes the pattern in the middle of the feature code for accurate positioning, so as to realize the precise operation of the unmanned vehicle.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the field of navigation and control, and in particular relates to a double-layer visual servo navigation method applied to visual servoing. Background Art

[0002] China's intelligent manufacturing industry is constantly developing, and unmanned vehicles play a very important role in the intelligent link. For heavy and repetitive tasks, the value of unmanned vehicles can be well exerted, and the widespread application of unmanned vehicle systems can greatly improve the operation and work efficiency of transportation and sorting factories.

[0003] At present, unmanned vehicles mostly use different sensors for navigation and positioning in the application field, such as lidar, IMU, odometer, magnetic sensor, etc. Different types of sensors have different limitations in positioning effects. Common lidar technology, Bluetooth positioning, WIFI positioning and UWB technology usually do not require fixed positioning marks during use, and have a higher degree of freedom on the operation path of the unmanned vehicle, but at the same time the positioning accuracy will be reduced. For magnetic tape navigation, the action path and accuracy of the unmanned vehicle can be greatly guaranteed, but the limitations of the moving path of magnetic tape navigation technology are difficult to meet the increasingly complex transportation conditions (Gherardini M, Mannini A, Cipriani C. Optimal Spatial Sensor Design for Magnetic Tracking in a Myokinetic Control Interface [J]. Computer Methods and Programs in Biomedicine, 2021, 211: 106407.). A single positioning and navigation technology that meets both high flexibility and high accuracy usually incurs high costs. Therefore, in modern factory environments, in order to complete complex and diverse transportation distribution tasks, multiple sensors are usually used to achieve a more accurate and stable positioning method through the joint action of multiple sensors, such as fusing IMU and lidar, fusing camera and IMU, etc. to reduce the cumulative error (Nazemzadeh P, Fontanelli D, Macii D, et al. Indoor localization of mobile robots through QR code detection and dead reckoning data fusion[J]. IEEE / ASME Transactions On Mechatronics, 2017, 22(6): 2588-2599.).Among the existing positioning solutions, the visual servo navigation method has the advantages of low cost and high positioning accuracy. In current application scenarios, most of them use the method of scanning the bottom code with a camera. Common positioning codes used for identification include QR code and AprilTAg code (Nazemzadeh P, Fontanelli D, Macii D, et al. Indoor localization of mobile robots through QR code detection and dead reckoning data fusion [J]. IEEE / ASME Transactions On Mechatronics, 2017, 22 (6): 2588-2599.). At present, most scenarios use the method of scanning the bottom code for positioning. After the unmanned vehicle reaches the top of the feature code, the camera installed at the bottom recognizes the ground positioning code to achieve the positioning function. However, the positioning method of scanning the bottom code can only complete accurate positioning when the unmanned vehicle reaches the bottom of the feature code. When moving on a road section without a code, the vehicle uses inertial navigation, which is unstable. Summary of the invention

[0004] In order to solve the above technical problems, the present invention proposes a double-layer visual servo navigation method applied to visual servo, comprising: an unmanned vehicle with a camera installed on the top, which can obtain the position and posture of the unmanned vehicle by identifying the feature code on the ground before the unmanned vehicle reaches the top of the feature code, and an unmanned vehicle with a feature code installed on the bottom, which obtains the position and posture of the unmanned vehicle by identifying the AprilTag in the middle of the feature code when the unmanned vehicle reaches the top of the feature code, and uses a double-layer servo navigation method based on the feature code to respectively identify the feature code through dual cameras, obtain control deviations before reaching the feature code and when reaching the feature code, respectively, and control the unmanned vehicle to reach the center of the feature code, thereby improving the navigation accuracy and stability of the visual servo unmanned vehicle in the indoor environment.

[0005] The present invention is achieved by at least one of the following technical solutions.

[0006] A double-layer visual servo navigation method for motion control comprises the following steps:

[0007] 1) Establish a kinematic model for an unmanned vehicle with differential wheels;

[0008] 2) A camera is installed on the top of the differential wheeled unmanned vehicle to identify the feature code laid on the ground;

[0009] 3) When the camera installed on the top of the vehicle detects the feature code, the feature code is recognized and the relative position information between the unmanned vehicle and the feature code is calculated to obtain the error parameter;

[0010] 4) Establish a top camera model, convert the coordinates obtained by identifying the feature code in the camera coordinate system to the world coordinate system, and complete the visual positioning of the top of the unmanned vehicle;

[0011] 5) When the camera installed at the bottom of the vehicle detects the feature code, it identifies the AprilTag code information in the middle of the feature code, calculates the deviation between the current position and target position of the unmanned vehicle, and obtains the position information of the unmanned vehicle through the conversion of the coordinate system;

[0012] 6) Constructing an unmanned vehicle control model based on posture deviation through the obtained deviation information and the corresponding unmanned vehicle kinematic model;

[0013] 7) When the top camera and the bottom camera simultaneously identify and obtain corresponding positioning information, the positioning accuracy is further improved through data fusion.

[0014] Furthermore, the differential wheels installed on the unmanned vehicle include two driving wheels and two driven wheels. The driving wheels are controlled by two motors respectively. The differential wheel unmanned vehicle can realize rotation in place and forward and backward movement. In the world coordinate system, the following kinematic model is established:

[0015] The dynamics of the differential wheeled unmanned vehicle is described as follows:

[0016]

[0017] By deriving the kinematic parameter matrix, the corresponding kinematic model is obtained. where is the derivative of the unmanned vehicle’s posture, v is the linear velocity of the unmanned vehicle in the forward direction, ω is the angular velocity of the unmanned vehicle, and θ is the angle between the unmanned vehicle and the x-axis of the world coordinate system.

[0018] Furthermore, the top of the unmanned vehicle is installed in front of the vehicle, and the optical axis is Angle, the camera pointing to the direction of the unmanned vehicle. During operation, the camera on the top of the unmanned vehicle can identify the feature code in front in advance; the feature code is mapped in a grid type, the installation height of the unmanned vehicle camera is H, and the feature code laying spacing is defined as:

[0019]

[0020] Where δ is the visible area parameter of the camera in the vertical direction.

[0021] Furthermore, the top monocular camera is used to identify the feature code and pre-process the acquired feature code image:

[0022] First, the original image is cropped to retain the vertical pixel length and reduce the irrelevant areas on both sides of the original image. The dimension of the image information obtained by the camera is 3·Z·256, where Z is the total number of image pixels. The image information is reduced in dimension and the image is grayed using the weighted average method. The graying scheme is as follows:

[0023] F(x c ,y c )=[R(x c ,y c )W r +G(x c ,y c )W g +B(x c ,y c )W b ]

[0024] Where W r , W g , W b They are the weight control parameters of the corresponding red, green and blue channels, (x c ,y c ) is the coordinate point on the image, R(x c ,y c )、G(x c ,y c )、B(x c ,y c ) are the image coordinate points (x c ,y c ) The values ​​of the red, green, and blue channels. When the running scene changes significantly, the grayscale effect can be changed by adjusting the parameters. The data to be identified by grayscale is Z·256 dimensions.

[0025] Furthermore, the conversion of step 4) is: when the camera installed on the top of the unmanned vehicle converts the positioning in the camera coordinate system obtained by scanning the code into the world coordinate system through the transformation of the coordinate system during the recognition process, so as to obtain the precise positioning of the unmanned vehicle, specifically including the following steps:

[0026] First, for the top camera, the coordinates of a certain point in the camera coordinate system and the corresponding deviation are obtained by identifying the feature code, and the coordinates in the camera coordinate system are converted into coordinates in the world coordinate system according to the following formula:

[0027]

[0028] During the recognition process, x c ,y c Represented as the coordinates of a certain point relative to the camera, z cIt is represented as the distance between a certain point and the camera plane in the direction of the optical axis, R is a 3x3 matrix, t represents the translation relationship of the coordinates, M represents the relationship matrix between the camera and the world coordinate system, and the relationship matrix represents the installation angle of the camera relative to the position of the unmanned vehicle; the feature code captured by the camera is converted into the coordinates in the corresponding world coordinate system, and the conversion process is expressed as:

[0029]

[0030] Among them, z c is the distance of a certain point on the optical axis of the camera, f x 、f y 、u 0 、v 0 These are the parameters obtained through internal calibration of the camera. u and v represent the coordinates of a certain point on the image. Since the camera has a fixed height and the feature code size is fixed, depth information can be obtained through monocular vision.

[0031] Furthermore, the top monocular camera recognizes the feature code image to obtain corresponding coordinate information, and the feature code is recognized after preprocessing. The recognition adopts a line-by-line scanning method. Each line in the feature code image is divided into black and white lines by line-by-line scanning, and then the line length is judged. When the length of the line satisfies black line: white line: black line: white line: black line = 1:1:4:1:1, the line belongs to the first pattern in the new feature code, and when the number of lines is greater than 10, it is determined that the camera recognizes the feature code and starts the positioning process. The line segments and corresponding center points in the second pattern, the fourth pattern, the sixth pattern, and the eighth pattern are obtained in the same way. The difference is that the length relationship of the lines satisfies: black line: white line: black line: white line: black line = 1:1:2:1:1. The line segments are classified according to the corresponding position relationship between the new feature code and the first pattern, and finally the center points of the second pattern, the fourth pattern, the sixth pattern, and the eighth pattern are obtained. The specific process is as follows:

[0032] (1) For the set of line segments that satisfy the corresponding relationship, the midpoint is obtained as the coordinate of the set of line segments in the image coordinate system;

[0033] (2) Calculate the mean of the midpoint coordinates of the line segments that satisfy 1:1:2:1:1. The mean corresponds to the center p of the feature code. c , dividing the line segment set into 4 categories according to the positional relationship between the second pattern, the fourth pattern, the sixth pattern, the eighth pattern and the first pattern;

[0034] (3) Obtain the center points of the four types of line segment sets, and use these four center points as the coordinates p of the second pattern, the fourth pattern, the sixth pattern, and the eighth pattern in the image coordinate system, respectively. 2 、p4 、p 6 、p 8 ;

[0035] (4) According to the conversion in step 4), the relative coordinates of the feature code center and the unmanned vehicle in the world coordinate system are obtained, including the horizontal deviation x in the world coordinate system. e and e and the heading angle θ, which provide parameters for achieving deviation-based control;

[0036] (5) According to p 2 、p 4 、p 6 、p 8 and their corresponding positional relationships, defining the coordinates of the four points after affine transformation, converting the oblique image into a plane image through affine transformation and obtaining the third pattern, the fifth pattern, and the seventh pattern respectively;

[0037] (6) Respectively identify the third pattern, the fifth pattern, and the seventh pattern, wherein the third pattern, the fifth pattern, and the seventh pattern contain position information of the feature code in the identification coordinate system, and combine the deviation value obtained in step (4) to obtain the positioning information of the unmanned vehicle in the world coordinate system.

[0038] Further, step 5) comprises the following steps:

[0039] First, use the findcounters function to identify the boundary of the AprilTag code, calculate the mean of the boundary to obtain the midpoint of the AprilTag, and use this midpoint as the midpoint coordinate (x b ,y b );

[0040] The AprilTag code adopts the 36h11 type, in which the number of valid squares is 36. In the decoding process, black squares are defined as 1 and white squares as 0. The code is identified from the upper left corner to the lower right corner, so that the decoding values ​​obtained in different directions are different. The AprilTag code needs to be decoded multiple times for each identification. After each decoding, the AprilTag code is rotated, and the number of rotations k is recorded to determine the heading angle θ of the unmanned vehicle in the world coordinate system:

[0041]

[0042] where θ i Indicates the inclination angles of the four sides of the circumscribed quadrilateral in AprilTag, and i indicates the i-th side of the circumscribed quadrilateral.

[0043] Furthermore, the posture of the unmanned vehicle in the bottom camera coordinate system is obtained by the bottom camera and converted into the posture of the unmanned vehicle in the world coordinate system.

[0044] Compared with the coordinate transformation relationship of the top camera, bottom scanning is simpler. Since the camera is fixedly installed on the unmanned vehicle, the distance between the bottom camera and the feature code installed on the ground is fixed, that is, z c , which is the height of the camera from the ground. The external parameter matrix of the camera is simplified. The coordinates of the unmanned vehicle in the world coordinate system are expressed as:

[0045]

[0046] where x b ,y b is the coordinate of the center of the feature code in the bottom camera image coordinate system, f x 、f y 、u 0 、v 0 It is the internal parameter of the camera.

[0047] Further, step 6) comprises the following steps:

[0048] The deviation based on the target point is expressed as:

[0049]

[0050] where x d ,y d ,θ d Respectively represent the coordinates of the target point and the corresponding heading angle, x e ,y e ,θ e They represent the two-axis deviation and angle deviation between the unmanned vehicle and the target point respectively, and x, y, and θ are the coordinates of the unmanned vehicle in the current state, that is, the heading angle. By taking the derivative of ue, we can get

[0051]

[0052] in It is expressed as the derivative of the posture deviation of the unmanned vehicle, u = [u 1 ,u 2 ] T , defined as [-v,-ω] T , to realize the control method of differential wheeled unmanned vehicle based on deviation;

[0053] Where θ is the angle between the world coordinate system and the vehicle's orientation, ω represents the angular velocity of the unmanned vehicle, and v represents the linear velocity of the unmanned vehicle. The relationship between the corresponding two-wheel speeds and v and ω is calculated, so

[0054]

[0055]

[0056] Where d is the distance between the two wheels, v l Indicates the speed of one wheel, v r Indicates the speed of the other side wheel.

[0057] Further, step 7) includes: when the bottom camera recognizes the feature code and the top camera recognizes the front feature code, data fusion is performed by Kalman filtering:

[0058] During the movement of the unmanned vehicle, when it runs above a feature code and faces the next feature code, the top camera and the bottom camera detect the feature code at the same time, and can simultaneously identify and obtain the corresponding posture information, and perform data fusion through the extended Kalman filter algorithm.

[0059] Compared with the prior art, the present invention has the following beneficial effects:

[0060] The present invention proposes a double-layer visual servo navigation method for motion control, which has the following advantages compared with the current technology: the present invention is based on the kinematic model of the unmanned vehicle and the visual servo control model, designs a new type of feature code, combines the top camera with the bottom camera recognition, and designs a navigation method with high stability and high positioning accuracy. When there is a feature code below the unmanned vehicle, the bottom camera recognizes the AprilTag in the middle of the feature code to achieve high-precision positioning and control the steering of the unmanned vehicle at the same time. When the unmanned vehicle is traveling on a road section without a feature code below, the feature code in front of the road is recognized by the top camera, and the error caused by inertial navigation is corrected in time to improve the stability of the unmanned vehicle. In addition, when the unmanned vehicle is above the feature code, the bottom camera recognizes the bottom feature code, and the top camera recognizes the front feature code and fuses it through Kalman filtering to achieve high-precision positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 This is a structural diagram of a double-layer visual servo navigation method applied to motion control in this embodiment;

[0062] Figure 2 This is a schematic diagram of the characteristic code of this embodiment;

[0063] Figure 3 This is a schematic diagram of the top identification feature code of the differential wheeled unmanned vehicle in this embodiment;

[0064] Figure 4 This is a schematic diagram of the bottom camera of the differential wheeled unmanned vehicle in this embodiment;

[0065] Figure 5This is a schematic diagram of a storage system in which various types of AprilTag codes are placed in this embodiment. DETAILED DESCRIPTION

[0066] The present invention will be further described in detail below in conjunction with embodiments and drawings, but the embodiments of the present invention are not limited thereto.

[0067] Example 1

[0068] like Figure 1 As shown, a double-layer visual servo navigation method applied to motion control comprises the following steps:

[0069] 1) Establish a kinematic model for an unmanned vehicle with differential wheels;

[0070] 2) A camera is installed on the top of the differential wheeled unmanned vehicle to identify feature codes laid on the ground according to certain rules, such as Figure 3 When the camera installed on the top of the vehicle detects the feature code, the feature code is recognized, and the relative position information between the unmanned vehicle and the feature code is calculated to obtain the error parameter; a top camera model is established, and the coordinates obtained by recognizing the feature code in the camera coordinate system are converted to the world coordinate system to complete the visual positioning of the top of the unmanned vehicle;

[0071] The top monocular camera recognizes the feature code and simplifies the positioning and recognition process through image processing. Usually, the image obtained by the camera contains a lot of information that is irrelevant to the recognition feature code, so the obtained image needs to be preprocessed. Figure 2 , 3 As shown in the figure, when the unmanned vehicle runs to the position where the top camera can capture the feature code, the captured image is processed and converted to finally obtain the position and posture of the unmanned vehicle. The specific process is as follows:

[0072] Considering the heading angle deviation of the unmanned vehicle during actual operation, the feature code can always be kept in the field of view. First, the original image is cropped to retain the vertical pixel length and reduce the irrelevant areas on both sides. The dimension of the image information obtained by the camera is 3 x c ·y c 256. For embedded image processing, in order to improve the real-time processing, the image information is further reduced in dimension and the image is grayed using the weighted average method. The graying scheme is as follows:

[0073] F(x c ,y c )=[R(x c ,y c )W r +G(x c ,y c )W g +B(xc ,y c )W b ]

[0074] Where W r , W g , W b They are the weight control parameters of the corresponding red, green and blue channels, (x c ,y c ) The coordinate point on the image is R(x c ,y c )、G(x c ,y c )、B(x c ,y c ) are the image coordinate points (x c ,y c ) The values ​​of the red, green, and blue channels. When the running scene changes significantly, the grayscale effect can be changed by adjusting the parameters. The grayscale data to be identified is x c ·y c 256 dimensions.

[0075] In order to further simplify the feature code recognition process and take into account the uneven light in the environment, the local threshold method is used to binarize the image. The implementation scheme is through the adaptiveThreshold function in the opencv library. The function parameters mainly depend on the camera parameters and light intensity.

[0076] After the binary processing, the feature code is identified. The identification adopts the line-by-line scanning method, and each line in the image is divided into black and white lines through line-by-line scanning. Then the line length is judged. When the line length meets:

[0077] Black line: white line: black line: white line: black line = 1:1:4:1:1

[0078] like Figure 2 As shown, the line belongs to the first pattern 1 of the feature code, and when the number of the lines is greater than 10, it is determined that the camera recognizes the feature code and starts the positioning process. The line segments and corresponding center points in the second pattern 2, the fourth pattern 4, the sixth pattern 6, and the eighth pattern 8 are obtained in the same way, except that the length relationship of the lines is different, satisfying:

[0079] Black line: white line: black line: white line: black line = 1:1:2:1:1

[0080] The center points of the four types of patterns can be obtained through the corresponding position correspondence. The specific process is as follows.

[0081] (1) For the set of line segments that satisfy the corresponding relationship, the midpoint is obtained as the coordinate of the set of line segments in the image coordinate system;

[0082] (2) Calculate the mean of the midpoint coordinates of the line segments that satisfy 1:1:2:1:1. The mean corresponds to the center p of the feature code. c , the line segment set is divided into 4 categories according to the positional relationship between the 4 patterns and the center of the feature code;

[0083] (3) Find the center points of the four types of line segment sets, and use these four center points as the coordinates p of the second pattern 2, the fourth pattern 4, the sixth pattern 6, and the eighth pattern 8 in the image coordinate system, respectively. 2 、p 4 、p 6 、p 8 ;

[0084] (4) Since the distances between the second positioning pattern 2, the fourth positioning pattern 4, the sixth positioning pattern 6, and the eighth positioning pattern 8 are fixed, the relative coordinates of the center of the feature code and the unmanned vehicle are obtained through the conversion process of the camera model, including the horizontal deviation x in the world coordinate system. e and e and heading angle θ;

[0085] (5) Given p 2 、p 4 、p 6 、p 8 And the corresponding positional relationship thereof, the coordinates of the four points after affine transformation can be defined, the oblique image is converted into a plane image through affine transformation and the third pattern 3, the fifth pattern 5, and the seventh pattern 7 are obtained respectively;

[0086] (6) Respectively identify the third pattern 3, the fifth pattern 5, and the seventh pattern 7, wherein the pattern contains the position information of the feature code in the identification coordinate system, and combine it with the deviation value obtained in step (4) to obtain the positioning information of the unmanned vehicle in the world coordinate system.

[0087] When the camera installed on the top of the unmanned vehicle is in the recognition process, the positioning coordinates in the camera coordinate system obtained by scanning the feature code are converted to the world coordinate system through the transformation of the coordinate system to obtain the precise positioning of the unmanned vehicle, including the following steps:

[0088] First, for the top camera, the coordinates of the target point in the camera coordinate system and the corresponding deviation can be obtained by identifying the feature code. Therefore, the coordinates in the camera coordinate system can be converted into coordinates in the world coordinate system according to the following formula:

[0089]

[0090] In the recognition process, R is a 3x3 matrix, t represents the translation relationship of the coordinates, and the M matrix represents the relationship between the camera and the world coordinate system. This matrix represents the installation angle of the camera and its position relative to the unmanned vehicle. In order to achieve accurate navigation, the feature code representation captured by the camera needs to be converted into the coordinates in the corresponding world coordinate system. The conversion process can be expressed as:

[0091]

[0092] Among them, z c is the distance of the target point on the optical axis of the camera, f x 、f y 、u 0 , are the internal parameters of the camera, which can be obtained through camera calibration. Since the camera has a fixed height and the feature code size is fixed, it is possible to obtain depth information through monocular vision.

[0093] 3) When the camera installed at the bottom of the vehicle detects the feature code, it identifies the AprilTag code information in the middle of the feature code, calculates the deviation between the current position of the unmanned vehicle and the target position, and obtains the position information of the unmanned vehicle through the conversion of the coordinate system; scans the feature code on the ground with the camera, obtains the center of the feature code by a fast recognition method, and controls the deviation correction of the unmanned vehicle to make it reach the center point of the feature code:

[0094] The unmanned vehicle obtains control information processed by the top camera to ensure that the unmanned vehicle moves above the feature code, providing protection for the AprilTag recognition process. Figure 4 As shown in the figure, the bottom camera is installed at the bottom of the vehicle and is perpendicular to the ground and downward. When the unmanned vehicle moves above the feature code, the boundary of the AprilTag code is first identified by the findcounters function. The midpoint of the AprilTag can be obtained by calculating the mean of the boundary, and this midpoint is used as the midpoint coordinate (x b ,y b ).

[0095] The AprilTag code adopts the 36h11 type, in which the number of valid squares is 36. In the decoding process, black squares are defined as 1 and white squares as 0. The recognition starts from the upper left corner and ends at the lower right corner. According to this method, the decoding values ​​obtained in different four directions are different. The AprilTag code needs to be decoded multiple times each time it is recognized. After each decoding, the AprilTag code is rotated, and the number of rotations k is recorded to determine the heading angle θ of the unmanned vehicle in the world coordinate system.

[0096]

[0097] where θ iIndicates the inclination angles of the four sides of the circumscribed quadrilateral in AprilTag, and i indicates the i-th side of the circumscribed quadrilateral.

[0098] Compared with the coordinate transformation relationship of the top camera, bottom scanning is simpler. Since the camera is fixedly installed on the unmanned vehicle, the distance between the bottom camera and the feature code installed on the ground is fixed, that is, z c is the height of the camera from the ground, and the external parameter matrix of the camera is simplified. The coordinates of the unmanned vehicle in the world coordinate system can be expressed as:

[0099]

[0100] where x b ,y b is the coordinate of the center of the feature code in the bottom camera image coordinate system, f x 、f y 、u 0 、v 0 It is the internal parameter of the camera.

[0101] 4) Constructing an unmanned vehicle control model based on posture deviation through the obtained deviation information and the corresponding unmanned vehicle kinematic model;

[0102] The unmanned vehicle is equipped with differential wheels, which consist of two active wheels and two driven wheels. The active wheels are controlled by two motors respectively. The differential wheel unmanned vehicle can rotate in place and move forward and backward. In the world coordinate system, the dynamics of the differential wheel unmanned vehicle are described as follows:

[0103]

[0104] By deriving the kinematic parameter matrix, the corresponding kinematic model is obtained, where is the derivative of the unmanned vehicle’s posture, v is the linear velocity of the unmanned vehicle in the forward direction, ω is the angular velocity of the unmanned vehicle, and θ is the angle between the unmanned vehicle and the θ-axis of the world coordinate system.

[0105] The deviation based on the target point can be expressed as:

[0106]

[0107] where x d ,y d ,θ d Respectively represent the coordinates of the target point and the corresponding heading angle, x e , y, and e represent the two-axis deviation and angle deviation between the unmanned vehicle and the target point, respectively. x, y, and θ are the coordinates of the unmanned vehicle in the current state, i.e., the heading angle. e Derivation

[0108]

[0109] in It is expressed as the derivative of the posture deviation of the unmanned vehicle, u = [u 1 ,u 2 ] T , defined as [-v,-ω] T , to realize the control method of differential wheeled unmanned vehicle based on deviation;

[0110] Where θ is the angle between the world coordinate system and the vehicle's orientation, ω is the angular velocity of the unmanned vehicle, and v is the linear velocity of the unmanned vehicle. Further calculations are made on the relationship between the corresponding two-wheel speeds and v and ω, so

[0111]

[0112]

[0113] Where d is the distance between the two wheels, v l represents the speed of the left wheel, v r Indicates the speed of the right wheel.

[0114] 5) When the top camera and the bottom camera simultaneously identify and obtain corresponding positioning information, the positioning accuracy is further improved through data fusion.

[0115] When the bottom camera recognizes the feature code and the top camera recognizes the front feature code, the Kalman filter method is used to perform data fusion:

[0116] During the movement of the unmanned vehicle, when it moves to the position above the feature code and facing the next feature code, the top camera and the bottom camera detect the feature code at the same time, and can simultaneously identify and obtain the corresponding posture information, and can perform data fusion through the extended Kalman filter algorithm. Therefore, the present invention adopts a combination of top code scanning and bottom code scanning to achieve the positioning of the unmanned vehicle, reduces the cost of the sensor by using the camera, performs high-precision positioning by scanning the bottom camera, and improves the stability of the unmanned vehicle by scanning the top camera.

[0117] Example 2

[0118] In the present invention, the AprilTag code in the middle of the feature code can also be replaced. In industrial scenarios, there are situations where a large number of unique feature codes need to be laid or the feature code to be identified contains more information. In this case, the AprilTag code in the middle of the feature code, i.e. Figure 4Area 9 in the image is replaced with a QR code for identification. There are many types of QR codes, which can be divided by version and error correction level. Different types of QR codes contain different data types and data capacities. Large capacity is often accompanied by reduced recognition efficiency, but its diverse types provide a feasible solution. When using QR codes for positioning, its positioning accuracy is slightly insufficient compared to the positioning effect of the present invention. At the same time, identification during movement has higher requirements on the camera's motion shooting capability.

[0119] There are three positioning marks in the QR code, which can be used to obtain the position deviation and heading angle of the unmanned vehicle. Similar to the AprilTag code, the moving target point and the corresponding deviation are determined by identifying the positioning mark, and the unmanned vehicle is controlled to move accurately above the feature code through the deviation information. At the same time, the accurate positioning effect is achieved through the conversion of the coordinate system. The rest of the QR code is used to contain the corresponding internal information, which can usually include the unique identifier of the feature code, the action that the unmanned vehicle needs to generate when moving to the feature code, the control information for different types of vehicles, classification information, etc.

[0120] The top of the unmanned vehicle is installed in front of the vehicle, and the optical axis is aligned with the ground. Angle, the camera pointing in the direction of the unmanned vehicle's advance, the camera on the top of the unmanned vehicle can identify the feature code in front in advance during operation. The feature code is mapped using a grid type, and the installation height of the unmanned vehicle camera is H. In order to ensure that the feature code in front can be identified in advance during driving, the corresponding feature code laying spacing is defined as:

[0121]

[0122] Where δ is the visible area parameter of the camera in the vertical direction.

[0123] Example 3

[0124] The unmanned vehicle precise positioning system of the present invention can be applied to the autonomous charging system of the unmanned vehicle. In the automatic charging process of the unmanned vehicle, poor contact and inaccurate positioning usually cause charging difficulties. Usually, in order to improve the stability and fault tolerance of charging, the common solution adopts low-voltage charging and charging by connecting an external large charging interface. Low-voltage charging ensures safety during the charging process, and the expansion of the charging interface can improve the fault tolerance and stability, but this method also has the problem of low charging efficiency.

[0125] The present invention can be applied to lay a characteristic code in front of the charging pile, and the unmanned vehicle can move above the code when the battery is low. When the unmanned vehicle is above the characteristic code, it can accurately adjust its posture, complete specific movements, and achieve the precise docking task of the charging interface.

[0126] Example 4

[0127] The present invention can also be applied to a warehouse logistics system where multiple unmanned vehicles collaborate, such as Figure 5 The figure shows the 4x4 feature code map laid in the warehouse logistics system. With the grid map, the unmanned vehicle only has straight-line driving, right-angle turns and backward steering during its movement. Due to the single operation mode, it can ensure that every time the vehicle reaches a feature code during operation, the top camera can recognize the previous feature code in advance, and improve the positioning accuracy through multi-sensor fusion.

[0128] When the unmanned vehicle moves from the feature code (0,0,0) node to the feature code (0,2,2) node, the unmanned vehicle first determines the current coordinates and heading angle by identifying the bottom feature code, determines its own position, and moves to the node (0,0,1). During the movement, the top camera identifies the feature code of the (0,0,1) node and controls the operation of the unmanned vehicle through visual servoing to ensure that the unmanned vehicle does not deviate from the normal operating trajectory, greatly improving the stability of the unmanned vehicle during operation. When the unmanned vehicle runs to the next node and the next step is to drive in a straight line, the node control scheme is adopted to set the node in front to an occupied state, which is used to predict the node usage of the grid map. When multiple vehicles need to enter the node at the same time, avoidance or path replanning can be performed in advance.

[0129] The above embodiments are only used for detailed explanation to help understand the technical solutions of the present invention. For those skilled in the art, any improvements and substitutions made without departing from the principles of the present invention shall fall within the protection scope of the present invention.

Claims

1. A dual-layer visual servo navigation method for motion control, characterized in that: The following steps are involved: 1) Establish a kinematic model for an unmanned vehicle with differential wheels; 2) A camera is installed on the top of the differential wheeled unmanned vehicle to identify the feature code laid on the ground; 3) When the camera installed on the top of the vehicle detects the feature code, the feature code is recognized and the relative position information between the unmanned vehicle and the feature code is calculated to obtain the error parameter; 4) Establish a top camera model, convert the coordinates obtained by identifying the feature code in the camera coordinate system to the world coordinate system, and complete the visual positioning of the top of the unmanned vehicle; obtain the corresponding coordinate information by identifying the feature code image through the top monocular camera, and identify the feature code after preprocessing. The recognition adopts a line-by-line scanning method. Each line in the feature code image is divided into black and white lines through line-by-line scanning, and then the line length is judged. When the length of the line satisfies black line: white line: black line: white line: black line = 1:1:4:1:1, the line belongs to the first pattern in the new feature code, and when the number of lines is greater than 10, it is determined that the camera recognizes the feature code and starts the positioning process. The line segments and corresponding center points in the second pattern, the fourth pattern, the sixth pattern, and the eighth pattern are obtained in the same way. The difference is that the length relationship of the lines satisfies black line: white line: black line: white line: black line = 1:1:2:1:

1. The line segments are classified according to the corresponding position relationship between the new feature code and the first pattern, and finally the center points of the second pattern, the fourth pattern, the sixth pattern, and the eighth pattern are obtained. The specific process is as follows: (1) For the set of line segments that satisfy the corresponding relationship, the midpoint is obtained as the coordinate of the set of line segments in the image coordinate system; (2) Calculate the mean of the midpoint coordinates of the line segments that satisfy black line: white line: black line: white line: black line = 1:1:2:1:

1. The mean corresponds to the center p of the feature code. c , dividing the line segment set into 4 categories according to the positional relationship between the second pattern, the fourth pattern, the sixth pattern, the eighth pattern and the first pattern; (3) finding the center points of the four types of line segment sets, and using the four center points as the coordinates p2, p4, p6, and p8 of the second pattern, the fourth pattern, the sixth pattern, and the eighth pattern in the image coordinate system, respectively; (4) According to the coordinate transformation in step 4), the relative coordinates of the feature code center and the unmanned vehicle in the world coordinate system are obtained, including the horizontal deviation x in the world coordinate system. e and e and the heading angle θ, which provide parameters for achieving deviation-based control; (5) According to p2, p4, p6, p8 and their corresponding positional relationships, the coordinates of the four points after affine transformation are defined, the oblique image is converted into a plane image through affine transformation, and the third pattern, the fifth pattern, and the seventh pattern are obtained respectively; (6) respectively identifying the third pattern, the fifth pattern, and the seventh pattern, wherein the third pattern, the fifth pattern, and the seventh pattern contain position information of the feature code in the identification coordinate system, and combining the deviation value obtained in step (4) to obtain the positioning information of the unmanned vehicle in the world coordinate system; 5) When the camera installed at the bottom of the vehicle detects the feature code, it identifies the AprilTag code information in the middle of the feature code, calculates the deviation between the current position of the unmanned vehicle and the target position, and obtains the position information of the unmanned vehicle through the conversion of the coordinate system; 6) Constructing an unmanned vehicle control model based on posture deviation through the obtained deviation information and the corresponding unmanned vehicle kinematic model; 7) When the top camera and the bottom camera simultaneously identify and obtain corresponding positioning information, the positioning accuracy is further improved through data fusion.

2. A dual-layer visual servo navigation method for motion control according to claim 1, characterized in that: The differential wheels installed on the unmanned vehicle include two active wheels and two driven wheels. The active wheels are controlled by two motors respectively. The differential wheel unmanned vehicle can rotate in place and move forward and backward. In the world coordinate system, the following kinematic model is established: The dynamics of the differential wheeled unmanned vehicle is described as follows: By deriving the kinematic parameter matrix, the corresponding kinematic model is obtained. where is the derivative of the unmanned vehicle’s posture, v is the linear velocity of the unmanned vehicle in the forward direction, ω is the angular velocity of the unmanned vehicle, and θ is the angle between the unmanned vehicle and the x-axis of the world coordinate system.

3. A dual-layer visual servo navigation method for motion control according to claim 1, characterized in that: The top of the unmanned vehicle is installed in front of the vehicle, and the optical axis is aligned with the ground. Angle, the camera points to the direction of the unmanned vehicle. During operation, the camera on the top of the unmanned vehicle can identify the feature code in front in advance; the feature code is mapped in a grid type, the installation height of the unmanned vehicle camera is H, and the feature code laying spacing is defined as: Where δ is the visible area parameter of the camera in the vertical direction.

4. A dual-layer visual servo navigation method for motion control according to claim 1, characterized in that: Use the top monocular camera to identify the feature code and pre-process the acquired feature code image: First, the original image is cropped to retain the vertical pixel length and reduce the irrelevant areas on both sides of the original image. The dimension of the image information obtained by the camera is 3·Z·256, where Z is the total number of image pixels. The image information is reduced in dimension and the image is grayed using the weighted average method. The graying scheme is as follows: F(x,y)=[R(x,y)W r +G(x,y)W g +B(x,y)W b ] Where W r , W g , W b are the weight control parameters of the corresponding red, green and blue channels respectively. R(x, y), G(x, y) and B(x, y) are the values ​​of the red, green and blue channels of the image coordinate point (x, y) respectively. When the running scene changes significantly, the grayscale effect can be changed by adjusting the parameters. The data to be identified by grayscale is Z·256-dimensional.

5. A double-layer visual servo navigation method for motion control according to claim 1, characterized in that: The conversion of step 4) is as follows: when the camera installed on the top of the unmanned vehicle converts the position obtained by scanning the code in the camera coordinate system to the world coordinate system through the transformation of the coordinate system during the recognition process, so as to obtain the precise positioning of the unmanned vehicle, specifically including the following steps: First, for the top camera, the coordinates of a certain point in the camera coordinate system and the corresponding deviation are obtained by identifying the feature code, and the coordinates in the camera coordinate system are converted into coordinates in the world coordinate system according to the following formula: During the recognition process, x c ,y c Represented as the coordinates of a certain point relative to the camera, z c It is represented as the distance between a certain point and the camera plane in the direction of the optical axis, R is a 3x3 matrix, t represents the translation relationship of the coordinates, M represents the relationship matrix between the camera and the world coordinate system, and the relationship matrix represents the installation angle of the camera relative to the position of the unmanned vehicle; the feature code captured by the camera is converted into the coordinates in the corresponding world coordinate system, and the conversion process is expressed as: Among them, z c is the distance of a certain point on the optical axis of the camera, f x 、f y , u0, v0 are parameters obtained through internal calibration of the camera, u and v represent the coordinates of a certain point on the image. Since the camera has a fixed height and the feature code size is fixed, it is possible to obtain depth information through monocular vision.

6. A double-layer visual servo navigation method for motion control according to claim 1, It is characterized in that Step 5) comprises the following steps: First, use the findcounters function to identify the boundary of the AprilTag code, calculate the mean of the boundary to obtain the midpoint of the AprilTag, and use this midpoint as the midpoint coordinate of the feature code in the bottom camera coordinate system (x b ,y b ); The AprilTag code adopts the 36h11 type, in which the number of valid squares is 36. In the decoding process, black squares are defined as 1 and white squares as 0. The code is identified from the upper left corner to the lower right corner, so that the decoding values ​​obtained in different directions are different. The AprilTag code needs to be decoded multiple times for each identification. After each decoding, the AprilTag code is rotated, and the number of rotations k is recorded to determine the heading angle θ of the unmanned vehicle in the world coordinate system: where θ i Indicates the inclination angles of the four sides of the circumscribed quadrilateral in AprilTag, and i indicates the i-th side of the circumscribed quadrilateral.

7. A double-layer visual servo navigation method for motion control according to claim 5, characterized in that: The position of the unmanned vehicle in the bottom camera coordinate system is obtained by the bottom camera and converted into the position of the unmanned vehicle in the world coordinate system; Compared with the coordinate transformation relationship of the top camera, bottom scanning is simpler. Since the camera is fixedly installed on the unmanned vehicle, the distance between the bottom camera and the feature code installed on the ground is fixed, that is, z c , which is the height of the camera from the ground. The external parameter matrix of the camera is simplified. The coordinates of the unmanned vehicle in the world coordinate system are expressed as: where x b ,y b is the coordinate of the center of the feature code in the bottom camera image coordinate system, f x 、f y , u0, and v0 are parameters obtained through internal calibration of the camera.

8. A double-layer visual servo navigation method for motion control according to any one of claims 1 to 7, characterized in that: Step 7) includes: when the bottom camera recognizes the feature code and the top camera recognizes the front feature code, data fusion is performed by Kalman filtering: During the movement of the unmanned vehicle, when it runs above a feature code and faces the next feature code, the top camera and the bottom camera detect the feature code at the same time, and can simultaneously identify and obtain the corresponding posture information, and perform data fusion through the extended Kalman filter algorithm.

Citation Information

Patent Citations

  • Multiple sensor-integrated medium-speed unmanned vehicle detection and obstacle avoidance system

    CN108037756A

  • Plant protection unmanned vehicle and control method with integrated visual navigation and Beidou positioning

    CN108669046A