Hydraulic mechanical arm structure control method and system based on data fusion

Abstract

The application provides a hydraulic mechanical arm structure control method and system based on data fusion, which first initializes the mechanical arm structure and unifies the global coordinate system, then collects multi-source heterogeneous sensor data in real time, carries out data fusion through an unscented Kalman filtering algorithm, and obtains real-time high-dimensional state estimation of the mechanical arm structure. According to the construction task requirements, obstacle information and space constraint conditions of the construction area, the system uses a path generation algorithm to plan the expected trajectory of the end effector in the world coordinate system. Finally, combined with the real-time high-dimensional state estimation and the expected trajectory, an operation space dynamics-based control law is applied to generate optimal control instructions for driving the mechanical arm frame and base, so that accurate control is realized. The application effectively improves the operation precision and adaptability of the mechanical arm in a complex environment.

Description

Technical Field

[0001] This invention belongs to the field of engineering intelligent control technology, specifically relating to a hydraulic robotic arm structure control method and system based on data fusion. Background Technology

[0002] In the field of contemporary construction engineering, the precise control of automated hydraulic robotic arms has always been a critical technical challenge that urgently needs to be addressed. Especially in complex and ever-changing construction environments, the motion accuracy, trajectory planning rationality, and adaptability of the equipment directly affect construction quality and efficiency. Traditional control methods often rely on a single information source for decision-making, failing to comprehensively perceive the equipment status and environmental conditions, leading to problems such as insufficient control precision, unreasonable trajectory planning, and poor adaptability to complex environments. These problems are particularly prominent in construction scenarios requiring high precision, such as concrete pouring and other processes demanding precise positioning and smooth movement.

[0003] Modern construction equipment is typically equipped with multiple sensors, but effectively integrating these heterogeneous data sources to construct an accurate equipment state model and achieve intelligent control based on it has always been a technical challenge. Traditional methods often process sensor data in isolation, failing to fully exploit the complementarity and synergy between the data, leading to inaccurate state estimation and consequently affecting control effectiveness. Summary of the Invention

[0004] This invention provides a hydraulic robotic arm structure control method and system based on data fusion to solve the problem of poor control effect of robotic arm structure.

[0005] In a first aspect, the present invention provides a hydraulic robotic arm structure control method based on data fusion, which is applied to a robotic arm structure including a base and a robotic arm frame. The robotic arm frame is rotatably connected to the base, and the robotic arm frame includes several four-axis closed-loop mechanisms composed of hydraulic rods and connecting rods. An end effector is provided at the end of the robotic arm frame.

[0006] The method includes the following steps:

[0007] Initialize the robotic arm structure and use the base as a reference to unify the global coordinates of the base, robotic arm frame and end effector;

[0008] Real-time acquisition of multi-source heterogeneous sensor data of the robotic arm structure, and application of unscented Kalman filtering algorithm to fuse the multi-source heterogeneous sensor data to obtain real-time high-dimensional state estimation of the robotic arm structure;

[0009] Based on the construction task and the obstacle information and spatial constraints of the area to be constructed, a path generation algorithm is used to plan the desired trajectory of the end effector in the world coordinate system.

[0010] By combining real-time high-dimensional state estimation and desired trajectory, and applying control laws based on operation space dynamics, optimal control commands are generated to drive the robotic boom and base.

[0011] Optionally, initializing the robotic arm structure and unifying the global coordinates of the base, robotic arm, and end effector based on the base includes the following steps:

[0012] The initial base pose relative to the world coordinate system is determined by scanning multiple pre-set markers at the construction site using a lidar sensor installed on the base.

[0013] Read the initial angles of all four-axis closed-loop mechanisms and the initial strokes of all hydraulic rods;

[0014] The initial end effector pose relative to the base is calculated based on a complete forward kinematics model including DH parameters and closed-loop constraints, combined with initial angles and initial formation.

[0015] The initial poses of the base, robotic arm, and end effector in the world coordinate system are calculated by combining the initial base pose and the initial end effector pose and using the coordinate transformation superposition principle.

[0016] Optionally, the multi-source heterogeneous sensor data includes base navigation data, end effector IMU data, joint encoder data of the four-axis closed-loop mechanism, and displacement sensor data of the hydraulic rod.

[0017] Optionally, real-time acquisition of multi-source heterogeneous sensor data of the robotic arm structure, and application of an unscented Kalman filter algorithm to fuse the multi-source heterogeneous sensor data to obtain a real-time high-dimensional state estimate of the robotic arm structure includes the following steps:

[0018] Optionally, real-time acquisition of multi-source heterogeneous sensor data from the robotic arm structure;

[0019] Define the base pose and velocity in the world coordinate system, the pose and velocity of the end effector, the angle and angular velocity of all four-axis closed-loop mechanisms, the stroke and extension speed of all hydraulic rods, and the state vector of the IMU zero bias.

[0020] An unscented Kalman filter algorithm is used, and a nonlinear state transition model based on system kinematics is employed to perform a state prediction step on the state vector to generate a priori state estimate.

[0021] By combining multi-source heterogeneous sensor data and prior state estimation, the posterior estimate is output as a real-time high-dimensional state estimate of the robotic arm structure through the measurement update step of the unscented Kalman filter algorithm.

[0022] Optionally, based on the construction task and the obstacle information and spatial constraints of the area to be constructed, the path generation algorithm is used to plan the desired trajectory of the end effector in the world coordinate system, including the following steps:

[0023] Based on the construction task and the obstacle information and spatial constraints of the area to be constructed, a path generation algorithm is used to plan the smooth and continuous desired attitude path of the end effector in the world coordinate system.

[0024] The desired attitude path of the end effector is planned to always satisfy the constraint that the Z-axis of the end effector is aligned with the Z-axis of the world coordinate system, and the desired pose, desired velocity and desired acceleration trajectory of the end effector are output.

[0025] Optionally, combining real-time high-dimensional state estimation and desired trajectory with the application of a control law based on operation space dynamics to generate optimal control commands for driving the robotic boom and base includes the following steps:

[0026] Calculate the pose error and the derivative of the pose error between the actual pose and the desired pose of the current end effector in the world coordinate system;

[0027] Using the desired acceleration trajectory as feedforward, the desired end effector's corrected acceleration command in the world coordinate system is calculated by using a task space PID controller and combining the pose error and the corresponding pose error derivative.

[0028] Using the initial base pose from real-time high-dimensional state estimation, the corrected acceleration command in the world coordinate system is transformed into the base coordinate system;

[0029] After coordinate transformation, the optimal control commands are generated by applying control laws based on operational space dynamics to drive the robotic arm and base and maintain the robotic arm, which includes hydraulic rods, in a dynamic pressure-holding state.

[0030] Optionally, after coordinate transformation, applying a control law based on operational space dynamics to generate optimal control commands that drive the robotic arm and base and maintain the robotic arm, including the hydraulic rods, in a dynamic pressure-holding state includes the following steps:

[0031] Based on real-time high-dimensional state estimation, the extended Jacobian matrix under the current configuration and the model parameters of the complete forward dynamic model are calculated in real time. The model parameters include the extended inertia matrix, Coriolis matrix, centrifugal force matrix and gravity vector considering the base attitude.

[0032] By applying the operational space dynamics control law and combining the corrected acceleration command and model parameters in the base coordinate system, the operational space control force required to counteract nonlinear dynamic effects and generate the desired acceleration in the base coordinate system is calculated.

[0033] By extending the transpose of the Jacobian matrix, the control force in the operating space is mapped into the basic control commands that drive the robotic arm structure.

[0034] Based on redundancy analysis of the robotic arm, the basic control commands are optimized into the optimal control commands.

[0035] Optionally, optimizing the basic control commands into optimal control commands based on redundancy analysis of the robotic boom includes the following steps:

[0036] When the degrees of freedom of the robotic arm are greater than the degrees of freedom required by the task space, the control system is considered to have redundancy.

[0037] If the control system has redundancy, then under the condition of satisfying the main task tracking of the basic control command, the control components for optimizing secondary objectives are injected into the basic control command by using the null space projection of the extended Jacobian matrix, so as to obtain the optimal control command.

[0038] In a second aspect, the present invention also provides a hydraulic robotic arm structure control system based on data fusion, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the hydraulic robotic arm structure control method based on data fusion as described in the first aspect.

[0039] Thirdly, the present invention also provides a computer-readable storage medium storing instructions that, when executed by a processor, configure the processor to perform the data fusion-based hydraulic robotic arm structure control method described in the first aspect.

[0040] The beneficial effects of this invention are:

[0041] This invention significantly improves the control accuracy and intelligence level of a robotic arm structure through the innovative application of multi-source heterogeneous data fusion and advanced control algorithms, solving the technical problems of insufficient accuracy and poor adaptability of traditional control methods in complex environments. Compared with traditional single-sensor data processing methods, this invention uses an unscented Kalman filter algorithm to fuse multi-source heterogeneous sensor data, effectively overcoming the shortcomings of high noise and limited accuracy in single-sensor measurements, and constructing a more accurate real-time high-dimensional state model of the robotic arm, laying a solid foundation for precise control. In terms of trajectory planning, this invention automatically generates the optimal motion trajectory based on construction task requirements and environmental constraints, significantly improving the operating efficiency and safety of the robotic arm in complex environments, and avoiding the inefficiency and uncertainty of traditional manual path planning. In particular, in terms of control strategy, this invention establishes a control law based on operational space dynamics, fully considering the nonlinear dynamic characteristics of the robotic arm and environmental interaction factors, realizing closed-loop control from high-dimensional state estimation to optimal control commands, and greatly improving trajectory tracking accuracy and system stability. This globally optimized control method enables robotic arms to complete complex construction tasks more smoothly and accurately, reducing the need for manual intervention, lowering the skill requirements and labor intensity of operators, while improving construction quality and efficiency. It provides strong technical support for the widespread application of robotic arms in various complex construction scenarios. Attached Figure Description

