Fusion positioning method, device and system of multi-joint system and medium

Abstract

The application relates to the technical field of measurement, in particular to a fusion positioning method, device and system of a multi-joint system and a medium. The multi-joint system comprises at least two joint arms connected in sequence, each joint arm comprises a rigid connecting rod and IMUs located at two ends of the rigid connecting rod, all the IMUs are electrically connected to a master control device, and a method corresponding to execution of the master control device is provided. Compared with the related art, the application deploys IMUs at two ends of each rigid connecting rod respectively, introduces a length parameter between the IMUs at two ends of the rigid connecting rod as one of constraint factors for constructing a hybrid factor graph model, converts the rigid geometric properties of the joint itself into continuous observation constraints, effectively suppresses the accumulation and divergence of the IMU integral drift with time without any external reference device, greatly reduces the system cost and structural complexity, avoids the problem that in the related art, the drift error accumulates with time and reaches the decimeter level in a few minutes, and improves the environmental adaptability.

Description

Technical Field

[0001] This application relates to the field of measurement technology, and in particular to a fusion positioning method, device, system and medium for a multi-joint system. Background Technology

[0002] In the field of precise positioning and motion capture for multi-joint systems such as industrial robotic arms, collaborative robots, and human lower limbs, Inertial Measurement Unit (IMU) technology is considered a fundamental solution due to its advantages such as complete autonomy, independence from the external environment, high dynamic response, and all-weather operation. However, IMUs based on Micro-Electro-Mechanical Systems (MEMS) technology have inherent physical limitations. Sensor bias instability, scaling factor errors, and white noise / random walk noise accumulate over time during integration operations. In particular, accelerometer errors produce displacement drift proportional to the cube of time after double integration. Even with complex online bias estimation techniques (such as Kalman filtering), relative position estimation errors inevitably reach decimeter or even meter levels within minutes without any external observation. This makes this solution completely unsuitable for the continuous and stable accuracy requirements of industrial precision operations or clinical gait analysis, severely limiting its practicality.

[0003] To overcome the above-mentioned shortcomings, several external auxiliary correction schemes have been proposed in related technologies. For example, by using a camera mounted on a multi-joint system or an external camera to capture environmental features and use visual geometric constraints to provide absolute or relative pose observations for the IMU, so as to periodically correct the IMU integral error; another example is to directly measure the joint angle by a high-precision photoelectric encoder at the joint, thereby achieving the accuracy of pose observation and error correction; and to provide positioning information through navigation systems such as GNSS, etc.

[0004] The aforementioned solutions significantly increase system cost and structural complexity, and suffer from poor environmental adaptability, being easily limited by specific scenarios. For example, GNSS is completely ineffective in indoor, underground, urban, and canyon environments. Therefore, designing a method that does not rely on any external reference equipment, is highly adaptable to various environments, and can suppress the inherent integration drift error of the IMU has become an urgent problem to be solved. Summary of the Invention

[0005] In view of the above-mentioned shortcomings and deficiencies of the prior art, this application provides a fusion positioning method, device, system and medium for multi-joint systems. The main purpose is to solve the problem that the current traditional multi-joint systems rely on external reference devices to suppress IMU integral drift error, resulting in high system cost and poor environmental adaptability.

[0006] To achieve the above objectives, the main technical solutions adopted in this application include:

[0007] In a first aspect, embodiments of this application provide a fusion positioning method for a multi-joint system, the multi-joint system comprising at least two articulated arms connected in sequence, each articulated arm comprising: a rigid link and IMUs located at both ends of the rigid link, all IMUs being electrically connected to a main control device; the method comprising:

[0008] For each joint arm, acquire the sensor data reported by the two IMUs of the joint arm at each time step;

[0009] Based on the zero-bias information and installation error information of each joint arm determined during the system initialization phase, the sensing data is compensated to obtain the compensated sensing data at each moment.

[0010] Based on the compensated sensor data, the IMU pre-integration factor used to characterize the relative motion increment at adjacent time points is calculated using a pre-integration method.

[0011] Based on the rotational degree of freedom, length parameter, IMU pre-integration factor and global constraints of each joint arm, a hybrid factor graph model including multiple constraint factors is constructed, and the hybrid factor graph model is iteratively solved using an incremental nonlinear optimization method to obtain the optimal pose estimate at the current time.

[0012] The length parameter is determined based on the spatial straight-line distance between the IMUs located at both ends of the rigid link, and the global constraint is the historical best pose estimate used in the process of iteratively solving the current best pose estimate.

[0013] Optionally, the system initialization phase includes: performing static calibration on the multi-joint system to obtain the zero-bias information of each IMU in the multi-joint system and the length parameters of each joint arm; the zero-bias information includes gyroscope zero-bias and accelerometer zero-bias; for each joint arm, performing dynamic calibration on each joint arm to obtain installation error information; the installation error information is used to characterize the error introduced by the IMUs at both ends of the rigid link of the joint arm during installation.

[0014] Optionally, for each joint arm, dynamic calibration is performed to obtain installation error information. This includes: for a multi-joint system, setting the joint arm to be calibrated to an active state and the uncalibrated joint arm to a locked state; controlling the joint arm to be calibrated to move at a specified frequency and amplitude, and acquiring data reported by the two IMUs of the current joint arm; using the length parameter as a constraint, using the offset vector of the IMU from the measurement center to the joint center and the actual rotation axis direction as the state variables to be estimated, and establishing a constraint relationship between the acceleration at the IMU measurement center and the acceleration at the joint center according to the acceleration transfer formula a_IMU=a_joint+α×r+ω×(ω×r); where a_IMU is the acceleration measured by the IMU, a_joint is the acceleration at the joint center, ω and α are the angular velocity and angular acceleration data reported by the IMU, and r is the offset vector to be estimated; performing nonlinear optimization on the constraint relationship to obtain the offset vector and actual rotation axis direction of the IMUs at both ends of each joint arm, as the installation error information.

[0015] Optionally, based on the current joint arm's zero-bias information and installation error information determined during the system initialization phase, the sensing data is compensated to obtain compensated sensing data at each moment, including: for the angular velocity measurement value and acceleration measurement value in the sensing data, subtracting the gyroscope's zero-bias from the angular velocity measurement value to obtain a first angular velocity value, and subtracting the accelerometer's zero-bias from the acceleration measurement value to obtain a first acceleration value; based on the installation error information, utilizing the constraint relationship between the IMU measurement center acceleration and the joint center acceleration, converting the first acceleration value from the IMU measurement center to the joint center to obtain a second acceleration value; and using the first angular velocity value and the second acceleration value as the compensated sensing data.

[0016] Optionally, the step of calculating the IMU pre-integration factor for characterizing the relative motion increment at adjacent time points based on the compensated sensing data using a pre-integration method includes: acquiring sensing data from the IMU sensor in real time at a first sampling frequency; optimizing the sensing data at a pre-set second sampling frequency, wherein multiple sensing data acquired at the first sampling frequency are included between two adjacent optimization solutions; wherein the first sampling frequency is an integer multiple of the second sampling frequency; performing pre-integration processing on the sensing data between two adjacent optimization solutions, and merging the pre-integrated data into a single relative motion increment to generate the IMU pre-integration factor connecting the two adjacent optimization solutions.

[0017] Optionally, based on the rotational degree of freedom attribute, length parameter, IMU pre-integration factor, and global constraints of each joint arm, a hybrid factor graph model including multiple constraint factors is constructed, including: when the current joint arm rotational degree of freedom attribute is multi-degree of freedom, determining the constraint factors used to construct the hybrid factor graph model, including the IMU pre-integration factor and the distance constraint factor; wherein, the distance constraint factor is the length parameter; when the current joint arm rotational degree of freedom attribute is single-degree of freedom, determining the constraint factors used to construct the hybrid factor graph model, including the IMU pre-integration factor, the distance constraint factor, and the attitude constraint factor associated with the joint axis direction; and maintaining a sliding window of a preset length, wherein the sliding window stores the historical pose estimation values ​​of the current optimization solution that are not at the current time; each time an optimization solution is executed, using the IMU attitude and position as variables to be optimized, adding the constraint factors determined at the current time for constructing the hybrid factor graph model to the factor graph, and updating the factor graph with the historical pose estimation values ​​stored in the sliding window as global constraints.

[0018] Optionally, the steps for obtaining the IMU pre-integration factor during the current optimization solution include: calculating the current optimization solution t k+1 Time and previous optimization solution t k The pre-integration increment at time step is used to obtain the IMU pre-integration factor; the IMU pre-integration factor includes: relative rotation ΔR{k,k+1} i The change in velocity Δv{k,k+1} i and positional change Δp{k,k+1} i Where i is the corresponding IMU number, i = 1, 2, ..., m, the number of IMU pre-integration factors is m × (N-1), and N is the preset length for maintaining the sliding window; the distance constraint factor is obtained by the following formula: ||p_k^b - p_k^a|| = L j Where p_k^a and p_k^b are the position variables of IMU a and IMU b, respectively, and the number of distance constraint factors is J × N, where J is the number of joints in the multi-joint system; the attitude constraint factors are obtained by the following formula: calculate the relative rotation of IMU a and IMU b R_k^a→b=(R_k^a) T R_k^b is converted to axis-angle representation to obtain the instantaneous rotation axis direction u_actual; the instantaneous rotation axis is defined relative to the actual rotation axis u in the installation error information. j Parallel, where the number of attitude constraint factors is J_hinge×N, where J_hinge is the number of hinge joints.

