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

Abstract

The application discloses a monocular hand-eye robot vision servo method for three-dimensional space dynamic depth estimation, comprising establishing a parameterized vision servo system model, constructing an estimation error driven adaptive law and executing position-based vision servo control. The application establishes a parameterized model of the robot-camera-target unification, estimates the depth changes caused by the target motion and the robot motion in the same framework, which enables the application to effectively deal with the more common and more complex working conditions that both the robot and the target are moving. The application solves the problem that the nonlinear time-varying depth is difficult to measure in the monocular camera vision servo without prior geometric knowledge of the observed object and under the condition that both the target and the robot are moving, and realizes the rapid convergence of the control error and the estimation error in the hand-eye robot system.

Description

Technical Field

[0001] This invention belongs to the field of hand-eye robot control technology, specifically relating to a visual servoing method for monocular hand-eye robots with dynamic depth estimation in three-dimensional space. Background Technology

[0002] Visual servoing technology is an important means of robot control. By using visual information as feedback, robots can be controlled to perform various tasks, such as positioning and assembly, tracking and docking, grasping and placing. This technology is widely used in fields such as drones, humanoid robots, and robotic arm control.

[0003] In robot vision servoing systems, the two-dimensional pixel coordinates of target feature points in image space are typically acquired by a camera, and then mapped to three-dimensional spatial coordinates in the robot's base coordinate system through projection transformation. During this coordinate transformation, the transformation relationship between different coordinate systems can be pre-obtained based on the camera's intrinsic and extrinsic parameter 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 and cannot be obtained through prior knowledge or static calibration. To address this, some researchers have proposed constructing a target motion observer, combining visual feedback information to estimate the target's motion parameters in the base coordinate system in real time, and further substituting these parameters into a depth model to estimate the time-varying depth value. Other studies have attempted to fuse the vision system with other types of sensors or employ binocular vision schemes to recover depth information from images using triangulation principles. However, the introduction of additional sensors has several drawbacks in practical applications, including increased system cost, increased structural complexity, reduced overall reliability, and a significantly heavier computational burden. Given these engineering implementation limitations, recent research has gradually focused on vision servoing strategies relying solely on a monocular camera, and using analytical algorithms to compensate for the lack of depth information.

[0004] For image feature points acquired by monocular vision systems, existing research has introduced adaptive estimation mechanisms to address the problem of unknown time-varying depth of feature points during visual servoing. However, traditional adaptive estimation methods are only applicable to depth changes caused by a single dynamic scene of "target stationary, robot moving," and are difficult to cover dual dynamic scenes of "target and robot moving simultaneously."

[0005] Therefore, it is essential to develop a visual servoing method for monocular hand-eye robots that can solve the above problems by performing dynamic depth estimation in three-dimensional space. Summary of the Invention

[0006] To address the limitations of traditional methods in terms of scene constraints and the inability to estimate time-varying depth, this invention aims to provide a monocular hand-eye robot visual servoing method for dynamic depth estimation in three-dimensional space. This invention establishes a unified parameterized model for the robot, camera, and target, estimating the depth changes caused by target motion and robot motion within the same framework. This enables the invention to effectively handle the more common and complex working conditions where both the robot and the target are in motion.

[0007] The objective of this invention is achieved by including the following steps:

[0008] S1. Establish a parametric visual servoing system model: Based on the forward kinematics model of the hand-eye robot, the intrinsic and extrinsic parameters of the camera, and the image Jacobian matrix, perform a unified parametric modeling of the closed-loop system consisting of the robot, camera, and target.

[0009] S2. Construct an estimation error-driven adaptive law: For the system model obtained in step S1, introduce auxiliary filter variables to reconstruct the measurable signal and extract estimation error information related to the unknown depth parameters; based on the estimation error information, design an adaptive law that satisfies the Lyapunov convergence condition to estimate the time-varying depth online in real time, obtain the depth estimate, and ensure the convergence of the estimation error in a provable manner.

[0010] S3. Perform position-based visual servo control: Using the depth estimate obtained in real time in step S2, back-project the image plane feature points to the robot base coordinate system to obtain the estimated value of the target's three-dimensional position; define the tracking error between the target's three-dimensional position and the desired position, design a feedback controller in combination with the robot dynamics model, and map the tracking error into joint torque and speed commands to drive the robot to move until the tracking error converges to a preset threshold range.

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