[0042] Figure 1 This is a schematic diagram of the robotic arm structure in one embodiment of this application.

[0043] Figure 2 This is a schematic diagram of the robotic arm structure and hydraulic station in one embodiment of this application.

[0044] Figure 3 This is a schematic diagram of the hydraulic station in one embodiment of this application.

[0045] Figure 4 This is a flowchart illustrating a data fusion-based hydraulic robotic arm structure control method in one embodiment of this application.

[0046] Explanation of reference numerals in the attached figures:

[0047] 1. Mechanical boom; 2. Base; 3. Hydraulic rod; 4. End effector; 5. Hydraulic station; 6. Servo motor; 7. Gear pump; 8. Pressure transmitter; 9. Accumulator; 10. Return oil filter; 11. Inlet oil filter; 12. Oil tank; 13. Air cooling. Detailed Implementation

[0048] The technical solutions of the embodiments of this application will be clearly described below with reference to the accompanying drawings. 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 are within the scope of protection of this application.

[0049] The terms "first," "second," etc., used in the specification and claims of this application are used to distinguish similar objects and not to describe a specific order or sequence. It should be understood that such use of data can be interchanged where appropriate so that embodiments of this application can be implemented in orders other than those illustrated or described herein, and the objects distinguished by "first," "second," etc., are generally of the same class and the number of objects is not limited; for example, a first object can be one or more. Furthermore, in the specification and claims, "and / or" indicates at least one of the connected objects, and the character " / " generally indicates that the preceding and following objects are in an "or" relationship.

[0050] Figure 1 An advanced robotic arm structure was showcased, primarily composed of a robust base and a complex robotic arm. The base features an extended, trapezoidal design, ensuring excellent stability during operation. The robotic arm is rotatably connected to the base via a precision slewing bearing, enabling 360° horizontal rotation and significantly expanding the working range. The robotic arm employs a truss structure, composed of multiple connecting rods in a two-section layout. The first section is the main arm, utilizing a triangular truss structure with multiple support rods forming a stable force transmission system. The second section, the auxiliary arm, also uses a truss structure but is smaller in size to improve end-effector flexibility and precision. This design ensures both strength and rigidity of the robotic arm while reducing overall weight.

[0051] The robotic boom integrates several sets of four-axis closed-loop mechanisms, each consisting of precisely coordinated hydraulic rods and connecting rods. As shown in the diagram, the main hydraulic rods are located at key nodes of the robotic boom, including the connection between the base and the main boom, the connection between the main boom and the auxiliary boom, and inside the auxiliary boom. These hydraulic rods work in concert through a control system to achieve precise lifting, lowering, and extension movements of the robotic boom. The closed-loop mechanism design makes force transmission more efficient, while also improving the overall structural stability and load-bearing capacity. At the end of the robotic boom, a dedicated end effector is installed. As shown in the diagram, it adopts a circular design, facilitating the installation of different types of tools. The end effector is connected to the auxiliary boom through a precise connecting mechanism, enabling multi-degree-of-freedom movement to adapt to various complex working conditions. Multiple interfaces are also reserved around the actuator, allowing for the installation of various specialized tools, such as concrete placing devices, gripping devices, drill bits, and cutting tools, according to actual application requirements.

[0052] All joints in the entire robotic arm structure are connected using high-precision bearings to ensure smoothness and accuracy during movement. As can be seen in the diagram, reinforcing ribs and protective devices are installed at key connection points, effectively improving the structure's durability and safety. The robotic arm's control system achieves precise control of the arm's posture by adjusting the extension and retraction length and speed of each hydraulic rod. The system is also equipped with multiple sensors distributed at various key locations on the robotic arm to monitor its working status, load, and environmental parameters in real time, ensuring safe and reliable operation. This robotic arm structure design is suitable for various industrial scenarios, such as heavy-duty operating environments like construction, mining, and port loading and unloading, and can also be applied to precision operation fields, such as hazardous environment detection and precision assembly. Its modular design concept makes maintenance and upgrades simple and efficient, greatly extending the equipment's service life and improving economic benefits.

[0053] The robotic arm structure is equipped with a safety control system, which includes three main aspects: safety limit, safety locking, and safety monitoring. The safety limit is triggered by setting an absolute position sensor or an absolute angle sensor, which triggers the corresponding mechanism when the movement exceeds the preset value. The safety locking is achieved by locking the actuator through an electronic switching valve to ensure that the posture is maintained during movement or in an emergency. Safety monitoring is further subdivided into torque monitoring, pressure monitoring, and obstacle monitoring: Torque monitoring uses torque sensors to monitor the force on the robotic arm. When the force exceeds the safe range, or the pressure at the cylinder inlet / outlet and the pressure sensor at the pump station exceeds the maximum pressure range and reaches the maximum working range of the hydraulic system, the system will initiate braking and alarm. Pressure monitoring includes cylinder pressure monitoring and hydraulic station pressure monitoring. Cylinder pressure monitoring monitors the pressure in the inlet and outlet oil circuits and pressure sensor data, and determines the safe pressure range based on load, speed, and pressure loss. Once this range is exceeded or the hydraulic pipeline experiences rapid pressure loss, braking and alarm will be triggered. Hydraulic station pressure monitoring ensures that the pressure is maintained within the safe range during operation; otherwise, braking and alarm will also be triggered. Obstacle monitoring encompasses ultrasonic monitoring and video monitoring to detect and mitigate potential risks.

[0054] In one embodiment, the complete robotic arm structure control system may include a hydraulic station that provides core power, as shown in the reference. Figure 2 and Figure 3 The hydraulic power unit mainly consists of a servo motor that provides the core power and a hydraulic pump (such as...). Figure 3 The system consists of a gear pump (as shown), responsible for converting electrical energy into hydraulic energy; the oil tank serves as a storage container for the hydraulic oil and also functions to dissipate heat and settle impurities; to ensure the cleanliness of the oil, the system is equipped with a filter, such as... Figure 3The inlet filter shown is used for preliminary filtration before the pump draws in oil, while the return filter is used to clean the oil returning to the tank. Pressure sensors (including pressure sensors with displays and pressure transmitters) are used to monitor the pressure at key points in the system in real time and convert the pressure signal into an electrical signal output. The relief valve is an important pressure control and protection component that limits the maximum pressure of the system and prevents overload. The check valve ensures that the hydraulic oil can only flow in one direction in the pipeline and prevents backflow. The accumulator is used to store a certain amount of pressurized oil so that it can quickly release energy, absorb system pressure pulsations, or serve as an emergency power source when needed. The air-cooling device cools the high-temperature hydraulic oil through forced air cooling to maintain the oil within the normal operating temperature range. In addition, it also includes pipelines connecting various components and various valves used to control the direction, pressure, and flow of the oil, such as solenoid valves and shut-off valves, which together constitute the complete hardware system of the hydraulic power unit.

[0055] The hydraulic power unit's operation begins with hydraulic oil in the tank. A servo motor starts and drives a connected hydraulic pump (gear pump) to rotate at high speed. The pump draws hydraulic oil from the tank through an inlet filter, pressurizes it, and outputs high-pressure oil to the system pipeline. As the high-pressure oil flows towards the actuators, its pressure is monitored in real-time by a pressure sensor. If the system pressure exceeds a set value, a relief valve opens, returning excess pressurized oil to the tank, serving both pressure regulation and safety protection purposes. A check valve is installed at a critical location to prevent backflow, ensuring the oil flows in the intended direction. The accumulator absorbs pressure from the pipeline as needed by the system. Impacts and pulsations, or the need to replenish pressurized oil to the system in a short period of time; the return oil after the actuator completes its work, as well as the oil that may be released from the relief valve, will flow through the return oil pipeline. A portion of this oil (such as the path of "return oil from the cylinder → solenoid switch valve → relief valve → shut-off valve → air cooling") will enter the air cooling device for forced cooling to reduce the oil temperature. The cooled oil will then merge with other return oil and pass through the return oil filter for final filtration to remove contaminants generated during operation. The clean hydraulic oil will eventually return to the oil tank, completing a complete work cycle, thereby continuously and stably providing the required hydraulic power to external equipment.