[0019] Optionally, an incremental nonlinear optimization method is used for real-time iterative solution to obtain the optimal pose estimate at the current moment, including: using an incremental nonlinear optimization method to perform real-time iterative solution on the hybrid factor graph model; in each iteration, adjusting the IMU attitude and position variables at each optimization solution moment within the sliding window according to the global constraints and the constraint factors to obtain the optimal solution; and using the optimal solution as the optimal pose estimate at the current moment.

[0020] Optionally, the incremental nonlinear optimization method for real-time iteratively solving the hybrid factor graph model includes: performing real-time iterative solving of the hybrid factor graph model using an incremental solver; after each iteration, marginalizing the state variable at the earliest optimization time within the sliding window, retaining the marginalized information as a prior constraint in the factor graph, and removing the state variable from the optimization variables; using the prior constraint and the remaining historical pose estimates within the sliding window as the initial values ​​for the next iteration, and continuing to iteratively solve the factor graph model at the current time.

[0021] Optionally, all IMUs in the multi-joint system share the same synchronization clock; the master control device acquires the sensing data reported by the two IMUs of the joint arm at each moment by: reading the sensing data of each IMU in parallel through direct memory access (DMA) and timestamping the data read in each frame.

[0022] Secondly, embodiments of this application provide a fusion positioning device for a multi-joint system. The multi-joint system includes at least two articulated arms connected in sequence. Each articulated arm includes a rigid link and IMUs located at both ends of the rigid link. All IMUs are electrically connected to a main control device. The fusion positioning device includes:

[0023] The acquisition unit is configured to acquire the sensor data reported by the two IMUs of each joint arm at each time step.

[0024] The compensation unit is configured to compensate the sensing data based on the zero bias information and installation error information of each joint arm determined during the system initialization phase, so as to obtain the compensated sensing data at each moment.

[0025] The pre-integration unit is configured to calculate the IMU pre-integration factor for characterizing the relative motion increment at adjacent time points using a pre-integration method based on the compensated sensing data.

[0026] The processing unit is configured to construct a hybrid factor graph model including multiple constraint factors based on the rotational degree of freedom attributes, length parameters, IMU pre-integration factors and global constraints of each joint arm, and to iteratively solve the hybrid factor graph model using an incremental nonlinear optimization method to obtain the optimal pose estimate at the current moment.

[0027] The length parameter is determined based on the spatial straight-line distance between the IMUs located at both ends of the rigid link, and the global constraint is the historical best pose estimate used in the process of iteratively solving the current best pose estimate.

[0028] Thirdly, this application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the fusion positioning method for the multi-joint system described in the first aspect.

[0029] Fourthly, this application provides a fusion positioning system for a multi-joint system, including a storage medium, a processor, and a computer program stored on the storage medium and executable on the processor. When the processor executes the computer program, it implements the fusion positioning method for the multi-joint system described in the first aspect.

[0030] By employing the above technical solution, this application provides a fusion positioning method for a multi-joint system. The multi-joint system includes at least two sequentially connected articulated arms. Each articulated arm includes a rigid link and IMUs located at both ends of the rigid link. All IMUs are electrically connected to a main control device. The positioning method includes: for each articulated arm, acquiring the sensing data reported by the two IMUs of the articulated arm at each moment. Then, based on the zero-bias information and installation error information of each articulated arm determined during the system initialization phase, the sensing data is compensated to obtain the compensated sensing data at each moment. Based on the compensated sensing data, the IMU pre-integration factor used to characterize the relative motion increment at adjacent moments is calculated using a pre-integration method. Based on the rotational degree of freedom attribute, length parameter, IMU pre-integration factor, and global constraints of each articulated arm, a hybrid factor graph model including multiple constraint factors is constructed. An incremental nonlinear optimization method is used to iteratively solve the hybrid factor graph model to obtain the optimal pose estimate at the current moment. Among them, the length parameter is determined based on the spatial straight-line distance between the IMUs located at both ends of the rigid link, and the global constraint is the historical optimal pose estimate used in the process of iteratively solving the optimal pose estimate at the current moment. Compared with related technologies, by deploying IMUs at both ends of each rigid link and introducing the length parameter between the IMUs at both ends of the rigid link as one of the constraint factors for constructing the hybrid factor graph model, the fixed geometric length of the articulated arm itself is transformed into a real-time virtual observation constraint. This effectively suppresses the cumulative divergence of IMU integral drift over time without any external reference equipment, significantly reducing system cost and structural complexity. It avoids the problem in related technologies where drift error accumulates to the decimeter level within minutes, while also improving environmental adaptability. Attached Figure Description

[0031] Figure 1 A flowchart illustrating a fusion positioning method for a multi-joint system provided in an embodiment of this application;

[0032] Figure 2 A schematic diagram of a multi-joint system provided in an embodiment of this application;

[0033] Figure 3 A flowchart illustrating another fusion positioning method for a multi-joint system provided in an embodiment of this application;

[0034] Figure 4 An error comparison test result diagram provided for an embodiment of this application;

[0035] Figure 5 This is a schematic diagram of a fusion positioning device for a multi-joint system provided in an embodiment of this application. Detailed Implementation

[0036] To better understand the above technical solutions, exemplary embodiments of this application will be described in more detail below with reference to the accompanying drawings. Although exemplary embodiments of this application are shown in the drawings, it should be understood that this application can be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this application can be understood more clearly and thoroughly, and that the scope of this application can be fully conveyed to those skilled in the art.

[0037] To address the issues of high cost and poor environmental adaptability associated with traditional multi-joint systems that rely on external reference devices (such as expensive encoders) to suppress IMU integral drift errors, this application proposes a fusion localization method for multi-joint systems. In this embodiment, the multi-joint system does not use an encoder, but only a low-cost IMU. Specifically, the following... Figure 1 The method is applicable to multi-joint systems comprising at least two articulated arms connected in sequence. Each articulated arm includes a rigid link and an IMU located at both ends of the rigid link. The IMU is fixedly connected to the rigid link, such as by adhesive bonding or mechanical clamping, to ensure integral movement with the link. Figure 2 As shown, all IMUs in the multi-joint system are electrically connected to the master control unit, which is used to execute the fusion positioning method of the multi-joint system.

[0038] The multi-joint system corresponding to the method in this embodiment refers to a kinematic chain structure composed of at least two rigid links connected in series or parallel through a joint connection mechanism, wherein each rigid link can rotate or move relative to the joint axis. Examples include industrial robot arms, collaborative robots, bionic robots, and even human skeletons. In one specific embodiment, the multi-joint system includes at least two articulated arms connected in series, each articulated arm including a rigid link and IMUs located at both ends of the rigid link, and all IMUs are connected to a master control device for master control.

[0039] Using a robotic arm as an example, the specific setup of the aforementioned multi-joint system will be explained, such as... Figure 2 The diagram illustrates a multi-joint system comprising six articulated arms. Adjacent arms are connected via joint connection mechanisms. Each arm includes a rigid link and two IMUs (Integrated Mutor Units) at both ends of the rigid link, such as IMU 1A and IMU 1B, forming an IMU pair corresponding to the first rigid link, and so on. As a possible configuration, combined with... Figure 2The IMU can be set at the center of the end face of the joint connection mechanism, so that the IMU pair corresponding to the rigid link can obtain the spatial straight distance between the two IMU mounting points of each joint arm, for example, the spatial straight distance between IMU 1A and IMU 1B is L1, which can be used as a key constraint in the subsequent processing.

[0040] In another feasible implementation, taking a human upper limb as an example as a multi-joint system, the upper limb includes two joint arms: the upper arm and the forearm. Four IMUs can be installed in the upper limb (at the joints), divided into two groups of IMUs to collect data on the distance between the upper arm and the forearm, respectively. This allows for the acquisition of sensor data reported by the IMUs at both ends of each joint arm at any given time. The sensor data includes angular velocity data and acceleration data. The IMUs can be installed using adhesive or mechanical clamps to ensure a secure connection with the connecting rods.

[0041] Furthermore, such as Figure 1 As shown, this embodiment provides a fusion positioning method for a multi-joint system. This method is described from the perspective of fusion positioning during dynamic use, and its execution subject can be a main control device. The method includes:

[0042] S101 acquires the sensor data reported by the two IMUs of each joint arm at each moment.

[0043] In practical applications, the main control device receives sensor data reported by all IMUs. The sensor data reported by each IMU includes angular velocity measurement and acceleration measurement. In this embodiment, the main control device can locate the joint arm based on the sensor data reported by the IMUs at both ends of each joint arm. Therefore, steps S101 to S105 below describe the data processing process for a single joint arm, and the data processing process is the same for all joint arms.

