Motion information and visual information fused mechanical arm space pose self-adaptive estimation method

Abstract

A motion information and visual information fused mechanical arm space pose self-adaptive estimation method comprises the steps that firstly, an internal reference matrix of a binocular camera is combined, and three-dimensional reconstruction of two-dimensional features of an image is achieved based on an epipolar constraint method; then, hand-eye calibration is completed in a laser tracker and binocular camera combined measurement mode, and the relative position relation between the machining tail end of the mechanical arm and the center of the workpiece is calculated based on the calibration relation; and finally, a self-adaptive extended Kalman filter is provided, a known binocular camera is combined to measure a noise covariance matrix, state noise covariance matrixes of the tail ends of the mechanical arms at different positions are adjusted in a self-adaptive mode, and smooth estimation of the tail end positions in the movement process of the mechanical arms is achieved. And finally, the tail end of the mechanical arm accurately moves to the effective view field range of the binocular camera. According to the method, the problem that the tail end position of the mechanical arm is difficult to accurately estimate under the complex working condition is solved, and self-adaptive accurate estimation of the tail end position of the mechanical arm can be achieved without priori accurate observation and motion noise covariance matrixes.

Description

Technical Field

[0001] The invention belongs to the field of digital measurement and robots, and relates to a method for adaptively estimating the spatial posture of a mechanical arm by fusing motion information with visual information. Background Art

[0002] The size of high-end equipment components in the aerospace field is getting larger and larger, and the structure is becoming more and more complex, which has gradually exceeded the operating range of machine tools. Mobile industrial robotic arms have great potential in the processing of such large parts due to their advantages such as large operating range, good flexibility and high efficiency. However, the absolute positioning accuracy of industrial robotic arms is low, and it is difficult to meet the processing accuracy requirements by relying solely on their own positioning accuracy. Additional accuracy assurance technology must be introduced. At present, the methods to improve the absolute positioning accuracy of robotic arms mainly include offline calibration method and online positioning method. The former requires calibration and compensation of each joint parameter of the robotic arm to improve the absolute positioning accuracy of the robotic arm, but it is difficult to accurately calibrate the joint parameters when the field environment is complex and the end load changes dynamically; the latter uses additional sensors to obtain information on the end of the robotic arm. Through online feedback of measurement information, the current actual posture of the end of the robotic arm can be accurately obtained, without serial calculation through each joint parameter, thereby ensuring the absolute positioning accuracy of the end of the robotic arm. This method has great potential in robot-assisted positioning.

[0003] Among them, the robot end-assisted positioning of "eyes on hands" is one of the most widely used online positioning methods. That is, a binocular camera is installed at the end of the robotic arm, and the coordinate information of the positioning points in the area to be measured is obtained at close range through the binocular camera, and the positioning information is transmitted to the robotic arm control system, thereby realizing high-precision positioning of the end of the robotic arm. However, the field of view and depth of field of the binocular vision camera are limited. Under complex working conditions, it is often difficult for the end of the robotic arm to move directly to the binocular positioning area, which seriously reduces the robustness and reliability of binocular positioning. In order to assist the end of the robotic arm to move reliably to the binocular vision measurable area, it is also necessary to accurately estimate the spatial pose of the end of the robotic arm in combination with the real-time motion information of the robotic arm. The traditional spatial pose estimation method of the end of the robotic arm requires a priori accurate observation and motion state noise covariance matrix. It has insufficient adaptive ability under complex working conditions, and still needs an adaptive pose estimation method for the end of the robotic arm that is suitable for complex sites.

[0004] The patent "A method for self-calibration of robot arm kinematics based on binocular vision", patent number ZL202111045806.9, discloses a method for calculating the end position of the robot arm by combining camera information and joint angle information. The patent "A method for positioning and grasping a robot arm target based on binocular vision", patent ZL201710936467.0, discloses a method for estimating the grasping posture of the robot arm based on the internal parameters and image information of the binocular camera. The above two inventions realize the estimation of the robot arm posture through binocular cameras and other robot arm parameters, but the robot arm still faces the problem of difficulty in accurately calibrating the robot arm joint parameters during operation, and it is difficult to accurately estimate the end position of the robot arm based on motion information alone, and the robot arm cannot be accurately moved to the effective field of view of the binocular camera. Therefore, the present invention proposes a method for adaptively estimating the spatial posture of the robot arm by fusing motion information and visual information, which realizes adaptive and accurate estimation of the end position of the robot arm. Summary of the invention