[0012] S101. Define the three-dimensional spatial coordinates of the target feature points in the camera coordinate system as follows: The coordinates of this point projected onto the camera's imaging plane are: The three-dimensional spatial coordinates in the camera coordinate system and the imaging plane coordinates have the following relationship:

[0013] ;

[0014] S102. Define the coordinates of the feature points in the pixel plane as follows: The pixel plane coordinates and the imaging plane coordinates have the following relationship:

[0015] ;

[0016] in, The camera along The principal point position of the axis, The camera along The focal lengths of the axes together form the camera intrinsic parameter matrix. :

[0017] ;

[0018] S103. Combine the relationship between the three-dimensional spatial coordinates and the imaging plane coordinates in step S101 (Formula (1)) and the relationship between the pixel plane coordinates and the imaging plane coordinates in step S102 (Formula (2)):

[0019] ;

[0020] Take the formula Time derivative:

[0021] ;

[0022] Relating image feature velocity to camera spatial velocity:

[0023] ;

[0024] If the coordinates of the feature points do not change with time, then Let be the camera velocity in the camera coordinate system; if the coordinates of the feature points change with time, then... The camera velocity in the camera coordinate system minus the feature point velocity in the camera coordinate system;

[0025] make:

[0026] ;

[0027] Formula Simplify and rewrite as

[0028] ;

[0029] S104. Using the robot's forward kinematics, obtain the rotation matrix between the base coordinate system and the robot's end effector coordinate system. Translation vector The rotation matrix between the robot's end effector coordinate system and the camera coordinate system is obtained using camera extrinsic parameter calibration. Translation vector Using the rotation matrix and translation vector between the coordinate systems, the end velocity in the base coordinate system is converted into the camera velocity in the camera coordinate system. The conversion relationship is as follows:

[0030] ;

[0031] in, It is the terminal velocity in the base coordinate system; It is the camera speed in the camera coordinate system; Represented by translation vector The oblique symmetric matrix formed by the three parameters;

[0032] S105, via the robot Jacobian matrix With robot joint speed , For the robot's degrees of freedom, obtain the end effector velocity in the base coordinate system:

[0033] ;

[0034] The formula obtained from step S103 (formula) Parameterize it, let , , We can obtain:

[0035] ;

[0036] in, , ;

[0037] This completes the construction of the parametric visual servo system model.

[0038] Preferably, step S2 specifically includes the following steps:

[0039] S201. For the parametric visual servoing system model, the filter variables are defined as follows:

[0040] ;

[0041] in These are the filter coefficients, which can be set to a very small constant value;

[0042] S202, Define the intermediate matrix , :

[0043] ;

[0044] in, Used to ensure the intermediate regression matrix Bounded, constant It acts as a forgetting factor, and can be considered a small constant; if The smaller the value, the more data the matrix contains. Historical information; then the formula The solution is:

[0045] ;

[0046] in, It is an estimation error; It is the residual of the derivative of the time-varying parameter after low-pass filtering;

[0047] S203, Next, define auxiliary variables. , :

[0048] ;

[0049] Therefore, it can be seen that the estimation error of the time-varying parameter has been included in the auxiliary variable. In this process, an adaptive estimation law driven by estimation error is constructed using auxiliary variables:

[0050] ;

[0051] in, It is a constant learning gain; It is also a constant used to balance the ability to estimate rapidly changing parameters and robustness; the adaptive law is calculated from this. Then obtain through points And because Therefore, it is possible to obtain an estimate of depth information. The three-dimensional estimated coordinates of the target feature points in the robot's base coordinate system are reconstructed by using the pixel coordinates of the target feature points inside and outside the camera. Participate in visual servo control.

[0052] Preferably, step S3 specifically includes the following steps:

[0053] S301. Obtain the pixel coordinates of the target feature points using the camera. Then, the estimated depth information is used to obtain the coordinates of the feature points in the camera coordinate system:

[0054] ;

[0055] S302. Use the transformation matrices to convert the feature point coordinates in the camera coordinate system to the feature point coordinates in the robot base coordinate system:

[0056] ;

