Visual servo-based robotic arm pose control system
By using dual-color channel composite structured light marking and dynamic gain adjustment, the problems of incomplete normal field reconstruction and poor dynamic response adaptability under complex curved surface conditions in the visual servo control of robotic arms were solved, achieving higher pose control accuracy and dynamic performance.
Patent Information
- Application Number
- CN202611575372.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-10-12
- Publication Date
- 2026-11-17
AI Technical Summary
Existing visual servo control methods for robotic arms struggle to reconstruct complete surface normal vector fields and exhibit poor dynamic response adaptability under complex curved surface conditions, resulting in limitations in end-effector pose control accuracy and dynamic performance.
By employing dual-color channel composite structured light markers, the local normal vector field is reconstructed through the projection of sinusoidal distributed grating stripes and rectangular grid arrays. The velocity feedforward gain is dynamically adjusted according to the drift rate of feature points, thereby improving the pose control accuracy and dynamic response of the robotic arm end effector.
It improves the pose control accuracy and dynamic response performance of the robotic arm end effector under complex curved surface conditions, reduces the noise impact caused by incomplete normal vector field reconstruction and fixed gain, and enhances the speed and stability of the system.
Smart Images

Figure CN123103686A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of visual servo control technology for robotic arms, specifically a posture control system for robotic arms based on visual servoing. Background Technology
[0002] In visual servo control of robotic arms, binocular cameras are often used to acquire workpiece surface markings or projected structured light patterns. The pose deviation of the robotic arm's end effector is calculated based on changes in image features, and motion compensation is generated accordingly. For target workpieces with complex surface topography, it is necessary to obtain the local normal vector field to determine the end effector pose deviation. Existing methods often use grating stripes or speckle patterns with a single color channel for surface measurement, recovering the surface gradient through stripe deformation or speckle displacement. Some methods rely on workpiece surface markings or edge features for spatial positioning. The velocity feedforward gain in the visual servo control loop is usually set to a fixed value based on experience, remaining constant as the robotic arm's end effector speed changes. With a single structured light pattern, when the curvature of the target workpiece surface varies greatly or there is local occlusion, grating stripes are prone to aliasing and breakage, speckle feature matching stability decreases, and grid corner detection is prone to missing points, resulting in incomplete reconstruction of the local normal vector field and significant noise impact on the pose deviation calculation results. Using workpiece surface markings or edge features yields insufficient features in areas with weak texture or drastic surface curvature changes, leading to large spatial positioning errors. Fixed-gain visual servo control struggles to simultaneously meet the requirements of fast response and stability when image feature drift rates change significantly, easily leading to end-effector jitter, slow convergence, or overshoot. These issues limit the pose control accuracy and dynamic response adaptability of robotic arms under complex curved surface conditions. It is necessary to obtain more complete surface normal field reconstruction information without adding additional marking hardware, and to match the visual servo feedforward gain with the image feature motion speed. Summary of the Invention
[0003] This invention aims to provide a vision servo-based robotic arm pose control system. By projecting a composite structured light marker with a first color channel and a second color channel, the system obtains the curvature of the grating stripes and the offset of the grid nodes to reconstruct the local normal vector field. The system also dynamically adjusts the velocity feedforward gain according to the drift rate of the feature points of the composite structured light marker. This addresses the problems of incomplete normal vector field reconstruction under complex curved surfaces and poor dynamic response adaptability caused by fixed gain, thereby improving the pose control accuracy and dynamic performance of the robotic arm end effector.
[0004] To achieve the above objectives, the present invention provides the following technical solution: The present invention provides a visual servo-based robotic arm posture control system, including a spatial projection marking module, an image synchronous acquisition module, a projection distortion correction module, a posture deviation calculation module, a servo drive control module, and a dynamic response adjustment module. The spatial projection marking module, responding to the current posture parameters of the robotic arm's end effector, controls a laser projector to project a composite structured light mark with a first color channel and a second color channel onto the surface of the target workpiece, providing a stable and identifiable spatial feature reference for visual posture measurement; the image synchronous acquisition module triggers a binocular camera to synchronously acquire left-view and right-view images carrying the composite structured light mark according to a preset sampling frequency, ensuring the temporal consistency of the two images; the projection distortion correction module reconstructs the local normal vector field of the target workpiece surface based on the curvature of the grating fringes of the first color channel and the grid node offset of the second color channel in the left-view and right-view images, achieving an accurate description of the local geometric shape of the workpiece surface; the posture deviation calculation module… Based on the spatial mapping relationship between the local normal vector field and the pre-stored standard workpiece model normal vector field, the module calculates the rotational deviation quaternion and translational deviation vector of the robotic arm end effector relative to the desired pose, providing real-time deviation information for pose correction. The servo drive control module converts the rotational deviation quaternion and translational deviation vector into the angular velocity compensation of each joint of the robotic arm, and superimposes the angular velocity compensation onto the velocity feedforward term of the position closed-loop controller to improve the speed and smoothness of end effector pose correction. The dynamic response adjustment module monitors the drift rate of feature points of the composite structured light markers in the left and right view images, and adjusts the gain coefficient of the velocity feedforward term according to the feature point drift rate, so that the system maintains a suitable dynamic response when the end effector motion state changes.
[0005] As a technical solution of the present invention, the spatial projection marking module includes a first projection channel submodule, a second projection channel submodule, and a synchronization triggering submodule. The first projection channel submodule configures the first color channel in the composite structured light mark as sinusoidal distributed grating fringes extending laterally along the surface of the target workpiece; the second projection channel submodule configures the second color channel in the composite structured light mark as a rectangular grid array superimposed at the peak positions of the sinusoidal distributed grating fringes; the synchronization triggering submodule calculates the projection focal length adjustment of the laser projector based on the spatial coordinate values in the current pose parameters of the robotic arm end effector, and drives the laser projector to zoom and project according to the projection focal length adjustment. Through the configuration of the dual-channel composite structured light mark and the synchronous adjustment of the projection focal length, it is possible to ensure that the mark has clear imaging quality and sufficient spatial feature density under different distances and local curvature conditions.
[0006] Preferably, the spatial frequency of the sinusoidal grating fringes adaptively adjusts to changes in the local curvature of the target workpiece surface. Preferably, the area of each grid cell in the rectangular grid array increases proportionally with the projection distance of the laser projector. This method avoids excessively sparse markings or excessively dense grids due to changes in workpiece surface curvature or increased projection distance, thus improving the stability of subsequent distortion extraction.
[0007] As a technical solution of the present invention, the image synchronization acquisition module includes a left camera trigger submodule, a right camera trigger submodule, and a frame synchronization verification submodule. The left camera trigger submodule generates a left camera exposure pulse according to a preset sampling frequency, driving the left camera in the binocular camera to acquire a left-view image. The right camera trigger submodule generates a right camera exposure pulse with the same phase as the left camera exposure pulse according to a preset sampling frequency, driving the right camera in the binocular camera to acquire a right-view image. The frame synchronization verification submodule reads the timestamp information of the left and right view images, calculates the time interval between the timestamps, and pairs the left and right view images as a synchronized image pair when the time interval is less than or equal to an allowable deviation threshold. This effectively avoids pose calculation errors caused by time mismatch between the left and right images.
[0008] As a technical solution of the present invention, the projection distortion correction module includes a first color separation submodule, a grating distortion extraction submodule, a mesh offset extraction submodule, and a normal vector reconstruction submodule. The first color separation submodule performs color channel separation operations on the left-view image and the right-view image respectively, extracting the first monochrome image corresponding to the first color channel and the second monochrome image corresponding to the second color channel. The grating distortion extraction submodule performs grayscale centroid extraction on the first monochrome image to extract the center line of the grating stripes, and calculates the curvature of the grating stripes based on the offset of the center line of the grating stripes from the standard horizontal baseline. The mesh offset extraction submodule performs corner detection operations on the second monochrome image to extract the corner points of each grid in the rectangular grid array, and calculates the grid node offset based on the offset vector of each grid corner point relative to the reference position of the standard rectangular grid. The normal vector reconstruction submodule substitutes the grating stripe curvature and the grid node offset into a preset projection geometry mapping equation, solves for the normal vector tilt angle value corresponding to each pixel on the target workpiece surface, and combines the normal vector tilt angle values into a local normal vector field. By utilizing the dual distortion information from grating fringes and grid nodes, the robustness and spatial resolution of local normal field reconstruction can be improved.
[0009] As a technical solution of the present invention, the pose deviation calculation module includes a model normal vector extraction submodule, a principal component alignment submodule, a quaternion transformation submodule, and a translation deviation calculation submodule. The model normal vector extraction submodule is used to extract the local region normal vector field corresponding to the spatial position of the local normal vector field from the pre-stored standard workpiece model normal vector field. The principal component alignment submodule is used to perform principal component analysis on the local normal vector field and the local region normal vector field respectively, obtaining the first principal direction vector of the local normal vector field and the second principal direction vector of the local region normal vector field. The quaternion transformation submodule is used to calculate the rotation axis and rotation angle that rotate the first principal direction vector to the second principal direction vector based on the directional difference between the first and second principal direction vectors, and convert the rotation axis and rotation angle into rotation deviation quaternions. The translation deviation calculation submodule is used to calculate the translation deviation vector based on the coordinate difference between the centroid coordinates of the local normal vector field and the centroid coordinates of the local region normal vector field. Through principal component alignment and centroid difference calculation, rotation deviation and translation deviation can be obtained quickly analytically, improving the real-time performance of visual servoing.
[0010] As a technical solution of the present invention, the servo drive control module includes a Jacobi mapping submodule, a rotation mapping submodule, a velocity superposition submodule, and a feedforward injection submodule. The Jacobi mapping submodule calculates the Jacobi matrix of the robotic arm based on the current joint angle values of each joint, and maps the translational deviation vector to the first angular velocity compensation component of each joint through the pseudo-inverse of the Jacobi matrix. The rotation mapping submodule converts the rotational deviation quaternion into an equivalent rotational vector, and maps the equivalent rotational vector to the second angular velocity compensation component of each joint through the pseudo-inverse of the Jacobi matrix. The velocity superposition submodule adds the first and second angular velocity compensation components of each joint to obtain the angular velocity compensation amount of each joint. The feedforward injection submodule injects the angular velocity compensation amount of each joint into the velocity loop input of the position closed-loop controller in the form of a velocity feedforward signal. This allows visual deviation to be directly converted into velocity compensation in the joint space, reducing position loop lag and improving trajectory tracking accuracy.
[0011] As a technical solution of the present invention, the dynamic response adjustment module includes a feature point matching submodule, a drift rate calculation submodule, a gain adjustment submodule, and a gain update submodule. The feature point matching submodule performs nearest neighbor matching on each grid corner point of the rectangular grid array in two consecutive left-view images to obtain the pixel drift vector of each grid corner point. The drift rate calculation submodule divides the magnitude of each pixel drift vector by the time interval between two adjacent frames to obtain the instantaneous drift rate of each grid corner point, and averages the instantaneous drift rates of all grid corner points to obtain the feature point drift rate. The gain adjustment submodule, based on the magnitude of the feature point drift rate, looks up the gain adjustment coefficient of the velocity feedforward term according to a preset gain mapping relationship, and multiplies the gain adjustment coefficient by the original gain value of the velocity feedforward term to obtain the updated gain value. The gain update submodule writes the updated gain value into the velocity feedforward gain register of the position closed-loop controller to complete the dynamic adjustment of the gain coefficient. Dynamically adjusting the feedforward gain according to the feature point drift rate enhances the response capability when the end-effector is moving rapidly, and avoids overshoot and vibration when the end-effector is moving smoothly.
[0012] Preferably, the gain mapping relationship is a piecewise linear mapping relationship between the feature point drift rate and the gain adjustment coefficient, and the gain adjustment coefficient increases monotonically with the increase of the feature point drift rate, so that the feedforward gain increases accordingly when the drift rate is large and decreases accordingly when the drift rate is small, thus taking into account both tracking accuracy and system stability.
[0013] The technical effects and advantages provided by the present invention in the above technical solution are as follows: A dual-color-channel composite structured light marking method is employed. The first color channel is configured as sinusoidal distributed grating fringes, and the second color channel superimposes a rectangular grid array at the peak positions of the sinusoidal distributed grating fringes. The curvature of the grating fringes reflects the continuous gradient change along the fringe direction on the surface of the target workpiece, and the grid node offset provides local discrete geometric constraints. In the projection distortion correction module, the center line of the grating fringes is extracted from the first monochrome image, and the offset from the standard horizontal baseline is calculated. The offset vector of the grid corner points relative to the standard rectangular grid reference position is extracted from the second monochrome image. The grating fringes curvature and grid node offset are substituted into the projection geometric mapping equation to solve for the normal vector tilt angle value of each pixel on the surface of the target workpiece, forming a local normal vector field. Since the first and second color channels are separated in the spectrum, the color separation submodule can independently extract monochrome images, reducing the interference of inter-channel crosstalk on the extraction of the grating fringes center line and grid corner point positions. Compared to single-grating or single-grid projection, this dual-channel information complementarity method reduces the loss of normal vector information caused by fringe aliasing and corner point omissions under conditions of large surface curvature variations or local occlusion, thus improving the integrity and accuracy of the local normal vector field. The pose deviation calculation module calculates rotational deviation quaternions and translational deviation vectors based on the differences in principal component directions and centroid coordinates between the local normal vector field and the standard workpiece model's normal vector field. This reduces pose deviation calculation errors under complex surface conditions and improves the accuracy of the robotic arm's end-effector posture correction.
[0014] The dynamic response adjustment module performs nearest neighbor matching on each corner point of the rectangular grid array in two consecutive left-view images to obtain pixel drift vectors. Based on the magnitude of the pixel drift vectors and the time interval between adjacent frames, it calculates the feature point drift rate. According to a preset gain mapping relationship, it looks up the gain adjustment coefficient of the velocity feedforward term in a table and updates the velocity feedforward gain register of the position closed-loop controller. The gain mapping relationship uses a piecewise linear mapping, with the gain adjustment coefficient monotonically increasing as the feature point drift rate increases. This increases the compensation capability of the feedforward gain when the feature motion accelerates and decreases the compensation amount when the feature motion slows down, adapting to the servo control requirements at different stages. The velocity feedforward gain is dynamically adjusted with the feature point drift rate. When changes in the end-effector's motion speed cause changes in the image feature motion speed, the feedforward gain can match the current motion state, avoiding insufficient response of fixed gain at high speeds and end-effector jitter caused by overcompensation during low-speed approach, thus improving the dynamic response stability and convergence speed of visual servo control. By superimposing the joint angular velocity compensation obtained by converting the rotational deviation quaternion and translational deviation vector into the velocity feedforward term of the position closed-loop controller, the pose deviation can be compensated in advance at the joint velocity level, reducing the impact of visual measurement delay on the control loop and further improving the dynamic tracking accuracy. Attached Figure Description
[0015] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments recorded in this invention. For those skilled in the art, other drawings can be obtained based on these drawings.
[0016] Figure 1 This is a schematic diagram of a vision servo-based robotic arm pose control system. Figure 2 This is a schematic diagram of the pose deviation calculation module. Figure 3 This is a schematic diagram of the exposure pulse time interval distribution and allowable deviation threshold of a binocular camera; Figure 4 This is a graph showing the relationship between the feature point drift rate and the velocity feedforward gain adjustment. Detailed Implementation
[0017] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, 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, 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.
[0018] See Figure 1 The present invention provides a vision servo-based robotic arm posture control system, including a spatial projection marking module, an image synchronous acquisition module, a projection distortion correction module, a posture deviation calculation module, a servo drive control module, and a dynamic response adjustment module.
[0019] The spatial projection marking module responds to the current pose parameters of the robotic arm end effector and controls the laser projector to project a composite structured light mark with a first color channel and a second color channel onto the surface of the target workpiece. The first color channel corresponds to grating stripes, and the second color channel corresponds to grid marks.
[0020] The image synchronous acquisition module triggers the binocular camera to synchronously acquire left-view and right-view images carrying composite structured light markers according to a preset sampling frequency. Both the left-view and right-view images contain two-dimensional projection information of the composite structured light markers.
[0021] The projection distortion correction module reconstructs the local normal vector field of the target workpiece surface based on the curvature of the grating stripes of the first color channel and the offset of the grid nodes of the second color channel in the left and right view images. Each element in the local normal vector field corresponds to the normal vector tilt angle value at a pixel on the target workpiece surface.
[0022] The pose deviation calculation module calculates the rotational deviation quaternion and translational deviation vector of the robotic arm end relative to the desired pose based on the spatial mapping relationship between the local normal vector field and the pre-stored standard workpiece model normal vector field.
[0023] The servo drive control module converts the rotational deviation quaternion and translational deviation vector into the angular velocity compensation of each joint of the robotic arm, and then adds the angular velocity compensation to the velocity feedforward term of the position closed-loop controller.
[0024] The dynamic response adjustment module monitors the drift rate of feature points of the composite structured light marker in the left and right view images, and adjusts the gain coefficient of the velocity feedforward term according to the drift rate of the feature points.
[0025] In practical implementation, the spatial projection marking module responds to the current pose parameters of the robotic arm's end effector and controls the laser projector to project a composite structured light mark with a first color channel and a second color channel onto the surface of the target workpiece. The spatial projection marking module includes a first projection channel submodule, a second projection channel submodule, and a synchronization triggering submodule.
[0026] In specific implementation, the first projection channel submodule configures the first color channel of the composite structured light mark as sinusoidal distributed grating stripes extending laterally along the surface of the target workpiece. The first projection channel submodule acquires the sinusoidal distributed grating stripe pattern data. The grayscale value of each row of pixels in the sinusoidal distributed grating stripe pattern data varies according to a sine function. The phase of the sine function increases linearly along the lateral direction of the target workpiece surface, and the amplitude of the sine function corresponds to the contrast of the stripes. The first projection channel submodule writes the sinusoidal distributed grating stripe pattern data into the first color channel display buffer of the laser projector. The first color channel corresponds to the red projection channel of the laser projector. The laser projector reads the sinusoidal distributed grating stripe pattern data from the first color channel display buffer and projects red sinusoidal distributed grating stripes onto the surface of the target workpiece through the red projection channel.
[0027] The spatial frequency of the sinusoidal grating fringes adaptively adjusts to changes in the local curvature of the target workpiece surface. The first projection channel submodule acquires local curvature distribution data of the target workpiece surface, where each element represents the curvature value at the corresponding location on the surface. Based on a pre-stored mapping table between local curvature and spatial frequency, the first projection channel submodule maps each curvature value to a spatial frequency value, generating a fringe pattern with variable spatial frequency. In areas with higher local curvature, the mapped spatial frequency value is higher; in areas with lower local curvature, the mapped spatial frequency value is lower. The first projection channel submodule adjusts the period parameter of the sine function in the sinusoidal grating fringe pattern data according to the spatial frequency value.
[0028] In specific implementation, the second projection channel submodule configures the second color channel of the composite structured light mark as a rectangular grid array superimposed at the peak positions of the sinusoidal grating stripes. The second projection channel submodule iterates through the extreme gray values of each row in the sinusoidal grating stripe pattern data, extracting the row coordinates of each peak position. The second projection channel submodule generates horizontal grid lines of the rectangular grid array at the row coordinates of each peak position, and generates vertical grid lines along the longitudinal direction of the target workpiece surface at a reference spacing between the horizontal grid lines. The spacing of the vertical grid lines is the same as the spacing of the horizontal grid lines of the rectangular grid array, forming a rectangular grid array. The second projection channel submodule writes the rectangular grid array pattern data into the second color channel display buffer of the laser projector, the second color channel corresponding to the blue projection channel of the laser projector. The laser projector reads the rectangular grid array pattern data from the second color channel display buffer and projects a blue rectangular grid array onto the target workpiece surface through the blue projection channel. The blue rectangular grid array and the red sinusoidal grating stripes are superimposed on the target workpiece surface to form a composite structured light mark.
[0029] In a rectangular grid array, the area of each grid cell increases proportionally with the projection distance of the laser projector. The second projection channel submodule stores the side length values of the reference grid cells at the reference projection distance. When the projection distance of the laser projector changes, the second projection channel submodule calculates the ratio of the current projection distance to the reference projection distance, multiplies the side length of each grid cell in the rectangular grid array by the projection distance ratio to obtain the updated side length of the grid cells, and regenerates the rectangular grid array pattern data based on the updated side lengths. Since the area of each grid cell in the rectangular grid array changes with the square of the side length of the grid cell, the area of each grid cell in the rectangular grid array increases proportionally with the projection distance of the laser projector.
[0030] In practical implementation, the synchronization triggering submodule calculates the projection focal length adjustment of the laser projector based on the spatial coordinate values in the current pose parameters of the robotic arm's end effector, and drives the laser projector to zoom and project according to the focal length adjustment. The synchronization triggering submodule obtains the spatial coordinate values from the current pose parameters of the robotic arm's end effector from the robotic arm controller. These spatial coordinate values contain three-dimensional coordinate components. The synchronization triggering submodule extracts the Z-axis component from the three-dimensional coordinate components, denoted as... The unit is millimeters. The synchronous triggering submodule reads the Z-axis reference coordinate value of the target workpiece surface in the robot base coordinate system. Read the focal length of the laser projector at the reference projection distance. Read the reference projection distance between the laser projector and the target workpiece surface under the reference pose of the robotic arm end effector. The synchronous triggering submodule calculates the projection focal length adjustment amount according to the projection focal length adjustment formula. :
[0031] in, This indicates the amount of focal length adjustment, in millimeters. A positive value indicates that the focal length of the laser projector has increased, while a negative value indicates that the focal length of the laser projector has decreased. This indicates the focal length of the laser projector at the reference projection distance, which is determined by the laser projector's optical parameter manual and is in millimeters. In this embodiment, it is set to 50 millimeters, based on the nominal focal length of 50 millimeters corresponding to the laser projector's product specification at the reference projection distance of 500 millimeters. The Z-axis component of the spatial coordinates in the current pose parameters of the robotic arm's end effector is provided in real time by the robotic arm controller and is expressed in millimeters. The Z-axis reference coordinate value of the target workpiece surface in the robot base coordinate system is determined by the offline calibration process and stored in the storage unit of the synchronous trigger submodule, with the unit being millimeters; The reference projection distance between the laser projector at the reference position of the robotic arm end effector and the surface of the target workpiece is determined by the offline calibration process and stored in the storage unit of the synchronous trigger submodule, and the unit is millimeters. This represents the change in distance between the end effector of the robotic arm and the surface of the target workpiece along the Z-axis of the robot's base coordinate system. When the value is positive, the end effector of the robotic arm moves away from the surface of the target workpiece, the projection distance of the laser projector increases, and the projection focus adjustment is a positive value; when... When the value is negative, the end of the robotic arm approaches the surface of the target workpiece, the projection distance of the laser projector decreases, and the projection focal length adjustment is negative.
[0032] The synchronous trigger submodule sends the projection focal length adjustment amount to the zoom control unit of the laser projector. The zoom control unit adjusts the position of the focusing lens of the laser projector according to the projection focal length adjustment amount, thus completing the zoom projection of the laser projector. When the projection focal length adjustment amount is positive, the zoom control unit increases the focal length of the laser projector; when the projection focal length adjustment amount is negative, the zoom control unit decreases the focal length of the laser projector.
[0033] The spatial projection marking module, through the cooperation of the first projection channel submodule, the second projection channel submodule, and the synchronous triggering submodule, projects a composite structured light mark with a first color channel and a second color channel onto the surface of the target workpiece. The first color channel forms sinusoidal distributed grating stripes, and the second color channel forms a rectangular grid array. The spatial frequency of the sinusoidal distributed grating stripes is adaptively adjusted according to the local curvature, and the area of the grid unit of the rectangular grid array is proportionally enlarged as the projection distance changes.
[0034] In practical implementation, the image synchronization acquisition module triggers the binocular cameras to simultaneously acquire left-view and right-view images carrying composite structured light markers according to a preset sampling frequency. The image synchronization acquisition module includes a left-camera triggering submodule, a right-camera triggering submodule, and a frame synchronization verification submodule.
[0035] In implementation, the left camera trigger submodule generates a left camera exposure pulse based on a preset sampling frequency, driving the left camera in the binocular camera system to acquire a left-view image. The left camera trigger submodule internally has a preset sampling frequency register. The preset sampling frequency is determined by an external controller based on the visual servo task requirements and written to the left camera trigger submodule's preset sampling frequency register via a communication interface. The left camera trigger submodule also has an internal counter, which counts using a reference clock signal generated by a crystal oscillator, which is the same clock source shared by both the left and right camera trigger submodules. The left camera trigger submodule reads the preset sampling frequency value from the preset sampling frequency register, divides the reference clock signal frequency by the preset sampling frequency value, and obtains the left camera counting threshold. The counter in the left camera trigger submodule increments by one for each reference clock signal cycle. When the counter value reaches the left camera counting threshold, the left camera trigger submodule generates a left camera exposure pulse and simultaneously resets the counter value to zero, restarting the counting process. The left camera exposure pulse is output to the hardware trigger input port of the left camera. The left camera begins exposure at the rising edge of the left camera exposure pulse, acquiring a frame of a left-view image.
[0036] In implementation, the right camera trigger submodule generates a right camera exposure pulse with the same phase as the left camera exposure pulse based on a preset sampling frequency, driving the right camera in the binocular camera system to acquire a right-view image. The right camera trigger submodule internally has a preset sampling frequency register. The preset sampling frequency is written to this register by an external controller, and the same preset sampling frequency value is written to the right camera trigger submodule's register as the left camera trigger submodule's register. The right camera trigger submodule also has an internal counter, which uses the same reference clock signal as the left camera trigger submodule's counter. The right camera trigger submodule reads the preset sampling frequency value from its register, divides the reference clock signal frequency by the preset sampling frequency value, and obtains the right camera counting threshold. The counter increments by one for each reference clock signal cycle. When the counter reaches the right camera counting threshold, the right camera trigger submodule generates a right camera exposure pulse and simultaneously resets the counter to zero, restarting the counting process. The image synchronization acquisition module internally includes a synchronization reset signal. This signal is connected to the counters of both the left and right camera trigger submodules. When the synchronization reset signal is active, the counter values of both submodules are simultaneously reset to zero. The counters of both submodules begin counting simultaneously after the synchronization reset signal ends, and since the left and right camera counting thresholds are equal, the rising edges of the left and right camera exposure pulses occur within the same reference clock cycle, resulting in synchronized phase. The right camera exposure pulse is output to its hardware trigger input port, and the right camera begins exposure on the rising edge of its pulse, acquiring a frame of right-view image. The exposure start times of the left and right cameras are aligned.
[0037] In practical implementation, the frame synchronization verification submodule reads the timestamp information of the left-view and right-view images, calculates the time interval between the timestamps, and pairs the left-view and right-view images as a synchronized image pair when the time interval is less than or equal to the allowable deviation threshold. When the left and right cameras acquire images, the timestamp generation unit inside the image synchronization acquisition module writes the timestamp information of the left-view image to the corresponding left-view image data frame header when the left camera's exposure pulse is generated, and writes the timestamp information of the right-view image to the corresponding right-view image data frame header when the right camera's exposure pulse is generated. The clock source used by the timestamp generation unit is the same as the reference clock signal used by the left-camera triggering submodule and the right-camera triggering submodule. The frame synchronization verification submodule reads the timestamp information of the left-view image from the left-view image data frame header and the timestamp information of the right-view image from the right-view image data frame header. The frame synchronization verification submodule calculates the time interval between the timestamp information of the left-view image and the timestamp information of the right-view image. The formula for calculating the time interval is as follows:
[0038] in, The time interval between the timestamp information of the left-view image and the timestamp information of the right-view image is expressed in microseconds. The timestamp information representing the left-view image is written into the left-view image data frame header by the timestamp generation unit when the left camera exposure pulse is generated, and the unit is microseconds. The timestamp information representing the right-view image is written into the right-view image data frame header by the timestamp generation unit when the right camera exposure pulse is generated, and the unit is microseconds.
[0039] The frame synchronization verification submodule internally includes a tolerance deviation threshold register, which stores the tolerance deviation threshold. In this embodiment, the tolerance deviation threshold is set to 10 microseconds. This threshold is based on the determination made during the calibration of the binocular camera vision system: the maximum allowable exposure start time difference is 10 microseconds when the spatial position deviation of the composite structured light marker in the left and right view images does not exceed a preset pixel error range. Compared with the allowable deviation threshold in the allowable deviation threshold register, when When the time is less than or equal to 10 microseconds, the frame synchronization check submodule pairs the left-view image and the right-view image into a synchronized image pair and outputs the synchronized image pair. When When the time exceeds 10 microseconds, the frame synchronization verification submodule does not pair the left-view image and the right-view image, and discards the left-view image and the right-view image to prevent asynchronous images from entering the subsequent processing stage.
[0040] See Figure 3 The figure shows the time interval distribution of exposure pulses from the left and right cameras in the image synchronization acquisition module. The horizontal axis represents the time interval between exposure pulses of two adjacent frames of left-view and right-view images, in microseconds, and the vertical axis represents the probability density of the corresponding time interval, in 1 / microsecond. The blue bars represent the frequency distribution of the actual acquired time intervals, and the orange curve represents the trend of the corresponding time interval probability density distribution. The right side of the figure is marked with a red dashed line indicating the position of the allowable deviation threshold of 10 microseconds in the frame synchronization verification submodule.
[0041] As shown in the figure, most time intervals are concentrated in the range of 0 to 10 microseconds, with a single-peaked probability density distribution. The peak occurs in the range of approximately 4 to 6 microseconds, indicating that the time differences between the exposure pulses from the left and right cameras are mostly small and concentrated, which meets the allowable deviation threshold conditions set by the frame synchronization verification submodule. At the allowable deviation threshold of 10 microseconds, the red dashed line clearly defines the maximum allowable exposure time difference. All exposure pulse pairs corresponding to time intervals exceeding this threshold will be discarded by the frame synchronization verification submodule to prevent asynchronous image pairs from entering subsequent processing.
[0042] The overall trend shows that the image synchronization acquisition module effectively ensures the phase consistency between the exposure pulses of the left camera and the right camera by sharing the reference clock, synchronous reset of the counter, and the same preset sampling frequency through the design of the left camera trigger submodule and the right camera trigger submodule. This achieves high-precision image synchronization acquisition and provides a reliable synchronous image pair data foundation for the subsequent projection distortion correction module and pose deviation calculation module.
[0043] In practical implementation, the projection distortion correction module receives synchronized image pairs output by the image synchronization acquisition module. These synchronized image pairs include a left-view image and a right-view image. The projection distortion correction module comprises a first color separation submodule, a raster distortion extraction submodule, a mesh offset extraction submodule, and a normal vector reconstruction submodule.
[0044] In its implementation, the first color separation submodule performs color channel separation operations on the left-view and right-view images respectively, extracting the first monochrome image corresponding to the first color channel and the second monochrome image corresponding to the second color channel. The first color separation submodule internally includes a left-view image input buffer and a right-view image input buffer. The left-view image input buffer stores all pixel data of the left-view image in the synchronized image pair, and the right-view image input buffer stores all pixel data of the right-view image in the synchronized image pair. The first color separation submodule reads each pixel data from the left-view image in the left-view image input buffer. Each pixel data contains the grayscale values of the red, green, and blue channels, with the first color channel being the red channel and the second color channel being the blue channel. The first color separation submodule extracts the red channel grayscale value from each pixel data and arranges all extracted red channel grayscale values according to the row and column positions of the left-view image to generate the first monochrome image. The first monochrome image corresponds to the first color channel separation result of the left-view image. The first color separation submodule extracts the blue channel grayscale value from each pixel data, arranges all extracted blue channel grayscale values according to the row and column positions of the left-view image, and generates a second monochrome image. The second monochrome image corresponds to the second color channel separation result of the left-view image. The first color separation submodule reads each pixel data from the right-view image input buffer, extracts the red channel grayscale value from each pixel data, arranges all extracted red channel grayscale values according to the row and column positions of the right-view image, and generates a first monochrome image for the right view. The first monochrome image for the right view corresponds to the first color channel separation result of the right-view image. The first color separation submodule also extracts the blue channel grayscale value from each pixel data of the right-view image, arranges all extracted blue channel grayscale values according to the row and column positions of the right-view image, and generates a second monochrome image for the right view. The second monochrome image for the right view corresponds to the second color channel separation result of the right-view image. The first color separation submodule stores the first monochrome image from the left view, the second monochrome image from the left view, the first monochrome image from the right view, and the second monochrome image from the right view into the output storage area of the first color separation submodule, respectively. The output storage area is labeled with the first monochrome image from the left view, the second monochrome image from the left view, the first monochrome image from the right view, and the second monochrome image from the right view.
[0045] In specific implementation, the raster distortion extraction submodule extracts the center line of the raster fringes using the gray-scale centroid method on the first monochrome image, and calculates the curvature of the raster fringes based on the offset of the center line from the standard horizontal baseline. The raster distortion extraction submodule reads the first monochrome images from the left and right perspectives from the output storage area of the first color separation submodule. Taking the first monochrome image from the left perspective as the processing object, the raster distortion extraction submodule traverses each row of pixels in the first monochrome image from the left perspective, performing the gray-scale centroid method on each row of pixels. The gray-scale centroid method calculates the gray-scale centroid position of the r-th row of pixels. The gray-scale centroid position of the r-th row of pixels is obtained by dividing the sum of the products of the column coordinate and gray value of each pixel in the r-th row by the sum of the gray values of all pixels in the r-th row. The gray-scale centroid position of the r-th row of pixels represents the center position of the fringe brightness distribution in the r-th row of pixels. The grayscale centroid method calculates the grayscale centroid position for each row of pixels under the condition that multiple grating fringes exist simultaneously, obtaining the coordinates of the grating fringe centerline corresponding to each row of pixels. The grating distortion extraction submodule connects the coordinates of the grating fringe centerline corresponding to each row of pixels in the first monochrome image from the left viewpoint to obtain the grating fringe centerline of the first monochrome image from the left viewpoint. The grating fringe centerline coincides with the standard horizontal baseline under the condition that there is no distortion on the surface of the target workpiece. The standard horizontal baseline is a set of horizontal straight lines parallel to the horizontal direction of the first monochrome image from the left viewpoint. The grating distortion extraction submodule calculates the offset between the centerline of each grating fringe in the first monochrome image from the left viewpoint and the corresponding standard horizontal baseline. The offset is the difference between the row coordinates of the grating fringe centerline coordinate point and the row coordinates of the standard horizontal baseline. The grating distortion extraction submodule statistically analyzes the offsets of all coordinate points on the centerline of each grating fringe and uses the average of the offsets of all coordinate points on the centerline of each grating fringe as the grating fringe curvature. The grating fringe curvature reflects the degree of curvature of the grating fringe caused by the change in the height of the target workpiece surface. The grating distortion extraction submodule performs the same gray-scale centroid method to extract the center line of the grating fringes on the first monochrome image from the right view, thus obtaining the center line of the grating fringes and the curvature of the grating fringes in the first monochrome image from the right view.
[0046] In specific implementation, the grid offset extraction submodule performs corner detection on the second monochrome image to extract the corner points of each grid in the rectangular grid array. It calculates the grid node offset based on the offset vector of each corner point relative to the reference position of the standard rectangular grid. The grid offset extraction submodule reads the left-view and right-view second monochrome images from the output storage area of the first color separation submodule. Using the left-view second monochrome image as the processing object, the submodule performs corner detection. The corner detection operation uses a local neighborhood grayscale change detection algorithm to detect the grid corner points of the rectangular grid array in the left-view second monochrome image. A grid corner point is the intersection of the horizontal and vertical grid lines in the rectangular grid array. The local neighborhood grayscale change detection algorithm calculates a grayscale change matrix in the neighborhood of each pixel in the left-view second monochrome image. It determines whether a pixel is a corner point based on the eigenvalues of the grayscale change matrix. When the smaller eigenvalue of the grayscale change matrix is greater than the corner point determination threshold, the pixel is determined to be a grid corner point. The corner detection threshold is set to 15. This threshold is based on the fact that, compared to the background area, the smaller eigenvalue of the grayscale change matrix at the grid corners in the second monochrome image from the left perspective is between 10 and 20. Selecting the intermediate value of 15 allows for the differentiation between grid corners and the background area. The grid offset extraction submodule obtains the row and column coordinates of all grid corners in the second monochrome image from the left perspective. The standard rectangular grid reference position is the location of each grid corner in the rectangular grid array when no distortion occurs. This reference position is determined by an offline calibration process. The offline calibration process projects a rectangular grid array onto a standard planar workpiece, acquires an image of the rectangular grid array on the workpiece, extracts the grid corner coordinates from the image, and saves the extracted corner coordinates as the standard rectangular grid reference position. The mesh offset extraction submodule matches the row and column coordinates of each mesh corner point in the left-view second monochrome image with the corresponding row and column coordinates of the mesh corner point in the standard rectangular mesh reference position. It calculates the offset vector of each mesh corner point relative to the standard rectangular mesh reference position. The offset vector is a two-dimensional vector composed of the difference between the row and column coordinates of the mesh corner point and the row and column coordinates of the standard rectangular mesh reference position. The mesh offset extraction submodule averages the magnitudes of the offset vectors of all mesh corner points to obtain the mesh node offset. The mesh node offset reflects the degree of node position offset caused by the curvature change of the target workpiece surface in the rectangular mesh array. The mesh offset extraction submodule performs the same corner point detection and offset vector calculation processing on the right-view second monochrome image to obtain the mesh node offset of the right-view second monochrome image.
[0047] In its implementation, the normal vector reconstruction submodule substitutes the grating fringe curvature and grid node offset into a preset projection geometry mapping equation to calculate the normal vector tilt angle value corresponding to each pixel on the target workpiece surface, and combines these values into a local normal vector field. The normal vector reconstruction submodule obtains the grating fringe curvature of the first monochrome image from the left-view perspective and the first monochrome image from the right-view perspective from the grating distortion extraction submodule, and obtains the grid node offset of the second monochrome image from the left-view perspective and the second monochrome image from the right-view perspective from the grid offset extraction submodule. The normal vector reconstruction submodule uses the first monochrome image from the left-view perspective and the second monochrome image from the left-view perspective as a left-view image pair, and the first monochrome image from the right-view perspective and the second monochrome image from the right-view perspective as a right-view image pair. The normal vector reconstruction submodule applies the preset projection geometry mapping equation to the left-view image pair and the right-view image pair respectively. The preset projection geometry mapping equation represents the mapping relationship between the normal vector tilt angle value corresponding to a pixel on the target workpiece surface and the grating fringe curvature and grid node offset. The preset projection geometry mapping equation is as follows:
[0048] in, This represents the tilt angle value of the normal vector corresponding to a pixel on the surface of the target workpiece. The tilt angle value of the normal vector represents the angle between the normal vector of the target workpiece surface and the normal vector of the reference plane of the target workpiece surface, and the unit is radians. This indicates the curvature of the grating stripes, calculated by the grating distortion extraction submodule, and is expressed in pixels. This represents the grid node offset, calculated by the grid offset extraction submodule, and is expressed in pixels. The first projection mapping coefficient represents the curvature of the grating stripes to the tilt angle of the normal vector. The first projection mapping coefficient is determined by the projection geometry calibration process. The value range of the first projection mapping coefficient is from 0.5 radians / pixel to 1.5 radians / pixel. In this embodiment, the first projection mapping coefficient is set to 0.8 radians / pixel. The basis for setting the first projection mapping coefficient to 0.8 radians / pixel is that, under the reference projection distance, for every 1 radian change in the tilt angle of the normal vector of the target workpiece surface, the average change in the curvature of the grating stripes is 1.25 pixels. Therefore, the first projection mapping coefficient is 1 divided by 1.25, which is 0.8 radians / pixel. The second projection mapping coefficient represents the distance from the mesh node offset to the normal vector tilt angle. The second projection mapping coefficient is determined by the projection geometry calibration process. The value range of the second projection mapping coefficient is from 0.3 radians / pixel to 1.0 radians / pixel. In this embodiment, the second projection mapping coefficient is set to 0.6 radians / pixel. The basis for setting the second projection mapping coefficient to 0.6 radians / pixel is that, under the reference projection distance, for every 1 radian change in the tilt angle of the normal vector on the target workpiece surface, the average change in the mesh node offset is 1.67 pixels. Therefore, the second projection mapping coefficient is 1 divided by 1.67, which is 0.6 radians / pixel.
[0049] The normal vector reconstruction submodule substitutes the grating fringe curvature and grid node offset corresponding to each pixel in the left-view image pair into a preset projection geometry mapping equation to calculate the normal vector tilt angle value corresponding to each pixel in the left-view image pair. The normal vector reconstruction submodule then arranges the normal vector tilt angle values corresponding to all pixels in the left-view image pair according to pixel position to generate a local normal vector field for the left view. The normal vector reconstruction submodule performs the same calculation on the right-view image pair, substituting the grating fringe curvature and grid node offset corresponding to each pixel in the right-view image pair into the preset projection geometry mapping equation to calculate the normal vector tilt angle value corresponding to each pixel in the right-view image pair. It then arranges the normal vector tilt angle values corresponding to all pixels in the right-view image according to pixel position to generate a local normal vector field for the right view. The normal vector reconstruction submodule averages the normal vector tilt angle values at corresponding pixels in the left-view and right-view local normal vector fields, using this average as the normal vector tilt angle value at that pixel on the target workpiece surface, thus generating a local normal vector field. The normal vector reconstruction submodule then outputs the local normal vector field.
[0050] In specific implementation, please refer to Figure 2 The pose deviation calculation module receives the local normal vector field output by the projection distortion correction module. The pose deviation calculation module includes a model normal vector extraction submodule, a principal component alignment submodule, a quaternion transformation submodule, and a translation deviation calculation submodule.
[0051] In practical implementation, the model normal vector extraction submodule extracts the local region normal vector field corresponding to the spatial position of the local normal vector field from the pre-stored standard workpiece model normal vector field. The standard workpiece model normal vector field is stored in a non-volatile storage unit within the model normal vector extraction submodule. This field is generated during the offline modeling process and contains the normal vector direction information corresponding to each surface point on the standard workpiece surface. The standard workpiece model normal vector field is organized using a three-dimensional coordinate index in the standard workpiece coordinate system. The model normal vector extraction submodule reads the normal vector tilt angle value corresponding to each pixel in the local normal vector field and the corresponding image pixel coordinate range. Based on the binocular camera intrinsic parameters and the hand-eye calibration matrix, the model normal vector extraction submodule maps the image pixel coordinate range corresponding to the local normal vector field to the standard workpiece coordinate system, obtaining the spatial coverage area of the local normal vector field in the standard workpiece coordinate system. The model normal vector extraction submodule searches for the corresponding normal vector data in the normal vector field of the standard workpiece model based on the spatial coverage area. It extracts all normal vector data located within the spatial coverage area from the normal vector field of the standard workpiece model, forming a local region normal vector field. The local region normal vector field corresponds to the local normal vector field in spatial location, and each element in the local region normal vector field corresponds to the normal vector direction of the standard workpiece model at the corresponding spatial location.
[0052] In its implementation, the principal component alignment submodule performs principal component analysis on both the local normal vector field and the local region normal vector field, obtaining the first principal direction vector of the local normal vector field and the second principal direction vector of the local region normal vector field. The submodule contains two principal component analysis units: the first unit processes the local normal vector field, and the second unit processes the local region normal vector field. The first unit treats the normal vector tilt angle value corresponding to each pixel in the local normal vector field as a three-dimensional normal vector. This three-dimensional normal vector is calculated by combining the normal vector tilt angle value with the image coordinates of that pixel in the local normal vector field. The three components of the three-dimensional normal vector are the cosine of the normal vector tilt angle value, the sine of the normal vector tilt angle value, and the image depth normalized value of that pixel. The first unit then constructs a local normal vector data matrix from all the three-dimensional normal vectors in the local normal vector field. The number of rows in the local normal vector data matrix is the number of pixels in the local normal vector field, and the number of columns is 3. Finally, the first unit calculates the covariance matrix of the local normal vector data matrix, which is a 3x3 matrix. The first principal component analysis (PCA) unit performs eigenvalue decomposition on the covariance matrix to obtain all eigenvalues and the corresponding eigenvectors. The first PCA unit selects the largest eigenvalue from all eigenvalues and uses the eigenvector corresponding to the largest eigenvalue as the first principal direction vector of the local normal vector field. The second PCA unit performs the same operation on the local normal vector field as the first PCA unit. The second PCA unit converts each element in the local normal vector field into a three-dimensional normal vector. Each element in the local normal vector field originally contained the normal vector direction information of the standard workpiece model at the corresponding spatial position. The three components of the three-dimensional normal vector are the X-axis, Y-axis, and Z-axis components of the normal vector direction information in the standard workpiece coordinate system. The second PCA unit forms a local normal vector data matrix from all the three-dimensional normal vectors in the local normal vector field, calculates the covariance matrix of the local normal vector data matrix, performs eigenvalue decomposition on the covariance matrix, and selects the eigenvector corresponding to the largest eigenvalue as the second principal direction vector of the local normal vector field.
[0053] In its implementation, the quaternion conversion submodule calculates the rotation axis and rotation angle that rotate the first principal direction vector to the second principal direction vector based on the directional difference between the first and second principal direction vectors, and converts the rotation axis and rotation angle into rotation deviation quaternions. The quaternion conversion submodule obtains the first principal direction vector of the local normal vector field and the second principal direction vector of the local region normal vector field from the principal component alignment submodule. The quaternion conversion submodule normalizes the first and second principal direction vectors, making the magnitude of both vectors equal to 1. The quaternion conversion submodule calculates the dot product between the first and second principal direction vectors and uses the inverse cosine function to obtain the angle between them, which is the rotation angle. The quaternion conversion submodule calculates the cross product of the first and second principal direction vectors to obtain the rotation axis direction vector, and normalizes this vector to obtain the rotation axis unit vector. The quaternion transformation submodule substitutes the three components of the rotation axis unit vector and the rotation angle into the quaternion construction formula to generate a rotation deviation quaternion. The rotation deviation quaternion is in quaternion form, and the construction formula for the rotation deviation quaternion is as follows:
[0054] in, Represents the rotational deviation quaternion. It is a quaternion containing one scalar component and three vector components; This represents the rotation angle, which is the angle by which the first principal direction vector is rotated to the second principal direction vector, and the unit is radians. This represents the component of the rotation axis unit vector in the X-axis direction. The rotation axis unit vector is obtained by normalizing the rotation axis direction vector using the quaternion conversion submodule. This represents the component of the unit vector of the rotation axis along the Y-axis. This represents the component of the unit vector of the rotation axis in the Z-axis direction.
[0055] In practical implementation, the translation deviation calculation submodule calculates the translation deviation vector based on the coordinate difference between the centroid coordinates of the local normal vector field and the centroid coordinates of the local region normal vector field. The submodule obtains the local region normal vector field from the model normal vector extraction submodule and from the projection distortion correction module. It then calculates the average of the 3D coordinates of all pixels in the local normal vector field to obtain the centroid coordinates. Next, it calculates the average of the 3D coordinates of all elements in the local region normal vector field in the standard workpiece coordinate system to obtain the centroid coordinates of the local region normal vector field. Finally, it subtracts the centroid coordinates of the local region normal vector field from the centroid coordinates of the local normal vector field to obtain the translation deviation vector. This translation deviation vector is a 3D vector representing the spatial translation difference between the local and local region normal vector fields.
[0056] The pose deviation calculation module outputs the rotation deviation quaternion and translation deviation vector to the servo drive control module.
[0057] In practical implementation, the servo drive control module receives the rotational deviation quaternion and translational deviation vector output by the pose deviation calculation module. The dynamic response adjustment module receives the left-view image sequence output by the image synchronization acquisition module. The servo drive control module includes a Jacobian mapping submodule, a rotation mapping submodule, a velocity superposition submodule, and a feedforward injection submodule. The dynamic response adjustment module includes a feature point matching submodule, a drift rate calculation submodule, a gain adjustment submodule, and a gain update submodule.
[0058] In practical implementation, the Jacobi mapping submodule calculates the Jacobi matrix of the robotic arm based on the current joint angle values of each joint, and maps the translational deviation vector to the first angular velocity compensation component of each joint through the pseudo-inverse of the Jacobi matrix. The Jacobi mapping submodule reads the current joint angle values of each joint from the robotic arm controller; these joint angle values are measured in real-time by the encoders of each joint. Based on the current joint angle values, link length values, and link torsion angle values of each joint, the Jacobi mapping submodule calculates the Jacobi matrix corresponding to the current joint angle values using the forward kinematics method. The number of rows in the Jacobi matrix is the same as the number of degrees of freedom in the robotic arm's workspace, and the number of columns is the same as the number of joints in the robotic arm. The Jacobi mapping submodule performs a pseudo-inverse operation on the Jacobi matrix to obtain its pseudo-inverse. The Jacobi mapping submodule multiplies the translational deviation vector by the pseudo-inverse of the Jacobi matrix to obtain the first angular velocity compensation component of each joint. The first angular velocity compensation component is a vector, where each element corresponds to the angular velocity compensation value of a robotic arm joint. The dimension of the first angular velocity compensation component is the same as the number of robotic arm joints. The translational deviation vector is output by the translational deviation calculation submodule in the pose deviation solution module, and it is a three-dimensional vector.
[0059] In implementation, the rotation mapping submodule converts the rotational deviation quaternion into an equivalent rotational vector, and then maps the equivalent rotational vector into the second angular velocity compensation component of each joint using the pseudo-inverse of the Jacobian matrix. The rotation mapping submodule obtains the rotational deviation quaternion from the quaternion conversion submodule in the pose deviation calculation module. It extracts the scalar and vector components from the rotational deviation quaternion and converts it into an equivalent rotational vector using the quaternion-to-equivalent rotational vector conversion relationship. The direction of the equivalent rotational vector is the same as the direction of the unit vector of the rotation axis corresponding to the rotational deviation quaternion, and the magnitude of the equivalent rotational vector is equal to the rotation angle corresponding to the rotational deviation quaternion. The rotation mapping submodule obtains the pseudo-inverse of the Jacobian matrix from the Jacobian mapping submodule. It multiplies the equivalent rotational vector by the pseudo-inverse of the Jacobian matrix to obtain the second angular velocity compensation component of each joint. The second angular velocity compensation component is a vector, and each element in the second angular velocity compensation component corresponds to the angular velocity compensation value of a robotic arm joint. The dimension of the second angular velocity compensation component is the same as the number of robotic arm joints.
[0060] In implementation, the velocity superposition submodule adds the first and second angular velocity compensation components of each joint to obtain the angular velocity compensation amount for each joint. The velocity superposition submodule obtains the first angular velocity compensation component from the Jacobian mapping submodule and the second angular velocity compensation component from the rotation mapping submodule. Following the joint numbering order of the robotic arm, the velocity superposition submodule adds each element of the first angular velocity compensation component to the corresponding element of the second angular velocity compensation component one by one to obtain the angular velocity compensation amount for each joint. The angular velocity compensation amount for each joint is a vector, where each element corresponds to the angular velocity compensation value of a robotic arm joint, and the dimension of the angular velocity compensation amount for each joint is the same as the number of robotic arm joints. The velocity superposition submodule outputs the angular velocity compensation amount for each joint to the feedforward injection submodule.
[0061] In practical implementation, the feedforward injection submodule injects the angular velocity compensation of each joint into the velocity loop input of the position closed-loop controller in the form of a velocity feedforward signal. The feedforward injection submodule obtains the angular velocity compensation of each joint from the velocity superposition submodule. The feedforward injection submodule arranges the angular velocity compensation of each joint according to the joint numbering order of the robotic arm, generating a velocity feedforward signal. The feedforward injection submodule outputs the velocity feedforward signal to the velocity loop input of the position closed-loop controller. The velocity loop input of the position closed-loop controller is connected in parallel with the velocity feedforward signal input. The velocity feedforward signal and the original velocity loop input signal in the position closed-loop controller are superimposed at the velocity loop input, jointly driving the actuators of each joint of the robotic arm.
[0062] In its implementation, the feature point matching submodule performs nearest neighbor matching on each grid corner point of the rectangular grid array in two consecutive left-view images to obtain the pixel drift vector of each grid corner point. The feature point matching submodule acquires the left-view image sequence from the image synchronization acquisition module, storing adjacent left-view images in chronological order. The feature point matching submodule selects the preceding and following left-view images with adjacent timestamp information from the left-view image sequence. The feature point matching submodule performs corner detection on the preceding left-view image, extracting each grid corner point of the rectangular grid array in the preceding left-view image. The feature point matching submodule performs corner detection on the following left-view image, extracting each grid corner point of the rectangular grid array in the following left-view image. For each grid corner point in the preceding left-view image, the feature point matching submodule searches for the grid corner point in the following left-view image that is closest to the grid corner point in the preceding left-view image, using Euclidean distance as the distance metric. The feature point matching submodule establishes a matching relationship between the grid corner points of the previous frame's left-view image and the corresponding grid corner points of the next frame's left-view image. Based on this matching relationship, the feature point matching submodule calculates the pixel drift vector for each grid corner point. The pixel drift vector is a two-dimensional vector obtained by subtracting the row and column coordinates of the previous frame's left-view image grid corner point from the row and column coordinates of the next frame's left-view image grid corner point. The feature point matching submodule outputs the pixel drift vectors of all successfully matched grid corner points to the drift rate calculation submodule.
[0063] In specific implementation, the drift rate calculation submodule divides the magnitude of each pixel drift vector by the time interval between two adjacent frames to obtain the instantaneous drift rate of each grid corner point, and averages the instantaneous drift rates of all grid corner points to obtain the feature point drift rate. The drift rate calculation submodule obtains the pixel drift vectors of all successfully matched grid corner points from the feature point matching submodule, and obtains the timestamp information of two adjacent left-view images from the image synchronization acquisition module. The drift rate calculation submodule calculates the difference between the timestamp information of two adjacent left-view images to obtain the time interval between the two adjacent frames. The drift rate calculation submodule calculates the magnitude of each pixel drift vector; the magnitude of the pixel drift vector is the square root of the sum of the squares of the two components in the pixel drift vector. The drift rate calculation submodule divides the magnitude of each pixel drift vector by the time interval between two adjacent frames to obtain the instantaneous drift rate of each grid corner point. The drift rate calculation submodule adds the instantaneous drift rates of all grid corner points and then divides them by the total number of grid corner points to obtain the feature point drift rate. The formula for calculating the feature point drift rate is as follows:
[0064] in, This represents the feature point drift rate, which is the average of the instantaneous drift rates of all grid corner points, expressed in pixels per second. This represents the total number of successfully matched grid corner points, which is calculated by the feature point matching submodule. This indicates the index of the successfully matched grid corner, ranging from 1 to the total number of successfully matched grid corners. Take values one by one; Indicates the first The pixel drift vector of each successfully matched grid corner point is calculated by the feature point matching submodule and is in pixels. Indicates the first The magnitude of the pixel drift vector of a successfully matched grid corner point is the square root of the sum of the squares of the two components of the pixel drift vector, in pixels. This represents the time interval between two adjacent frames. The time interval between two adjacent frames is calculated by the drift rate calculation submodule based on the difference in timestamp information between the two adjacent left-view images, and the unit is seconds.
[0065] In practical implementation, the gain adjustment submodule obtains the gain adjustment coefficient of the velocity feedforward term by looking up a table according to a preset gain mapping relationship based on the feature point drift rate. It then multiplies the gain adjustment coefficient by the original gain value of the velocity feedforward term to obtain the updated gain value. The gain adjustment submodule internally contains a gain mapping relationship lookup table, which stores multiple sets of feature point drift rate intervals and their corresponding gain adjustment coefficients. The gain mapping relationship is a piecewise linear mapping between the feature point drift rate and the gain adjustment coefficient, and the gain adjustment coefficient monotonically increases with the feature point drift rate. Each linear mapping interval in the gain mapping relationship lookup table consists of a lower limit value of the feature point drift rate, an upper limit value of the feature point drift rate, a lower limit value of the gain adjustment coefficient, and an upper limit value of the gain adjustment coefficient. The gain adjustment submodule obtains the feature point drift rate from the drift rate calculation submodule, searches for the linear mapping interval in the gain mapping relationship lookup table that the feature point drift rate falls into, and calculates the gain adjustment coefficient by linear interpolation based on the position of the feature point drift rate within that linear mapping interval. The gain adjustment submodule reads the original gain value from the speed feedforward register of the position closed-loop controller, multiplies the gain adjustment coefficient by the original gain value, and obtains the updated gain value. The gain adjustment submodule then outputs the updated gain value to the gain update submodule.
[0066] In practice, the gain update submodule writes the updated gain value into the speed feedforward gain register of the position closed-loop controller, completing the dynamic adjustment of the gain coefficient. The gain update submodule obtains the updated gain value from the gain adjustment submodule. It then writes this updated gain value into the speed feedforward gain register of the position closed-loop controller, updating the speed feedforward gain coefficient stored in the register to the updated gain value. In subsequent control cycles, the position closed-loop controller reads the updated gain value from the speed feedforward gain register as the gain coefficient for the speed feedforward term.
[0067] See Figure 4 In the figure, the horizontal axis represents the feature point drift rate, in pixels per second, ranging from 0 to 200 pixels per second; the left vertical axis is the probability density, in units of 1 / (pixel·second), reflecting the distribution frequency of the feature point drift rate; the right vertical axis is the gain adjustment coefficient, in dimensionless units, representing the gain of the velocity feedforward term after dynamic adjustment due to the feature point drift rate.
[0068] The bar chart shows the probability density distribution of feature point drift rates, indicating that the drift rates are mainly concentrated in the range of 0 to 50 pixels / second, with the highest probability density (approximately 0.035) in the range of 0 to 10 pixels / second. This suggests that most grid corner points have low drift rates, and the system is in a relatively stable dynamic state. As the drift rate increases, the probability density gradually decreases, and the probability density for drift rates exceeding 100 pixels / second approaches zero, indicating that high drift rate conditions are rare.
[0069] The orange solid line curve represents the gain adjustment coefficient of the velocity feedforward term, which is adjusted according to the feature point drift rate. The gain adjustment coefficient increases monotonically with the drift rate: when the drift rate is close to 0, the gain adjustment coefficient is approximately 0.8; when the drift rate reaches 50 pixels / second, the gain adjustment coefficient is approximately 1.05; and near a drift rate of 200 pixels / second, the gain adjustment coefficient increases to approximately 1.2. This gain adjustment curve conforms to the set gain mapping relationship and is dynamically calculated through table lookup and linear interpolation.
[0070] The green dashed line represents the original gain value of the velocity feedforward term, calibrated to 1.0, serving as a reference for dynamic adjustment. The actual gain adjustment coefficient is lower than the reference value at low drift rates to avoid over-response; it exceeds the reference value as the drift rate increases, increasing the velocity feedforward gain to enhance the robot arm's response speed and stability.
[0071] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application.
Claims
1. A vision servo-based robotic arm pose control system, characterized in that, The system includes: The spatial projection marking module, in response to the current pose parameters of the robotic arm end effector, controls the laser projector to project a composite structured light mark with a first color channel and a second color channel onto the surface of the target workpiece. The image synchronous acquisition module triggers the binocular camera to synchronously acquire left-view and right-view images carrying the composite structured light markers according to a preset sampling frequency; The projection distortion correction module reconstructs the local normal vector field of the target workpiece surface based on the curvature of the grating stripes of the first color channel and the offset of the grid nodes of the second color channel in the left-view image and the right-view image. The pose deviation calculation module calculates the rotational deviation quaternion and translational deviation vector of the robotic arm end relative to the desired pose based on the spatial mapping relationship between the local normal vector field and the pre-stored standard workpiece model normal vector field. The servo drive control module converts the rotational deviation quaternion and the translational deviation vector into angular velocity compensation values for each joint of the robotic arm, and superimposes the angular velocity compensation values onto the velocity feedforward term of the position closed-loop controller. The dynamic response adjustment module monitors the drift rate of feature points of the composite structured light marker in the left-view image and the right-view image, and adjusts the gain coefficient of the velocity feedforward term according to the drift rate of the feature points.
2. The vision servo-based robotic arm pose control system according to claim 1, characterized in that, The spatial projection marking module includes: The first projection channel submodule is used to configure the first color channel in the composite structured light mark as sinusoidal distributed grating stripes extending laterally along the surface of the target workpiece. The second projection channel submodule is used to configure the second color channel in the composite structured light mark as a rectangular grid array superimposed at the peak position of the sinusoidal distributed grating stripes; The synchronous triggering submodule is used to calculate the projection focal length adjustment of the laser projector based on the spatial coordinate values in the current pose parameters of the robotic arm end effector, and drive the laser projector to zoom and project according to the projection focal length adjustment.
3. The vision servo-based robotic arm pose control system according to claim 1, characterized in that, The image synchronous acquisition module includes: The left camera triggering submodule is used to generate a left camera exposure pulse according to the preset sampling frequency, and drive the left camera in the binocular camera to acquire the left-view image; The right camera triggering submodule is used to generate a right camera exposure pulse that is in phase with the left camera exposure pulse according to the preset sampling frequency, and drive the right camera in the binocular camera to acquire the right view image; The frame synchronization verification submodule is used to read the timestamp information of the left-view image and the right-view image, calculate the time interval of the timestamp information, and pair the left-view image and the right-view image as a synchronized image pair when the time interval is less than or equal to the allowable deviation threshold.
4. The vision servo-based robotic arm pose control system according to claim 2, characterized in that, The projection distortion correction module includes: The first color separation submodule is used to perform color channel separation operations on the left-view image and the right-view image respectively, and extract the first monochrome image corresponding to the first color channel and the second monochrome image corresponding to the second color channel; The grating distortion extraction submodule is used to extract the center line of the grating stripes by performing the gray-scale centroid method on the first monochrome image, and to calculate the curvature of the grating stripes based on the offset of the center line of the grating stripes from the standard horizontal reference line. The grid offset extraction submodule is used to perform corner detection operation on the second monochrome image to extract each grid corner of the rectangular grid array, and calculate the grid node offset based on the offset vector of each grid corner relative to the reference position of the standard rectangular grid. The normal vector reconstruction submodule is used to substitute the curvature of the grating stripes and the offset of the grid nodes into a preset projection geometry mapping equation, solve the normal vector tilt angle value corresponding to each pixel point on the surface of the target workpiece, and combine the normal vector tilt angle values into the local normal vector field.
5. The vision servo-based robotic arm pose control system according to claim 1, characterized in that, The pose deviation calculation module includes: The model normal vector extraction submodule is used to extract the local region normal vector field corresponding to the spatial position of the local normal vector field from the pre-stored normal vector field of the standard workpiece model; The principal component alignment submodule is used to perform principal component analysis on the local normal vector field and the local region normal vector field respectively, to obtain the first principal direction vector of the local normal vector field and the second principal direction vector of the local region normal field; The quaternion conversion submodule is used to calculate the rotation axis and rotation angle that rotate the first principal direction vector to the second principal direction vector based on the directional difference between the first principal direction vector and the second principal direction vector, and convert the rotation axis and the rotation angle into the rotation deviation quaternion. The translation deviation calculation submodule is used to calculate the translation deviation vector based on the coordinate difference between the centroid coordinates of the local normal vector field and the centroid coordinates of the local region normal vector field.
6. The vision servo-based robotic arm posture control system according to claim 1, characterized in that, The servo drive control module includes: The Jacobian mapping submodule is used to calculate the Jacobian matrix of the robotic arm based on the joint angle values of each joint at present, and to map the translation deviation vector to the first angular velocity compensation component of each joint through the pseudo-inverse of the Jacobian matrix. The rotation mapping submodule is used to convert the rotation deviation quaternion into an equivalent rotation vector, and to map the equivalent rotation vector into the second angular velocity compensation component of each joint through the pseudo-inverse of the Jacobian matrix. The velocity superposition submodule is used to add the first angular velocity compensation component and the second angular velocity compensation component of each joint to obtain the angular velocity compensation amount of each joint. The feedforward injection submodule is used to inject the angular velocity compensation of each joint into the velocity loop input terminal of the position closed-loop controller in the form of a velocity feedforward signal.
7. The vision servo-based robotic arm pose control system according to claim 2, characterized in that, The dynamic response adjustment module includes: The feature point matching submodule is used to perform nearest neighbor matching on each grid corner point of the rectangular grid array in two consecutive left-view images to obtain the pixel drift vector of each grid corner point; The drift rate calculation submodule is used to divide the magnitude of each pixel drift vector by the time interval between two adjacent frames to obtain the instantaneous drift rate of each grid corner point, and to take the average value of the instantaneous drift rates of all grid corner points to obtain the feature point drift rate. The gain adjustment submodule is used to obtain the gain adjustment coefficient of the velocity feedforward term by looking up a table according to the magnitude of the feature point drift rate and a preset gain mapping relationship, and multiply the gain adjustment coefficient by the original gain value of the velocity feedforward term to obtain the updated gain value. The gain update submodule is used to write the updated gain value into the speed feedforward gain register of the position closed-loop controller to complete the dynamic adjustment of the gain coefficient.
8. The vision servo-based robotic arm pose control system according to claim 2, characterized in that, The spatial frequency of the sinusoidal grating fringes adaptively adjusts with the local curvature change of the target workpiece surface.
9. The vision servo-based robotic arm pose control system according to claim 2, characterized in that, The area of each grid cell in the rectangular grid array increases proportionally with the increase of the projection distance of the laser projector.
10. The vision servo-based robotic arm pose control system according to claim 7, characterized in that, The gain mapping relationship is a piecewise linear mapping relationship between the feature point drift rate and the gain adjustment coefficient, and the gain adjustment coefficient increases monotonically with the increase of the feature point drift rate.
