A robot vision control system and method
By introducing a control obstacle function and a quadratic programming optimization framework, the problem of feature points moving out of the camera's field of view in a robot vision control system was solved, achieving stable and efficient visual servo control.
Patent Information
- Application Number
- CN202611022022.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-07-09
- Publication Date
- 2026-11-10
AI Technical Summary
In existing robot vision control systems, feature points can easily move out of the camera's field of view during large-scale movements, making it impossible to calculate image deviations, causing the interaction matrix to degenerate into a singular state, and leading to the divergence and failure of the control system. Existing solutions have high computational overhead or introduce control discontinuity problems.
A control barrier function (CBF) and quadratic programming (QP) constraint optimization framework is introduced. By constructing the control barrier function as a constraint and embedding the time-by-time solution process of the control law, the feature points are ensured to be within the field of view boundary, and the corrected visual servo speed command is generated to avoid the field of view from going out of bounds.
This approach achieves proactive prevention of field of view out-of-bounds behavior while ensuring closed-loop convergence, thus avoiding control discontinuities and chattering, and improving system stability and efficiency.
Smart Images

Figure CN123060640A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of robot control, and more particularly to a robot vision control system and method. Background Technology
[0002] Image-based visual servoing (IBVS) is a mainstream method in the field of robot vision control. Its basic idea is to utilize the deviation between the pixel coordinates of target feature points (such as corners, edges, or SIFT features) in a real-time image acquired by a camera and the coordinates of corresponding feature points in the desired image. This deviation is then mapped to the robot's 6-DOF velocity commands via an interaction matrix, forming a closed-loop control system. Compared to position-based visual servoing (PBVS), IBVS does not require precise 3D reconstruction of the target and exhibits better robustness to calibration errors, thus it is widely used in practical engineering.
[0003] However, IBVS has a fundamental flaw: during large-scale robot movements, feature points can easily move out of the camera's field of view, leading to uncalculated image deviations, the degradation of the interaction matrix into a singular state, and ultimately causing the entire control system to diverge and fail. This problem is particularly prominent in typical scenarios such as large-angle posture adjustments by robotic arms or large-angle turns by mobile robots.
[0004] To address the aforementioned field-of-view constraint problem, existing technologies have attempted several strategies: First, a path planning method is used to pre-plan a collision-free trajectory in Cartesian space that ensures features are always visible. However, this method has high computational overhead and is not suitable for dynamic environments. Second, a forced switch to PBVS mode is implemented when feature points are near the image boundary. However, frequent mode switching can introduce control discontinuities and jitter issues. Third, a heuristic backtracking strategy is adopted, which pauses the servo and performs backtracking motion when a feature is detected to be about to be lost. However, this significantly extends the task execution time and lacks convergence guarantees.
[0005] Therefore, how to fundamentally prevent feature points from moving out of the camera's field of view during visual servoing while ensuring closed-loop convergence is a technical problem that urgently needs to be solved in this field. Summary of the Invention
[0006] The purpose of this invention is to disclose a robot vision control system and method to address the shortcomings of the prior art.
[0007] To achieve the above objectives, the present invention provides the following technical solution: On one hand, the present invention provides a robot vision control method, comprising: S1, acquire the current image captured by the camera installed on the robot, and extract the current image coordinates of at least one feature point from the current image; S2, calculate the image feature error based on the current image coordinates and the corresponding expected image coordinates of the at least one feature point; S3, based on the image feature error and the combined interaction matrix composed of the interaction matrices of the at least one feature point, a nominal visual servo speed command is generated; wherein, the nominal visual servo speed command is a robot speed command that drives the image feature error to converge to zero under the condition of no field of view constraint. S4, based on the field of view boundary of the camera, construct at least one control obstacle function for the at least one feature point; wherein, the function value of the control obstacle function reflects the distance between the corresponding feature point and the field of view boundary of the camera; S5, using the nominal visual servo speed command as a reference and the at least one control obstacle function as a constraint, construct a quadratic programming problem; the objective function of the quadratic programming problem is to minimize the deviation between the variable to be solved in the quadratic programming problem and the nominal visual servo speed command; the constraint condition of the quadratic programming problem is: the sum of the time derivative of each control obstacle function under the action of the variable to be solved and the product of the current function value of the control obstacle function and the corresponding preset positive coefficient is greater than or equal to zero; S6, Solve the quadratic programming problem to obtain the corrected visual servo speed command; S7, control the robot's movement with the corrected visual servo speed command.
[0008] Optionally, the extraction process of the current image coordinates includes: S11, acquire the current image captured by the camera, and extract candidate feature points from the current image using a feature detection algorithm; S12, perform feature matching between the candidate feature points and the expected feature points corresponding to the expected image coordinates to obtain the matching feature points; S13, a random sampling consensus algorithm is used to remove mismatched points from the matching feature points, and the remaining matching feature points after removing the mismatched points are taken as the at least one feature point, and the current image coordinates of the at least one feature point are obtained.
[0009] Optionally, the image feature error is calculated using the following formula:
[0010] in, The image feature error at time t, The vector formed by the current image coordinates of the at least one feature point at time t. The vector formed by the corresponding expected image coordinates.
[0011] Optionally, the nominal visual servo speed command is generated using the following formula:
[0012] in, The nominal visual servo speed command at time t. To preset the positive constant gain, The pseudo-inverse of the interaction matrix is... The image feature error, The vector formed by the current image coordinates of the at least one feature point at time t. The depth value of the at least one feature point.
[0013] Optionally, the field of view boundary of the camera is a convex polygon region defined in the image coordinate space, and the control obstacle function is a function that reflects the signed distance between the corresponding feature point and each edge of the convex polygon region; in S4, for any feature point, the minimum value of the signed distance between the feature point and each edge of the convex polygon region is taken as the function value of the control obstacle function corresponding to the feature point.
[0014] Optionally, constructing the quadratic programming problem includes, S51, the variable to be solved is set to a velocity variable v with the same dimension as the nominal visual servo speed command; S52, to minimize As the objective function, where The nominal visual servo speed command at time t; S53, for each of the control barrier functions, calculate its partial derivative with respect to the current image coordinates of the at least one feature point, and use the interaction matrix to convert the partial derivative into a gradient with respect to the variable to be solved v, thereby expressing the constraint as a linear inequality constraint with respect to v. S54, combine the objective function with all the linear inequality constraints to form the quadratic programming problem.
[0015] Optional, in S52 Using the weighted L2 norm form:
[0016] in, It is a preset positive definite weight matrix used to apply different correction cost weights to each velocity component.
[0017] Optionally, in S53, for any i-th control barrier function... The linear inequality constraints are expressed as follows:
[0018] in, The current image coordinates of the i-th control barrier function relative to the i-th feature point. The partial derivative vector, The interaction matrix of the i-th feature point. Let be the depth value of the i-th feature point. Let be the variable to be solved. The preset positive coefficient, This is the current function value of the i-th control barrier function.
[0019] Optionally, S7 includes, S71, Based on the corrected visual servo speed command and the preset control cycle, the corrected visual servo speed command is integrated to obtain the robot's expected pose increment in the current control cycle. S72, the desired pose increment is converted into joint angular displacement commands for each joint of the robot using the inverse kinematics model of the robot; S73, the joint angular displacement command is sent to the robot's underlying controller to drive the robot to move to the pose at the next moment.
[0020] On the other hand, the present invention provides a robot vision control system, comprising: An extraction module is used to acquire the current image captured by a camera installed on the robot, and extract the current image coordinates of at least one feature point from the current image; The calculation module is used to calculate the image feature error based on the current image coordinates and the corresponding expected image coordinates of the at least one feature point; The generation module is used to generate a nominal visual servo speed command based on the image feature error and a combined interaction matrix composed of the interaction matrices of the at least one feature point; wherein the nominal visual servo speed command is a robot speed command that drives the image feature error to converge to zero under the condition of no field of view constraint. A construction module is configured to construct at least one control obstacle function for the at least one feature point based on the field of view boundary of the camera; wherein the function value of the control obstacle function reflects the distance between the corresponding feature point and the field of view boundary of the camera; The construction module is further configured to construct a quadratic programming problem based on the nominal visual servo speed command and constrained by at least one control obstacle function; the objective function of the quadratic programming problem is to minimize the deviation between the variable to be solved in the quadratic programming problem and the nominal visual servo speed command; the constraint condition of the quadratic programming problem is that the sum of the time derivative of each control obstacle function under the action of the variable to be solved and the product of the current function value of the control obstacle function and the corresponding preset positive coefficient is greater than or equal to zero; The solver module is used to solve the quadratic programming problem and obtain the corrected visual servo speed command; A control module is used to control the movement of the robot using the corrected visual servo speed command.
[0021] The technical solution of this application solves the technical problems described in the background art by introducing a control barrier function (CBF) and a quadratic programming (QP) constraint optimization framework. Its core mechanism and beneficial effects are as follows: Unlike existing technologies that rely on reactive remediation (such as passively reversing or switching control modes after detecting a feature point near the boundary), this application encodes the camera's field of view boundary as a control obstacle function and uses this control obstacle function as the time-by-time solution process for the constraint-embedded control law. The constraint condition constructed in S5, "the sum of the product of the time derivative of each control obstacle function under the action of the variable to be solved and the current function value of the control obstacle function and the preset positive coefficient is greater than or equal to zero," strictly guarantees that as long as the current feature point is within the field of view boundary, under the action of the corrected visual servo speed command obtained from the QP solution, the feature point will necessarily remain within the field of view boundary in the next time step. This property is independent of the length of the motion path or the quality of the initial pose, realizing a fundamental shift from passively responding to field of view boundary violations to actively preventing them.
[0022] This application achieves the effect of minimizing the deviation between the variable to be solved and the nominal visual servo speed command by designing the objective function to "minimize the deviation between the variable to be solved and the nominal visual servo speed command," thus ensuring the safety of the field of view while minimizing the deviation from the nominal visual servo objective. When the feature point is far from the boundary of the field of view, the constraint condition is naturally satisfied, and the solution result is almost equivalent to the nominal visual servo speed command, allowing the system to execute the servo task with maximum efficiency. Only when the feature point approaches the boundary of the field of view does the constraint condition begin to actively intervene and correct the speed command, and the correction amount is only the minimum amount necessary to satisfy the constraint condition. Attached Figure Description
[0023] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0024] Figure 1 This is a schematic diagram of a robot vision control method according to the present invention.
[0025] Figure 2 This is a schematic diagram illustrating the current image coordinate extraction process of the present invention.
[0026] Figure 3 This is a schematic diagram illustrating the process of controlling the robot's motion using modified visual servo speed commands according to the present invention.
[0027] Figure 4 This is a schematic diagram of a robot vision control system according to the present invention. Detailed Implementation
[0028] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0029] refer to Figure 1 This invention provides a robot vision control method, comprising: S1, acquire the current image captured by the camera installed on the robot, and extract the current image coordinates of at least one feature point from the current image.
[0030] Robots are automated devices equipped with cameras and capable of moving within their workspace according to control commands. Their achievable motion dimensions include at least some or all degrees of freedom along the three translation axes and three rotation axes in Cartesian space. Robots include, but are not limited to, fixed-base industrial robotic arms, wheeled or tracked mobile robot platforms, multi-rotor or fixed-wing drones, and humanoid robots and other biomimetic configurations. All of these types of robots can change their own or their end effector's working position and orientation relative to a target by receiving speed or pose commands.
[0031] The current image refers to a frame of two-dimensional digital image captured and output in real time by a camera mounted on the robot at the start of the current control cycle. This image is typically represented as a pixel matrix, where each pixel location corresponds to a radiometric value (e.g., grayscale value, or red, green, and blue color channel values) of a scene point within the spatial viewpoint. The current image contains complete visual information about the target pattern or object as observed by the robot in its current pose.
[0032] Feature points refer to local geometric elements in the current image that can be identified and extracted by a feature detection algorithm, possessing discriminative and repeatable extraction characteristics. Feature points include, but are not limited to, corner points, edge points, spots, line segment endpoints, or the geometric center of closed contours in the image. The characteristics that distinguish feature points from their surrounding pixel regions are typically defined by the gradient distribution or gray-level statistical features of their local neighborhood. By matching the corresponding feature points of the same physical space point in the current image with those in a pre-acquired or preset desired image, a visual error signal describing the deviation of the current pose from the desired pose can be obtained. Feature points do not depend on the prior 3D geometric model of the target object; detection and matching can be completed based solely on their 2D image appearance.
[0033] Here, the current image coordinates refer to the two-dimensional position representation of a feature point in the pixel coordinate system of the current image. It is usually represented as a coordinate pair of the form (x, y), where each coordinate component is a real number, representing the pixel position of the feature point in the horizontal and vertical directions of the image, respectively. When there are multiple feature points, the current image coordinates of each feature point are usually stacked or arranged into a column vector to provide data for subsequent steps, such as the calculation of image feature errors and the construction of the interaction matrix.
[0034] Optional, see reference Figure 2 The process of extracting the current image coordinates includes: S11, acquire the current image captured by the camera, and extract candidate feature points from the current image using a feature detection algorithm.
[0035] In one embodiment, the camera is a monocular RGB camera mounted on the end effector of a six-degree-of-freedom industrial robotic arm, which continuously acquires grayscale images with a resolution of 640×480 pixels at a frame rate of 30 frames per second. At the start of the current control cycle, the grayscale image acquired by the camera is the current image.
[0036] In step S11, the FAST algorithm is used to extract candidate feature points from the current image. Specifically, for each pixel p in the current image, within a circular neighborhood of 16 pixels (centered on p and with a radius of 3 pixels), the grayscale values of the surrounding pixels are checked sequentially. If there are 12 consecutive pixels on the circumference whose grayscale values are all higher than the grayscale value of p plus a preset threshold (e.g., 20 grayscale levels), or all lower than the grayscale value of p minus the preset threshold, then pixel p is determined to be a candidate feature point. After traversing all pixels in the current image, a set of candidate feature points is obtained. It is understood that in other embodiments, SIFT, SURF, ORB algorithms, or neural network feature extraction algorithms based on deep learning such as SuperPoint can also be used to obtain candidate feature points, and this invention does not limit this.
[0037] S12, perform feature matching between the candidate feature points and the expected feature points corresponding to the expected image coordinates to obtain the matching feature points.
[0038] In one embodiment, the desired image is a grayscale image that is captured and stored in advance after the camera is moved to the desired pose by a robotic arm. The desired feature points are extracted and stored in advance from the desired image using the same feature detection algorithm as in S11. Each desired feature point is appended with a descriptor vector, which is generated by the BRIEF descriptor. This descriptor is a 256-bit binary encoding of the grayscale comparison result of random points in the neighborhood of the candidate feature point in S11.
[0039] In S12, for each candidate feature point obtained in S11, its BRIEF descriptor is calculated, and then the Hamming distance (i.e., the number of different bits in two binary strings) is calculated between this descriptor and the descriptors of all expected feature points. For each candidate feature point, the expected feature point with the smallest Hamming distance that is less than a preset threshold (e.g., 50) is selected as its matching object. If the smallest Hamming distance is greater than or equal to the preset threshold, it is determined that the candidate feature point has no matching expected feature point and is discarded. After completing the above matching process, the remaining candidate feature points constitute the set of matching feature points, and each matching feature point forms a pairing relationship with a unique expected feature point. It can be understood that in some other embodiments, brute-force matching combined with distance ratio testing (i.e., requiring the ratio of the nearest distance to the second nearest distance to be less than 0.8) can also be used for feature matching to further eliminate ambiguous matching pairs.
[0040] S13, a random sampling consensus algorithm is used to remove mismatched points from the matching feature points, and the remaining matching feature points after removing the mismatched points are taken as the at least one feature point, and the current image coordinates of the at least one feature point are obtained.
[0041] In one embodiment, the set of matching feature points obtained in S12 may still contain mismatched points caused by image noise, repetitive textures, or local occlusion. In S13, the RANSAC algorithm is used to refine the set of matching feature points.
[0042] The specific process is as follows: A minimum subset (e.g., 4 matching pairs) is randomly selected from the set of matching feature points. A homography transformation matrix H is fitted based on this minimum subset—this homography matrix describes the planar projection transformation relationship from the current image to the desired image. Then, the current image coordinates of all other matching feature points in the set are projected onto the desired image coordinate space through H, and the geometric distance error between each projected coordinate and its corresponding desired image coordinate is calculated. If the distance error is less than a preset inlier threshold (e.g., 3 pixels), the matching feature point is marked as an inlier; otherwise, it is marked as an outlier. The above random sampling and inlier counting process is repeated a preset number of times (e.g., 1000 iterations), and the inlier set corresponding to the homography transformation with the largest number of inliers is selected as the final effective set of matching feature points. The feature points in this effective set of matching feature points are the at least one feature point finally output in step S1, and the pixel position coordinates of each feature point are its current image coordinates. The current image coordinates of each feature point are stacked in a uniform order (e.g., according to the spatial order of the feature points in the image from top to bottom and from left to right) into a column vector form s(t), which is used in the subsequent step S2.
[0043] It is understandable that in other embodiments, alternative methods such as M-estimation or the PROSAC algorithm can be used to remove mismatched points. For scenarios without obvious planar structure, the fundamental matrix under epipolar geometry constraints combined with RANSAC can also be used for outlier removal.
[0044] S2, calculate the image feature error based on the current image coordinates and the corresponding expected image coordinates of the at least one feature point.
[0045] Here, "current image coordinates" refers to a column vector formed by arranging the two-dimensional positions of each feature point output in step S1 within the pixel coordinate system of the current image according to a predetermined rule. For example, if step S1 outputs n feature points, the pixel position of the i-th feature point is ( , If the current image coordinates are such that the coordinates are represented as a column vector, then the current image coordinates can be represented as a column , where t represents the time marker of the current control cycle. The dimension of this column vector is 2n, and its arrangement order is consistent with the output order of the feature points in step S1.
[0046] Here, the desired image coordinates refer to the target position vectors that correspond one-to-one with each feature point in the current image coordinates. The desired image coordinates are extracted from the desired image acquired beforehand under the desired state, and their arrangement rules and dimensions are strictly consistent with the current image coordinates. In step S2, the desired image coordinates are represented as column vectors. ,in( Let be the pixel position of the i-th feature point in the desired image. The desired image coordinates remain unchanged during the execution of the visual servoing task and serve as the reference target for closed-loop control.
[0047] The image feature error refers to the vector difference between the current image coordinates and the desired image coordinates. It quantifies the deviation of the current camera's observed feature point position from the target position, and its dimension is the same as that of the current image coordinates and the desired image coordinates (both are 2n-dimensional). The smaller the norm of the image feature error, the closer the current pose is to the desired pose. When the current image coordinates of all feature points coincide with their desired image coordinates, the image feature error degenerates into a zero vector. The image feature error is the driving signal for generating the nominal visual servo speed command in step S3.
[0048] Optionally, the image feature error is calculated using the following formula:
[0049] in, The image feature error at time t, The vector formed by the current image coordinates of the at least one feature point at time t. The vector formed by the corresponding expected image coordinates.
[0050] S3, based on the image feature error and the combined interaction matrix formed by combining the interaction matrices of the at least one feature point, a nominal visual servo speed command is generated. The nominal visual servo speed command is the robot speed command that drives the image feature error to converge to zero under conditions without field-of-view constraints.
[0051] The interaction matrix, also known as the image Jacobian matrix, is a linear transformation matrix that maps the robot's spatial velocity to the velocity of the corresponding feature point on the image plane. For a feature point mapped from a 3D spatial point to the image plane, the linear relationship between the feature point's velocity in the image and the robot's end effector's spatial velocity is described by the interaction matrix. Specifically, for the... Each feature point has an interaction matrix. The dimension is Satisfying the relation: ,in This is the velocity vector of the feature point on the image plane. This represents the 6-dimensional spatial velocity vector of the robot's end effector (containing three translational velocity components and three rotational velocity components). The elements of the interaction matrix depend on the coordinates of the feature point in the current image and the depth value of the feature point relative to the camera coordinate system. When step S1 extracts... When there are n feature points, the interaction matrices of each feature point are stacked row-wise to form a dimension of n. Combination interaction matrix ,satisfy: ,in This is a column vector consisting of the current image coordinates of all feature points. Step S3 will utilize the pseudo-inverse of this combined interaction matrix. Image feature errors are converted into speed commands. It can be understood that the specific construction of the interaction matrix depends on the type of feature points selected (for example, the interaction matrix corresponding to point features and the interaction matrix corresponding to line segment features have different analytical forms), but regardless of the feature type used, the interaction matrix establishes a linear mapping relationship from the robot's feature space to the image feature space.
[0052] The field-of-view constraint refers to the restriction that feature points must always remain within the camera's field of view boundary. In the unconstrained standard visual servoing framework, the control law only aims to minimize image feature error, without actively considering whether feature points approach or exceed the image boundary during movement. The existence of the field-of-view constraint means that even if the current image feature error is far from convergent to zero, once the distance between the feature point and the camera's field of view boundary decreases to a certain extent, this constraint restricts the generation of speed commands, preventing feature points from continuing to move towards the boundary. In this invention, the field-of-view constraint is not directly used as a condition for filtering or rejecting speed commands, but is equivalently implemented through the control obstacle function and quadratic programming constraint in steps S4 and S5—that is, in the form that "the sum of the product of the time derivative of each control obstacle function under the action of the variable to be solved and the current function value of the control obstacle function and the preset positive coefficient is greater than or equal to zero," constraints are continuously and in real-time applied during the generation of speed commands. In contrast, the nominal visual servo speed command generated in step S3 is generated under the assumption that the above-mentioned field of view constraints are not considered. Therefore, it only guarantees the convergence of image feature errors, but does not guarantee the safety of the field of view of feature points.
[0053] In this context, the robot speed command refers to a set of speed reference values sent to the robot's underlying motion control system to drive the robot's movement from its current pose to its pose at the next moment. Within the framework of this invention, the robot speed command is a 6-dimensional vector, typically denoted as... ,in , , The robot end effector along the camera coordinate system axis, axis, The translational linear velocity component of the axis, , , The robot end effector revolves around the camera coordinate system. axis, axis, The rotational angular velocity component of the axis. Robot speed commands are divided into two levels: nominal visual servo speed commands and corrected visual servo speed commands. The nominal visual servo speed command is generated in step S3 under conditions without field-of-view constraints, and its spatial speed command components are nominal linear velocity and nominal angular velocity. The corrected visual servo speed command is obtained after solving a quadratic programming problem in step S6, and its components deviate from the corresponding nominal components to the minimum extent while ensuring that the field-of-view constraints are satisfied. Each component of the robot speed command is expressed in physical units; for example, the unit of the linear velocity component is meters per second.
[0054] Optionally, the nominal visual servo speed command is generated using the following formula:
[0055] in, The nominal visual servo speed command at time t. To preset the positive constant gain, The pseudo-inverse of the interaction matrix is... The image feature error, The vector formed by the current image coordinates of the at least one feature point at time t. The depth value of the at least one feature point.
[0056] In one embodiment, the preset positive constant gain λ is set to 0.5. At this value, when the image feature error is 10 pixels and the effective amplification factor of the pseudo-inverse of the interaction matrix is about 1, the order of magnitude of each component of the nominal visual servo speed command is about 5 units of speed (e.g., the linear velocity component is about 0.05 meters per second, and the angular velocity component is about 0.05 radians per second). This speed level is within the safe operating range of a typical industrial robotic arm, is not prone to exciting mechanical resonance, and can ensure that the servo convergence task is completed within dozens of control cycles.
[0057] In another embodiment, the value of the preset positive constant gain is determined according to the following rule: First, the maximum safe operating speed of the robot is estimated. Secondly, before the servo task begins, the robot is driven to a calibration pose with a known deviation from the desired pose, and image feature errors under this calibration pose are collected. and the corresponding pseudo-inverse of the interaction matrix Calculate the upper limit of normalized gain. Finally, take ,in To match the camera frame rate and robot dynamic response bandwidth, automatic convergence is achieved to avoid generating excessively large velocity commands. When the image feature error decreases, the velocity commands naturally decay because the norm of the pseudo-inverse of the interaction matrix usually does not change drastically, and overshoot oscillations will not be caused by excessive velocity.
[0058] In another embodiment, the preset normal gain λ remains fixed during the servo process, and its value is empirically set according to the specific configuration of the robot: for heavy-duty industrial robotic arms (end-effector load greater than 20 kg), λ is 0.1 to 0.3 to reduce motion impact and protect the reducer; for lightweight collaborative robotic arms (end-effector load less than 5 kg), λ is 0.5 to 1.0 to make full use of its high dynamic response capability to accelerate convergence; for underdamped platforms such as drones, λ is 0.2 to 0.4, and works in conjunction with the low-pass filtering in step S7 to suppress high-frequency components in the speed command.
[0059] It is understood that the preset positive gain λ can also take other positive values outside the above range, such as 0.05 or 2.0, to adapt to special speed requirements or specific robot dynamics characteristics. The specific value of λ does not affect the operating principle of the field of view constraint mechanism introduced in steps S4 to S6 of this invention—regardless of the value of λ, the quadratic programming problem is constrained and corrected based on the nominal visual servo speed command corresponding to λ, and the corrected speed command always satisfies the field of view constraint conditions.
[0060] S4, based on the camera's field of view boundary, construct at least one control obstacle function for the at least one feature point. The function value of the control obstacle function reflects the distance between the corresponding feature point and the camera's field of view boundary.
[0061] Here, the field of view boundary refers to a set of geometric boundaries that define the effective area of the image in the two-dimensional image coordinate space acquired by the camera. In the context of step S4, the field of view boundary directly corresponds to the representation of the four edges of the physical imaging area of the camera image sensor in the image coordinate system. For a resolution of... For a 1-pixel image, the field of view is typically defined by four straight lines: the left boundary... Right boundary Upper boundary Lower boundary (or adopt) , , , (Represented by four boundary values). When the image coordinates of a feature point are within the closed region enclosed by the field of view boundary, the feature point is observed normally by the camera and can be used for visual servoing; when the image coordinates of a feature point are outside the field of view boundary, the feature point has moved out of the camera's field of view, its image coordinates are unusable, and the visual servoing system loses the error signal provided by the feature point. In a more general sense, the field of view boundary is not limited to a rectangle, but can also be any convex polygonal region in the image coordinate space, such as an irregular polygon after cropping the effective pixel area of the camera, or an effective photosensitive area limited by lens distortion or imaging plane. The geometric parameters of the field of view boundary (the equations of the straight lines on each side or the coordinates of each vertex) are the inputs to constructing the control obstacle function in step S4 and remain fixed during the execution of the servoing task.
[0062] A control barrier function is a class of scalar real-valued functions defined in image coordinate space. The sign and magnitude of its function value reflect the safety level of a feature point relative to the view boundary. Control barrier functions have the following properties: when the function value is greater than zero, the corresponding feature point is in a safe region inside the view boundary; when the function value is equal to zero, the feature point is exactly on the view boundary; when the function value is less than zero, the feature point has exceeded the view boundary. The output of the control barrier function constructed for each feature point in step S4 is its function value, which participates in the construction of the quadratic programming problem as part of the constraints in step S5. The essence of the control barrier function lies in the fact that by applying an inequality constraint on the function with respect to its time derivative (i.e., the form specified by the constraints in step S5), it can be strictly guaranteed that: as long as the function value of the control barrier function is greater than or equal to zero at any given moment, then under the velocity command that satisfies this constraint, the function value at the next moment will also be greater than or equal to zero. This means that once a feature point enters the view boundary, it will never move out of the view. The specific form of the obstacle control function can be flexibly selected according to the geometry of the field of view boundary—for example, for a rectangular field of view, the minimum of the signed distances from the feature point to the four boundary lines can be taken as the function value; in other embodiments, a smooth approximation can also be used to represent the combined signed distances from the feature point to the four boundary lines. For an arbitrary convex polygonal field of view, the signed distance can be calculated side by side and the minimum value can be taken.
[0063] Optionally, the field of view boundary of the camera is a convex polygon region defined in the image coordinate space, and the control obstacle function is a function that reflects the signed distance between the corresponding feature point and each edge of the convex polygon region; in S4, for any feature point, the minimum value of the signed distance between the feature point and each edge of the convex polygon region is taken as the function value of the control obstacle function corresponding to the feature point.
[0064] Here, the corresponding feature point refers to a specific feature point among the at least one feature point output in step S1. The field of view boundary is defined by the edges of the convex polygon region, which is a fixed reference geometry; the corresponding feature point is a current observation point whose positional safety relative to the convex polygon region needs to be checked one by one. In other words, step S4 calculates the safety margin of each feature point relative to the field of view boundary individually. In this process, the feature point currently being calculated is the feature point corresponding to the control barrier function. When step S1 outputs n feature points, step S4 constructs n control barrier functions in total. The i-th control barrier function uniquely corresponds to the i-th feature point, and there is a one-to-one correspondence between the two.
[0065] The signed distance function is a scalar function that takes the image coordinates of a feature point and a given edge of a convex polygon region as input, and outputs the perpendicular distance from the feature point to that edge, with a signed value. The sign convention for the signed distance is as follows: if the feature point is located inside the edge (i.e., facing the interior of the convex polygon region), the signed distance is positive, and its value is equal to the perpendicular Euclidean distance from the feature point to the line containing the edge; if the feature point is exactly on the line containing the edge, the signed distance is zero; if the feature point is located outside the edge (i.e., away from the interior of the convex polygon region), the signed distance is negative, and its absolute value is the perpendicular Euclidean distance from the feature point to the line containing the edge. For the typical case of a rectangular viewpoint boundary, the four edges are defined as the left boundary, right boundary, top boundary, and bottom boundary, and the corresponding signed distances are as follows: , , and ,in The current image coordinates of the corresponding feature point. , , These are the four boundary values of the rectangular field of view in the image coordinate system. For each edge of the convex polygon region, the signed distance of the corresponding feature point is calculated, and the minimum value among all signed distances is taken. This minimum value is the function value of the control barrier function corresponding to the feature point. This minimum value reflects the "shortest safe distance" of the feature point from the edge closest to the field of view boundary and carries the sign information of whether the feature point has crossed the edge.
[0066] S5. Using the nominal visual servo speed command as a reference and the at least one control obstacle function as a constraint, a quadratic programming problem is constructed. The objective function of the quadratic programming problem is to minimize the deviation between the variable to be solved and the nominal visual servo speed command. The constraint condition of the quadratic programming problem is that the sum of the product of the time derivative of each control obstacle function under the action of the variable to be solved and the product of the current function value of the control obstacle function and the corresponding preset positive coefficient is greater than or equal to zero.
[0067] Optionally, constructing the quadratic programming problem includes, S51, the variable to be solved is set as a velocity variable v with the same dimension as the nominal visual servo speed command.
[0068] In one embodiment, the nominal visual servo speed command generated in step S3 It is a 6-dimensional column vector, that is The first three components are linear velocity components, and the last three components are angular velocity components. In S51, the variables to be solved are... It is also set as a 6-dimensional column vector. The physical meaning and units of its components are the same as those of the components. All components are completely identical. This variable to be solved... In the subsequent quadratic programming solution process, as an unknown quantity, after combined optimization from S52 to S54, its optimal solution is the corrected visual servo speed command output in step S6. In another embodiment, when the robot only has planar motion capabilities (e.g., a differential wheel mobile robot), the effective dimension of the nominal visual servo speed command is reduced to 3 dimensions (e.g., forward linear velocity, lateral linear velocity, and rotational angular velocity about the vertical axis). In this case, the variables to be solved in S51 are... The dimension is set to 3-dimensional, keeping the same dimension as the nominal visual servo speed command.
[0069] S52, to minimize As the objective function, where The nominal visual servo speed command at time t.
[0070] Furthermore, in S52 Using the weighted L2 norm form:
[0071] in, It is a preset positive definite weight matrix used to apply different correction cost weights to each velocity component.
[0072] In one embodiment, the objective function directly takes the variable to be solved. With nominal visual servo speed command The square of the Euclidean norm of the difference, i.e. ,in The dimension of the velocity vector (usually) ), and They are respectively and The There are several components. In this form, the objective function is about... A strictly convex quadratic function whose Hessian matrix is the identity matrix and whose gradient is at... The value is zero at this point. This objective function expresses the design intent that, while satisfying constraints, the corrected speed command should be as close as possible to the nominal visual servo speed command. In another embodiment, the objective function adopts a weighted L2 norm form. .
[0073] The weight matrix For example, a predefined positive definite diagonal matrix. This weight matrix assigns a unit correction cost to the three linear velocity components and a lower correction cost to the three angular velocity components (i.e., it is more tolerant of angular velocity correction). This means that when field-of-view constraints need to be addressed, the solution to the quadratic programming problem tends to correct the angular velocity components rather than the linear velocity components. This design is suitable for robotic arm scenarios where angular velocity adjustment has less impact on motion stability than linear velocity adjustment.
[0074] S53, for each of the control barrier functions, calculate its partial derivative with respect to the current image coordinates of the at least one feature point, and use the interaction matrix to convert the partial derivative into a gradient with respect to the variable to be solved, thereby expressing the constraint as a linear inequality constraint with respect to v.
[0075] Furthermore, in S53, for any i-th control barrier function... The linear inequality constraints are expressed as follows:
[0076] in, The current image coordinates of the i-th control barrier function relative to the i-th feature point. The partial derivative vector, The interaction matrix of the i-th feature point. Let be the depth value of the i-th feature point. Let be the variable to be solved. The preset positive coefficient, This is the current function value of the i-th control barrier function.
[0077] In one embodiment, the first The control barrier function corresponding to each feature point is denoted as . First calculate Relative to the current image coordinates of this feature point partial derivative vector With a rectangular field of view boundary and CBF as... For example, suppose in the case of The minimum value term at time is (i.e., the left boundary is closest), then the partial derivative is... If the minimum value term is (The right boundary is closest), then If the minimum value term is (Lowest boundary), then If the minimum value term is (Nearest upper boundary), then This partial derivative is always a unit vector, pointing in the normalized direction that the feature point moves away from the nearest boundary. Subsequently, using the... Interaction matrix of feature points (dimension is) This transforms the partial derivatives from the image coordinate space to the robot velocity space. The specific calculation method is as follows: The result is a 6-dimensional row vector, which is the control barrier function. Relative to the variable to be solved The gradient (or more precisely, the gradient) time derivative Compared to (The coefficient row vector). The constraint in step S5 is "the sum of the time derivative of each control obstacle function under the action of the variable to be solved and the product of the current function value of the control obstacle function and the preset positive coefficient is greater than or equal to zero", and the corresponding expression is: The inequality constraint is about For strictly linear — where For a known row vector of constant coefficients (in the current control cycle) (The time can be calculated) For the known nonnegative constant term (where As a preset positive coefficient, for (The control barrier function value at time). Therefore, this inequality is the form required in standard quadratic programming. (or equivalent) Linear constraints. Taking a scene with 4 feature points as an example, step S53 will obtain 4 linear inequality constraints. Each constraint is an inequality with a 6-dimensional coefficient row vector and a constant value. The 4 constraints together ensure the safety of the field of view for all 4 feature points.
[0078] In another embodiment, when the view boundary is an arbitrary convex polygon and the target feature point is near a vertex of the polygon, the minimum term of the control barrier function may switch between two different boundary edges, at which point the partial derivatives are discontinuous. To address this issue, the control barrier function can be reconstructed using a smooth approximation: the minimum operation is approximated as... ,in For the first The nth feature point to the convex polygon The signed distance of the edge. The preset positive smoothing parameter (e.g., a value between 10 and 100) The larger the value, the closer it approximates the true minimum function. In this smooth form, the partial derivatives of the control barrier function with respect to the feature point image coordinates are everywhere differentiable and can be substituted into the partial derivative calculation in step S53 in a conventional manner. In another embodiment, to solve the depth value of the feature points... There is a problem that estimation errors lead to inaccurate coefficients in the QP constraint. In step S53, the depth value is predetermined. The upper bound of the estimation error is denoted as . If the nominal estimate of the feature point depth is Then its true value falls within the interval In this embodiment, the depth value within the aforementioned interval that imposes the most stringent constraints (i.e., the largest magnitude of the coefficient term) is substituted into the construction of the QP constraints, thereby ensuring that even if there are errors in the depth estimation, the solution of QP still satisfies the constraints in the worst case.
[0079] S54, combine the objective function with all the linear inequality constraints to form the quadratic programming problem.
[0080] In one embodiment, after processing by S52 and S53, the quadratic programming problem has the following standard form, which can be directly delivered to the standard QP solver for solving: ; subjectto ; Among them, the Hessian matrix ( for identity matrix, usually ), gradient vector Inequality constraint matrix The Behavior (Note the sign conversion; rewrite the form in 2 using the solver's convention.) (Form), constraining upper bound vector The The elements are In a specific numerical example: Suppose time There are 3 feature points, and their CBF values and partial derivatives have been calculated. Then... for A diagonal matrix, with each diagonal element consisting of 2 elements; It is a 6-dimensional vector, and each component is equal to -2 times the corresponding component; for A matrix, each row consisting of corresponding feature points. Calculated; It is a 3-dimensional vector, and each component is equal to the corresponding feature point. By inputting this standard form of quadratic programming problem into an active set method or interior point method QP solver, the optimal solution variables can be obtained in a time of several microseconds to hundreds of microseconds. This refers to correcting the visual servo speed command. It is understood that in other embodiments, dedicated solvers optimized for small-scale QP, such as OSQP, may be used, or implicit QP solving methods based on neural network approximation may be used on embedded platforms to meet microsecond-level control cycle requirements.
[0081] S6. Solve the quadratic programming problem to obtain the corrected visual servo speed command.
[0082] In one embodiment, the quadratic programming problem constructed in step S5 is a small-scale convex quadratic programming problem (variables to be solved) The dimension is typically 6, and the number of constraints equals the number of feature points, usually 4 to 20. S6 uses the activity set method to solve this quadratic programming problem. The basic process of the activity set method is: starting from an initial feasible point (e.g., taking...). If the point satisfies all linear inequality constraints, it is directly output as the optimal solution. In each iteration, an equality-constrained quadratic programming subproblem containing only the set of constraints currently identified as active is solved, and the unsolved variables are updated based on the solution to the subproblem. The active set method is used to solve for the set of active constraints until the KKT optimality condition is met. Due to the small problem size, the active set method can usually converge within 1 to 5 iterations, with a solution time of approximately tens to hundreds of microseconds on a 1GHz processor, which can meet the robot control cycle requirements of up to several hundred hertz. In a specific numerical example: assume time There are 4 feature points, and 4 linear inequality constraints are obtained through S53 calculation. The initial point is... Upon inspection, two constraints were found to be unsatisfied (i.e., if moving at the nominal velocity, the two corresponding feature points would move outwards from the viewpoint boundary). The active set method activated these two constraints, resulting in a corrected solution to the subproblem. After two iterations, it converges and outputs... As a correction to the visual servo speed command, it can be seen that the correction mainly focuses on the speed components corresponding to feature points at risk of going out of bounds, while the other components remain close to their nominal values.
[0083] In another embodiment, S6 employs the interior-point method. The interior-point method transforms the inequality constraints into penalty terms of the objective function by introducing a logarithmic barrier term, thus converting the original constrained optimization problem into a series of unconstrained or equality-constrained subproblems. For scenarios with a large number of constraints (e.g., more than 20 feature points), the interior-point method outperforms the active-set method in terms of convergence stability, but its computational cost per iteration is slightly higher. For scenarios with a typical number of feature points (4 to 10), the performance difference between the two methods is not significant.
[0084] In another embodiment, S6 employs a small-scale QP solver specifically developed for embedded platforms, such as OSQP (Operator Splitting Quadratic Program). OSQP, based on operator splitting and first-order optimization methods, requires no matrix inversion or exact linear search, making it suitable for running on embedded microcontrollers without full operating system support. In this embodiment, S6 packages the Hessian matrix, gradient vector, constraint matrix, and upper constraint bounds of the QP problem into the OSQP data format, calls the OSQP solver interface, and extracts the optimal variable to be solved from the OSQP output structure after the solution is completed. The corrected visual servo speed command is obtained. It can be understood that, regardless of the specific solution algorithm used, the corrected visual servo speed command output by S6 satisfies all the linear inequality constraints constructed in step S5, thus ensuring that no feature point moves out of the field of view under the action of the speed command.
[0085] In one embodiment, when the quadratic programming problem has no feasible solution within the current control cycle—that is, no speed command can simultaneously satisfy all control obstacle function constraints—the following fallback strategy is adopted: First, slack variables are introduced to relax the constraints, modifying the original constraints into a softened form. ,in For the first Slack variables for constraints; adding penalty terms for slack variables to the objective function. ,in The pre-set large value penalty coefficient (e.g., a value of 1) to Solve the relaxed quadratic programming problem. If a feasible solution is found, it is used as the corrected visual servo speed command; if no feasible solution is found after relaxation (e.g., due to numerical ill-conditioning), the zero vector (i.e., ...) is used. The corrected visual servo speed command for the current control cycle pauses the robot's movement while maintaining its previous pose until the next control cycle. This fallback strategy eliminates the risk of unpredictable robot movements due to QP solution failure from a safety perspective.
[0086] S7, control the robot's movement with the corrected visual servo speed command.
[0087] Optional, see reference Figure 3 S7 includes, S71, based on the corrected visual servo speed command and the preset control cycle, integrate the corrected visual servo speed command to obtain the robot's expected pose increment within the current control cycle.
[0088] The preset control cycle refers to the fixed time interval between two adjacent visual servo control iterations. The preset control cycle is typically matched to the camera's frame rate; for example, when the camera continuously acquires data at a frame rate of 30 frames per second, the preset control cycle is 1 / 30 of a second (approximately 33.3 milliseconds). The preset control cycle determines the maximum duration of the robot's motion execution in each control loop and is a fundamental timing parameter of the entire visual servo closed-loop control system.
[0089] The current control cycle refers to the current complete control loop being executed during the continuous iteration of steps S1 to S7. In each control loop, the current control cycle begins with step S1 acquiring the current image and ends with step S7 driving the robot to move to the pose at the next moment. The starting time of the current control cycle is denoted as t, and its time span is the preset control cycle.
[0090] The expected pose increment refers to the expected change in pose of the robot in the end effector space as it moves from its current pose towards the target direction within the current control cycle. The expected pose increment is a 6-dimensional vector containing three position increments and three orientation increments, corresponding to translational motion along the robot's end effector coordinate system and rotational motion about each axis, respectively. Each component of the expected pose increment is obtained by multiplying the corresponding component of the corrected visual servo speed command with a preset control cycle, and its physical units are meters and radians, respectively.
[0091] In one embodiment, the preset control period is approximately 33.3 milliseconds, i.e. Seconds. The corrected visual servo speed command output in step S6 in S71. Time integration is performed using direct Euler integration: the desired pose increment for each component is equal to the product of the corresponding velocity component and the preset control period; that is, the position increment component is the linear velocity component multiplied by... The attitude increment component is the angular velocity component multiplied by .
[0092] In a specific numerical example: the visual servo speed command is corrected as follows: (Linear velocity is in meters per second, angular velocity is in radians per second), control cycle The desired pose increment is obtained after integration over seconds. That is, within the current control cycle, the desired path of the robot's end effector along the camera coordinate system. The axis shifts approximately 1.67 mm in the positive direction, along... The axis is translated 3.00 mm in the positive direction, accompanied by a small attitude adjustment. In another embodiment, when the camera frame rate is low (e.g., 15 frames per second) resulting in a long preset control period, trapezoidal integration is used instead of direct Euler integration to improve integration accuracy. Trapezoidal integration uses the average of the current corrected visual servo speed command and the previous corrected visual servo speed command multiplied by the preset control period as the desired pose increment, which can significantly reduce the trajectory drift introduced by Euler integration under long control periods.
[0093] S72, the desired pose increment is converted into joint angular displacement commands for each joint of the robot using the robot's inverse kinematics model.
[0094] Inverse kinematics (IK) refers to a set of computational rules or analytical formulas that map the target pose of a robot's end effector in Cartesian space to the target angle values of each joint. IK depends on the robot's specific configuration parameters (such as link lengths and joint axis directions). For a typical 6-DOF articulated robotic arm satisfying the Pieper criterion, the IK has a closed analytical expression; for configurations with redundant degrees of freedom or those not satisfying analytical solvability conditions, the IK needs to be implemented through numerical iteration.
[0095] Joint angular displacement commands refer to the target angular change that drives each joint of the robot to move from the current angular position to the next angular position within the current control cycle. A joint angular displacement command is a vector whose dimension is equal to the number of joints in the robot (e.g., 6-dimensional for a 6-axis robotic arm). Each component corresponds to the rotational drive of a joint, measured in degrees or radians. The joint angular displacement command is the equivalent representation of the desired pose increment in step S71 in joint space, determining the target direction and magnitude of the drive by each joint controller.
[0096] In one embodiment, the robot is a 6-DOF (DoF) articulated industrial robotic arm. S72 first superimposes the desired pose increment obtained in S71 onto the current end-effector pose to obtain the desired end-effector pose matrix for the next moment. Then, using the analytical inverse kinematics model of the robotic arm, the desired end-effector pose matrix is solved inversely to obtain the target angle values for each joint. The analytical inverse kinematics solution is based on the principle of minimizing the total joint angular displacement, selecting the set that minimizes the weighted sum of the absolute values of the angle changes of each joint from multiple feasible inverse solutions. The difference between the target angle value and the current angle value of each joint is the joint angular displacement command for that joint.
[0097] In one embodiment, the desired pose increment To obtain the six-dimensional pose increments (including three position increments and three attitude increments) of the robot's end effector in the camera coordinate system, it is necessary to map them into angular displacement increments of each driven joint through inverse kinematics. Specifically, the angle values of each joint of the robot at the current moment are first obtained. (For a six-axis serial robotic arm) Then, based on the current end-effector pose and the increment of the desired pose, the desired target end-effector pose is calculated, and the corresponding target joint angle vector is obtained through inverse kinematics. The final joint angular displacement command is In a specific numerical example: the current angle values of the six joints are... After combining forward and inverse kinematics, a set of optimal joint target angle values were obtained. Then the joint angle displacement command output by S72 is The joint angular displacement command is then sent to the servo drivers of each joint of the robot for execution, driving the robot to move towards the desired pose for one control cycle. It is understood that when the robot configuration has redundant degrees of freedom, the inverse kinematics solution may have infinitely many solutions. In this case, the optimal solution needs to be selected according to preset optimization criteria (such as minimum joint motion or singularity avoidance). The solution given in this embodiment... This is the optimal result under this optimization criterion.
[0098] In another embodiment, when the robot lacks an analytical inverse kinematics model, S72 employs a numerical iterative method based on the pseudo-inverse of the Jacobian matrix to solve the problem. Using the current joint angle as the initial value, the robot iteratively corrects the joint angle using the pseudo-inverse of the Jacobian matrix, causing the end-effector pose to successively approximate the target pose until the pose error is less than a preset convergence threshold (e.g., position error less than 0.1 mm, attitude error less than 0.01 radians). The change in joint angle after iterative convergence is then output as a joint angular displacement command.
[0099] S73, the joint angular displacement command is sent to the robot's underlying controller to drive the robot to move to the pose at the next moment.
[0100] The underlying controller refers to the robot's joint-level servo drive system, typically composed of independent servo drivers for each joint and corresponding servo motors. The underlying controller receives joint angular displacement commands as reference inputs and drives the motors through its internal closed-loop control circuit, enabling the joint's actual position to track the target angle. The control cycle of the underlying controller is usually much shorter than the control cycle of visual servoing (e.g., a servo control cycle of 1 millisecond), allowing it to complete the tracking and execution of joint angular displacement commands within one visual servoing control cycle.
[0101] The pose at the next moment refers to the new position and orientation of the robot's end effector in Cartesian space at the end of the current control cycle. The pose at the next moment is the result of superimposing the current pose with the desired pose increment. Under this new pose, the camera's position and orientation relative to the target change, and the image acquisition phase of the next control cycle begins.
[0102] In one embodiment, the underlying controller is a position-mode servo driver for each joint of the robot. S73 sends the angular displacement commands for each joint output by S72 to the corresponding servo driver via the EtherCAT real-time communication bus. Upon receiving the target angular displacement, each servo driver drives the servo motor to move the joint to the target angular position. In another embodiment, when the robot is running in speed mode, S73 divides the joint angular displacement command by the underlying control cycle to convert it into a joint angular velocity command before sending it.
[0103] In one embodiment, S73 performs a limiting process before sending the joint angular displacement command: if the angular displacement command of any joint exceeds the maximum allowable displacement of the joint in the current control cycle (determined by the product of the joint's speed limit and the preset control cycle), then the angular displacement commands of all joints are scaled proportionally so that the angular displacement of all joints does not exceed the limit, thus ensuring the mechanical safety of the movement.
[0104] After the motion is completed, the robot end effector reaches the pose of the next moment, and the camera enters a new control cycle. At the beginning of the new control cycle, the camera acquires the next frame of the current image, and the method returns to step S1. Steps S1 to S7 are then re-executed using the newly acquired current image as input, forming a closed-loop iterative control.
[0105] refer to Figure 4 The present invention provides a robot vision control system, comprising: An extraction module is used to acquire the current image captured by a camera installed on the robot, and extract the current image coordinates of at least one feature point from the current image; The calculation module is used to calculate the image feature error based on the current image coordinates and the corresponding expected image coordinates of the at least one feature point; The generation module is used to generate a nominal visual servo speed command based on the image feature error and a combined interaction matrix composed of the interaction matrices of the at least one feature point; wherein the nominal visual servo speed command is a robot speed command that drives the image feature error to converge to zero under the condition of no field of view constraint. A construction module is configured to construct at least one control obstacle function for the at least one feature point based on the field of view boundary of the camera; wherein the function value of the control obstacle function reflects the distance between the corresponding feature point and the field of view boundary of the camera; The construction module is further configured to construct a quadratic programming problem based on the nominal visual servo speed command and constrained by at least one control obstacle function; the objective function of the quadratic programming problem is to minimize the deviation between the variable to be solved in the quadratic programming problem and the nominal visual servo speed command; the constraint condition of the quadratic programming problem is that the sum of the time derivative of each control obstacle function under the action of the variable to be solved and the product of the current function value of the control obstacle function and the corresponding preset positive coefficient is greater than or equal to zero; The solver module is used to solve the quadratic programming problem and obtain the corrected visual servo speed command; A control module is used to control the movement of the robot using the corrected visual servo speed command.
[0106] The preferred embodiments of the present invention disclosed above are merely illustrative of the invention. These preferred embodiments do not exhaustively describe all details, nor do they limit the invention to the specific implementations described. Clearly, many modifications and variations can be made based on the content of this specification. This specification selects and specifically describes these embodiments to better explain the principles and practical applications of the invention, thereby enabling those skilled in the art to better understand and utilize the invention. The invention is limited only by the claims and their full scope and equivalents.
Claims
1. A robot vision control method, characterized in that, include: S1, acquire the current image captured by the camera installed on the robot, and extract the current image coordinates of at least one feature point from the current image; S2, calculate the image feature error based on the current image coordinates and the corresponding expected image coordinates of the at least one feature point; S3, based on the image feature error and the combined interaction matrix composed of the interaction matrices of the at least one feature point, a nominal visual servo speed command is generated; wherein, the nominal visual servo speed command is a robot speed command that drives the image feature error to converge to zero under the condition of no field of view constraint. S4, based on the field of view boundary of the camera, construct at least one control obstacle function for the at least one feature point; wherein, the function value of the control obstacle function reflects the distance between the corresponding feature point and the field of view boundary of the camera; S5, using the nominal visual servo speed command as a reference and the at least one control obstacle function as a constraint, construct a quadratic programming problem; the objective function of the quadratic programming problem is to minimize the deviation between the variable to be solved in the quadratic programming problem and the nominal visual servo speed command; the constraint condition of the quadratic programming problem is: the sum of the time derivative of each control obstacle function under the action of the variable to be solved and the product of the current function value of the control obstacle function and the corresponding preset positive coefficient is greater than or equal to zero; S6, Solve the quadratic programming problem to obtain the corrected visual servo speed command; S7, control the robot's movement with the corrected visual servo speed command.
2. The robot vision control method according to claim 1, characterized in that, The process of extracting the current image coordinates includes: S11, acquire the current image captured by the camera, and extract candidate feature points from the current image using a feature detection algorithm; S12, perform feature matching between the candidate feature points and the expected feature points corresponding to the expected image coordinates to obtain the matching feature points; S13, a random sampling consensus algorithm is used to remove mismatched points from the matching feature points, and the remaining matching feature points after removing the mismatched points are taken as the at least one feature point, and the current image coordinates of the at least one feature point are obtained.
3. The robot vision control method according to claim 1, characterized in that, The image feature error is calculated using the following formula: ; in, The image feature error at time t, The vector formed by the current image coordinates of the at least one feature point at time t. The vector formed by the corresponding expected image coordinates.
4. The robot vision control method according to claim 3, characterized in that, The nominal visual servo speed command is generated by the following formula: ; in, The nominal visual servo speed command at time t. To preset the positive constant gain, The pseudo-inverse of the interaction matrix is... The image feature error, The vector formed by the current image coordinates of the at least one feature point at time t. The depth value of the at least one feature point.
5. The robot vision control method according to claim 1, characterized in that, The field of view boundary of the camera is a convex polygon region defined in the image coordinate space, and the control obstacle function is a function that reflects the signed distance between the corresponding feature point and each edge of the convex polygon region; in S4, for any feature point, the minimum value of the signed distance between the feature point and each edge of the convex polygon region is taken as the function value of the control obstacle function corresponding to the feature point.
6. The robot vision control method according to claim 1, characterized in that, The construction of the quadratic programming problem includes, S51, the variable to be solved is set to a velocity variable v with the same dimension as the nominal visual servo speed command; S52, to minimize As the objective function, where The nominal visual servo speed command at time t; S53, for each of the control barrier functions, calculate its partial derivative with respect to the current image coordinates of the at least one feature point, and use the interaction matrix to convert the partial derivative into a gradient with respect to the variable to be solved v, thereby expressing the constraint as a linear inequality constraint with respect to v. S54, combine the objective function with all the linear inequality constraints to form the quadratic programming problem.
7. The robot vision control method according to claim 6, characterized in that, S52 Using the weighted L2 norm form: ; in, It is a preset positive definite weight matrix used to apply different correction cost weights to each velocity component.
8. A robot vision control method according to claim 6, characterized in that, In S53, for any i-th control barrier function The linear inequality constraints are expressed as follows: ; in, The current image coordinates of the i-th control barrier function relative to the i-th feature point. The partial derivative vector, The interaction matrix of the i-th feature point. Let be the depth value of the i-th feature point. Let be the variable to be solved. The preset positive coefficient, This is the current function value of the i-th control barrier function.
9. The robot vision control method according to claim 1, characterized in that, S7 includes, S71, Based on the corrected visual servo speed command and the preset control cycle, the corrected visual servo speed command is integrated to obtain the robot's expected pose increment in the current control cycle. S72, the desired pose increment is converted into joint angular displacement commands for each joint of the robot using the inverse kinematics model of the robot; S73, the joint angular displacement command is sent to the robot's underlying controller to drive the robot to move to the pose at the next moment.
10. A robot vision control system, characterized in that, include: An extraction module is used to acquire the current image captured by a camera installed on the robot, and extract the current image coordinates of at least one feature point from the current image; The calculation module is used to calculate the image feature error based on the current image coordinates and the corresponding expected image coordinates of the at least one feature point; The generation module is used to generate a nominal visual servo speed command based on the image feature error and a combined interaction matrix composed of the interaction matrices of the at least one feature point; wherein the nominal visual servo speed command is a robot speed command that drives the image feature error to converge to zero under the condition of no field of view constraint. A construction module is configured to construct at least one control obstacle function for the at least one feature point based on the field of view boundary of the camera; wherein the function value of the control obstacle function reflects the distance between the corresponding feature point and the field of view boundary of the camera; The construction module is further configured to construct a quadratic programming problem based on the nominal visual servo speed command and constrained by at least one control obstacle function; the objective function of the quadratic programming problem is to minimize the deviation between the variable to be solved in the quadratic programming problem and the nominal visual servo speed command; the constraint condition of the quadratic programming problem is that the sum of the time derivative of each control obstacle function under the action of the variable to be solved and the product of the current function value of the control obstacle function and the corresponding preset positive coefficient is greater than or equal to zero; The solver module is used to solve the quadratic programming problem and obtain the corrected visual servo speed command; A control module is used to control the movement of the robot using the corrected visual servo speed command.