[0044] Understandably, in order to better process the data, in this embodiment, the sensor data reported by the two IMUs of the articulated arm at each moment can be timestamped and stored in a buffer for easy processing in subsequent steps.

[0045] S102, based on the zero-bias information and installation error information of each joint arm determined during the system initialization phase, the sensing data is compensated to obtain the compensated sensing data at each moment.

[0046] In this step, the system initialization phase can be understood as the system power-on process or the system start-up phase. Calibration is performed during this phase, including static and dynamic calibration. The zero-bias information may include gyroscope and accelerometer zero-bias. Installation error information characterizes the errors introduced by the IMUs at both ends of the rigid linkage of the articulated arm during installation. These installation errors include two types: positional error, specifically the offset vector of the IMU center relative to the joint center; and orientation error, manifested in the rotational deviation between the IMU coordinate system and the joint coordinate system. The IMU measures the acceleration centered on the IMU, but the acceleration at the joint center is required. This is an error that inevitably arises during IMU installation and must be considered. Based on this, installation error information is determined during the dynamic calibration phase, specifically including the offset vector of the IMUs at both ends of each articulated arm and the actual rotation axis direction. This error is then eliminated during subsequent compensation, ensuring the feasibility and accuracy of the solution.

[0047] The installation error information is obtained based on the spatial straight-line distance in the above architecture, rather than through conventional data accuracy adjustments. Since the manual installation process of the IMU cannot guarantee absolute precision, it can lead to a misalignment between the IMU measurement center and the joint geometric center. The inventive concept of this embodiment is precisely based on the spatial straight-line distance in the above architecture. By using the spatial straight-line distance as a constraint, dynamic calibration is performed to obtain installation error information, thereby achieving online compensation for installation errors and ensuring that the physical constraint (spatial straight-line distance) can be correctly utilized by the algorithm.

[0048] S103, based on the compensated sensing data, uses a pre-integration method to calculate the IMU pre-integration factor used to characterize the relative motion increment at adjacent time points.

[0049] In S103, adjacent time points can be two consecutive data acquisition times, or adjacent optimization solution times. Optimization solution times refer to optimizing the acquisition frequency of data acquired two or more times at adjacent time points. In practical applications, due to the high sampling frequency of the IMU, directly integrating the raw sensor data results in an enormous computational burden, failing to meet real-time requirements. A key characteristic of pre-integration is that its result depends only on the IMU measurements and estimated zero bias within that time period, and is independent of the absolute pose. This means it eliminates the need to repeatedly integrate large amounts of raw sensor data. Therefore, by optimizing the acquisition frequency and then pre-integrating the data at the optimization solution time, the system processing burden is reduced.

[0050] S104. Based on the rotational degree of freedom, length parameter, IMU pre-integration factor and global constraints of each joint arm, a hybrid factor graph model including multiple constraint factors is constructed. An incremental nonlinear optimization method is used to iteratively solve the hybrid factor graph model to obtain the optimal pose estimate at the current time.

[0051] The length parameter is determined based on the linear spatial distance between the IMUs located at both ends of the rigid link. The hybrid factor graphical model is a constraint model with multiple constraint factors, using the attitude and position of the IMUs at various times as the variables to be optimized within a sliding window. Specifically, the constraint factors of the hybrid factor graphical model include an IMU pre-integration factor, a distance constraint factor related to the length parameter of the current articulated arm, and an optional attitude constraint factor. More specifically, when the current articulated arm's rotational degree of freedom is multi-degree-of-freedom, the constraint factors include the IMU pre-integration factor and the distance constraint factor; when the current articulated arm's rotational degree of freedom is single-degree-of-freedom, the constraint factors include the IMU pre-integration factor, the distance constraint factor, and the attitude constraint factor associated with the joint axis direction. Here, rotational degree of freedom refers to whether the joint has multiple directions of rotation. For example, for hinge joints, such as the rotary joint of a robotic arm or the human knee joint, rotation is only possible in a single direction, which is a single degree of freedom; while for ball joints or flexible joints, such as the human shoulder joint, it is multi-degree-of-freedom.

[0052] Global constraints are historical best pose estimates (not from the current time) used in the iterative process of finding the optimal pose estimate at the current moment. Specifically, this can be achieved by maintaining a sliding window of a preset length, which stores high-precision pose estimates obtained at previous moments. Although these historical pose estimates have a certain time lag, they are highly accurate due to corrections made during previous optimization processes and can be used as global constraints for the current moment. Simultaneously, the IMU pre-integration factor at the current moment provides real-time local motion information. Combining historical high-precision poses with current real-time motion information ensures both estimation accuracy and real-time response to changes in system motion.

[0053] After the model is built, an incremental nonlinear optimization method is used to iteratively solve the hybrid factor graph model. The goal of the iterative solution is to find a set of optimal IMU pose and position variables that simultaneously satisfy all constraints. Specifically, the solver uses the optimal pose estimate from the previous time step within the sliding window as the initial value, and iteratively calculates the residuals of each constraint factor, continuously adjusting the values ​​of the variables to be optimized until convergence.

[0054] Through the above optimization solution, the optimal pose estimate at the current moment is obtained. The role of this optimal pose estimate is to suppress IMU integral drift error. Since the length parameter acts as a constraint, it forces the positional relationship between the two IMUs at both ends of the same rigid link to satisfy a fixed spatial straight-line distance (length parameter). This geometric constraint continuously generates a corrective force during the optimization process, pulling the position estimates that have drifted apart due to IMU noise and zero bias back to the correct geometric relationship, thereby achieving closed-loop suppression of drift error.

[0055] Meanwhile, in this embodiment, the sliding window adopts a rolling update mechanism, which adds the current result to the window after each solution is completed and marginalizes the earliest state, ensuring that the optimization scale is constant and achieving real-time and stable positioning output.

[0056] This embodiment provides a fusion positioning method for a multi-joint system, wherein the multi-joint system includes at least two sequentially connected joint arms. Each joint arm includes a rigid link and IMUs located at both ends of the rigid link, and all IMUs are electrically connected to a main control device. The positioning method includes: for each joint arm, acquiring the sensing data reported by the two IMUs of the joint arm at each moment. Then, based on the zero-bias information and installation error information of each joint arm determined during the system initialization phase, the sensing data is compensated to obtain the compensated sensing data at each moment. Based on the compensated sensing data, the IMU pre-integration factor used to characterize the relative motion increment at adjacent moments is calculated using a pre-integration method. Based on the rotational degree of freedom attribute, length parameter, IMU pre-integration factor, and global constraints of each joint arm, a hybrid factor graph model including multiple constraint factors is constructed, and the hybrid factor graph model is iteratively solved using an incremental nonlinear optimization method to obtain the optimal pose estimate at the current moment. Here, the length parameter is determined based on the spatial straight-line distance between the IMUs located at both ends of the rigid link, and the global constraint is the historical optimal pose estimate used in the process of iteratively solving the optimal pose estimate at the current moment. Compared with related technologies, by deploying IMUs at both ends of each rigid link and introducing the length parameter between the IMUs at both ends of the rigid link as one of the constraint factors for constructing the hybrid factor graph model, the fixed geometric length of the articulated arm itself is transformed into a real-time virtual observation constraint. This effectively suppresses the cumulative divergence of IMU integral drift over time without any external reference equipment, significantly reducing system cost and structural complexity. It avoids the problem in related technologies where drift error accumulates to the decimeter level within minutes, while also improving environmental adaptability.

[0057] Optionally, the system initialization phase includes: performing static calibration on the multi-joint system to obtain the zero-bias information of each IMU in the multi-joint system and the length parameters of each joint arm; the zero-bias information includes gyroscope zero-bias and accelerometer zero-bias; for each joint arm, performing dynamic calibration on each joint arm to obtain installation error information; the installation error information is used to characterize the error introduced by the IMUs at both ends of the rigid link of the joint arm during the installation process.

[0058] In this embodiment, the processes of static and dynamic calibration are described. Static calibration is performed after the multi-joint system is powered on and before the actual task is executed. Specifically, the multi-joint system is kept completely still for at least a period of time, and static output data from all IMUs are collected. The average output values ​​of the gyroscope and accelerometer of each IMU are calculated as the initial zero bias under the current environment, i.e., zero bias information. This zero bias information is subtracted from the original data before subsequent data processing. Specifically, the gyroscope zero bias is subtracted from the angular velocity measurement value, and the accelerometer zero bias is subtracted from the acceleration measurement value to eliminate the inherent constant bias of the sensors and provide a cleaner data source for subsequent pre-integration.

[0059] The dynamic calibration process involves driving the multi-joint system to execute preset excitation movements and obtaining installation error information during the movement of the multi-joint system. The preset excitation movements refer to simple and repetitive movements that are pre-set, such as controlling the joint arm to swing sinusoidally, or making the upper limb swing at a low frequency with a fixed amplitude (such as swinging 30° to the left and right).