[0057] S303. To ensure the robot's end effector reaches above the target feature point and moves with it, the error is defined by combining the estimated coordinates of the feature point in the base coordinate system with the robot's end effector posture:

[0058] ;

[0059] in It is the pose of the target feature point in the robot's 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. This represents the pose of the robot's end effector in the robot's base coordinate system; and the time derivative of the error is taken:

[0060] ;

[0061] S304, the controller design is based on Dynamics model of a robot with degrees of freedom:

[0062] ;

[0063] in, These represent the robot's joint displacement, velocity, and acceleration, respectively. It is the torque of inertia. It is the centripetal torque. It is the gravitational torque. It is the control torque of the robot;

[0064] S305 uses a PD controller, and the controller design is as follows:

[0065] ;

[0066] Among them, diagonal array It is about controlling the gain. These are the pose error and velocity error defined above, respectively.

[0067] Compared with the prior art, the present invention has the following technical effects:

[0068] 1. Existing monocular vision servoing methods typically assume that the target is stationary or only the robot is moving. Depth estimation relies on a static or single dynamic model. However, existing technologies struggle to address the nonlinear and time-varying depth changes that occur when both the target and robot move simultaneously. Traditional adaptive estimation methods cannot effectively handle this, leading to divergent estimation errors or degraded control performance. This invention establishes a unified parameterized vision servoing system model encompassing the robot, monocular camera, and target. By modeling the depth changes caused by both target and robot motion within the same framework, this invention solves the challenge of time-varying depth estimation in dual-dynamic scenarios, overcoming the scene limitations of traditional methods.

[0069] 2. Traditional adaptive laws typically rely on prior knowledge or assumptions about the target motion model, which presents challenges in ensuring convergence and slow estimation speed in dual-dynamic scenarios. This invention proposes an adaptive law driven by estimation error. By introducing auxiliary filtering variables to reconstruct the measurable signals, estimation error information related to unknown depth parameters is extracted. This adaptive law requires no prior geometric knowledge of the target motion; it utilizes only measurable signals such as image feature velocity, joint velocity, and the velocity of target feature points in the base coordinate system to achieve online real-time estimation of time-varying depth.

[0070] 3. Existing technologies employ multi-sensor fusion or binocular vision solutions to address the depth estimation problem, but these increase system cost, complexity, and computational burden. This invention relies solely on a monocular camera and compensates for the lack of depth information through algorithmic innovation. The entire solution requires no additional hardware, only a standard robot forward kinematics model and camera calibration parameters, making it easy to integrate into existing hand-eye robot systems, reducing system complexity and cost, and suitable for monocular vision systems. Attached Figure Description

[0071] Figure 1 This is a schematic diagram of the method flow of the present invention;

[0072] Figure 2 This is a graph showing the estimation error in Example 3;

[0073] Figure 3 This is the pose error diagram for Example 3;

[0074] Figure 4 This is the speed error diagram for Example 3;

[0075] Figure 5 This is a trajectory diagram of the target object and the robot's end effector in Example 3. Detailed Implementation

[0076] The present invention will be further described below with reference to the embodiments and accompanying drawings, but this does not limit the present invention in any way. Any changes or substitutions made based on the teachings of the present invention shall fall within the protection scope of the present invention.

[0077] Example 1

[0078] This embodiment of the three-dimensional spatial dynamic depth estimation method for monocular hand-eye robot visual servoing includes the following steps:

[0079] S1. Establish a parametric visual servoing system model: Based on the forward kinematics model of the hand-eye robot, the intrinsic and extrinsic parameters of the camera, and the image Jacobian matrix, perform a unified parametric modeling of the closed-loop system consisting of the robot, camera, and target.

[0080] S2. Construct an estimation error-driven adaptive law: For the system model obtained in step S1, introduce auxiliary filter variables to reconstruct the measurable signal and extract estimation error information related to the unknown depth parameters; based on the estimation error information, design an adaptive law that satisfies the Lyapunov convergence condition to estimate the time-varying depth online in real time, obtain the depth estimate, and ensure the convergence of the estimation error in a provable manner.

