Robot Manual Guidance Device Integrating Multiple IMUs and ArUco Markers

Abstract

The present invention provides a robot manual guidance device that integrates multiple IMUs and ArUco markers. The device includes a camera, a manual guidance body, a host computer, and a robot. A plurality of ArUco markers are arranged on the outer surface of the manual guidance body. The camera is used to take pictures during the process of the manual guidance body executing a teaching trajectory and send the obtained image information to the host computer. Three IMUs are arranged inside the manual guidance body, and the three IMUs of the manual guidance body send three groups of sensing data collected during the movement to the host computer. The host computer processes the received photographed images and the three groups of sensing data to obtain a replicated trajectory, and controls the robot to perform teaching through the replicated trajectory. The embodiment of the present invention provides a robot manual guidance device with low cost, high accuracy, and high robustness.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and particularly to a robot manual guidance device integrating multiple IMUs and ArUco markers. Background Art

[0002] With the continuous emergence of advanced technologies in the field of collaborative robots, the teaching methods have also been continuously evolving. Similar to industrial robots, the initial teaching device for collaborative robots was a portable robot operation terminal integrated with multiple buttons. Later, with the support of computer technology, the teaching pendant had an intuitive operation interface, enabling the operator to complete the programming of paths and actions by "teaching" the robot.

[0003] With the rapid development of industrial automation and computer technology, the teaching methods have evolved from the initial slow movement and point teaching. Later, combined with robot dynamics, drag teaching was invented, incorporating more debugging, maintenance, and fault diagnosis functions. The combination of various technologies has gradually made the development of robot teaching tend towards intelligence and integration.

[0004] Manual guidance is a requirement that robots must meet during collaborative operation. It allows users to program the robot by physically dragging the robot between the desired positions in the workspace. Since it is an online programming method, physical access to the robot is required during programming. In addition, it requires touching the robot while it is running, so the robot must run at a low speed in safe mode. Existing manual guidance devices are still difficult to achieve high robustness and refined dynamic perception capabilities while maintaining cost-effectiveness. Summary of the Invention

[0005] The present invention provides a robot manual guidance device integrating multiple IMUs and ArUco markers, and its main purpose is to provide a robot manual guidance device with low cost, high accuracy, and high robustness.

[0006] An embodiment of the present invention provides a robot manual guidance device integrating multiple IMUs and ArUco markers, including a camera, a manual guidance body, a host computer, and a robot. Multiple ArUco markers are arranged on the outer surface of the manual guidance body. The camera is used to take pictures during the process of the manual guidance body executing the teaching trajectory and send the obtained image information to the host computer. Three IMUs are arranged inside the manual guidance body, and the three IMUs inside the manual guidance body send three groups of sensing data collected during the movement process to the host computer;

[0007] The host computer processes the received photographed image and the three groups of sensing data through the following steps to obtain a replicated trajectory, and controls the robot to perform teaching through the replicated trajectory. The steps include:

[0008] Two-dimensional coordinates are obtained based on the image information, and the two-dimensional coordinates are subjected to projection conversion processing to obtain three-dimensional coordinates, and the image information includes a plurality of ArUco marker codes;

[0009] Based on the two-dimensional coordinates and the three-dimensional coordinates, combined with the multi-reference marker fusion pose estimation algorithm, a rotation matrix and a translation matrix are obtained;

[0010] The CNN-KF network is used to fuse and process three groups of the sensing data to obtain one group of fused sensing data, and the CNN-KF network is obtained by combining a CNN model and a KF model;

[0011] Based on the rotation matrix, the translation matrix, and the fused sensing data, the replicated trajectory is obtained.

[0012] Further, the step of obtaining the rotation matrix and the translation matrix according to the two-dimensional coordinates and the three-dimensional coordinates, combined with the multi-reference marker fusion pose estimation algorithm, includes:

[0013] Based on the two-dimensional coordinates and the three-dimensional coordinates, combined with the SQPnP algorithm, an initial rotation matrix and an initial translation matrix are obtained;

[0014] The initial rotation matrix and the initial translation matrix are used as the initial solution of the LM algorithm, and the LM algorithm is used for iteration to obtain the rotation matrix and the translation matrix.

[0015] Further, the CNN-KF network includes an improved CNN model and a KF model, and the step of using the CNN-KF network to fuse and process three groups of the sensing data to obtain one group of fused sensing data includes:

[0016] The three groups of the sensing data are input into the improved CNN model to obtain predicted sensing data;

[0017] The predicted sensing data is used as the prior value of the KF model for prediction to obtain the fused sensing data.

[0018] Further, the improved CNN model sequentially includes a regularization layer, a first CNN model, a first activation function layer, a second CNN model, a second activation function layer, and a fully connected layer. The first CNN model and the second CNN model include two convolutional layers and one fully connected layer. The convolutional kernel of one convolutional layer is 1*1, and the number of output channels is 6. The convolutional kernel of the other convolutional layer is 2*2, and the number of output channels is 9.

[0019] Further, the step of using the predicted sensing data as the prior value of the KF model for prediction to obtain the fused sensing data includes:

[0020] For the current moment, use the predicted sensing data at the previous moment as the prior value for the current iteration, use the predicted sensing data as the observed value, perform prediction using the KF model, and use the predicted value as the fused sensor data.

[0021] Furthermore, the calculation formula for the training loss of the CNN-KF network is as follows:

[0022] ;

[0023] Wherein, represents the training loss of the CNN-KF network, represents the error of the KF model, represents the error of the improved CNN network.

[0024] Furthermore, the steps of obtaining the replicated trajectory according to the rotation matrix, the translation matrix, and the fused sensing data include:

[0025] Process the rotation matrix, the translation matrix, and the fused sensing data using the ESKF algorithm to obtain the pose;

[0026] Use the LS algorithm to smooth the pose to obtain the replicated trajectory.

[0027] Furthermore, the end of the manual guidance body includes two cubes stacked at a 45-degree angle, and each cube is provided with an ArUco marker code on four sides.

[0028] Furthermore, 3 IMUs are vertically arranged at 60 degrees inside the manual guidance body.

[0029] A robot manual guidance device that fuses multiple IMUs and ArUco marker codes proposed by the present invention. The manual guidance device includes a manual guidance body, a host computer, and a robot. When performing teaching, only need to let the manual guidance body execute the teaching trajectory, and after parsing by the host computer, control the robot to replicate the same trajectory, thus completing remote teaching. Compared with traditional robot teaching devices, it is safer; and it only needs to move the manual guidance body in different scenarios, which is more convenient to use; and the hardware of this manual guidance device only requires a camera, IMUs, and ArUco marker codes, with a lower cost.

[0030] In addition, when the host computer generates the replicated trajectory, since the image information includes ArUco marker codes, the assistance of the ArUco marker codes makes the two-dimensional coordinate accuracy higher. Moreover, by using the multi-reference marker fusion pose estimation algorithm, the problem of inaccurate two-dimensional corner detection during the movement process leading to a decrease in accuracy can be solved, thereby improving the robustness of the manual guidance device. Finally, by using the CNN-KF network, the noise in the three groups of sensing data can be effectively eliminated, improving the accuracy of the manual guidance device. Description of the Drawings

[0031] Figure 1 FIG. is a schematic structural diagram of a robot manual guidance device integrating multiple IMUs and ArUco marker codes provided by an embodiment of the present invention;

[0032] Figure 2 FIG. is a flowchart of data processing by a host computer provided by an embodiment of the present invention;

[0033] Figure 3 FIG. is a schematic structural diagram of a CNN-KF network provided by an embodiment of the present invention;

[0034] Figure 4 FIG. is an overall implementation diagram of a manual guidance body provided by an embodiment of the present invention;

[0035] Figure 5 FIG. is a schematic diagram of the actual internal circuit structure of a manual guidance body provided by an embodiment of the present invention;

[0036] Figure 6 FIG. is a comparison diagram of rotational errors of different algorithms under different noise ratios provided by an embodiment of the present invention;

[0037] Figure 7 FIG. is a comparison diagram of translational errors of different algorithms under different noise ratios provided by an embodiment of the present invention;

[0038] Figure 8 FIG. is a comparison diagram of rotational errors of different algorithms under outliers provided by an embodiment of the present invention;

[0039] Figure 9 FIG. is a comparison diagram of translational errors of different algorithms under outliers provided by an embodiment of the present invention;

[0040] Figure 10 FIG. is a comparison diagram of the acceleration change curve obtained by fusing different methods and the true value in the X-axis direction provided by an embodiment of the present invention;

[0041] Figure 11 FIG. is a comparison diagram of the acceleration change curve obtained by fusing different methods and the true value in the Y-axis direction provided by an embodiment of the present invention;

[0042] Figure 12 A comparison diagram of the acceleration change curve obtained by fusing different methods and the true value in the Z-axis direction provided by an embodiment of the present invention;

[0043] Figure 13 A comparison diagram of the angular velocity change curve obtained by fusing different methods and the true value in the X-axis direction provided by an embodiment of the present invention;

[0044] Figure 14 A comparison diagram of the angular velocity change curve obtained by fusing different methods and the true value in the Y-axis direction provided by an embodiment of the present invention;

[0045] Figure 15 A comparison diagram of the angular velocity change curve obtained by fusing different methods and the true value in the Z-axis direction provided by an embodiment of the present invention;

[0046] Figure 16 A typical trajectory tracking comparison diagram provided by an embodiment of the present invention;

[0047] Figure 17 A schematic diagram of the application of a taught trajectory provided by an embodiment of the present invention;

[0048] Figure 18 Another schematic diagram of the application of a taught trajectory provided by an embodiment of the present invention.

[0049] The realization, functional features, and advantages of the object of the present invention will be further described in conjunction with the embodiments with reference to the accompanying drawings. Detailed implementation manners

[0050] The following details the implementation manners of the present application. The examples of the implementation manners are shown in the accompanying drawings, where the same or similar reference numerals represent the same or similar elements or elements with the same or similar functions from beginning to end. The implementation manners described below with reference to the accompanying drawings are exemplary only for explaining the present application and should not be construed as a limitation to the present application.

