Heavy-load mechanical arm pose estimation method based on multi-source heterogeneous data collaborative fusion

Abstract

The invention belongs to the field of mechanical arm pose estimation, and particularly discloses a heavy-load mechanical arm pose estimation method based on multi-source heterogeneous data collaborative fusion. The method comprises the steps that firstly, the tail end acceleration, angular velocity and far infrared signals of the mechanical arm are obtained; randomly generating a group of particles, wherein each particle represents a possible mechanical arm pose; predicting particle poses based on the acceleration and the angular velocity, and updating the weights of the particles based on the far infrared signals; and finally, the particle swarm is resampled according to the weight of the particles, and the estimated pose of the mechanical arm is obtained according to the pose and the weight of the particle swarm. According to the method, the two kinds of heterogeneous data are effectively fused, the advantages of the two kinds of heterogeneous data are integrated, and therefore high-precision estimation of the tail end pose of the mechanical arm is achieved.

Description

Technical Field

[0001] This application belongs to the field of robotic arm pose estimation, and more specifically, relates to a method for estimating the pose of a heavy-duty robotic arm through collaborative fusion of multi-source heterogeneous data. Background Art

[0002] With the wide application of heavy-duty robotic arms in industrial manufacturing, aerospace, construction, and other fields, high-precision pose perception has become the key to achieving complex operation tasks.

[0003] Due to the characteristics of large range, large load, high speed, and complex operating environment of heavy-duty robotic arms, traditional motion capture systems are affected by sensor noise, drift, and integration errors, resulting in a decrease in accuracy after long-term use. Summary of the Invention

[0004] In view of the above defects or improvement requirements of the prior art, this application provides a method for estimating the pose of a heavy-duty robotic arm through collaborative fusion of multi-source heterogeneous data, aiming to solve the technical problem of insufficient accuracy in the pose estimation of existing heavy-duty robotic arms.

[0005] To achieve the above objective, in the first aspect, this application provides a method for estimating the pose of a heavy-duty robotic arm through collaborative fusion of multi-source heterogeneous data, including: Obtain the acceleration, angular velocity, and far-infrared signal at the end of the robotic arm; Randomly generate a particle swarm, where each particle represents a pose of the robotic arm; Predict the particle pose based on the acceleration and angular velocity, and update the weight of the particle based on the far-infrared signal; Resample the particle swarm according to the weights of the particles, and obtain the estimated pose of the robotic arm from the poses and weights of the particle swarm.

[0006] Preferably, predicting the particle pose based on the acceleration and angular velocity specifically includes: updating the particle position by integrating the acceleration; updating the particle attitude by integrating the angular velocity; and combining the updated value of the particle position and the updated value of the particle attitude to obtain the predicted value of the particle pose.

[0007] Preferably, updating the weight of the particle based on the far-infrared signal specifically includes: obtaining the observed value of the particle pose from the far-infrared signal; and updating the particle weight according to the error between the observed value of the particle pose and the predicted value of the particle pose.

[0008] Preferably, after updating the weight of the particle, normalize the particle weight.

[0009] Preferably, resampling the particle swarm according to the weights of the particles specifically includes deleting a certain number of particles with the smallest weights in the particle swarm and replicating a certain number of particles with the largest weights in the particle swarm, and ensuring that the number of particles in the particle swarm remains unchanged after deletion and replication.

[0010] Preferably, the estimated pose of the robotic arm is obtained from the poses and weights of the particle swarm, specifically: the estimated pose of the robotic arm is obtained by weighted averaging the particle poses of all particles in the particle swarm.

[0011] Preferably, the acquisition frequencies of the acceleration, angular velocity, and far-infrared signal at the end of the robotic arm are greater than 100 Hz.

[0012] Preferably, each particle represents a pose of the robotic arm, specifically: The pose represented by the n-th particle is a set of the particle position and the particle attitude:

[0013] wherein, represents the particle position, represents the particle attitude, and the superscript represents the serial number of the particle.

[0014] Preferably, the weights of the particles are updated based on the far-infrared signal, specifically: the observed value of the particle pose is obtained from the far-infrared signal ; the weight of the particle is updated to:

[0015] wherein, represents the particle weight, the parameter superscript represents the particle serial number, and the parameter subscript represents the time; represents the predicted value at the t-th time of the n-th particle pose and the observed value

[0016] wherein, represents the natural exponential function; is the observation noise covariance matrix; is the transpose matrix.

[0017] In a second aspect, the present application provides an electronic device, including: at least one memory for storing a program; at least one processor for executing the program stored in the memory, and when the program stored in the memory is executed, the processor is used to execute the method described in the first aspect or any possible implementation manner of the first aspect.