[0060] Furthermore, for each articulated arm, a dynamic calibration method is used to dynamically calibrate each articulated arm to obtain installation error information. This includes: for multi-joint systems, setting the articulated arm to be calibrated to an active state and the uncalibrated articulated arms to a locked state; controlling the articulated arm to be calibrated to move at a specified frequency and amplitude, and acquiring data reported by the two IMUs of the current articulated arm; using the length parameter as a constraint, the offset vector of the IMU from the measurement center to the joint center and the actual rotation axis direction are used as state variables to be estimated. According to the acceleration transfer formula a_IMU=a_joint+α×r+ω×(ω×r), a constraint relationship between the acceleration at the IMU measurement center and the acceleration at the joint center is established; where a_IMU is the acceleration measured by the IMU, a_joint is the acceleration at the joint center, ω and α are the angular velocity and angular acceleration data reported by the IMU, and r is the offset vector to be estimated; the constraint relationship is solved by nonlinear optimization to obtain the offset vector and actual rotation axis direction of the IMUs at both ends of each articulated arm, which are used as installation error information.

[0061] Specifically, when driving the multi-joint system to perform preset excitation motion, within the sliding window optimization framework, the spatial straight-line distance between the two IMU mounting points of the current joint arm is used as a constraint. The offset vector of the IMU from the measurement center to the joint center and the actual rotation axis direction are used as state variables to be estimated. The offset vector of the IMU and the actual rotation axis direction are obtained through nonlinear optimization.

[0062] In this embodiment, the dynamic calibration process is not simply an adjustment of the accuracy of the installation data. Instead, it is based on the spatial straight-line distance between the two IMU mounting points of the current articulated arm as a constraint, thereby obtaining the installation error information under this constraint. The installation error information includes the IMU offset vector and the actual rotation axis direction.

[0063] It's important to note that the installation process introduces two errors: positional error, specifically the offset vector of the IMU center relative to the joint center; and directional error, manifested in the rotational deviation between the IMU coordinate system and the joint coordinate system. The IMU measures the acceleration centered at itself, but we need the acceleration at the joint center. The joint center is the rotational center between two adjacent rigid links; simply put, it's the point that remains stationary when the joint rotates. This installation error is inevitable during IMU installation. To resolve this error, a constraint relationship needs to be established between the acceleration at the IMU measurement center and the acceleration at the joint center, converting the acceleration from the IMU center to the joint center. This allows for the calculation of the offset vector, which is then used to eliminate this error during subsequent compensation. Specifically, in the above transfer formula, when the joint rotates with angular velocity ω and angular acceleration α, the acceleration measured by the IMU, a_IMU, and the acceleration at the joint center, a_joint, satisfy the acceleration transfer formula of rigid body kinematics. By collecting IMU acceleration and angular velocity data during joint swing, a nonlinear optimization problem is constructed, and the IMU offset vector (three-dimensional) r is used as an unknown quantity and solved together with the joint axis direction u.

[0064] Furthermore, since the acceleration a_joint at the joint center is an unknown quantity during the dynamic calibration phase, it can be eliminated using the dual-IMU differential method:

[0065] Let the vectors of the IMUs (IMU A and IMU B) at both ends of the articulated arm be r_A and r_B respectively, and let r_B = r_A + d, where d is the length parameter, that is, the straight-line distance between the two IMU mounting points of the current articulated arm.

[0066] The acceleration measurements from the two IMUs satisfy the following:

[0067] a_A=a_joint+α×r_A+ω×(ω×r_A);

[0068] a_B=a_joint+α×r_B+ω×(ω×r_B);

[0069] Subtracting the two equations eliminates the unknown a_joint:

[0070] a_B - a_A = α × (r_B - r_A) + ω × (ω × (r_B - r_A)). Since r_B - r_A = d, substituting these values, we get: a_B - a_A = α × d + ω × (ω × d). Here, d is a known quantity (determined by the length parameter and the IMU mounting direction). The joint axis direction u can be obtained by projecting the angular velocity vector onto the main rotation direction, or it can be obtained as follows: For a hinge joint, the joint can only rotate around a fixed axis. When the joint rotates around axis u with an angular velocity ω, the angular velocity vector ω_IMU measured by the IMU satisfies ω_IMU = ω·u, meaning the direction of ω_IMU is consistent with the joint axis direction u. Therefore, by collecting the angular velocity data of the IMU during the joint's swing and statistically averaging the angular velocity vector directions at multiple moments, the joint axis direction u can be obtained, representing the only direction of rotation allowed for the joint.

[0071] In this embodiment, the installation error parameters obtained through a dynamic calibration process are then compensated for in subsequent processes. This solves the problem that manual installation cannot be absolutely precise, leading to a misalignment between the IMU measurement center and the joint geometric center, thus rendering the length parameter as a constraint ineffective on the model. Through the aforementioned dynamic calibration process, online compensation for installation errors is automatically completed, ensuring that the physical constraints of this embodiment can be correctly utilized, thereby improving practicality and robustness.

[0072] Optionally, based on the current joint arm's zero-bias information and installation error information determined during the system initialization phase, the sensing data is compensated to obtain the compensated sensing data at each moment, including: for the angular velocity measurement value and acceleration measurement value in the sensing data, subtracting the gyroscope zero-bias from the angular velocity measurement value to obtain the first angular velocity value, and subtracting the accelerometer zero-bias from the acceleration measurement value to obtain the first acceleration value; based on the installation error information, using the constraint relationship between the IMU measurement center acceleration and the joint center acceleration, the first acceleration value is converted from the IMU measurement center to the joint center to obtain the second acceleration value; the first angular velocity value and the second acceleration value are used as the compensated sensing data.

[0073] In this embodiment, the compensation process first performs compensation based on zero bias information to obtain a first angular velocity value and a first acceleration value. Then, based on the installation error information, the first acceleration value is converted from the IMU measurement center to the joint center using the constraint relationship between the IMU measurement center acceleration and the joint center acceleration to obtain a second acceleration value. The specific compensation process is also based on the above-mentioned constraint relationship between the IMU measurement center acceleration and the joint center acceleration, that is, the centripetal acceleration and tangential acceleration components generated by the rotation around the joint are subtracted from the acceleration measured by the IMU to obtain the second acceleration value at the joint center, ensuring the accuracy and feasibility of the calculation.

[0074] Optionally, based on the compensated sensing data, an IMU pre-integration factor for characterizing the relative motion increment at adjacent time points is calculated using a pre-integration method. This includes: acquiring sensing data from the IMU sensor in real time at a first sampling frequency; optimizing the sensing data at a pre-set second sampling frequency, wherein multiple sensing data acquired at the first sampling frequency are included between two adjacent optimization solutions; wherein the first sampling frequency is an integer multiple of the second sampling frequency; performing pre-integration processing on the sensing data between two adjacent optimization solutions, and merging the pre-integrated data into a single relative motion increment to generate an IMU pre-integration factor connecting two adjacent optimization solutions.

[0075] In this embodiment, after the system undergoes dynamic calibration during initialization, the multi-joint system can perform other actions normally. During this process, sensor data collected by each IMU is acquired and compensated.

[0076] Furthermore, an IMU pre-integration factor characterizing the relative motion increment is calculated based on the compensated sensor data. This relative motion increment specifically includes relative rotation, velocity change, and position change. In one feasible implementation, to avoid repeating high-frequency integration calculations in subsequent iterations, before calculating the pre-integration factor, adjacent time points are optimized using a pre-set optimization frequency, i.e., a second sampling frequency. This encapsulates high-frequency IMU data into low-frequency optimization time points. Specifically, considering common IMU acquisition frequencies, if the first sampling frequency is 100Hz, the second sampling frequency can be set to 10Hz. This results in 10 original sensor data points between adjacent optimization time points. These 10 sensor data points are merged into a single relative motion increment, and numerical integration is used to obtain the IMU pre-integration factor connecting two adjacent optimization time points, thereby reducing the system's computational load.

[0077] Optionally, based on the rotational degree of freedom attributes, length parameters, IMU pre-integration factors, and global constraints of each joint arm, a hybrid factor graph model including multiple constraint factors is constructed. This includes: when the current joint arm rotational degree of freedom attribute is multi-degree-of-freedom, determining the constraint factors used to construct the hybrid factor graph model, including IMU pre-integration factors and distance constraint factors; wherein, the distance constraint factor is a length parameter; when the current joint arm rotational degree of freedom attribute is single-degree-of-freedom, determining the constraint factors used to construct the hybrid factor graph model, including IMU pre-integration factors, distance constraint factors, and attitude constraint factors associated with the joint axis direction; and maintaining a sliding window of a preset length, the sliding window storing historical pose estimates not at the current time in this optimization solution; each time an optimization solution is executed, using IMU attitude and position as variables to be optimized, adding the constraint factors determined at the current time for constructing the hybrid factor graph model to the factor graph, and updating the factor graph with the historical pose estimates stored in the sliding window as global constraints.