[0005] The present invention is aimed at the positioning needs in the operation scenes of industrial robotic arms. In order to solve the problem that it is difficult to accurately estimate the position information of the processing end by relying solely on the motion information of the robotic arm itself, a method for adaptively estimating the spatial position of the robotic arm by fusing motion information and visual information is invented. First, the intrinsic parameter matrix of the binocular camera is combined to realize the three-dimensional reconstruction of the two-dimensional features of the image based on the polar constraint method; then, the "hand-eye calibration" is completed by combining the laser tracker and the binocular camera for measurement, and the relative position relationship between the processing end of the robotic arm and the center of the workpiece is calculated based on the calibration relationship; finally, an improved adaptive extended Kalman filter is proposed, combined with the known binocular camera measurement noise covariance matrix, and the state noise covariance matrix of the robotic arm end at different positions is adaptively adjusted to realize the smooth estimation of the end position of the robotic arm during the movement of the robotic arm, and finally the end of the robotic arm is accurately moved to the measurable range of the binocular camera, which solves the problem that the position of the robotic arm end is difficult to accurately estimate under complex working conditions, takes into account both accuracy and robustness, and has good versatility and generalizability.

[0006] The technical solution of the present invention:

[0007] A method for adaptively estimating the spatial pose of a robotic arm by fusing motion information and visual information, the steps are as follows:

[0008] The first step is to obtain the three-dimensional coordinates of the feature points in the camera coordinate system

[0009] First, calibrate the internal and external parameters of the binocular camera to obtain the internal parameter matrix K of the left and right cameras. L and K RAnd the external parameter matrix R and t; then, arrange the visual landmarks in the local area to be tested, use the binocular camera to obtain the image of the area to be tested, and then divide the image into the foreground and background of the image by binary, and then perform edge detection on the image according to the edge detection algorithm. Finally, the circularity constraint and area constraint are used to filter the image features. The result after image filtering is as shown in formula (1):

[0010]

[0011] Among them, (S min ,S max ) is the area threshold, T C is the roundness threshold, C is the perimeter of each region outline, S is the area of ​​each region outline, and e represents the roundness of each region outline;

[0012] Then, the basic matrix is ​​calculated according to the intrinsic parameter matrix of the stereo camera:

[0013]

[0014] in, The antisymmetric matrix calculated for the translation vector between the left and right cameras;

[0015] Secondly, the center coordinates [u L v L 1] T and[u R v R 1] T And construct the following equation:

[0016]

[0017] Among them, [abc] T is the basis matrix F and [u R v R 1] T The product of

[0018] Converting equation (3) into a straight line equation is the polar line equation:

[0019] au L +bv L +c=0 (4)

[0020] When the distance between an image point and the corresponding epipolar line is less than the set constraint threshold, it is considered to be a matching point corresponding to the image point of another image;

[0021] Finally, the depth information Z of each landmark point is combined to achieve the three-dimensional reconstruction of the landmark point:

[0022]

[0023] Among them, (XYZ) is the three-dimensional coordinates of the marker point in the camera coordinate system, c x and c y is the pixel coordinate of the camera principal point, f is the focal length of the camera, d is the parallax of the left and right images, and B is the distance between the optical centers of the left and right cameras;

[0024] The second step is online positioning of the end of the robot arm based on combined measurement

[0025] First, place n laser tracker and visual cooperation target points on the calibration board, and use the laser tracker and binocular camera to measure their 3D coordinates respectively:

[0026] P c ={p1 p2 p3 …p n}, Q l ={q1 q2 q3 …q n} (6)

[0027] Among them, P c and Q l are the three-dimensional coordinates of the visual cooperation target point in the camera coordinate system and the laser tracker coordinate system respectively;

[0028] Then, the transformation relationship between the camera coordinate system and the laser tracker coordinate system is solved based on the least squares principle:

[0029]

[0030] in, are the rotation matrix and translation vector between the two coordinate systems respectively;

