Mechanical arm tail end track planning method for non-common-view-field binocular measurement

Abstract

The invention relates to the technical field of multi-degree-of-freedom mechanical arm control, in particular to a non-common-view-field binocular measurement mechanical arm tail end trajectory planning method, which is characterized in that two cooperative target positions on a station are measured by using two cameras at the tail end of a mechanical arm in a non-common view field, and trajectory planning is performed through a COBYLA nonlinear optimization algorithm. According to the mechanical arm tail end trajectory planning method based on non-common-view-field binocular measurement, the variable quantity of joint movement of the mechanical arm is calculated, the tail end executor reaches the expected position and executes work, and the mechanical arm tail end trajectory planning method based on non-common-view-field binocular measurement has the advantages of being high in optimization speed, accurate in joint trajectory planning result and small in error.

Description

Technical Field

[0001] The invention relates to the technical field of multi-degree-of-freedom robotic arm control, and in particular to a robotic arm end trajectory planning method for non-common-field binocular measurement. Background Art

[0002] Nowadays, with the development of science and technology, intelligent machinery and robotics technology have been widely used in the field of industrial automation to assist humans in production, manufacturing and transportation. For example, parts assembly and welding machines in automobile factories, sorting machines in express stations, picking machines in plantations, etc. have greatly reduced manpower losses, and the probability of human injuries has been greatly reduced in high-risk areas such as earthquake relief, emergency rescue, blasting and mining. Resource exploration robots in deep sea areas and lunar robots that land in space have helped humans broaden their horizons and explore the universe.

[0003] The robotic arm is an important part of robotics technology and plays a key role in industrial production, medical treatment, service industry and scientific exploration. Trajectory planning is an important research direction in the study of robotic arms. Through trajectory planning, continuous and executable motion trajectories can be generated for each joint of the robotic arm under the condition of physical constraints of motion, so as to achieve precise motion of the end of the robotic arm from the initial position to the target position in space, so as to improve work efficiency.

[0004] The existing robot arm trajectory planning methods mainly plan the target position under the same field of view. Once the camera field of view cannot cover a very large object, manual assistance is required, which is time-consuming and labor-intensive. For the robot arm with binocular measurement, it has a larger working field of view, but the existing binocular robot arm trajectory planning methods mainly plan the trajectory for the same target in the common field of view. In the case of non-common field of view, the problem of robot arm end trajectory planning based on the cooperative target position on the workstation remains to be solved. Therefore, in view of the above situation, it is urgent to develop a robot arm end trajectory planning method for non-common field of view binocular measurement to overcome the shortcomings in current practical applications. Summary of the invention

[0005] The purpose of the present invention is to provide a robot arm end trajectory planning method for non-common field of view binocular measurement to solve the problems raised in the above background technology.

[0006] To achieve the above object, the present invention provides the following technical solutions: A method for robot arm end trajectory planning for non-common field of view binocular measurement, comprising the following steps: Step 1: Build a binocular measurement system at the end of the robot arm, install two cameras, and install two cooperative targets on the workstation. Calibrate the two cameras separately and save the calibration results. Step 2: Establish a DH parameter table and apply the standard DH modeling method to describe the kinematic model of the robot arm; Step 3: Collect images, perform image processing on the images acquired by the two cameras, detect the contours of the two cooperative targets, and obtain the position coordinates of the two cooperative targets in the pixel coordinate system respectively; Step 4: According to the homogeneous transformation relationship between the pixel coordinate system, the image coordinate system, the camera coordinate system, the robot end coordinate system and the robot base coordinate system, the position coordinates of the two cooperative targets in the pixel coordinate system are converted to the position coordinates in the robot base coordinate system; Step 5: According to the kinematic model of the robot and the positions of the two cooperative targets, combined with the physical motion limits of the robot joints, the COBYLA nonlinear optimization algorithm and the kinematic model of the robot are used for trajectory planning to solve the change in the motion of the robot joints so that the end points of the horizontal link at the end of the robot reach the positions of the two cooperative targets respectively, and the end effector of the robot reaches the desired position.

[0007] As a further solution of the present invention: the mechanical arm is a four-degree-of-freedom mechanical arm, comprising four joints, wherein the first joint is a horizontal rotation joint, the second joint is a vertical translation joint, the third joint is a horizontal translation joint, and the fourth joint is a horizontal rotation joint; The four joints of the robotic arm are connected in sequence through connecting rods, one end of the first joint is connected to the robotic arm base, and one end of the fourth joint is connected to the end of the robotic arm; a horizontal connecting rod is installed at the end of the robotic arm, and a camera is installed at each end of the connecting rod.

[0008] As a further solution of the present invention: in step 1, two cameras respectively capture chessboard images at multiple angles and multiple times, and use a camera calibration tool to respectively obtain the intrinsic parameter matrix, extrinsic parameter matrix and distortion coefficient of the two cameras.