[0078] Furthermore, the steps for obtaining the IMU pre-integration factor during the current optimization solution include: calculating the current optimization solution t k+1 Time and previous optimization solution t k The pre-integration increment at time step is used to obtain the IMU pre-integration factor; the IMU pre-integration factor includes: relative rotation ΔR{k,k+1} i The change in velocity Δv{k,k+1} i and positional change Δp{k,k+1} i Where i is the corresponding IMU number, i = 1, 2, ..., m, m is a positive integer, the number of IMU pre-integration factors is m × (N-1), and N is the preset length of the maintenance sliding window; the distance constraint factor is obtained by the following formula: ||p_k^b - p_k^a|| = L j Where p_k^a and p_k^b are the position variables of IMU a and IMU b, respectively, and the number of distance constraint factors is J×N, where J is the number of joints in the multi-joint system; calculate the relative rotation of IMU a and IMU b R_k^a→b=(R_k^a). T R_k^b is converted to axis-angle representation to obtain the instantaneous rotation axis direction u_actual; the instantaneous rotation axis is then defined relative to the actual rotation axis u in the installation error information. j Parallel, where the number of attitude constraint factors is J_hinge×N, where J_hinge is the number of hinge joints.

[0079] In this embodiment, the determination of the corresponding constraint factors based on the current rotational degree of freedom attributes of the articulated arm is not elaborated further. In a specific implementation, the preset length of the sliding window can be 10-20 frames, corresponding to 1-2 seconds of historical data. Since the hybrid factor graph model is a constraint model with multiple constraint factors using the attitude and position of the IMU at each moment as the variables to be optimized within the sliding window, the historical pose estimates of each optimization solution moment are stored in real time within the sliding window during the actual execution of other behaviors by the multi-joint system. Furthermore, during each optimization solution, the IMU pre-integration factor at the current moment and the determined constraint factors are added to the factor graph to obtain the hybrid factor graph model. The determined constraint factors here include at least the IMU pre-integration factor and the distance constraint factor associated with the spatial straight-line distance between the two IMU mounting points of the current articulated arm. The real-time IMU pre-integration factor provides local motion continuity, while the distance constraint factor can always suppress the drift error accumulated over time, thus ensuring both estimation accuracy and avoiding cumulative lag in the model.

[0080] Furthermore, in the process of solving the attitude constraint factor, IMU a and IMU b refer to two IMUs mounted on opposite sides of the same joint, located on two adjacent rigid links respectively. First, the relative rotation between them, R_k^a→b, is calculated and converted to an axis-angle representation to obtain the instantaneous rotation axis direction. Then, constraints are added to ensure that the instantaneous rotation axis direction is consistent with the actual rotation axis direction u. j Parallelism allows for the limitation of the direction of rotation.

[0081] Optionally, an incremental nonlinear optimization method is used for real-time iterative solution to obtain the optimal pose estimate at the current moment, including: using an incremental nonlinear optimization method to perform real-time iterative solution of the hybrid factor graph model; in each iteration, adjusting the IMU attitude and position variables at each optimization solution moment within the sliding window according to global constraints and constraint factors to obtain the optimal solution; and using the optimal solution as the optimal pose estimate at the current moment.

[0082] In this embodiment, the incremental nonlinear optimization solution targets the variables to be optimized in the hybrid factor graph model, namely the attitude and position of each IMU at each optimization time point within the sliding window. The aim is to find the optimal solution that satisfies the constraints, thereby pulling the position estimates that have drifted due to IMU noise and zero bias back to the correct geometric relationship, and achieving closed-loop suppression of drift errors.

[0083] Optionally, the incremental nonlinear optimization method performs real-time iterative solution of the mixed factor graph model, including: performing real-time iterative solution of the mixed factor graph model through an incremental solver; after each iteration, the state variable at the earliest optimization time within the sliding window is marginalized, the marginalized information is retained as a prior constraint in the factor graph, and the state variable is removed from the optimization variables; the prior constraint and the remaining historical pose estimates within the sliding window are used as the initial values ​​for the next iteration, and the factor graph model at the current time is iteratively solved.

[0084] In this embodiment, the incremental solver can employ algorithms such as ISAM2. After each solution, the state variable information from the earliest time step is transformed into prior constraints and retained in the factor graph, rather than simply discarding it. For example, a 20-frame sliding window is maintained, storing the optimal pose estimates corresponding to the past 20 optimization solution times. After obtaining the sensor data and constraint factors corresponding to the 21st frame, the latest data and factors are added to the sliding window to form state variables, and the oldest state from the 1st frame is marginalized, its information is transformed into prior constraints and retained in the window, and then the old state is removed. Finally, the incremental solver efficiently solves the factor graph within the window to obtain the optimal pose estimate corresponding to the 21st frame. This approach controls the computational scale, preserves the statistical influence of historical information, and avoids the accuracy loss caused by directly discarding old data.

[0085] Optionally, all IMUs in the multi-joint system share the same synchronization clock; the master control device acquires the sensing data reported by the two IMUs of the joint arm at each moment by: reading the sensing data of each IMU in parallel through direct memory access (DMA) and timestamping the data read in each frame.

[0086] In this embodiment, all IMUs share the same synchronization clock signal to ensure time consistency in data acquisition. The main control chip reads the raw data from each IMU in parallel via DMA (Direct Memory Access), improving data acquisition efficiency.

[0087] Next, combine Figure 3 This application describes another fusion positioning method for a multi-joint system proposed in its embodiments. Specifically, it includes:

[0088] S201 obtains zero bias information and length parameters through a static calibration process.

[0089] The details of how to install and deploy the IMU sensors in the multi-joint system will not be elaborated here. After installation, control the system to operate and keep the multi-joint system completely still for at least 3 minutes. Collect static output data from all IMUs, and calculate the average output of the gyroscope and accelerometer for each IMU, using this as the zero-bias information for the current environment. Before subsequent data processing, subtract this zero-bias from the raw data. The purpose is to eliminate the inherent constant bias of the sensors and significantly reduce the starting error of the integration calculation. The length parameter refers to the straight-line spatial distance between the two IMU mounting points at both ends of each joint arm.

[0090] Furthermore, to facilitate subsequent coordinate transformations and kinematic modeling, two coordinate systems are defined: the IMU measurement center coordinate system, with the measurement center of each IMU as the origin, and the x, y, and z axes aligned with the measurement directions of the IMU; and the link local coordinate system, or joint center coordinate system, with the joint rotation center as the origin, and the axis directions defined according to practical conventions.

[0091] S202, installation error information is obtained through a dynamic calibration process.

[0092] For example, the target joint arm is controlled to be perpendicular to the ground and then swings at a frequency of 0.5Hz with an amplitude of 30° to the left and right, while the other joints remain stationary. During the swinging motion, sensor data from the IMUs at both ends of the joint is simultaneously acquired. Within a sliding window of a preset length of 20 frames, using the length parameter obtained in S201 as a constraint, and setting the three-dimensional offset vector of the IMU and the actual rotation axis direction of the joint as the variables to be optimized, the installation error information is obtained through nonlinear optimization. The correction accuracy can reach 0.5mm, and the axis direction accuracy can reach 0.1°.

[0093] In S202, during the swinging process, data from IMUs at both ends of the joint are collected synchronously. When the joint rotates with angular velocity ω and angular acceleration α, the acceleration a_IMU measured by the IMU and the acceleration a_joint at the center of the joint satisfy the acceleration transfer formula of rigid body kinematics:

[0094] a_IMU=a_joint+α×r+ω×(ω×r);

[0095] By collecting IMU acceleration and angular velocity data during joint swing, a nonlinear optimization problem is constructed, and the three-dimensional offset vector r of the IMU is used as an unknown quantity and solved together with the joint axis direction u.

[0096] S203 collects sensor data in real time and compensates for the sensor data using zero-bias information and installation error information.

[0097] S204 optimizes the acquisition frequency and calculates the IMU pre-integration factor based on the compensated sensor data.

[0098] In S203 and S204 above, the main control device in the system architecture is connected to all IMU sensors simultaneously, and all IMUs share a common synchronization clock signal to ensure the consistency of data acquisition time. The main control chip reads the raw angular velocity and acceleration data of each IMU in parallel via DMA at a first sampling frequency of 100Hz, and timestamps each frame of data and stores it in a buffer.

[0099] Then, the raw angular velocity and acceleration data of each IMU are compensated using zero-bias information and installation error information. Specifically, the gyroscope zero bias is subtracted from the angular velocity measurement, and the accelerometer zero bias is subtracted from the acceleration measurement. At the same time, compensation is made using the IMU offset vector and the actual rotation axis direction from the installation error information, thereby converting the acceleration measurement value from the IMU center to the joint center.

[0100] The compensated sensor data is re-optimized and encapsulated at a second sampling frequency of 10Hz, meaning optimization is performed every 100ms. Ten raw data points are included between adjacent optimization solution times. These ten data points are then numerically integrated using a pre-integration technique, merging them into a single relative motion increment, including relative rotation, velocity change, and position change.

[0101] S205, Based on the rotational degree of freedom properties of the articulated arm, determine the constraint factors used to construct the hybrid factor graph model.

[0102] S206, construct a mixed factor graph model and perform real-time iteration.

[0103] In S206, each optimization iteration requires inputting the pre-integration factors of each IMU (including relative rotation, velocity change, and position change), and the distance constraint factor associated with the linear spatial distance between the two IMU mounting points of the current articulated arm. If it is a multi-degree-of-freedom attribute, it also includes the attitude constraint factor associated with the joint axis direction.

