A visual servoing method based on image error weighted vector

Abstract

The present invention discloses a visual servoing method based on image error weighted vector. It includes: obtaining the target image captured when the camera in the visual servoing task is in the initial and preset postures, and then using the feature extraction algorithm to extract the image feature points respectively, and then obtaining the initial and expected all feature point sets respectively; then using the M-estimation sampling consistency algorithm to screen the initial all feature point set, and screen to obtain the optimal qualified point set; then assigning different weights to each feature point, and then constructing the initial and preset control point sets, and then calculating the image error weighted vector, and performing visual servoing on the target image based on the image error weighted vector. The present invention not only reduces the amount of calculation when screening a large number of image feature points, but also reduces the amount of calculation of the subsequent visual servoing link by selecting a small number of high-quality feature points, and at the same time allows the control points lost during the visual servoing task to be replaced by other high-weight feature points, thereby ensuring the quality of the control points.

Description

Technical Field

[0001] The invention belongs to a visual servo method in the field of robot visual servo, and in particular relates to a visual servo method based on an image error weighted vector. Background Art

[0002] With the increasing complexity of robot operation tasks and the continuous development of machine vision technology, robot control methods that rely on machine vision have been continuously proposed and improved, gradually forming the visual servo technology in the field of robot control, that is, using visual information to realize the autonomous movement and operation of robots. Among the current visual servoing methods, there is a type of visual servoing that only relies on planar image data, called image-based visual servoing. In the image-based visual servoing task, it is necessary to extract feature points from the two-dimensional image space and select a suitable number of feature points as control points, and then calculate the image error weighted vector to complete the visual servoing task in the three-dimensional space. In the image-based visual servoing task, if the quality of a set of control points is poor, the control effect of the visual servoing will be extremely unstable, and even cause the visual servoing task to fail. Therefore, it is very necessary to select a suitable set of feature points as control points before the start of the visual servoing task and define a suitable image error weighted vector calculation method.

[0003] Current hardware and algorithms already support real-time extraction of image feature points, but selecting appropriate feature points from thousands of feature points using brute force traversal methods will result in huge time and computing power overhead, and the random selection of control points method has no reasonable feature point screening mechanism, so it is difficult to guarantee the quality of the randomly obtained control points. It often requires repeated attempts or increasing the number of control points to obtain a set of control points that are barely usable. In addition, during the visual servoing process, if the previously selected control points are mismatched, tracking fails, or exceed the image boundary, the image-based visual servoing algorithm will fail, which will inevitably lead to the failure of the visual servoing task. At this time, the appropriate control points must be reselected according to certain rules, but the currently commonly used method of randomly reselecting control points also has the problem of difficulty in guaranteeing the quality of feature points. Summary of the invention

[0004] In order to solve the problems and needs in the background technology, the present invention discloses a visual servoing method based on image error weighted vector. Aiming at the application process of image-based visual servoing, the present invention proposes a specific expression of the loss function value required for M estimation, so as to apply the M estimation sampling consistency algorithm to the screening of image feature points; further, based on the loss function value, the feature points with higher quality in the visual servoing task are screened out and weighted; finally, several feature points with the highest weight are selected as control points according to the weight, and the weight value is used for the weighted operation when the control points form the image error weighted vector, so as to realize the generation of the image error weighted vector. The present invention not only reduces the amount of calculation when screening feature points, but also reduces the amount of calculation in the subsequent links of visual servoing by selecting a small number of high-weight feature points as control points, and at the same time allows the control points lost during the visual servoing task to be replaced by other high-weight feature points, while taking into account the overall efficiency of the visual servoing method and the robustness of the visual servoing system, the quality of the control points is guaranteed.

[0005] In order to achieve the above object, the specific technical scheme of the present invention is as follows:

[0006] The present invention comprises the following steps:

[0007] Step 1: Obtain the target images taken when the camera in the visual servoing task is in the initial posture and the preset posture respectively, and then use the feature extraction algorithm to extract the feature points of the target images when the camera is in the initial posture and the preset posture respectively, and obtain the initial and expected feature point sets respectively;

[0008] Step 2: Based on the expected set of all feature points, the M-estimation sampling consensus algorithm is used to screen the initial set of all feature points to obtain the optimal qualified point set;

[0009] Step 3: Assign different weights to each feature point in the optimal qualified point set, and then select several feature points from the optimal qualified point set in descending order as initial control points to form the initial control point set s when the camera is in the initial position. control , according to the initial control point set, select the corresponding preset control point set from the expected set of all feature points Then, the image error weighted vector is calculated according to the initial control point set, the weights corresponding to each initial control point, and the preset control point set, and the target image is visually served based on the image error weighted vector; if any control point tracking fails during the visual servoing task, the remaining feature point with the highest weight in the optimal qualified point set is selected to replace the lost control point and update the image error weighted vector.