[0031] Then, three laser tracker targets are installed on the tooling of the binocular camera, and their coordinates are measured using the laser tracker. The tooling coordinate system is established with these three points, and the coordinates of these three points in the tooling coordinate system are calculated:

[0032]

[0033] Then, the conversion relationship R between the tooling coordinate system and the laser tracker coordinate system is calculated. f l and t f l Based on the relationship between the camera coordinate system and the laser tracker coordinate system, the relationship between the camera coordinate system and the tooling coordinate system R is calculated. c f and t c f :

[0034]

[0035] Then, by changing the position of the robot arm, the transformed position between the tooling coordinate systems under multiple robot arm positions can be obtained. And the transformation pose between the coordinate system of the end of the robot It represents the posture transformation relationship of the tooling coordinate system from the jth posture to the ith posture state. Representing the posture transformation relationship of the robot end coordinate system from the jth posture to the ith posture state, the following relationship can be obtained:

[0036]

[0037] in, and They are the relationship between the tooling coordinate system and the robot end coordinate system at the j-th and i-th positions, respectively. The transformation relationship is fixed at any position.

[0038] Right now:

[0039]

[0040] Among them, R and t represent the rotation part and translation part of the corresponding posture transformation relationship T respectively;

[0041] The calculation process can be regarded as solving a typical nonlinear optimization problem AX = XB. By solving the above equations, we can get That is, the conversion relationship between the tooling coordinate system and the robot end coordinate system Combined with formula (9), the transformation relationship R between the camera coordinate system and the robot end coordinate system can be calculated: c e ,t c e , that is, the "hand-eye calibration" is completed:

[0042]

[0043] After obtaining the "hand-eye calibration" relationship, a laser tracker target is fixed at the processing end on the flange at the end of the robot arm, and it is used as the positioning object. The laser tracker is used to measure and obtain its coordinates: Based on the conversion relationship between the laser tracker and the binocular camera in formula (7): The three-dimensional coordinates of the processing end positioning point in the camera coordinate system can be obtained

[0044]

[0045] Then, the three-dimensional coordinates of the center of the workpiece to be processed in the camera coordinate system are obtained according to formula (5): Based on the posture information obtained by the robot itself, the transformation relationship between the robot end coordinate system and the robot base coordinate system is solved as follows: Solve the three-dimensional coordinates of the machining end and the workpiece center in the robot base coordinate system and

[0046]

[0047] in, It is the conversion relationship between the camera coordinate system and the robot end coordinate system

[0048] Finally, the relative position between the machining end and the center of the workpiece is calculated and transmitted to the robot arm controller to control the robot arm end to reach the target area along the specified path.

[0049] The third step is to estimate the end position of the manipulator based on the improved adaptive extended Kalman filter

[0050] In order to enable the robotic arm to reach the target area accurately, the end position of the robotic arm during its movement is dynamically estimated through an improved adaptive extended Kalman filter.

[0051] First, calculate the observation matrix of the system and linearize it:

[0052] Z k =H·X k +H·X k|k-1 +v k (15)

[0053] Among them, v k is the system observation noise with mean 0 and covariance matrix R, H is the observation matrix, X k is the state vector of the robot end at time k, X k|k-1 It is the state prediction at time k based on the information at time k-1;

[0054] Then, the motion state prediction equation of the end of the robotic arm is:

[0055] X k =A·X k-1 +Bu k +w k (16)

[0056] Among them, A is the state transfer matrix, B is the control input matrix, and u k is the control vector, w k is the motion state noise with mean 0 and covariance matrix Q;

[0057] Based on the initialized motion state noise covariance matrix Q, the adjustment value ΔQ is introduced. The covariance matrix R of the observation noise can be obtained through calibration. It is a known quantity. Then the covariance matrix estimation error comes from ΔQ. The prediction equation of the actual error covariance matrix is ​​adjusted as follows:

[0058]

[0059] Among them, P k is the error covariance matrix at time k, P k|k-1 is the error covariance matrix of the prediction at time k based on the information at time k-1, P k-1 and Q k-1 are the observation and motion state noise covariance matrices at time k-1, ΔQ k-1 is the k-1 time adjusted motion state noise covariance matrix;

[0060] According to the covariance matching theory of Kalman filtering, the covariance expectation of the observation residual can be expressed as:

[0061]

[0062] in, is the observation residual;

[0063] Then, based on the expectation of the observed residual, the formula for adjusting the motion state noise covariance matrix is ​​derived:

[0064] ΔQ=(K·H) -1 (KE(ε k ·ε k T )K T -P k|k-1 H T ·K T )·((K·H) -1 ) T (19)

[0065] Where K is the Kalman gain;

[0066] Finally, based on the above information, the motion state noise covariance matrix is ​​updated Motion state matrix X k , error covariance matrix:

[0067]

[0068] Process all observation data in sequence and dynamically adjust the motion state noise variance matrix The system's state estimation is updated based on the new observation values, so that the processing end of the robotic arm can be moved smoothly and accurately into the effective field of view of the binocular camera.

[0069] Beneficial effects of the present invention: The present invention proposes an improved adaptive Kalman filter, which can achieve accurate dynamic estimation of the position of the end of the robot arm through a cyclic process of prediction-update-noise estimation-prediction, without the need for a priori accurate observation and motion state noise covariance matrix, and also ensures that the robot arm system moves accurately, smoothly and stably within the binocular field of view, taking into account both accuracy and robustness, and solves the problem that the end position of the robot arm is difficult to accurately estimate under complex working conditions, and has broad application prospects. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] Figure 1 It is a flow chart of the method for adaptively estimating the spatial pose of a robotic arm by fusing motion information and visual information.

[0071] Figure 2 It is a scene diagram of the pose estimation of the end of the robot arm during the processing of thin-walled tube segments.

[0072] Figure 3 It is a schematic diagram of establishing a binocular camera tooling coordinate system.

[0073] Figure 4 is the estimated error of the end position of the robot arm.

[0074] In the figure: 1-robot arm; 2-binocular camera; 3-camera tooling; 4-laser tracker; 5-calibration board; 6-cooperative target point; 7-laser tracker target point; 8-processing end; B -X B Y B Z B is the robot base coordinate system, O C -X C Y C Z C is the camera coordinate system, O L -X L Y L Z L is the laser tracker coordinate system. DETAILED DESCRIPTION

[0075] The specific implementation of the present invention is described in detail below in combination with the technical scheme and the accompanying drawings.

[0076] This embodiment uses an online positioning scene in the process of simulating a robot arm processing a simplified thin-walled tube segment, including 5 cooperative target points and 3 laser tracker target points.

[0077] The first step is to obtain the three-dimensional coordinates of the feature points in the camera coordinate system

[0078] First, calibrate the internal and external parameters of the camera to obtain the following internal parameter matrix K l , K r And the external parameter matrix R, t:

[0079]

[0080] Then, set 5 cooperative targets on the calibration board and use the binocular camera on the robotic arm to measure and obtain the coordinates of each target in the camera coordinate system:

[0081]

[0082] The second step is online positioning of the end of the robot arm based on combined measurement

[0083] Use a laser tracker to measure the coordinates of the cooperative target point. According to the acquired three-dimensional coordinates of the marker point, calculate the conversion relationship between the camera coordinate system and the laser tracker coordinate system. Then, set three laser tracker target bases on the tooling. Use the laser tracker to measure the coordinates of each point to establish the tooling coordinate system, and then establish the relationship between the tooling coordinate system and the camera coordinate system. Then, change the position and posture of the robot arm to obtain the position and posture transformation relationship between multiple tooling coordinate systems and the robot arm end coordinate system. Then, based on equations (10), (11), and (12), complete the "hand-eye calibration". Install an electric spindle at the end of the robot arm, fix a laser tracker target base on the electric spindle as the positioning object, and use the laser tracker to measure its coordinates. Finally, calculate the three-dimensional coordinates of the model end and the workpiece center in the robot arm base coordinate system according to equations (13) and (14):

[0084]

[0085] The third step is to estimate the end position of the manipulator based on the improved adaptive extended Kalman filter

[0086] Based on the above steps, the processing end needs to move from the initial position (281.84, 5.82, 834.19) to (281.84, -84.18, 744.19) along a straight line, combining visual measurement information and robotic arm motion state information, and using an adaptive Kalman filter to achieve accurate positioning of the end.

[0087] Set the initial parameters as follows:

[0088] The initial state quantity X is:

[0089] X=[281.84,5.82,834.19,-38.4,-30.3] T

[0090] Initialize the state transfer matrix A = I(3), input matrix B = [1,1,1];

[0091] Initialize the motion state noise covariance matrix as:

[0092] Q=0.0001·I(3)

[0093] Initialize the observation noise covariance matrix:

[0094] R=0.0025·I(15)

[0095] The improved adaptive extended Mann filter dynamically adjusts the noise covariance matrix of the end motion state of the robot arm to achieve accurate estimation of the end position. During the movement of the robot arm, the end position is evenly measured 7 times using a laser tracker. The measurement results are compared with the estimated end position and the error is calculated, as shown in the attached figure. Figure 4 , verifying the effectiveness of the improved adaptive extended Kalman filter.

[0096] The present invention proposes an improved adaptive extended Kalman filter, which can realize accurate dynamic estimation of the end position of the robot arm through a cyclic process of prediction-update-noise estimation-prediction, without the need for a priori accurate observation and motion state noise covariance matrix, and also ensures that the robot arm system moves accurately, smoothly and stably within the field of view of the binocular camera, taking into account both accuracy and robustness, and solving the problem of accurate estimation of the end position of the robot arm under complex working conditions. It has broad application prospects.

Claims