[0051] In the description of the present application, it should be understood that the orientation or positional relationships indicated by the terms "center", "longitudinal", "transverse", "length", "width", "thickness", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", "clockwise", "counterclockwise", "axial", "circumferential", "radial", etc. are based on the orientation or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present application and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be construed as a limitation to the present application.

[0052] To enable those skilled in the art to better understand the solution of this application, the technical solutions in the embodiments of this application will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of this application. Obviously, the described embodiments are only a part of the embodiments of this application, rather than all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by those skilled in the art without creative efforts belong to the scope protected by this application.

[0053] In the embodiments of this application, "at least one" means one or more; "a plurality" means two or more. In the description of this application, terms such as "first", "second", and "third" are only used for the purpose of distinguishing descriptions, and cannot be understood as indicating or implying relative importance, nor can they be understood as indicating or implying order. In addition, the terms "first" and "second" are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the quantity of the indicated technical features. Thus, features defined with "first" and "second" may explicitly or implicitly include one or more of such features. In the description of this application, "a plurality" means two or more, unless otherwise specifically defined.

[0054] The reference to "an embodiment" or "some embodiments" etc. described in this specification means that specific features, structures, or characteristics described in connection with the embodiment are included in one or more embodiments of this application. Thus, the terms "including", "comprising", "having" and their variants in this specification all mean "including but not limited to", unless otherwise specifically emphasized in other ways.

[0055] The technologies and sensor types adopted in the prior art can generally be divided into four categories: methods based on visual pose estimation, methods based on laser tracking, methods based on IMU, and devices based on multi-sensor fusion.

[0056] The first type of handheld teaching device adopts visual pose estimation technology. The pose of the handheld teaching device is estimated through the visual system, and it is used to replace the pose of the robot end tool for trajectory tracking. Although the cost and accuracy of the visual pose estimation system basically meet the requirements of high-precision trajectory teaching, its environmental adaptability in the industrial field is difficult to meet. A representative application is the developed handheld teaching device, which adopts VPE (Visual Pose Estimation) technology and estimates the device pose by the camera identifying the optical marks on the handheld device. Although this device can provide high teaching accuracy in a static situation, relying solely on vision for trajectory tracking will limit the acquisition of its high-frequency data and is easily affected by factors such as on-site lighting conditions and occlusion, resulting in a decrease in accuracy and tracking loss.

[0057] The second type of handheld teaching device uses laser tracking technology LT (Laser Tracking) and combines digital twin technology. It uses a high-precision laser measuring instrument to track the pose of the handheld teaching device to obtain the teaching trajectory. A representative product is the TwinTrack Probe (dual detector) of ROBOTDK. Such devices have high static and dynamic teaching accuracy, but the high cost of the laser measuring instrument and the high requirements for the working environment result in high costs. Their cost usually exceeds 50,000 yuan. At the same time, similar to the vision system, there is also an occlusion problem.

[0058] The third type uses trajectory tracking technology based on a high-precision IMU (Inertial Measurement Unit), but the accuracy is not high (5 cm - 1 m). This method directly calculates the trajectory through double integration and performs real-time ZUPT (Zero Velocity Update) based on the motion characteristics of the device. The disadvantage is that the error accumulates over time and complex trajectory tracking cannot be performed.

[0059] The fourth type of handheld teaching device uses multi-sensor fusion technology. For example, a VR handle system that combines laser and IMU is used to achieve virtual dragging of the robot. Although this method enables users to intuitively control the robotic arm and does not require a high learning cost, it cannot directly determine the target position of the robot and it is difficult to achieve refined dynamic perception.

[0060] It can be seen that existing manual guidance devices are still difficult to achieve high robustness and refined dynamic perception capabilities while maintaining cost-effectiveness.

[0061] In view of the above problems, Figure 1 This is a schematic structural diagram of a robot manual guidance device that integrates multiple IMUs and ArUco marker codes provided by an embodiment of the present invention. As Figure 1 shown, the manual guidance device includes a camera, a manual guidance body, a host computer, and a robot. Multiple ArUco marker codes are arranged on the outer surface of the manual guidance body. The camera is used to take pictures during the process of the manual guidance body executing the teaching trajectory and send the obtained image information to the host computer. Three IMUs are arranged inside the manual guidance body, and the three IMUs inside the manual guidance body send three groups of sensing data collected during the movement to the host computer.

[0062] The host computer processes the received image information and three groups of sensing data through the following steps to obtain a replicated trajectory, and controls the robot to perform teaching through the replicated trajectory. Figure 2 This is a flowchart of the data processing by the host computer provided by an embodiment of the present invention. As Figure 2 shown, the steps include:

[0063] Obtain two-dimensional coordinates based on the image information, and perform projection conversion processing on the two-dimensional coordinates to obtain three-dimensional coordinates. The image information includes multiple ArUco marker codes;

[0064] Based on the two-dimensional coordinates and the three-dimensional coordinates, combined with the multi-reference marker fusion pose estimation algorithm, obtain the rotation matrix and the translation matrix;

[0065] Use the CNN-KF network to perform fusion processing on 3 groups of the sensing data to obtain 1 group of fused sensing data. The CNN-KF network is obtained by combining the CNN model and the KF model;

[0066] Based on the rotation matrix, the translation matrix, and the fused sensing data, obtain the replicated trajectory.

