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

Abstract

The invention provides a hydraulic mechanical arm structure control method and system based on data fusion, and the method comprises the steps: firstly initializing a mechanical arm structure and unifying a global coordinate system, then collecting multi-source heterogeneous sensor data in real time, carrying out the data fusion through an unscented Kalman filtering algorithm, and obtaining the real-time high-dimensional state estimation of the mechanical arm structure. According to construction task requirements and obstacle information and spatial constraint conditions of a construction area, the system uses a path generation algorithm to plan an expected trajectory of an end effector in a world coordinate system. And finally, in combination with 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, and accurate control is achieved. The operation precision and adaptability of the mechanical arm in a complex environment are effectively improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of engineering intelligent control, and in particular relates to a hydraulic mechanical arm structure control method and system based on data fusion. Background Art

[0002] In contemporary construction engineering, precise control of automated hydraulic manipulators remains a critical technical challenge that demands urgent resolution. Particularly in complex and ever-changing construction environments, the equipment's motion accuracy, trajectory planning rationality, and environmental adaptability directly impact construction quality and efficiency. Traditional control methods often rely on a single information source for decision-making, failing to fully perceive equipment status and environmental conditions. This leads to problems such as insufficient control accuracy, irrational trajectory planning, and poor adaptability to complex environments. These issues are particularly acute in high-precision construction scenarios, such as concrete pouring, which require precise positioning and smooth motion.

[0003] Modern construction equipment is typically equipped with a variety of sensors. However, effectively integrating these heterogeneous data sources to build accurate equipment status models and implement intelligent control based on these data has always been a technical challenge. Traditional methods often fragment sensor data processing, failing to fully exploit the complementarity and synergy between data. This leads to inaccurate state estimation, which in turn affects control effectiveness. Summary of the Invention

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

[0005] In a first aspect, the present invention provides a hydraulic manipulator structure control method based on data fusion, which is applied to a manipulator structure comprising a base and a manipulator frame, wherein the manipulator frame is rotatably connected to the base, the manipulator frame comprises a plurality of four-axis closed-loop mechanisms composed of hydraulic rods and connecting rods, and an end effector is provided at the end of the manipulator frame; The method comprises the following steps: Initialize the robotic arm structure and use the base as a reference to complete the global coordinate unification of the base, robotic arm frame and end effector; Real-time acquisition of multi-source heterogeneous sensor data of the robotic arm structure, and the application of the unscented Kalman filter algorithm to fuse the multi-source heterogeneous sensor data to obtain a real-time high-dimensional state estimation of the robotic arm structure; Based on the construction task and the obstacle information and spatial constraints of the construction area in the construction task, a path generation algorithm is used to plan the desired trajectory of the end effector in the world coordinate system; Optimal control commands for driving the robotic arm and base are generated by combining real-time high-dimensional state estimation and desired trajectory and applying control laws based on the dynamics of the operating space.

[0006] Optionally, initializing the robotic arm structure and unifying the global coordinates of the base, robotic arm frame, and end effector using the base as a reference includes the following steps: The laser radar installed on the base is used to scan multiple markers preset at the construction site, and the initial base posture relative to the world coordinate system is determined based on the scanning results; Read 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 including DH parameters and closed-loop constraints, the initial end pose of the end effector relative to the base is calculated in combination with the initial angle and initial shape.

[0007] 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 pose and using the coordinate transformation superposition principle.

[0008] 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.

[0009] Optionally, real-time acquisition of multi-source heterogeneous sensor data of the manipulator 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 estimation of the manipulator structure include the following steps: Optionally, real-time acquisition of multi-source heterogeneous sensor data of the robotic arm structure; Define the state vector including the base position and velocity in the world coordinate system, the position and velocity of the end effector, the angle and angular velocity of all four-axis closed-loop mechanisms, the stroke and extension velocity of all hydraulic rods, and the IMU zero bias; The unscented Kalman filter algorithm is used to perform a state prediction step on the state vector using a nonlinear state transition model based on system kinematics to generate a priori state estimates; Combining multi-source heterogeneous sensor data and prior state estimation, the posterior estimation is output as the real-time high-dimensional state estimation of the robotic arm structure through the measurement update step of the unscented Kalman filter algorithm.

[0010] Optionally, planning a desired trajectory of the end effector in a world coordinate system using a path generation algorithm based on the construction task and obstacle information and spatial constraints of the area to be constructed in the construction task includes the following steps: Based on the construction task and the obstacle information and spatial constraints of the construction area, a path generation algorithm is used to plan a smooth and continuous desired posture path of the end effector in the world coordinate system. The desired posture 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 posture, expected velocity and expected acceleration trajectory of the end effector are output.

[0011] Optionally, combining the real-time high-dimensional state estimation and the desired trajectory and applying a control law based on the dynamics of the operating space to generate optimal control instructions for driving the manipulator arm and base includes the following steps: Calculate the pose error and pose error derivative between the actual pose and the desired pose of the current end effector in the world coordinate system; Using the desired acceleration trajectory as feedforward, the task space PID controller is combined with the pose error and the corresponding pose error derivative to calculate the desired end effector correction acceleration command in the world coordinate system. Using the initial base pose in real-time high-dimensional state estimation, the corrected acceleration command in the world coordinate system is converted to the base coordinate system; After the coordinates are converted, the control law based on the dynamics of the operating space is applied to generate the optimal control instructions for driving the robotic arm and the base and maintaining the robotic arm including the hydraulic rod in a dynamic pressure holding state.

[0012] Optionally, after the coordinates are converted, applying a control law based on the dynamics of the operating space to generate an optimal control instruction for driving the robotic arm and the base and maintaining the robotic arm including the hydraulic rod in a dynamic pressure holding state includes the following steps: Based on real-time high-dimensional state estimation, the extended Jacobian matrix of the current configuration and the model parameters of the complete forward dynamics model are calculated in real time. The model parameters include the extended inertia matrix, Coriolis matrix, centrifugal force matrix and the gravity vector considering the base attitude; Applying the dynamic control law of the operating space, combined with the modified acceleration command and model parameters in the base coordinate system, the operating space control force required to offset the nonlinear dynamic effect and generate the desired acceleration in the base coordinate system is calculated; By extending the transpose of the Jacobian matrix, the control force in the operating space is mapped into the basic control instructions for driving the robotic arm structure; Based on the redundancy analysis of the robotic arm, the basic control instructions are optimized into the optimal control instructions.

[0013] Optionally, optimizing the basic control instructions into optimal control instructions based on redundancy analysis of the robotic arm includes the following steps: When the degree of freedom of the manipulator is greater than the degree of freedom required by the task space, the control system is judged to be redundant; If the control system has redundancy, then under the condition that the main task tracking of the basic control instruction is satisfied, the control component for optimizing the secondary objective is injected into the basic control instruction by using the null space projection of the extended Jacobian matrix to obtain the optimal control instruction.

[0014] In the 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 runnable 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.

[0015] In a third aspect, the present invention further provides a computer-readable storage medium having instructions stored thereon, which, when executed by a processor, configures the processor to execute the hydraulic robotic arm structure control method based on data fusion described in the first aspect.

[0016] The beneficial effects of the present invention are: Through the innovative application of multi-source heterogeneous data fusion and advanced control algorithms, this invention significantly improves the control accuracy and intelligence level of the robotic arm structure, resolving the technical challenges of traditional control methods, which suffer from insufficient precision and poor adaptability in complex environments. Compared to traditional single-sensor data processing methods, this invention utilizes an unscented Kalman filter algorithm to fuse multi-source heterogeneous sensor data, effectively overcoming the drawbacks of single-sensor measurement noise and limited precision. This method constructs a more accurate real-time high-dimensional state model of the robotic arm, laying a solid foundation for precise control. Regarding trajectory planning, this invention automatically generates optimal motion trajectories based on construction task requirements and environmental constraints, significantly improving the efficiency and safety of the robotic arm in complex environments and avoiding the inefficiency and uncertainty of traditional manual path planning. Specifically, in terms of control strategy, this invention establishes a control law based on the dynamics of the operating space, fully considering the nonlinear dynamic characteristics of the robotic arm and environmental interaction factors. This achieves closed-loop control from high-dimensional state estimation to optimal control instructions, significantly improving trajectory tracking accuracy and system stability. This globally optimized control method enables the robotic arm to complete complex construction tasks more smoothly and accurately, reduces the need for manual intervention, lowers the operator's skill requirements and labor intensity, while improving construction quality and efficiency, and provides strong technical support for the widespread application of robotic arms in various complex construction scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 This is a schematic structural diagram of the robotic arm structure in one embodiment of the present application.

