Method, system and apparatus for powertrain and foundation bearing component co-pack control

Abstract

The application provides a method, system and device for power assembly and foundation bearing part combined control, applied to the technical field of robot assembly, comprising: obtaining a measured pose difference of a robot; obtaining measured data of a UWB module to determine a relative measured distance between any two robots; determining a measured pose of a power assembly according to a three-dimensional model; determining a relative measured pose between any two robots according to the external parameter of a camera and the measured pose of the power assembly; and determining a relative estimated pose of any two robots according to the measured pose difference, the relative measured pose and the relative measured distance, to control the robots. The relative estimated pose of the robots can be more accurate in the scene where the combined surface is blocked by combining the relative measured pose and the relative measured distance, and the measured pose difference, so as to reduce the risk of assembly collision and part damage caused by position deviation and save production cost.

Description

Technical Field

[0001] This application relates to the field of robot assembly technology, and in particular to methods, systems and devices for the assembly and control of powertrain and basic load-bearing components. Background Technology

[0002] Currently, in the final assembly of automobiles, ships, and other products, the assembly of the powertrain (such as an engine or electric drive system) with the basic load-bearing components (such as the body or hull) is a critical process. During assembly, the powertrain needs to be precisely installed onto the basic load-bearing components (such as the chassis), and any positional deviation directly affects the product's performance and safety.

[0003] In existing technologies, industrial cameras are installed around the assembly station to visually identify the positions of the powertrain and basic load-bearing components, guiding the robot to perform assembly.

[0004] However, because the robot's arm and gripping fixture can easily obstruct the camera's view, especially in the assembly interface area (such as the part where the powertrain and the base support components come into contact, dock, and are finally fixed during installation), it is difficult to obtain real-time visual feedback of the assembly process. The robot can only move along a predetermined trajectory, which can easily cause positional deviations, resulting in assembly collisions. In severe cases, it can also cause damage to the powertrain or base support components, increasing production costs. Summary of the Invention

[0005] In view of the above problems, methods, systems, and apparatuses are proposed to provide powertrain and foundation load-bearing component assembly control to overcome or at least partially solve the above problems, including: In a first aspect, this application provides a method for controlling the assembly of a powertrain and its underlying load-bearing components. The assembly station is equipped with multiple robots, each with a camera and a UWB module at its end effector. The method includes: Obtain the measured pose difference of the robot; Acquire the measurement data from the UWB module, and determine the relative measurement distance between any two robots based on the measurement data from the UWB module; Based on the image data acquired by the camera and the camera's intrinsic and extrinsic parameters, a three-dimensional model of the powertrain is constructed, and the measurement pose of the powertrain is determined based on the three-dimensional model. Based on the extrinsic parameters of the camera and the measured pose of the powertrain, the measured pose of the robot relative to the powertrain is determined, and based on the measured pose of the robot relative to the powertrain, the relative measured pose between any two robots is determined. Based on the measured pose difference between any two robots and the relative measured pose and relative measured distance between any two robots, the relative estimated pose of any two robots is determined, and the robots are assembled and controlled based on the relative estimated pose of any two robots.

[0006] Optionally, the relative estimated pose of any two robots is determined based on the measured pose difference between any two robots and the relative measured pose and relative measured distance between any two robots, including: Construct a factor graph; wherein the factor graph includes multiple factors and multiple pose variables, the multiple factors including: an odometry factor with the measured pose difference as the measured value, a UWB factor with the relative measured distance as the measured value, and a visual factor with the relative measured pose as the measured value; the odometry factor connects the pose variables of the same robot at adjacent time points, and the UWB factor and the visual factor respectively connect the pose variables of different robots at the same time point; Based on the residuals between the measured values ​​and the corresponding estimated values ​​of each factor, and the pre-set noise standard deviation, the robot's pose variables in the factor graph are iteratively optimized. From the optimized pose variables, determine the relative estimated pose of any two robots.

[0007] Optionally, based on the residuals between the measured values ​​and corresponding estimated values ​​of each factor, and a pre-set noise standard deviation, the robot's pose variables in the factor graph are iteratively optimized, including: Based on the residual between the measured value and the corresponding estimated value of the odometer factor, and the noise standard deviation of the odometer factor, a cost term for the odometer factor is constructed. Based on the residual between the measured value and the corresponding estimated value of the UWB factor, and the noise standard deviation of the UWB factor, a cost term for the UWB factor is constructed. Based on the residual between the measured value and the corresponding estimated value of the visual factor, and the noise standard deviation of the visual factor, a cost term for the visual factor is constructed. With the goal of minimizing the sum of the cost terms of the odometry factor, UWB factor, and vision factor, the robot's pose variables in the factor graph are iteratively optimized.

[0008] Optionally, based on the image data acquired by the camera and the camera's intrinsic and extrinsic parameters, a three-dimensional model of the powertrain is constructed, and based on the three-dimensional model, the measurement pose of the powertrain is determined, including: The image data acquired by the camera is semantically segmented to generate a semantic mask; The Gaussian sphere set is rendered based on the camera's intrinsic and extrinsic parameters, and the parameters of the Gaussian sphere set are iteratively optimized and updated based on the semantic mask to construct a three-dimensional model of the powertrain. Specifically, when the image data is the first frame of image data, the Gaussian sphere set is generated based on a preset CAD powertrain model point cloud; when the image data is the Nth frame of image data, the Gaussian sphere set is an optimized Gaussian sphere set using the (N-1)th frame of image data, where N is a positive integer greater than 1. The point cloud of the powertrain is selected from the three-dimensional model, and the selected point cloud is registered with the preset CAD powertrain model point cloud to obtain the measurement pose of the powertrain.

[0009] Optionally, the Gaussian sphere set is rendered based on the intrinsic and extrinsic parameters of the camera, and the parameters of the Gaussian sphere set are iteratively optimized and updated based on the semantic mask to construct a three-dimensional model of the powertrain, including: Based on the intrinsic and extrinsic parameters of the camera, the Gaussian sphere set is rendered to obtain a rendered image; The loss function value is determined based on the sum of squared pixel differences between the rendered image and the image data acquired by the camera, and the weights corresponding to the semantic masks to which each pixel belongs; wherein, the semantic mask is used to distinguish the powertrain region, the basic load-bearing component region, and the background region in the image data, and the weights of pixels in the powertrain region and the basic load-bearing component region are higher than the weights of pixels in the background region. With the goal of minimizing the loss function value, the gradient descent method is used to iteratively optimize the parameters of the Gaussian sphere set in order to construct a three-dimensional model of the powertrain.

[0010] Optionally, it also includes: Construct a feature hash table; wherein the feature hash table includes multiple layers of grids, each layer of grids has an independent feature hash table, and the feature hash table of each layer of grids is used to store spatial point feature vectors of a preset dimension; During the rendering of the Gaussian sphere set, the feature hash table is queried according to the coordinates of the spatial points, and the feature vectors of the spatial points obtained from the feature hash table of each layer of the grid are concatenated to obtain the total feature vector of the spatial points, so as to perform rendering based on the total feature vector and the parameters of the Gaussian sphere set; During the iterative optimization and updating of the parameters of the Gaussian sphere set, the feature hash table is iteratively optimized and updated.

[0011] Optionally, the camera's intrinsic parameters are obtained through offline calibration, and the camera's extrinsic parameters are obtained through online calibration; before obtaining the robot's measured pose difference, the method further includes: The joint angle data of the robot is obtained, the pose of the robot end effector is determined based on the joint angle data, and the theoretical extrinsic parameters of the camera are determined by combining the pose of the robot end effector and the fixed installation relationship between the camera and the robot end effector. The actual external parameters of the camera are determined based on the world coordinates of the preset markers within the assembly station and the image coordinates of the markers in the image data acquired by the camera. By combining the theoretical extrinsic parameters and the actual extrinsic parameters, the calibrated extrinsic parameters are obtained.

[0012] Optionally, based on the measurement data from the UWB module, the relative measurement distance between any two robots is determined, including: The robot's measurement position is determined based on the multiple measurement distances between the UWB module and multiple UWB base stations fixedly installed in the assembly station. Determine the relative measurement distance between any two robots based on their measured positions.

[0013] Secondly, this application provides a system for controlling the assembly of a powertrain and its underlying load-bearing components, the system comprising: Multiple robots are set up at the assembly station, and each robot is equipped with a camera and a UWB module at its end effector. Multiple UWB base stations are installed at the assembly station; The synchronization controller, which connects to the hardware trigger lines of all cameras, is used to control the synchronized exposure of all cameras. An industrial control computer is used to execute the methods described above.

[0014] Thirdly, this application provides a device for controlling the assembly of a powertrain and its underlying load-bearing components. The assembly station is equipped with multiple robots, each with a camera and a UWB module at its end effector. The device includes: A pose difference acquisition module is used to acquire the pose difference of the robot. The relative measurement distance determination module is used to acquire the measurement data of the UWB module and determine the relative measurement distance between any two robots based on the measurement data of the UWB module. The measurement pose determination module is used to construct a three-dimensional model of the powertrain based on the image data acquired by the camera and the camera's intrinsic and extrinsic parameters, and to determine the measurement pose of the powertrain based on the three-dimensional model. The relative measurement pose determination module is used to determine the measurement pose of the robot relative to the powertrain based on the extrinsic parameters of the camera and the measurement pose of the powertrain, and to determine the relative measurement pose between any two robots based on the measurement pose of the robot relative to the powertrain. The assembly control module is used to determine the relative estimated pose of any two robots based on the measured pose difference between any two robots and the relative measured pose and relative measured distance between any two robots, and to perform assembly control on the robots based on the relative estimated pose of any two robots.

[0015] The embodiments of this application have the following advantages: In this embodiment, the following methods are employed: 1) Obtain the robot's measured pose difference; 2) Obtain measurement data from the UWB module and determine the relative measurement distance between any two robots based on the UWB module's measurement data; 3) Construct a three-dimensional model of the powertrain based on image data acquired by the camera and the camera's intrinsic and extrinsic parameters, and determine the powertrain's measured pose based on the three-dimensional model; 4) Determine the robot's measured pose relative to the powertrain based on the camera's extrinsic parameters and the powertrain's measured pose, and determine the relative measured pose between any two robots based on the robot's measured pose relative to the powertrain; 5) Determine the relative estimated pose between any two robots based on the measured pose difference, the relative measured pose, and the relative measurement distance between any two robots; 6) Perform assembly control on the robots based on the relative estimated pose. This achieves a more accurate relative estimated pose of the robots even when the assembly joint surface is obscured by combining the relative measured pose, relative measurement distance, and measured pose difference for fusion estimation, reducing the risk of assembly collisions and part damage caused by positional deviations, and saving production costs. Attached Figure Description

[0016] To more clearly illustrate the technical solution of this application, the drawings used in the description of this application will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0017] Figure 1 This is a schematic diagram of the architecture of an optional powertrain and basic load-bearing component integrated control system provided in some embodiments of this application; Figure 2 This is a schematic flowchart of one of the optional powertrain and basic load-bearing component assembly control methods provided in some embodiments of this application; Figure 3This is a flowchart illustrating an optional method for calibrating the extrinsic parameters of a robot camera, provided in some embodiments of this application. Figure 4 This is a flowchart illustrating an optional measurement pose method based on semantic segmentation and Gaussian sphere optimization provided in some embodiments of this application. Figure 5 This is an optional factor graph provided in some embodiments of this application; Figure 6 This is a flowchart illustrating a second optional method for controlling the assembly of a powertrain and a base load-bearing component, provided in some embodiments of this application. Figure 7 This is a flowchart illustrating a third optional method for controlling the assembly of a powertrain and a foundation load-bearing component, provided in some embodiments of this application. Figure 8 This is a schematic diagram of the structure of an optional powertrain and basic load-bearing component assembly and control device provided in some embodiments of this application. Detailed Implementation

[0018] To make the above-mentioned objectives, features, and advantages of this application more apparent and understandable, the application will be further described in detail below with reference to the accompanying drawings and specific embodiments. Obviously, the described embodiments are only some, not all, of the embodiments of this application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.