[0104] During the construction process, the first step is to determine the variables to be optimized and adjusted, and maintain a sliding window containing the most recent N optimization moments, where N is between 10 and 20, corresponding to 1 to 2 seconds of historical data. Let these N moments be t1, t2, ..., tt n , where t n This refers to the current time. For each of these N time points, the positions p and poses R of all IMUs at that time are used as variables to be optimized. Where: historical time points (t1 to t2) n-1 The pose of (t): derived from the result of the previous optimization, possessing high confidence, and used as a global prior constraint in this optimization. Current time (t) nThe pose of ) is the key object to be solved in this optimization. The initial value is obtained from the pre-integral increment t. n-1 It can be obtained by recursion.

[0105] During the optimization process, the newly added IMU pre-integration factor and the distance constraint factor based on the length parameter will affect all variables. However, since the historical pose has prior information (high confidence), the adjustment of the historical pose by the new data is small; while the adjustment of the current pose (no prior or low confidence) is large, thus realizing the core of using the high-precision historical pose to constrain the current pose estimation.

[0106] If the system has m IMUs, then at each time step there are m position variables and m attitude variables, for a total of 2m variables. Over N time steps, there are a total of 2m × N variables.

[0107] Furthermore, an IMU pre-integration factor is added to constrain adjacent variables over time. For each IMU i (i=1 to m), its value at time t is taken out. k and t k+1 The position and attitude variables at each time point (k=1 to N-1). Then, the calculated IMU values ​​for the time period [t] are retrieved. k , t k+1 The pre-integral increment within ] . Add a factor node to the factor graph and connect this factor node to IMU i in t k and t k+1 On the variable at time t. This factor node represents a constraint: from t k to t k+1 At any given time, the pose change of IMU i must equal the pre-integration increment. If the changes in these two variables during optimization are inconsistent with the pre-integration result, this factor will generate a corrective force, adjusting the variables in the correct direction. This same operation is then performed on every pair of adjacent time points for each IMU. Therefore, the number of added IMU pre-integration factors is m × (N-1).

[0108] The purpose of adding a distance constraint factor is to constrain the positions of two IMUs at the same time using a length parameter. For each joint j (j=1 to J, where J is the total number of joints), the positions of its two IMUs at the same time are constrained using the length parameter L measured by S201. j Connect them.

[0109] The specific operation is as follows: For joint j, let the IMUs installed at its two ends be numbered a and b. For each time t... k (k=1 to N), find the position variable p_k^a of IMU a and the position variable p_k^b of IMU b, and extract their corresponding length parameter L. j .

[0110] The system adds a factor node to the factor graph, connecting this node to both location variables. This factor node represents a constraint: the distance between IMU a and IMU b must be equal to L. j The mathematical expression for this constraint is: ||p_k^b - p_k^a|| = L j During the optimization process, the distance between the two current position variables is calculated, and then L is subtracted. j A difference is obtained. If the difference is greater than 0 (indicating the distance is too far), this factor will generate a force pulling the two positions towards the center; if the difference is less than 0 (indicating the distance is too close), this factor will generate a force pushing the two positions outwards. During the optimization process, this factor will continue to act until the distance between the two positions equals L. j The same operation is performed on every joint at every time step. Therefore, the number of distance constraint factors added is J×N.

[0111] The attitude constraint factor is illustrated using a single-degree-of-freedom hinge joint. For hinge joint j, let the IMUs mounted at its two ends be numbered a and b. For each time t... k (k=1 to N), perform the following operations:

[0112] Obtain the attitude variables R_k^a and R_k^b of IMU a and IMU b, respectively, as well as the actual rotation axis direction u of the joint. j .

[0113] Calculate the relative rotation between two IMUs: R_k^{a→b}=(R_k^a) T ·R_k^b converts the relative rotation R_k^{a→b} into an axis-angle representation, obtaining the instantaneous rotation axis direction u_actual. Then, a factor node is added to the factor diagram, connected to both the attitude variables R_k^a and R_k^b. This factor node represents a constraint: the relative rotation axis between IMU a and IMU b must be aligned with the actual rotation axis u. j Parallel. During optimization, if the instantaneous rotation axis direction u_actual is parallel to the actual rotation axis u... j If the axes are not parallel (there are rotational components about other axes), and the residual is greater than 0, this factor will generate a correction force to adjust the relative rotation back to only about u. j The state of rotation.

[0114] The mathematical expression for this constraint is: relative rotation R_k^a→b=(R_k^a) T R_k^b, its axis of rotation must be parallel to u jParallel. During optimization, if the relative rotation introduces rotational components about other axes, this factor will generate a corrective force to adjust it back to only rotate around u. j The rotational state. The same operation is performed on every hinge joint at every moment. Therefore, the number of attitude constraint factors added is J_hinge × N, where J_hinge is the number of hinge joints.

[0115] Finally, the solution process follows. After adding all the factors mentioned above, the factor graph includes IMU pre-integration factors, distance constraint factors, and optional attitude constraint factors. These factors together constitute an optimization problem: finding a set of state variables (i.e., the position and attitude of each IMU at each time step) such that the constraints represented by all factors are satisfied as simultaneously as possible.

[0116] This optimization problem is solved by calling the iSAM2 solver from the GTSAM library. The solver iteratively calculates and adjusts the values ​​of various variables until it finds a set of optimal solutions. These optimal solutions represent the optimal pose estimates of each IMU at all times within the sliding window. Therefore, the IMU pre-integration factor provides smoothness and dynamic consistency over time. The distance constraint factor, on the other hand, is like the value at each time t... k A virtual rigid link with a length parameter L is "nailed" to the positions of the two IMUs. During the optimization process, this factor continuously generates gradients, strongly pulling the two position estimates, which have drifted apart due to IMU noise and zero bias, back to the correct geometric relationship with a distance of L. This directly and in real-time solves the fundamental problem of error accumulation and drift in related technologies, achieving closed-loop suppression at the level of physical laws.

[0117] Finally, regarding the optimization and marginalization of the sliding window, the 20-frame sliding window stores the optimal pose estimates corresponding to the past 20 optimization solution times. After obtaining the sensor data and constraint factors corresponding to the 21st frame, the latest data and factors are added to the sliding window to form state variables, and the oldest state from the 1st frame is marginalized, its information is converted into prior constraints and retained in the window, and then the old state is removed. Finally, an incremental solver is used to efficiently solve the factor graph within the window to obtain the optimal pose estimate corresponding to the 21st frame. This approach controls the computational scale, preserves the statistical influence of historical information, and avoids the accuracy loss caused by directly discarding old data.

[0118] Based on the fusion positioning method for the multi-joint system provided in the embodiments of this application, an error comparison experiment was also conducted, and the experimental results can be referred to. Figure 4 Compared to traditional IMU integration methods, the method provided in this application embodiment has smaller accumulated errors over time without the aid of any external devices, and further solves the problem of poor applicability in related technical scenarios.

[0119] Furthermore, as Figures 1 to 4 In a specific implementation of the method shown, this embodiment provides a fusion positioning device for a multi-joint system. The multi-joint system includes at least two articulated arms connected in sequence. Each articulated arm includes a rigid link and IMUs located at both ends of the rigid link. All IMUs are electrically connected to a main control device. Figure 5 As shown, the fusion positioning device includes:

[0120] The acquisition unit 501 is configured to acquire the sensor data reported by the two IMUs of each joint arm at each time step.

[0121] The compensation unit 502 is configured to compensate the sensing data based on the zero bias information and installation error information of each joint arm determined during the system initialization phase, so as to obtain the compensated sensing data at each moment.

[0122] The pre-integration unit 503 is configured to calculate the IMU pre-integration factor for characterizing the relative motion increment at adjacent time points using a pre-integration method based on the compensated sensing data.

[0123] The processing unit 504 is configured to construct a hybrid factor graph model including multiple constraint factors based on the rotational degree of freedom attributes, length parameters, IMU pre-integration factors and global constraints of each joint arm, and to iteratively solve the hybrid factor graph model using an incremental nonlinear optimization method to obtain the optimal pose estimate at the current moment.

[0124] The length parameter is determined based on the spatial straight-line distance between the IMUs located at both ends of the rigid link, and the global constraint is the historical best pose estimate used in the process of iteratively solving the current best pose estimate.

[0125] In specific application scenarios, the acquisition unit 501 is further configured to acquire the zero-bias information of each IMU in the multi-joint system and the length parameters of each joint arm; the zero-bias information includes gyroscope zero-bias and accelerometer zero-bias, and the length parameters are determined based on the IMUs located at both ends of the rigid link in each joint arm; and to acquire installation error information; the installation error information is used to characterize the error introduced by the IMUs at both ends of the rigid link of the joint arm during the installation process.