[0018] Figure 2 This is a structural diagram of the robotic arm structure and hydraulic station in one embodiment of the present application.

[0019] Figure 3 This is a structural diagram of a hydraulic station in one embodiment of the present application.

[0020] Figure 4 This is a flow chart of a hydraulic manipulator structure control method based on data fusion in one embodiment of the present application.

[0021] Description of reference numerals: 1. Robotic arm; 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. Fuel tank; 13. Air cooling. DETAILED DESCRIPTION

[0022] The following will be combined with the accompanying drawings in the embodiments of the present application to clearly describe the technical solutions in the embodiments of the present application. Obviously, the embodiments described are part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field are within the scope of protection of this application.

[0023] The terms "first," "second," and the like in the specification and claims of this application are used to distinguish similar objects, and are not used to describe a specific order or precedence. It should be understood that the terms used in this manner are interchangeable where appropriate, so that the embodiments of this application can be implemented in an order other than that illustrated or described herein, and that the objects distinguished by "first," "second," and the like are generally of the same type, and do not limit the number of objects; for example, the first object can be one or more. In addition, the term "and / or" in the specification and claims refers to at least one of the connected objects, and the character " / " generally indicates that the objects connected are in an "or" relationship.

[0024] Figure 1 An advanced robotic arm structure was demonstrated, consisting primarily of a sturdy base and a complex robotic arm frame. The base adopts an extended design, presenting a trapezoidal structure, ensuring good stability of the overall system during operation. The robotic arm frame is rotatably connected to the base via a precise slewing bearing, enabling 360° horizontal rotation, significantly expanding the working range. The robotic arm frame adopts a truss-type structural design, composed of multiple connecting rods, with an overall two-section layout. The first section is the main arm, which adopts a triangular truss structure and forms a stable force transmission system through multiple support rods. The second section is the auxiliary arm, which also adopts a truss structure but is relatively smaller in size to improve the flexibility and precision of the end. This design ensures the strength and rigidity of the robotic arm while reducing the overall weight.

[0025] The robotic arm frame incorporates several four-axis closed-loop mechanisms, each constructed from precisely coordinated hydraulic rods and connecting rods. As shown in the image, the main hydraulic rods are located at key points within the arm, including the base-to-main arm connection, the main arm-to-auxiliary arm connection, and within the auxiliary arm. These hydraulic rods work in concert through a control system to achieve precise lifting, lowering, and retracting movements of the arm. The closed-loop design ensures more efficient force transmission while enhancing the overall structural stability and load-bearing capacity. At the end of the robotic arm frame is a dedicated end effector, whose circular design facilitates the attachment of various tools. Connected to the auxiliary arm via a sophisticated linkage, the end effector achieves multi-degree-of-freedom motion, adapting to diverse working conditions. Multiple interfaces are also provided around the actuator to accommodate specialized tools such as concrete spreading devices, gripping devices, drills, and cutting tools, tailored to specific application requirements.

[0026] Each joint in the entire robotic arm structure is connected using high-precision bearings to ensure smoothness and precision during movement. As can be seen from the figure, key connection points are equipped with reinforcing ribs and protective devices, effectively improving the durability and safety of the structure. The robotic arm's control system achieves precise control of the robotic arm's posture by adjusting the telescopic 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 the arm's operating status, load conditions, and environmental parameters in real time to ensure safe and reliable operation. This robotic arm structure design is suitable for a variety of industrial scenarios, such as heavy-duty operations such as construction, mining, and port loading and unloading. It can also be used in precision operations such as hazardous environment detection and precision assembly. Its modular design makes maintenance and upgrades simple and efficient, greatly extending the equipment's service life and improving economic benefits.

[0027] A safety control system is installed in the robotic arm structure. The safety control system includes three main aspects: safety limit, safety lock and safety monitoring. The safety limit triggers the corresponding mechanism when the movement exceeds the preset value by setting an absolute position sensor or an absolute angle sensor; the safety lock locks the actuator through an electronic switch valve to ensure that the posture is maintained during movement or emergency. Safety monitoring is further divided into torque monitoring, pressure monitoring and obstacle monitoring: Torque monitoring uses torque sensors to monitor the force of the robotic arm. When the force exceeds the safety range or the pressure of the cylinder inlet and outlet and the pump station pressure sensor exceeds the maximum pressure range and reaches the maximum working range of the hydraulic system force, the system will start braking and alarm; Pressure monitoring includes cylinder pressure monitoring and hydraulic station pressure monitoring. Cylinder pressure monitoring monitors the pressure of the inlet and return oil lines and the pressure sensor data, and determines the pressure safety range based on the load, speed and pressure loss. Once the range is exceeded or the hydraulic pipeline loses pressure rapidly, braking and alarming will be triggered, while hydraulic station pressure monitoring ensures that the pressure in the working state is maintained within the safety range, otherwise the brake alarm will also be triggered; Obstacle monitoring covers ultrasonic monitoring and video monitoring to detect and avoid potential risks.