[0081] S3. Perform position-based visual servo control: Using the depth estimate obtained in real time in step S2, back-project the image plane feature points to the robot base coordinate system to obtain the estimated value of the target's three-dimensional position; define the tracking error between the target's three-dimensional position and the desired position, design a feedback controller in combination with the robot dynamics model, and map the tracking error into joint torque and speed commands to drive the robot to move until the tracking error converges to a preset threshold range.

[0082] Example 2

[0083] The three-dimensional spatial dynamic depth estimation method for monocular hand-eye robot visual servoing in this embodiment is based on Embodiment 1, wherein:

[0084] Step S1 specifically includes the following steps:

[0085] S101. Define the three-dimensional spatial coordinates of the target feature points in the camera coordinate system as follows: The coordinates of this point projected onto the camera's imaging plane are: The three-dimensional spatial coordinates in the camera coordinate system and the imaging plane coordinates have the following relationship:

[0086] ;

[0087] S102. Define the coordinates of the feature points in the pixel plane as follows: The pixel plane coordinates and the imaging plane coordinates have the following relationship:

[0088] ;

[0089] in, The camera along The principal point position of the axis, The camera along The focal lengths of the axes together form the camera intrinsic parameter matrix. :

[0090] ;

[0091] S103, Formula With formula United:

[0092] ;

[0093] Take the formula Time derivative:

[0094] ;

[0095] Relating image feature velocity to camera spatial velocity:

[0096] ;

[0097] If the coordinates of the feature points do not change with time, then Let be the camera velocity in the camera coordinate system; if the coordinates of the feature points change with time, then... The camera velocity in the camera coordinate system minus the feature point velocity in the camera coordinate system;

[0098] make:

[0099] ;

[0100] Formula Simplify and rewrite as

[0101] ;

[0102] S104. Using the robot's forward kinematics, obtain the rotation matrix between the base coordinate system and the robot's end effector coordinate system. Translation vector The rotation matrix between the robot's end effector coordinate system and the camera coordinate system is obtained using camera extrinsic parameter calibration. Translation vector Using the rotation matrix and translation vector between the coordinate systems, the end velocity in the base coordinate system is converted into the camera velocity in the camera coordinate system. The conversion relationship is as follows:

[0103] ;

[0104] in, It is the terminal velocity in the base coordinate system; It is the camera speed in the camera coordinate system; Represented by translation vector The oblique symmetric matrix formed by the three parameters;

[0105] S105, via the robot Jacobian matrix With robot joint speed , For the robot's degrees of freedom, obtain the end effector velocity in the base coordinate system:

[0106] ;

[0107] For the formula Parameterize, let , , We can obtain:

[0108] ;

[0109] in, , ;

[0110] This completes the construction of the parametric visual servo system model;

[0111] Step S2 specifically includes the following steps:

[0112] S201. For the parametric visual servo system model (formula (11)), the filter variables are defined as follows:

[0113] ;

[0114] in These are the filter coefficients, which can be set to a very small constant value;

[0115] S202, Define the intermediate matrix , :

[0116] ;

[0117] in, Used to ensure the intermediate regression matrix Bounded, constant It acts as a forgetting factor, and can be considered a small constant; if The smaller the value, the more data the matrix contains. Historical information; then the formula The solution is:

[0118] ;

[0119] in, It is an estimation error; It is the residual of the derivative of the time-varying parameter after low-pass filtering;

[0120] S203, Next, define auxiliary variables. , :

[0121] ;

[0122] Therefore, it can be seen that the estimation error of the time-varying parameter has been included in the auxiliary variable. In this process, an adaptive estimation law driven by estimation error is constructed using auxiliary variables:

[0123] ;

[0124] in, It is a constant learning gain; It is also a constant used to balance the ability to estimate rapidly changing parameters and robustness; the adaptive law is calculated from this. Then obtain through points And because Therefore, it is possible to obtain an estimate of depth information. The three-dimensional estimated coordinates of the target feature points in the robot's base coordinate system are reconstructed by using the pixel coordinates of the target feature points inside and outside the camera. Participating in visual servo control;

[0125] Step S3 specifically includes the following steps:

[0126] S301. Obtain the pixel coordinates of the target feature points using the camera. Then, the estimated depth information is used to obtain the coordinates of the feature points in the camera coordinate system:

[0127] ;

[0128] S302. Use the transformation matrices to convert the feature point coordinates in the camera coordinate system to the feature point coordinates in the robot base coordinate system:

[0129] ;

[0130] S303. To ensure the robot's end effector reaches above the target feature point and moves with it, the error is defined by combining the estimated coordinates of the feature point in the base coordinate system with the robot's end effector posture:

[0131] ;

[0132] in It is the pose of the target feature point in the robot's 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. This represents the pose of the robot's end effector in the robot's base coordinate system; and the time derivative of the error is taken:

[0133] ;

[0134] S304, the controller design is based on Dynamics model of a robot with degrees of freedom:

[0135] ;

[0136] in, These represent the robot's joint displacement, velocity, and acceleration, respectively. It is the torque of inertia. It is the centripetal torque. It is the gravitational torque. It is the control torque of the robot;

[0137] S305 uses a PD controller, and the controller design is as follows:

[0138] ;

[0139] Among them, diagonal array It is about controlling the gain. These are the pose error and velocity error defined above, respectively.

[0140] Example 3

[0141] This invention utilizes an adaptive estimation law driven by estimation error to rapidly estimate depth information for robot control. This embodiment verifies the effectiveness of the method by building a 7-DOF robot model in MATLAB / Simulink. Simulation results show that the method solves the problem of difficulty in measuring nonlinear time-varying depth in monocular camera visual servoing when there is prior geometric knowledge of the observed object and both the target and the robot are moving.

[0142] Specifically, in the Simulink simulation, the method of Example 2 was used to implement visual servo control, and the results were observed. The simulation results are as follows: Figure 2 The estimation error is the difference between the depth information estimated by the adaptive law and the true depth information. As can be seen from the figure, the adaptive estimation law of the present invention can make the error between the true depth information and the estimated value quickly converge to 0. Figure 3-4 The figure shows that the pose error and velocity error are represented by the figure. It can be seen from the figure that after the motion trajectory of the target feature point is given, the controller used in this invention can make the pose error and velocity error converge to 0 quickly. Figure 5 For the trajectory of the robot's end effector and the target object, based on the error definition in step S3, in actual operation, the robot's end effector and the target feature point cannot coincide. If they do coincide, the pixel coordinates will expand rapidly, leading to estimation failure. Therefore, the final result is set as the robot's end effector being located 15cm above the target feature point, and continuous tracking is performed. Figure 3 and Figure 5 As can be seen, the robot end effector can be positioned 15cm above the target feature point within 0.3s after the simulation starts and continues to track it. This shows that the adaptive estimation law and controller can quickly and accurately complete the visual servoing task in a dual dynamic scene using the model given in this invention.

[0143] As can be seen, the method of this invention can design a corresponding adaptive estimation law driven by the estimation error, which is used to estimate feature position and depth parameters. It solves the problem of difficulty in measuring nonlinear time-varying depth in monocular camera visual servoing when there is no prior geometric knowledge of the observed object and both the target and the robot are moving, thus achieving rapid convergence of control and estimation errors in hand-eye robot systems.

Claims

1. A visual servoing method for monocular hand-eye robots based on three-dimensional dynamic depth estimation, characterized in that... Includes the following steps: S1. Establish a parametric visual servoing system model: Based on the forward kinematics model of the hand-eye robot, the intrinsic and extrinsic parameters of the camera, and the image Jacobian matrix, perform a unified parametric modeling of the closed-loop system consisting of the robot, camera, and target. S2. Construct an estimation error-driven adaptive law: For the system model obtained in step S1, introduce auxiliary filter variables to reconstruct the measurable signal and extract the estimation error information related to the unknown depth parameters. Based on the estimation error information, an adaptive law satisfying the Lyapunov convergence condition is designed to perform online real-time estimation of time-varying depth, obtain depth estimates, and guarantee the convergence of estimation errors in a provable manner. S3. Perform position-based visual servo control: Using the depth estimation value obtained in real time in step S2, back-project the image plane feature points to the robot base coordinate system to obtain the estimated value of the target's three-dimensional position; define the tracking error between the target's three-dimensional position and the desired position, design a feedback controller in combination with the robot dynamics model, and map the tracking error into joint torque and velocity commands to drive the robot to move until the tracking error converges to a preset threshold range; Specifically, step S2 includes the following steps: S201. For the parametric visual servoing system model, the filter variables are defined as follows: ； in These are the filter coefficients, set to a very small constant value; S202, Define the intermediate matrix , : ； in, Used to ensure the intermediate regression matrix Bounded, constant It acts as a forgetting factor, as a small constant; if The smaller the value, the more data the matrix contains. The historical information; then the solution to this formula is: ； in, It is an estimation error; It is the residual of the derivative of the time-varying parameter after low-pass filtering; S203, Next, define auxiliary variables. , : ； Therefore, it can be seen that the estimation error of the time-varying parameter has been included in the auxiliary variable. In this process, an adaptive estimation law driven by estimation error is constructed using auxiliary variables: ； in, It is a constant learning gain; It is also a constant used to balance the ability to estimate rapidly changing parameters and robustness; the adaptive law is calculated from this. Then obtain through points And because Therefore, it is possible to obtain an estimate of depth information. The three-dimensional estimated coordinates of the target feature points in the robot's base coordinate system are reconstructed by using the pixel coordinates of the target feature points inside and outside the camera. Participate in visual servo control.