[0126] In specific application scenarios, the acquisition unit 501 is further configured to, for multi-joint systems, set the joint arm to be calibrated to an active state and the uncalibrated joint arm to a locked state, control the joint arm to be calibrated to move at a specified frequency and amplitude, and acquire data reported by the two IMUs of the current joint arm; using the length parameter as a constraint, the offset vector of the IMU from the measurement center to the joint center and the actual rotation axis direction are used as state variables to be estimated, and a constraint relationship between the acceleration at the IMU measurement center and the acceleration at the joint center is established according to the acceleration transfer formula a_IMU=a_joint+α×r+ω×(ω×r); where a_IMU is the acceleration measured by the IMU, a_joint is the acceleration at the joint center, ω and α are the angular velocity and angular acceleration data reported by the IMU, and r is the offset vector to be estimated; the constraint relationship is solved by nonlinear optimization to obtain the offset vector and actual rotation axis direction of the IMUs at both ends of each joint arm, which are used as the installation error information.

[0127] In a specific application scenario, the compensation unit 502 is further configured to, for the angular velocity measurement value and the acceleration measurement value in the sensing data, subtract the gyroscope zero bias from the angular velocity measurement value to obtain a first angular velocity value, and subtract the accelerometer zero bias from the acceleration measurement value to obtain a first acceleration value; based on the installation error information, using the constraint relationship between the acceleration at the IMU measurement center and the acceleration at the joint center, convert the first acceleration value from the IMU measurement center to the joint center to obtain a second acceleration value; and use the first angular velocity value and the second acceleration value as the compensated sensing data.

[0128] In specific application scenarios, the pre-integration unit 503 is further configured to: acquire sensing data from the IMU sensor in real time at a first sampling frequency; optimize the sensing data at a pre-set second sampling frequency, wherein there are multiple sensing data acquired at the first sampling frequency between two adjacent optimization solutions; wherein the first sampling frequency is an integer multiple of the second sampling frequency; perform pre-integration processing on the sensing data between two adjacent optimization solutions, and merge the pre-integrated data into a relative motion increment to generate an IMU pre-integration factor connecting the two adjacent optimization solutions.

[0129] In specific application scenarios, the processing unit 504 is further configured to: when the current joint arm rotational degree of freedom attribute is multi-degree-of-freedom, determine the constraint factors used to construct the hybrid factor graph model, including the IMU pre-integration factor and the distance constraint factor; wherein the distance constraint factor is the length parameter; when the current joint arm rotational degree of freedom attribute is single-degree-of-freedom, determine the constraint factors used to construct the hybrid factor graph model, including the IMU pre-integration factor, the distance constraint factor, and the attitude constraint factor associated with the joint axis direction; and maintain a sliding window of a preset length, wherein the sliding window stores the historical pose estimation values ​​of the current optimization solution that are not at the current time; each time an optimization solution is executed, with the IMU attitude and position as the variables to be optimized, add the constraint factors determined at the current time for constructing the hybrid factor graph model to the factor graph, and update the factor graph with the historical pose estimation values ​​stored in the sliding window as global constraints.

[0130] In specific application scenarios, the pre-integration unit 503 is further configured to calculate the current optimized solution t. k+1 Time and previous optimization solution t k The pre-integration increment at time step is used to obtain the IMU pre-integration factor; the IMU pre-integration factor includes: relative rotation ΔR{k,k+1} i The change in velocity Δv{k,k+1} i and positional change Δp{k,k+1} i Where i is the corresponding IMU number, i = 1, 2, ..., m, the number of IMU pre-integration factors is m × (N-1), and N is the preset length for maintaining the sliding window; the distance constraint factor is obtained by the following formula: ||p_k^b - p_k^a|| = L j Where p_k^a and p_k^b are the position variables of IMU a and IMU b, respectively, and the number of distance constraint factors is J × N, where J is the number of joints in the multi-joint system; the attitude constraint factors are obtained by the following formula: calculate the relative rotation of IMU a and IMU b R_k^a→b=(R_k^a) T R_k^b is converted to axis-angle representation to obtain the instantaneous rotation axis direction u_actual; the instantaneous rotation axis is defined relative to the actual rotation axis u in the installation error information. j Parallel, where the number of attitude constraint factors is J_hinge×N, where J_hinge is the number of hinge joints.

[0131] In specific application scenarios, the processing unit 504 is further configured to perform real-time iterative solution of the hybrid factor graph model using an incremental nonlinear optimization method; in each iteration, the IMU attitude and position variables at each optimization solution time within the sliding window are adjusted according to the global constraints and the constraint factors to obtain the optimal solution; and the optimal solution is used as the optimal pose estimate at the current time.

[0132] In specific application scenarios, the processing unit 504 is further configured to perform real-time iterative solution of the hybrid factor graph model using an incremental solver; after each iteration, the state variable at the earliest optimization time within the sliding window is marginalized, the marginalized information is retained as a prior constraint in the factor graph, and the state variable is removed from the optimization variables; the prior constraint and the remaining historical pose estimates within the sliding window are used as the initial values ​​for the next iteration, and the factor graph model at the current time is iteratively solved.

[0133] In specific application scenarios, the acquisition unit 501 is further configured to read the sensing data of each IMU in parallel through direct memory access (DMA) and to timestamp the data read in each frame.

[0134] It should be noted that other corresponding descriptions of the functional units involved in the fusion positioning device for a multi-joint system provided in this embodiment can be found in [reference]. Figures 1 to 4 The corresponding description in [the document] will not be repeated here.

[0135] Based on the above, Figures 1 to 4 Accordingly, this embodiment also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the above-described method. Figures 1 to 4 The method shown.

[0136] Based on this understanding, the technical solution of this application can be embodied in the form of a software product, which can be stored in a non-volatile storage medium (such as CD-ROM, USB flash drive, mobile hard drive, etc.) and includes several instructions to cause a computer device (such as personal computer, server, or network device, etc.) to execute the methods of various implementation scenarios of this application.

[0137] Based on the above, Figures 1 to 4 The method shown, and Figure 5 To achieve the above objectives, the present application also provides a fusion positioning system for a multi-joint system, which can be configured on a computer side, etc. The system includes a storage medium and a processor; the storage medium stores a computer program; the processor executes the computer program to achieve the above-described objectives. Figures 1 to 4The method shown.

[0138] The storage medium may also include an operating system and a network communication module. The operating system is a program that manages the hardware and software resources of the aforementioned physical device, supporting the operation of information processing programs and other software and / or programs. The network communication module is used to enable communication between the various components within the storage medium, as well as communication with other hardware and software in the information processing physical device.

[0139] Through the above description of the embodiments, those skilled in the art can clearly understand that this application can be implemented by means of software plus necessary general-purpose hardware platforms, or it can be implemented by hardware. By applying the solution of this embodiment, compared with related technologies, by deploying IMUs at both ends of each rigid link and introducing the length parameter between the IMUs at both ends of the rigid link as one of the constraint factors for constructing the hybrid factor graph model, the fixed geometric length of the joint arm itself is transformed into a real-time virtual observation constraint. Thus, without any external reference equipment, the cumulative divergence of IMU integral drift over time is effectively suppressed, significantly reducing system cost and structural complexity, avoiding the problem in related technologies where drift error accumulates to the decimeter level within minutes, and improving environmental adaptability.

[0140] In the description of this application, it should be understood that the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Therefore, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of this application, "multiple" means two or more, unless otherwise explicitly specified.

[0141] In the description of this specification, the terms "one embodiment," "some embodiments," "embodiment," "example," "specific example," or "some examples," etc., refer to specific features, structures, materials, or characteristics described in connection with that embodiment or example, which are included in at least one embodiment or example of this application. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0142] Although embodiments of this application have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting this application. Those skilled in the art can make modifications, alterations, substitutions and variations to the above embodiments within the scope of this application.

Claims

1. A fusion positioning method for a multi-joint system, characterized in that, The multi-joint system includes at least two articulated arms connected in sequence, each articulated arm including: a rigid link and IMUs located at both ends of the rigid link, all IMUs being electrically connected to a main control device; the method includes: For each joint arm, acquire the sensor data reported by the two IMUs of the joint arm at each time step; Based on the zero-bias information and installation error information of each joint arm determined during the system initialization phase, the sensing data is compensated to obtain the compensated sensing data at each moment. Based on the compensated sensor data, the IMU pre-integration factor used to characterize the relative motion increment at adjacent time points is calculated using a pre-integration method. Based on the rotational degree of freedom, length parameter, IMU pre-integration factor and global constraints of each joint arm, a hybrid factor graph model including multiple constraint factors is constructed, and the hybrid factor graph model is iteratively solved using an incremental nonlinear optimization method to obtain the optimal pose estimate at the current time. The length parameter is determined based on the spatial straight-line distance between the IMUs located at both ends of the rigid link, and the global constraint is the historical best pose estimate used in the process of iteratively solving the current best pose estimate. The calculation of the IMU pre-integration factor for representing the relative motion increment at adjacent time points based on the compensated sensing data, using a pre-integration method, includes: real-time acquisition of sensing data from the IMU sensor at a first sampling frequency; optimization of the sensing data at a pre-set second sampling frequency, wherein multiple sensing data acquired at the first sampling frequency are included between two adjacent optimization solutions; wherein the first sampling frequency is an integer multiple of the second sampling frequency; pre-integration processing of the sensing data between two adjacent optimization solutions, and merging the pre-integrated data into a single relative motion increment to generate the IMU pre-integration factor connecting the two adjacent optimization solutions; Based on the rotational degree of freedom attributes, length parameters, IMU pre-integration factors, and global constraints of each joint arm, a hybrid factor graph model including multiple constraint factors is constructed. This includes: when the current joint arm's rotational degree of freedom attribute is multi-degree-of-freedom, determining the constraint factors used to construct the hybrid factor graph model, including the IMU pre-integration factor and distance constraint factors; wherein the distance constraint factor is the length parameter; when the current joint arm's rotational degree of freedom attribute is single-degree-of-freedom, determining the constraint factors used to construct the hybrid factor graph model, including the IMU pre-integration factor, the distance constraint factor, and an attitude constraint factor associated with the joint axis direction; and maintaining a sliding window of a preset length, the sliding window storing historical pose estimates not present in the current optimization solution; each time an optimization solution is executed, using the IMU attitude and position as variables to be optimized, adding the constraint factors determined at the current moment for constructing the hybrid factor graph model to the factor graph, and updating the factor graph using the historical pose estimates stored in the sliding window as global constraints.