[0019] Currently, in the final assembly of automobiles, ships, and other products, the assembly of the powertrain (such as an engine or electric drive system) with the basic load-bearing components (such as the body or hull) is a critical process. During assembly, the powertrain needs to be precisely installed onto the basic load-bearing components (such as the chassis), and any positional deviation directly affects the product's performance and safety.

[0020] In related technologies, the assembly solutions mainly include the following two types: Option one is mechanical tooling guidance. This involves designing dedicated positioning fixtures for each product model (e.g., vehicle type). During assembly, the powertrain is first fixed to the fixture, and then pushed into the base support components by a robot or manually. However, with this option, when the product model changes, the fixtures need to be redesigned and manufactured, resulting in time-consuming on-site changeovers. As manufacturing moves towards multi-model mixed-line production, this mechanical solution struggles to meet the demands of rapid production line changeovers.

[0021] Option two is fixed vision guidance. This involves installing several fixed industrial cameras around the assembly station to visually identify the positions of the powertrain and the base support components, guiding the robot during assembly. However, in actual production, the robot's arm and gripping fixtures often obstruct the camera's view, especially the mating surface between the powertrain and the base support components, which is frequently completely blocked. Because the cameras cannot see the mating surface, they cannot provide real-time feedback during assembly, forcing the robot to move along a predetermined trajectory, which can easily lead to assembly deviations.

[0022] In some related technologies, fault codes (such as "position out of tolerance") can be recorded when assembly deviations occur. However, these fault codes only provide a rough indication, lacking detailed information such as the specific direction and value of the deviation, thus offering limited help in locating the root cause of the problem. Secondly, fault codes are post-analysis data and cannot guide the robot to make adjustments in real time during assembly, making it difficult to avoid assembly deviations in real-time assembly.

[0023] In other words, the relevant technologies have the following shortcomings in dynamic assembly scenarios: 1. The mating surfaces are easily obscured, and real-time visual feedback is lacking. Due to the limited field of view of the fixed camera, the movement of the robot and tooling can block the mating surfaces, making it impossible to continuously perceive the assembly gaps and posture. During assembly, the robot can only install the powertrain onto the base support component according to a pre-set trajectory. Because the relative positional error of the mating surfaces is not acquired in real time, the robot's posture cannot be dynamically corrected during assembly. Once a deviation occurs (such as fluctuations in part dimensions or positioning offset), the robot will not automatically adjust, which may lead to incomplete assembly or damage to parts.

[0024] 2. Long changeover time for mechanical tooling. Each time a product model is changed, mechanical fixtures need to be replaced or adjusted, requiring a lot of manual intervention and time-consuming changeover, which cannot meet the requirements of rapid changeover in mixed-line production.

[0025] 3. Fault feedback is delayed and cannot be used for real-time control. Fault codes or offline detection data are only suitable for post-event analysis and cannot provide real-time position correction during assembly.

[0026] 4. The fault information is vague, making it difficult to pinpoint the specific cause. The fault codes only provide out-of-tolerance warnings, lacking precise three-dimensional spatial data, making it difficult for engineers to quickly determine whether the problem is with part dimensions, robot trajectory, or tooling positioning.

[0027] 5. Relying on human experience to solve problems. When assembly deviations occur, experienced technicians need to spend time troubleshooting and analyzing the issues. This process is tedious, and different personnel may arrive at inconsistent conclusions, which is not conducive to standardized operations.

[0028] Based on this, this application proposes an explicit 3D reconstruction method based on 3DGS (3D Gaussian Splatting). By replacing the implicit MLP (Multilayer Perceptron) representation of NeRF (Neural Radiance Fields) with 3DGS, differentiable rendering is achieved. Each Gaussian sphere is defined with parameters such as position, radius, color, and opacity. Pixel colors are calculated using a differentiable rendering formula, and gradient descent is used to iteratively optimize the 3D model parameters. This improves the 3D reconstruction speed from seconds to milliseconds (approximately 45 milliseconds per frame).

[0029] This application proposes a dual acceleration strategy of multi-resolution hash encoding and semantic guidance: on the one hand, a feature hash table is constructed, which may include multiple layers of grids, each layer of grids has an independent feature hash table, used to store spatial point feature vectors of a preset dimension. By hash querying the spatial point coordinates and concatenating them with the feature vectors of each layer, the feature query time is greatly compressed.

[0030] On the other hand, by utilizing the semantic mask generated by semantic segmentation, dense sampling is performed only on the powertrain region and the basic load-bearing component region during rendering and ray sampling, while the background region is given a lower loss weight. The combined effect of these two methods can reduce the overall computational load by more than 70%, ensuring that the single-frame processing latency is within 50 milliseconds.

[0031] This application proposes a kinematic information-assisted camera dynamic calibration and reconstruction optimization: On the one hand, the robot end-effector pose is calculated based on joint angle data using the robot's forward kinematics solution, and the theoretical extrinsic parameters of the camera are determined by combining the fixed installation relationship between the camera and the robot end-effector; Based on the world coordinates of the pre-set markers (such as QR codes) in the assembly station and their coordinates in the image, the actual extrinsic parameters of the camera are obtained by PnP (Perspective-n-Point), and the theoretical extrinsic parameters and actual extrinsic parameters are fused by a Kalman filter to obtain the calibrated extrinsic parameters, thereby effectively solving the dynamic vibration problem.

[0032] On the other hand, using the pre-set CAD (Computer-Aided Design) powertrain model as prior information, the CAD model is used to help distinguish the powertrain region during the semantic segmentation stage; during the point cloud registration stage, the selected powertrain point cloud and the CAD powertrain model point cloud are registered using ICP (Iterative Closest Point) to accelerate reconstruction convergence.

[0033] This application proposes a high-precision localization method that tightly couples UWB (Ultra-Wideband) and a visual factor graph. The method uses the relative distance between any two robots measured by the UWB module as the UWB factor measurement, the relative pose between any two robots reconstructed visually as the visual factor measurement, and the difference in robot pose measurement as the odometry factor measurement. By constructing a factor graph containing odometry, UWB, and visual factors, and employing the iSAM2 (incremental smoothing and mapping 2) optimization algorithm in a sliding window manner for iterative optimization, the optimal robot pose variables are solved by minimizing the weighted sum of squared residuals of each factor, thereby obtaining the estimated relative pose between the robots.

[0034] This application also provides a system for the combined control of the powertrain and the basic load-bearing components, which may include multiple robots, multiple UWB base stations, a synchronous controller, and an industrial control computer.

[0035] like Figure 1 As shown, five industrial robots can be deployed at the assembly station, with four industrial cameras (global shutter, 1280×1024 resolution, 30 frames / second) mounted on the end flange of each robot. The cameras are fixed by a custom-designed L-shaped stainless steel bracket, which has undergone rigidity simulation analysis to ensure that camera jitter is less than 0.05 mm during high-speed robot movement. The hardware trigger lines of the 20 cameras are connected to a synchronization controller, which is designed using an FPGA (Field-Programmable Gate Array) chip. After receiving trigger commands from the PLC (Programmable Logic Controller), the controller simultaneously outputs 20 channels of TTL (Transistor-Transistor Logic) level pulses with a pulse width of 10 milliseconds and a trigger accuracy of ±1 microsecond, thus ensuring that all cameras are exposed at the same time.

[0036] Each robot's end effector is also equipped with a UWB module, and four identical UWB base stations are fixed on the four corner pillars of the workshop. The UWB modules communicate using DS-TWR (Double-Sided Two-Way Ranging), with a ranging frequency of 100Hz and an initial ranging accuracy of ±5 cm. UWB data is collected by an RS485 bus to a data acquisition unit, and then transmitted to an industrial control computer via a network cable.

[0037] The industrial PC (such as an edge computing server) is housed in a cabinet in the workshop. Its configuration includes a high-performance CPU (Central Processing Unit), 64GB of DDR5 (Double Data Rate 5) memory, a large-memory graphics card (24GB of VRAM), and runs Ubuntu 20.04 (a Linux operating system). The industrial PC is connected to a 5G TSN (5G Time-Sensitive Networking) switch via a gigabit Ethernet cable. All camera image data and UWB data are aggregated to the industrial PC through this switch. The industrial PC connects to the robot controller via an industrial Ethernet network, such as Profinet (Process Field Net), for sending control commands.

[0038] The overall data flow is as follows: the synchronous controller sends a trigger signal → 20 cameras take pictures synchronously → the images are transmitted to the industrial computer via the network port → the industrial computer runs the algorithm → outputs the calculation results (relative pose between robots) → and sends them to the robot controller via Profinet.

[0039] In the embodiments of this application, at least the following advantages are present: 1. By deploying multiple robots (e.g., 5 robots) and installing multiple cameras (e.g., 4 cameras) at the end of each robot, multi-view synchronous image acquisition can be achieved. Even if some views are obstructed, the image data from the remaining views can still be used for 3D reconstruction. Combined with an explicit 3D reconstruction algorithm based on 3DGS and a dual acceleration strategy of multi-resolution hash coding and semantic guidance, the 3D reconstruction of a single frame takes about 45 milliseconds. This enables the assembly process to have visualization and closed-loop control capabilities, avoiding visual blind spots caused by occlusion.

[0040] 2. By adopting 3DGSplatting to replace the implicit MLP representation of NeRF, differentiable rendering is achieved. Multi-resolution hash coding is used to compress feature query time, and semantic segmentation is used to guide ray sampling. The time for single dynamic reconstruction and pose calculation is compressed to less than 50 milliseconds (approximately 45 milliseconds in actual measurement). This enables the robot to adjust its movement in real time according to the reconstruction results, avoiding the problem of slow reconstruction speed failing to meet the real-time control requirements of industrial sites.

[0041] 3. By using the relative measurement distance (accuracy of about 5 cm) obtained by the UWB module as the UWB factor and the relative measurement pose obtained by visual reconstruction (accuracy of about 0.2 mm) as the visual factor, and fusing the robot odometry factor, a sliding window factor graph optimization (iSAM2 algorithm) is used to solve the problem in a tightly coupled manner, the relative estimated pose between the robots (measured error of about 0.28 mm) is obtained. This enables the maintenance of relative positioning accuracy in complex environments with multi-robot collaboration and vibration interference, thus meeting the assembly process requirements.

[0042] 4. By changing the preset CAD powertrain model and the model parameters of the semantic segmentation network, it is possible to adapt to the assembly requirements of different product models, reducing the changeover time from more than half an hour to less than 5 minutes, thereby improving the changeover efficiency and production line adaptability of mixed-line production, and avoiding the problem of production efficiency being affected by long changeover time of mechanical tooling.

[0043] 5. By outputting the 3D model of the powertrain (such as a Gaussian sphere set) and its measured pose in real time, and recording the pose data throughout the assembly process, when deviations occur during assembly, the output 3D model and pose history data can be viewed directly to quickly analyze the source of the deviation (such as part size, robot trajectory or tooling positioning), thereby realizing the digitalization and traceability of the assembly process, reducing reliance on manual experience, and improving the efficiency of problem troubleshooting.

[0044] Reference Figure 2 The diagram illustrates a flow chart of an optional powertrain and basic load-bearing component assembly control method provided in some embodiments of this application, wherein the robot end effector is equipped with a camera and a UWB module.

[0045] In practical applications, a combined assembly station can be configured with multiple robots. Each robot's end effector (such as the mounting location of the robot arm's end effector) can be equipped with a camera and a UWB module. The camera can be used to acquire image data including the powertrain and basic load-bearing components, while the UWB module can be used to communicate with a UWB base station to obtain ranging data.

[0046] The assembly station refers to the work area on the final assembly line where powertrains (such as engines and electric drive systems) are assembled with basic supporting components. Robots, fixtures, sensors, and other equipment can be placed in the assembly station to complete processes such as docking and fastening the powertrain with the basic supporting components.

[0047] In some embodiments, the camera may be an industrial camera, such as a global shutter camera, to ensure clear imaging during robot movement. The UWB module may be an ultra-wideband wireless communication module for achieving distance measurement with centimeter-level accuracy.

[0048] For example, multiple cameras (e.g., four) can be installed at the robot's end effector to simultaneously acquire images from different angles. The cameras and UWB modules can be rigidly fixed to the robot's end effector flange to ensure a fixed relative position between them.