2. The monocular hand-eye robot visual servoing method for three-dimensional spatial dynamic depth estimation according to claim 1, characterized in that... Step S1 specifically includes the following steps: S101. Define the three-dimensional spatial coordinates of the target feature points in the camera coordinate system as follows: The coordinates of this point projected onto the camera's imaging plane are: The three-dimensional spatial 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 follows: The pixel plane coordinates and the imaging plane coordinates have the following relationship: ； in, The camera along The principal point position of the axis, The camera along The focal lengths of the axes together form the camera intrinsic parameter matrix. : ； S103. Combine the relationship between the three-dimensional spatial coordinates and the imaging plane coordinates from step S101, and the relationship between the pixel plane coordinates and the imaging plane coordinates from step S102: ； Take the time derivative of this formula: ； Relating image feature velocity to camera spatial velocity: ； If the coordinates of the feature points do not change with time, then Let be the camera velocity in the camera coordinate system; if the coordinates of the feature points change with time, then... The camera velocity in the camera coordinate system minus the feature point velocity in the camera coordinate system; make: ； Simplify and rewrite the formula as follows ； S104. Using the robot's forward kinematics, obtain the rotation matrix between the base coordinate system and the robot's end effector coordinate system. Translation vector The rotation matrix between the robot's end effector coordinate system and the camera coordinate system is obtained using camera extrinsic parameter calibration. Translation vector Using the rotation matrix and translation vector between the coordinate systems, the end velocity in the base coordinate system is converted into the camera velocity in the camera coordinate system. The conversion relationship is as follows: ； in, It is the terminal velocity in the base coordinate system; It is the camera speed in the camera coordinate system; Represented by translation vector The oblique symmetric matrix formed by the three parameters; S105, via the robot Jacobian matrix With robot joint speed , For the robot's degrees of freedom, obtain the end effector velocity in the base coordinate system: ； The formula obtained in step S103 is parameterized, and then... , , We can obtain: ； in, , ; This completes the construction of the parametric visual servo system model.

3. The monocular hand-eye robot visual servoing method for three-dimensional spatial dynamic depth estimation according to claim 1, characterized in that... Step S3 specifically includes the following steps: S301. Obtain the pixel coordinates of the target feature points using the camera. Then, the estimated depth information is used to obtain the coordinates of the feature points in the camera coordinate system: ； S302. Use the transformation matrices to convert the feature point coordinates in the camera coordinate system to the feature point coordinates in the robot base coordinate system: ； S303. To ensure the robot's end effector reaches above the target feature point and moves with it, the error is defined by combining the estimated coordinates of the feature point in the base coordinate system with the robot's end effector posture: ； in It is the pose of the target feature point in the robot's 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. This represents the pose of the robot's end effector in the robot's base coordinate system; and the time derivative of the error is taken: ； S304, the controller design is based on Dynamics model of a robot with degrees of freedom: ； in, These represent the robot's joint displacement, velocity, and acceleration, respectively. It is the torque of inertia. It is the centripetal torque. It is the gravitational torque. It is the control torque of the robot; S305 uses a PD controller, and the controller design is as follows: ； Among them, diagonal array It's about controlling the gain. These are the pose error and velocity error defined above, respectively.

Images