Motion control method of insulated boom type aerial vehicle fused with positioning
By using segmented target point control and multi-level closed-loop control, combined with RTK-GNSS and lidar, the problems of low motion control accuracy and collision risk of insulated bucket trucks have been solved, achieving high-precision autonomous motion and safety.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-12-26
- Publication Date
- 2026-03-27
AI Technical Summary
The existing insulated boom trucks have low motion control precision, rely on operator experience, and are prone to mechanical backlash and deformation leading to end-effector deviation. Furthermore, automatic control is susceptible to collisions, failing to meet the precision and safety requirements for high-altitude operations.
By employing segmented target point control and multi-level closed-loop control methods, and combining RTK-GNSS and lidar for environmental modeling and collision detection through scaling intermediate points and error compensation, high-precision autonomous motion is achieved.
It achieves high-precision autonomous movement of the insulated bucket truck, with end-position accuracy within ±5cm, avoiding collision risks and meeting the safety and precision requirements of high-altitude operations.
Smart Images
Figure CN121735137A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of mechanical control technology, specifically a motion control method for an insulated bucket truck with integrated positioning. Background Technology
[0002] As a key piece of equipment for the maintenance and live-line work of high-voltage electrical equipment, the motion control precision and autonomous operation capability of aerial insulated booms directly affect the efficiency and safety of operations.
[0003] The insulated boom truck is a redundant robotic arm with multiple joints connected in series. When carrying out business, the movement of the boom truck generally has two schemes. (1) Manual operation mode of "manual coarse adjustment + teaching pendant fine adjustment" (2) Automatic control of the bucket is achieved by calculating the inverse solution of the joint angle of the boom truck. However, since the boom truck is a hydraulic system, it has characteristics such as large inertia, high delay, and mechanical deformation, resulting in low control accuracy.
[0004] For manual bucket movement, the core reliance is on the operator's experience and real-time observation. The specific process has three stages: Manual coarse positioning stage: After the operation begins, the operator manually controls the bucket arm's rotation, undulation, and extension joint movements using a joystick, combined with on-site visual observation (such as visually judging the distance between the bucket and the target area), to move the bucket to the approximate area of the task. This stage does not rely on precise numerical references; it only needs to meet the basic requirement that "the bucket enters the target working range." Modeling and data acquisition stage: After the bucket reaches the coarse positioning position, the system activates the environmental modeling function, collecting environmental data of the current area through sensors (such as RTK-GNSS positioning), generating a preliminary 3D model or position and attitude data, and synchronizing the bucket's real-time position, joint angles, and other information to the teach pendant interface. Teach pendant fine-tuning stage: The operator views the bucket's position and attitude values displayed on the teach pendant, compares them with the parameters of the ideal working point, and makes fine adjustments using the joystick. For example, if the teach pendant shows that the bucket's current height is 0.2 meters lower than the target value, the operator fine-tunes the undulation joints, gradually correcting the position until the bucket reaches the ideal working point, completing the final positioning. The above are the mainstream solutions for controlling the bucket, but they require high skill from the operators, especially near the target point, and require a lot of time.
[0005] For automatic bucket shifting, the target point is usually calculated visually and sent to trigger automatic bucket shifting. Automatic bucket shifting obtains the angle of each joint by performing inverse kinematics calculation on the series joints. This scheme depends on the DH parameters of the inverse kinematics. If there is a certain deviation in the DH parameters, it will lead to a certain difference in the joint angles obtained by inverse kinematics. Moreover, the insulated boom itself has mechanical clearance, load deformation and other factors, which will cause the end of the insulated bucket boom (bucket body) to deviate significantly. In addition, it is impossible to guarantee that the insulated vehicle will not collide with the outside world during this process. Summary of the Invention
[0006] To address the problems of existing technologies, this invention provides a motion control method for insulated boom lifts with integrated positioning. This method eliminates the need for a precise DH parameter model for the boom lift and avoids the issue of "commands arriving but position deviating" during large boom extensions and multi-joint linkage operations. Its control logic is simple, requiring no complex calculations, resulting in low modification costs. It is particularly adaptable to various boom lift models and has high practical value in high-altitude operations in power and municipal engineering.
[0007] A motion control method for an insulated bucket truck with integrated positioning, characterized by the following steps:
[0008] 1) Top-level target point processing and error compensation:
[0009] 1.1) Preprocessing of the original target point: Calculate the target pose of the insulated truck bucket through the high-altitude operation point, scale the position parameters by x%, generate the pose of the intermediate target point, perform inverse kinematics solution on the intermediate target point, generate the target angle of each joint, control the arm to execute in place, and ensure that the end bucket reaches the intermediate target point.
[0010] 1.2) Position and orientation difference calculation and physical error compensation: After the boom reaches the intermediate target point, the actual position of the bucket is collected by the RTK-GNSS positioning module and denoted as P. act The theoretical pose of the intermediate target point is denoted as P. mid Perform the difference calculation: pose difference ΔP = P act -P mid The positional difference ΔP comprehensively reflects the cumulative deviation caused by physical factors;
[0011] 1.3) Final Target Point Correction and Distribution: Based on the pose difference ΔP and the remaining position 1-x% component of the original target point, calculate the final target point:
[0012] Final target point position P=P act +ΔP+original position component×(1-x%);
[0013] The attitude components still maintain the attitude requirements of the original target point. After generating the final target point pose, they are sent to the planning node to ensure that the influence of physical factors is offset by error compensation.
[0014] 2) Path node planning:
[0015] 2.1) Using the target point pose issued by the top layer, and taking the intermediate target point or the final target point as input, a kinematic model is established based on the multi-joint structural parameters of the arm using the DH parameter method.
[0016] 2.2) Solve the inverse kinematics equations using a numerical iteration method, converting the pose parameters (x, y, z) and (ɑ, β, γ) of the end-effector into target angle combinations for each joint, θ1, θ2, ..., θ nn is the number of joints, ensuring the feasibility of the solution and satisfying the joint angle range constraint;
[0017] 2.3) Conduct collision detection and motion rehearsal;
[0018] 3) Execution node precise control strategy:
[0019] 3.1) Precise Joint Angle Execution: Receives the joint angle sequence output by the planning node, including the target angle θ at each time step. t Generate corresponding control signals based on the joint drive type:
[0020] 3.2) Real-time closed-loop feedback control:
[0021] 3.21) Feedback Acquisition: The actual angle θ of each joint is acquired in real time through joint encoders and angle sensors;
[0022] 3.22) PID control: Design an independent PID controller for each joint, calculate the deviation e between the target angle and the actual angle, and adjust the output control quantity through proportional (P), integral (I), and derivative (D) to dynamically correct the drive signal.
[0023] Further improvements are made, and the specific process of preprocessing the original target points in step 1.1) is as follows:
[0024] 1.11) Calculate the target pose of the insulated truck bed from the high-altitude work point; its rotation matrix is T. tar The base coordinate system is the coordinate system defined by RTK, and RTK_GNSS detects the pose of the bucket as T. cur Simultaneously, the pose T of the fighting body in the base coordinate system can be obtained. robot Therefore, the target pose of the fighting body can be obtained as: T robot T cur -1 T tar ;
[0025] 1.12) Accept the original target point pose in Cartesian space, which contains position components (x,y,z) and attitude components (ɑ,β,γ). Scale the position parameters: calculate the (x,y,z) components and the current position component (x0,y0,z0) at x% of their original values, while keeping the attitude components (ɑ,β,γ) unchanged, thus generating an intermediate target point pose.
[0026] 1.13) Perform inverse kinematics solution on the intermediate target point to generate the target angles of each joint, control the boom to execute in place, and ensure that the end bucket reaches the intermediate target point.
[0027] As a further improvement, the scaling value of x% in step 1) is 90%.
[0028] Further improvements include, in step 2.1), the multi-joint structural parameters include joint type, link length, and rotation axis constraint.
[0029] Further improvements are made to the collision detection and motion pre-play in step 2.3), which are as follows:
[0030] Environmental modeling: By scanning the working environment with LiDAR, the 3D models of obstacles such as utility poles, power lines, and buildings are imported into the simulation environment;
[0031] Trajectory planning and safety verification: The trajectory of the arm is pre-planned in a simulation environment. The collision detection algorithm is used to determine the distance between the arm and itself and obstacles. If the minimum distance is less than the safety threshold, the trajectory is replanned and the joint movement sequence or velocity curve is adjusted.
[0032] Step 3.1) converts the target angle into a hydraulic valve opening command, controls the hydraulic oil flow, and determines whether to execute the next set of joint angles by detecting whether each set of joint angles is within the error threshold range.
[0033] The beneficial effects of this invention are as follows:
[0034] 1. Segmented target point control: By scaling the intermediate point by 90% and correcting for errors, the influence of physical factors such as mechanical clearance and deformation is effectively offset, and the final measured target point accuracy is within ±5cm.
[0035] 2. Simulation and pre-run mechanism: Combining visual environment modeling and collision detection, potential safety hazards in the motion trajectory are eliminated in advance to avoid collision risks in actual operations.
[0036] 3. Multi-level closed-loop control: A two-layer closed loop from end-effector pose feedback (RTK-GNSS) to joint angle feedback (encoder) ensures control robustness and accuracy.
[0037] 4. This solution achieves high-precision autonomous movement of the aerial insulated boom through a layered and refined control process, meeting the stringent requirements for control precision and safety in scenarios such as live-line work. Attached Figure Description
[0038] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0039] Figure 1 A simplified diagram of a redundant five-degree-of-freedom boom truck;
[0040] Figure 2This is a simplified control flowchart for a boom truck. Detailed Implementation
[0041] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0042] In this invention, the insulated bucket truck is as follows: Figure 1 As shown, an Inverse Kinematics Calculation (URDF) is established based on the boom truck to obtain relevant parameters during the inverse kinematics process. This facilitates kinematic modeling of the boom and establishes the relationship between the target pose and the joint angles through inverse kinematics calculation, laying the foundation for normal operation. The overall control flow is as follows: Figure 2 .
[0043] I. Detailed Execution Process of Layered Control Architecture:
[0044] (I) Top-level target point processing and error compensation mechanism:
[0045] 1. Preprocessing of raw target points (90% scaling strategy):
[0046] The pose of the insulated truck bed is calculated from the high-altitude work point, and its rotation matrix is T. tar The base coordinate system is the coordinate system defined by RTK, and RTK_GNSS detects the pose of the bucket as T. cur Simultaneously, the pose T of the fighting body in the base coordinate system can be obtained. robot Therefore, the target pose of the fighting body can be obtained as: T robot T cur -1 T tar After receiving the original target point pose in Cartesian space (containing position components (x,y,z) and attitude components (ɑ,β,γ), the position parameters are scaled: the (x,y,z) components and the current position component (x0,y0,z0) are calculated at 90% of their original values, while the attitude components (ɑ,β,γ) remain unchanged, thus generating the intermediate target point pose.
[0047] The kinematic inverse solution of the intermediate target point is used to generate the target angles of each joint, control the boom to execute in place, and ensure that the end bucket reaches the intermediate target point.
[0048] 2. Position difference calculation and physical error compensation:
[0049] Once the boom reaches the intermediate target point, the actual position of the bucket (denoted as P) is collected by the RTK-GNSS positioning module. act), and the theoretical pose of the intermediate target point (denoted as P). mid Perform the difference calculation:
[0050] Position difference ΔP=P act -P mid ;
[0051] The positional difference ΔP comprehensively reflects the cumulative deviation caused by physical factors such as mechanical clearance, load deformation, and joint parameter errors, providing a data basis for subsequent accurate compensation.
[0052] 3. Final target point correction and distribution:
[0053] Based on the pose difference ΔP and the remaining 10% position components of the original target point, calculate the final target point:
[0054] Final target point position P=P act +ΔP + original position component × 10%;
[0055] The attitude components still maintain the attitude requirements of the original target point. After generating the final target point pose, they are sent to the planning node to ensure that the influence of physical factors is offset by error compensation.
[0056] (II) Implementation of the core functions of planning nodes
[0057] Using the target point pose (intermediate target point or final target point) issued by the top level as input, and based on the multi-joint structural parameters of the arm (such as joint type, link length, rotation axis constraints, etc.), the kinematic model is established using the DH parameter method.
[0058] The inverse kinematics equations are solved by numerical iteration methods (such as the Newton-Raphson method), and the pose parameters (x, y, z) and (ɑ, β, γ) of the end-effector are converted into target angle combinations (θ1, θ2, ..., θ) of each joint. n (where n is the number of joints), ensuring the feasibility of the solution (satisfying the joint angle range constraint).
[0059] Collision detection and motion simulation
[0060] Environmental modeling: By scanning the working environment with LiDAR, the 3D models of obstacles such as utility poles, power lines, and buildings are imported into the simulation environment.
[0061] Trajectory planning and safety verification: The trajectory of the arm is pre-planned in a simulation environment. The collision detection algorithm is used to determine the distance between the arm and itself and obstacles. If the minimum distance is less than the safety threshold, the trajectory is replanned (adjusting the joint movement sequence or velocity curve).
[0062] (iii) Precise control strategy for execution nodes
[0063] Precise joint angle execution
[0064] Receive the joint angle sequence output by the planning node (including the target angle θ at each time step) t Based on the joint drive type (hydraulic drive or electric motor drive), corresponding control signals are generated:
[0065] Hydraulic joint: Converts the target angle into a hydraulic valve opening command, and controls the hydraulic oil flow through a proportional valve;
[0066] Real-time closed-loop feedback control
[0067] Feedback acquisition: The actual angle θ of each joint is acquired in real time through joint encoders and angle sensors, with a sampling frequency of 100Hz.
[0068] PID control: A PID controller is designed independently for each joint, and the deviation between the target angle and the actual angle is calculated as e=θ. tar -θ act The output control quantity is adjusted by proportional (P), integral (I), and derivative (D) functions to dynamically correct the drive signal.
[0069] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to interchangeably. Each embodiment focuses on its differences from other embodiments. In particular, for the device embodiments, the above descriptions are merely preferred embodiments of the present invention. Since they are fundamentally similar to the method embodiments, the descriptions are relatively simple, and relevant parts can be referred to the descriptions of the method embodiments. The above descriptions are merely specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention, without departing from the principle of the present invention, should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.
Claims
1. A method for motion control of a fusion positioned insulated boom truck, characterized by Comprise the following steps: 1) top target point processing and error compensation: 1.1) original target point preprocessing, through high-altitude operation point, calculate the target pose of the insulated car body, scale the position parameters by x%, generate the intermediate target point pose, perform kinematics inverse solution on the intermediate target point, generate the target angle of each joint, control the arm to execute to the position, and ensure that the end of the bucket body reaches the intermediate target point; 1.2) Pose difference calculation and physical error compensation: After the arm reaches the intermediate target point, the actual position of the bucket body is collected by the RTK-GNSS positioning module, denoted as P act , and the theoretical pose of the intermediate target point is denoted as P mid . The difference is calculated: pose difference ΔP=P act -P mid , which reflects the cumulative deviation caused by physical factors. 1.3) final target point correction and issuance: based on the pose difference ΔP and the remaining position 1-x% component of the original target point, calculate the final target point: Final target point position P = P act + ΔP + original position component x (1 - x%) The attitude component still maintains the attitude requirement of the original target point, and the final target point pose is generated and issued to the planning node to ensure that the physical factors are offset through error compensation; 2) path node planning: 2.1) using the target point pose issued by the top layer as input, based on the multi-joint structure parameters of the arm, a kinematics model is established using D-H parameter method; 2.2) Solve the inverse kinematics equation by numerical iteration method, convert the pose parameters (x, y, z) and (ɑ, β, γ) of the end bucket body into the target angle combination of each joint θ1, θ2,..., θn n , n is the number of joints, ensure the feasibility of the solution, meet the joint angle range constraints; 2.3) collision detection and motion simulation; 3) precise control strategy of execution node: 3.1) Precise joint angle execution: receive the joint angle sequence output by the planning node, such as the target angle θ t , generate the corresponding control signal according to the joint drive type; 3.2) real-time closed-loop feedback control: 3.21) feedback collection: real-time collection of actual angles θ of each joint through joint encoder and angle sensor; 3.22) PID adjustment: a PID controller is designed for each joint to calculate the deviation e between the target angle and the actual angle, and the control amount is output through proportional (P), integral (I), and differential (D) adjustment to dynamically correct the drive signal.
2. The fusion-positioned, insulated boom truck motion control method of claim 1, wherein: The specific process of step 1.1) is as follows: 1.11) Calculate the target pose of the insulated car body through the aerial operation point, whose rotation matrix is T tar , and the base coordinate system is the coordinate system defined by RTK. At the same time, the RTK_GNSS detects the pose of the body as T cur , and the pose of the body in the base coordinate system can be obtained as T robot , and thus the target pose of the body is T robot T cur -1 T tar ; 1.12) accept the original target point pose in Cartesian space, which includes position components (x, y, z) and attitude components (ɑ, β, γ), scale the position parameters: calculate the (x, y, z) component with the current position component (x0, y0, z0) by x% of the original value, and the attitude component (ɑ, β, γ) remains unchanged, to generate the intermediate target point pose; 1.13) perform kinematics inverse solution on the intermediate target point to generate the target angle of each joint, control the arm to execute to the position, and ensure that the end of the bucket body reaches the intermediate target point.
3. The fusion-positioned, insulated boom truck motion control method of claim 1 or 2, wherein: The scaling value of step 1) is 90%.
4. The fusion-positioned, insulated boom truck motion control method of claim 1, wherein: The multi-joint structure parameters of step 2.1) include joint type, link length, and rotation axis constraint.
5. The fusion-positioned, insulated boom truck motion control method of claim 1, wherein: The collision detection and motion simulation of step 2.3) are as follows: Environment modeling: scan the working environment in front of the laser radar, and import the three-dimensional models of obstacles such as power poles, wires, and buildings into the simulation environment; Trajectory planning and safety verification: simulate the motion trajectory of the arm in the simulation environment, judge the distance between the arm and itself and obstacles through collision detection algorithm, if the minimum distance is less than the safety threshold, then re-plan the trajectory and adjust the joint motion sequence.
6. The fusion-positioned, insulated boom truck motion control method of claim 1, wherein: Step 3.1) converts the target angle into hydraulic valve opening command, controls the hydraulic oil flow, and determines whether to execute the next joint angle based on whether the current each group of joint angles is within the error threshold.