[0018] Generally speaking, compared with the prior art, the above technical solution conceived by the present application has the following beneficial effects: (1) This application obtains the dynamic posture information of the robot arm by acquiring high-frequency acceleration and angular velocity data at the end of the robot arm, and obtains the static posture information of the robot arm by high-frequency far-infrared signals. By improving the nonlinear and non-Gaussian processing capabilities of the particle filter, these two types of heterogeneous data are accurately combined, and the advantages of both are integrated to achieve high-precision estimation of the posture of the end of the robot arm.

[0019] (2) This application uses an improved particle filter as the core algorithm for pose estimation, which plays a key role in the collaborative fusion of multi-source heterogeneous data. This method effectively avoids the common error accumulation problem through weight updating, resampling and weighted averaging, thereby improving the accuracy and robustness of state estimation. In addition, this method can flexibly cope with nonlinear systems and non-Gaussian noise, adapt to different sensor data qualities, and ensure stable estimation during high-speed dynamics or intense motion.

[0020] (3) In this application, far-infrared signals are used to replace traditional sensor signals for motion capture. Far-infrared signals are particularly suitable for posture estimation in complex environments where heavy-duty robotic arms are located. Far-infrared light has a longer wavelength and better penetration, and can provide stable posture data in environments with complex lighting, smoke, dust, or thin obstructions. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 It is a flow chart of the construction and data collection of the far-infrared motion capture system provided in the embodiment of the present application.

[0022] Figure 2 It is a flow chart of the inertial motion capture system construction and data collection provided in the embodiment of the present application.

[0023] Figure 3 It is a flow chart of the robot arm posture estimation method provided in an embodiment of the present application.

[0024] Figure 4 It is a flow chart of the improved particle filtering method provided in the embodiment of the present application.

[0025] Figure 5 It is a schematic diagram of the structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0026] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application. In the embodiments of the present application, words such as "exemplary" or "for example" are used to represent examples, illustrations or explanations. Any embodiment or design solution described as "exemplary" or "for example" in the embodiments of the present application should not be construed as being more preferred or having more advantages than other embodiments or design solutions. Rather, the use of words such as "exemplary" or "for example" is intended to present relevant concepts in a specific manner.

[0027] In the description of the embodiments of the present application, unless otherwise specified, "a plurality of" means two or more. For example, a plurality of particles means two or more particles, etc. "Several" means one or more. For example, several particles means one or more particles, etc.

[0028] The embodiments of the present application will be described below with reference to the accompanying drawings in the embodiments of the present application.

[0029] First, build a motion capture system for the heavy-duty robotic arm: As Figure 1 shown, the process of building and data acquisition of the far-infrared motion capture system includes the following steps: According to the site space and the required capture space, twelve far-infrared cameras are erected around the robotic arm in a uniform arrangement, and the directions and angles of the cameras are adjusted to ensure that at least three camera fields of view can cover the capture area. The height of the capture space needs to be greater than the maximum movement height of the end of the robotic arm.

[0030] Remove the reflective objects in the site to avoid the influence of their reflection on the measurement. Use a calibration board with multiple reflection balls to perform multiple samplings in the capture area, calibrate the cameras, and calculate the positions, angles, and distortions of each camera, etc.

[0031] Fix reflection balls at the end of the robotic arm. The reflection balls should not be deformed during the fixing process, and the number should be more than three.

[0032] During the process of the robotic arm executing the motion instruction, the far-infrared motion capture cameras capture the far-infrared information of the robotic arm at a frequency of more than 100 Hz, and at the same time upload the far-infrared information to participate in data fusion.

[0033] As Figure 2 shown, the process of building and data acquisition of the inertial motion capture system includes the following steps: Install an IMU (Inertial Measurement Unit) on the end effector of the robotic arm and record the installation deviation.

[0034] During the process of the robotic arm executing the motion instruction, the IMU captures the acceleration information and angular velocity information at a frequency of more than 100 Hz, and at the same time uploads the acceleration information and angular velocity information to participate in data fusion.

[0035] After the motion capture system of the heavy-duty manipulator is built, the pose estimation and pose control of the heavy-duty manipulator can be carried out. As Figure 3 shown, the pose estimation and control method of the heavy-duty manipulator includes the following steps: Step 1: The heavy-duty manipulator moves under the drive of control instructions; Step 2: The far-infrared motion capture system collects the far-infrared signals at the end of the manipulator in real time; the inertial motion capture system collects the acceleration and angular velocity at the end of the manipulator in real time; Subsequently, the heterogeneous data fusion is carried out by improving the particle filter method. As Figure 4 shown, it specifically includes: Step 3: Randomly generate a group of particles, and each particle represents a pose of the manipulator; where the pose represented by the th particle is