[0049] Specifically, it may include the following steps: Step 201: Obtain the measured pose difference of the robot.

[0050] In practical applications, measuring pose difference can represent the relative motion of a robot between adjacent moments, including position changes and posture changes.

[0051] In some embodiments, the measured pose difference can be calculated based on the robot's joint angle values. For example, by using forward kinematics, the joint angle values ​​are converted into the poses of the robot's end effector at adjacent time points, and then the poses at adjacent time points are subtracted to obtain the measured pose difference.

[0052] The measured pose difference can include translational components (such as the distance moved along the X, Y, and Z directions) and rotational components (such as the rotation angle (Euler angle) around the X, Y, and Z axes, or represented by quaternions).

[0053] For example, the industrial control computer can read the six joint angle values ​​output by the robot controller every 10 milliseconds. Using pre-calibrated Denavit-Hartenberg parameters (DH parameters), a forward kinematics function can be written to calculate the 4×4 transformation matrix of the robot's end effector flange at the current moment, i.e., the pose (position and orientation) of the robot's end effector flange at the current moment. The same calculation is then performed on the joint angles at the previous moment to obtain the transformation matrix for the previous moment, i.e., the pose (position and orientation) of the robot's end effector flange at the previous moment.

[0054] By multiplying the transformation matrix at the current moment by the inverse of the transformation matrix at the previous moment, the relative transformation matrix between adjacent moments is obtained, which serves as the measurement pose difference. The translation vector in the relative transformation matrix represents the position change, and the rotation matrix can be converted into Euler angles or quaternions to represent the pose change, i.e., the rotation component.

[0055] Step 202: Obtain the measurement data of the UWB module, and determine the relative measurement distance between any two robots based on the measurement data of the UWB module.

[0056] In some embodiments, multiple UWB base stations can be set up at fixed locations within the assembly station. The UWB base stations can serve as reference points with known coordinates and communicate with the UWB module installed at the robot's end effector. By measuring the time difference (i.e., time of flight) between the signal emitted by the UWB module and the return signal from the UWB base station, the distance between the UWB module and each UWB base station can be calculated.

[0057] The measurement data from the UWB module can include multiple raw distance values ​​between the UWB module and each UWB base station. Based on the raw distance values ​​and the preset world coordinates of each UWB base station, the measured position of each robot can be calculated using trilateration or least squares method. The Euclidean distance between any two robots is calculated based on their measured positions, and this Euclidean distance is used as the relative measured distance between the two robots.

[0058] For example, four UWB base stations can be fixed at the four corners of the assembly station. The world coordinates of the UWB base stations are pre-calibrated using a laser tracker. Each robot (e.g., robot A, robot B) has a UWB module installed at its end effector. The UWB module sends ranging requests to the four base stations at a frequency of 100Hz, obtaining four distance values ​​using a dual-side bidirectional ranging (DS-TWR) method. After receiving the distance values, the industrial control computer uses the least squares method to calculate the measured position coordinates (e.g., P_A and P_B) of robots A and B. By calculating the Euclidean distance between P_A and P_B, the relative measured distance between robots A and B is obtained.

[0059] Step 203: Based on the image data acquired by the camera and the camera's intrinsic and extrinsic parameters, construct a three-dimensional model of the powertrain, and determine the measurement pose of the powertrain based on the three-dimensional model.

[0060] In some embodiments, image data captured by a camera can be used, combined with the camera's intrinsic parameters (such as focal length, principal point, and distortion coefficient) and extrinsic parameters (the camera's position and orientation in the world coordinate system), to generate a 3D model of the powertrain through 3D reconstruction. The 3D model can be an optimized set of Gaussian spheres (each Gaussian sphere containing position, radius, color, and opacity), obtained through rendering and iterative optimization.

[0061] By extracting the powertrain point cloud from the 3D model and registering it with the preset CAD powertrain model point cloud, a 4×4 transformation matrix is ​​output, which is the measured pose of the powertrain (including position and orientation).

[0062] Step 204: Determine the measurement pose of the robot relative to the powertrain based on the extrinsic parameters of the camera and the measurement pose of the powertrain, and determine the relative measurement pose between any two robots based on the measurement pose of the robot relative to the powertrain.

[0063] In some embodiments, the extrinsic parameters of the camera can be represented as the pose of the camera (i.e., the robot end effector) in the world coordinate system, and the measured pose of the powertrain can be represented as the pose of the powertrain in the world coordinate system.

[0064] By using coordinate transformation (e.g., T_R_E = inv(T_W_R) * T_W_E), the measured pose of the robot relative to the powertrain can be calculated. This involves transforming the powertrain's pose T_W_E in the world coordinate system to the robot coordinate system, thus obtaining the robot's measured pose T_R_E relative to the powertrain. Here, T_W_R represents the robot's (camera's) pose in the world coordinate system, and inv(T_W_R) is its inverse transformation, used to convert world coordinates to robot coordinates.

[0065] For any two robots (such as robot A and robot B), their measured poses (T_A_E and T_B_E) relative to the same powertrain can be obtained respectively. Then, the relative measured poses between the two robots can be obtained through relative transformation (T_A_B=T_A_E*inv(T_B_E)).

[0066] In other words, the measurement pose T_A_E of robot A relative to the powertrain and the measurement pose T_B_E of robot B relative to the powertrain are used to calculate the measurement pose T_A_B of robot A relative to robot B. Here, inv(T_B_E) represents the inverse transformation of T_B_E, used to transform points in the powertrain coordinate system to the robot B coordinate system. The entire product T_A_E*inv(T_B_E) first transforms the pose of robot A to the powertrain coordinate system, and then transforms it to the robot B coordinate system, thus obtaining the relative measurement pose of robot A relative to robot B. The relative measurement pose is a 6-DOF pose, including translation and rotation.

[0067] Step 205: Determine the relative estimated pose of any two robots based on the measured pose difference between any two robots and the relative measured pose and relative measured distance between any two robots, and perform assembly control on the robots based on the relative estimated pose of any two robots.

[0068] In some embodiments, the measured pose difference, relative measured pose, and relative measured distance of any two robots can be input into a factor graph optimizer. By minimizing the weighted sum of squares of the residuals of each factor, the optimal relative pose between any two robots, i.e., the relative estimated pose, can be solved.

[0069] After determining the relative estimated pose, the relative estimated pose (e.g., the relative estimated pose of robot A relative to robot B) can be sent to the robot controller in real time via industrial Ethernet. The controller compares the received relative estimated pose with the preset target assembly pose (i.e., the ideal relative pose required by the assembly process, such as the relative pose when the engine and the base support component are aligned) and calculates the deviation. Based on the deviation, the controller issues control commands to drive the robot to adjust its position and attitude until the deviation is less than the allowable range (e.g., ±0.5 mm), thereby completing the assembly of the powertrain and the base support component.

[0070] In this embodiment, the robot's measured pose difference is obtained; measurement data from the UWB module is obtained, and the relative measurement distance between any two robots is determined based on the UWB module's measurement data; a three-dimensional model of the powertrain is constructed based on the image data acquired by the camera and the camera's intrinsic and extrinsic parameters, and the measured pose of the powertrain is determined based on the three-dimensional model; the robot's measured pose relative to the powertrain is determined based on the camera's extrinsic parameters and the powertrain's measured pose, and the relative measured pose between any two robots is determined based on the robot's measured pose relative to the powertrain; the relative estimated pose between any two robots is determined based on the measured pose difference between any two robots and the relative measured pose and relative measurement distance between any two robots, and the robot's assembly control is performed based on the relative estimated pose between any two robots. This achieves more accurate robot relative estimated poses in scenarios where the assembly joint surface is obscured by combining relative measured poses, relative measurement distances, and measured pose differences for fusion estimation, reducing the risk of assembly collisions and part damage caused by positional deviations, and saving production costs.

[0071] In some embodiments of this application, the intrinsic parameters of the camera are obtained through offline calibration, and the extrinsic parameters of the camera are obtained through online calibration; before obtaining the measured pose difference of the robot, the method further includes: The robot's joint angle data is acquired. Based on the joint angle data, the pose of the robot's end effector is determined. Combined with the pose of the robot's end effector and the fixed installation relationship between the camera and the robot's end effector, the theoretical extrinsic parameters of the camera are determined. Based on the world coordinates of the preset markers in the assembly station and the image coordinates of the markers in the image data acquired by the camera, the actual extrinsic parameters of the camera are determined. Combining the theoretical extrinsic parameters and the actual extrinsic parameters, the calibrated extrinsic parameters are obtained.

[0072] In practical applications, robot joint angle data can include joint angle values.

[0073] In some embodiments, offline calibration can be performed by taking multiple images using a checkerboard calibration board and calculating the intrinsic parameter matrix and distortion coefficients using the calibrateCamera (Camera Calibration) function of OpenCV (Open Source Computer Vision Library).

[0074] Online calibration can be performed every frame or at regular intervals. The pose of the robot end effector can be calculated from joint angles using forward kinematics solutions (e.g., based on Denavit-Hartenberg parameters). The fixed mounting relationship between the camera and the robot end effector can be a pre-measured transformation matrix (e.g., translation and rotation).

[0075] The theoretical extrinsic parameters can be the transformation matrix obtained by multiplying the robot's end-effector pose by a fixed mounting relationship. The marker can be an AprilTag (a visual reference tagging system used for robot localization and pose estimation) QR code or a checkerboard pattern, whose world coordinates can be pre-calibrated using a laser tracker. The actual extrinsic parameters can be solved using PnP, such as by calling OpenCV's solvePnP (Solve Perspective-n-Point, a pose estimation function in the OpenCV library). The combination of theoretical and actual extrinsic parameters can be achieved using methods such as Kalman filtering or weighted averaging to output calibrated extrinsic parameters.

[0076] Optionally, camera calibration can be divided into two steps: offline intrinsic parameter calibration and online extrinsic parameter calibration. For offline intrinsic parameter calibration, a 30cm × 30cm checkerboard calibration board (grid spacing 20mm, 7×7 grid points) can be used. Each camera takes 15 photos of the calibration board at different positions (tilted, translated, rotated). The intrinsic parameter matrix and distortion coefficients are calculated using OpenCV's `calibrateCamera` function.

[0077] Alternatively, you can write a Python script to process them in batches, as shown in the following code: import cv2,glob,numpy as np # Import OpenCV, glob (global, global file matching mode) for file matching, and NumPy (Numerical Python, numerical computation library); CHECKERBOARD=(6,6) # Defines the number of corner points in the Checkerboard (a planar pattern used for camera calibration): 6 rows and 6 columns (the actual number of corner points is 6×6=36). criteria=(cv2.TERM_CRITERIA_EPS+cv2.TERM_CRITERIA_MAX_ITER,30,0.001) # Set the corner subpixel refinement termination condition: stop after 30 iterations or when the precision reaches 0.001; objp=np.zeros((CHECKERBOARD[0]*CHECKERBOARD[1],3),np.float32) # Create an array to store the coordinates of corner points in the world coordinate system, with the shape (number of interior corner points, 3); objp[:,:2]=np.mgrid[0:CHECKERBOARD[0],0:CHECKERBOARD[1]].T.reshape(-1,2)*20# Set the actual side length of each grid cell of the chessboard to 20mm, generate X and Y coordinates, and keep the Z coordinate 0; objpoints=[]; # Stores a list of world coordinate points for all images; imgpoints=[] # Stores a list of all pixel coordinates detected in the image; images=glob.glob('cam1 / *.png') # Use glob to get all .png image files in the cam1 folder; for fname in images: # Iterate through each image; img=cv2.imread(fname); # Read the image; gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # Convert to grayscale image; ret, corners = cv2.findChessboardCorners(gray, CHECKERBOARD, None) # Detects the corners of the chessboard, returns whether it was successful (ret) and the coordinates of the corners (corners); if ret: # If corner points are successfully detected; objpoints.append(objp) # Add world coordinates (all images share the same set of coordinates); corners2=cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)# Perform sub-pixel refinement on corner points to improve accuracy; imgpoints.append(corners2) # Add the refined pixel coordinates; ret,mtx,dist,rvecs,tvecs=cv2.calibrateCamera(objpoints,imgpoints,gray.shape[::-1],None,None) # Perform camera calibration: Input world coordinates, pixel coordinates, image size, output intrinsic parameter matrix, distortion coefficients, rotation / translation vector; np.savez('cam1_intrinsics.npz',mtx=mtx,dist=dist) # Saves the intrinsic parameter matrix and distortion coefficients as a .npz file for easy loading later; The output file contains the camera matrix (3×3) and distortion coefficients (1×5), for example: mtx=[[1680.2,0,640.5],[0,1679.8,512.3],[0,0,1]]#Example of intrinsic parameter matrix: fx=1680.2,fy=1679.8,cx=640.5,cy=512.3; dist=[-0.032,0.021,-0.0008,0.0005,0.0012]#Example of distortion coefficients: k1=-0.032,k2=0.021,p1=-0.0008,p2=0.0005,k3=0.0012; #The intrinsic parameter files for all 20 cameras are stored under / config / cam_intrinsics / and are loaded into shared memory once when the program starts.