[0028] In one embodiment, the complete set of mechanical arm structure control system may include a hydraulic station that provides core power, referring to Figure 2 and Figure 3 The hydraulic station is mainly composed of a servo motor and a hydraulic pump (such as Figure 3 The gear pump shown in the figure is responsible for converting electrical energy into hydraulic energy; the oil tank serves as a storage container for the hydraulic oil and has the functions of heat dissipation and precipitation of impurities; in order to ensure the cleanliness of the oil, the system is equipped with a filter, such as Figure 3 The oil inlet filter shown in the figure is used for preliminary filtration before the pump sucks in the oil, and the return oil filter is used to clean the oil returned to the oil tank; the pressure sensor (including the pressure sensor with display and the pressure transmitter) is used to monitor the pressure of each key point of the system in real time and convert the pressure signal into an electrical signal output; the overflow valve is an important pressure control and protection component, which can limit the maximum pressure of the system to prevent overload; the one-way valve ensures that the hydraulic oil can only flow in one direction in the pipeline to prevent backflow; the accumulator is used to store a certain amount of pressurized oil so that it can quickly release energy when needed, absorb system pressure pulsation or serve as an emergency power source; the air cooling device cools the high-temperature hydraulic oil by forced air cooling to maintain the oil within the normal operating temperature range; in addition, it also includes pipelines connecting the various components and various valves used to control the flow direction, pressure and flow of the oil, such as solenoid switch valves, stop valves, etc., which together constitute a complete hardware system of the hydraulic power unit.

[0029] The working process of the hydraulic station starts with the hydraulic oil in the oil tank. The servo motor starts and drives the hydraulic pump (gear pump) connected to it to rotate at high speed. The hydraulic pump draws hydraulic oil from the oil tank through the oil inlet filter, pressurizes it and outputs high-pressure oil to the system pipeline; in the process of high-pressure oil flowing to the actuator, its pressure is monitored in real time by the pressure sensor. If the system pressure exceeds the set value, the relief valve will open and guide the excess pressure oil back to the oil tank to play a role in pressure regulation and safety protection; the one-way valve is installed at the key position where the oil needs to be prevented from flowing back to ensure that the oil flows in the predetermined direction; the accumulator absorbs the pressure in the pipeline according to system requirements. Shock and pulsation, or replenishing pressurized oil to the system in a short time; the return oil of the actuator after completing work, and the oil that may be discharged from the relief valve, etc., will flow through the return oil pipeline, and a part of the oil (such as the "return oil from the cylinder → solenoid switch valve → relief valve → stop valve → air cooling" path) 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 pollutants generated during work. The clean hydraulic oil will eventually return to the oil tank, completing a complete working cycle, thereby continuously and stably providing the required hydraulic power to external equipment.

[0030] The system's movements are executed by a series of actuators, including a servo cylinder with a built-in stroke sensor for linear extension and retraction, and a servo rotary drive with built-in position feedback and driven by a hydraulic motor for rotational motion. In addition, there are electronic switch valves to control the on-off of the oil circuit, pressure sensors to monitor the actuator load, and angle encoders to accurately feedback the joint angle. The servo valve is a key control element that accurately adjusts the flow and direction of the hydraulic oil flowing to the actuator. In order to achieve precise positioning, the system is equipped with a positioning system, including lidar, tags and laser gyroscopes, which are used to obtain the precise position and posture information of the robotic arm in the workspace. Ensuring the safety of the operation process depends on a complete safety system. The system integrates visual sensors for environmental perception, ultrasonic sensors for obstacle detection, and torque sensors for monitoring the force conditions of each joint of the robotic arm; the motion control of the robotic arm is the responsibility of the motion controller, whose core functions include trajectory planning to generate a smooth and efficient motion path, and specific motion control, which relies on the real-time feedback provided by the stroke sensor and angle encoder, and achieves precise closed-loop control through the stroke and angle mapping control algorithm; the coordinated operation of all subsystems is uniformly scheduled and collaboratively controlled by the central controller; finally, the entire control system is mounted on the mobile chassis and acts on the precise arm structure, together forming a complete robotic arm structure control system.

[0031] Figure 4 FIG. 1 is a flow chart of a method for controlling a hydraulic mechanical arm structure based on data fusion in one embodiment. It should be understood that although Figure 4The steps in the flowchart are shown in sequence 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 restriction for the execution of these steps, and these steps can be executed in other orders. In addition, Figure 4 At least part of the steps in the above process may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same time, but may be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but may be executed in turn or alternately with other steps or at least part of the sub-steps or stages of other steps. Figure 4 As shown, the hydraulic mechanical arm structure control method based on data fusion disclosed in the present invention specifically includes the following steps: 1. Device initialization: S101. Initialize the robotic arm structure and complete the global coordinate unification of the base, robotic arm frame and end effector based on the base.

[0032] Among them, initializing the robotic arm structure is the premise and basis for the operation of the entire control system. Its core goal is to establish a unified global coordinate system so that the positions and postures of all components in the robotic arm structure can be represented and calculated in the same coordinate system. The process first uses a laser radar installed on the base to scan the markers pre-arranged at the construction site. These markers are usually objects with clear geometric features, such as reflectors or special-shaped markers. The laser radar measures the distance and angle information of the marker relative to the base by emitting a laser beam and receiving the reflected signal. Based on this information, combined with the known position of the marker in the world coordinate system, the initial pose matrix of the base relative to the world coordinate system can be solved by the least squares method or other registration algorithms. . This matrix contains the rotation matrix and translation vectors , the complete expression is: Next, you need to read the initial angles of all four-axis closed-loop mechanisms in the robotic arm. and initial travel of all hydraulic rods These data are usually obtained through encoders installed at the joints and displacement sensors on the hydraulic rods. Based on these initial parameters, a complete forward kinematics model including DH parameters and closed-loop constraints is used to calculate the initial position and orientation 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, , connecting rod torsion angle , connecting rod offset and joint angles For a four-axis closed-loop mechanism, closed-loop constraints also need to be considered, which are usually expressed as a set of geometric constraint equations. Through these parameters and constraints, the pose matrix of the end effector relative to the base can be calculated. .

[0033] Finally, the initial base pose and the initial end pose are combined, and the coordinate transformation superposition principle is used to calculate the initial pose of the base, robotic arm, and end effector in the world coordinate system. Specifically, the pose matrix of the end effector in the world coordinate system is This can be obtained by the following matrix multiplication: Similarly, the position and posture 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 posture of all components in the robotic arm structure can be expressed in a unified world coordinate system, providing accurate initial conditions for subsequent state estimation, path planning, and control, eliminating inconsistencies between different coordinate systems, and improving the positioning and control accuracy of the entire system.

[0034] 2. Collect and import data: S102. Collect multi-source heterogeneous sensor data of the robotic arm structure in real time, apply the unscented Kalman filter algorithm to fuse the multi-source heterogeneous sensor data, and obtain a real-time high-dimensional state estimation of the robotic arm structure.

[0035] Device posture: Device posture mainly refers to the posture 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 posture information.

[0036] Absolute positioning: RTK, the device's position xyz relative to the world map, with a maximum accuracy of centimeters. It is greatly affected by the environment and requires additional mobile base stations in special circumstances. For example, in bad weather, the base station is far away, and signal obstruction may result in an inability to obtain a fixed solution and insufficient accuracy.

[0037] Relative positioning: positioning tags and lidar solutions, setting positioning tags based on the mission map, and calculating the relative position (xyz and direction) of the device based on automatic scanning and ranging by lidar, with an accuracy of up to millimeter level; IMU inertial navigation system: The overall posture information of the device during movement, that is, the tilt, direction, speed and acceleration of the mobile base.

[0038] End effector pose: IMU inertial navigation system: refers to the system installed on the end effector to monitor the attitude, speed and acceleration information of the end effector in real time.

[0039] Travel sensor: collects the travel position of each joint.

[0040] Angle sensor: collects angle information of each joint.

[0041] In subsequent motion planning, both forward and inverse solutions for the robotic arm require real-time device pose information. The robotic arm kinematics solution uses the device (mobile base) coordinate system as an intermediate coordinate system for transformation. Because the device pose is based on the uneven construction site surface, real-time device pose information is required for calculation during the forward and inverse solutions to ensure the end effector moves according to the planned trajectory. Otherwise, significant errors will occur. The IMU pose information on the end effector can be verified in real time against the planned motion trajectory.

[0042] 1. The position of the end effector relative to the base coordinate system is calculated based on the base pose information, DH parameters, and the angles and stroke positions of each joint, and then uniformly converted into the world coordinate system, which is the positive kinematic solution of the robotic arm.

[0043] 2. Calculate the trajectory of each joint based on the trajectory of the end effector, the base posture information and the DH parameters, that is, the inverse kinematics of the robot arm.

[0044] In robotic arm control systems, real-time state estimation is crucial for achieving precise control. First, it requires real-time acquisition of heterogeneous sensor data from multiple sources within the robotic arm structure. 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 (joint angles) from the four-axis closed-loop mechanism, and displacement sensor data (hydraulic rod stroke). These sensors operate at different frequencies and provide measurements with varying accuracy and noise characteristics. Therefore, an efficient data fusion algorithm is required to comprehensively process this heterogeneous data.

[0045] The Unscented Kalman Filter (UKF) algorithm is a state estimation method suitable for nonlinear systems. Compared to the traditional Extended Kalman Filter (EKF), the UKF approximates the probability distribution after nonlinear transformation through a set of carefully selected sampling points (sigma points), avoiding the errors caused by linearization. In this application, a high-dimensional state vector x is first defined, which contains the base position and velocity in the world coordinate system, the position and velocity of the end effector, the angles and angular velocities of all four-axis closed-loop mechanisms, the travel and extension velocity of all hydraulic rods, and the IMU zero bias. The state vector can be expressed as: in, and Represent the position and attitude quaternions of the base respectively, and represents the linear velocity and angular velocity of the base, and The quaternion representing the position and attitude of the end effector, and represents the linear velocity and angular velocity of the end effector, and represents the angle and angular velocity of the i-th joint, and represents the stroke and extension speed of the j-th hydraulic rod, and Indicates the zero bias of the IMU's accelerometer and gyroscope.

[0046] The implementation of the UKF algorithm consists of two main steps: state prediction and measurement update. In the state prediction step, the state vector is predicted based on the nonlinear state transition model of the system kinematics. The state transition equation can be expressed as: , where f is the nonlinear state transfer function, is the control input, is the process noise, which obeys a Gaussian distribution with zero mean and covariance Q. Specifically, the state transfer function takes into account the kinematic constraints of the robotic arm structure, such as the relationship between the joint angle and the hydraulic rod stroke, and the relationship between the base motion and the end effector motion.

[0047] In the measurement update step, the multi-source heterogeneous sensor data is combined with the prior state estimate, and the posterior estimate is output through 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, is the measurement noise, which follows a Gaussian distribution with zero mean and covariance R. The measurement update process involves calculating the Kalman gain, updating the state estimate, and updating the error covariance matrix. In this way, the UKF algorithm effectively integrates multi-source heterogeneous sensor data, suppressing the noise and errors of each sensor, and providing real-time, high-dimensional state estimation of the manipulator structure. This state estimation is characterized by high accuracy and low latency, providing reliable state information for subsequent trajectory planning and control, significantly improving the stability and robustness of the manipulator control system.

[0048] 3. Trajectory planning: The trajectory is the path in the time dimension. The trajectories of multiple joints and the mobile base are planned based on the trajectory of the end effector. The trajectory of the end effector output by the business planning system (the path under business and time constraints) is imported. The trajectory of the end effector is combined with the actual position of the current device to plan the motion trajectory of each joint and the mobile chassis. The actuator is then used to control the movement of each joint and the mobile chassis according to the trajectory in real time.

[0049] 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.

[0050] Among them, path planning is a key link in the robot arm control system. Its goal is to generate an optimal path that meets the requirements of the construction task and avoids obstacles. First, it is necessary to obtain the specific requirements of the construction task, including the starting point, target point, intermediate passing points, and specific requirements for the end effector posture. At the same time, obstacle information in the construction area is obtained through lidar, visual sensors or pre-established environmental maps. These obstacles may be static (such as walls, columns) or dynamic (such as mobile equipment, personnel). Spatial constraints include the robot arm's workspace limitations, joint angle limitations, and constraints for specific construction tasks.

[0051] 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, probabilistic road map method PRM) and optimization-based methods (such as artificial potential field method, A* algorithm). In this technical solution, the path generation algorithm needs to consider the kinematic constraints and dynamic characteristics of the robot 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 closest to the sampling point in the tree, and tries to expand a new node from the node in the direction of 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, which is to reselect the parent node of the newly added node to ensure the optimality of the path. The specific implementation can be expressed as: 1. Initialize the root node of tree T as the starting point .