[0056] The system's motion execution relies on a series of actuators, including servo cylinders with built-in stroke sensors for linear extension and retraction, and servo rotary actuators with built-in position feedback and driven by hydraulic motors for rotary motion. Additionally, there are electronic on / off valves to control oil circuit opening and closing, pressure sensors to monitor actuator load, and angle encoders to accurately provide feedback on joint angles. Servo valves, as key control elements, precisely regulate the flow and direction of hydraulic oil to the actuators. For precise positioning, the system is equipped with a positioning system, including LiDAR and tags, and a laser gyroscope, to acquire the robotic arm's precise position and attitude information within the workspace. Ensuring operational safety relies on a comprehensive safety system. The system integrates visual sensors for environmental perception, ultrasonic sensors for obstacle detection, and torque sensors for monitoring the forces acting on the joints of the robotic arm. The motion control of the robotic arm is handled by a motion controller, whose core functions include trajectory planning to generate smooth and efficient motion paths, as well as specific motion control. This part relies on real-time feedback from stroke sensors and angle encoders, and achieves precise closed-loop control through a stroke-angle mapping control algorithm. The coordinated operation of all subsystems is uniformly scheduled and coordinated by a central controller. Finally, the entire control system is mounted on a mobile chassis and operates on a precise boom structure, together forming a complete robotic arm structure control system.

[0057] Figure 4 This is a flowchart illustrating a data fusion-based hydraulic robotic arm structure control method in one embodiment. It should be understood that, although... Figure 4 The steps in the flowchart are shown sequentially as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified in this document, there is no strict order in which these steps are executed, and they can be performed in other orders. Furthermore, Figure 4 At least some steps in the process may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed alternately or in turn with other steps or at least a portion of the sub-steps or stages of other steps. For example Figure 4 As shown, the hydraulic robotic arm structure control method based on data fusion disclosed in this invention specifically includes the following steps:

[0058] I. Device Initialization:

[0059] S101. Initialize the robotic arm structure and use the base as a reference to unify the global coordinates of the base, robotic arm frame and end effector.

[0060] Initializing the robotic arm structure is the prerequisite and foundation for the operation of the entire control system. Its core objective is to establish a unified global coordinate system, enabling the positions and orientations of all components within the robotic arm structure to be represented and calculated within the same coordinate system. This process begins by using a lidar mounted on the base to scan pre-placed markers at the construction site. These markers are typically objects with defined geometric features, such as reflectors or specially shaped marks. The lidar measures the distance and angle of the markers relative to the base by emitting laser beams and receiving reflected signals. Based on this information, combined with the known positions of the markers in the world coordinate system, the initial pose matrix of the base relative to the world coordinate system can be calculated using the least squares method or other registration algorithms. This matrix contains the rotation matrix. Translation vector The complete representation is:

[0061]

[0062] Next, it is necessary to read the initial angles of all four-axis closed-loop mechanisms in the robotic arm. and the initial stroke of all hydraulic rods These data are typically acquired via encoders mounted at the joints and displacement sensors on the hydraulic rods. Based on these initial parameters, a complete forward kinematic model incorporating DH parameters and closed-loop constraints is used to calculate the initial pose of the end effector relative to the base. The DH parameter method is a standard method for describing the relative position and orientation between links; for each link, four parameters are defined: link length... Linkage torsion angle Linkage offset and joint angle For a four-axis closed-loop mechanism, closed-loop constraints also need to be considered, which are typically expressed as a set of geometric constraint equations. Using these parameters and constraints, the pose matrix of the end effector relative to the base can be calculated. .

[0063] Finally, by combining the initial base pose and the initial end effector pose, and utilizing the principle of coordinate transformation superposition, the initial poses of the base, robotic arm, and end effector in the world coordinate system are calculated. Specifically, the pose matrix of the end effector in the world coordinate system is... It can be obtained through the following matrix multiplication: Similarly, the pose of any point p on the robotic arm in the world coordinate system can be calculated through a similar coordinate transformation. This initialization process ensures that the position and orientation of all components in the robotic arm structure can be represented in a unified world coordinate system, providing accurate initial conditions for subsequent state estimation, path planning, and control. It eliminates inconsistencies between different coordinate systems and improves the positioning accuracy and control accuracy of the entire system.

[0064] II. Collect and import data:

[0065] S102. Real-time acquisition of multi-source heterogeneous sensor data of the robotic arm structure, and application of unscented Kalman filtering algorithm to fuse the multi-source heterogeneous sensor data to obtain real-time high-dimensional state estimation of the robotic arm structure.

[0066] Device pose: Device pose mainly refers to the pose of the mobile base, which includes real-time information such as position, orientation and attitude. It integrates multiple types of sensors and uses multi-data fusion algorithms to obtain high-precision real-time device pose information.

[0067] Absolute positioning: RTK, the device's position relative to the world map (xyz), with the highest accuracy at the centimeter level. It is greatly affected by the environment, and in special cases, mobile base stations need to be added, such as when the weather is bad, the base station is far away, or the signal is blocked, resulting in the inability to obtain a fixed solution and the accuracy cannot be met.

[0068] Relative positioning: Positioning tag and LiDAR solution, based on the mission map, setting positioning tags, and automatically scanning and calculating the relative position (xyz and direction) of the device based on LiDAR, with an accuracy of millimeters;

[0069] IMU (Inertial Measurement Unit): Overall attitude information during the movement of the equipment, including information such as the tilt, direction, speed, and acceleration of the moving base.

[0070] End effector pose:

[0071] IMU (Inertial Measurement Unit): This refers to an inertial navigation system installed on an end effector to monitor the attitude, velocity, and acceleration information of the end effector in real time.

[0072] Travel sensors: collect travel positions of each joint.

[0073] Angle sensor: Collects angle information of each joint.

[0074] In subsequent motion planning, both the forward and inverse kinematics of the robotic arm require the real-time pose information of the equipment. The robotic arm kinematics solution uses the equipment (mobile base) coordinate system as an intermediate coordinate system for transformation. Because the equipment pose is based on the uneven terrain of the construction site, real-time equipment pose information is necessary for calculation during the forward and inverse kinematics processes to ensure the end effector moves according to the planned trajectory; otherwise, significant errors will occur. The IMU attitude information on the end effector can be compared with the planned motion trajectory in real time.

[0075] 1. Calculate the pose of the end effector relative to the base coordinate system using the base pose information, DH parameters, and the angles and stroke positions of each joint, and then convert it to the world coordinate system, which is the forward kinematics solution of the robotic arm.

[0076] 2. The trajectory of each joint is calculated from the trajectory of the end effector, the pose information of the base and the DH parameters, i.e., the inverse kinematics solution of the robot arm.

[0077] In robotic arm control systems, real-time state estimation is a crucial step in achieving precise control. First, it's necessary to acquire multi-source heterogeneous sensor data from the robotic arm structure in real time. This data includes base navigation data (such as RTK positioning data), end effector IMU data (acceleration, angular velocity, and magnetic field strength), joint encoder data from the four-axis closed-loop mechanism (joint angles), and displacement sensor data from the hydraulic rods (hydraulic rod stroke). These sensors operate at different frequencies, providing measurements with varying accuracy and noise characteristics. Therefore, an efficient data fusion algorithm is needed to comprehensively process this heterogeneous data.

[0078] The Unscented Kalman Filter (UKF) algorithm is a state estimation method suitable for nonlinear systems. Compared to the traditional Extended Kalman Filter (EKF), UKF approximates the probability distribution after nonlinear transformation using a carefully selected set of sampling points (sigma points), avoiding errors introduced by linearization. In this application, a high-dimensional state vector x is first defined, containing the pose and velocity of the base station in the world coordinate system, the pose and velocity of the end effector, the angles and angular velocities of all four-axis closed-loop mechanisms, the stroke and extension speeds of all hydraulic rods, and the IMU zero bias. The state vector can be represented as:

[0079]

[0080] in, and These represent the quaternions representing the position and orientation of the base, respectively. and This indicates the linear velocity and angular velocity of the base. and The quaternions representing the position and orientation of the end effector and This indicates the linear velocity and angular velocity of the end effector. and This represents the angle and angular velocity of the i-th joint. and This represents the stroke and extension / retraction speed of the j-th hydraulic rod. and This indicates the zero bias of the IMU's accelerometer and gyroscope.

[0081] The UKF algorithm implementation consists of two main steps: state prediction and measurement update. In the state prediction step, a nonlinear state transition model based on the system's kinematics is used to predict the state vector. The state transition equation can be expressed as: Where f is a nonlinear state transition function, It is a control input. It is process noise, following a Gaussian distribution with zero mean and covariance Q. Specifically, the state transition function considers the kinematic constraints of the robotic arm structure, such as the relationship between joint angles and hydraulic rod stroke, and the relationship between base motion and end effector motion.

[0082] In the measurement update step, multi-source heterogeneous sensor data is combined with prior state estimates, and the posterior estimate is output using the measurement update formula of the UKF algorithm. The measurement equation can be expressed as: Where h is a nonlinear measurement function that maps the state vector to the sensor measurement space. The measurement noise follows a Gaussian distribution with zero mean and covariance R. The measurement update process includes calculating the Kalman gain, updating the state estimate, and updating the error covariance matrix. In this way, the UKF algorithm can effectively fuse multi-source heterogeneous sensor data, suppress noise and errors from each sensor, and provide real-time high-dimensional state estimation of the robotic arm structure. This state estimation is characterized by high accuracy and low latency, providing reliable state information for subsequent trajectory planning and control, and significantly improving the stability and robustness of the robotic arm control system.

[0083] III. Trajectory Planning: The trajectory is the path in the time dimension. The trajectory of the multi-joint and mobile base is planned based on the trajectory of the end effector. The trajectory of the end effector (the path under business and time constraints) output by the business planning system is imported. Based on the trajectory of the end effector and the actual pose of the current equipment, the motion trajectory of each joint and the mobile chassis is planned and handed over to the actuator to control each joint and the mobile chassis to move according to the trajectory in real time.