[0067] As Figure 1 shown, the robot manual guiding device includes a camera, a manual guiding body, a host computer, and a robot. The camera is arranged at any position where the manual guiding body can be photographed, and the camera communicates with the host computer through TCP. The camera can send the photographed image information to the host computer, and its sending frequency can be 15 hz; the camera can be an ordinary industrial camera, and generally several of them are about a thousand yuan. When using the manual guiding device for teaching, the user operates the manual guiding body to draw the corresponding teaching trajectory. Three IMUs are arranged inside the manual guiding body, and 3 groups of sensing data can be collected through these 3 IMU sensors. The manual guiding body communicates with the host computer through WIFI, and its frequency can be 100 hz. The price of the IMU is generally about dozens of yuan; and multiple ArUco marker codes are arranged on the outer surface. When the camera takes pictures of the manual guiding body, the ArUco marker codes will be included, that is, the ArUco marker codes are included in the image information.

[0068] Among them, the ArUco marker code is a binary square reference marker for camera pose estimation, which has the characteristics of simple, fast, and strong robustness in detection. Its detection process is based on finding the correspondence between points in the real environment and their 2D image projections. Usually, synthetic or reference markers are used to simplify this process. The ArUco marker code can help locate the manual guiding body. By detecting the ArUco marker codes in the environment, the position and pose of the manual guiding body during movement can be determined.

[0069] It should be noted that in the embodiments of the present invention, 8 ArUco marker codes are arranged outside the manual guidance body. The end of the manual guidance body includes two cubes stacked at a 45-degree angle. Each cube is provided with an ArUco marker code on each of its four sides. Therefore, no matter which angle the camera takes a picture of the manual guidance body, the captured picture information will contain at least 3 ArUco marker codes. These 3 ArUco marker codes can accurately assist in positioning the manual guidance body, thereby improving the estimation accuracy of subsequent poses.

[0070] After the host computer receives the image information and 3 sets of sensing data, it processes these data to generate a replicated trajectory and sends the replicated trajectory to the robot. After the robot performs kinematic analysis, it reproduces the teaching trajectory.

[0071] Among them, the processing process of the host computer is as follows:

[0072] According to the received image information, since the image information includes multiple ArUco marker codes, taking these multiple ArUco marker codes as a reference benchmark, the image information is processed to determine the two-dimensional coordinates of the manually moved body. And in combination with 3 sets of sensing data, the two-dimensional coordinates are transformed to obtain three-dimensional coordinates.

[0073] Using the two-dimensional coordinates and three-dimensional coordinates, in combination with the multi-reference marker fusion pose estimation algorithm, a rotation matrix and a translation matrix are obtained; and the 3 sets of sensing data are fused using the CNN-KF network to obtain 1 set of fused data; finally, according to the rotation matrix, translation matrix and fused sensing data, a replicated trajectory is obtained and sent to the robot, and the robot moves according to the replicated trajectory.

[0074] Among them, the CNN-KF network is obtained by fusing CNN (Convolutional Neural Networks) and KF (Kalman Filter).

[0075] As an implementation method, the step of obtaining the rotation matrix and the translation matrix by combining the multi-reference marker fusion pose estimation algorithm according to the two-dimensional coordinates and the three-dimensional coordinates includes:

[0076] According to the two-dimensional coordinates and the three-dimensional coordinates, in combination with the SQPnP algorithm, an initial rotation matrix and an initial translation matrix are obtained;

[0077] Taking the initial rotation matrix and the initial translation matrix as the initial solution of the LM algorithm, the LM algorithm is used for iteration to obtain the rotation matrix and the translation matrix.

[0078] The existing PnP (Perspective-n-Point) algorithm has poor detection effect on the four corner points of the ArUco marker code during the movement process, and there is a problem of insufficient robustness when feature points are misdetected. To solve this problem, in the embodiments of the present invention, the SQPnP algorithm is used for pose estimation. The two-dimensional coordinates and three-dimensional coordinates are input into the SQPnP algorithm, and the initial pose parameters are obtained based on the SQPnP non-iterative algorithm. This method uses a known 3D point set in the world coordinate system and its corresponding 2D image projection points to directly solve the closed-form solutions of the rotation matrix R and the translation vector t, and obtain the initial rotation matrix and the initial translation matrix.

[0079] To further solve the problem of accuracy degradation caused by the inaccuracy of two-dimensional corner detection in the PnP algorithm, in the embodiments of the present invention, a non-linear optimization objective function based on reprojection error is established:

[0080] , where .

[0081] Among them : is the projection function including the camera internal parameter matrix .

[0082] On this basis, the LM (Levenberg-Marquardt) optimization framework is introduced. By the adaptive damping factor dynamically fuse the gradient descent method and the Gauss-Newton method to optimize the result of pose estimation and improve the robustness of the algorithm.

[0083] It should be noted that the SQPnP algorithm is an efficient PnP solution algorithm aimed at solving the camera pose estimation problem. The core of the SQPnP algorithm lies in using mathematical optimization methods to solve the PnP problem. Compared with traditional local optimization methods, the SQPnP algorithm provides a global optimal solution, which helps to improve the accuracy and reliability of the solution; the SQPnP algorithm uses efficient mathematical optimization methods and can obtain results in a relatively short time.