[0078] Optionally, since the camera pose changes in real time during robot movement, a combination of coarse and fine calibration can be used to calibrate the camera's extrinsic parameters online, such as... Figure 3 The following process can be followed: 1. Coarse Calibration: The industrial computer can read the angles (in degrees) of the six joints in the robot controller every 10 milliseconds. Based on the pre-calibrated Denavit-Hartenberg parameters, fkine (Forward Kinematics) can be written. The forward kinematics function can return a 4×4 transformation matrix T_f, which represents the pose of the robot end effector flange in the robot's base coordinate system.

[0079] The camera is fixedly mounted on a flange, and its mounting offset T_cam2flange can be obtained in advance through measurement (e.g., the translation vector is [50mm, 30mm, 100mm], and the rotation matrix is ​​set as an identity matrix). Therefore, the camera's theoretical extrinsic parameter T_cam_theory can be calculated using the formula T_cam_theory = T_f * T_cam2flange.

[0080] 2. Precision Calibration: Twelve AprilTag QR codes, each 100mm x 100mm in size, can be affixed to the floor of the assembly station. Their precise world coordinates can be pre-measured using a laser tracker. Every second, it is checked whether any QR code appears simultaneously in the field of view of at least two cameras. If a QR code is present, PnP (Programmable Point-to-Plate) calculation is performed, including: detecting the image coordinates (sub-pixel accuracy) of the four corner points of the QR code; calling the OpenCV solvePnP function, taking the known world coordinates and the detected image coordinates as input, and outputting the precise pose T_cam2tag of the camera relative to the QR code; since the world coordinates of the QR code are known, the true pose T_cam_real of the camera in the world coordinate system can be further obtained.

[0081] 3. Kalman filter fusion: After obtaining the theoretical extrinsic parameter T_cam_theory and the actual extrinsic parameter T_cam_real, a Kalman filter can be used to fuse the theoretical and actual extrinsic parameters, and output the calibrated extrinsic parameter T_cam_corrected.

[0082] The filter parameters can be set as follows: process noise covariance Q = 0.001 × I, measurement noise covariance R = 0.01 × I. By combining coarse and fine calibration, accurate initial values ​​of camera extrinsic parameters can be obtained before each frame of 3D reconstruction, thereby ensuring the accuracy of subsequent reconstruction and positioning.

[0083] In some embodiments of this application, determining the relative measurement distance between any two robots based on the measurement data from the UWB module includes: The system acquires multiple measurement distances between the UWB module and multiple UWB base stations fixedly installed within the assembly station, and determines the measurement position of the robot based on the multiple measurement distances and the preset world coordinates of the multiple UWB base stations; and determines the relative measurement distance between any two robots based on the measurement positions of any two robots.

[0084] In some embodiments, the UWB base station can be a device fixed at the four corners of the assembly station or at a specific location, and its world coordinates can be pre-calibrated using high-precision instruments such as a laser tracker. The measured distance can be obtained through bilateral two-way ranging (DS-TWR), and an original distance value can be formed between each UWB module and each UWB base station.

[0085] The robot's measurement position can be determined using trilateration or least squares methods: given the coordinates of multiple UWB base stations and the distances from the UWB modules to each UWB base station, the coordinates of the UWB modules at the robot's end effector in the world coordinate system can be calculated. This measurement position can be a three-dimensional coordinate point (e.g., x, y, z). The relative measurement distance can be the Euclidean distance between the measurement positions of two robots.

[0086] For example, four UWB base stations are fixed at the four corners of the assembly station, with coordinates B1, B2, B3, and B4, respectively, and calibrated using a laser tracker. The UWB module at the end effector of robot A communicates with the four base stations to obtain distance values ​​dA1, dA2, dA3, and dA4; the UWB module at the end effector of robot B obtains distance values ​​dB1, dB2, dB3, and dB4. The industrial control computer uses the least squares method to calculate the measured position PA(xA, yA, zA) of robot A and the measured position PB(xB, yB, zB) of robot B, respectively. By calculating the Euclidean distance between PA and PB, this Euclidean distance is the relative measured distance between robot A and robot B.

[0087] Optionally, the UWB module can send ranging requests to four base stations at a frequency of 100Hz, obtaining four distance values ​​d1, d2, d3, and d4. Based on the known base station coordinates B1, B2, B3, and B4, the measurement position (rough coordinates in the world coordinate system) P_tag of the robot containing the UWB module is calculated using the least squares method. The code is as follows: def trilateration(dists, bases): # Defines a trilateration function, taking a list of distances and a list of base station coordinates as inputs; #dists:[d1,d2,d3,d4],bases:4x3 coordinates #dists stores four distance values, and bases stores the (x,y,z) coordinates of four base stations; A=[]# Initialize the coefficient matrix A to store the coefficients of the linear equation system; b = [] # Initialize the constant term vector b, which is used to store the right-hand side terms of the linear equation system; for i in range(3): # Loop through the first three base stations (i=0,1,2) and construct the equation by difference with the fourth base station; A.append([2*(bases[i][0]-bases[3][0]), # Add the coefficient of x in the i-th equation: 2*(x_i-x_4); 2*(bases[i][1]-bases[3][1]), # Add the coefficient of y in the i-th equation: 2*(y_i-y_4); 2*(bases[i][2]-bases[3][2])])# Add the coefficient of z in the i-th equation: 2*(z_i-z_4); b.append(dists[3]**2-dists[i]**2) # Calculates the constant term: d4^2-di^2; -bases[3][0]**2+bases[i][0]**2# Subtract x4^2 and add xi^2; -bases[3][1]**2+bases[i][1]**2# Subtract y4^2 and add yi^2; -bases[3][2]**2+bases[i][2]**2)# Subtract z4^2 and add zi^2; return np.linalg.lstsq(A,b,rcond=None)[0]# Call the least squares method to solve the linear equation system A*pos=b, and return the position coordinates (x,y,z); After obtaining the measured positions of each robot (e.g., the measured position of robot i is denoted as P_tag_i, and the measured position of robot j is denoted as P_tag_j), the Euclidean distance between any two robots can be calculated. The Euclidean distance formula can be used: D_uwb = ||P_tag_i - P_tag_j||. The noise standard deviation of the measured values ​​is approximately 5 cm.

[0088] In some embodiments of this application, a three-dimensional model of the powertrain is constructed based on image data acquired by the camera and the camera's intrinsic and extrinsic parameters, and the measurement pose of the powertrain is determined based on the three-dimensional model, including: Sub-step 2031: Semantic segmentation is performed on the image data acquired by the camera to generate a semantic mask.

[0089] In practical applications, semantic masks can be used to distinguish different regions in image data, such as the powertrain region, the basic load-bearing component region, and the background region.

[0090] In some embodiments, semantic segmentation can employ lightweight neural networks, such as MobileNetV3-Seg (MobileNetV3 Semantic Segmentation, a semantic segmentation network based on the third version of the mobile network architecture), to perform pixel-level classification on the input image data. The input image data can be compressed to a preset resolution (e.g., 512×512 pixels), and through forward inference of the network, a semantic mask of the same size as the input image is output.

[0091] In the semantic mask, each pixel's value corresponds to a category label (e.g., 0 represents the powertrain, 1 represents the basic load-bearing components, and 2 represents the background). The semantic mask can be used in subsequent rendering optimization processes to set different loss weights for pixels in different regions, so that the optimization focuses on the powertrain and basic load-bearing component areas.

[0092] Optionally, a lightweight semantic segmentation network can be pre-trained. The training data can come from 5,000 images taken on-site, and LabelMe (an open-source image manual annotation tool) can be used to manually annotate three types of regions in the images: "powertrain (such as engine)," "basic load-bearing components," and "background."

[0093] In actual operation, such as Figure 4 It can compress the image data acquired by the camera to 512×512 pixels, input it into the semantic segmentation network, and output a semantic mask of the same size as the input image. The value of each pixel in the semantic mask can be 0, 1, or 2, corresponding to the background, engine area, and basic load-bearing component area, respectively.

[0094] The semantic segmentation process can be accelerated in parallel using CUDA (Compute Unified Device Architecture), with a total processing time of approximately 8-12 milliseconds for 20 images. After generating the semantic mask, only regions with values ​​of 1 or 2 (i.e., the engine and basic supporting components) can be retained, while the background region is assigned a lower weight during subsequent light sampling.

[0095] In some embodiments of this application, a three-dimensional model of the powertrain is constructed based on image data acquired by the camera and the camera's intrinsic and extrinsic parameters, and the measurement pose of the powertrain is determined based on the three-dimensional model, including: Sub-step 2032: Render the Gaussian sphere set according to the intrinsic and extrinsic parameters of the camera, and iteratively optimize and update the parameters of the Gaussian sphere set according to the semantic mask to construct the three-dimensional model of the powertrain; wherein, when the image data is the first frame image data, the Gaussian sphere set is generated according to the preset CAD powertrain model point cloud, and when the image data is the Nth frame image data, the Gaussian sphere set is the Gaussian sphere set optimized using the (N-1)th frame image data, where N is a positive integer greater than 1.

[0096] like Figure 4After semantic segmentation, it can be determined whether the image data is the first frame. If the current image data captured by the camera is the first frame, the Gaussian sphere set can be generated according to the preset CAD powertrain model point cloud. If the current image data captured by the camera is the Nth frame (N is a positive integer greater than 1), the Gaussian sphere set can be the Gaussian sphere set optimized from the N-1th frame image data.

[0097] In some embodiments, the Gaussian sphere set can be a three-dimensional representation, with each Gaussian sphere containing parameters such as position, radius, color, and opacity. Rendering refers to projecting the Gaussian spheres in three-dimensional space onto a two-dimensional image plane according to camera parameters to generate a rendered image. Semantic masks can be used to distinguish between powertrain regions, foundation support component regions, and background regions in an image. During iterative optimization, different loss weights can be set for different regions (e.g., a higher weight for the powertrain region).

[0098] Iterative optimization and updates can employ gradient descent, using the pixel difference between the rendered image and the corresponding camera-acquired image data as the loss function, and gradually adjusting the Gaussian sphere parameters to make the 3D model gradually approximate the shape and appearance of the real powertrain.

[0099] The 3D model can be a set of Gaussian spheres of the powertrain obtained through iterative optimization and updates. Each Gaussian sphere in the set contains parameters such as position, radius, color, and opacity, which together describe the 3D shape and appearance of the powertrain.

[0100] A pre-defined CAD powertrain model (STL file format) can be a 3D computer-aided design model of a powertrain (such as an engine) pre-stored in the system. The CAD powertrain model can contain the powertrain's geometry and dimensional information, such as point clouds or triangular meshes.

[0101] Optionally, the structure of each Gaussian sphere can be predefined, such as: struct GaussianPoint{ / / Defines a Gaussian sphere structure to represent an ellipsoid in three-dimensional space; float pos[3]; / / Position coordinates, unit: millimeters, storing x, y, z values ​​sequentially; float scale[3]; / / Radius scaling factor, unit: millimeters, representing the radius size along the x, y, z axes respectively; float color[3]; / / Color component, value range [0,1], representing red, green and blue respectively; float opacity; / / Opacity, value range [0,1], 0 represents completely transparent, 1 represents completely opaque; }