[0009] As a further solution of the present invention: in step 3, the image processing comprises the following steps: S3.1: According to the camera's internal parameters, the captured image is distorted and corrected, and the noise points in the image are eliminated by using Gaussian filtering. Dynamic threshold segmentation is performed to segment the target area and other areas, and the opening operation is applied to eliminate the small connected parts generated by other areas, and the target area is retained; S3.2: Call the contour point search function in OpenCV to detect the coordinates of all pixel points that make up the contour of the target area, pass all the contour pixel point coordinates to the ellipse fitting function to fit into an ellipse contour, and output the position coordinates of the two cooperative targets in the pixel coordinate system.

[0010] As a further solution of the present invention: in step 4, according to the homogeneous transformation relationship between the pixel coordinate system, the image coordinate system, the camera coordinate system, the robot end coordinate system, and the robot base coordinate system, the position of the target in the robot base coordinate system is obtained, which specifically includes the following steps: S4.1: Obtain the position of the target in the image coordinate system according to the transformation relationship from the pixel coordinate system to the image coordinate system; S4.2: Obtain the position of the target in the camera coordinate system according to the transformation relationship from the image coordinate system to the camera coordinate system; S4.3: According to the transformation relationship from the camera coordinate system to the robot end coordinate system, the position of the target in the robot end coordinate system is obtained; S4.4: According to the transformation relationship from the robot end coordinate system to the robot base coordinate system, the position of the target in the robot base coordinate system is obtained.

[0011] As a further solution of the present invention: in step S4.4, the position coordinates of the target in the robot base coordinate system are obtained. and , the specific relationship operation is as follows: ; in, and Perform image processing on the images acquired by the two cameras respectively, detect the contours of the two cooperative targets, and obtain the position coordinates of the two cooperative targets in the pixel coordinate system respectively; To describe the kinematic model of the robot arm using the standard DH modeling method, that is, the transformation relationship matrix between the robot arm end coordinate system and the robot arm base coordinate system; is the transformation relationship matrix between the camera coordinate system and the robot end coordinate system; is the transformation relationship matrix between the image coordinate system and the camera coordinate system; It is the transformation matrix between the pixel coordinate system and the image coordinate system.

[0012] As a further solution of the present invention: in step 5, the objective function of the COBYLA nonlinear optimization algorithm is the sum of the squares of the distances between the positions of the end points on both sides of the connecting rod in the robot base coordinate system and the positions of the cooperative targets, and the constraint function is the physical motion limit of the robot joints.

[0013] As a further solution of the present invention: the optimization process of the COBYLA nonlinear optimization algorithm includes the following steps: S5.1: Set up a nonlinear constrained optimization problem, where the objective function is the sum of the squares of the distances between the positions of the end points of the connecting rod on both sides and the positions of the cooperative targets in the robot base coordinate system, and the constraint function is the physical motion restrictions of the robot joints; S5.2: Given the initial point of the optimization variable, set the trust region radius, control the search range, and set the convergence error; S5.3: Construct a linear approximation model and use a linear function to approximate the objective function and constraint function near the current point; S5.4: Solve the linear approximation constrained optimization problem in the current trust region to obtain new candidate points; S5.5: Calculate the ratio of the actual improvement value to the predicted improvement value, and automatically adjust the trust region radius according to the ratio; S5.6: Check the termination condition. If the convergence condition is met, stop the optimization. Otherwise, return to S5.3 to continue the optimization.

[0014] As a further solution of the present invention: the error of the end effector of the robotic arm reaching the desired position is within an allowable range, and the change in joint motion is iteratively solved by the COBYLA nonlinear optimization algorithm so that the endpoints on both sides of the horizontal connecting rod at the end of the robotic arm can reach the positions of two cooperative targets respectively.

[0015] As a further solution of the present invention: the cameras on both sides of the horizontal connecting rod at the end of the robotic arm independently collect images of two cooperative targets on the workstation, and the fields of view of the two cameras have no overlapping areas. Through binocular measurement under non-common field of view, the position coordinates of the two cooperative targets in the coordinate system of the robotic arm base are respectively obtained, and trajectory planning is performed based on the position coordinates.

[0016] Compared with the prior art, the present invention has the following beneficial effects: The robot end trajectory planning method of the present invention for non-common field binocular measurement uses a camera to respectively collect two cooperative targets on the workstation under non-common field of view, and uses the homogeneous transformation relationship between each coordinate system to accurately obtain the positions of the two cooperative targets in the robot base coordinate system. The robot kinematic model and COBYLA nonlinear optimization algorithm are used for trajectory planning, and the optimal joint motion variation is iteratively obtained, and the end effector of the robot reaches the desired position. This method has the characteristics of fast optimization speed, accurate joint trajectory planning results and small error. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 It is an overall execution flow chart of an embodiment of the present invention.

[0018] Figure 2 This is a flow chart of trajectory planning of the COBYLA nonlinear optimization algorithm in an embodiment of the present invention.