[0052] 2. Loop N times: a. Randomly sample a point in the configuration space .

[0053] b. Find the distance in the tree Nearest Node .

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

[0055] d. If No collision with the environment: i. Find All nodes in the sphere with a center and a radius of r constitute the set .

[0056] ii. From Select the value from the starting point to The node with the smallest path cost is The parent node of .

[0057] iii. Add to the tree T.

[0058] iv. For Each node in If you pass arrive If the path cost is less than the current path cost, reconnect The parent node is .

[0059] 3. Find the distance target point in the tree The nearest node, building a path from the starting point to this node.

[0060] When planning the desired pose path of the end effector, it is also necessary to enforce the constraint that the end effector's Z axis is aligned with the world coordinate system's Z axis. This constraint is often used for specific construction tasks, such as vertical drilling or vertical placement of objects. Mathematically, this constraint can be expressed as the end effector pose matrix The third column (Z-axis direction vector) is the same as the Z-axis unit vector of the world coordinate system Stay consistent: .

[0061] In order 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 the expected acceleration trajectory These trajectory data will serve as input for subsequent control algorithms, guiding the robotic arm to move along a predetermined path. Through this path planning method, an end-effector trajectory can be generated that meets the construction task requirements, avoids obstacles, and moves smoothly, thereby improving construction efficiency and safety. At the same time, the mandatory posture constraints ensure that the end-effector can perform specific construction tasks in the correct posture, such as vertical drilling, vertical material laying, or precise placement.

[0062] 4. Motion Control Execution: S104. Combine the real-time high-dimensional state estimation and the desired trajectory and apply the control law based on the dynamics of the operation space to generate the optimal control instructions for driving the robotic arm and the base.

[0063] Among them, control based on operational space dynamics is an advanced control strategy that can directly control the operational space of the end effector, avoiding the complex inverse kinematics calculations in traditional joint space control. First, it is necessary to calculate the position error between the actual position of the current end effector in the world coordinate system and the desired position. Position error It can be calculated directly by vector subtraction: ,in is the desired position, is the actual position. Attitude error You need to consider the characteristics of the rotation matrix or quaternion, and you can use the logarithmic mapping to calculate: ,in is the desired pose matrix, is the actual attitude matrix. The attitude error derivative includes the position error derivative and attitude error derivatives ,in and are the desired linear and angular velocities, respectively, and are the actual linear velocity and angular velocity respectively.

[0064] Next, with the expected acceleration trajectory As a feedforward, the task space PID controller is used to calculate the desired end effector correction acceleration command in the world coordinate system by combining the pose error and the corresponding pose error derivative. The output of the PID controller can be expressed as: in, 、 and are the proportional, differential and integral gain matrices respectively, is the pose error vector, is the pose error derivative vector.

[0065] Since the dynamic control of the operating space needs to be performed in the base coordinate system, it is necessary to use the base posture in the real-time high-dimensional state estimation to transform the correction acceleration command in the world coordinate system into the base coordinate system. The coordinate transformation can be achieved by the rotation matrix accomplish: After the coordinates are transformed, the optimal control instructions for driving the manipulator arm and base are generated by applying the control law based on the dynamics of the operating space. First, the extended Jacobian matrix of the current configuration is calculated in real time based on the real-time high-dimensional state estimation. , as well as the model parameters of the complete forward dynamics model, including the extended inertia matrix , Coriolis matrix , centrifugal force matrix and gravity vector considering the base attitude . Extended Jacobian matrix The mapping relationship between joint velocity and end effector velocity is described, and the influence of base motion on end effector motion is considered. The dynamic control law of the operating space can be expressed as: in, is the operating space inertia matrix, is the nonlinear term in the operating space, is the operating space gravity term, is the joint velocity vector, It is an external force.

[0066] By extending the transpose of the Jacobian matrix, the operating space control force F is mapped to the basic control instructions for driving the robotic arm structure. When the degrees of freedom of the manipulator are greater than the degrees of freedom required by the task space, the control system has redundancy. In this case, the optimal control instruction can be obtained by injecting the control component for optimizing the secondary objective into the basic control instruction by using the null space projection of the extended Jacobian matrix while satisfying the main task tracking of the basic control instruction. : Where I is the identity matrix, It is the zero-space control force used to optimize secondary goals, such as avoiding joint limitations, optimizing the robot arm configuration, avoiding singular positions, etc.

[0067] This control method, based on the dynamics of the operating space, enables precise trajectory tracking of the end-effector in the world coordinate system while fully utilizing the redundant degrees of freedom of the manipulator to optimize secondary objectives, improving the performance and robustness of the control system. This control method is suitable for complex manipulator structures and can effectively handle nonlinear dynamic effects and external disturbances, ensuring precise execution of construction tasks. The control process begins by defining a business plan in the global world coordinate system. Planning data containing the 3D trajectory and pose of the end-effector and a pre-set path for the mobile chassis are imported. Simultaneously, integrated sensors perform real-time positioning in the manipulator coordinate system, acquiring its current relative pose and position. Forward kinematics is then used to calculate the precise current coordinates of the end-effector in real time. Subsequently, the end-effector coordinate analysis phase begins. Based on the forward kinematics model, the planned target end point is determined to be reachable within the current manipulator configuration. If unreachable, this may trigger a chassis movement plan or adjust the end-effector trajectory to achieve the desired result through joint coordination. The global optimality of the selected path is also evaluated. The core motion planning steps are further refined into: trajectory planning for the mobile chassis, generating a safe and efficient path and evaluating its feasibility and optimality; and trajectory planning for the robotic arm joints. This process relies on inverse kinematics, which reversely solves the angle or displacement sequence of each joint based on the expected position and posture sequence of the end effector in the world coordinate system, and also verifies the existence and optimality of the solution.

[0068] The robustness and accuracy of the entire planning process are due to a set of key "planning logics", the core task of which is to dynamically handle the inconsistencies between the real-time perceived position data and the preset planning data. Specifically, if it is detected that the current actual position of the end does not match the starting point of the planned trajectory, the system will use the current real-time end position as the new reference starting point to re-solve the subsequent trajectory; at the same time, if the actual position of the mobile chassis deviates from the planned value, the deviation will be compensated when the robot performs kinematic inverse solution to calculate the joint angle, ensuring that the end effector can accurately reach the target point in its world coordinate system. Ultimately, the precise trajectory data of each joint (including position, velocity, and acceleration instructions) generated by the compensated and optimized kinematic inverse solution is accurately transmitted to the underlying motion controller to drive each axis to strictly execute according to the plan, and form a closed-loop control through continuous sensor feedback, ensuring the high precision and high stability of the motion task.

[0069] The actual control process can also achieve dynamic pressure maintenance of the robot arm. This capability is achieved through comprehensive pressure maintenance control and efficient energy-saving control strategies, which together ensure the efficient, stable and precise operation of the robot arm hydraulic system under various working conditions. At the pressure maintenance control level, 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 accumulators into the hydraulic system, it can effectively absorb and release pressure fluctuations caused by load changes or actuator movements, acting like a hydraulic buffer to maintain relative stability of system pressure. At the same time, corresponding relief valves are carefully set for each independent motion axis or key hydraulic circuit of the robot arm. These relief valves serve as safety protection and pressure upper limit setting units. They can automatically open when the pressure of a specific axis exceeds the preset safety threshold, directing excess hydraulic oil back to the tank, thereby preventing system overpressure and ensuring the safety of various components. The active pressure maintenance mechanism reflects the system's high intelligence and dynamic response capabilities. Its core lies in the use of high-precision pressure sensors to continuously monitor the pressure in the hydraulic system in real time and feed back the collected pressure data to the central controller. The controller 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 output to compensate for pressure loss or reducing output to avoid excessive pressure, thereby achieving precise and active closed-loop control of the system pressure.