2. The method according to claim 1, characterized in that, The system initialization phase includes: The multi-joint system is statically calibrated to obtain the zero-bias information of each IMU in the multi-joint system and the length parameters of each joint arm; the zero-bias information includes gyroscope zero-bias and accelerometer zero-bias. For each articulated arm, a dynamic calibration method is used to dynamically calibrate each articulated arm and obtain installation error information; the installation error information is used to characterize the error introduced by the IMUs at both ends of the rigid connecting rod of the articulated arm during the installation process.

3. The method according to claim 2, characterized in that, For each articulated arm, a dynamic calibration method is used to dynamically calibrate each articulated arm and obtain installation error information, including: For multi-joint systems, the joint arm that needs to be calibrated is set to an active state, while the non-calibrated joint arm is locked. The joint arm that needs to be calibrated is controlled to move at a specified frequency and amplitude, and the data reported by the two IMUs of the current joint arm is acquired. Using the length parameter as a constraint, the offset vector of the IMU from the measurement center to the joint center and the actual rotation axis direction are taken as the state variables to be estimated. The constraint relationship between the acceleration at the IMU measurement center and the acceleration at the joint center is established according to the acceleration transfer formula a_IMU=a_joint+α×r+ω×(ω×r); where a_IMU is the acceleration measured by the IMU, a_joint is the acceleration at the joint center, ω and α are the angular velocity and angular acceleration data reported by the IMU, and r is the offset vector to be estimated. The constraint relationship is solved by nonlinear optimization to obtain the offset vector of the IMU at both ends of each joint arm and the actual rotation axis direction, which are used as the installation error information.

4. The method according to claim 3, characterized in that, Based on the zero-bias information and installation error information of the current articulated arm determined during the system initialization phase, the sensing data is compensated to obtain the compensated sensing data at each moment, including: For the angular velocity measurement value and acceleration measurement value in the sensing data, the first angular velocity value is obtained by subtracting the zero bias of the gyroscope from the angular velocity measurement value, and the first acceleration value is obtained by subtracting the zero bias of the accelerometer from the acceleration measurement value. Based on the installation error information, the first acceleration value is converted from the IMU measurement center to the joint center using the constraint relationship between the IMU measurement center acceleration and the joint center acceleration to obtain the second acceleration value; The first angular velocity value and the second acceleration value are used as the compensated sensing data.

5. The method according to claim 1, characterized in that, The steps for obtaining the IMU pre-integration factor during the current optimization solution include: Calculate the current optimal solution t k+1 Time and previous optimization solution t k The pre-integration increment at time step is used to obtain the IMU pre-integration factor; The IMU pre-integration factor includes: relative rotation ΔR{k,k+1} i The change in velocity Δv{k,k+1} i and positional change Δp{k,k+1} i Where i is the corresponding number of the IMU, i = 1, 2, ..., m, the number of pre-integration factors of the IMU is m × (N-1), and N is the preset length for maintaining the sliding window; The distance constraint factor is obtained by the following formula: ||p_k^b-p_k^a||=L j Where p_k^a and p_k^b are the position variables of IMUa and IMU b, respectively, and the number of distance constraint factors is J × N, where J is the number of joints in the multi-joint system; The attitude constraint factor is obtained by the following formula: Calculate the relative rotation of IMU a and IMU b: R_k^a→b=(R_k^a) T R_k^b is converted to axis-angle representation to obtain the instantaneous rotation axis direction u_actual; The instantaneous rotation axis is defined as the actual rotation axis u in the installation error information. j Parallel, where the number of attitude constraint factors is J_hinge×N, where J_hinge is the number of hinge joints.

6. The method according to claim 1, characterized in that, An incremental nonlinear optimization method is used for real-time iterative solution to obtain the optimal pose estimate at the current time, including: The hybrid factor graph model is solved iteratively in real time using an incremental nonlinear optimization method. In each iteration, the IMU attitude and position variables at each optimization time point within the sliding window are adjusted according to the global constraints and the constraint factors to obtain the optimal solution; The optimal solution is used as the optimal pose estimate at the current moment.

7. The method according to claim 6, characterized in that, The incremental nonlinear optimization method performs real-time iterative solution to the mixed factor graphical model, including: The mixed factor graph model is solved iteratively in real time using an incremental solver; After each iteration is completed, the state variable at the earliest optimization time within the sliding window is marginalized, the marginalized information is retained as a prior constraint in the factor graph, and the state variable is removed from the optimization variables. Using the prior constraints and the remaining historical pose estimates within the sliding window as the initial values ​​for the next iteration, the factor graph model at the current time is iteratively solved.

8. The method according to claim 1, characterized in that, All IMUs in the multi-joint system share the same synchronization clock; The main control device acquires the sensor data reported by the two IMUs of the articulated arm at each moment, including: The sensor data of each IMU is read in parallel using direct memory access (DMA) and each frame of data is timestamped.

9. A fusion positioning device for a multi-joint system, characterized in that, The multi-joint system includes at least two articulated arms connected in sequence, each articulated arm including: a rigid link and IMUs located at both ends of the rigid link, all IMUs being electrically connected to the main control device; the fusion positioning device includes: The acquisition unit is configured to acquire the sensor data reported by the two IMUs of each joint arm at each time step. The compensation unit is configured to compensate the sensing data based on the zero bias information and installation error information of each joint arm determined during the system initialization phase, so as to obtain the compensated sensing data at each moment. The pre-integration unit is configured to calculate the IMU pre-integration factor for characterizing the relative motion increment at adjacent time points using a pre-integration method based on the compensated sensing data. The processing unit is configured to construct a hybrid factor graph model including multiple constraint factors based on the rotational degree of freedom attributes, length parameters, IMU pre-integration factors and global constraints of each joint arm, and to iteratively solve the hybrid factor graph model using an incremental nonlinear optimization method to obtain the optimal pose estimate at the current moment. The length parameter is determined based on the spatial straight-line distance between the IMUs located at both ends of the rigid link, and the global constraint is the historical best pose estimate used in the process of iteratively solving the current best pose estimate. The calculation of the IMU pre-integration factor for representing the relative motion increment at adjacent time points based on the compensated sensing data, using a pre-integration method, includes: real-time acquisition of sensing data from the IMU sensor at a first sampling frequency; optimization of the sensing data at a pre-set second sampling frequency, wherein multiple sensing data acquired at the first sampling frequency are included between two adjacent optimization solutions; wherein the first sampling frequency is an integer multiple of the second sampling frequency; pre-integration processing of the sensing data between two adjacent optimization solutions, and merging the pre-integrated data into a single relative motion increment to generate the IMU pre-integration factor connecting the two adjacent optimization solutions; Based on the rotational degree of freedom attributes, length parameters, IMU pre-integration factors, and global constraints of each joint arm, a hybrid factor graph model including multiple constraint factors is constructed. This includes: when the current joint arm's rotational degree of freedom attribute is multi-degree-of-freedom, determining the constraint factors used to construct the hybrid factor graph model, including the IMU pre-integration factor and distance constraint factors; wherein the distance constraint factor is the length parameter; when the current joint arm's rotational degree of freedom attribute is single-degree-of-freedom, determining the constraint factors used to construct the hybrid factor graph model, including the IMU pre-integration factor, the distance constraint factor, and an attitude constraint factor associated with the joint axis direction; and maintaining a sliding window of a preset length, the sliding window storing historical pose estimates not present in the current optimization solution; each time an optimization solution is executed, using the IMU attitude and position as variables to be optimized, adding the constraint factors determined at the current moment for constructing the hybrid factor graph model to the factor graph, and updating the factor graph using the historical pose estimates stored in the sliding window as global constraints.

10. A fusion positioning system for a multi-joint system, comprising a storage medium, a processor, and a computer program stored on the storage medium and executable on the processor, characterized in that, When the processor executes the computer program, it implements the method of any one of claims 1 to 8.

11. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the method of any one of claims 1 to 8.

Images