[0019] Figure 3 This is a result diagram of the cooperation target position calculated in an embodiment of the present invention.

[0020] Figure 4 This is a graph of joint variable results output by trajectory planning in an embodiment of the present invention.

[0021] Figure 5 It is a curve diagram of iterative optimization of four joints of the robotic arm by the COBYLA nonlinear optimization algorithm in an embodiment of the present invention; Among them, the four figures correspond to the optimization iteration curves of the four joints of the robot arm.

[0022] Figure 6 This is a diagram showing the position results of the two end points of the end link of the robot arm after movement according to the optimized joint variables in an embodiment of the present invention.

[0023] Figure 7 A diagram showing the motion trajectory of the robot arm joints in an embodiment of the present invention; Among them, the four figures correspond to the trajectories of the four joints of the robot in sequence. DETAILED DESCRIPTION

[0024] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0025] The specific implementation of the present invention is described in detail below in conjunction with specific embodiments.

[0026] See also Figure 1-Figure 7 , a method for robot arm end trajectory planning for non-common field of view binocular measurement provided by an embodiment of the present invention comprises the following steps: Step 1: Build a binocular measurement system at the end of the robot arm, install two cameras, and install two cooperative targets on the workstation. Calibrate the two cameras separately and save the calibration results. Step 2: Establish a standard DH parameter table and apply the standard DH modeling method to describe the kinematic model of the robot arm; Step 3: Collect images, perform image processing on the images acquired by the two cameras, detect the contours of the two cooperative targets, and obtain the position coordinates of the two cooperative targets in the pixel coordinate system respectively; Step 4: According to the homogeneous transformation relationship between the pixel coordinate system, the image coordinate system, the camera coordinate system, the robot end coordinate system and the robot base coordinate system, the position coordinates of the two cooperative targets in the pixel coordinate system are converted to the position coordinates in the robot base coordinate system; Step 5: According to the kinematic model of the robot and the positions of the two cooperative targets, combined with the physical motion limits of the robot joints, the COBYLA nonlinear optimization algorithm and the kinematic model of the robot are used for trajectory planning to solve the change in the motion of the robot joints so that the end points of the horizontal link at the end of the robot reach the positions of the two cooperative targets respectively, and the end effector of the robot reaches the desired position.

[0027] In one embodiment of the present invention, the robotic arm is a four-degree-of-freedom robotic arm, comprising four joints, wherein the first joint is a horizontal rotation joint, the second joint is a vertical translation joint, the third joint is a horizontal translation joint, and the fourth joint is a horizontal rotation joint; The four joints of the robotic arm are connected in sequence through connecting rods, one end of the first joint is connected to the robotic arm base, and one end of the fourth joint is connected to the end of the robotic arm; a horizontal connecting rod is installed at the end of the robotic arm, and a camera is installed at each end of the connecting rod.

[0028] In one embodiment of the present invention, see Figure 1-Figure 7 ,In step 1, two cameras respectively capture chessboard images at multiple angles and multiple times, and use the camera calibration tool to obtain the intrinsic parameter matrix, extrinsic parameter matrix and distortion coefficient of the two cameras respectively.

[0029] In step 3, image processing includes the following steps: S3.1: According to the camera's internal parameters, the captured image is distorted and corrected, and the noise points in the image are eliminated by using Gaussian filtering. Dynamic threshold segmentation is performed to segment the target area and other areas, and the opening operation is applied to eliminate the small connected parts generated by other areas, and the target area is retained; S3.2: Call the contour point search function in OpenCV to detect the coordinates of all pixel points that make up the contour of the target area, pass all the contour pixel point coordinates to the ellipse fitting function to fit into an ellipse contour, and output the position coordinates of the two cooperative targets in the pixel coordinate system.

[0030] In step 4, the position of the target in the robotic arm base coordinate system is obtained according to the homogeneous transformation relationship between the pixel coordinate system, the image coordinate system, the camera coordinate system, the robotic arm end coordinate system, and the robotic arm base coordinate system, which specifically includes the following steps: S4.1: Obtain the position of the target in the image coordinate system according to the transformation relationship from the pixel coordinate system to the image coordinate system; S4.2: Obtain the position of the target in the camera coordinate system according to the transformation relationship from the image coordinate system to the camera coordinate system; S4.3: According to the transformation relationship from the camera coordinate system to the robot end coordinate system, the position of the target in the robot end coordinate system is obtained; S4.4: According to the transformation relationship from the robot end coordinate system to the robot base coordinate system, the position of the target in the robot base coordinate system is obtained.

[0031] In step S4.4, the position coordinates of the target in the robot base coordinate system are obtained and , the specific relationship operation is as follows: ; in, and Perform image processing on the images acquired by the two cameras respectively, detect the contours of the two cooperative targets, and obtain the position coordinates of the two cooperative targets in the pixel coordinate system respectively; To describe the kinematic model of the robot arm using the standard DH modeling method, that is, the transformation relationship matrix between the robot arm end coordinate system and the robot arm base coordinate system; is the transformation relationship matrix between the camera coordinate system and the robot end coordinate system; is the transformation relationship matrix between the image coordinate system and the camera coordinate system; It is the transformation matrix between the pixel coordinate system and the image coordinate system.

