Monocular hand-eye robot visual servo method for three-dimensional space dynamic depth estimation

Abstract

The invention discloses a monocular hand-eye robot visual servo method for three-dimensional space dynamic depth estimation. The monocular hand-eye robot visual servo method comprises the steps of establishing a parameterized visual servo system model, constructing an estimation error driving type self-adaptive law and executing visual servo control based on positions. According to the method, the unified parameterized model of the robot, the camera and the target is established, and the target motion and the depth change caused by the robot motion are estimated under the same frame, so that the method can effectively cope with the more general and more complex working condition that the robot and the target both move. According to the method, the problem that the nonlinear time-varying depth is difficult to measure under the conditions that the prior geometric knowledge of the observed object does not exist and both the target and the robot move in the visual servo of the monocular camera is solved, and the rapid convergence of the control error and the estimation error in the hand-eye robot system is realized.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of hand-eye robot control, and particularly relates to a monocular hand-eye robot visual servo method for dynamic depth estimation in three-dimensional space. BACKGROUND

[0002] Visual servoing technology is an important means of robot control, which uses visual information as feedback to control the robot to achieve various work, such as positioning assembly, tracking docking, grabbing and placing, etc. This technology is widely used in unmanned aerial vehicles, humanoid robots, and mechanical arm control fields.

[0003] In a robot visual servo system, the two-dimensional pixel coordinates of the target feature points in the image space are usually collected by a camera, and are further mapped to the three-dimensional space coordinates under the robot base coordinate system through projection transformation. In the above coordinate transformation process, the conversion relationship between the coordinate systems can be obtained based on the camera's intrinsic and extrinsic parameters calibration results and the robot's forward kinematics model. However, the depth information of the camera relative to the target object changes dynamically over time, which cannot be obtained through prior knowledge or static calibration. Therefore, some researchers propose to construct a target motion observer, combine visual feedback information, and estimate the motion parameters of the target in the base coordinate system in real time, and further substitute into the depth model to estimate the depth value changing over time. Other researches try to fuse the visual system with other types of sensors, or use a binocular vision scheme to recover the depth information from the image through the triangulation principle. However, the introduction of additional sensors has many defects in practical application, including the increase of system cost, the increase of structural complexity, the decrease of overall reliability, and the significant increase of computational burden. In view of the above engineering implementation level limitations, recent researches have gradually focused on visual servo strategies relying only on monocular cameras, and through analytical algorithms to compensate for the lack of depth information.

[0004] For the image feature points obtained by the monocular visual system, existing researches introduce an adaptive estimation mechanism to solve the problem of time-varying depth unknown in the visual servo process. However, the traditional adaptive estimation method is only applicable to the depth change caused by the single dynamic scene of "target static, robot motion", and is difficult to cover the double dynamic scene composed of "target and robot moving simultaneously".

[0005] Therefore, it is necessary to develop a monocular hand-eye robot visual servo method for dynamic depth estimation in three-dimensional space to solve the above problems. SUMMARY

[0006] In order to solve the problems of limited scene and inability to estimate time-varying depth in the traditional method, the purpose of the present application is to provide a monocular hand-eye robot visual servo method for dynamic depth estimation in three-dimensional space, which places the depth changes caused by target motion and robot motion in the same framework for estimation by establishing a parameterized model of the robot-camera-target three unification, so that the present application can effectively deal with the more common and complex working condition that both the robot and the target are moving.

[0007] The purpose of the present application is achieved by including the following steps:

[0008] S1, establishing a parameterized visual servo system model: according to the forward kinematics model of the hand-eye robot, the internal and external parameters of the camera and the image Jacobian matrix, the closed-loop system composed of the robot-camera-target three is uniformly parameterized modeled;

[0009] S2, constructing an estimation error driven adaptive law: for the system model obtained in step S1, the measurable signal is reorganized by introducing an auxiliary filter variable, and the estimation error information related to the unknown depth parameter is extracted; based on the estimation error information, an adaptive law satisfying the Lyapunov convergence condition is designed to estimate the time-varying depth online in real time, and the depth estimation value is obtained, and the estimation error is guaranteed to converge in a provable way;