[0084] The LM algorithm is an estimation method for the least squares estimation of regression parameters in non-linear regression. This method is a combination of the steepest descent method and the linearization method (Taylor series). Because the steepest descent method is suitable for the case where the parameter estimation value is far from the optimal value at the beginning stage of iteration, and the linearization method, that is, the Gauss-Newton method, is suitable for the later stage of iteration within the range where the parameter estimation value is close to the optimal value.

[0085] In the embodiment of the present invention, the SQPnP algorithm generates a closed solution for the pose, and this closed solution is used as the initial solution of the LM algorithm, so that the LM algorithm converges faster and has higher accuracy in the subsequent iterative process.

[0086] As an implementation manner, the CNN-KF network includes an improved CNN model and a KF model. The method for fusing the three groups of sensing data by using the CNN-KF network to obtain a group of fused sensing data includes the following steps:

[0087] Input the three groups of sensing data into the improved CNN model to obtain predicted sensing data;

[0088] Use the predicted sensing data as the prior value of the KF model for prediction to obtain the fused sensing data.

[0089] Since the noise of the IMU is divided into linear noise and non-linear noise, and the Kalman filter can effectively track and eliminate jitter of the linear noise in the system, and the convolutional neural network has good spatial feature extraction ability and the ability to remove non-linear noise, the two are combined in the embodiment of the present invention.

[0090] Figure 3 FIG. is a schematic structural diagram of a CNN-KF network provided by an embodiment of the present invention. As shown in the figure, the cache layer is used to cache the predicted value of the previous moment. Since the scales of the acceleration and the angular velocity are different and the feature relationship is not obvious, the same network is used to process the two respectively. The accelerations or angular velocities of the three IMUs are regarded as vectors in space, where: Figure 3 ;

[0091] ;

[0092] Among them, represents the vector in space input into the CNN-KF network, , , represent accelerations or angular velocities in different directions, represents different IMUs.

[0093] Specifically, the improved CNN model sequentially includes a regularization layer, a first CNN model, a first activation function layer, a second CNN model, a second activation function layer, and a fully connected layer. The first CNN model and the second CNN model each include two convolutional layers and one fully connected layer. The convolutional kernel of one convolutional layer is 1×1, and the number of output channels is 6. The convolutional kernel of the other convolutional layer is 2×2, and the number of output channels is 9.

[0094] To avoid affecting the real-time performance and generalization of fusion, in the embodiments of the present invention, both the first CNN model and the second CNN model have only two convolutional layers and a fully connected layer for output. For the first layer of convolution, we use a 1*1 convolution with an output channel number of 6, aiming to increase the depth of the feature map to extract features of different depths of 3 IMU measurements. For the second layer of convolution, a 2*2 convolution kernel is used with an output channel number of 9, aiming to extract spatial features of different IMUs and their different coordinate axes. Finally, the fully connected layer extracts and fuses the feature information and then outputs the target value.

[0095] After passing through the CNN-KF network, the 3 groups of sensing data are tightened into a group of fused sensing data, which is used as the observation value and input into the Kalman filter. Combining with the predicted value at the previous moment for optimal iteration, the final result is obtained, which is the fused sensing data corresponding to the current moment.

[0096] The core of integrating the Kalman filter into the convolutional neural network is to focus on removing linear noise and non-linear noise by taking the outputs of both CNN-KF and CNN as supervision signals and incorporating them into the loss function. The total loss function is defined as follows:

[0097] ;

[0098] ;

[0099] ;

[0100] Among them, represents the training loss of the CNN-KF network, represents the error of the KF model, represents the error of the improved CNN network. Use to evaluate the similarity between the fused sensing data output by CNN-KF and the training label . On the other hand, is used to evaluate the similarity between the predicted sensing data output by CNN and the target output . is the number of training data points.

[0101] As an implementation, the steps of obtaining the replicated trajectory according to the rotation matrix, the translation matrix, and the fused sensing data include:

[0102] According to the rotation matrix, the translation matrix, and the fused sensing data, use the ESKF algorithm for processing to obtain the pose;

[0103] The pose is smoothed using the LS algorithm to obtain the replicated trajectory.

[0104] Among them, ESKF (Error State Kalman Filter) is an attitude estimation method based on Kalman filter. Its core idea is to take the attitude error as the state variable and use the Kalman filter to estimate the attitude error. Different from the traditional Kalman filter, ESKF does not directly estimate the state of the system, but estimates the error between the system state and the true state.

[0105] The LS (Laplace Smoothing) algorithm realizes the smoothing effect by weighted averaging the positions of each vertex with the positions of its neighboring vertices. By smoothing the pose, the replicated trajectory is obtained.