[0032] In step 5, the objective function of the COBYLA nonlinear optimization algorithm is the sum of the squares of the distances between the positions of the end points on both sides of the connecting rod in the robot base coordinate system and the positions of the cooperative targets, and the constraint function is the physical motion restrictions of the robot joints.

[0033] The optimization process of the COBYLA nonlinear optimization algorithm includes the following steps: S5.1: Set up a nonlinear constrained optimization problem, where the objective function is the sum of the squares of the distances between the positions of the end points of the connecting rod on both sides and the positions of the cooperative targets in the robot base coordinate system, and the constraint function is the physical motion restrictions of the robot joints; S5.2: Given the initial point of the optimization variable, set the trust region radius, control the search range, and set the convergence error; S5.3: Construct a linear approximation model and use a linear function to approximate the objective function and constraint function near the current point; S5.4: Solve the linear approximation constrained optimization problem in the current trust region to obtain new candidate points; S5.5: Calculate the ratio of the actual improvement value to the predicted improvement value, and adjust the trust region radius according to the ratio; S5.6: Check the termination condition. If the convergence condition is met, stop the optimization. Otherwise, return to S5.3 to continue the optimization.

[0034] The error of the end effector of the robotic arm reaching the desired position is within the allowable range, and the joint motion variation iteratively solved by the COBYLA nonlinear optimization algorithm can enable the endpoints on both sides of the horizontal link at the end of the robotic arm to reach the positions of two cooperative targets respectively.

[0035] The robot end trajectory planning method of the non-common field binocular measurement of the present invention uses two cameras at the end of the robot arm to measure the two cooperative target positions on the workstation under the non-common field of view, and performs trajectory planning through the COBYLA nonlinear optimization algorithm to calculate the change in the robot arm joint motion, so that the end effector reaches the desired position and performs the work. The specific steps are as follows: Step 1: Build a binocular measurement system at the end of the robotic arm, install two cooperative targets on the workstation, calibrate the two cameras separately and save the calibration results.

[0036] Step 2: Establish the DH parameter table and apply the standard DH modeling method to describe the kinematic model of the robot arm, denoted as .

[0037] Step 3: Collect images, process the images obtained by the two cameras, detect the contours of the two cooperative targets, and obtain the position coordinates of the two cooperative targets in the pixel coordinate system, which are recorded as , ; Step 4: According to the homogeneous transformation relationship between the pixel coordinate system, image coordinate system, camera coordinate system, robot end coordinate system, and robot base coordinate system, the position coordinates of the two cooperative targets in the pixel coordinate system are respectively converted to , Convert to the position coordinates in the robot base coordinate system, recorded as , ; Step 5: According to the robot arm kinematics model and the locations of the two cooperation targets , Combined with the physical motion limits of the four joints of the robotic arm, the COBYLA nonlinear optimization algorithm and the kinematic model of the robotic arm are used for trajectory planning to solve the change in the motion of the robotic arm joints. According to the kinematic model of the robotic arm, the end points on both sides of the horizontal connecting rod at the end of the robotic arm are respectively reached to the positions of the two cooperative targets, so that the end effector of the robotic arm reaches the desired position.

[0038] The robot arm used in the system is a four-degree-of-freedom robot arm, which is equipped with four joints, including two moving joints and two rotating joints, wherein the first joint is a horizontal rotating joint, the second joint is a vertical moving joint, the third joint is a horizontal moving joint, and the fourth joint is a horizontal rotating joint. The four joints of the robot arm are connected in sequence through connecting rods, one end of the first joint is connected to the robot arm base, one end of the fourth joint is connected to the end of the robot arm, the axes of the first joint and the second joint are parallel to each other, the axes of the second joint and the third joint are perpendicular to each other, and the axes of the third joint and the fourth joint are perpendicular to each other. In the system, a horizontal connecting rod is installed at the end of the robot arm, and a camera is installed at each end of the connecting rod.

[0039] The two cameras captured chessboard images multiple times at multiple angles, and the camera calibration tool was used to obtain the intrinsic parameter matrix, extrinsic parameter matrix and distortion coefficient of the two cameras.

[0040] A standard DH parameter table is established, and the standard DH method is applied to obtain the homogeneous transformation matrix between the coordinate systems of each link, and the kinematic model of the robot arm is established.

[0041] Two cameras collect images of two cooperative targets on the workstation respectively, and correct the images according to the camera calibration parameters to eliminate image distortion; perform Gaussian filtering to eliminate noise points on the image; perform dynamic threshold segmentation and opening operations to retain the target area; call findContours (contour point search function) in OpenCV (open source computer vision library) to detect all pixel points that make up the contour of the target area, and pass all contour pixel points to fitEllipse (ellipse fitting function) to fit into an elliptical contour, which can reduce the target position offset error caused by the shooting angle, and output the position coordinates of the two cooperative targets in the pixel coordinate system. , .