[0010] S3, performing position-based visual servo control: using the depth estimation value obtained in real time in step S2, the image plane feature points are back projected to the robot base coordinate system to obtain the estimation value of the three-dimensional position of the target; the tracking error between the three-dimensional position of the target and the desired position is defined, a feedback controller is designed combined with the robot dynamics model, and the tracking error is mapped to joint torque and speed command to drive the robot motion until the tracking error converges to within a preset threshold range.

[0011] Preferably, step S1 specifically includes the following steps:

[0012] S101, defining the three-dimensional space coordinates of the target feature points in the camera coordinate system as , the coordinates of the point projected to the camera imaging plane are , and the three-dimensional space coordinates in the camera coordinate system and the imaging plane coordinates have the following relationship: ;

[0013] S102, defining the coordinates of the feature points in the pixel plane as , and the pixel plane coordinates and the imaging plane coordinates have the following relationship: ;

[0014] wherein, respectively the principal point position of the camera along the axis, respectively the focal length of the camera along the axis, jointly constitute the camera intrinsic matrix : ;

[0015] S103, the three-dimensional space coordinates of S101 step and imaging plane coordinates relationship (formula (1)), S102 step pixel plane coordinates and imaging plane coordinates relationship (formula (2)) associated: ;

[0016] Take the time derivative of formula : ;

[0017] The image feature velocity is related to the spatial velocity of the camera: ;

[0018] Wherein, if the coordinates of the feature points do not change with time, then is the camera velocity in the camera coordinate system; if the coordinates of the feature points change with time, then is the camera velocity in the camera coordinate system minus the feature point velocity in the camera coordinate system;

[0019] Let: ;

[0020] Simplify and rewrite formula to ;

[0021] S104, using robot forward kinematics, from which the rotation matrix between the base coordinate system and the robot end coordinate system and the translation vector ; using camera extrinsic calibration to obtain the rotation matrix between the robot end coordinate system and the camera coordinate system and the translation vector ; using the rotation matrix and the translation vector between each coordinate system, the end velocity in the base coordinate system is converted into the camera velocity in the camera coordinate system, and the conversion relationship is as follows: ;

[0022] wherein, is the end-effector velocity in the base coordinate system; is the camera velocity in the camera coordinate system; represents the translation vector is a skew-symmetric matrix composed of three parameters;

[0023] S105, the end-effector velocity in the base coordinate system is obtained by the robot Jacobian matrix and the robot joint velocity , is the degree of freedom of the robot, and the end-effector velocity in the base coordinate system is obtained: ;

[0024] The formula (formula ) obtained in the step S103 is parameterized, and , , , and the following can be obtained: ;

[0025] wherein, , ;

[0026] The parameterized visual servoing system model is completed.

[0027] Preferably, the step S2 specifically comprises the following steps:

[0028] S201, for the parameterized visual servoing system model, each filter variable is defined as follows: ;

[0029] wherein is a filter coefficient, which can be set as a very small constant;

[0030] S202, the intermediate matrix is defined, : ;

[0031] wherein, is used to ensure that the intermediate regression matrix is bounded, and the constant plays a role of a forgetting factor, which can be set as a small constant; if is smaller, the matrix contains more historical information of ; and the solution of the formula is: ;

[0032] wherein, is the estimation error; is the residual of the derivative of the time-varying parameter after low-pass filtering;

[0033] S203, then define the auxiliary variable , : ;

[0034] Thus, it can be known that the estimation error of the time-varying parameter has been contained in the auxiliary variable , and the adaptive estimation law driven by the estimation error is constructed by the auxiliary variable: ;

[0035] wherein, is a constant learning gain; is also a constant, which is used to balance the ability of estimating rapidly changing parameters and robustness; by the adaptive law, the is calculated, and the is obtained by integration, and because , the estimated value of the depth information can be obtained, and the three-dimensional estimated coordinates of the target feature point in the robot base coordinate system are reconstructed by the pixel point coordinates of the target feature point participating in the camera internal and external, and the target feature point coordinates in the camera coordinate system are reconstructed by the estimated depth information: participate in the visual servo control.

[0036] Preferably, the S3 step specifically comprises the following steps:

[0037] S301, obtaining the pixel coordinates of the target feature point by the camera , and then obtaining the feature point coordinates in the camera coordinate system by the estimated depth information: ;

[0038] S302, converting the feature point coordinates in the camera coordinate system into the feature point coordinates in the robot base coordinate system by using each conversion matrix: ;