[0070] On the other hand, the energy-saving control strategy focuses on improving the energy utilization efficiency of the system and reducing unnecessary energy consumption. This strategy intelligently adjusts the working mode and output power of the servo pump based on the real-time motion state of the entire robot arm, including the motion and speed information of its various joints. When the robot arm performs high-load, high-speed tasks, the servo pump runs at full power to provide sufficient power; when the robot arm is in a non-moving state or low-load standby, 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 ultimate control accuracy of the entire hydraulic control system. Any slight deviation in pressure control may 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 robot arm's own posture, real-time changes in load size during operation, adjustments to the actuator's movement speed, the impact of ambient temperature fluctuations on hydraulic oil viscosity, changes in friction between moving parts, and the stability of the hydraulic station's own working conditions (such as oil temperature, oil cleanliness, pump efficiency, etc.), will 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.

[0071] At the motion control level, highly sophisticated coordinated control is implemented for both the robotic arm and its underlying mobile chassis to achieve precise positioning and smooth movement in complex operating environments. Speed ​​control of the robotic arm primarily relies on accurate calibration of joint dynamics. Under simulated, actual operating conditions, the dynamic parameters of each joint axis, including but not limited to speed, required drive torque, and moment of inertia, are comprehensively tested and recorded in various motion spaces and postures. This lays the physical model foundation for subsequent precise control. Secondly, closely integrating the specific structural characteristics and performance parameters of the hydraulic cylinders or motors used in the robotic arm, the flow rates and corresponding pressures required to drive these actuators are accurately calculated using the principles of fluid mechanics and mechanical conversion. Based on this, the rated pressure gain parameters for each operating state are further determined, with a certain adjustment range reserved to accommodate varying operating conditions. Pressure sensors continuously play a key role in this process, dynamically monitoring and providing feedback on the actual output torque and speed of the servo pump. The system then dynamically adjusts the pressure setpoint accordingly, forming a synergistic effect between active and passive pressure maintenance mechanisms.

[0072] Furthermore, the system utilizes a simulation fitting algorithm based on multi-sensor data fusion. This means it integrates information from various sensor types (such as position, velocity, and torque sensors) and calculates simulated PID (proportional-integral-derivative) control parameters using advanced algorithms. These parameters are dynamically adjusted based on real-time feedback, achieving precise closed-loop control of the robot arm's joint angular velocity and actuator linear travel speed. This sophisticated control also extends to the optimization of simulation parameters, the coordination of multi-machine hybrid parameters (which may refer to the coordinated operating parameters of multiple actuators or subsystems), and the introduction of dynamic velocity feedforward calculation parameters to improve system response speed and dynamic performance. Furthermore, the system integrates advanced position control algorithms and precise mapping algorithms between travel and angle to ensure high consistency between commanded and actual positions. Regarding trajectory planning, the robot arm's motion control supports a variety of velocity trajectory profiles, including T-type (trapezoidal acceleration and deceleration), S-type (smooth acceleration and deceleration), and more complex polynomial curves, allowing the operator to select the most appropriate motion mode based on 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. This algorithm was initially designed to address practical issues, such as the possibility that planned angular trajectories may not be completely smooth due to algorithmic or sampling rate limitations; inconsistent motion caused by a mismatch between the planned instruction cycle (e.g., 100 milliseconds) and the underlying controller's execution cycle (e.g., 1 millisecond); and the nonlinear mapping relationship between planned trajectory data, which is typically based on joint angles, and the linear stroke of the hydraulic cylinder, which is actually directly controlled.

[0073] To solve these problems, the system's trajectory planning steps include: first, accurately converting the planned angle trajectory into the stroke trajectory of each cylinder through kinematic inverse solution or 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 sophisticated trajectory processing ultimately achieves high standards of motion quality in both semi-automatic and fully automatic modes, regardless of angle closed-loop control or stroke closed-loop control. For the mobile chassis that carries the robotic arm, its motion control also follows similar precise control logic, covering an independent speed control system and trajectory planning functions, ensuring that the entire equipment system can act as a coordinated and unified whole to efficiently and safely complete various operational tasks in complex environments.

[0074] In one embodiment, initializing the robotic arm structure and unifying the global coordinates of the base, the robotic arm frame, and the end effector with the base as a reference includes the following steps: The laser radar installed on the base is used to scan multiple markers preset at the construction site, and the initial base posture relative to the world coordinate system is determined based on the scanning results; Read 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 including DH parameters and closed-loop constraints, the initial end pose of the end effector relative to the base is calculated in combination with the initial angle and initial shape.

[0075] 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 pose and using the coordinate transformation superposition principle.

[0076] In this embodiment, the laser radar measures the distance and angle of an object by emitting a laser beam and receiving the reflected signal. A number of markers with clear geometric features (such as reflectors or special-shaped markers) are pre-placed at the construction site. The world coordinate system positions of these markers are known. When the laser radar scans these markers, their polar coordinates relative to the base are obtained. ,in It's distance, and These polar coordinates can be converted to Cartesian coordinates in the base coordinate system. .

[0077] The position of the known marker in the world coordinate system , a corresponding relationship can be established: , is the rotation matrix, Is the translation vector. Solve the transformation matrix using the least squares method: , get the pose matrix of the base relative to the world coordinate system This step ensures that the robotic arm control system can accurately locate the initial position and posture of the base in the world coordinate system, providing a basis for subsequent coordinate transformation.

[0078] The four-axis closed-loop mechanism is the core component of the robotic arm. Each closed-loop mechanism consists of four rotating joints and connecting rods. The encoder installed at the joint can read the initial angle of each joint. (where i = 1, 2, ..., n, where n is the total number of joints). Encoders typically work based on photoelectric, magnetic, or resistive principles, converting angle changes into electrical signals. For example, a photoelectric encoder consists of a light source, a code disk, and a photodetector. When the code disk rotates with the joint, the changes in the light signal received by the photodetector are converted into angle information. The initial stroke of the hydraulic rod (where j = 1, 2, ..., m, where m is the total number of hydraulic rods) is acquired using displacement sensors mounted on the hydraulic rods. Common displacement sensors include linear variable differential transformers (LVDTs), magnetostrictive displacement sensors, or resistive displacement sensors. These sensors convert the hydraulic rod's telescopic displacement into electrical signals. After signal conditioning and analog-to-digital conversion, the precise stroke of the hydraulic rod is obtained. These initial angles and stroke values ​​constitute the initial configuration of the manipulator arm, providing the necessary input parameters for subsequent forward kinematic calculations and ensuring that the manipulator control system accurately understands the initial state of the manipulator arm.

[0079] The DH parameter method is a standard method for describing the relative position and orientation of the links in a robotic arm. For each link, four parameters are defined: link length , connecting rod torsion angle , connecting rod offset and joint angles Based on these parameters, the transformation matrix from joint i-1 to joint i can be constructed: For the four-axis closed-loop mechanism, closed-loop constraints also need to be considered, which can be expressed as geometric constraint equations: Substituting the read initial angle and hydraulic rod stroke into these equations, the transformation matrix from the base to the end effector is calculated by successive multiplications: ,in is the transformation matrix from the last joint to the end effector. This gives the initial pose matrix of the end effector relative to the base, which contains the position vector and the rotation matrix : This step ensures that the robot control system can accurately calculate the initial position and attitude of the end effector relative to the base.

[0080] The principle of coordinate transformation superposition is a basic concept in robotics, which is used to transform the pose representations in different coordinate systems into a unified 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 by matrix multiplication: After expansion, 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 postures of all components in the robotic arm structure (the base, the various joints and links of the robotic arm frame, and the end effector) can be unified and expressed in the world coordinate system. This step unifies 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 and control accuracy of the entire system.