[0084] S103. Based on the construction task and the obstacle information and spatial constraints of the area to be constructed in the construction task, a path generation algorithm is used to plan the desired trajectory of the end effector in the world coordinate system.

[0085] Path planning is a crucial component of the robotic arm control system, aiming to generate an optimal path that meets the construction task requirements while avoiding obstacles. First, the specific requirements of the construction task need to be obtained, including the starting point, target point, intermediate transit points, and specific requirements for the end effector's posture. Simultaneously, obstacle information about the construction area is acquired using LiDAR, vision sensors, or a pre-established environmental map. These obstacles may be static (e.g., walls, pillars) or dynamic (e.g., mobile devices, personnel). Spatial constraints include limitations on the robotic arm's workspace, joint angle limitations, and constraints specific to the construction task.

[0086] Based on this information, a path generation algorithm is used to plan the desired trajectory of the end effector. Commonly used path planning algorithms include sampling-based methods (such as Fast Random Tree (RRT) and Probabilistic Path Map (PRM)) and optimization-based methods (such as the artificial potential field method and the A* algorithm). In this technical solution, the path generation algorithm needs to consider the kinematic constraints and dynamic characteristics of the robotic arm to generate a smooth and continuous desired posture path. Taking the RRT* algorithm as an example, its basic idea is to randomly sample in the configuration space and construct a tree structure from the starting point to the target point. The algorithm first randomly generates a sampling point in the configuration space, then finds the node in the tree closest to the sampling point, and attempts to expand a new node from that node towards the sampling point. If the new node does not collide with the surrounding environment, it is added to the tree. The RRT* algorithm also includes an important optimization step, namely, reselecting the parent node of the newly added node to ensure the optimality of the path. The specific implementation can be represented as follows:

[0087] 1. Initialize the root node of tree T as the starting point. .

[0088] 2. Repeat N times:

[0089] a. Randomly sample a point in the configuration space. .

[0090] b. Find the distance in the tree The nearest node .

[0091] c. From Towards Expand a new node in the direction .

[0092] d. If It will not collide with the environment:

[0093] i. Find The set of all nodes inside a sphere centered at radius r is formed. .

[0094] ii. From Select to make from the starting point to The node with the minimum path cost is used as The parent node.

[0095] iii. Add to tree T.

[0096] iv. Regarding Each node in If through arrive If the path cost is less than the current path cost, then reconnect. The parent node is .

[0097] 3. Find the distance from the target point in the tree. The nearest node constructs the path from the starting point to that node.

[0098] When planning the desired attitude path of an end effector, it is also necessary to enforce the constraint that the end effector's Z-axis is aligned with the Z-axis of the world coordinate system. This constraint is typically used for specific construction tasks, such as vertical drilling or vertical object placement. Mathematically, this constraint can be represented as the end effector attitude matrix. The third column (Z-axis direction vector) and the Z-axis unit vector of the world coordinate system Maintain consistency: .

[0099] To ensure the smoothness and continuity of the path, the original path is usually post-processed, such as using B-spline curves or polynomial interpolation for smoothing. The final output is the desired pose trajectory of the end effector in the world coordinate system. Expected velocity trajectory and expected acceleration trajectory These trajectory data will serve as input for subsequent control algorithms, guiding the robotic arm to move along a predetermined path. This path planning method generates end-effector trajector trajector trajectories that meet construction task requirements, avoid obstacles, and exhibit smooth motion, improving construction efficiency and safety. Simultaneously, the enforced attitude constraints ensure that the end-effector can perform specific construction tasks in the correct posture, such as vertical drilling, vertical material placement, or precise positioning.

[0100] IV. Execution of Motion Control:

[0101] S104. Combine real-time high-dimensional state estimation and desired trajectory, and apply control laws based on operation space dynamics to generate optimal control commands to drive the robotic arm and base.

[0102] Among them, operation space dynamics-based control is an advanced control strategy that enables direct control within the operation space of the end effector, avoiding the complex inverse kinematics calculations of traditional joint space control. First, it is necessary to calculate the pose error between the actual and desired pose of the current end effector in the world coordinate system. Position error It can be calculated directly using vector subtraction: ,in It is the desired location. This is the actual position. Attitude error. Then we need to consider the properties of rotation matrices or quaternions, and can use logarithmic mapping for calculation: ,in It is the desired pose matrix. This is the actual attitude matrix. The pose error derivative includes the position error derivative. and attitude error derivative ,in and These are the desired linear velocity and angular velocity, respectively. and These are the actual linear velocity and angular velocity, respectively.

[0103] Next, with the desired acceleration trajectory As a feedforward, the desired corrected acceleration command for the end effector in the world coordinate system is calculated using a task-space PID controller, combined with the pose error and its corresponding derivative. The output of a PID controller can be expressed as:

[0104]

[0105] in, , and These are the proportional, differential, and integral gain matrices, respectively. It is the pose error vector. It is the vector of the pose error derivative.

[0106] Since operational space dynamics control needs to be performed in the base coordinate system, it is necessary to utilize the base pose from real-time high-dimensional state estimation to transform the corrected acceleration commands from the world coordinate system to the base coordinate system. Coordinate transformation can be achieved using a rotation matrix. accomplish:

[0107]

[0108] After coordinate transformation, optimal control commands for driving the robotic boom and base are generated using control laws based on operational space dynamics. First, the extended Jacobian matrix under the current configuration is calculated in real-time based on real-time high-dimensional state estimation. And the model parameters of the complete forward dynamics model, including the extended inertia matrix. Coriolis matrix Centrifugal force matrix and gravity vector considering base attitude Extended Jacobian matrix The mapping relationship between joint velocity and end effector velocity is described, considering the influence of base motion on end effector motion. The operational space dynamics control law can be expressed as:

[0109]

[0110] in, It is the operation space inertia matrix. It is an operation space nonlinear term. It is the gravity term in the operating space. It is the joint velocity vector. It is an external force.

[0111] By extending the transpose of the Jacobian matrix, the operational space control force F is mapped into the basic control commands that drive the robotic arm structure. When the degrees of freedom of the robotic arm exceed the degrees of freedom required in the task space, the control system exhibits redundancy. In this case, while ensuring the primary task tracking meets the basic control commands, the null-space projection of the extended Jacobian matrix can be used to inject control components for optimizing secondary objectives into the basic control commands, thereby obtaining the optimal control commands. :

[0112]

[0113] Where I is the identity matrix, It is zero-space control force, used to optimize secondary objectives, such as avoiding joint limitations, optimizing the robotic arm configuration, and avoiding singular positions.

[0114] This control method, based on operational space dynamics, enables precise trajectory tracking of the end effector in the global coordinate system. It also fully utilizes the redundant degrees of freedom of the robotic arm to optimize secondary objectives, improving the performance and robustness of the control system. This method is suitable for complex robotic arm structures, effectively handling nonlinear dynamic effects and external disturbances, ensuring the precise execution of construction tasks. The control process begins by defining the operational plan in the global coordinate system, importing planning data containing the three-dimensional spatial trajectory and attitude of the end effector, as well as the preset path of the moving platform. In parallel, integrated sensors perform real-time positioning in the robotic arm coordinate system to obtain its current relative attitude and position, and forward kinematics is used to calculate the precise current coordinates of the end effector in real time. Subsequently, the end-effector coordinate analysis stage begins. Based on the forward kinematics model, it is determined whether the planned target end point is reachable from the current robotic arm configuration. If unreachable, it may trigger platform movement planning or adjust the end-effector trajectory to achieve reach through joint coordination, while simultaneously evaluating the global optimality of the selected path. The core motion planning steps are further refined into: trajectory planning for the mobile chassis, generating safe and efficient paths and evaluating their feasibility and optimality; and trajectory planning for the robotic arm joints, which relies on inverse kinematics. Based on the desired position and attitude sequence of the end effector in the world coordinate system, the required angle or displacement sequence of each joint is solved in reverse, and the existence and optimality of the solution are also verified.

[0115] The robustness and accuracy of the entire planning process benefit from a key "planning logic," the core of which is to dynamically handle inconsistencies between real-time perceived position data and preset planning data. Specifically, if the actual position of the current end effector is detected to be inconsistent with the starting point of the planned trajectory, the system will recalculate the subsequent trajectory using the current real-time end effector position as the new reference starting point. At the same time, if the actual position of the moving chassis deviates from the planned value, this deviation will be compensated when the robotic arm performs inverse kinematics calculations to calculate joint angles, ensuring that the end effector can accurately reach its target point in its world coordinate system. Finally, the accurate trajectory data of each joint (including position, velocity, and acceleration commands) generated by the compensated and optimized inverse kinematics solution is accurately transmitted to the underlying motion controller to drive each axis to strictly execute according to the plan, and a closed-loop control is formed through continuous sensor feedback, ensuring high precision and high stability of the motion task.