[0010] The step 2 is specifically as follows:

[0011] 2.1) From the initial set of all feature points sall Randomly extract some feature points from the Inlier , the initial set of all feature points s all The set of remaining feature points in is taken as the non-interior feature point set s Other , calculate the current interior point set s Inlier The corresponding image Jacobian matrix And the estimated value of camera pose change and as an interior point model;

[0012] 2.2) Calculate the current non-inlier feature point set s based on the expected set of all feature points Other The model error of each non-interior feature point in the current inlier model; then calculate the cost function value of each non-interior feature point according to the model error, and set the cost function value less than the outlier judgment threshold Thr Outlier All non-interior feature points are added to the qualified point set s good , if the current qualified point set s good The number of non-inlier feature points in count good Greater than the minimum qualified point threshold count min , then the current qualified point set s good As a set of candidate qualified points;

[0013] 2.3) Repeat steps 2.1)-2.2) to obtain the initial set of all feature points s all In the above, some feature points are randomly selected multiple times and multiple candidate qualified point sets are calculated, and then the sum of the model errors of the non-interior feature points of each candidate qualified point set is calculated. uOthers , sum the model errors of non-inlier feature points uOthers The smallest set of candidate qualified points is taken as the optimal set of qualified points s best .

[0014] In step 2.2), for the non-interior feature point set s Other The cost function value u of the i-th non-interior feature point in Others The calculation formula for [i] is:

[0015] u Others [i] = min{e Others [i] 2 ,Thr Outlier 2}

[0016]

[0017] Among them, e Other [i] represents the model error of the i-th non-inlier feature point, Indicates the feature point corresponding to the i-th non-interior feature point in the expected set of all feature points;

[0018] The weight of each feature point in the optimal qualified point set in step 3 is calculated by the cost function value of each feature point in the optimal qualified point set. For the i-th feature point in the optimal qualified point set, the calculation formula of its weight is:

[0019]

[0020] Among them, w best [j] represents the weight of the jth feature point, u best [j] represents the optimal qualified point set s best The cost function value of the j-th feature point in , E represents the noise error allowed for defining the visual signal.

[0021] The calculation formula of the image error weighted vector p is as follows:

[0022]

[0023] Where D is a diagonal matrix composed of normalized control point weights.

[0024] The number of initial control points in step three is greater than or equal to 3.

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

[0026] The present invention defines a calculation method for model error and feature point weight based on the characteristics of image-based visual servoing, and quantifies a screening criterion for feature points.

[0027] Compared with the method of brute-force traversal of all feature points, the method proposed in the present invention greatly reduces the time cost, and due to the existence of effective screening and weighting mechanism, the number of control points finally selected can be very small, making the subsequent visual servoing link faster and improving the overall efficiency of the visual servoing task.

[0028] The method proposed by the present invention selects excellent feature points as control points according to the model error, reduces the pixel error of the control point, and further reduces the control error of the visual servoing.

[0029] When the control point selected by the present invention is lost, the feature point with the highest weight in the remaining optimal qualified point set can be selected as a replacement, thereby ensuring the robustness when the control point is mismatched, tracked failed or exceeds the image boundary during the visual servoing task. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 It is a schematic diagram of the present invention applied in a visual servo system.

[0031] Figure 2 It is a flow chart of each step of the present invention.

[0032] Figure 3 This is a control error comparison diagram of the present invention applied in a robot visual servo experiment;

[0033] Figure 4 It is the simulation experiment result of the present invention when the feature points are lost. DETAILED DESCRIPTION

[0034] The embodiments of the present invention are described in detail below. The embodiments described below with reference to the accompanying drawings are exemplary and intended to be used to explain the present invention, but should not be construed as limiting the present invention.

[0035] like Figure 1 and 2 As shown, the present invention comprises the following steps:

[0036] Step 1: Obtain the target images taken when the camera in the visual servo task is in the initial posture and the preset posture respectively, and then use the feature extraction algorithm to extract the feature points of the target images when the camera is in the initial posture and the preset posture respectively, and obtain the initial and expected feature point sets respectively; the feature points in the initial and expected feature point sets correspond to each other one by one; feature extraction algorithms such as SIFT, SURF, ORB and other algorithms.

[0037] Step 2: Based on the expected set of all feature points, the initial set of all feature points is screened using the M-estimation sampling consensus algorithm to obtain the optimal qualified point set and the cost function value of each feature point in the optimal qualified point set;