[0039] S303, in order to let the robot end reach above the target feature point and follow the target feature point to move, the error is defined by combining the feature point coordinate estimation value in the base coordinate system with the robot end pose: ;

[0040] wherein is the pose of the target feature point in the robot base frame, since the target feature point is assumed to only have translational motion in the base frame, the pose of the target feature point does not change with time; is the pose of the robot end-effector in the robot base frame; and taking the time derivative of the error:

[0041] S304, the design of the controller is based on the dynamics model of the robot with n degrees of freedom:

[0042] wherein, respectively represent the joint displacement, velocity and acceleration of the robot, is the moment of inertia, is the centripetal moment, is the gravitational moment, is the control moment of the robot;

[0043] S305, a PD controller is adopted, and the controller is designed as follows:

[0044] wherein, the diagonal matrix is the control gain, are the pose error and the velocity error defined above, respectively.

[0045] Compared with the prior art, the present application has the following technical effects:

[0046] 1. The existing monocular visual servoing method usually assumes that the target is stationary or only the robot moves, and the depth estimation depends on a static or single dynamic model. When the target and the robot move simultaneously, the depth change has nonlinearity and time-varying nature, and the traditional adaptive estimation method cannot effectively handle it, resulting in the technical problems of divergence of estimation error or decline of control performance. The present application establishes a parameterized visual servoing system model of the robot-monocular camera-target, models the depth change caused by the target motion and the robot motion in the same framework, solves the time-varying depth estimation problem in the double dynamic scene, and breaks through the scene limitation of the traditional method;

[0047] ​​​2、Traditional adaptive law usually relies on prior knowledge or assumptions of target motion model, and it is difficult to ensure convergence in the prior art under the double dynamic scene, and the technical problem of slow estimation speed. In the present application, an adaptive law based on estimation error driving is designed, the measurable signals are reorganized by introducing auxiliary filtering variables, and the estimation error information related to the unknown depth parameter is extracted, the adaptive law does not require prior geometric knowledge of target motion, only uses measurable signals such as image feature speed, joint speed and target feature point speed in the base coordinate system, and realizes online real-time estimation of time-varying depth;

[0048] 3、The prior art has some schemes that use multi-sensor fusion or binocular vision scheme to solve the depth estimation problem, but the system cost, complexity and calculation burden are increased. The present application only relies on a monocular camera, and the lack of depth information is compensated by algorithm innovation; the whole scheme of the present application does not require additional hardware, only requires standard robot forward kinematics model and camera calibration parameters, is easy to integrate into the existing hand-eye robot system, reduces the system complexity and cost, and is suitable for monocular vision system. BRIEF DESCRIPTION OF DRAWINGS

[0049] Figure 1 is a method flowchart of the present application;

[0050] Figure 2 is an estimation error graph of example 3;

[0051] Figure 3 is a pose error graph of example 3;

[0052] Figure 4 is a speed error graph of example 3;

[0053] Figure 5 is a trajectory graph of the target and the robot end of example 3. DETAILED DESCRIPTION

[0054] The present application will be further described below in conjunction with the embodiments and drawings, but in no way limits the present application, any transformation or replacement based on the teaching of the present application belongs to the protection scope of the present application.

[0055] Example 1

[0056] The monocular hand-eye robot vision servo method for three-dimensional space dynamic depth estimation of the present embodiment comprises the following steps:

[0057] S1, establishing a parameterized visual servo system model: according to the forward kinematics model of the hand-eye robot, the internal and external parameters of the camera and the image Jacobian matrix, the closed-loop system composed of the robot-camera-target is uniformly parameterized modeling;

[0058] S2, constructing an estimation error driven adaptive law: for the system model obtained in S1, an auxiliary filtering variable is introduced to reorganize the measurable signals, and the estimation error information related to the unknown depth parameter is extracted; based on the estimation error information, an adaptive law satisfying the Lyapunov convergence condition is designed to online estimate the time-varying depth in real time, and the depth estimation value is obtained, and the estimation error is guaranteed to converge in a provable manner;