[0102] Optionally, upon determining that the image data captured by the camera is the first frame, a point cloud can be extracted from a preset CAD powertrain model, such as STL (STereoLithography, a 3D model file format). Taking an engine as an example, its point cloud contains approximately 50,000 points; the point cloud of the basic load-bearing components contains approximately 80,000 points. Each point in the CAD powertrain model point cloud can be traversed, generating a corresponding Gaussian sphere for each point to form a Gaussian sphere set. The initial parameters of each Gaussian sphere can be set as follows: pos = point cloud coordinates / / Set the center position of the Gaussian sphere to the three-dimensional coordinates of the current point in the point cloud; scale=[2.0,2.0,2.0] / / Sets the radius scaling factor of the Gaussian sphere in the X, Y, and Z directions, with an initial radius of 2 millimeters; color=[0.5,0.5,0.5] / / Sets the color of the Gaussian sphere to gray, with the three components corresponding to red, green, and blue respectively, and the value range is 0~1; opacity=0.5 / / Sets the opacity of the Gaussian sphere to 0.5, with a value range of 0 to 1, where 0 is completely transparent and 1 is completely opaque; Optionally, when it is determined that the image data captured by the camera is not the first frame (i.e., the Nth frame image data), the Gaussian sphere parameters of the Gaussian sphere set optimized from the N-1th frame image data can be used, and all Gaussian spheres can be retained (the number will not be reduced after it stabilizes).

[0103] In some embodiments of this application, it also includes: Construct a feature hash table; wherein the feature hash table includes multiple layers of grids, each layer of grids has an independent feature hash table, and the feature hash table of each layer of grids is used to store spatial point feature vectors of a preset dimension; During the rendering of the Gaussian sphere set, the feature hash table is queried according to the coordinates of the spatial points, and the feature vectors of the spatial points obtained from the feature hash table of each layer of the grid are concatenated to obtain the total feature vector of the spatial points, so as to perform rendering based on the total feature vector and the parameters of the Gaussian sphere set; During the iterative optimization and updating of the parameters of the Gaussian sphere set, the feature hash table is iteratively optimized and updated.

[0104] In some embodiments, the feature hash table can be a multi-resolution data structure used to accelerate feature lookup during the rendering process. A multi-layered mesh can contain multiple resolution levels, such as a 100mm side length for layer 0, a 50mm side length for layer 1, and so on up to a approximately 3mm side length for layer 15.

[0105] Each layer can independently maintain an independent feature hash table. Each independent feature hash table stores the spatial point feature vectors at the vertices of that layer's mesh. Each spatial point feature vector can be 32-dimensional. A spatial point can refer to the center point of a Gaussian sphere or a 3D coordinate point upsampled by a rendering ray.

[0106] During a query, for a given spatial point, its voxel vertex coordinates in each mesh layer can be calculated. The corresponding spatial point feature vector is then looked up using a feature hash table (or obtained through interpolation of adjacent vertices if it does not exist). By concatenating the spatial point feature vectors obtained from each layer in hierarchical order, a total feature vector is formed (e.g., 16 layers × 32 dimensions = 512 dimensions). This total feature vector, along with the Gaussian sphere parameters (position, radius, color, opacity) from the Gaussian sphere set, is used in rendering calculations, such as determining pixel color.

[0107] While iteratively optimizing the Gaussian sphere parameters, the gradient of the loss function with respect to the spatial point feature vectors in the feature hash table can be calculated, and the spatial point feature vectors can be updated synchronously using the gradient descent method, so that the hash table can adaptively learn spatial features that are conducive to reconstruction.

[0108] For example, during the rendering process, for a spatial point P (such as the center of a Gaussian sphere), the feature hash tables of layer 0 (100mm resolution), layer 1 (50mm resolution), ..., layer 15 (approximately 3mm resolution) are queried. Each layer yields a 32-dimensional spatial point feature vector. These multiple 32-dimensional vectors are then concatenated layer by layer to form a 512-dimensional total feature vector. This total feature vector can be used to modulate the color contribution of the Gaussian sphere or as additional input.

[0109] During the optimization phase, the Adam (Adaptive Moment Estimation) optimizer can be used to simultaneously update the position, radius, color, and opacity of the Gaussian sphere, as well as the 32-dimensional spatial point feature vectors in the independent feature hash tables of each layer. After 5 iterations, the feature hash tables and Gaussian sphere parameters converge together, reducing the difference between the rendered image and the image data acquired by the camera.

[0110] Optionally, a multi-resolution hash table can be used to store spatial features instead of a large neural network. This feature hash table can be divided into 16 grid layers, each with a different size: layer 0 has a grid size of 100 mm, layer 1 is 50 mm, and so on, with layer 15 being approximately 3 mm. Each layer has an independent feature hash table, where the key is the integer coordinates (i, j, k) of the grid vertices, and the corresponding value is a 32-dimensional spatial point feature vector.

[0111] The spatial point feature vectors in the hash table can be optimized synchronously with the Gaussian sphere parameters. In each gradient descent iteration, not only are the position, color, and other parameters of the Gaussian sphere updated, but the partial derivative of the loss function with respect to the spatial point feature vectors in the hash table is also calculated, and the feature values ​​are updated along the gradient direction. This allows the spatial point feature vectors to automatically learn spatial codes beneficial for reconstruction during the process. Initially, all spatial point feature vectors can be randomly initialized to a uniform distribution between [0,1].

[0112] When querying the characteristics of a spatial point P, the following operations can be performed: For each mesh layer, the coordinates of the vertex containing P are calculated, and the spatial point feature vector f_layer is retrieved from the independent feature hash table of that layer. If no corresponding spatial point feature vector is found, it can be obtained by interpolation using the spatial point feature vectors of the surrounding 8 vertices. The spatial point feature vectors f_layer obtained from the 16 layers are concatenated into a 512-dimensional total feature vector F. This query process can be completed in parallel on the CUDA platform, requiring approximately 500,000 spatial points to be queried per frame, taking approximately 3 milliseconds.

[0113] In some embodiments of this application, the Gaussian sphere set is rendered based on the intrinsic and extrinsic parameters of the camera, and the parameters of the Gaussian sphere set are iteratively optimized and updated based on the semantic mask to construct a three-dimensional model of the powertrain, including: Sub-step 20321: Render the Gaussian sphere set according to the intrinsic and extrinsic parameters of the camera to obtain a rendered image.

[0114] In some embodiments, the Gaussian spheres in the Gaussian sphere set can be transformed from the world coordinate system to the camera coordinate system according to the extrinsic parameters, and the points in the camera coordinate system can be projected onto the two-dimensional image plane according to the intrinsic parameters. The contribution of each Gaussian sphere to the pixel color can be accumulated (e.g., the weight can be calculated based on the distance between the Gaussian sphere and the ray) to generate a two-dimensional rendered image.

[0115] For example, the camera's intrinsic parameter matrix is ​​[[1680,0,640],[0,1680,512],[0,0,1]], and the extrinsic parameter is a 4×4 transformation matrix from the world coordinate system to the camera coordinate system. The industrial control computer reads the set of Gaussian spheres currently being optimized. For each Gaussian sphere, its center is first transformed to the camera coordinate system using the extrinsic parameter, and then projected to the image coordinate system using the intrinsic parameter. The color contribution of the Gaussian sphere to the surrounding pixels is calculated. The contributions of all Gaussian spheres are superimposed to obtain a rendered image with the same resolution as the image data acquired by the camera.

[0116] Optionally, during the rendering process, the rendered color of a pixel with coordinates (u,v) on the image data can be calculated in the following way: By back-projecting this pixel into 3D space, a ray emanating from the camera center is obtained. Among all Gaussian spheres, those whose center-to-ray distance is less than three standard deviations (i.e., the scale of the Gaussian sphere) are identified. The color contribution weight of each selected Gaussian sphere to this pixel can be represented as: w=opacity*exp(-d^2 / (2*s^2)).

[0117] Where w represents the weight of a Gaussian sphere's contribution to the color of a pixel; opacity represents the opacity of the Gaussian sphere, ranging from 0 to 1; exp represents the natural exponential function; d represents the shortest distance from the pixel ray to the center of the Gaussian sphere; and s represents the radius of the Gaussian sphere (which can be the average of the three scale axis components). The color of the pixel can be calculated as a weighted average of the colors of all contributing Gaussian spheres, with the weights normalized and then summed. The entire rendering process can be implemented on the CUDA platform, and the calculation for each pixel can be executed in parallel.

[0118] Sub-step 20322: Determine the loss function value based on the sum of squared pixel differences between the rendered image and the image data acquired by the camera, and the weights corresponding to the semantic mask to which each pixel belongs; wherein, the semantic mask is used to distinguish the powertrain region, the basic support component region, and the background region in the image data, and the weights of pixels in the powertrain region and the basic support component region are higher than the weights of pixels in the background region.

[0119] In some embodiments, the sum of squared pixel differences can be calculated by summing the squared difference between the pixel value of the rendered image at each corresponding pixel location and the pixel value of the image data acquired by the camera over all pixels. The semantic mask can be a matrix of the same size as the image, with each location storing a category label (e.g., 0 for background, 1 for powertrain, and 2 for basic load-bearing components).

[0120] Based on the semantic mask, different weight coefficients can be assigned to pixels of different categories. For example, the weight of the powertrain and basic load-bearing component regions can be set to 1.0, while the weight of the background region can be set to 0.1. The loss function value can be obtained by multiplying the squared difference of each pixel by its corresponding weight and then summing them up.

[0121] For example, for a rendered image and the corresponding image data captured by the camera, the difference d at each pixel position (i,j) is calculated as d = pixel value of the rendered image - pixel value of the image data captured by the camera, and squared to obtain d². If the semantic mask label of the pixel is powertrain (1) or basic load-bearing component (2), it is multiplied by a weight of 1.0; if the label is background (0), it is multiplied by a weight of 0.1. The weighted squared differences of all pixels are summed to obtain the loss function value. The loss function value can be used for subsequent gradient descent optimization to update the parameters of the Gaussian sphere set.

[0122] Sub-step 20323: With the goal of minimizing the loss function value, the gradient descent method is used to iteratively optimize the parameters of the Gaussian sphere set in order to construct a three-dimensional model of the powertrain.

[0123] In some embodiments, gradient descent can be used to update parameters such as the position, radius, color, and opacity of each Gaussian sphere in the Gaussian sphere set. Each iteration calculates the partial derivatives (gradients) of the loss function with respect to each parameter and adjusts the parameter values ​​along the gradient descent direction, causing the loss function value to gradually decrease.

[0124] The iteration can be repeated multiple times (e.g., 5 times) until the loss function value converges or the preset number of iterations is reached. Through iterative optimization, the parameters of the Gaussian sphere set can gradually approximate the shape and appearance of the powertrain to construct a three-dimensional model of the powertrain.

[0125] For example, the Adam optimizer can be used with a learning rate of 0.01 and 5 iterations. In each iteration, the weighted sum of squared pixel differences between the rendered images from all camera views and the image data acquired by the camera is calculated as the loss function value. The gradients of the position, radius, color, and opacity of each Gaussian sphere are calculated through backpropagation, and the position, radius, color, and opacity of each Gaussian sphere are updated accordingly. After 5 iterations, the parameters of the Gaussian sphere set can fit the three-dimensional structure of the powertrain well, thus completing the construction of the three-dimensional model.

[0126] Optionally, the optimization objective can be achieved by jointly optimizing the Gaussian sphere parameters and the feature hash table using gradient descent. The loss function can be defined as the sum of squared pixel differences between the rendered images and the image data acquired by the cameras from all camera views. The optimizer can use the Adam algorithm, with a learning rate of 0.01 and 5 iterations.

[0127] In practical applications, to focus on the target region, different weights can be assigned to the errors of different pixels: for target pixels marked as engines or basic load-bearing components in the semantic mask, their weight can be set to 1.0; for background pixels, their weight can be set to 0.1. Through this weighted setting, the system can prioritize optimizing the geometry and appearance of the powertrain and basic load-bearing component regions, while the background region is less affected, thus achieving fine reconstruction of the target region within a limited number of iterations. A single optimization (5 iterations) takes approximately 18–22 milliseconds.