[0038] Step 2 is as follows:

[0039] 2.1) From the initial set of all feature points s all Randomly extract some feature points from the Inlier , the initial set of all feature points s all The set of remaining feature points in is taken as the non-interior feature point set s Other , calculate the current interior point set s Inlier The corresponding image Jacobian matrix And the estimated value of camera pose change and as an interior point model;

[0040] Image Jacobian Matrix And the estimated value of camera pose change The formula is:

[0041]

[0042]

[0043] Among them, the number of internal points is n, then J sIni (i=1,2,…,n) is the 2-row and 6-column Jacobian matrix of the i-th feature point in the interior point set, Z i (i=1,2,…,n) is the distance between the i-th feature point in the internal point set and the camera along the optical axis, x i (i=1,2,…,n) and y i (i=1,2,…,n) is the horizontal and vertical coordinates of the i-th feature point in the internal point set at the pixel position of the initial pose target image, and It is the horizontal and vertical coordinates of the pixel position of the i-th feature point in the internal point set at the preset pose target image.

[0044] 2.2) Calculate the current non-inlier feature point set s based on the expected set of all feature points Other The model error of each non-interior feature point in the current inlier model; then calculate the cost function value of each non-interior feature point according to the model error, and set the cost function value less than the outlier judgment threshold Thr Outlier All non-interior feature points are added to the qualified point set s good , if the current qualified point set s good The number of non-inlier feature points in count good Greater than the minimum qualified point threshold count min , then the current qualified point set s good As a set of candidate qualified points;

[0045] In step 2.2), for the non-interior feature point set s Other The cost function value u of the i-th non-interior feature point in Others The calculation formula for [i] is:

[0046] u Others [i] = min{e Others [i] 2 ,Thr Outlier 2}

[0047]

[0048] Among them, e Other [i] represents the model error of the i-th non-inlier feature point, The feature point corresponding to the i-th non-interior feature point in the expected set of all feature points represents the expected feature point corresponding to the i-th non-interior feature point; is the image error of the i-th non-inlier feature point.

[0049] 2.3) Repeat steps 2.1)-2.2) to obtain the initial set of all feature points s all In the above, some feature points are randomly selected multiple times and multiple candidate qualified point sets are calculated, and then the sum of the model errors of the non-interior feature points of each candidate qualified point set is calculated. uOthers , sum the model errors of non-inlier feature points uOthers The smallest set of candidate qualified points is taken as the optimal set of qualified points s best .

[0050] Step 3: Assign different weights to each feature point in the optimal qualified point set, and then select several feature points from the optimal qualified point set in descending order as initial control points to form the initial control point set s when the camera is in the initial position. control In the specific implementation, the number of initial control points is greater than or equal to 3. all From the expected set of all feature points Select the corresponding preset control point set Specifically, according to the initial control point set s all From the expected set of all feature points Select the corresponding feature points as preset control points and form a preset control point set for the camera in the preset position The initial control point set corresponds one-to-one to the control points in the preset control point set, and then the image error weighted vector is calculated according to the initial control point set and the weights corresponding to each initial control point and the preset control point set, and the target image is visually served based on the image error weighted vector; if any control point tracking fails during the visual servoing task, the remaining feature point with the highest weight in the optimal qualified point set is selected to replace the lost control point and the image error weighted vector is updated.

[0051] The weight of each feature point in the optimal qualified point set in step 3 is calculated by the cost function value of each feature point in the optimal qualified point set. For the jth feature point in the optimal qualified point set, the calculation formula of its weight is:

[0052]

[0053] Among them, w best [j] represents the weight of the jth feature point, u best [j] represents the optimal qualified point set s best The cost function value of the j-th feature point in , E represents the noise error allowed for defining the visual signal, which is an adjustable quantity. The larger E is, the more average the weights of different feature points are, and the smaller E is, the greater the difference in weights of different feature points is. That is, when the allowed noise of the visual signal is large, the quality of different feature points (that is, the change in the loss function) has little effect on the weight.

[0054] The calculation formula of the above image error weighted vector p is as follows:

[0055]

[0056] Where D is the diagonal matrix composed of normalized control point weights, satisfying Assume the number of control points is n, and the kth initial control point is s control [k] = {x control [k],y control [k]}, x control [k] represents the kth initial control point s control [k] is the pixel position horizontal coordinate, y control [k] represents the kth initial control point s control [k] is the pixel position horizontal coordinate, and the preset control point is Then the image error weighted vector of the kth initial control point is

[0057] At this point, the image-based visual servoing method can be used to generate motion control signals for the robot based on the image error weighted vector. A typical algorithm is:

[0058]

[0059] Among them, v is the moving speed vector required by the robot end, which can be used as the robot motion control signal, and λ is the step coefficient of the visual servo algorithm, which is an adjustable quantity. Increasing λ can make the robot end move faster under visual servo control, but in order to ensure stability, it cannot be too large.

[0060] like Figure 4 As shown in (a), after applying the method proposed in the present invention to the robot visual servo, the experimental results can be obtained as follows Figure 3 As shown, compared with the traditional image-based visual servoing method, the proposed method can reduce the control error of the visual servoing.

[0061] Selecting control points according to weights can help replace lost control points when mismatching, tracking failure or exceeding image boundaries occur during visual servoing. The image error weighting vector p changes accordingly, ensuring the robustness of the visual servo system without affecting the quality of the control points.

[0062] When control points are lost during visual servoing, the specific implementation is to use occluding part of the target image, such as Figure 4As shown in (b)-(d), the robotic arm can still remain stable, proving that the method proposed in the present invention ensures the robustness of visual servo control when control points are lost.

Claims

1. A visual servoing method based on image error weighted vector, characterized in that: The following steps are involved: Step 1: Obtain the target images taken when the camera in the visual servoing task is in the initial posture and the preset posture respectively, and then use the feature extraction algorithm to extract the feature points of the target images when the camera is in the initial posture and the preset posture respectively, and obtain the initial and expected feature point sets respectively; Step 2: Based on the expected set of all feature points, the M-estimation sampling consensus algorithm is used to screen the initial set of all feature points to obtain the optimal qualified point set; Step 3: Assign different weights to each feature point in the optimal qualified point set, and then select several feature points from the optimal qualified point set in descending order as initial control points to form the initial control point set s when the camera is in the initial position. control , according to the initial control point set, select the corresponding preset control point set from the expected set of all feature points Then, the image error weighted vector is calculated according to the initial control point set, the weights corresponding to each initial control point, and the preset control point set, and the target image is visually servoed based on the image error weighted vector; If a control point fails to be tracked during the visual servoing task, the remaining feature point with the highest weight in the optimal qualified point set is selected to replace the lost control point and the image error weighted vector is updated; The step 2 is specifically as follows: 2.1) From the initial set of all feature points s all Randomly extract some feature points from the Inlier , the initial set of all feature points s all The set of remaining feature points in is taken as the non-interior feature point set s Other , calculate the current interior point set s Inlier The corresponding image Jacobian matrix And the estimated value of camera pose change and as an interior point model; 2.2) Calculate the current non-inlier feature point set s based on the expected set of all feature points Other The model error of each non-interior feature point in the current inlier model; then calculate the cost function value of each non-interior feature point according to the model error, and set the cost function value less than the outlier judgment threshold Thr Outlier All non-interior feature points are added to the qualified point set s good , if the current qualified point set s good The number of non-inlier feature points in count good Greater than the minimum qualified point threshold count min , then the current qualified point set s good As a set of candidate qualified points; 2.3) Repeat steps 2.1)-2.2) to obtain the initial set of all feature points s all In the above, some feature points are randomly selected multiple times and multiple candidate qualified point sets are calculated, and then the sum of the model errors of the non-interior feature points of each candidate qualified point set is calculated. uOthers , sum the model errors of non-inlier feature points uOthers The smallest set of candidate qualified points is taken as the optimal set of qualified points s best .

2. A visual servoing method based on image error weighted vector according to claim 1, characterized in that: In step 2.2), for the non-interior feature point set s Other The cost function value u of the i-th non-interior feature point in Others The calculation formula for [i] is: u Others [i]=min{e Others [i] 2 ,Thr Outlier 2 } Among them, e Other [i] represents the model error of the i-th non-inlier feature point, Represents the feature point corresponding to the i-th non-inlier feature point in the expected set of all feature points.

3. The visual servoing method based on image error weighted vector according to claim 1, characterized in that: In the step 3, the weight of each feature point in the optimal qualified point set is calculated by the cost function value of each feature point in the optimal qualified point set. For the i-th feature point in the optimal qualified point set, the calculation formula of its weight is: Among them, w best [j] represents the weight of the jth feature point, u best [j] represents the optimal qualified point set s best The cost function value of the j-th feature point in , E represents the noise error allowed for defining the visual signal.

4. The visual servoing method based on image error weighted vector according to claim 1, characterized in that: The calculation formula of the image error weighted vector p is as follows: Where D is a diagonal matrix composed of normalized control point weights.

5. The visual servoing method based on image error weighted vector according to claim 1, characterized in that: In step three, the number of initial control points is greater than or equal to 3.

Images