[0116] In actual control, dynamic pressure maintenance of the robotic arm can also be achieved. This capability is realized through comprehensive pressure maintenance control and efficient energy-saving control strategies, jointly ensuring the efficient, stable, and precise operation of the robotic arm's hydraulic system under various working conditions. At the pressure maintenance control level, both passive and active pressure maintenance mechanisms are cleverly integrated. The passive pressure maintenance mechanism mainly relies on the inherent characteristics of key hydraulic components. For example, by integrating an accumulator into the hydraulic system, pressure fluctuations caused by load changes or actuator movements can be effectively absorbed and released, acting as a hydraulic buffer to maintain relatively stable system pressure. Simultaneously, for each independent motion axis or key hydraulic circuit of the robotic arm, corresponding relief valves are carefully designed. These relief valves, as safety protection and pressure upper limit setting units, can automatically open when the pressure on a specific axis exceeds a preset safety threshold, guiding excess hydraulic oil back to the tank, thereby preventing system overpressure and ensuring the safety of all components. The active pressure holding mechanism embodies the system's high level of intelligence and dynamic response capability. Its core lies in using high-precision pressure sensors to continuously monitor the pressure in the hydraulic system in real time and feeding back the collected pressure data to the central controller. The controller then dynamically adjusts the output torque and speed of the servo pump according to the preset control algorithm. When the system pressure deviates from the target value, the servo pump can respond quickly, either increasing the output to compensate for the pressure loss or reducing the output to avoid excessive pressure, thereby achieving precise and active closed-loop control of the system pressure.

[0117] On the other hand, the energy-saving control strategy focuses on improving the system's energy utilization efficiency and reducing unnecessary energy consumption. This strategy intelligently adjusts the servo pump's operating mode and output power based on the real-time motion status of the robotic arm, including the motion and rotational speed information of its various joints. When the robotic arm performs high-load, high-speed tasks, the servo pump operates at full power to provide sufficient power; while when the robotic arm is in a non-moving state or in low-load standby mode, the system automatically switches the servo pump to the lowest energy consumption mode, maintaining only the basic pressure balance required by the system, greatly reducing energy waste. It is worth noting that the accuracy of dynamic pressure maintenance has a crucial impact on the final control accuracy of the entire hydraulic control system; any small deviation in pressure control can be amplified and ultimately reflected in the positioning and motion accuracy of the end effector. In addition, various dynamic factors, such as changes in the posture of the robotic arm itself, real-time changes in the load during operation, adjustments in the speed of the actuator, the impact of ambient temperature fluctuations on the viscosity of hydraulic oil, changes in friction between moving parts, and the stability of the hydraulic station's own operating conditions (such as oil temperature, oil cleanliness, pump efficiency, etc.), all pose challenges to the stability of hydraulic control. The system needs to have strong adaptability and robustness to overcome these uncertainties and ensure continuous and reliable control.

[0118] At the motion control level, highly precise coordination and control are implemented for both the robotic arm itself and its underlying mobile chassis to achieve accurate positioning and smooth movement in complex working environments. For speed control of the robotic arm, the first step relies on precise calibration of joint dynamics. Under simulated actual working conditions, the dynamic parameters of each joint axis of the robotic arm in different motion spaces and postures are comprehensively tested and recorded, including but not limited to key data such as its movement speed, required driving torque, and moment of inertia, laying the physical model foundation for subsequent precise control. Secondly, closely combining the specific structural characteristics and performance parameters of the hydraulic cylinders or hydraulic motors used on the robotic arm, the flow rate and corresponding pressure values ​​required to drive these actuators are accurately calculated using fluid mechanics and mechanical conversion principles. Based on this, the rated pressure gain parameters for each working state are further determined, while reserving a certain adjustment range to adapt to changes in working conditions. Throughout this process, pressure sensors play a crucial role, dynamically monitoring and feeding back the actual output torque and speed of the servo pump. The system dynamically adjusts the pressure setpoint accordingly, forming a collaborative operation of active and passive pressure holding mechanisms.

[0119] Furthermore, the system employs a simulation fitting algorithm based on multi-sensor data fusion. This means it can integrate information from different types of sensors (such as position, velocity, and torque sensors), calculate simulated PID (proportional-integral-derivative) control parameters using advanced algorithms, and dynamically adjust these parameters based on real-time feedback. This achieves precise closed-loop control of the angular velocities of each joint of the robotic arm and the linear travel speed of the actuators. This precise control extends to the optimization of simulation parameters, the coordination of multi-machine hybrid parameters (potentially referring to the collaborative working parameters of multiple actuators or subsystems), and the introduction of dynamic calculation parameters for velocity feedforward, to improve the system's response speed and dynamic performance. In addition, the system integrates advanced position control algorithms and precise mapping algorithms between travel and angle, ensuring a high degree of consistency between commanded position and actual position. Regarding trajectory planning, the robotic arm's motion control supports various velocity trajectory curves, such as T-shaped (trapezoidal acceleration / deceleration), S-shaped (smooth acceleration / deceleration), and more complex polynomial curves, allowing the operator to select the most suitable motion mode according to specific task requirements, such as fine-tuning in manual mode. To ensure smooth motion, the system employs a smooth interpolation algorithm, which involves in-depth application of robotic arm kinematics. Its design aims to address practical problems such as: the planned angular trajectory may not be perfectly smooth due to algorithm or sampling rate limitations; the inconsistency between the cycle period of the planning instructions (e.g., 100 milliseconds) and the execution cycle period of the underlying controller (e.g., 1 millisecond) can lead to discontinuous motion; and the planned trajectory data is typically based on joint angles, while the actual directly controllable data is the linear stroke of the hydraulic cylinder, creating a non-linear mapping between the two.

[0120] To address these issues, the system's trajectory planning steps include: first, accurately converting the planned angle trajectory into the stroke trajectory of each cylinder using inverse kinematics or a preset mapping relationship; then, further smoothing and interpolation operations are performed on the converted stroke trajectory to generate a more continuous and smooth actual execution trajectory. This refined trajectory processing ultimately achieves high-standard motion quality in both semi-automatic and fully automatic modes for both angle closed-loop control and stroke closed-loop control. For the mobile chassis carrying the robotic arm, its motion control also follows similar precise control logic, encompassing an independent speed control system and trajectory planning function, ensuring that the entire equipment system can function as a coordinated and unified whole, efficiently and safely completing various operational tasks in complex environments.

[0121] In one embodiment, initializing the robotic arm structure and unifying the global coordinates of the base, robotic arm frame, and end effector based on the base includes the following steps:

[0122] The initial base pose relative to the world coordinate system is determined by scanning multiple pre-set markers at the construction site using a lidar sensor installed on the base.

[0123] Read the initial angles of all four-axis closed-loop mechanisms and the initial strokes of all hydraulic rods;

[0124] The initial end effector pose relative to the base is calculated based on a complete forward kinematics model including DH parameters and closed-loop constraints, combined with initial angles and initial formation.

[0125] The initial poses of the base, robotic arm, and end effector in the world coordinate system are calculated by combining the initial base pose and the initial end effector pose and using the coordinate transformation superposition principle.

[0126] In this embodiment, the lidar measures the distance and angle of an object by emitting a laser beam and receiving the reflected signal. Multiple markers with clearly defined geometric features (such as reflectors or specially shaped markers) are pre-positioned at the construction site; the world coordinate system positions of these markers are known. When the lidar scans these markers, it acquires their polar coordinates relative to the base. ,in It's distance. and These are horizontal and vertical angles. These polar coordinates can be converted to Cartesian coordinates in the base coordinate system. .

[0127] The position of the known landmark in the world coordinate system A corresponding relationship can be established: , It is a rotation matrix. It is a translation vector. The transformation matrix is ​​solved using the least squares method: The pose matrix of the base relative to the world coordinate system is obtained. This step ensures that the robotic arm control system can accurately locate the initial position and orientation of the base in the world coordinate system, providing a basis for subsequent coordinate transformations.

[0128] The four-axis closed-loop mechanism is the core component of the robotic arm. Each closed-loop mechanism contains four rotary joints and connecting rods. The initial angle of each joint can be read using encoders mounted at the joints. (Where i = 1, 2, ..., n, and n is the total number of joints). Encoders typically operate based on photoelectric, magnetic, or resistive principles, converting angular changes into electrical signals. For example, a photoelectric encoder consists of a light source, a code disk, and a photodetector. As the code disk rotates with the joint, the changes in the light signal received by the photodetector are converted into angular information. The initial stroke of the hydraulic rod. (Where j = 1, 2, ..., m, and m is the total number of hydraulic rods) is obtained through displacement sensors mounted on the hydraulic rods. Commonly used displacement sensors include linear variable differential transformers (LVDTs), magnetostrictive displacement sensors, or resistive displacement sensors. These sensors convert the extension and retraction displacement of the hydraulic rods into electrical signals. After signal conditioning and analog-to-digital conversion, the precise stroke value of the hydraulic rods is obtained. These initial angles and stroke values ​​constitute the initial configuration of the robotic arm, providing the necessary input parameters for subsequent forward kinematics calculations, ensuring that the robotic arm control system can accurately understand the initial state of the robotic arm.

[0129] The DH parameter method is a standard method for describing the relative position and orientation between links in a robotic arm. For each link, four parameters are defined: link length... Linkage torsion angle Linkage offset and joint angle Based on these parameters, the transformation matrix from joint i-1 to joint i can be constructed:

[0130]

