Kinematics Parameter Identification and Calibration Method for Unmanned Engineering Vehicles Based on Multi-ocular Vision

Abstract

The present invention discloses a method for identifying and calibrating the kinematic parameters of an unmanned engineering vehicle based on multi-camera vision, which includes the following steps: The first step is to plan the camera array of the multi-camera vision system; the second step is to calibrate the working space error of the multi-camera; the third step is to locate the position of the end of the robotic arm; the fourth step is to identify the error parameters; the fifth step is to evaluate the motion accuracy of the engineering vehicle. The present invention can measure the position of the actuator of engineering vehicles of different sizes, has high reliability, can greatly reduce the measurement error of the multi-camera vision system, improve the measurement accuracy of the system, quickly and conveniently calibrate the robotic arm of the engineering vehicle, and provide a standard for the evaluation of unmanned engineering vehicles.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned engineering vehicles, and particularly relates to a method for identifying and calibrating kinematic parameters of unmanned engineering vehicles based on multi-view vision. Background Technique

[0002] Engineering vehicles are widely used in mine exploitation, traffic construction, military construction, etc. With the development of technology and the needs of applications, unmanned engineering vehicle technology has emerged as the times require. Compared with traditional engineering vehicles, unmanned engineering vehicles have higher requirements for the control accuracy of the robotic arms on the vehicles. There are two methods to improve the positioning accuracy of the end of the robotic arm: (1) using better production processes. (2) calibrating the kinematic parameters of the robotic arm. The first method will greatly increase the production cost, so calibrating the kinematic parameters of the robotic arm has become the best choice. When calibrating the kinematic parameters of engineering vehicles, there are high requirements for the pose measurement accuracy of the end effector of the engineering vehicle. Commonly used measuring instruments include wire rope sensors, laser trackers, and coordinate measuring machines, etc. These measurement methods have great limitations: laser trackers are expensive; wire rope sensors have many operation steps; coordinate measuring machines are large in volume and weight, not easy to move, and require many auxiliary devices.

[0003] Vision measurement schemes based on cameras have the advantages of low cost, high detection accuracy, fast detection speed, etc. However, currently commercial multi-view vision detection systems cannot be directly applied to the pose detection of engineering vehicles; when using multi-view cameras, there is currently no highly integrated, convenient and effective solution for the position planning of the cameras, and only manual adjustment can be carried out little by little, with low efficiency. Irregular camera placement will cause large errors in the measurement results; there is a lack of a method for evaluating the measurement accuracy of the working space of multi-view cameras; there is no method for evaluating the unmanned operation accuracy. Summary of the Invention

[0004] The purpose of the present invention is to provide a method for identifying and calibrating kinematic parameters of unmanned engineering vehicles based on multi-view vision, which can realize the position measurement of the end effector for engineering vehicles of different sizes, has high reliability, can greatly reduce the measurement error of the multi-view vision system, improve the measurement accuracy of the system, can calibrate the robotic arm of the engineering vehicle quickly and conveniently, and provides a standard for the evaluation of unmanned engineering vehicles.

[0005] To achieve the above purpose, the method for identifying and calibrating kinematic parameters of unmanned engineering vehicles based on multi-view vision of the present invention includes the following steps:

[0006] The first step, camera array planning of the multi-view vision system:

[0007] According to the camera field-of-view projection equation, design a genetic algorithm-based scheme to adjust the layout of the multi-view vision system;

[0008] Step 2. Calibration of the working space error of the multi-camera:

[0009] Calibrate the multi-vision system using the one-dimensional calibration rod calibration method;

[0010] Step 3. Positioning of the end of the robotic arm:

[0011] Use the D-H parameter method to perform kinematic modeling on the robotic arm of the engineering vehicle, obtain the pose matrix of the end of the bucket of the working device, and use this to control the movement of the bucket on the movement plane;

[0012] After completing the kinematic modeling of the robotic arm, use the multi-vision system to measure the position of the end of its bucket;

[0013] Step 4. Identification of error parameters:

[0014] Build a kinematic parameter error model for the robotic arm of the engineering vehicle, use the least squares method combined with the measured bucket position data to complete the identification of the kinematic parameter errors of the robotic arm, and perform error compensation on the kinematic parameters;

[0015] Step 5. Evaluation of the movement accuracy of the engineering vehicle:

[0016] After the D-H parameter correction, input the robotic arm joint angle data of some points on the movement plane into the engineering vehicle controller, and then use the multi-vision system to measure the position of the end effector to obtain the corresponding spatial point coordinates. Comparing these with the given positions of these points can obtain the position error, and taking the average value of these point errors as the absolute positioning error after the robotic arm calibration.