[0059] S3, performing position-based visual servoing control: using the depth estimation value obtained in real time in S2, the image plane feature points are back projected to the robot base coordinate system to obtain the estimation value of the target three-dimensional position; the tracking error between the target three-dimensional position and the expected position is defined, a feedback controller is designed in combination with the robot dynamics model, and the tracking error is mapped to the joint torque and speed command to drive the robot to move until the tracking error converges to within a preset threshold range.

[0060] Embodiment 2

[0061] The monocular hand-eye robot visual servoing method for three-dimensional space dynamic depth estimation of this embodiment is based on embodiment 1, wherein:

[0062] S1 specifically includes the following steps:

[0063] S101, define the three-dimensional space coordinates of the target feature points in the camera coordinate system as , the coordinates of the point projected to the camera imaging plane are , and the three-dimensional space coordinates in the camera coordinate system and the imaging plane coordinates have the following relationship: ;

[0064] S102, define the coordinates of the feature points in the pixel plane as , and the pixel plane coordinates and the imaging plane coordinates have the following relationship: ;

[0065] wherein, are the principal point positions of the camera along the axis, are the focal lengths of the camera along the axis, which together form the camera intrinsic matrix : ;

[0066] S103, solve the formula and the formula simultaneously: ;

[0067] Take the time derivative of the formula : ;

[0068] Link the image feature velocity to the spatial velocity of the camera: ;

[0069] Where, if the coordinates of the feature points do not change with time, then is the camera velocity in the camera coordinate system; if the coordinates of the feature points change with time, then is the camera velocity in the camera coordinate system minus the feature point velocity in the camera coordinate system;

[0070] Let: ;

[0071] Simplify and rewrite the formula : ;

[0072] S104, use robot forward kinematics to obtain the rotation matrix between the base coordinate system and the robot end coordinate system , and the translation vector between the robot end coordinate system and the camera coordinate system; use the camera extrinsic calibration to obtain the rotation matrix between the robot end coordinate system and the camera coordinate system; use the rotation matrix and the translation vector between the coordinate systems to convert the end velocity in the base coordinate system into the camera velocity in the camera coordinate system, and the conversion relationship is as follows: ;

[0073] Where, is the end velocity in the base coordinate system; is the camera velocity in the camera coordinate system; represents a skew-symmetric matrix composed of three parameters in the translation vector ;

[0074] S105, through the robot Jacobian matrix and the robot joint velocity , is the degree of freedom of the robot, to obtain the end velocity in the base coordinate system: ;

[0075] Parametrizing the formula , , , , ;

[0076] where , ;

[0077] The parametrized visual servoing system model is constructed;

[0078] The S2 step specifically includes the following steps:

[0079] S201, for the parametrized visual servoing system model (formula (11)), define each filter variable as follows: ;

[0080] where is a filter coefficient, which can be set as a very small constant;

[0081] S202, define the intermediate matrix , : ;

[0082] where is used to ensure that the intermediate regression matrix is bounded, and the constant acts as a forgetting factor, which can be a small constant; if is smaller, the matrix contains more historical information; then the solution of the formula is: ;

[0083] where is the estimation error; is the derivative of the time-varying parameter after low-pass filtering the residual;

[0084] S203, then define the auxiliary variable , : ;

[0085] Thus, it can be seen that the estimation error of the time-varying parameter has been included in the auxiliary variable In the middle, by auxiliary variable to construct with the estimated error as the driving adaptive estimation law: ;

[0086] wherein, is a constant learning gain; is also a constant, for balancing the ability to estimate rapidly changing parameters and robustness; by the adaptive law to calculate , and through the integral to obtain , and because , so can get depth information of the estimated value , and through the camera inside and outside the target feature points of pixel coordinates to reconstruct the target feature points in the robot base coordinate system under the three-dimensional estimated coordinates Participate in visual servo control;

[0087] S3 step specifically includes the following steps:

[0088] S301, through the camera to obtain the pixel coordinates of the target feature points , and then use the estimated depth information to obtain the feature point coordinates in the camera coordinate system: ;

[0089] S302, use each conversion matrix to convert the feature point coordinates in the camera coordinate system into the feature point coordinates in the robot base coordinate system: ;

[0090] S303, in order to let the robot end reach the upper of the target feature point and follow the target feature point to move, use the feature point coordinate estimate value in the base coordinate system combined with the robot end posture to define the error: ;