[0081] In one embodiment, real-time acquisition of multi-source heterogeneous sensor data of a 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 estimation of the robotic arm structure include the following steps: Real-time collection of multi-source heterogeneous sensor data of the robotic arm structure; Define the state vector including the base position and velocity in the world coordinate system, the position and velocity of the end effector, the angle and angular velocity of all four-axis closed-loop mechanisms, the stroke and extension velocity of all hydraulic rods, and the IMU zero bias; The unscented Kalman filter algorithm is used to perform a state prediction step on the state vector using a nonlinear state transition model based on system kinematics to generate a priori state estimates; Combining multi-source heterogeneous sensor data and prior state estimation, the posterior estimation is output as the real-time high-dimensional state estimation of the robotic arm structure through the measurement update step of the unscented Kalman filter algorithm.

[0082] In this embodiment, the robotic arm structure is equipped with various sensors to monitor its motion status and environmental information in real time. Base navigation data is acquired via GPS or RTK positioning systems, providing the base's absolute position 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 mounted on the end effector includes a three-axis accelerometer, a three-axis gyroscope, and a three-axis magnetometer, measuring linear acceleration, angular velocity, and the direction of the Earth's magnetic field, respectively. The accelerometer typically has a measurement range of ±16g, and the gyroscope has a measurement range of ±2000° / s, with a sampling rate of up to 200Hz. Furthermore, the combination of tags and a lidar relative positioning measurement system enables millimeter-level orientation and position accuracy. Encoders are installed at the joints of the four-axis closed-loop mechanism, typically with a resolution of 4096 pulses / revolution, enabling precise measurement of joint angles. Displacement sensors on the hydraulic rods, such as LVDTs or magnetostrictive sensors, measure rod travel with an accuracy of up to 0.1mm. This sensor data is transmitted to the central controller via communication protocols such as the CAN bus, RS485, or Ethernet. The sampling period is set between 10 and 100 milliseconds based on control requirements. A timestamp synchronization mechanism is used during data collection to ensure temporal consistency between different sensor data. Data interpolation or extrapolation methods are used to compensate for occasional data loss or communication interruptions. Real-time collection of this multi-source, heterogeneous sensor data provides comprehensive and reliable observation information for subsequent state estimation, laying the foundation for high-precision control.

[0083] The state vector is a mathematical representation that describes the complete dynamic characteristics of the robot structure and includes all key state variables of the system. The high-dimensional state vector x is defined as follows: in, Indicates the three-dimensional position of the base in the world coordinate system; is the unit quaternion representing the base attitude, and represents the linear velocity and angular velocity of the base, and The quaternion representing the position and attitude of the end effector, and represents the linear velocity and angular velocity of the end effector, and represents the angle and angular velocity of the i-th joint, and represents the stroke and extension speed of the j-th hydraulic rod, and Represents the IMU's accelerometer and gyroscope biases. This state representation method has several advantages: first, it contains complete motion information of the manipulator structure, facilitating subsequent state estimation and control; second, using quaternions to represent the attitude avoids the gimbal lock problem of Euler angles; finally, including the IMU bias in the state vector allows for online estimation and compensation of sensor errors, improving system accuracy.

[0084] The Unscented Kalman Filter (UKF) algorithm is an advanced state estimation method for nonlinear systems. It approximates the probability distribution after nonlinear transformation by carefully selecting sigma points. First, define the augmented state vector , where w and v are process noise and measurement noise respectively. Calculate the covariance matrix of the augmented state vector , where P is the state estimate 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): in is the scaling parameter, Control the distribution of sigma points, is a secondary scaling parameter. These sigma points are propagated through the nonlinear state transfer function: The state transfer function is based on the system kinematic model, including quaternion integration, angular velocity integration, position update, etc. For example, the base position update is: Quaternion updates use the quaternion differential equation: This is achieved through discretization: Compute the prior state estimate and covariance: The weight coefficient is: parameter It is used to incorporate prior knowledge about the distribution. In this way, the UKF algorithm can effectively handle the nonlinear dynamic characteristics of the manipulator structure and generate accurate prior state estimates.

[0085] The measurement update step combines the sensor observation data with the prior state estimate to generate a more accurate posterior state estimate. First, the prior sigma point Mapped to the measurement space through the measurement function h: The measurement function varies depending on the sensor type. For example, for base GPS data, the measurement function simply extracts the position component of the state vector; for IMU data, the measurement function considers the relationship between gravity, acceleration, and angular velocity: Compute the predicted measurements and their covariance: Compute the cross-covariance of the state and measurement: Calculate the Kalman gain: Update state estimates and covariances: in is the actual measurement value. The quaternion component requires special processing to maintain the unit quaternion constraint. In this way, the UKF algorithm can effectively fuse multi-source heterogeneous sensor data, suppress the noise and errors of each sensor, and provide real-time high-dimensional state estimation of the manipulator structure. This provides reliable state information for subsequent trajectory planning and control, significantly improving the stability and robustness of the manipulator control system.

[0086] In one embodiment, planning a desired trajectory of an end effector in a world coordinate system using a path generation algorithm based on a construction task and obstacle information and spatial constraints of an area to be constructed in the construction task includes the following steps: Based on the construction task and the obstacle information and spatial constraints of the construction area, a path generation algorithm is used to plan a smooth and continuous desired posture path of the end effector in the world coordinate system. The desired posture 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 posture, expected velocity and expected acceleration trajectory of the end effector are output.

[0087] In this embodiment, path planning first needs to clarify the starting point, target point, and intermediate passing points of the construction task, and obtain obstacle information in the construction area. Obstacle information can be obtained through a pre-established environmental map or real-time sensors (such as lidar, visual sensors), and represented as a series of geometric bodies (such as cubes, cylinders, or polyhedrons). Spatial constraints include the working space limitations of the robot arm, joint angle limitations, and the requirements of specific construction tasks. The RRT* (Rapid Random Tree Optimization) algorithm is used for path planning. The algorithm randomly samples in the configuration space and constructs a tree structure from the starting point to the target point. After the path is generated, it is smoothed using a B-spline curve to ensure that the path is smooth and meets the acceleration continuity requirements. In this way, a desired posture path for the end effector can be generated that avoids obstacles, meets spatial constraints, and is smooth and continuous, providing a basis for subsequent trajectory tracking control.

[0088] For example, in a concrete spreading scenario, the end effector (e.g., a spreading tube) must maintain a vertical downward orientation to ensure uniform concrete flow and accurate placement. This requires the end effector's Z axis (usually the tool axis) to be aligned with the Z axis of the world coordinate system (usually pointing away from the sky). Mathematically, this constraint can be expressed as the end effector pose matrix: The third column (Z-axis direction vector) is the same as the Z-axis unit vector of the world coordinate system parallel: Under this constraint, the posture of the end effector still has one degree of freedom, that is, rotation around the Z axis. This rotation angle can be determined based on the robot arm configuration optimization or other task requirements. Combined with the pose constraint, a complete pose trajectory can be generated: in Is the attitude quaternion that satisfies the Z-axis alignment constraint. By taking the first and second order derivatives, we can get the desired velocity trajectory and the desired acceleration trajectory: in and are the expected linear velocity and linear acceleration, and are the desired angular velocity and angular acceleration respectively. To ensure the smoothness of the trajectory, you can set velocity and acceleration constraints, such as the maximum linear velocity , maximum angular velocity , maximum linear acceleration and maximum angular acceleration The time scale is adjusted to ensure that the trajectory meets these constraints: like , 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 instructions for the concrete placement process, effectively improving placement quality and construction efficiency.

[0089] In one embodiment, the trajectory of the end effector can also be planned based on the construction task and the obstacle information of the area to be constructed in the construction task, the spatial constraints and the real-time posture information of the current equipment. Then, based on the trajectory of the end effector, the motion trajectories of the joints and the mobile base of the equipment are planned. When there is no solution for planning the trajectories of the joints of the robotic arm, that is, when there is no solution for completing the trajectory of the end effector corresponding to the trajectories of the joints, the trajectory of the mobile base and the motion trajectory of each joint of the equipment in the same world coordinate are determined while ensuring that the motion trajectory of the end effector, obstacle avoidance (mobile obstacle avoidance and posture obstacle avoidance), maximum performance constraints (performance parameters such as the maximum speed and acceleration of each joint and mobile chassis), stability (acceleration smoothness), and optimal efficiency are met. Finally, real-time error compensation is performed between the actual posture and the expected posture in the initial state and the motion process state.