[0106] To verify the above-mentioned manual guiding device Figure 4 FIG. [FIGURE NUMBER] is an overall implementation diagram of a manual guiding body provided by an embodiment of the present invention Figure 5 FIG. [FIGURE NUMBER] is a schematic diagram of the actual internal circuit structure of a manual guiding body provided by an embodiment of the present invention, as Figure 4 and Figure 5 shown. In the actual implementation process, the manual guiding body is made by 3D printing and adopts a modular design as a whole. It can be divided into three parts: a device main body, a holding handle, and a teaching tool head. Among them, the device main body is a carrier for electronic components such as 3 IMU sensors and an MCU. The top of the main body is designed with two stacked cubes at a 45° angle, and ArUco marker codes for visual pose estimation are pasted around the cubes. This stacked design enables three marker codes to be recognized by the camera under the condition of space saving, improving the stability of visual pose estimation.

[0107] Moreover, the cross-section of the internal channel of the device main body is an equilateral triangle. The purpose of this design is to make the three vertically arranged IMUs form a 60° angle, so that the coordinate axes of the three IMUs are located on a horizontal line, thereby evenly sensing the inertial data when the teaching device moves. The holding handle and the teaching tool head are connected to the device main body through pins, which is convenient for users to replace different accessories according to different teaching scenarios.

[0108] Note: In the above translation, the "[FIGURE NUMBER]" in "FIG. [FIGURE NUMBER] is an overall implementation diagram of a manual guiding body provided by an embodiment of the present invention " and "FIG. [FIGURE NUMBER] is a schematic diagram of the actual internal circuit structure of a manual guiding body provided by an embodiment of the present invention, as " should be replaced with the actual figure number. Since the original text doesn't provide specific figure numbers, they are left as placeholders here.The device body is equipped with a mini development board, which has a dual-core processor with a main frequency of 240 MHz, supports SIMD (Single Instruction Multiple Data) instructions and floating-point operations (FPU), provides 3 UARTs (Universal Asynchronous Receiver / Transmitter), and can simultaneously receive 100hz inertial data sent by three IMUs. Its integrated 2.4GHz Wi-Fi (802.11 b / g / n, up to 150Mbps) can meet the demand for high-speed communication with the host computer. The cost of the IMU is about 15$. This sensor has high precision, specifically including an acceleration resolution of 0.0005g and a gyroscope resolution of 0.061° / s, and can meet the demand for inertial data of the handheld teaching device.

[0109] As Figure 1 shown, three IMUs communicate with the MCU (Micro Controller Unit) through the serial port. The MCU communicates with the host computer through WIFI. The camera sends image information to the host computer through Ethernet. The host computer communicates with the robot through Ethernet. In the hardware system, the MCU is only responsible for receiving and sending IMU data, and the sensor fusion algorithm runs on the host computer.

[0110] Finally, the effectiveness of the manual guidance device in the embodiment of the present invention is verified through experiments.

[0111] 1. Comparative experiment on multi-reference marker code fusion pose estimation algorithm

[0112] In order to verify the effectiveness of the multi-reference marker code fusion pose estimation algorithm proposed in the embodiment of the present invention, Figure 6 This is a comparison graph of the rotational errors of different algorithms under different noise ratios provided in the embodiment of the present invention. In the graph, the abscissa is the noise standard deviation, and the ordinate is the rotational error, with the unit of degree. Figure 7 This is a comparison graph of the translational errors of different algorithms under different noise ratios provided in the embodiment of the present invention. In the graph, the abscissa is the noise standard deviation, and the ordinate is the translational error, with the unit of %. Figure 8 This is a comparison graph of the rotational errors of different algorithms under outliers provided in the embodiment of the present invention. In the graph, the abscissa is the number of points, and the ordinate is the rotational error, with the unit of degree. Figure 9 This is a comparison graph of the translational errors of different algorithms under outliers provided in the embodiment of the present invention. In the graph, the abscissa is the noise standard deviation, and the ordinate is the translational error, with the unit of %, as Figures 6 to 9As shown in the figure, DLT (Direct Linear Transformation), +LM, CPnP (Consistent PnP), and EPnP (Efficient PnP) in the figure are all existing methods, and SQPnP+LM is the multi-reference marker code fusion pose estimation algorithm proposed in the embodiments of the present invention.

[0113] The embodiments of the present invention set up a synthetic data simulation experiment. In the simulation, for the intrinsic parameters of the camera, the focal lengths are set to fx = fy = 800, the resolution of the image is set to 640×480, the image origin is set in the upper left corner, and (uc, vc) = (320, 240). For the 3D points, the embodiments of the present invention randomly generate them from the region [-2, 2]×[-2, 2]×[4, 8]. The 3D-2D point pairs can be obtained by projecting the 3D points onto the image plane, and this data is used for algorithm verification. The algorithm of the embodiments of the present invention is compared with the other three (DLT+LM, EPnP, CPnP). All algorithm simulations are run 500 times, and the average value is taken as the result.

[0114] In this simulation experiment, to prove the anti-noise performance of the improved algorithm in the embodiments of the present invention and to solve the problem of the decrease in accuracy caused by the incorrect detection of 2D points in actual experiments, in the experiment, the embodiments of the present invention add Gaussian noise to the corresponding 2D points and add 5% outliers. Given the true camera rotation Rtrue and translation ttrue, by 、 calculate the error, The unit of is degrees, and the unit of

[0115] is percentage.