[0091] Wherein is the pose of the target feature point in the robot base coordinate system, since the target feature point is assumed to exist only in the base coordinate system translation motion, so the pose of the target feature point does not change with time change; is the pose of the robot end in the robot base coordinate system; and take the time derivative of the error: ;

[0092] S304, the design of the controller is based on dof robot dynamics model: ;

[0093] wherein, respectively represent joint displacement, velocity and acceleration of the robot, is the moment of inertia, is the centripetal moment, is the gravity moment, is the control moment of the robot;

[0094] S305, a PD controller is adopted, and the controller is designed as follows: ;

[0095] wherein, the diagonal matrix is the control gain, respectively are the pose error and the velocity error defined above.

[0096] Embodiment 3

[0097] The present application estimates the depth information rapidly by using the adaptive estimation law driven by the estimation error for controlling the robot. The effectiveness of the method of the present application is verified by building a 7-DOF robot model in MATLAB / Simulink. From the simulation results, it can be seen that the method of the present application solves the problem of non-linear time-varying depth measurement in the case of single camera visual servoing, where the priori geometric knowledge of the observed object is known and both the target and the robot are moving.

[0098] Specifically, in the Simulink simulation, the visual servoing control is completed by using the method of embodiment 2, and the results are observed. The simulation results are as follows: Figure 2 represents the estimation error, i.e. the difference between the depth information estimated by the adaptive estimation law and the true depth information. From the figure, it can be seen that the adaptive estimation law of the present application can make the error between the true value and the estimated value of the depth information quickly converge to 0; Figures 3-4 represents the pose error and the velocity error. From the figure, it can be seen that after the motion trajectory of the target feature point is given, the controller used in the present application can make the pose error and the velocity error quickly converge to 0; Figure 5 is the trajectory of the robot end and the target object. Based on the error defined in step S3, the robot end and the target feature point cannot be coincident in actual operation. If they are coincident, it will cause the pixel coordinates to rapidly expand, resulting in estimation failure. Therefore, the final result is set to be that the robot end is located 15 cm above the target feature point, and continues to track. Combined with Figure 3 and Figure 5It can be seen that after the simulation starts, the robot end can be located above the target feature point at 15cm after 0.3s, and the tracking is continued, which shows that using the model, the adaptive estimation law and the controller given by the application, the visual servo task in the double dynamic scene can be quickly and accurately completed.

[0099] It can be seen that the method of the application can be driven by estimation error, and a corresponding adaptive estimation law is designed for estimating feature position and depth parameters. The problem of non-linear time-varying depth being difficult to measure in the case that the priori geometric knowledge of the observed object is not known and the target and the robot are both moving in monocular camera visual servo is solved, and the fast convergence of control error and estimation error in the hand-eye robot system is realized.

Claims

1. A monocular hand-eye robot vision servoing method for three-dimensional space dynamic depth estimation, characterized in that The method comprises the following steps: S1, establishing a parameterized visual servo system model: according to the forward kinematics model of the hand-eye robot, the internal and external parameters of the camera, and the image Jacobian matrix, the closed-loop system composed of the robot, the camera and the target is uniformly parameterized and modeled; S2, constructing an estimation error driven adaptive law: for the system model obtained in step S1, the measurable signals are reorganized by introducing auxiliary filter variables, and the estimation error information related to the unknown depth parameter is extracted; Based on the estimation error information, an adaptive law satisfying the Lyapunov convergence condition is designed to online estimate the time-varying depth in real time, obtain the depth estimation value, and ensure the convergence of the estimation error in a provable manner; S3, performing position-based visual servo control: using the depth estimation value obtained in step S2 in real time, the image plane feature points are back-projected to the robot base coordinate system to obtain the estimation value of the three-dimensional position of the target; the tracking error between the three-dimensional position of the target and the expected position is defined, a feedback controller is designed combined with the robot dynamics model, and the tracking error is mapped to joint torque and speed command to drive the robot to move until the tracking error converges to within a preset threshold range.