[0017] As a further solution of the present invention: The steps of camera array planning of the multi-vision system in the first step are as follows:

[0018] (1) Constrain the movement of the end of the bucket of the robotic arm of the unmanned engineering vehicle to a plane and determine its movement trajectory;

[0019] (2) According to the camera projection equation, obtain the projection of each camera field of view on the movement plane;

[0020] (3) Use the genetic algorithm to encode the pose, taking the maximum trajectory coverage area as the index, ensure that each point on the trajectory to be detected is at least within the overlapping range of the fields of view of two cameras, and reduce the distance between the camera and the projection plane and increase the overlapping degree of the camera fields of view to improve the measurement accuracy, and finally obtain the pose information of each camera.

[0021] As a further solution of the present invention: The steps of calibrating the multi-vision system using the one-dimensional calibration rod method in the second step are as follows:

[0022] (1) Binarize the one-dimensional calibration rod image captured by the camera to extract the edges of the marked points on the calibration rod;

[0023] (2) Use the edges of the extracted marked points and the least squares method to obtain the pixel coordinates of the centers of the marked points;

[0024] (3) Optimize the internal and external parameters of the camera with the reprojection error as the index.

[0025] As a further solution of the present invention: In the third step, the measurement process of the position of the bucket end is as follows:

[0026] (1) Select several spatial coordinate points on the movement plane of the end of the robotic arm bucket as the given trajectory;

[0027] (2) Use the coordinate transformation of the bucket end relative to the base coordinate system of the engineering vehicle to find the kinematic inverse solution of these points, and obtain the robotic arm joint angles corresponding to these spatial points on the movement plane;

[0028] (3) Use these joint angles to control the robotic arm, and finally obtain the actual position data of the reflective target point at the end of the engineering vehicle bucket using multi-view vision;

[0029] (4) There is a coordinate transformation between the position of the reflective target point and the position of the bucket end. Use coordinate transformation on the position of the reflective target point to obtain the actual position of the bucket end.

[0030] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0031] By planning the positions of the multi-view cameras, it is possible to measure the positions of the actuators of engineering vehicles of different sizes, with high reliability;

[0032] By using the method with the reprojection error as the index, the measurement error of the multi-view vision system can be greatly reduced, making the measurement accuracy of the system higher;

[0033] Using the method based on the D-H parameter error model to calibrate the robotic arm of the engineering vehicle is fast and convenient, and can save a large amount of costs;

[0034] A method for evaluating the control accuracy of unmanned engineering vehicles is used, providing a standard for the evaluation of unmanned engineering vehicles. Description of the Drawings

[0035] Figure 1 It is a schematic diagram of measuring the bucket position by a multi-view camera.

[0036] Figure 2 It is a projection diagram of the camera on the movement plane of the bucket end.

[0037] Figure 3It is a schematic diagram of a calibration rod.

[0038] Figure 4 It is a schematic diagram of the projection relationship of a one-dimensional calibration rod under a multi-camera.

[0039] Figure 5 It is a simplified schematic diagram of a robotic arm. Specific implementation manners

[0040] The present invention will be further described below in conjunction with the accompanying drawings by taking a small unmanned excavator as the calibration object.

[0041] The kinematic parameter identification and calibration method for an unmanned engineering vehicle based on multi-view vision is as follows:

[0042] First step, camera array planning for the multi-view vision system:

[0043] When measuring the position of the end of the robotic arm actuator, it is necessary to plan the camera position so that the multi-view vision system can have a suitable measurement position for unmanned engineering vehicles of different sizes. According to the camera field-of-view projection equation, a genetic algorithm-based scheme is designed to adjust the layout of the multi-view vision system. Further, the camera position planning steps are as follows:

[0044] (1) Constrain the movement of the end of the bucket of the robotic arm of the unmanned engineering vehicle on a plane and determine its movement trajectory.

[0045] (2) According to the camera projection equation, obtain the projection of each camera field-of-view on the movement plane.

[0046] (3) Use the genetic algorithm to encode the pose. Taking the maximum trajectory coverage area as the index, ensure that each point on the trajectory to be detected is at least within the overlapping range of the fields-of-view of two cameras, reduce the distance between the camera and the projection plane, and increase the overlapping degree of the camera fields-of-view to improve the measurement accuracy. Finally, obtain the pose information of each camera.