[0036] Among them, represents the particle position, represents the particle attitude, and the superscript represents the particle serial number.

[0037] Step 4: Predict the particle pose based on the acceleration and angular velocity, update the particle position by integrating the acceleration, and update the particle attitude by integrating the angular velocity; combine the updated value of the particle position and the updated value of the particle attitude to obtain the predicted value of the particle pose. Specifically:

[0038]

[0039]

[0040] Among them, represents the particle position, represents the particle attitude; represents the particle pose, and the pose is the set of position and attitude; represents the acceleration, represents the angular velocity; the parameter subscript represents the time, and the parameter superscript represents the particle serial number; is the system noise.

[0041] Step 5: Update the weight of the particle based on the far-infrared signal, obtain the observed value of the particle pose from the far-infrared signal, and update the particle weight according to the error between the observed value of the particle pose and the predicted value of the particle pose. Specifically: Obtain the observed value of the particle pose from the far-infrared signal ; The weight of the particle is updated as follows:

[0042] Where, represents the particle weight, the parameter superscript represents the particle number, and the parameter subscript represents the time; represents the th predicted value of the pose of the th particle at time and the likelihood function between the observed value

[0043] Where, represents the natural exponential function; is the observation noise covariance matrix; is the transpose matrix.

[0044] Step 6: Normalize the particle weights. Since the update of the weights may cause their sum to deviate from 1, the weights need to be normalized:

[0045] Step 7: Resample the particle swarm according to the particle weights. Delete several particles with the smallest weights in the particle swarm and copy several particles with the largest weights in the particle swarm. After deletion and copying, ensure that the number of particles in the particle swarm remains unchanged. In this embodiment, 3 particles with the smallest weights are deleted, and 3 particles with the largest weights are copied once. It is also possible to choose: delete 3 particles with the smallest weights and copy 1 particle with the largest weight 3 times. It is also possible to choose: delete 1 particle with the smallest weight and copy 1 particle with the largest weight once.

[0046] Step 8: Obtain the estimated pose of the robotic arm from the poses and weights of the particle swarm. The estimated pose of the robotic arm is obtained by weighted averaging the particle poses of all particles in the particle swarm. The estimated pose of the robotic arm is specifically:

[0047] Where, represents the total number of particles in the particle swarm.

[0048] Step 9: Issue a control command to the robotic arm based on the estimated pose of the robotic arm.

[0049] In the embodiment of the present application, by improving the particle filter, the data of the IMU and the far-infrared motion capture system can be effectively fused, overcoming the limitations of single-modal data. The IMU provides real-time dynamic updates, and the motion capture system provides high-precision pose information. The combination of the two can significantly improve the accuracy and robustness of the pose estimation of the robotic arm end.

[0050] Based on the method in the above embodiments, an embodiment of the present application provides an electronic device, such as Figure 5 shown, including: a processor, a communications interface, a memory, and a communication bus. Among them, the processor, the communications interface, and the memory complete communication with each other through the communication bus. The processor can call the logical instructions in the memory to execute the method in the above embodiments.

[0051] In addition, when the logical instructions in the above-mentioned memory are implemented in the form of software functional units and sold or used as an independent product, they can be stored in a computer-readable storage medium. Based on such an understanding, the technical solution of the present application, in essence, or the part that contributes to the prior art, or a part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present application.

[0052] Based on the method in the above embodiments, an embodiment of the present application provides a computer-readable storage medium. The computer-readable storage medium stores a computer program. When the computer program runs on the processor, the processor is caused to execute the method in the above embodiments.

[0053] Based on the method in the above embodiments, an embodiment of the present application provides a computer program product. When the computer program product runs on the processor, the processor is caused to execute the method in the above embodiments.

[0054] It can be understood that the processor in the embodiments of the present application may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application specific integrated circuits (ASICs), field programmable gate arrays (FPGAs), or other programmable logic devices, transistor logic devices, hardware components, or any combination thereof. The general-purpose processor may be a microprocessor or any conventional processor.