[0090] In one embodiment, combining real-time high-dimensional state estimation and desired trajectory and applying a control law based on the dynamics of the operating space to generate optimal control instructions for driving the manipulator arm and base includes the following steps: Calculate the pose error and pose error derivative between the actual pose and the desired pose of the current end effector in the world coordinate system; Using the desired acceleration trajectory as feedforward, the task space PID controller is combined with the pose error and the corresponding pose error derivative to calculate the desired end effector correction acceleration command in the world coordinate system. Using the initial base pose in real-time high-dimensional state estimation, the corrected acceleration command in the world coordinate system is converted to the base coordinate system; After the coordinates are converted, the control law based on the dynamics of the operating space is applied to generate the optimal control instructions for driving the robotic arm and the base and maintaining the robotic arm including the hydraulic rod in a dynamic pressure holding state.

[0091] In this embodiment, pose error calculation is the fundamental link of the control system, which is used to quantify the difference between the current pose of the end effector and the target pose. First, the actual pose of the end effector is obtained from the real-time high-dimensional state estimation, including the position vector and attitude quaternion At the same time, the desired pose is obtained from the trajectory planning module, including the desired position and the desired attitude quaternion The position error is calculated directly by vector subtraction: The calculation of attitude error is relatively complex, and the quaternion difference method is used: ,in represents quaternion multiplication, yes The conjugate quaternion of . For ease of control, the quaternion error is converted into axis-angle representation: The pose error derivative includes the position error derivative and attitude error derivatives ,in and are the desired linear and angular velocities, and These errors and error derivatives will be used in subsequent controller calculations to provide the basis for accurate trajectory tracking.

[0092] The task space PID controller combines feedback control and feedforward control to generate a correction acceleration command to eliminate the posture error. The control law expression is: in, is the desired acceleration trajectory, which serves as a feedforward term; is the pose error vector; is the pose error derivative; is the integral term of the pose error; 、 and They are proportional, differential and integral gain matrices, usually diagonal matrices, which can adjust the response characteristics of position and attitude control respectively. Provides correction proportional to the error, moving the system in the direction of reducing the error; the differential term Provides damping to suppress overshoot and oscillation; integral term Eliminate static errors and improve the steady-state accuracy of the system. To prevent integral saturation, integral limiting or conditional integration strategies are usually adopted.

[0093] PID parameter adjustment follows the first adjustment , then adjust , and finally adjust The attitude control parameter is usually smaller than the position control parameter. In this way, the controller generates the corrected acceleration command. It is able to effectively track the desired trajectory while compensating for external disturbances and model uncertainties.

[0094] Since the dynamic model and control of the manipulator are usually expressed in the base coordinate system, it is necessary to convert the correction acceleration command in the world coordinate system to the base coordinate system. The pose of the base in the world coordinate system is obtained from the real-time high-dimensional state estimation, including the position vector and the rotation matrix The rotation matrix describes the orientation of the base coordinate system relative to the world coordinate system. Its transpose Can be used to transform a vector in world coordinates to base coordinates.

[0095] Corrected acceleration command Center, linear acceleration and angular acceleration Need to convert separately: After merging, the corrected acceleration instruction in the base coordinate system is obtained: This coordinate transformation takes into account the possible motion and posture changes of the base, ensuring that control instructions are executed in the correct coordinate system, and providing accurate input for subsequent control based on the dynamics of the operating space. This step is particularly crucial when the base is movable because it links the globally planned trajectory with the locally executed control. Finally, based on the transformed coordinates and according to the control law of the dynamics of the operating space, the optimal control instructions for driving the robotic arm and base are generated. Control based on the dynamics of the operating space is an advanced control strategy that performs control directly in the operating space of the end effector, avoiding complex inverse kinematics calculations. This control method can effectively handle nonlinear dynamic effects and external interference, ensuring accurate trajectory tracking.

[0096] In one embodiment, after coordinate conversion, applying a control law based on the dynamics of the operating space to generate optimal control instructions for driving the robotic arm and the base and maintaining the robotic arm including the hydraulic rod in a dynamic pressure holding state includes the following steps: Based on real-time high-dimensional state estimation, the extended Jacobian matrix of the current configuration and the model parameters of the complete forward dynamics model are calculated in real time. The model parameters include the extended inertia matrix, Coriolis matrix, centrifugal force matrix and the gravity vector considering the base attitude; Applying the dynamic control law of the operating space, combined with the modified acceleration command and model parameters in the base coordinate system, the operating space control force required to offset the nonlinear dynamic effect and generate the desired acceleration in the base coordinate system is calculated; By extending the transpose of the Jacobian matrix, the control force in the operating space is mapped into the basic control instructions for driving the robotic arm structure; Based on the redundancy analysis of the robotic arm, the basic control instructions are optimized into the optimal control instructions.

[0097] In this implementation, the extended Jacobian matrix bridges the joint space and the operation space, describing the mapping relationship between the joint velocity and the end effector velocity. The current manipulator configuration, including the base pose, joint angles, and hydraulic rod stroke, is obtained from the real-time high-dimensional state estimation. By the basis Jacobian matrix and the robot Jacobian matrix composition: For each joint or hydraulic rod, calculate its effect on the position and orientation of the end effector: ,in is the i-th generalized coordinate. Extended inertia matrix The composite rigid body algorithm is used to calculate the mass, inertia tensor and center of mass position of each link: ,in is the inertia matrix of the i-th link. Coriolis matrix and the centrifugal force matrix describes the nonlinear dynamic effects in motion: ,in is the Christopher symbol. Gravity vector Considering the base posture, it is expressed as: ,in is the mass of the i-th link, g is the gravitational acceleration vector, 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.

[0098] The control law for the operating space dynamics generates the control forces directly in the operating space of the end effector, avoiding the inverse kinematics calculation. The control law is based on the dynamic equations of the manipulator: ,in is the joint driving torque, is the external force. The dynamics of the end effector is expressed as: ,in is the operating space inertia matrix, are the operating space Coriolis and centrifugal force terms, is the operating space gravity term.

[0099] Based on this model, the operating space control force is calculated as: ,in It is the correction acceleration instruction in the base coordinate system. This control force consists of four parts: inertia force , Coriolis and centrifugal force compensation , gravity compensation p and external force compensation In this way, the control law can offset the nonlinear dynamic effects of the manipulator structure and generate a control force that causes the end effector to move according to the desired acceleration, ensuring the accuracy of trajectory tracking and the stability of the system.

[0100] The control force in the operating space needs to be converted into the actual control instructions for driving the joints and base of the manipulator. According to the principle of virtual work, the operating space force F is related to the joint space torque. There is a duality relationship between them: ,in is the virtual displacement of the end effector, is the virtual displacement of the joint. , we can get Therefore, the joint space control instructions can be expressed as: .

[0101] Transpose of the extended Jacobian matrix The six-dimensional force vector (three-dimensional force and three-dimensional torque) of the operation space is mapped to the torque vector of the joint space, including the force of the driving base and the torque of each joint of the driving manipulator. Specifically, ,in is the base driving force, is the driving torque of the robot joint. For hydraulically driven joints, the torque must also be converted into pressure or flow instructions for the hydraulic cylinder: ,in is the pressure of the i-th hydraulic cylinder, is the piston area, is the arm length. This conversion takes into account the kinematic structure and drive mode of the robot arm, ensuring that the control instructions can be accurately executed to produce the desired end effector motion. Finally, based on the redundancy analysis of the robot arm, the basic control instructions are optimized to the optimal control instructions. In one embodiment, optimizing basic control instructions into optimal control instructions based on redundancy analysis of the robotic arm includes the following steps: When the degree of freedom of the manipulator is greater than the degree of freedom required by the task space, the control system is judged to be redundant; If the control system has redundancy, then under the condition that the main task tracking of the basic control instruction is satisfied, the control component for optimizing the secondary objective is injected into the basic control instruction by using the null space projection of the extended Jacobian matrix to obtain the optimal control instruction.

[0102] In this embodiment, when the number of degrees of freedom of the manipulator is greater than the number of degrees of freedom required by the task space, the control system has redundancy, which provides additional degrees of freedom for optimizing system performance. Redundancy analysis first compares the total number of degrees of freedom of the manipulator with the dimensions of the task space (typically six degrees of freedom, including three positions and three postures). If the total number of degrees of freedom n is greater than the dimension of the task space m, then the system has nm redundant degrees of freedom. These redundant degrees of freedom form the extended Jacobian matrix The null space of The set of all vectors v in the null space. Motion in the null space does not affect the position and attitude of the end effector and can therefore be used to optimize the secondary objective. Redundancy can be confirmed by calculating the rank of the Jacobian matrix: if , then the system has redundancy. In actual implementation, it can be calculated by singular value decomposition (SVD) , where the last nm columns of V form the basis of the null space. Redundancy enables the system to optimize other performance indicators while completing the main task, such as avoiding joint limitations, minimizing energy consumption, or improving operational flexibility.