[0042] According to the positions of the two cooperative targets in the pixel coordinate system , And the transformation relationship matrix between the pixel coordinate system and the image coordinate system , we can get the target position description in the image coordinate system , ; According to the transformation relationship matrix between the image coordinate system and the camera coordinate system , we can get the target position description in the camera coordinate system , ; According to the transformation relationship matrix between the camera coordinate system and the robot end coordinate system And the transformation relationship matrix between the robot end coordinate system and the robot base coordinate system obtained by establishing the robot kinematic model , thereby obtaining the position coordinates of the cooperative target in the robot base coordinate system , , the specific relationship operation is as follows:

[0043] Use the homogeneous transformation matrix of the robot to calculate the positions of the two end points of the end link in the robot base coordinate system , , the positions of the two cooperation targets , The position of the end points of the horizontal link at the end of the robot arm is passed to the COBYLA nonlinear optimization algorithm. , Location with two cooperating targets , The square sum function of the distance between them is used as the objective function of the COBYLA nonlinear optimization algorithm, and the physical motion limit of the robot joint is used as the constraint function of the COBYLA nonlinear optimization algorithm. The trust region radius is set, the joint variables are adjusted, and random initialization is performed multiple times. The initialization range is dynamically adjusted according to the current error, and the solution is updated until the change in joint motion is solved so that the endpoints on both sides of the horizontal link at the end of the robot arm can reach the positions of the two cooperative targets, that is, the end effector of the robot arm reaches the desired position.

[0044] Through the above steps, binocular measurement and robot end trajectory planning methods can be implemented in non-common field of view conditions.

[0045] In one embodiment of the present invention, the cameras on both sides of the horizontal connecting rod at the end of the robotic arm independently capture images of two cooperative targets on the workstation, and the fields of view of the two cameras have no overlapping areas. Through binocular measurement under non-common field of view, the position coordinates of the two cooperative targets in the coordinate system of the robotic arm base are respectively obtained, and trajectory planning is performed based on the position coordinates.

[0046] Example 1: Figure 1 As shown, the overall execution of the robot arm end trajectory planning method for non-common field of view binocular measurement includes the following steps: S1. Build a binocular measurement robot system. In this step, select a suitable robot and camera. The two cameras are installed on both sides of the horizontal connecting rod at the end of the robot, and the two cameras do not have a common field of view.

[0047] S2. Calibrate the two cameras to obtain the internal parameters (such as focal length, principal point position) and external parameters (such as baseline, rotation matrix and translation vector between cameras) of the cameras for subsequent image processing and target position solution.

[0048] S3. According to the standard DH parameter method, the robot arm is regarded as composed of multiple joints and a series of links. A coordinate system is established on each link of the robot arm. The spatial position relationship between two adjacent links can be obtained through the homogeneous transformation relationship between the coordinate systems of each link. The kinematic model of the robot arm is established to obtain the homogeneous transformation matrix from the coordinate system of the end of the robot arm to the coordinate system of the base. According to the chain rule, the link coordinate system Relative to the connecting rod coordinate system The homogeneous transformation matrix The description is as follows: ; in, is the connecting rod length, defined as Axis to The distance along the axis The direction of the axis is positive; is the connecting rod torsion angle, defined as Axis to The rotation angle of the axis The positive rotation of the shaft is positive; is the joint distance, defined as the distance from Axis to The distance along the axis The direction of the axis is positive; is the joint rotation, defined as Axis to The rotation angle of the axis The positive rotation of the shaft is positive; The coordinate system is orbited Rotation The rotation matrix of The coordinate system is along Axis Translation The translation matrix of The coordinate system is along Axis Translation The translation matrix of The coordinate system is orbited Rotation The rotation matrix of .

[0049] This matrix formula is called the standard DH matrix, which is a 4x4 homogeneous transformation matrix that uses 4 DH parameters to represent the homogeneous transformation relationship between two adjacent links.

[0050] S4. Acquire images. The two cameras on the horizontal connecting rod at the end of the robot arm respectively capture images of the two cooperative targets on the workstation.

[0051] S5, performing image processing on the cooperative target images captured by the two cameras, detecting the contours of the two cooperative targets, and respectively obtaining the position coordinates of the two cooperative targets in the pixel coordinate system, including the following steps: S5.1. Target area extraction: According to the camera's internal parameters, the captured image is distorted and corrected. Gaussian filtering is used to eliminate noise points in the image. Dynamic threshold segmentation is performed to segment the target area and other areas. Opening operations are applied to eliminate small connected parts generated by other areas, and only the target area is retained.

[0052] S5.2, Contour detection: Call the contour point search function in the OpenCV open source computer vision library to detect the coordinates of all the pixels that make up the contour of the target area, pass all the contour pixel coordinates to the ellipse fitting function to fit the ellipse contour and draw it in the image. The quadratic equation of the ellipse is as follows: ; According to the position of the contour points, the best fitting ellipse parameters are obtained by calculation. .