[0116] It can be seen from the experimental results that in the data with added noise and 5% outliers, the improved algorithm (SQPnP+LM) in the embodiments of the present invention has a significant improvement in anti-noise performance and effectively reduces the influence of outliers on the pose estimation accuracy. The method adopted in the embodiments of the present invention has a 5.57-degree improvement in the accuracy of the rotation vector and a 6.29% improvement in the accuracy of the translation vector in the noise data with different proportions, and has better anti-noise performance.

[0116] In the data with outliers, the accuracy of the rotation vector can reach 3.817 degrees, and the accuracy of the translation vector can reach 0.987%. The accuracy of the rotation vector is improved by 2 degrees, and the accuracy of the translation vector is improved by 7.537%. It can effectively solve the problem of the decrease in accuracy caused by the inaccurate detection of two-dimensional corner points in the data, and finally reach the accuracy of pose estimation under normal conditions.

[0117] 2. Use the CNN-KF network to implement a comparative experiment on the multi-IMU fusion pose trajectory tracking algorithm

[0118] Figures 10 to 12 This is a comparison graph of the acceleration change curves and the true values obtained by fusing different methods in the X, Y, and Z axis directions provided by the embodiments of the present invention. Among them, the abscissa represents time, and the ordinate represents acceleration, with the unit of m / s 2 ; Figures 13 to 15 This is a comparison graph of the angular velocity change curves and the true values obtained by fusing different methods in the X, Y, and Z axis directions provided by the embodiments of the present invention. Among them, the abscissa represents time, and the ordinate represents angular velocity, with the unit of degrees.

[0119] Among them, SLMU represents a single IMU, LS represents the Laplace smoothing algorithm, MLP represents a multi-layer perceptron, and TRUE represents the true value.

[0120] In order to verify the effectiveness of the multi-IMU fusion method proposed by the embodiments of the present invention, the embodiments of the present invention used the publicly available Xsens DOT IMU dataset for experiments. This dataset contains inertial raw measurement data collected by nine Xsens DOT IMUs and relies on Inertial Labs MRU-P as the true value reference. In the experiment, the vehicle uses a car equipped with an IMU array and a ROS ROBOT platform.

[0121] The embodiments of the present invention selected the data of three IMUs as inputs and applied three different methods for fusion: the least squares method, the multi-layer perceptron (MLP), and our CNN-KF network. Finally, the embodiments of the present invention used the mean square error (MSE) as the evaluation index to compare the results of the three fusion methods with the output of a single IMU and the true value. Table 1 is a comparison table of the mean square errors of acceleration and angular velocity provided by the embodiments of the present invention, and the comparison results are as follows:

[0122] Table 1

[0123]

[0124] It can be seen from the experimental results that the method of the embodiments of the present invention has obvious advantages in terms of acceleration and angular velocity accuracy compared with other methods. At the acceleration level, the method of the embodiments of the present invention has an order of magnitude improvement in accuracy compared with the least squares method. Compared with the MLP method, the accuracy is improved by about 231% on the YZ axis. At the angular velocity level, the change degree of the Z axis is relatively high, and the accuracy difference between a single IMU and the other two multi-IMU fusion methods is not significant, but the method of the embodiments of the present invention still has an accuracy improvement of 126% compared with it. The higher acceleration and angular velocity accuracy are beneficial to improving the performance of trajectory tracking and enhancing the tracking accuracy of the system under discontinuous acceleration motion.

[0125] 3. Actual system trajectory tracking experiment

[0126] Figure 16 A typical trajectory tracking comparison diagram provided by an embodiment of the present invention. In the figure, Device represents the motion trajectory of the teaching device, Robot represents the actual trajectory of the robot, 2 cm / s represents the speed of the robot moving to point a, 4 cm / s represents the speed of the robot moving to point b, and a, b, and c are all vertices during the motion process. As Figure 16 shown, in order to verify the tracking accuracy and stability of the handheld teaching device in a dynamic environment, the embodiment of the present invention conducts trajectory tracking experiments on S-shaped curves, circular arcs, and their combined curves. These trajectories have sufficient acceleration changes in different directions, which is beneficial to testing the performance of the device's trajectory tracking. Figure 16 Shown is a typical trajectory, whose shape is a combined trajectory including straight lines, broken lines, and curves, and the motion state includes uniform motion and variable-speed motion. The experimental method is to fix the teaching device at the end of the robot and use the trajectory of the robot end as the reference trajectory. The experimental index is the difference between the output trajectory pose and the reference trajectory pose.

[0127] Table 2

[0128]

[0129] Figure 16 It is a typical trajectory tracking comparison diagram. It can be seen from the figure that the trajectory tracking is smooth and continuous during uniform linear motion, and when moving along variable-speed curves and broken lines, the device's tracking of the trajectory does not cause the tracking trajectory to drift or distort due to discontinuous acceleration. Table 2 shows the statistical values of the maximum error range obtained after 6 trajectory tracking experiments. Among them, the maximum trajectory error is 8.19 mm, the minimum trajectory error is 0.77 mm, and the average trajectory error is 2.71 mm. The attitude errors are uniform, and the average angular errors on each rotation axis are all within 1°.

[0130] 4. Actual welding demonstration