[0047] The present invention uses 4 cameras to measure the position of the end of the bucket of the robotic arm of a small unmanned excavator. After planning the cameras using the above steps, as Figure 1 shown, the 4 cameras are 1.5 meters apart from each other, each 1.5 meters from the ground, and at the same time, the 4 cameras are on a straight line parallel to the movement plane of the end of the bucket of the robotic arm. The distance between the 4 cameras and the movement plane is 3.5 meters. At this time, the projections of the fields-of-view of the 4 cameras on the movement plane cover each other pairwise, so that the detection range of the multi-camera system can completely cover the movement range of the end of the bucket of the robotic arm. The projections of the fields-of-view of the 4 cameras of the multi-camera measurement system on the movement plane are as Figure 2As shown in the figure, the C1 area represents the projection of the first camera's field of view on the motion plane; C1&C2 represents the overlapping area of the projection of the first camera's field of view and the projection of the second camera's field of view on the motion plane; C2&C3 represents the overlapping area of the projection of the second camera's field of view and the projection of the third camera's field of view on the motion plane; C3&C4 represents the overlapping area of the projection of the third camera's field of view and the projection of the fourth camera's field of view on the motion plane; the C4 area represents the projection of the fourth camera's field of view on the motion plane; the dotted line is the motion trajectory of the bucket end on the motion plane.

[0048] Step 2. Calibration of the working space error of the multi-camera:

[0049] After planning the positions of the cameras, the present invention uses a one-dimensional calibration rod as shown in Figure 3 to calibrate the multi-camera vision system.

[0050] Furthermore, the steps of calibrating the multi-camera vision system using the one-dimensional calibration rod method are mainly as follows:

[0051] (1) Binarize the image of the one-dimensional calibration rod captured by the camera to extract the edge of the marking points on the calibration rod. Since the reflective target points can produce a large brightness difference with the surrounding environment and the gray-scale contrast is obvious, the gray-scale image of the calibration rod is binarized to highlight the brightness of the reflective target points. In the image, if the pixel gray-scale value at the r-th row and c-th column is less than 60, the gray-scale value is changed to 0, and if it is greater than or equal to 60, the gray-scale value is changed to 255. The binarization settings are as follows:

[0052]

[0053] After binarizing the gray-scale image, extract the edge of the marking points.

[0054] (2) Using the edge of the extracted marking points, use the least squares method to obtain the pixel coordinates of the center of the marking points. Due to the influence of noise and distortion, after binarizing the image, the obtained marking points are an uneven near-circular image. The present invention obtains the two-dimensional position of the marking points by fitting the near-circle into a circle and solving the center of the circle.

[0055] (3) Taking the reprojection error as an index, optimize the internal and external parameters of the camera. The projection relationship of the one-dimensional calibration rod under the multi-camera is as shown in Figure 4 . Three collinear marking points are A, B, and C. Denote A j B j C j as different positions of the marking points in space j = 1, 2, 3... n. O iis the optical center of the i-th camera, where i = 0, 1, 2, 3... m. Denote L as the distance between different points. Then L1 = ||A - B||, L2 = ||B - C||, and L3 = ||A - C||. The imaging points of the feature points on the camera imaging plane are a ij , b ij , c ij .

[0056] Assume that after the estimated reconstruction matrix , the obtained image point is called the reprojection point. Minimize the distance between this point and the observation point, as shown in Equation (1):

[0057]

[0058] The function d(x, y) is the geometric distance function between x and y, and this function is called the reprojection error. According to the collinearity property of A j B j C j , taking A j as the center, establish a spherical coordinate system, and use A j to describe the positions of B j C j . Then the relationship between the spatial points B j C j and A j is as shown in Equation (2):

[0059]

[0060] Substitute the expression of Equation (2) into the projection coordinate formula of Equation (3):

[0061]

[0062] In Equation (3), the parameters of the matrix K are determined by the internal structure parameters of the camera and are called the internal parameter matrix; the matrix M is the pose of the camera relative to the world coordinate and is called the external parameter matrix of the camera; H can be regarded as a projection transformation matrix and is called the homography matrix. X W , Y W , Z W are the positions in the three directions of the world coordinate system, u and v are the positions in the two directions of the pixel coordinate system, and Z C represents the position of the object in the optical axis direction in the camera coordinate system. Then, for the reprojections of points A j B j C j , perform an overall error minimization solution. The minimized reprojection error expression is as shown in Equation (4):

[0063]