1. A method for adaptively estimating the spatial pose of a robotic arm by fusing motion information and visual information, characterized in that: Here are the steps: The first step is to obtain the three-dimensional coordinates of the feature points in the camera coordinate system First, calibrate the internal and external parameters of the binocular camera to obtain the internal parameter matrix K of the left and right cameras. L and K R And the external parameter matrix R and t; then, arrange the visual landmarks in the local area to be tested, use the binocular camera to obtain the image of the area to be tested, and then divide the image into the foreground and background of the image by binary, and then perform edge detection on the image according to the edge detection algorithm. Finally, the circularity constraint and area constraint are used to filter the image features. The result after image filtering is as shown in formula (1): Among them, (S min ,S max ) is the area threshold, T C is the roundness threshold, C is the perimeter of each region outline, S is the area of ​​each region outline, and e represents the roundness of each region outline; Then, the basic matrix is ​​calculated according to the intrinsic parameter matrix of the stereo camera: in, The antisymmetric matrix calculated for the translation vector between the left and right cameras; Secondly, the center coordinates [u L v L 1] T and[u R v R 1] T And construct the following equation: Among them, [abc] T is the basis matrix F and [u R v R 1] T The product of Converting equation (3) into a straight line equation is the polar line equation: At L +bv L +c=0 (4) When the distance between an image point and the corresponding epipolar line is less than the set constraint threshold, it is considered to be a matching point corresponding to the image point of another image; Finally, the depth information Z of each landmark point is combined to achieve the three-dimensional reconstruction of the landmark point: Among them, (XYZ) is the three-dimensional coordinates of the marker point in the camera coordinate system, c x and c y is the pixel coordinate of the camera principal point, f is the focal length of the camera, d is the parallax of the left and right images, and B is the distance between the optical centers of the left and right cameras; The second step is online positioning of the end of the robot arm based on combined measurement First, place n laser tracker and visual cooperation target points on the calibration board, and use the laser tracker and binocular camera to measure their 3D coordinates respectively: P c ={p1 p2 p3 …p n }, Q l ={q1 q2 q3 …q n } (6) Among them, P c and Q l are the three-dimensional coordinates of the visual cooperation target point in the camera coordinate system and the laser tracker coordinate system respectively; Then, the transformation relationship between the camera coordinate system and the laser tracker coordinate system is solved based on the least squares principle: in, are the rotation matrix and translation vector between the two coordinate systems respectively; Then, three laser tracker targets are installed on the tooling of the binocular camera, and their coordinates are measured using the laser tracker. The tooling coordinate system is established with these three points, and the coordinates of these three points in the tooling coordinate system are calculated: Then, the conversion relationship R between the tooling coordinate system and the laser tracker coordinate system is calculated. f l and t f l Based on the relationship between the camera coordinate system and the laser tracker coordinate system, the relationship between the camera coordinate system and the tooling coordinate system R is calculated. c f and t c f : R c f =(R f l ) -1 R c l ,t c f =(R f l ) -1 (t c l -t f l ) (9) Then, by changing the position of the robot arm, the transformed position between the tooling coordinate systems under multiple robot arm positions can be obtained. And the transformation pose between the coordinate system of the end of the robot It represents the posture transformation relationship of the tooling coordinate system from the jth posture to the ith posture state. Representing the posture transformation relationship of the robot end coordinate system from the jth posture to the ith posture state, the following relationship can be obtained: in, and They are the relationship between the tooling coordinate system and the robot end coordinate system at the j-th and i-th poses, respectively. The transformation relationship is fixed at any pose. Right now: Among them, R and t represent the rotation part and translation part of the corresponding posture transformation relationship T respectively; This calculation process can be regarded as solving a typical nonlinear optimization problem AX=XB. By solving the above equations, we can get That is, the conversion relationship between the tooling coordinate system and the robot end coordinate system Combined with formula (9), the transformation relationship R between the camera coordinate system and the robot end coordinate system can be calculated: c e ,t c e , that is, "hand-eye calibration" is completed: After obtaining the "hand-eye calibration" relationship, a laser tracker target is fixed at the processing end on the flange at the end of the robot arm, and it is used as the positioning object. The laser tracker is used to measure and obtain its coordinates as P l m , based on the conversion relationship T between the laser tracker and the binocular camera in formula (7) l c , the three-dimensional coordinates of the processing end positioning point in the camera coordinate system can be obtained Then, according to formula (5), the three-dimensional coordinates of the center of the workpiece to be processed in the camera coordinate system are obtained: Based on the posture information obtained by the robot itself, the transformation relationship between the robot end coordinate system and the robot base coordinate system is solved as follows: Solve the three-dimensional coordinates of the machining end and the workpiece center in the robot base coordinate system and in, It is the conversion relationship between the camera coordinate system and the robot end coordinate system Finally, the relative position between the machining end and the center of the workpiece is calculated and transmitted to the robot arm controller to control the robot arm end to reach the target area along the specified path. The third step is to estimate the end position of the manipulator based on the improved adaptive extended Kalman filter In order to enable the robotic arm to reach the target area accurately, the end position of the robotic arm during its movement is dynamically estimated through an improved adaptive extended Kalman filter. First, calculate the observation matrix of the system and linearize it: Z k =H·X k +H·X k|k-1 +v k (15) Among them, v k is the system observation noise with mean 0 and covariance matrix R, H is the observation matrix, X k is the state vector of the end of the robot at time k, X k|k-1 It is the state prediction at time k based on the information at time k-1; Then, the motion state prediction equation of the end of the robotic arm is: X k =A·X k-1 +Bu k +w k (16) Among them, A is the state transfer matrix, B is the control input matrix, and u k is the control vector, w k is the motion state noise with mean 0 and covariance matrix Q; Based on the initialized motion state noise covariance matrix Q, the adjustment value ΔQ is introduced. The covariance matrix R of the observation noise can be obtained through calibration. It is a known quantity. Then the covariance matrix estimation error comes from ΔQ. The prediction equation of the actual error covariance matrix is ​​adjusted as follows: Among them, P k is the error covariance matrix at time k, P k|k-1 is the error covariance matrix of the prediction at time k based on the information at time k-1, P k-1 and Q k-1 are the observation and motion state noise covariance matrices at time k-1, ΔQ k-1 is the k-1 adjusted motion state noise covariance matrix; According to the covariance matching theory of Kalman filtering, the covariance expectation of the observation residual can be expressed as: in, is the observation residual; Then, based on the expectation of the observed residual, the formula for adjusting the motion state noise covariance matrix is ​​derived: ΔQ=(K H) -1 (KE(ε k ·ε k T )K T -P k|k-1 H T ·K T )·((K·H) -1 , T (19) Where K is the Kalman gain; Finally, based on the above information, the motion state noise covariance matrix is ​​updated Motion state matrix X k , error covariance matrix: Process all observation data in sequence and dynamically adjust the motion state noise variance matrix The system's state estimation is updated based on the new observation values, so that the processing end of the robotic arm can be moved smoothly and accurately into the effective field of view of the binocular camera.

Images