[0053] S6. Use the ellipse centroid calculation formula to obtain the ellipse centroid That is, the position coordinates of the target in the pixel coordinate system, and the calculation formula is as follows: ; S7, obtaining the position of the target in the robotic arm base coordinate system according to the homogeneous transformation relationship among the pixel coordinate system, the image coordinate system, the camera coordinate system, the robotic arm end coordinate system and the robotic arm base coordinate system; The transformation relationship from pixel coordinate system to image coordinate system: ; in, is the position of the target in the image coordinate system; is the transformation relationship matrix between the pixel coordinate system and the image coordinate system; is the position of the target in the pixel coordinate system.

[0054] The transformation relationship from the image coordinate system to the camera coordinate system: ; in, is the transformation relationship matrix between the image coordinate system and the camera coordinate system; is the position of the target in the camera coordinate system.

[0055] The transformation relationship from the camera coordinate system to the robot end coordinate system: ; in, is the transformation relationship matrix between the camera coordinate system and the robot end coordinate system; is the position of the target in the coordinate system of the end of the robot.

[0056] The transformation relationship between the robot end coordinate system and the robot base coordinate system is as follows: ; in, is the transformation relationship matrix between the robot end coordinate system and the robot base coordinate system; is the position of the target in the robot base coordinate system.

[0057] The location of the cooperation target Described as: ; S8. Pass the position information of the two cooperative targets to the COBYLA nonlinear optimization algorithm to perform trajectory planning and solve the optimal joint motion change.

[0058] S9, output the results of the robot arm joints obtained through trajectory planning.

[0059] Example 2: Figure 2 As shown, the trajectory planning of the robot end trajectory planning method of non-common field of view binocular measurement using the COBYLA nonlinear optimization algorithm includes the following steps: S1. Set up a nonlinear constrained optimization problem of the following form: ; Satisfy the constraints: ; in: is the optimization variable ; is the objective function; is the constraint function.

[0060] S2, initial point of given optimization variable , which is equal to the variable value of each joint in the initial state of the robot; set the trust region radius , automatically adjusted by the COBYLA nonlinear optimization algorithm; set the convergence error.

[0061] S3, build a linear approximation model, at the current point Nearby, use linear functions to approximate the objective function and constraint function: ; in: is the current solution; is the approximate objective function; is an approximate constraint function; is the objective function of the setting; is the constraint function set; is the objective function set The gradient of is the constraint function set gradient.

[0062] S4. Solve the linear approximation constrained optimization problem in the current trust domain: ; satisfy:

[0063] S5. Solve to obtain new candidate points , calculate the ratio of the actual improvement to the predicted improvement: ; is the actual improvement value; is the forecast improvement value; According to the ratio Adjust the trust region radius ; if If is larger, the radius of the trust region is enlarged; if The smaller the trust region radius, the smaller the radius.

[0064] S6. Check the termination condition. If the relative error of the variable change is less than the convergence error, the optimization is considered to have converged, and the optimization is stopped. Otherwise, return to S3 and optimize again. The relative error calculation formula is as follows:

[0065] Embodiment 3: The present invention applies a four-degree-of-freedom robot arm having four joints, including two rotating joints and two moving joints. The standard DH parameter table is as follows:

[0066] According to the above table, take Substitute the data of 1, 2, 3, 4 and the corresponding rows into the connecting rod coordinate system Relative to the connecting rod coordinate system The homogeneous transformation matrix , we can get the homogeneous transformation matrices between the coordinate systems of each link: ; in, is the rotation angle of joint 1, is the distance joint 2 moves; is the distance joint 3 moves, is the rotation angle of joint 4; is the homogeneous transformation matrix between link coordinate system 1 and link coordinate system 0; is the homogeneous transformation matrix between link coordinate system 2 and link coordinate system 1; is the homogeneous transformation matrix between the connecting rod coordinate system 3 and the connecting rod coordinate system 2; is the homogeneous transformation matrix between the link coordinate system 4 and the link coordinate system 3.

[0067] Then the homogeneous transformation matrix from the robot end coordinate system to the robot base coordinate system is It can be described as: ; Then we can get the pose matrix of the left end point of the connecting rod and the pose matrix of the right end point of the connecting rod They are described as: ; in, , They are the transformation matrices of the left end point of the connecting rod and the right end point of the connecting rod relative to the end position of the robotic arm: ; Then the position function of the end points on both sides of the connecting rod in the robot base coordinate system is , They are as follows: ;

[0068] Setting the objective function of the COBYLA nonlinear optimization algorithm It is the sum of the squares of the distances between the positions of the end points on both sides of the link in the robot base coordinate system and the positions of the cooperative targets, as follows: ; in: Represents joint variables , is the rotation angle of joint 1, is the distance joint 2 moves, is the distance joint 3 moves, is the rotation angle of joint 4; are the locations of the two cooperation targets; are the positions of the end points on both sides of the connecting rod; They are exist Components on the axis; They are exist Components on the axis; They are exist Components on the axis; They are exist Components on the axis.