[0055] The method steps in the embodiments of the present application can be implemented in a hardware manner or by a processor executing software instructions. The software instructions can be composed of corresponding software modules, and the software modules can be stored in a random access memory (RAM), flash memory, read-only memory (ROM), programmable ROM (PROM), erasable PROM (EPROM), electrically erasable PROM (EEPROM), register, hard disk, removable hard disk, CD-ROM, or any other form of storage medium well known in the art. An exemplary storage medium is coupled to the processor, enabling the processor to read information from the storage medium and write information to the storage medium. Of course, the storage medium can also be a component of the processor. The processor and the storage medium can be located in an ASIC.

[0056] In the above embodiments, it can be implemented in whole or in part by software, hardware, firmware, or any combination thereof. When implemented using software, it can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, the processes or functions described in the embodiments of the present application are generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable devices. The computer instructions can be stored in a computer-readable storage medium or transmitted through the computer-readable storage medium. The computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center in a wired manner (such as coaxial cable, optical fiber, digital subscriber line (DSL)) or a wireless manner (such as infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that the computer can access or a data storage device such as a server or data center that includes one or more integrated available media. The available medium can be a magnetic medium (such as a floppy disk, hard disk, magnetic tape), an optical medium (such as a DVD), or a semiconductor medium (such as a solid state disk (SSD)), etc.

[0057] It can be understood that the various numerical numbers involved in the embodiments of the present application are only for convenience of description and are not used to limit the scope of the embodiments of the present application.

[0058] Those skilled in the art can easily understand that the above are only the preferred embodiments of the present application and are not intended to limit the present application. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present application shall be included within the protection scope of the present application.

Claims

1. A method for pose estimation of a heavy-duty robotic arm with collaborative fusion of multi-source heterogeneous data, characterized in that, Comprising: Obtaining the acceleration, angular velocity and far-infrared signal at the end of the robotic arm; Randomly generating a particle swarm, where each particle represents a pose of the robotic arm; Predicting the particle pose based on the acceleration and angular velocity, and updating the weight of the particle based on the far-infrared signal; Resampling the particle swarm according to the weights of the particles, and obtaining the estimated pose of the robotic arm from the poses and weights of the particle swarm.

2. The pose estimation method for the heavy-duty robotic arm according to claim 1, characterized in that Predicting the particle pose based on the acceleration and angular velocity, specifically: updating the particle position by integrating the acceleration; updating the particle attitude by integrating the angular velocity; combining the updated value of the particle position and the updated value of the particle attitude to obtain the predicted value of the particle pose.

3. The pose estimation method of the heavy-duty robotic arm according to claim 1, characterized in that Updating the weight of the particle based on the far-infrared signal, specifically: obtaining the observed value of the particle pose from the far-infrared signal; updating the particle weight according to the error between the observed value of the particle pose and the predicted value of the particle pose.

4. The pose estimation method for a heavy-duty robotic arm according to claim 1, wherein Normalizing the particle weights after updating the weights of the particles.

5. The pose estimation method for the heavy-duty robotic arm according to claim 1, characterized in that Resampling the particle swarm according to the weights of the particles, specifically, deleting several particles with the smallest weights in the particle swarm, and copying several particles with the largest weights in the particle swarm, and ensuring that the number of particles in the particle swarm remains unchanged after deletion and copying.

6. The pose estimation method for the heavy-duty robotic arm according to claim 1, wherein Obtaining the estimated pose of the robotic arm from the poses and weights of the particle swarm, specifically: obtaining the estimated pose of the robotic arm by weighted averaging the particle poses of all particles in the particle swarm.

7. The pose estimation method of the heavy-duty robotic arm according to claim 1, characterized in that The acquisition frequency of the acceleration, angular velocity and far-infrared signal at the end of the robotic arm is greater than 100 Hz.

8. The pose estimation method of the heavy-duty robotic arm according to claim 1, wherein Each particle represents a pose of the robotic arm, specifically: The pose represented by the nth particle is a set of the particle position and the particle attitude: Among them, represents the particle position, represents the particle attitude, and the superscript represents the particle serial number.

9. The pose estimation method for the heavy-duty robotic arm according to claim 1, wherein Updating the weight of the particle based on the far-infrared signal, specifically: Obtaining the observed value of the particle pose from the far-infrared signal ; The weight of the particle is updated to: Among them, represents the particle weight, the parameter superscript represents the particle number, and the parameter subscript represents the time; represents the time of the predicted value of the pose of the th particle and the likelihood function between the observed values: Among them, represents the natural exponential function; is the observation noise covariance matrix; is the transpose matrix.

10. An electronic device, characterized in that, Comprising: At least one memory for storing a computer program; At least one processor for executing the program stored in the memory, and when the program stored in the memory is executed, the processor is used to execute the method according to any one of claims 1-9.

Images