[0064] In formula (4), x is a variable that includes internal and external parameters, and the number of parameters to be optimized is relatively large. Therefore, the sparse LM algorithm is used for solution.

[0065] Step 3: Positioning the end position of the robotic arm:

[0066] First, kinematic parameter modeling is performed on the robotic arm of the unmanned excavator. Without considering the horizontal rotation of the turntable of the engineering vehicle, its robotic arm can be approximated as a 3-degree-of-freedom linkage mechanism, as Figure 5 shown. The D-H parameter model is used to perform kinematic modeling on the robotic arm, and the pose matrix of the end of the working device bucket is obtained to control the movement of the bucket on the movement plane. Specifically, a base coordinate system is established on the base of the robotic arm, and a coordinate system transformation is performed at each joint. Each transformation will obtain a transformation relationship matrix, and finally, the transformation relationship between the coordinates of the execution mechanism and the base coordinates is obtained.

[0067] The D-H parameter coordinate transformation from coordinate system i to coordinate system i + 1 is as shown in formula (5):

[0068]

[0069] In formula (5), i is the label of each joint; α i is the joint twist angle; θ i is the joint rotation angle; a i is the link length; d i is the distance between two adjacent coordinate systems i and i + 1 on the Z-axis of coordinate system i. Multiplying the transformation matrices of each working device in sequence can obtain the coordinate transformation matrix of the end of the robotic arm bucket relative to the base coordinate system, and the expression is as shown in formula (6):

[0070]

[0071] According to formula (6), the pose matrix of the end of the working device bucket is as shown in formula (7):

[0072]

[0073] where: c0 = cosθ0, c 01 = cos(θ0 + θ1), c 012 = cos(θ0 + θ1 + θ2), s0 = sinθ0, s 01 = sin(θ0 + θ1), s 012 = sin(θ0 + θ1 + θ2).

[0074] In formula (7) is the attitude matrix, is the position matrix.

[0075] The position of the end of the robotic arm bucket is given by Equation (8):

[0076]

[0077] In Equation (8), x represents the horizontal distance of the bucket end from the center of the robotic arm base, and z represents the vertical distance of the bucket end from the center of the robotic arm base.

[0078] After completing the kinematic modeling of the robotic arm, a multi-camera vision system is used to measure the position of its bucket end.

[0079] Furthermore, the measurement process is as follows: Select several spatial coordinate points on the movement plane of the robotic arm bucket end as the given trajectory; Use the coordinate transformation of the bucket end relative to the base coordinate system of the unmanned excavator to find the kinematic inverse solution of these points, and obtain the robotic arm joint angles corresponding to these spatial points on the movement plane; Control the robotic arm using these joint angles, and finally obtain the actual position data of the reflective target point at the bucket end of the unmanned excavator using the multi-camera vision. The position of the reflective target point differs from the position of the bucket end by a coordinate transformation. Use the coordinate transformation on the position of the reflective target point to obtain the actual position of the bucket end.

[0080] Fourth step, error parameter identification:

[0081] Build a kinematic parameter error model for the robotic arm of the engineering vehicle. Use the least squares method combined with the measured bucket position data to complete the identification of the kinematic parameter errors of the robotic arm, and perform error compensation on the kinematic parameters.

[0082] Specifically, according to the kinematic model of the robotic arm in the third step, the pose of the end of the robotic arm is determined as:

[0083] P = F(a, d, α, θ)

[0084] Due to the influence of manufacturing processes and assembly, the four kinematic parameters of the robotic arm will inevitably have some errors compared with the theoretical values. Also, since the rotation angles of each joint of the robotic arm are read by an encoder disc, it is not considered that there are errors in the joint rotation angles. For the three kinematic parameters a, d, α, there are errors Δa, Δd, Δα. The measured pose of the robotic arm should be:

[0085] P' = F(a + Δa, d + Δd, α + Δα)

[0086] ΔP = P - P'

[0087] In the case where the kinematic error is relatively small, the above equation can be approximated as a linear equation:

[0088]

[0089] At any pose P in spacei The time error equation is as follows:

[0090]

[0091] In Equation (10), ΔP ix , ΔP iy , ΔP iz respectively represent the errors of the end position of the robotic arm in the X, Y, and Z directions. After measuring the coordinates of n points in the working space of the robotic arm, the above equation can be converted into the following equation:

[0092] AΔX = b (11)

[0093] In Equation (11), A is a Jacobian matrix. Corresponding to the above equation, every three rows form a group, and the specific form is as follows:

[0094]

[0095] b is the error matrix of n groups of data:

[0096] b = (ΔP 1x ΔP 1y ΔP 1z ΔP 2x ΔP 2y ΔP 2z …ΔP nx ΔP ny ΔP nz ) T

[0097] ΔX is the error of the D-H parameters:

[0098] ΔX = (Δa1 Δa2 Δd1 Δd2 Δα1 Δα2 Δθ1Δθ2) T

[0099] After obtaining the error model of the D-H parameters, the least squares method can be used to find the optimal values of the D-H parameters to minimize the end position error of the robotic arm. The specific expression is as follows:

[0100] ΔX = (A T A) -1 ATb.

[0101] Step 5, Evaluation of the motion accuracy of the engineering vehicle:

[0102] After processing the data using the least squares one-time completion method, a D-H parameter error vector that meets expectations can be obtained. The original D-H parameters are compensated using the D-H parameter error quantity to obtain the calibrated D-H parameters. After calibrating the D-H parameters, the robotic arm joint angle data of the first 5 points on the motion plane in the above text is input into the small unmanned excavator controller, and then the multi-camera vision system is used to measure the position of the end effector to obtain the coordinates of 5 spatial points. Comparing these coordinates with the given positions of these 5 points can obtain the position error, and the average value of the errors of these 5 points is taken as the absolute positioning error after calibrating the robotic arm.

Claims

1. A kinematic parameter identification and calibration method for unmanned engineering vehicles based on multi-camera vision, characterized in that, It includes the following steps: The first step: Planning the camera array of the multi-view vision system According to the camera field-of-view projection equation, design a genetic algorithm-based scheme to adjust the layout of the multi-view vision system The specific steps are as follows: (1) Constrain the movement of the end of the manipulator bucket of the unmanned engineering vehicle to a plane and determine its movement trajectory (2) According to the camera projection equation, obtain the projection of each camera field of view on the movement plane (3) Use the genetic algorithm to encode the pose. Taking the maximum trajectory coverage area as the index, ensure that each point on the trajectory to be detected is at least within the overlapping range of the fields of view of two cameras, reduce the distance between the camera and the projection plane, and increase the overlapping degree of the camera fields of view to improve the measurement accuracy. Finally, obtain the pose information of each camera The second step: Calibrating the working space error of the multi-view cameras Calibrate the multi-view vision system using the one-dimensional calibration rod calibration method The specific steps are as follows: (1) Perform binary processing on the images of the one-dimensional calibration rod captured by the camera to extract the edge of the marked points on the calibration rod (2) Using the edges of the extracted marked points, use the least squares method to obtain the pixel coordinates of the center of the marked points (3) Taking the reprojection error as the index, optimize the internal and external parameters of the camera The third step: Positioning the end of the manipulator Use the D-H parameter method to perform kinematic modeling on the manipulator of the engineering vehicle, obtain the pose matrix of the end of the working device bucket, and use this to control the bucket to move on the movement plane After completing the kinematic modeling of the manipulator, use the multi-view vision system to measure the position of the end of its bucket The fourth step: Identifying error parameters Perform kinematic parameter error model modeling on the manipulator of the engineering vehicle. Using the least squares method combined with the measured bucket position data, complete the identification of the kinematic parameter errors of the manipulator and perform error compensation on the kinematic parameters The fifth step: Evaluating the movement accuracy of the engineering vehicle After correcting the D-H parameters, input the manipulator joint angle data of some points on the movement plane into the engineering vehicle controller, and then use the multi-view vision system to measure the position of the end effector to obtain the corresponding spatial point coordinates. Comparing them with the given positions of these points can obtain the position error. Take the average value of the errors of these points as the absolute positioning error after calibrating the manipulator 2. The kinematic parameter identification and calibration method for unmanned engineering vehicles based on multi-view vision according to claim 1, characterized in that: The process of measuring the position of the end of the bucket in the third step is as follows: (1) Select several spatial coordinate points on the movement plane of the end of the manipulator bucket as the given trajectory (2) Use the coordinate transformation of the end of the bucket relative to the base coordinate system of the engineering vehicle to find the kinematic inverse solution of these points, and obtain the manipulator joint angles corresponding to these spatial points on the movement plane (3) Use these joint angles to control the manipulator, and finally use the multi-view vision to obtain the actual position data of the reflective target point at the end of the engineering vehicle bucket (4) There is a coordinate transformation difference between the position of the reflective target point and the position of the end of the bucket. Use coordinate transformation on the position of the reflective target point to obtain the actual position of the end of the bucket

Images