[0069] Set the constraint function of the COBYLA nonlinear optimization algorithm to the physical motion restrictions of the robot joints: ; in: Represents joint variables , is the rotation angle of joint 1, is the distance joint 2 moves, is the distance joint 3 moves, is the rotation angle of joint 4.

[0070] Set the joint variables of the robot arm in the initial state as follows:

[0071] Place the two cooperative targets on the workstation, perform camera image acquisition and image processing, and output the position matrix of the two cooperative targets in the robot arm base coordinate system. ,like Figure 3 As shown, and retaining three decimal places, the two cooperative target positions are as follows:

[0072] Pass the data in the above table to the COBYLA nonlinear optimization algorithm for trajectory planning, and set the optimized convergence error to , output the optimized joint changes as Figure 4 , the results are as follows:

[0073] like Figure 5 This is the iterative optimization curve of the COBYLA nonlinear optimization algorithm. The four small figures from top to bottom are the four joint variables of the robot arm. Process graph of optimization iterations.

[0074] It can be seen that the precision and accuracy of the optimal solution are improved through the iteration of the COBYLA nonlinear optimization algorithm.

[0075] According to the joint variables in the above table and the calculation formula of the robot arm kinematic model, the positions of the two endpoints after the movement can be obtained respectively. The calculation results are as follows: The left endpoint is at The components on the axes are calculated as follows: ; The right end point is The components on the axes are calculated as follows:

[0076] like Figure 6 The two endpoints are The result diagram of the position calculation on the axis is:

[0077] By comparing the two cooperative target positions obtained from the image and the two endpoint positions after the movement, it can be seen that, within the allowable error range, the joint variables obtained by the COBYLA nonlinear optimization algorithm can enable the endpoints on both sides of the horizontal link at the end of the robotic arm to reach the positions of the cooperative targets respectively, that is, the end effector reaches the desired position.

[0078] like Figure 7 , is the motion trajectory of the four joints of the robot arm based on the optimized joint variables. The four small figures from top to bottom are the robot arm The trajectory curves of the four joint movements all reach the optimized joint variable positions.

[0079] According to the experiment, a non-common field of view binocular measurement method for the end-of-manipulator trajectory planning can accurately measure the positions of the two cooperative targets by taking images of the two cooperative targets on the workstations with two cameras at both ends of the end-of-manipulator link. Through the COBYLA nonlinear optimization algorithm trajectory planning, multiple iterations and optimizations are performed, and the obtained joint motion variation can enable the end-of-manipulator to accurately reach the desired position in the workspace to perform work when the field of view is insufficient, proving the feasibility of the method of the present invention.

[0080] It should be noted that in the present invention, it should be understood that although this specification is described according to implementation modes, not every implementation mode contains only one independent technical solution. This description of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation modes that those skilled in the art can understand.

Claims

1. A robot arm end trajectory planning method for non-common field of view binocular measurement, characterized in that: The following steps are involved: Step 1: Build a binocular measurement system at the end of the robot arm, install two cameras, and install two cooperative targets on the workstation. Calibrate the two cameras separately and save the calibration results. Step 2: Establish a DH parameter table and apply the standard DH modeling method to describe the kinematic model of the robot arm; Step 3: Collect images, perform image processing on the images acquired by the two cameras, detect the contours of the two cooperative targets, and obtain the position coordinates of the two cooperative targets in the pixel coordinate system respectively; Step 4: According to the homogeneous transformation relationship between the pixel coordinate system, the image coordinate system, the camera coordinate system, the robot end coordinate system and the robot base coordinate system, the position coordinates of the two cooperative targets in the pixel coordinate system are converted to the position coordinates in the robot base coordinate system; Step 5: According to the kinematic model of the robot and the positions of the two cooperative targets, combined with the physical motion limits of the robot joints, the COBYLA nonlinear optimization algorithm and the kinematic model of the robot are used for trajectory planning to solve the change in the motion of the robot joints so that the end points of the horizontal link at the end of the robot reach the positions of the two cooperative targets respectively, and the end effector of the robot reaches the desired position.

2. The robot arm end trajectory planning method for non-common field of view binocular measurement according to claim 1 is characterized in that: The robot arm is a four-degree-of-freedom robot arm, comprising four joints, wherein the first joint is a horizontal rotation joint, the second joint is a vertical translation joint, the third joint is a horizontal translation joint, and the fourth joint is a horizontal rotation joint; The four joints of the robotic arm are connected in sequence through connecting rods, one end of the first joint is connected to the robotic arm base, and one end of the fourth joint is connected to the end of the robotic arm; a horizontal connecting rod is installed at the end of the robotic arm, and a camera is installed at each end of the connecting rod.