2. The monocular hand-eye robot visual servoing method of 3D spatial dynamic depth estimation according to claim 1, wherein The step S1 specifically comprises the following steps: S101、Definition of the three-dimensional space coordinates of the target feature point in the camera coordinate system is The coordinates of the point projected to the camera imaging plane are The three-dimensional space coordinates in the camera coordinate system and the imaging plane coordinates have the following relationship: ； S102, define the coordinates of the feature points in the pixel plane as The pixel plane coordinates and the imaging plane coordinates have the following relationship: ； wherein, are the principal point positions of the camera along the x and y axes, respectively, are the focal lengths of the camera along the x and y axes, respectively, are the principal point positions of the camera along the x and y axes, respectively, are the focal lengths of the camera along the x and y axes, respectively, : ； S103, the three-dimensional space coordinates and the imaging plane coordinates relationship formula in step S101, and the pixel plane coordinates and the imaging plane coordinates relationship formula in step S102 are combined: ； Take the time derivative of the formula: ； The image feature velocity is related to the spatial velocity of the camera: ； wherein, if the coordinates of the feature point do not change with time, then is the camera velocity in the camera coordinate system; if the coordinates of the feature point change with time, then is the camera velocity in the camera coordinate system minus the feature point velocity in the camera coordinate system; Let: ； Simplify and rewrite the formula as ； S104, obtaining the rotation matrix between the base coordinate system and the robot end coordinate system by using robot forward kinematics , translation vector ; obtaining the rotation matrix between the robot end coordinate system and the camera coordinate system by using camera extrinsic calibration , translation vector ; converting the end speed in the base coordinate system into the camera speed in the camera coordinate system by using the rotation matrix and the translation vector between the coordinate systems, and the conversion relationship is as follows: ； wherein, is the end-effector velocity in the base coordinate frame; is the camera velocity in the camera coordinate frame; represents the translational vector skew-symmetric matrix of the three parameters S105, obtain the end velocity in the base coordinate system through the robot Jacobian matrix with the robot joint velocity , is the degree of freedom of the robot, obtain the end velocity in the base coordinate system: ； Parametrizing the equation obtained in step S103, let , , , we have: ； wherein , ; Thus, the parameterized visual servo system model is completed.

3. The monocular hand-eye robot visual servoing method of 3D spatial dynamic depth estimation of claim 1, wherein The step S2 specifically comprises the following steps: S201, for the parameterized visual servo system model, define each filter variable as follows: ； wherein is a filter coefficient, set to a very small constant; S202, define an intermediate matrix , : ； wherein, for ensuring the intermediate regression matrix bounded, constant acts as a forgetting factor, as a small constant; if is smaller, the matrix contains more historical information; then the solution of the formula is: ； wherein is an estimation error; is a residual of the derivative of the time-varying parameter after low-pass filtering; S203, then define auxiliary variables , : ； Thus, it can be seen that the estimation error of the time-varying parameter has been included in the auxiliary variable In this way, an adaptive estimation law driven by the estimation error is constructed by the auxiliary variable: ； wherein, is a constant learning gain; is also a constant, used to balance the ability to estimate rapidly changing parameters and robustness; through the adaptive law to calculate , and through the integral to obtain , and because , the estimated value of the depth information , and the three-dimensional estimated coordinates of the target feature points in the robot base coordinate system are reconstructed through the pixel point coordinates of the target feature points participating in the camera inside and outside participate in visual servo control.

4. The monocular hand-eye robot visual servoing method of dynamic depth estimation in three-dimensional space according to claim 1, wherein The step S3 specifically comprises the following steps: S301, acquire the pixel coordinates of the target feature points through the camera Obtain the feature point coordinates in the camera coordinate system by reusing the estimated depth information ； S302, use each conversion matrix to convert the feature point coordinates in the camera coordinate system to the feature point coordinates in the robot base coordinate system: ； S303, in order to let the robot end reach above the target feature point and follow the target feature point to move, define the error by using the feature point coordinate estimation value in the base coordinate system combined with the robot end posture: ； wherein is the pose of the target feature point in the robot base coordinate system, since it is assumed that the target feature point only has translational motion in the base coordinate system, the pose of the target feature point does not change with time; is the pose of the robot end in the robot base coordinate system; and taking the time derivative of the error: ； S304、the design of the controller is based on Degrees of freedom robot dynamics model: ； wherein, respectively represent joint displacement, velocity and acceleration of the robot, is the moment of inertia, is the centripetal moment, is the gravitational moment, is the control moment of the robot; S305, a PD controller is used, and the controller is designed as follows: ； wherein the diagonal matrix is a control gain, are the above defined pose error and velocity error, respectively.

Images