[0131] For a four-axis closed-loop mechanism, closed-loop constraints also need to be considered, which can be expressed as geometric constraint equations: Substituting the initial angle and hydraulic rod stroke into these equations, the transformation matrix from the base to the end effector is calculated through successive multiplication: ,in This is the transformation matrix from the last joint to the end effector. This yields the initial pose matrix of the end effector relative to the base, containing the position vector. and rotation matrix : This step ensures that the robotic arm control system can accurately calculate the initial position and orientation of the end effector relative to the base.

[0132] The principle of coordinate transformation superposition is a fundamental concept in robotics, used to transform pose representations from different coordinate systems to a unified coordinate system. Given the pose matrix of the base relative to the world coordinate system... and the pose matrix of the end effector relative to the base The pose matrix of the end effector in the world coordinate system can be calculated using matrix multiplication: After unfolding, the position vector of the end effector in the world coordinate system is: The rotation matrix is: Similarly, for any point p on the robotic arm, its position in the base coordinate system is... Then its position in the world coordinate system is: In this way, the positions and orientations of all components in the robotic arm structure (base, joints and links of the robotic arm, end effector) can be unified and represented in a world coordinate system. This step achieves the unification of the global coordinates of the robotic arm structure, providing accurate initial conditions for subsequent state estimation, path planning, and control, eliminating inconsistencies between different coordinate systems, and improving the positioning accuracy and control accuracy of the entire system.

[0133] In one implementation, real-time acquisition of multi-source heterogeneous sensor data of the robotic arm structure, and fusing the multi-source heterogeneous sensor data using an unscented Kalman filter algorithm to obtain a real-time high-dimensional state estimate of the robotic arm structure, includes the following steps:

[0134] Real-time acquisition of multi-source heterogeneous sensor data from the robotic arm structure;

[0135] Define the base pose and velocity in the world coordinate system, the pose and velocity of the end effector, the angle and angular velocity of all four-axis closed-loop mechanisms, the stroke and extension speed of all hydraulic rods, and the state vector of the IMU zero bias.

[0136] An unscented Kalman filter algorithm is used, and a nonlinear state transition model based on system kinematics is employed to perform a state prediction step on the state vector to generate a priori state estimate.

[0137] By combining multi-source heterogeneous sensor data and prior state estimation, the posterior estimate is output as a real-time high-dimensional state estimate of the robotic arm structure through the measurement update step of the unscented Kalman filter algorithm.

[0138] In this embodiment, various types of sensors are installed on the robotic arm structure to monitor its motion and environmental information in real time. Base navigation data is acquired via GPS or RTK positioning systems, providing the absolute position of the base in the world coordinate system with centimeter-level accuracy. Simultaneously, an IMU (Inertial Measurement Unit) on the base provides attitude and angular velocity information. The IMU installed on the end effector includes a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer, measuring linear acceleration, angular velocity, and geomagnetic field direction, respectively. The accelerometer measurement range is typically ±16g, the gyroscope measurement range is ±2000° / s, and the sampling frequency can reach 200Hz. Furthermore, a tag-based relative positioning measurement system with lidar enables the device's orientation and position accuracy to reach millimeter-level. Encoders are installed at the joints of the four-axis closed-loop mechanism, with a resolution typically of 4096 pulses / revolution, enabling precise measurement of joint angles. Displacement sensors (such as LVDTs or magnetostrictive sensors) on the hydraulic rods measure the stroke of the hydraulic rods with an accuracy of 0.1mm. These sensor data are transmitted to the central controller via communication protocols such as CAN bus, RS485, or Ethernet. The sampling period is set between 10-100ms depending on the control requirements. A timestamp synchronization mechanism is used during data acquisition to ensure the time consistency of data from different sensors. For occasional data loss or communication interruptions, data interpolation or extrapolation methods are used for compensation. By acquiring these multi-source heterogeneous sensor data in real time, comprehensive and reliable observation information is provided for subsequent state estimation, which is the foundation for achieving high-precision control.

[0139] A state vector is a mathematical representation describing the complete dynamic characteristics of a robotic arm structure, containing all key state variables of the system. The high-dimensional state vector x is defined as follows:

[0140]

[0141] in, This indicates the three-dimensional position of the base in the world coordinate system; It is a unit quaternion representing the base attitude. and This indicates the linear velocity and angular velocity of the base. and The quaternions representing the position and orientation of the end effector and This indicates the linear velocity and angular velocity of the end effector. and This represents the angle and angular velocity of the i-th joint. and This represents the stroke and extension / retraction speed of the j-th hydraulic rod. and This represents the zero bias of the IMU's accelerometer and gyroscope. This state representation method has several advantages: First, it contains complete motion information of the robotic arm structure, facilitating subsequent state estimation and control; second, using quaternions to represent attitude avoids the gimbal lock problem of Euler angles; finally, including the IMU zero bias in the state vector allows for online estimation and compensation of sensor errors, improving system accuracy.

[0142] The Unscented Kalman Filter (UKF) algorithm is an advanced state estimation method for nonlinear systems, approximating the probability distribution after nonlinear transformation by carefully selecting sigma points. First, the augmented state vector is defined. , where w and v are the process noise and measurement noise, respectively. Calculate the covariance matrix of the augmented state vector. Where P is the state estimation covariance, Q is the process noise covariance, and R is the measurement noise covariance. Generate 2L+1 sigma points (L is the dimension of the augmented state vector):

[0143]

[0144] in It is a scaling parameter. Controlling the distribution of sigma points, These are secondary scaling parameters. These sigma points are propagated through a nonlinear state transition function:

[0145]

[0146] The state transition function is based on the system's kinematic model and includes quaternion integration, angular velocity integration, and position updates. For example, the base position update is:

[0147]

[0148] Quaternion updates use quaternion differential equations:

[0149]

[0150] Achieved through discretization:

[0151]

[0152] Calculate the prior state estimate and covariance:

[0153]

[0154] The weighting coefficients are:

[0155]

[0156] parameter This is used to incorporate prior knowledge about the distribution. In this way, the UKF algorithm can effectively handle the nonlinear dynamic characteristics of the robotic arm structure and generate accurate prior state estimates.

[0157] The measurement update step combines sensor observation data with prior state estimates to generate a more accurate posterior state estimate. First, the prior sigma points... Mapping to the measurement space via the measurement function h:

[0158]

[0159] The measurement function varies depending on the sensor type. For example, for base-mounted GPS data, the measurement function simply extracts the position component from the state vector; for IMU data, the measurement function considers the relationship between gravity, acceleration, and angular velocity.

[0160]

[0161] Calculate the predicted measurements and their covariance:

[0162]

[0163] Calculate the cross-covariance between state and measurement:

[0164]

[0165] Calculate the Kalman gain:

[0166]

[0167] Update state estimates and covariance:

[0168]

[0169] in These are actual measured values. The quaternion part requires special handling to maintain the unit quaternion constraint. In this way, the UKF algorithm can effectively fuse multi-source heterogeneous sensor data, suppress noise and errors from each sensor, and provide real-time high-dimensional state estimation of the robotic arm structure. This provides reliable state information for subsequent trajectory planning and control, significantly improving the stability and robustness of the robotic arm control system.

[0170] In one implementation, the process of planning the desired trajectory of the end effector in the world coordinate system using a path generation algorithm, based on the construction task and the obstacle information and spatial constraints of the area to be constructed, includes the following steps:

[0171] Based on the construction task and the obstacle information and spatial constraints of the area to be constructed, a path generation algorithm is used to plan the smooth and continuous desired attitude path of the end effector in the world coordinate system.

[0172] The desired attitude path of the end effector is planned to always satisfy the constraint that the Z-axis of the end effector is aligned with the Z-axis of the world coordinate system, and the desired pose, desired velocity and desired acceleration trajectory of the end effector are output.

[0173] In this embodiment, path planning first requires defining the starting point, target point, and intermediate points of the construction task, and acquiring obstacle information of the construction area. Obstacle information can be obtained through a pre-established environmental map or real-time sensors (such as LiDAR or vision sensors), represented as a series of geometric shapes (such as cubes, cylinders, or polyhedra). Spatial constraints include the robotic arm's workspace limitations, joint angle limitations, and specific construction task requirements. The RRT* (Fast Random Tree Optimization) algorithm is used for path planning. This algorithm randomly samples in the configuration space to construct a tree structure from the starting point to the target point. After path generation, B-spline curves are used for smoothing to ensure the path is smooth and meets acceleration continuity requirements. In this way, a smooth and continuous end effector path that avoids obstacles, meets spatial constraints, and achieves the desired attitude can be generated, providing a foundation for subsequent trajectory tracking control.

[0174] Taking a concrete placement scenario as an example, in this scenario, the end effector (such as a placement pipe) needs to maintain a vertically downward orientation to ensure that the concrete flows out evenly and is placed accurately. This requires the Z-axis of the end effector (usually the tool axis) to be aligned with the Z-axis of the world coordinate system (usually pointing in the opposite direction to the sky). Mathematically, this constraint can be represented as the end effector attitude matrix. The third column (Z-axis direction vector) and the Z-axis unit vector of the world coordinate system parallel: Under this constraint, the end effector still retains one degree of freedom in its posture: rotation about the Z-axis. This rotation angle can be determined based on robotic arm configuration optimization or other task requirements. This will smooth the path. Combined with attitude constraints, a complete pose trajectory can be generated:

[0175]

[0176] in It is a pose quaternion that satisfies the Z-axis alignment constraint. This is achieved by analyzing the pose trajectory. By taking the first and second derivatives, we can obtain the desired velocity trajectory and the desired acceleration trajectory.