3. The robot arm end trajectory planning method for non-common field of view binocular measurement according to claim 1 or 2, characterized in that: In step 1, two cameras respectively capture chessboard images at multiple angles and multiple times, and use the camera calibration tool to obtain the intrinsic parameter matrix, extrinsic parameter matrix and distortion coefficient of the two cameras respectively.

4. The robot arm end trajectory planning method for non-common field of view binocular measurement according to claim 1 is characterized in that: In step 3, image processing includes the following steps: S3.1: According to the camera's internal parameters, the captured image is distorted and corrected, and the noise points in the image are eliminated by using Gaussian filtering. Dynamic threshold segmentation is performed to segment the target area and other areas, and the opening operation is applied to eliminate the small connected parts generated by other areas, and the target area is retained; S3.2: Call the contour point search function in OpenCV to detect the coordinates of all pixel points that make up the contour of the target area, pass all the contour pixel point coordinates to the ellipse fitting function to fit into an ellipse contour, and output the position coordinates of the two cooperative targets in the pixel coordinate system.

5. The method for robot end trajectory planning for non-common field of view binocular measurement according to claim 1, characterized in that: In step 4, the position of the target in the robotic arm base coordinate system is obtained according to the homogeneous transformation relationship between the pixel coordinate system, the image coordinate system, the camera coordinate system, the robotic arm end coordinate system, and the robotic arm base coordinate system, which specifically includes the following steps: S4.1: Obtain the position of the target in the image coordinate system according to the transformation relationship from the pixel coordinate system to the image coordinate system; S4.2: Obtain the position of the target in the camera coordinate system according to the transformation relationship from the image coordinate system to the camera coordinate system; S4.3: According to the transformation relationship from the camera coordinate system to the robot end coordinate system, the position of the target in the robot end coordinate system is obtained; S4.4: According to the transformation relationship from the robot end coordinate system to the robot base coordinate system, the position of the target in the robot base coordinate system is obtained.

6. The method for robot end trajectory planning for non-common field of view binocular measurement according to claim 5, characterized in that: In step S4.4, the position coordinates of the target in the robot base coordinate system are obtained and , the specific relationship operation is as follows: ； in, and Perform image processing on the images acquired by the two cameras respectively, detect the contours of the two cooperative targets, and obtain the position coordinates of the two cooperative targets in the pixel coordinate system respectively; To describe the kinematic model of the robot arm using the standard DH modeling method, that is, the transformation relationship matrix between the robot arm end coordinate system and the robot arm base coordinate system; is the transformation relationship matrix between the camera coordinate system and the robot end coordinate system; is the transformation relationship matrix between the image coordinate system and the camera coordinate system; It is the transformation matrix between the pixel coordinate system and the image coordinate system.

7. The method for robot end trajectory planning for non-common field of view binocular measurement according to claim 1, characterized in that: In step 5, the objective function of the COBYLA nonlinear optimization algorithm is the sum of the squares of the distances between the positions of the end points on both sides of the connecting rod in the robot base coordinate system and the positions of the cooperative targets, and the constraint function is the physical motion restrictions of the robot joints.

8. The method for robot end trajectory planning for non-common field of view binocular measurement according to claim 7, characterized in that: The optimization process of the COBYLA nonlinear optimization algorithm includes the following steps: S5.1: Set up a nonlinear constrained optimization problem, where the objective function is the sum of the squares of the distances between the positions of the end points of the connecting rod on both sides and the positions of the cooperative targets in the robot base coordinate system, and the constraint function is the physical motion restrictions of the robot joints; S5.2: Given the initial point of the optimization variable, set the trust region radius, control the search range, and set the convergence error; S5.3: Construct a linear approximation model and use a linear function to approximate the objective function and constraint function near the current point; S5.4: Solve the linear approximation constrained optimization problem in the current trust region to obtain new candidate points; S5.5: Calculate the ratio of the actual improvement value to the predicted improvement value, and automatically adjust the trust region radius according to the ratio; S5.6: Check the termination condition. If the convergence condition is met, stop the optimization. Otherwise, return to S5.3 to continue the optimization.

9. The method for robot end trajectory planning for non-common field of view binocular measurement according to claim 8, characterized in that: The error of the end effector of the robotic arm reaching the desired position is within the allowable range, and the variation of the joint motion is iteratively solved by the COBYLA nonlinear optimization algorithm so that the end points on both sides of the horizontal link at the end of the robotic arm can reach the positions of the two cooperative targets respectively.

10. The method for robot end trajectory planning for non-common field of view binocular measurement according to claim 1, characterized in that: The cameras on both sides of the horizontal connecting rod at the end of the robotic arm independently collect images of the two cooperative targets on the workstation, and the fields of view of the two cameras have no overlapping areas. Through binocular measurement under non-common field of view, the position coordinates of the two cooperative targets in the coordinate system of the robotic arm base are obtained respectively, and trajectory planning is performed based on the position coordinates.

Images