[0128] In some embodiments of this application, a three-dimensional model of the powertrain is constructed based on image data acquired by the camera and the camera's intrinsic and extrinsic parameters, and the measurement pose of the powertrain is determined based on the three-dimensional model, including: Sub-step 2033: Select the point cloud of the powertrain from the three-dimensional model, and register the selected point cloud with the preset CAD powertrain model point cloud to obtain the measurement pose of the powertrain.

[0129] In some embodiments, the powertrain point cloud can be filtered by constructing a three-dimensional bounding box based on the powertrain measurement pose output in the previous frame, extracting Gaussian spheres located within the bounding box and with an opacity greater than a preset threshold (e.g., 0.3) from the Gaussian sphere set, and using the positions of the extracted Gaussian spheres as the powertrain point cloud.

[0130] Registration can be performed using the Iterative Closest Point (ICP) algorithm, which aligns the selected powertrain point cloud with the preset CAD powertrain model point cloud. An upper limit for the number of iterations (e.g., 10) and a convergence accuracy threshold (e.g., 1e-6) are set, outputting a 4×4 transformation matrix. This transformation matrix represents the measured pose of the powertrain in the world coordinate system, including position (translation vector) and orientation (rotation matrix).

[0131] Optionally, after the optimization iteration is complete, all Gaussian spheres can be traversed to filter out the point cloud of the powertrain. The filtering method can be as follows: The CAD powertrain model is transformed to the estimated position at the current moment using the powertrain measurement pose T_prev output from the previous frame, and a 3D bounding box is formed by extending 50 mm outward from the center point of this estimated position. Points with opacity > 0.3 located within this bounding box are selected from all Gaussian spheres and are used as the powertrain point cloud P_cloud for the current frame. The number of 3D points contained in the powertrain point cloud P_cloud obtained by the selection ranges from approximately 10,000 to 20,000.

[0132] Point cloud registration was performed between the PCL (Point Cloud Library) ICP algorithm and the CAD powertrain model point cloud: pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ> icp; / / Creates an ICP registration object, with both input and output point cloud types being PointXYZ; icp.setMaximumIterations(10); / / Sets the maximum number of iterations to 10 to prevent infinite loops; icp.setTransformationEpsilon(1e-6); / / Sets the convergence threshold of the transformation matrix; the iteration stops when the difference in transformation between two iterations is less than 1e-6. icp.setEuclideanFitnessEpsilon(1e-5); / / Sets the convergence threshold for the sum of squared Euclidean distances; the convergence stops when the mean square error is less than 1e-5. icp.setInputSource(P_cloud); / / Set the source point cloud (the point cloud to be registered, i.e., the point cloud of the powertrain selected). icp.setInputTarget(cad_cloud); / / Set the target point cloud (i.e., the point cloud of the CAD powertrain model, as a reference); icp.align(final_transform); / / Perform registration and store the resulting transformation matrix into final_transform.

[0133] After registration is completed, a 4×4 transformation matrix final_transform is output. This matrix is ​​the measured pose (6 degrees of freedom pose) of the powertrain relative to the world coordinate system at the current moment, including translation and rotation.

[0134] In some embodiments of this application, determining the relative estimated pose of any two robots based on the measured pose difference between any two robots and the relative measured pose and relative measured distance between any two robots includes: Sub-step 2051: Construct a factor graph; wherein the factor graph includes multiple factors and multiple pose variables, the multiple factors including: an odometry factor with the measured pose difference as the measured value, a UWB factor with the relative measured distance as the measured value, and a visual factor with the relative measured pose as the measured value; the odometry factor connects the pose variables of the same robot at adjacent times, and the UWB factor and the visual factor respectively connect the pose variables of different robots at the same time.

[0135] In some embodiments, the factor graph can be a probabilistic graphical model used to describe the constraints between variables. The pose variable can represent the robot's position and orientation at different times (e.g., represented by a 4×4 transformation matrix).

[0136] The odometry factor can define a constraint: the relative change between pose variables of the same robot at adjacent moments should be consistent with the measured pose difference. The UWB factor can define a constraint: the distance between the pose variables of two robots at the same moment should be consistent with the relative measured distance. The vision factor can define a constraint: the relative transformation between the pose variables of two robots at the same moment should be consistent with the relative measured pose.

[0137] Optionally, a factor map can be constructed using GTSAM (Georgia Tech Smoothing and Mapping), with the sliding window length set to 1 second (containing 20 time points). For example... Figure 5 At each time t_i, two pose variable nodes can be set to represent the pose X_A_i of robot A and the pose X_B_i of robot B, respectively.

[0138] Factor graphs can include the following three types of factors: Odometry factor: Connects pose variables of the same robot at adjacent time points. For example, for robot A, connects X_A_i and X_A_{i+1}, and for robot B, connects X_B_i and X_B_{i+1}. This factor obtains the measured pose difference between adjacent time points based on the robot's own forward kinematics solution. Its residual can be defined as the deviation between the estimated pose difference calculated based on the pose variables of adjacent time points (such as X_A_i and X_A_{i+1} or X_B_i and X_B_{i+1}) in the current optimization and the measured pose difference.

[0139] UWB factor: Connecting X_A_i and X_B_i at the same time t_i, the residual can be defined as... residual=|pos_A-pos_B|-D_uwb.

[0140] Where pos_A and pos_B represent the positions extracted from X_A_i and X_B_i, respectively, and D_uwb is the relative distance between any two robots measured by the UWB module. The residual has a dimension of 1, and the standard deviation of the noise model can be set to 0.05 meters.

[0141] Visual factor: Connecting X_A_i and X_B_i at the same time t_i, its residual can be defined as the difference between the relative estimated pose and the relative measured pose. This residual is 6-dimensional (including three translation components and three rotation components).

[0142] Optionally, before constructing the factor plot, a noise model can be pre-built using pre-defined covariance parameters. This noise model is used to weight the residuals of each factor. For example, the noise model can be constructed using the following code: auto odom_noise=noiseModel::Diagonal::Sigmas(Vector6(0.01,0.01,0.01,0.01,0.01,0.01)); / / Set the noise model for the odometer factor: a 6-dimensional vector (3 translations + 3 rotations), with standard deviations of 0.01 meters (translation) and 0.01 radians (rotation) for each component. auto uwb_noise=noiseModel::Diagonal::Sigmas(Vector3(0.05,0.05,0.05)); / / Set the noise model for the UWB factor: a 3-dimensional vector with a standard deviation of 0.05 meters for each component (the actual residual is a 1-dimensional distance, which here is the diagonal element of the covariance matrix); auto visual_noise=noiseModel::Diagonal::Sigmas(Vector6(0.0002,0.0002,0.0002,0.001,0.001,0.001)); / / Set the noise model for the visual factor: a 6-dimensional vector, with the first three components (translation) having a standard deviation of 0.0002 meters and the last three components (rotation) having a standard deviation of 0.001 radians.

[0143] After constructing the noise model, the factor graph can be built using the following code: NonlinearFactorGraph graph; / / Creates an empty nonlinear factor graph object; for i in range(N-1): / / Iterate through all adjacent time pairs within the sliding window (N-1 pairs in total); graph.add(BetweenFactor <pose3>(X_A_i,X_A_{i+1},odom_meas_A_i,odom_noise)); / / Add the odometry factor for robot A, and connect X_A_i and X_A_{i+1}; / / BetweenFactor <pose3>: Represents the relative transformation constraint between two poses; / / X_A_i represents the pose of robot A at time t_i (variable node, to be solved); / / X_A_{i+1} represents the pose of robot A at time t_{i+1} (variable node, to be solved); / / odom_meas_A_i represents the odometry measurement value of robot A (the pose change from t_i to t_{i+1} measured by the sensor); / / odom_noise represents the noise model of the odometer (standard deviation, indicating the reliability of the measurement); graph.add(BetweenFactor <pose3>(X_B_i,X_B_{i+1},odom_meas_B_i,odom_noise)); / / Add the odometry factor for robot B, and connect X_B_i and X_B_{i+1}; / / X_B_i represents the pose of robot B at time t_i (variable node, to be solved); / / X_B_{i+1} represents the pose of robot B at time t_{i+1} (variable node, to be solved); / / odom_meas_B_i represents the odometry measurement value of robot B (the pose change from t_i to t_{i+1} measured by the sensor); / / odom_noise represents the noise model of the odometer (standard deviation, indicating the reliability of the measurement); for i in range(N): / / Iterate through each time point within the sliding window (a total of N time points); graph.add(UWBFactor(X_A_i,X_B_i,uwb_dist_i,uwb_noise)); / / Add a UWB factor, concatenating X_A_i and X_B_i at the same time; / / uwb_dist_i represents the UWB measurement value (the straight-line distance between A and B measured by the sensor, a scalar); / / uwb_noise: represents the noise model of UWB (standard deviation 0.05m, low confidence level); if(visual_available[i]): / / If the visual observation is available at the current time; graph.add(BetweenFactor <pose3>(X_A_i,X_B_i,visual_T_A_B_i,visual_noise)); / / Add a visual factor to connect X_A_i and X_B_i at the same time; / / visual_T_A_B_i represents the visual measurement value (the pose of A relative to B calculated from 3D reconstruction, with high precision); / / visual_noise: Represents the visual noise model (translation 0.2mm, rotation 0.06 degrees, extremely high confidence).

[0144] After constructing the factor graph, an optimizer can be configured and the solution obtained. For example, the iSAM2 (Incremental Smoothing and Graphing) optimizer can be used to solve the factor graph. This can be configured using the following code: ISAM2Params params; / / Creates the iSAM2 optimizer parameter object; params.relinearizeThreshold=0.01; / / Sets the relinearization threshold: relinearization is triggered when the variable estimate changes by more than 0.01; params.relinearizeSkip=1; / / Sets the number of times to skip relinearization: checks if relinearization is needed after each update; ISAM2 isam(params); / / Creates an iSAM2 optimizer instance based on the parameters; isam.update(graph, initialEstimate); / / Perform optimization update: Input factor graph and initial estimate, solve for optimal pose variables; Pose3 result = isam.calculateEstimate(X_A_current); / / Extract the pose estimation result of robot A at the current time from the optimizer.

[0145] The optimizer can output the pose estimation result of robot A at the current time at a frequency of 20Hz. Combined with the pose estimation result of robot B at the current time output by the optimizer, the relative estimated pose between robot A and robot B at the current time t_current can be obtained.

[0146] Sub-step 2052: Based on the residuals between the measured values ​​and the corresponding estimated values ​​of each factor, and the pre-set noise standard deviation, iteratively optimize the robot's pose variables in the factor graph.

[0147] In some embodiments, the estimated value is a corresponding quantity calculated based on pose variables guessed during the current optimization process (such as pose difference, distance, or relative pose derived from pose variables). The residual can be the difference between the estimated value and the measured value.

[0148] The noise standard deviation can be used to weight the residuals; the smaller the standard deviation, the more reliable the measurement. Iterative optimization can employ gradient descent or the iSAM2 algorithm, aiming to minimize the weighted sum of squared residuals, repeatedly updating the pose variables until convergence or a preset number of iterations is reached.

[0149] In some embodiments of this application, the robot's pose variables in the factor graph are iteratively optimized based on the residuals between the measured values ​​and corresponding estimated values ​​of each factor, and a pre-set noise standard deviation, including: Sub-step 20521: Based on the residual between the measured value and the corresponding estimated value of the odometer factor, and the noise standard deviation of the odometer factor, construct the cost term of the odometer factor.

[0150] In practical applications, the measured value of the odometry factor can be the difference in pose between adjacent time points obtained through the forward kinematics solution of the robot.

[0151] In some embodiments, the estimated value of the odometry factor is an estimated pose difference (e.g., inv(X_A_i)*X_A_{i+1}) calculated based on the pose variables (e.g., X_A_i and X_A_{i+1} of robot A) guessed during the current optimization process at adjacent time points.

[0152] The residual can be the difference between the estimated pose difference and the measured pose difference (usually a 6-dimensional vector including translation and rotation components).