[0177]

[0178] in and These are the desired linear velocity and linear acceleration, respectively. and These are the desired angular velocity and angular acceleration, respectively. To ensure the smoothness of the trajectory, velocity and acceleration constraints can be set, such as the maximum linear velocity. Maximum angular velocity Maximum linear acceleration and maximum angular acceleration By adjusting the time scale, we ensure that the trajectory meets these constraints:

[0179]

[0180] like If the time scale is magnified by a factor of s, the resulting trajectory not only satisfies the Z-axis alignment constraint but also ensures the continuity of velocity and acceleration, providing precise and smooth control commands for the concrete placement process, effectively improving placement quality and construction efficiency.

[0181] In one implementation, the trajectory of the end effector can be planned based on the construction task, obstacle information, spatial constraints, and real-time pose information of the current equipment in the construction area. Then, based on the trajectory of the end effector, the motion trajectories of each joint and the mobile base of the equipment are planned. If there is no solution for planning the trajectory of each joint of the robotic arm, that is, there is no solution for the trajectory of each joint corresponding to the trajectory of the end effector, the trajectory of the mobile base and the motion trajectory of each joint of the equipment in the same world coordinates are guaranteed to meet the constraints of the motion trajectory of the end effector, obstacle avoidance (movement obstacle avoidance and posture obstacle avoidance), maximum performance constraints (performance parameters such as maximum speed and acceleration of each joint and mobile chassis), smoothness (acceleration smoothness), and optimal efficiency. Finally, real-time error compensation is performed between the actual pose and the desired pose in the initial state and the motion process state.

[0182] In one implementation, generating optimal control commands to drive the robotic boom and base by combining real-time high-dimensional state estimation and desired trajectory and applying a control law based on operation space dynamics includes the following steps:

[0183] Calculate the pose error and the derivative of the pose error between the actual pose and the desired pose of the current end effector in the world coordinate system;

[0184] Using the desired acceleration trajectory as feedforward, the desired end effector's corrected acceleration command in the world coordinate system is calculated by using a task space PID controller and combining the pose error and the corresponding pose error derivative.

[0185] Using the initial base pose from real-time high-dimensional state estimation, the corrected acceleration command in the world coordinate system is transformed into the base coordinate system;

[0186] After coordinate transformation, the optimal control commands are generated by applying control laws based on operational space dynamics to drive the robotic arm and base and maintain the robotic arm, which includes hydraulic rods, in a dynamic pressure-holding state.

[0187] In this embodiment, pose error calculation is a fundamental step in the control system, used to quantify the difference between the current pose and the target pose of the end effector. First, the actual pose of the end effector, including the position vector, is obtained from the real-time high-dimensional state estimation. and attitude quaternions Simultaneously, the desired pose, including the desired position, is obtained from the trajectory planning module. and expected posture quaternion Position error is calculated directly using vector subtraction: Attitude error calculation is relatively complex, so a quaternion difference method is used: ,in To represent quaternion multiplication, yes The conjugate quaternion. For ease of control, the quaternion error is converted into an axis-angle representation:

[0188]

[0189] The pose error derivative includes the position error derivative. and attitude error derivative ,in and These are the expected linear velocity and angular velocity. and These are the actual linear velocity and angular velocity. These errors and their derivatives will be used in subsequent controller calculations, providing a foundation for achieving accurate trajectory tracking.

[0190] The taskspace PID controller combines feedback control and feedforward control to generate corrective acceleration commands to eliminate pose errors. The control law expression is:

[0191]

[0192] in, It is the expected acceleration trajectory, used as a feedforward term; It is the pose error vector; It is the derivative of the pose error; It is the integral term of the pose error; , and These are the proportional, derivative, and integral gain matrices, typically diagonal matrices, which can be used to adjust the response characteristics of position and attitude control, respectively. Proportional term Provides a correction proportional to the error, shifting the system in the direction that reduces the error; differential term Provides damping to suppress overshoot and oscillation; integral term To eliminate static errors and improve the steady-state accuracy of the system, integral limiting or conditional integration strategies are typically employed to prevent integral saturation.

[0193] PID parameter adjustment follows the principle of adjusting first. Adjust Finally, adjust The principle is that attitude control parameters are typically smaller than position control parameters. In this way, the controller generates corrective acceleration commands. It can effectively track the desired trajectory while compensating for external disturbances and model uncertainties.

[0194] Since the dynamics and control of the robotic arm are typically expressed in the base coordinate system, it is necessary to convert the corrected acceleration commands from the world coordinate system to the base coordinate system. The pose of the base in the world coordinate system, including the position vector, is obtained from the real-time high-dimensional state estimation. and rotation matrix The rotation matrix describes the orientation of the base coordinate system relative to the world coordinate system, and its transpose... It can be used to transform vectors in the world coordinate system to the base coordinate system.

[0195] Correct acceleration command In the middle, linear acceleration and angular acceleration They need to be converted separately:

[0196]

[0197] After merging, the corrected acceleration command in the base coordinate system is obtained:

[0198]

[0199] This coordinate transformation takes into account the possible motion and attitude changes of the base, ensuring that control commands are executed in the correct coordinate system and providing accurate input for subsequent operation space dynamics-based control. This step is particularly crucial when the base is movable, as it links the globally planned trajectory with the locally executed control. Finally, optimal control commands for driving the robotic arm and base are generated based on the transformed coordinates and the control laws of operation space dynamics. Operation space dynamics-based control is an advanced control strategy that operates directly within the operation space of the end effector, avoiding complex inverse kinematics calculations. This control method effectively handles nonlinear dynamic effects and external disturbances, ensuring accurate trajectory tracking.

[0200] In one embodiment, after coordinate transformation, applying a control law based on operational space dynamics to generate optimal control commands that drive the robotic arm and base and maintain the robotic arm, including the hydraulic rod, in a dynamic pressure-holding state includes the following steps:

[0201] Based on real-time high-dimensional state estimation, the extended Jacobian matrix under the current configuration and the model parameters of the complete forward dynamic model are calculated in real time. The model parameters include the extended inertia matrix, Coriolis matrix, centrifugal force matrix and gravity vector considering the base attitude.

[0202] By applying the operational space dynamics control law and combining the corrected acceleration command and model parameters in the base coordinate system, the operational space control force required to counteract nonlinear dynamic effects and generate the desired acceleration in the base coordinate system is calculated.

[0203] By extending the transpose of the Jacobian matrix, the control force in the operating space is mapped into the basic control commands that drive the robotic arm structure.

[0204] Based on redundancy analysis of the robotic arm, the basic control commands are optimized into the optimal control commands.

[0205] In this embodiment, the extended Jacobian matrix serves as a bridge connecting the joint space and the operational space, describing the mapping relationship between joint velocities and end effector velocities. The current robot arm configuration, including base pose, joint angles, and hydraulic rod stroke, is obtained from real-time high-dimensional state estimation. Extended Jacobian matrix Based on the Jacobian matrix Jacobian matrix of robotic arm composition: For each joint or hydraulic rod, calculate its effect on the position and orientation of the end effector: ,in It is the i-th generalized coordinate. Extended inertia matrix. The calculation was performed using a composite rigid body algorithm, taking into account the mass, inertia tensor, and center of mass position of each link: ,in It is the inertia matrix of the i-th link. (Coriolis matrix) The centrifugal force matrix describes the nonlinear dynamic effects in motion: ,in It's the Christopher symbol. Gravity vector. Taking the base attitude into account, it is expressed as: ,in Let be the mass of the i-th link, and g be the gravitational acceleration vector. It is the Jacobian matrix of the center of mass of the i-th link. These dynamic parameters provide an accurate model basis for the subsequent control law, ensuring that the control system can accurately compensate for nonlinear dynamic effects.

[0206] The control law for the operating space dynamics generates control forces directly within the operating space of the end effector, avoiding inverse kinematics calculations. The control law is based on the dynamic equations of the robotic arm: ,in It is the joint driving torque. It is an external force. The dynamics of the end effector can be expressed as: ,in It is the operation space inertia matrix. It is the Coriolis and centrifugal force terms in the operating space. It is the gravity term in the operating space.

[0207] Based on this model, the operational space control force is calculated as follows: ,in It is a corrected acceleration command in the base coordinate system. This control force comprises four parts: inertial force... Coriolis and centrifugal force compensation Gravity compensation p and external force compensation In this way, the control law can counteract the nonlinear dynamic effects of the robotic arm structure and generate a control force that causes the end effector to move with the desired acceleration, ensuring the accuracy of trajectory tracking and the stability of the system.

[0208] The control force in the operating space needs to be converted into actual control commands to drive the joints and base of the robotic arm. According to the principle of virtual work, the operating space force F and the joint space torque... There is a dual relationship between them: ,in It is the virtual displacement of the end effector. It is a virtual displacement of the joint. Because... , can be obtained Therefore, joint space control commands can be expressed as: .