[0103] If the control system has redundancy, the zero space control component can be superimposed on the basic control instruction to form the optimal control instruction. The primary task (end effector trajectory tracking) is ensured, while the zero-space control component is used to optimize the secondary objective. The optimal control command is calculated as: ,in is the pseudo-inverse of the extended Jacobian matrix, is the null space projection matrix, is the control force for optimizing the secondary objective. The null space projection considering the dynamic characteristics can be expressed as: The secondary objective can be in various forms, such as a joint limit avoidance function: ,in ; or a singular position avoidance function: ,in ; or the energy minimization function: In this way, the control system can ensure the execution accuracy of the main task while making full use of the redundant degrees of freedom to improve the overall performance and robustness of the system.

[0104] 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 runnable 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.

[0105] Among them, the processor can adopt a central processing unit (CPU). Of course, according to actual usage, other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. can also be adopted. The general-purpose processor can adopt a microprocessor or any conventional processor, etc., and this application does not impose any restrictions on this.

[0106] Among them, the memory can be an internal storage unit of a computer device, such as a hard disk or memory of a computer device, or an external storage device of a computer device, such as a plug-in hard disk, smart memory card (SMC), secure digital card (SD) or flash memory card (FC) equipped on the computer device. In addition, the memory can also be a combination of an internal storage unit and an external storage device 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 is to be output. This application does not impose any restrictions on this.

[0107] The present invention also discloses a computer-readable storage medium having instructions stored thereon. When the instructions are executed by a processor, the processor is configured to execute the hydraulic mechanical arm structure control method based on data fusion described in any one of the above embodiments.

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

[0109] Among them, the power transmission line comprehensive fault detection method in the above embodiment is stored in the computer-readable storage medium through the computer-readable storage medium, and is loaded and executed on the processor to facilitate the storage and application of the above method.

[0110] Those skilled in the art should understand that the discussion of any of the above embodiments is merely illustrative and is not intended to imply that the scope of protection of the present application is limited to these examples. In line with the present application, the technical features in the above embodiments or different embodiments may be combined, the steps may be implemented in any order, and there are many other variations of different aspects of one or more embodiments of the present application as above, which are not provided in detail for the sake of simplicity.

[0111] The one or more embodiments of this application are intended to encompass 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 of this application should be included in the scope of protection of this application.

Claims

1. A hydraulic manipulator structure control method based on data fusion, characterized in that: Applied to a robotic arm structure comprising a base and a robotic arm frame, the robotic arm frame is rotatably connected to the base, the robotic arm frame comprises a number of four-axis closed-loop mechanisms consisting of hydraulic rods and connecting rods, and an end effector is provided at the end of the robotic arm frame; The method comprises the following steps: Initialize the robotic arm structure and use the base as a reference to complete the global coordinate unification of the base, robotic arm frame and end effector; Real-time acquisition of multi-source heterogeneous sensor data of the robotic arm structure, and the application of the unscented Kalman filter algorithm to fuse the multi-source heterogeneous sensor data to obtain a real-time high-dimensional state estimation of the robotic arm structure; Based on the construction task and the obstacle information and spatial constraints of the construction area in the construction task, a path generation algorithm is used to plan the desired trajectory of the end effector in the world coordinate system; Optimal control commands for driving the robotic arm and base are generated by combining real-time high-dimensional state estimation and desired trajectory and applying control laws based on the dynamics of the operating space.

2. The hydraulic manipulator structure control method based on data fusion according to claim 1 is characterized in that: Initializing the robotic arm structure and unifying the global coordinates of the base, robotic arm frame, and end effector using the base as a reference includes the following steps: The laser radar installed on the base is used to scan multiple markers preset at the construction site, and the initial base posture relative to the world coordinate system is determined based on the scanning results; Read 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 including DH parameters and closed-loop constraints, the initial end-effector pose relative to the base is calculated in combination with the initial angle and initial shape; 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 pose and using the coordinate transformation superposition principle.

3. The hydraulic manipulator structure control method based on data fusion according to claim 1 is characterized in that: 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.

4. The hydraulic manipulator structure control method based on data fusion according to claim 3 is characterized in that: The real-time acquisition of multi-source heterogeneous sensor data of the manipulator structure and the application of the unscented Kalman filter algorithm to fuse the multi-source heterogeneous sensor data to obtain the real-time high-dimensional state estimation of the manipulator structure include the following steps: Real-time collection of multi-source heterogeneous sensor data of the robotic arm structure; Define the state vector including the base position and velocity in the world coordinate system, the position and velocity of the end effector, the angle and angular velocity of all four-axis closed-loop mechanisms, the stroke and extension velocity of all hydraulic rods, and the IMU zero bias; The unscented Kalman filter algorithm is used to perform a state prediction step on the state vector using a nonlinear state transition model based on system kinematics to generate a priori state estimates; Combining multi-source heterogeneous sensor data and prior state estimation, the posterior estimation is output as the real-time high-dimensional state estimation of the robotic arm structure through the measurement update step of the unscented Kalman filter algorithm.

5. The hydraulic manipulator structure control method based on data fusion according to claim 1 is characterized in that: Based on the construction task and the obstacle information and spatial constraints of the construction area, the path generation algorithm is used to plan the desired trajectory of the end effector in the world coordinate system, which includes the following steps: Based on the construction task and the obstacle information and spatial constraints of the construction area, a path generation algorithm is used to plan a smooth and continuous desired posture path of the end effector in the world coordinate system. The desired posture 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 posture, expected velocity and expected acceleration trajectory of the end effector are output.

6. The hydraulic manipulator structure control method based on data fusion according to claim 5 is characterized in that: Combining real-time high-dimensional state estimation and desired trajectory and applying a control law based on the dynamics of the operating space to generate optimal control commands for driving the manipulator arm and base involves the following steps: Calculate the pose error and pose error derivative between the actual pose and the desired pose of the current end effector in the world coordinate system; Using the desired acceleration trajectory as feedforward, the task space PID controller is combined with the pose error and the corresponding pose error derivative to calculate the desired end effector correction acceleration command in the world coordinate system. Using the initial base pose in real-time high-dimensional state estimation, the corrected acceleration command in the world coordinate system is converted to the base coordinate system; After the coordinates are converted, the control law based on the dynamics of the operating space is applied to generate the optimal control instructions for driving the robotic arm and the base and maintaining the robotic arm including the hydraulic rod in a dynamic pressure holding state.

7. The hydraulic manipulator structure control method based on data fusion according to claim 6 is characterized in that: After the coordinates are converted, the control law based on the dynamics of the operating space is applied to generate the optimal control instructions for driving the manipulator arm and the base and maintaining the manipulator arm including the hydraulic rod in a dynamic pressure holding state, which includes the following steps: Based on real-time high-dimensional state estimation, the extended Jacobian matrix of the current configuration and the model parameters of the complete forward dynamics model are calculated in real time. The model parameters include the extended inertia matrix, Coriolis matrix, centrifugal force matrix and the gravity vector considering the base attitude; Applying the dynamic control law of the operating space, combined with the modified acceleration command and model parameters in the base coordinate system, the operating space control force required to offset the nonlinear dynamic effect and generate the desired acceleration in the base coordinate system is calculated; By extending the transpose of the Jacobian matrix, the control force in the operating space is mapped into the basic control instructions for driving the robotic arm structure; Based on the redundancy analysis of the robotic arm, the basic control instructions are optimized into the optimal control instructions.

8. The hydraulic manipulator structure control method based on data fusion according to claim 7 is characterized in that: Optimizing basic control instructions to optimal control instructions based on redundancy analysis of the robotic arm includes the following steps: When the degree of freedom of the manipulator is greater than the degree of freedom required by the task space, the control system is judged to be redundant; If the control system has redundancy, then under the condition that the main task tracking of the basic control instruction is satisfied, the control component for optimizing the secondary objective is injected into the basic control instruction by using the null space projection of the extended Jacobian matrix to obtain the optimal control instruction.

9. A hydraulic manipulator structure control system based on data fusion, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the hydraulic mechanical arm structure control method based on data fusion as claimed in any one of claims 1 to 8 is implemented.

10. A computer-readable storage medium having instructions stored thereon, characterized in that: When the instruction is executed by a processor, the processor is configured to execute the hydraulic mechanical arm structure control method based on data fusion according to any one of claims 1 to 8.

Images