[0153] The noise standard deviation can be a pre-defined constant, such as a standard deviation of 0.01 meters for the translation component and 0.01 radians for the rotation component. The cost term of the odometer factor can be expressed as the square of the residual divided by the square of the noise standard deviation, i.e., (residual / standard deviation)². For multidimensional cases, the square of the residuals of each dimension can be calculated by dividing the sum of the corresponding variances.

[0154] Sub-step 20522: Based on the residual between the measured value and the corresponding estimated value of the UWB factor, and the noise standard deviation of the UWB factor, construct the cost term of the UWB factor.

[0155] In practical applications, the measured value of the UWB factor can be the relative distance between any two robots measured by the UWB module.

[0156] In some embodiments, the estimated value of the UWB factor can be calculated based on the pose variables of the two robots guessed during the current optimization process (such as the pose variable X_A_i of robot A and the pose variable X_B_i of robot B). For example, the Euclidean distance between the position components in the two pose variables can be calculated as the estimated distance.

[0157] The residual can be defined as the difference between the estimated distance and the measured distance. The noise standard deviation can be a pre-set constant, such as 0.05 meters. The cost term of the UWB factor can be expressed as the square of the residual divided by the square of the noise standard deviation, i.e., (residual / 0.05)².

[0158] Sub-step 20523: Construct the cost term of the visual factor based on the residual between the measured value and the corresponding estimated value of the visual factor, and the noise standard deviation of the visual factor.

[0159] In practical applications, the measured value of the visual factor can be the relative measured pose between any two robots obtained by visual reconstruction (such as the 6-DOF pose of robot A relative to robot B obtained by 3D reconstruction and coordinate transformation).

[0160] In some embodiments, the estimated value of the visual factor can be calculated based on the estimated relative pose obtained from the pose variables of the two robots guessed during the current optimization process (such as the pose variable X_A_i of robot A and the pose variable X_B_i of robot B), such as inv(X_A_i)*X_B_i.

[0161] The residual can be defined as the difference between the estimated relative pose and the measured relative pose. The residual is a 6-dimensional vector, including three translation components and three rotation components.

[0162] The noise standard deviation can be a pre-defined constant, such as a standard deviation of 0.0002 meters for the translation component and 0.001 radians for the rotation component. The cost term of the visual factor can be expressed as the sum of the squared residuals of each dimension divided by the corresponding variances, i.e., (residual translation / 0.0002)² for the translation component and (residual rotation / 0.001)² for the rotation component, and then summed.

[0163] Sub-step 20524: With the goal of minimizing the sum of the cost terms of the odometry factor, UWB factor and vision factor, iteratively optimize the robot's pose variables in the factor graph.

[0164] In practical applications, the optimization objective can be to minimize the sum of the cost terms of the odometry factor, the UWB factor, and the vision factor. By repeatedly adjusting the robot's pose variables in the factor graph, the sum of the cost terms can be continuously reduced until it reaches the minimum value.

[0165] In some embodiments, the cost terms of the odometry factor, UWB factor, and vision factor can be summed to obtain the total cost. The optimization objective is to find a set of robot pose variables (such as pose variable X_A_i of robot A and pose variable X_B_i of robot B) that minimizes this total cost.

[0166] Iterative optimization can employ the iSAM2 optimization algorithm. In each iteration, the residuals and costs are calculated based on the current pose variables, and the pose variables are updated to reduce the total cost. This process is repeated until the total cost converges.

[0167] In some embodiments of this application, determining the relative estimated pose of any two robots based on the measured pose difference between any two robots and the relative measured pose and relative measured distance between any two robots includes: Sub-step 2053: Determine the relative estimated pose of any two robots from the optimized pose variables.

[0168] In some embodiments, the optimized pose variables may include the poses of each robot at different times (e.g., pose variable X_A_i of robot A at time t_i, and pose variable X_B_i of robot B at time t_i). For any two robots (e.g., robot A and robot B), the relative estimated pose can be obtained by coordinate transformation based on their pose variables at the same or different times.

[0169] For example, at the same time t_i, the estimated relative pose of robot A with respect to robot B can be represented as inv(X_A_i)*X_B_i, which is a 4×4 transformation matrix containing translation and rotation components. This estimated relative pose is the optimal estimate after fusing odometry, UWB, and visual factors.

[0170] For example, after the sliding window optimization is completed, the current time is t_current, and the pose variable of robot A output by the optimizer is X_A_current, and the pose variable of robot B is X_B_current.

[0171] The estimated relative pose of robot A with respect to robot B can be calculated using T_A_B = inv(X_A_current) * X_B_current. This estimated pose can be directly used for assembly control, such as sending it to the robot controller to enable precise movement of robot B relative to robot A.

[0172] Some embodiments of this application provide an optional powertrain and basic load-bearing component assembly control system, the system comprising: multiple robots disposed at an assembly station, each robot having a camera and a UWB module at its end effector; multiple UWB base stations disposed at the assembly station; a synchronization controller connected to the hardware trigger lines of all cameras for controlling the synchronous exposure of all cameras; and an industrial control computer for executing the method described above.

[0173] like Figure 6 The system triggers all cameras and UWB modules to simultaneously acquire images and ranging data via a synchronous controller; camera extrinsic parameters are obtained through online camera calibration; semantic segmentation is performed on the acquired images, and a 3D model of the powertrain is constructed using Gaussian sphere reconstruction technology, outputting its measured pose (i.e., target pose); the relative distance measured by UWB and the relative pose obtained by visual reconstruction are used as factors, combined with robot odometry factors for factor graph fusion optimization, to calculate and output the relative estimated pose between robots; the relative estimated pose is then sent to the robot controller.

[0174] Reference Figure 7 The diagram illustrates a flowchart of another optional powertrain and base support component assembly control method provided in some embodiments of this application, wherein the robot end effector is equipped with a camera and a UWB module.

[0175] Specifically, it may include the following steps: Step 701: Obtain the joint angle data of the robot, determine the pose of the robot end effector based on the joint angle data, and determine the theoretical extrinsic parameters of the camera by combining the pose of the robot end effector and the fixed installation relationship between the camera and the robot end effector.

[0176] Step 702: Determine the actual external parameters of the camera based on the world coordinates of the preset markers in the assembly station and the image coordinates of the markers in the image data acquired by the camera.

[0177] Step 703: Combine the theoretical extrinsic parameters and the actual extrinsic parameters to obtain the calibrated extrinsic parameters.

[0178] Step 704: Obtain the measured pose difference of the robot.

[0179] Step 705: Obtain the measurement data of the UWB module, and determine the relative measurement distance between any two robots based on the measurement data of the UWB module.

[0180] Step 706: Based on the image data acquired by the camera and the camera's intrinsic and extrinsic parameters, construct a three-dimensional model of the powertrain, and determine the measurement pose of the powertrain based on the three-dimensional model.

[0181] Step 707: Determine the measurement pose of the robot relative to the powertrain based on the extrinsic parameters of the camera and the measurement pose of the powertrain, and determine the relative measurement pose between any two robots based on the measurement pose of the robot relative to the powertrain.

[0182] Step 708: Determine the relative estimated pose of any two robots based on the measured pose difference between any two robots and the relative measured pose and relative measured distance between any two robots, and perform assembly control on the robots based on the relative estimated pose of any two robots.

[0183] In this embodiment, the theoretical extrinsic parameters of the camera are calculated using the robot's own joint angle data, and the actual extrinsic parameters are obtained by combining the visual calculation of the preset markers on the ground. The two are then fused to obtain the calibrated camera extrinsic parameters, so that the camera can still obtain relatively accurate extrinsic parameters when the robot moves and vibrates, avoiding deviations in subsequent 3D reconstruction and pose calculation due to vibration.

[0184] By acquiring the robot's measured pose difference, the relative distance between robots measured by UWB, and the powertrain's measured pose reconstructed from camera images, the relative measured pose between the robots is derived. By fusing and optimizing the measured pose difference, relative measured pose, and relative measured distance, the estimated relative pose of the robots is obtained and used to control robot assembly. This allows the system to output accurate relative poses between robots even in dynamic occlusion and vibration environments, avoiding collision damage caused by blind movement and reducing production costs.

[0185] It should be noted that, for the sake of simplicity, the method embodiments are all described as a series of actions. However, those skilled in the art should understand that the embodiments of this application are not limited to the described order of actions, because according to the embodiments of this application, some steps can be performed in other orders or simultaneously. Secondly, those skilled in the art should also understand that the embodiments described in the specification are all preferred embodiments, and the actions involved are not necessarily required by the embodiments of this application.

[0186] Reference Figure 8 The diagram shows a schematic of the structure of an optional powertrain and basic load-bearing component assembly control device provided in some embodiments of this application, wherein the robot end effector is equipped with a camera and a UWB module.

[0187] Specifically, it can include the following modules: The pose difference acquisition module 801 is used to acquire the pose difference of the robot. The relative measurement distance determination module 802 is used to acquire the measurement data of the UWB module and determine the relative measurement distance between any two robots based on the measurement data of the UWB module. The measurement pose determination module 803 is used to construct a three-dimensional model of the powertrain based on the image data acquired by the camera and the camera's intrinsic and extrinsic parameters, and to determine the measurement pose of the powertrain based on the three-dimensional model. The relative measurement pose determination module 804 is used to determine the measurement pose of the robot relative to the powertrain based on the extrinsic parameters of the camera and the measurement pose of the powertrain, and to determine the relative measurement pose between any two robots based on the measurement pose of the robot relative to the powertrain. The assembly control module 805 is used to determine the relative estimated pose of any two robots based on the measured pose difference between any two robots and the relative measured pose and relative measured distance between any two robots, and to perform assembly control on the robots based on the relative estimated pose of any two robots.

[0188] In some embodiments of this application, the assembly control module 805 includes: A factor graph construction submodule is used to construct a factor graph; wherein, the factor graph includes multiple factors and multiple pose variables, the multiple factors include: an odometry factor with the measured pose difference as the measured value, a UWB factor with the relative measured distance as the measured value, and a visual factor with the relative measured pose as the measured value; the odometry factor connects the pose variables of the same robot at adjacent time points, and the UWB factor and the visual factor respectively connect the pose variables of different robots at the same time point; The pose variable optimization submodule is used to iteratively optimize the robot's pose variables in the factor graph based on the residuals between the measured values ​​and the corresponding estimated values ​​of each factor, as well as the pre-set noise standard deviation. The relative pose estimation determination submodule is used to determine the relative estimated pose of any two robots from the optimized pose variables.

[0189] In some embodiments of this application, the pose variable optimization submodule includes: The first cost term construction unit is used to construct a cost term for the odometer factor based on the residual between the measured value and the corresponding estimated value of the odometer factor, and the noise standard deviation of the odometer factor. The second cost term construction unit is used to construct the cost term of the UWB factor based on the residual between the measured value and the corresponding estimated value of the UWB factor, and the noise standard deviation of the UWB factor. The third cost term construction unit is used to construct a cost term for the visual factor based on the residual between the measured value and the corresponding estimated value of the visual factor, and the noise standard deviation of the visual factor. An optimization unit is used to iteratively optimize the robot's pose variables in the factor graph with the goal of minimizing the sum of the cost terms of the odometry factor, UWB factor, and vision factor.

[0190] In some embodiments of this application, the pose determination module 803 includes: The semantic mask generation submodule is used to perform semantic segmentation on the image data acquired by the camera and generate a semantic mask. The 3D model construction submodule is used to render the Gaussian sphere set according to the intrinsic and extrinsic parameters of the camera, and iteratively optimize and update the parameters of the Gaussian sphere set according to the semantic mask to construct the 3D model of the powertrain; wherein, when the image data is the first frame image data, the Gaussian sphere set is generated according to the preset CAD powertrain model point cloud, and when the image data is the Nth frame image data, the Gaussian sphere set is the Gaussian sphere set optimized using the N-1th frame image data, where N is a positive integer greater than 1; The pose determination submodule is used to filter the point cloud of the powertrain from the three-dimensional model and register the filtered point cloud with the preset CAD powertrain model point cloud to obtain the measured pose of the powertrain.