[0209] Transpose of the extended Jacobian matrix The six-dimensional force vector (three-dimensional force and three-dimensional torque) in the operating space is mapped to a torque vector in the joint space, including the force driving the base and the torque driving each joint of the robotic arm. Specifically, ,in It is the base driving force. This refers to the driving torque of the robotic arm joints. For hydraulically driven joints, the torque also needs to be converted into pressure or flow commands for the hydraulic cylinders. ,in It is the pressure of the i-th hydraulic cylinder. It is the piston area. This refers to the lever arm length. This conversion takes into account the kinematic structure and drive method of the robotic arm, ensuring that control commands are executed accurately to produce the desired end effector motion. Finally, based on redundancy analysis of the robotic arm, the basic control commands are optimized into optimal control commands.

[0210] In one implementation, optimizing the basic control commands into optimal control commands based on redundancy analysis of the robotic arm includes the following steps:

[0211] When the degrees of freedom of the robotic arm are greater than the degrees of freedom required by the task space, the control system is considered to have redundancy.

[0212] If the control system has redundancy, then under the condition of satisfying the main task tracking of the basic control command, the control components for optimizing secondary objectives are injected into the basic control command by using the null space projection of the extended Jacobian matrix, so as to obtain the optimal control command.

[0213] In this embodiment, the control system exhibits redundancy when the degrees of freedom of the robotic arm exceed the degrees of freedom required by the task space, providing additional degrees of freedom for optimizing system performance. Redundancy analysis first compares the total degrees of freedom of the robotic arm with the task space dimension (typically six degrees of freedom, including three positions and three orientations). If the total number of degrees of freedom n is greater than the task space dimension m, then the system has nm redundant degrees of freedom. These redundant degrees of freedom form the extended Jacobian matrix. The null space, that is, satisfying The set of all vectors v. Motion in null space does not affect the position and orientation of the end effector, and therefore can be used to optimize secondary objectives. Redundancy can be determined by calculating the rank of the Jacobian matrix: if If this is the case, the system has redundancy. In practical implementation, it can be calculated using Singular Value Decomposition (SVD). The last nm column of V forms the basis of the null space. Redundancy enables the system to optimize other performance metrics, such as avoiding joint constraints, minimizing energy consumption, or improving operational flexibility, while performing its primary task.

[0214] If the control system has redundancy, zero-space control components can be superimposed on the basic control commands to form the optimal control commands. Basic control commands The primary task (end-effector trajectory tracking) is ensured to be completed, while the zero-space control component is used to optimize secondary objectives. The optimal control command is calculated as follows: ,in It is the pseudo-inverse of the extended Jacobian matrix. It is the null projection matrix. It is the control force for optimizing secondary objectives. The null-space projection considering dynamic characteristics can be expressed as: Secondary objectives can take many forms, such as joint constraint avoidance functions: ,in ; or singular position avoidance function: ,in Or, the energy minimization function: In this way, the control system can improve the overall system performance and robustness by making full use of redundant degrees of freedom while ensuring the accuracy of the main task.

[0215] The present invention also discloses a hydraulic robotic arm structure control system based on data fusion, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the hydraulic robotic arm structure control method based on data fusion as described in any of the above embodiments.

[0216] The processor can be a central processing unit (CPU). Of course, depending on the actual use, it can also be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), off-the-shelf programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor or any conventional processor, etc., and this application does not limit it.

[0217] The memory can be an internal storage unit of a computer device, such as a hard disk or RAM, or an external storage device, such as a plug-in hard disk, smart memory card (SMC), secure digital card (SD), or flash memory card (FC) provided on the computer device. Furthermore, the memory can be a combination of internal storage units and external storage devices of a computer device. The memory is used to store computer programs and other programs and data required by the computer device. The memory can also be used to temporarily store data that has been output or will be output. This application does not limit this.

[0218] The present invention also discloses a computer-readable storage medium storing instructions that, when executed by a processor, cause the processor to perform the data fusion-based hydraulic robotic arm structure control method described in any of the above embodiments.

[0219] The computer program can be stored in a machine-readable medium. The computer program includes computer program code, which can be in the form of source code, object code, executable file, or certain middleware. The machine-readable medium includes any entity or device capable of carrying computer program code, recording media, USB flash drive, portable hard drive, magnetic disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the machine-readable medium includes, but is not limited to, the above-mentioned components.

[0220] The transmission line integrated fault detection method in the above embodiments is stored in the computer-readable storage medium and loaded and executed on the processor to facilitate the storage and application of the above method.

[0221] Those skilled in the art should understand that the discussion of any of the above embodiments is merely exemplary and is not intended to imply that the scope of protection of this application is limited to these examples; under the concept of this application, the technical features of the above embodiments or different embodiments can also be combined, the steps can be implemented in any order, and there are many other variations of different aspects of one or more embodiments of this application as described above, which are not provided in detail for the sake of brevity.

[0222] One or more embodiments in this application are intended to cover all such substitutions, modifications, and variations that fall within the broad scope of this application. Therefore, any omissions, modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of one or more embodiments in this application should be included within the protection scope of this application.

Claims

1. A hydraulic robot structure control method based on data fusion, characterized by, The application is applied to a mechanical arm structure comprising a base and a mechanical arm frame, the mechanical arm frame is rotationally connected with the base, the mechanical arm frame comprises a plurality of four-axis closed-loop mechanisms composed of hydraulic rods and connecting rods, and an end effector is arranged at the end of the mechanical arm frame; The method comprises the following steps: Initialize the mechanical arm structure and complete the global coordinate unification of the base, the mechanical arm frame and the end effector with the base as the reference; Real-time acquisition of multi-source heterogeneous sensor data of the mechanical arm structure, application of the unscented Kalman filtering algorithm to fuse the multi-source heterogeneous sensor data, and obtaining of real-time high-dimensional state estimation of the mechanical arm structure; According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system; Combined with the real-time high-dimensional state estimation and the expected trajectory, an optimal control instruction for driving the mechanical arm frame and the base is generated by applying a control law based on operation space dynamics; According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end effector in the world coordinate system, which comprises the following steps: According to the construction task and the obstacle information and spatial constraint conditions of the region to be constructed in the construction task, a path generation algorithm is used to plan the expected trajectory of the end eff The operational space control force is mapped to the base control command for driving the manipulator structure by extending the transpose of the Jacobian matrix; The base control command is optimized to an optimal control command based on a redundancy analysis of the manipulator structure; The operational space dynamics control law can be expressed as: wherein is the operational space inertia matrix, is the operational space nonlinear term, is the operational space gravity term, is the joint velocity vector, is the external force; The operational space control force is calculated as: wherein is the modified acceleration command in the base coordinate system, is the inertial forces, is the Coriolis and centrifugal force compensation, p is the gravity compensation, is the external force compensation.

2. The data fusion based hydraulic manipulator structure control method according to claim 1, characterized by, Initializing the manipulator structure and unifying the global coordinates of the base, the manipulator structure and the end effector based on the base includes the following steps: Scanning the construction site by the laser radar set on the base and a plurality of markers preset on the construction site, and determining the initial base pose of the base relative to the world coordinate system according to the scanning result; Reading the initial angles of all four-axis closed-loop mechanisms and the initial strokes of all hydraulic rods; Based on the complete forward kinematics model containing DH parameters and closed-loop constraints, and combined with the initial angles and the initial formation, the initial end pose of the end effector relative to the base is calculated; Combined with the initial base pose and the initial end pose, and using the coordinate transformation superposition principle, the initial poses of the base, the manipulator structure and the end effector in the world coordinate system are calculated.

3. The data fusion based hydraulic manipulator structure control method according to claim 1, characterized by, The multi-source heterogeneous sensor data includes base navigation data, end effector IMU data, joint encoder data of four-axis closed-loop mechanisms, and displacement sensor data of hydraulic rods.

4. The data fusion-based hydraulic manipulator structure control method according to claim 3, characterized by, Real-time acquisition of multi-source heterogeneous sensor data of the manipulator structure, fusion of multi-source heterogeneous sensor data by applying the unscented Kalman filtering algorithm, and obtaining real-time high-dimensional state estimation of the manipulator structure includes the following steps: Real-time acquisition of multi-source heterogeneous sensor data of the manipulator structure; Defining a state vector containing the base pose and velocity in the world coordinate system, the end effector pose and velocity, the angles and angular velocities of all four-axis closed-loop mechanisms, the strokes and extension velocities of all hydraulic rods, and the IMU zero offset; Using the unscented Kalman filtering algorithm and using the nonlinear state transition model based on the kinematics of the system to perform a state prediction step on the state vector to generate a prior state estimation; Combined with multi-source heterogeneous sensor data and prior state estimation, the posterior estimation is output as the real-time high-dimensional state estimation of the manipulator structure through the measurement update step of the unscented Kalman filtering algorithm.

5. The data fusion based hydraulic manipulator structure control method according to claim 1, characterized by, The base control command is optimized to an optimal control command based on a redundancy analysis of the manipulator structure includes the following steps: When the degrees of freedom of the manipulator structure are greater than the degrees of freedom required by the task space, it is determined that the control system has redundancy; If the control system has redundancy, control components for optimizing secondary goals are injected into the base control command to obtain an optimal control command while meeting the primary task tracking of the base control command.

6. A hydraulic manipulator structure control system based on data fusion, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized by, The processor executes the computer program to implement the hydraulic manipulator structure control method based on data fusion according to any one of claims 1-5.

7. A computer-readable storage medium having stored thereon instructions, the computer-readable storage medium comprising: The instructions, when executed by the processor, cause the processor to be configured to perform the hydraulic manipulator structure control method based on data fusion according to any one of claims 1-5.

Images