[0131] Figure 17 A schematic diagram of the application of a teaching trajectory provided by an embodiment of the present invention, Figure 18 Another schematic diagram of the application of a teaching trajectory provided by an embodiment of the present invention. As Figure 17 and Figure 18 shown, it can be seen that this manual teaching device can complete the teaching task well.

[0132] Each module in the above-mentioned manual teaching device can be implemented in whole or in part by software, hardware, and their combination. The above-mentioned modules can be embedded in the processor of the computer device in hardware form or independent of it, or stored in the memory of the computer device in software form, so that the processor can call and execute the operations corresponding to the above-mentioned modules.

[0133] Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above methods. Among them, any reference to a memory, storage, database, or other medium used in the embodiments provided in the present application can include non-volatile and / or volatile memories. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in many forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), Rambus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and Rambus dynamic RAM (RDRAM), etc.

[0134] Those skilled in the art can clearly understand that, for the convenience and brevity of description, only the above division of each functional unit and module is used as an example. In actual applications, the above functions can be allocated to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above.

[0135] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements for some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be included in the protection scope of the present invention.

Claims

1. A robot manual guiding device integrating multiple IMUs and ArUco marker codes, characterized in that, It includes a camera, a manual guiding body, a host computer and a robot. Multiple ArUco marker codes are arranged on the outer surface of the manual guiding body. The camera is used to take pictures during the process of the manual guiding body executing a teaching trajectory and send the obtained image information to the host computer. Three IMUs are arranged inside the manual guiding body, and the three IMUs of the manual guiding body send three groups of sensed data collected during the movement to the host computer; The host computer processes the received photographed image and three groups of sensed data through the following steps to obtain a replicated trajectory and controls the robot to perform teaching through the replicated trajectory. The steps include: Obtain two-dimensional coordinates according to the image information, and perform projection conversion processing on the two-dimensional coordinates to obtain three-dimensional coordinates. The image information includes multiple ArUco marker codes; According to the two-dimensional coordinates and the three-dimensional coordinates, combine the multi-reference marker fusion pose estimation algorithm to obtain a rotation matrix and a translation matrix; Use the CNN-KF network to perform fusion processing on three groups of the sensed data to obtain one group of fused sensed data. The CNN-KF network is obtained by combining a CNN model and a KF model; According to the rotation matrix, the translation matrix, and the fused sensed data, obtain the replicated trajectory; The CNN-KF network includes an improved CNN model and a KF model. The step of using the CNN-KF network to perform fusion processing on three groups of the sensed data to obtain one group of fused sensed data includes: Input three groups of the sensed data into the improved CNN model to obtain predicted sensed data; Use the predicted sensed data as the prior value of the KF model for prediction to obtain the fused sensed data.

2. The robot manual guiding device integrating multiple IMUs and ArUco markers according to claim 1, wherein, The step of obtaining the rotation matrix and the translation matrix according to the two-dimensional coordinates and the three-dimensional coordinates by combining the multi-reference marker fusion pose estimation algorithm includes: According to the two-dimensional coordinates and the three-dimensional coordinates, combine the SQPnP algorithm to obtain an initial rotation matrix and an initial translation matrix; Use the initial rotation matrix and the initial translation matrix as the initial solution of the LM algorithm, and use the LM algorithm for iteration to obtain the rotation matrix and the translation matrix.

3. The robot manual guiding device integrating multiple IMUs and ArUco markers according to claim 1, wherein The improved CNN model sequentially includes a regularization layer, a first CNN model, a first activation function layer, a second CNN model, a second activation function layer, and a fully connected layer. The first CNN model and the second CNN model each include two convolutional layers and one fully connected layer. The convolutional kernel of one convolutional layer is 1*1, and the number of output channels is 6. The convolutional kernel of the other convolutional layer is 2*2, and the number of output channels is 9.

4. The robot manual guidance device integrating multiple IMUs and ArUco markers according to claim 1, characterized in that, The step of using the predicted sensed data as the prior value of the KF model for prediction to obtain the fused sensed data includes: For the current moment, use the predicted sensed data of the previous moment as the prior value of the current iteration, use the predicted sensed data as the observed value, use the KF model for prediction, and use the predicted value as the fused sensor data.

5. The robot manual guidance device integrating multiple IMUs and ArUco markers according to claim 1, characterized in that, The calculation formula for the training loss of the CNN-KF network is as follows: ; Among them, represents the training loss of the CNN-KF network, represents the error of the KF model, represents the error of the improved CNN network.

6. The robot manual guidance device integrating multiple IMUs and ArUco marker codes according to any one of claims 1 to 5, characterized in that, The step of obtaining the replicated trajectory according to the rotation matrix, the translation matrix, and the fused sensed data includes: Based on the rotation matrix, the translation matrix, and the fused sensing data, use the ESKF algorithm for processing to obtain the pose. Use the LS algorithm to smooth the pose to obtain the replicated trajectory.

7. The robot manual guidance device integrating multiple IMUs and ArUco markers according to claim 1, characterized in that, The end of the manual guiding body includes two cubes stacked at a 45-degree angle, and each cube is provided with an ArUco marker code on four sides.

8. The robot manual guidance device integrating multiple IMUs and ArUco markers according to claim 1, characterized in that, Three IMUs are vertically arranged at 60 degrees inside the manual guiding body.

Images