[0191] In some embodiments of this application, the three-dimensional model construction submodule includes: The rendering image determination unit is used to render the Gaussian sphere set according to the intrinsic and extrinsic parameters of the camera to obtain a rendered image; The loss function value determination unit is used to determine the loss function value based on the sum of squared pixel differences between the rendered image and the image data acquired by the camera, and the weights corresponding to the semantic mask to which each pixel belongs; wherein, the semantic mask is used to distinguish the powertrain region, the basic load-bearing component region, and the background region in the image data, and the weights of pixels in the powertrain region and the basic load-bearing component region are higher than the weights of pixels in the background region; The model building unit is used to construct a three-dimensional model of the powertrain by iteratively optimizing the parameters of the Gaussian sphere set using the gradient descent method with the goal of minimizing the loss function value.

[0192] In some embodiments of this application, the apparatus further includes: A feature hash table construction module is used to construct a feature hash table; wherein, the feature hash table includes a multi-layer grid, each layer of the grid has an independent feature hash table, and the feature hash table of each layer of the grid is used to store spatial point feature vectors of a preset dimension; The Gaussian sphere rendering module is used to query the feature hash table based on the coordinates of spatial points during the rendering process of the Gaussian sphere set, and to concatenate the feature vectors of spatial points obtained from the feature hash table of each layer of the grid to obtain the total feature vector of the spatial points, so as to perform rendering based on the total feature vector and the parameters of the Gaussian sphere set. The feature hash table update module is used to iteratively optimize and update the feature hash table during the process of iteratively optimizing and updating the parameters of the Gaussian sphere set.

[0193] In some embodiments of this application, the intrinsic parameters of the camera are obtained through offline calibration, and the extrinsic parameters of the camera are obtained through online calibration; before acquiring the measured pose difference of the robot, the device further includes: The theoretical extrinsic parameter determination module is used to acquire the joint angle data of the robot, determine the pose of the robot end effector based on the joint angle data, and determine the theoretical extrinsic parameters of the camera by combining the pose of the robot end effector and the fixed installation relationship between the camera and the robot end effector. The actual extrinsic parameter determination module is used to determine the actual extrinsic parameters of the camera based on the world coordinates of the preset markers in the assembly station and the image coordinates of the markers in the image data acquired by the camera. The extrinsic parameter calibration module is used to combine the theoretical extrinsic parameters and the actual extrinsic parameters to obtain the calibrated extrinsic parameters.

[0194] In some embodiments of this application, the relative measurement distance determination module 802 includes: The measurement position determination submodule is used to obtain multiple measurement distances between the UWB module and multiple UWB base stations fixedly set in the assembly station, and determine the measurement position of the robot based on the multiple measurement distances and the preset world coordinates of the multiple UWB base stations. The distance determination submodule is used to determine the relative measurement distance between any two robots based on their measured positions.

[0195] Some embodiments of this application also provide an electronic device, including a processor, a memory, and a computer program stored in the memory and capable of running on the processor, wherein the computer program, when executed by the processor, implements the method described above.

[0196] Optionally, the electronic device includes a computer-readable storage medium, or the electronic device includes a computer program product.

[0197] Some embodiments of this application also provide a computer-readable storage medium on which a computer program is stored, and when the computer program is executed by a processor, it implements the method described above.

[0198] Some embodiments of this application also provide a computer program product, including a computer program that, when executed by a processor, implements the method described above.

[0199] As the device embodiment is basically similar to the method embodiment, the description is relatively simple, and relevant parts can be found in the description of the method embodiment.

[0200] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. The same or similar parts between the various embodiments can be referred to each other.

[0201] Those skilled in the art will understand that embodiments of this application can be provided as methods, apparatus, or computer program products. Therefore, embodiments of this application can take the form of entirely hardware embodiments, entirely software embodiments, or embodiments combining software and hardware aspects. Furthermore, embodiments of this application can take the form of computer program products implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0202] This application describes embodiments with reference to flowchart illustrations and / or block diagrams of methods, terminal devices (systems), and computer program products according to embodiments of this application. It should be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing terminal device to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing terminal device, generate instructions for implementing the flowchart illustrations. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0203] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing terminal device to operate in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0204] These computer program instructions can also be loaded onto a computer or other programmable data processing terminal equipment, causing a series of operational steps to be performed on the computer or other programmable terminal equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable terminal equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0205] Although preferred embodiments of the present application have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments as well as all changes and modifications falling within the scope of the embodiments of the present application.

[0206] Finally, it should be noted that in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or terminal device that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or terminal device. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or terminal device that includes the aforementioned element.

[0207] The above provides a detailed description of the method, system, and device for controlling the assembly of the powertrain and the foundation load-bearing components. Specific examples have been used to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the method and core ideas of this application. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of this application. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A method for controlling the assembly of a powertrain and its supporting components, characterized in that, The assembly station is equipped with multiple robots, each robot having a camera and a UWB module at its end effector. The method includes: Obtain the measured pose difference of the robot; Acquire the measurement data from the UWB module, and determine the relative measurement distance between any two robots based on the measurement data from the UWB module; Based on the image data acquired by the camera and the camera's intrinsic and extrinsic parameters, a three-dimensional model of the powertrain is constructed, and the measurement pose of the powertrain is determined based on the three-dimensional model. Based on the extrinsic parameters of the camera and the measured pose of the powertrain, the measured pose of the robot relative to the powertrain is determined, and based on the measured pose of the robot relative to the powertrain, the relative measured pose between any two robots is determined. Based on the measured pose difference between any two robots and the relative measured pose and relative measured distance between any two robots, the relative estimated pose of any two robots is determined, and the robots are assembled and controlled based on the relative estimated pose of any two robots.

2. The method according to claim 1, characterized in that, Determine the relative estimated pose of any two robots based on the measured pose difference between any two robots and the relative measured pose and relative measured distance between any two robots, including: Construct a factor graph; wherein the factor graph includes multiple factors and multiple pose variables, the multiple factors including: an odometry factor with the measured pose difference as the measured value, a UWB factor with the relative measured distance as the measured value, and a visual factor with the relative measured pose as the measured value; the odometry factor connects the pose variables of the same robot at adjacent time points, and the UWB factor and the visual factor respectively connect the pose variables of different robots at the same time point; Based on the residuals between the measured values ​​and the corresponding estimated values ​​of each factor, and the pre-set noise standard deviation, the robot's pose variables in the factor graph are iteratively optimized. From the optimized pose variables, determine the relative estimated pose of any two robots.

3. The method according to claim 2, characterized in that, Based on the residuals between the measured values ​​and corresponding estimated values ​​of each factor, and a pre-set noise standard deviation, the robot's pose variables in the factor graph are iteratively optimized, including: Based on the residual between the measured value and the corresponding estimated value of the odometer factor, and the noise standard deviation of the odometer factor, a cost term for the odometer factor is constructed. Based on the residual between the measured value and the corresponding estimated value of the UWB factor, and the noise standard deviation of the UWB factor, a cost term for the UWB factor is constructed. Based on the residual between the measured value and the corresponding estimated value of the visual factor, and the noise standard deviation of the visual factor, a cost term for the visual factor is constructed. With the goal of minimizing the sum of the cost terms of the odometry factor, UWB factor, and vision factor, the robot's pose variables in the factor graph are iteratively optimized.

4. The method according to any one of claims 1-3, characterized in that, Based on the image data acquired by the camera and the camera's intrinsic and extrinsic parameters, a three-dimensional model of the powertrain is constructed, and based on the three-dimensional model, the measurement pose of the powertrain is determined, including: The image data acquired by the camera is semantically segmented to generate a semantic mask; The Gaussian sphere set is rendered based on the camera's intrinsic and extrinsic parameters, and the parameters of the Gaussian sphere set are iteratively optimized and updated based on the semantic mask to construct a three-dimensional model of the powertrain. Specifically, when the image data is the first frame of image data, the Gaussian sphere set is generated based on a preset CAD powertrain model point cloud; when the image data is the Nth frame of image data, the Gaussian sphere set is an optimized Gaussian sphere set using the (N-1)th frame of image data, where N is a positive integer greater than 1. The point cloud of the powertrain is selected from the three-dimensional model, and the selected point cloud is registered with the preset CAD powertrain model point cloud to obtain the measurement pose of the powertrain.

5. The method according to claim 4, characterized in that, Based on the intrinsic and extrinsic parameters of the camera, the Gaussian sphere set is rendered, and the parameters of the Gaussian sphere set are iteratively optimized and updated according to the semantic mask to construct a three-dimensional model of the powertrain, including: Based on the intrinsic and extrinsic parameters of the camera, the Gaussian sphere set is rendered to obtain a rendered image; The loss function value is determined based on the sum of squared pixel differences between the rendered image and the image data acquired by the camera, and the weights corresponding to the semantic masks to which each pixel belongs; wherein, the semantic mask is used to distinguish the powertrain region, the basic load-bearing component region, and the background region in the image data, and the weights of pixels in the powertrain region and the basic load-bearing component region are higher than the weights of pixels in the background region. With the goal of minimizing the loss function value, the gradient descent method is used to iteratively optimize the parameters of the Gaussian sphere set in order to construct a three-dimensional model of the powertrain.

6. The method according to claim 4, characterized in that, Also includes: Construct a feature hash table; wherein the feature hash table includes multiple layers of grids, each layer of grids has an independent feature hash table, and the feature hash table of each layer of grids is used to store spatial point feature vectors of a preset dimension; During the rendering of the Gaussian sphere set, the feature hash table is queried according to the coordinates of the spatial points, and the feature vectors of the spatial points obtained from the feature hash table of each layer of the grid are concatenated to obtain the total feature vector of the spatial points, so as to perform rendering based on the total feature vector and the parameters of the Gaussian sphere set; During the iterative optimization and updating of the parameters of the Gaussian sphere set, the feature hash table is iteratively optimized and updated.

7. The method according to any one of claims 1-3, characterized in that, The camera's intrinsic parameters are obtained through offline calibration, and the camera's extrinsic parameters are obtained through online calibration; before obtaining the robot's measured pose difference, the method further includes: The joint angle data of the robot is obtained, the pose of the robot end effector is determined based on the joint angle data, and the theoretical extrinsic parameters of the camera are determined by combining the pose of the robot end effector and the fixed installation relationship between the camera and the robot end effector. The actual external parameters of the camera are determined based on the world coordinates of the preset markers within the assembly station and the image coordinates of the markers in the image data acquired by the camera. By combining the theoretical extrinsic parameters and the actual extrinsic parameters, the calibrated extrinsic parameters are obtained.

8. The method according to any one of claims 1-3, characterized in that, Based on the measurement data from the UWB module, determine the relative measurement distance between any two robots, including: The robot's measurement position is determined based on the multiple measurement distances between the UWB module and multiple UWB base stations fixedly installed in the assembly station. Determine the relative measurement distance between any two robots based on their measured positions.

9. A system for the combined control of a powertrain and its supporting components, characterized in that, The system includes: Multiple robots are set up at the assembly station, and each robot is equipped with a camera and a UWB module at its end effector. Multiple UWB base stations are installed at the assembly station; The synchronization controller, which connects to the hardware trigger lines of all cameras, is used to control the synchronized exposure of all cameras. An industrial computer for performing the method as described in any one of claims 1 to 8.

10. A device for controlling the combined assembly of a powertrain and a foundation-supporting component, characterized in that, The assembly station is equipped with multiple robots, each with a camera and a UWB module at its end effector. The device includes: A pose difference acquisition module is used to acquire the pose difference of the robot. The relative measurement distance determination module is used to acquire the measurement data of the UWB module and determine the relative measurement distance between any two robots based on the measurement data of the UWB module. The measurement pose determination module is used to construct a three-dimensional model of the powertrain based on the image data acquired by the camera and the camera's intrinsic and extrinsic parameters, and to determine the measurement pose of the powertrain based on the three-dimensional model. The relative measurement pose determination module is used to determine the measurement pose of the robot relative to the powertrain based on the extrinsic parameters of the camera and the measurement pose of the powertrain, and to determine the relative measurement pose between any two robots based on the measurement pose of the robot relative to the powertrain. The assembly control module is used to determine the relative estimated pose of any two robots based on the measured pose difference between any two robots and the relative measured pose and relative measured distance between any two robots, and to perform assembly control on the robots based on the relative estimated pose of any two robots.

Images