Longitudinal six-degree-of-freedom full-automatic mechanical arm and control system and control method thereof

Abstract

The application discloses a longitudinal six-degree-of-freedom full-automatic mechanical arm and a control system and a control method thereof, comprising a linear motion assembly and a mechanical arm assembly, wherein the linear motion assembly comprises a linear motion part and a linear driving part, the mechanical arm assembly comprises a plurality of connecting rods connected through rotary joints at the first and last ends, the plurality of connecting rods and the plurality of rotary joints form a connecting rod mechanism, one end of the connecting rod mechanism is connected to the linear motion part through a rotary joint, and the other end of the connecting rod mechanism is connected to an equipment assembly through a rotary joint; the linear driving part controls the linear motion of the linear motion part through a spatial kinematics modeling strategy, a partitioned trajectory planning strategy and a self-adaptive closed-loop control strategy, the rotary joints drive the connecting rod mechanism, the connecting rods and the equipment assembly to rotate around the respective connecting points through the spatial kinematics modeling strategy, the partitioned trajectory planning strategy and the self-adaptive closed-loop control strategy, and the linear motion assembly and the mechanical arm assembly are safely protected through a full-space safety protection strategy.

Description

Technical Field

[0001] This invention relates to the field of robotic arm equipment technology, specifically to a longitudinal six-degree-of-freedom fully automatic robotic arm and its control system and control method. Background Technology

[0002] With the popularization of industrial automation and the rapid development of the new energy vehicle industry, fully automated robotic arms have become core execution equipment in scenarios such as automatic vehicle charging, port material handling, and flexible industrial assembly, thanks to their advantages such as unmanned operation throughout the entire process, high-precision flexible docking, intelligent scheduling, and adaptability to all scenarios. They are of great significance for improving operational efficiency, reducing labor costs, and ensuring operational safety.

[0003] Currently, most mainstream fully automated robotic arm systems in the industry adopt suspended or fixed layouts, with configurations mainly divided into two categories: high-degree-of-freedom general-purpose and low-degree-of-freedom specialized types. Both types have inherent technical drawbacks that are difficult to balance in practical applications, specifically: High-degree-of-freedom robotic arms have too many joints, resulting in complex mechanical structures, high control difficulty, and high manufacturing costs; low-degree-of-freedom robotic arms lack operational flexibility, cannot adapt to changing working environments and complex posture adjustment requirements, and severely limit application scenarios. Existing robotic arms are not optimized for long-stroke, cantilever-type configurations, ignoring issues such as weight-induced deformation of the cantilever arm and the accumulation of errors over long strokes. They cannot effectively integrate visual and force sensor information to achieve precise trajectory planning and tracking, resulting in large positioning errors and difficulty in stably completing high-precision tasks such as charging gun insertion and precise docking. Most robotic arms lack folding and retracting structures, occupying a large space when not in operation, resulting in poor engineering adaptability in narrow and confined environments such as parking lots and underground garages; furthermore, there is no protective design when retracted, making the arm body and end effector prone to collisions and impacts, leading to high risk of equipment damage and poor durability. The general control algorithm is not adapted to the serial configuration of "linear motion and rotary joints". It lacks exclusive strategies such as linear motion and joint master-slave coordination, narrow space partitioning planning, and cantilever deformation compensation. It cannot balance long-stroke operation efficiency and near-end precision operation accuracy. At the same time, it lacks hierarchical safety protection, fault tolerance and folding and retraction control mechanisms, resulting in insufficient reliability under unattended operation conditions.

[0004] The aforementioned technical deficiencies severely restrict the industrialization of fully automated robotic arms in scenarios such as automatic vehicle charging and confined space handling. Developing a longitudinal dedicated fully automated robotic arm system with simplified joints, simple structure, low cost, adaptability to multiple scenarios, and protective features when retracted has become a key technical problem that the industry urgently needs to solve. Summary of the Invention

[0005] The purpose of this invention is to overcome the shortcomings of the aforementioned background technology and provide a longitudinal six-degree-of-freedom fully automatic robotic arm with simplified joints, simple structure, low cost, adaptability to multiple scenarios, and protective function after retraction, as well as its control system and control method.

[0006] To achieve this objective, the present invention designs a longitudinal six-degree-of-freedom fully automatic robotic arm, comprising a linear motion component and a robotic arm assembly mounted on the linear motion component. The linear motion component includes a linear motion part capable of reciprocating along a linear direction and a linear drive part driving the linear motion part to reciprocate along a linear direction. The robotic arm assembly includes multiple links whose ends are connected by rotary joints. The rotary joints can drive the links to rotate around their connection points. The multiple links and multiple rotary joints form a linkage mechanism. One end of the linkage mechanism is connected to the linear motion part via a rotary joint, and the other end of the linkage mechanism is connected to the device assembly via a rotary joint. The linear drive part performs linear motion control on the linear motion part using a spatial kinematics modeling strategy, a zoned trajectory planning strategy, and an adaptive closed-loop control strategy. The rotary joints drive the linkage mechanism, the links, and the device assembly to rotate around their respective connection points using the same spatial kinematics modeling strategy, zoned trajectory planning strategy, and adaptive closed-loop control strategy. The linear motion component and the robotic arm assembly are protected by a full-space safety protection strategy.

[0007] Furthermore, a control system for a longitudinal six-DOF fully automated robotic arm includes: a spatial kinematics modeling module, used to model the configuration of the longitudinal six-DOF robotic arm, consisting of one linear translational degree of freedom and five rotational degrees of freedom, and construct a kinematic model incorporating cantilever elastic deformation compensation. The kinematic model incorporating cantilever elastic deformation compensation is used to solve for the end-effector spatial pose, joint target variables, link deformation compensation, joint limit, and singular configuration constraint data; and a partitioned trajectory planning module, used to, based on the end-effector spatial pose, joint target variables, link deformation compensation, joint limit, and singular configuration constraint data, combine the narrow-angle movement... The system generates a longitudinally progressive continuous motion trajectory for each degree of freedom of the robotic arm and a multi-segment smooth fusion strategy based on the work space zoning rules, the target pose of the work, and the control requirements of the work stage. It outputs the target trajectory commands, zoning control parameters, and trajectory safety constraint parameters for each degree of freedom of the robotic arm. The adaptive closed-loop control module is used to implement master-slave collaborative closed-loop control and adaptive robust correction for the robotic arm based on the target trajectory commands, zoning control parameters, and trajectory safety constraint parameters for each degree of freedom of the robotic arm, combined with real-time sensor feedback data of vision, force, position, and current. This enables the robotic arm to complete the target work action and provides real-time feedback on the end-effector pose and motion state data of the robotic arm.

[0008] Furthermore, the system also includes a full-space safety protection module, which is used to perform graded collision detection, fault identification operation and longitudinal folding active fault-tolerant control on the robotic arm based on the joint limit and singular configuration constraint data, the robotic arm motion state data and multi-sensor monitoring information, output safety intervention commands, and perform real-time constraint and closed-loop control on the robotic arm motion trajectory.

[0009] Furthermore, the method for modeling the longitudinal six-DOF manipulator configuration consisting of one linear translational degree of freedom and five rotational degrees of freedom, and constructing a kinematic model integrating cantilever elastic deformation compensation, includes: establishing a kinematic coordinate system including a base coordinate system, a linear motion coordinate system, a joint coordinate system, and an end-effector coordinate system; defining the system's generalized joint variables as: ,in, For linear motion displacement, Let the angle be the i-th rotational joint angle; construct the homogeneous transformation matrix of the translational degrees of freedom of the linear motion component. : Construct the homogeneous transformation matrix for each rotational joint. :

[0010] ;in, The length of the link. For the connecting rod torsion angle, Given the link offset; establish a segmented link deflection model and calculate the deflection and rotation angle of each link stage: , ,in, Let be the equivalent load on the i-th link segment. For equivalent bending moment, For the length of the rod, For elastic modulus, Given the moment of inertia of the cross section; generate the deformation compensation vector at the end of the robotic arm: ;in, For the first Deflection of the connecting rod segment in the x-direction; For the first Deflection of the connecting rod in the y-direction. For the first The deflection of the connecting rod in the z-direction. For the first The connecting rod rotates at an angle in the x-direction. For the first The connecting rod rotates at an angle in the y-direction. For the first For the connecting rod with rotation angle in the z-direction, the kinematic model with fused deformation compensation is obtained: ,in, Let be the transformation matrix from the base coordinate system to the tool coordinate system. This is the homogeneous coordinate transformation matrix from the linear motion unit to the first rotary joint. Let be the homogeneous coordinate transformation matrix from revolute joint 1 to revolute joint 2. Let be the homogeneous coordinate transformation matrix from revolute joint 2 to revolute joint 3. This is the homogeneous coordinate transformation matrix from revolute joint 3 to revolute joint 4. This is the homogeneous coordinate transformation matrix from revolute joint 4 to revolute joint 5. This is the homogeneous coordinate transformation matrix from the No. 5 rotary joint to the tool coordinate system.

[0011] Furthermore, the method for solving the end-effector spatial pose, joint target variables, link deformation compensation, joint limit, and singular configuration constraint data by fusing the kinematic model of cantilever elastic deformation compensation includes: defining a six-dimensional pose vector for the robotic arm end-effector: ,in, This is the pose vector of the robotic arm's end effector. The coordinates of the robotic arm's end effector position; The attitude angles of the robotic arm's end effector are calculated by separating the position vector and rotation matrix of the robotic arm's end effector from the total homogeneous transformation matrix.

[0012] ; ; ;in, Using the rotation matrix elements from the base coordinate system to the tool coordinate system at the end of the robotic arm, and combining them with the cantilever deformation compensation amount, we obtain the actual pose of the end effector after compensation: ,in, Let be the rigid body kinematic mapping function from joint variables to end-effector pose. This is the compensation amount for link deformation; based on the target end pose of the robotic arm, the joint target variables are solved by inverse solution, and the inverse solution formula is: ,in, The target end effector pose of the robotic arm. The inverse mapping of the positive kinematic function; assigning longitudinal displacement of linear motion: ,in, For the displacement of the target moving in a straight line, The X-axis coordinate of the target at the end of the robotic arm. Let be the angle between the link and the direction of linear motion, and m be the number of links involved in the longitudinal projection. The cosine theorem is used to solve for the rotational joint angle at the front end of the robotic arm, and attitude constraints are used to correct the remaining joint variables. An adaptive Jacobian matrix is ​​obtained by superimposing deformation compensation terms on the rigid body Jacobian matrix. ;in, For the deformation adaptive Jacobian matrix, For an ideal rigid body, the Jacobian matrix is... This is a deformation compensation correction term. This is the set of estimated load, attitude, and other parameters; a damping pseudo-inverse is used to avoid matrix singularity, as shown in the formula: ;in, This is the damped pseudo-inverse of the Jacobian matrix. The damping factor, Let T be the identity matrix and T be the inverted form of the matrix; read the hardware parameters of the robotic arm, set the upper and lower limits of joint angles, velocities, accelerations, and linear motion strokes to obtain joint limit data; calculate the singularity index to determine singular configurations, using the formula: ;in, As a singularity index, For the determinant of a matrix, when When this occurs, it is determined to be a singular configuration, and singular configuration constraint data is generated, where, This is the threshold for determining singularity.

[0013] Furthermore, the method for generating longitudinally progressive continuous motion trajectories and multi-segment smooth fusion strategies for each degree of freedom of the robotic arm based on the end-effector spatial pose, joint target variables, link deformation compensation, joint limit and singular configuration constraint data, combined with the narrow workspace zoning rules, work target pose and work stage control requirements, and outputting the target trajectory commands, zoning control parameters and trajectory safety constraint parameters for each degree of freedom of the robotic arm includes: defining a position distance discrimination function and an attitude error discrimination function. , ;in, This represents the straight-line distance from the current position of the robotic arm's end effector to the target position. This represents the attitude error between the current posture of the robotic arm's end effector and the target posture. and These represent the current position of the robotic arm's end effector and the target position of the robotic arm's end effector, respectively. and These represent the current posture and the target posture of the robotic arm's end effector, respectively. The system is divided into six working regions based on distance and posture error thresholds, with a region determination function as follows:

[0014] ;in, ~ These correspond to the standby retraction zone, long-stroke approach zone, near-end coarse alignment zone, fine alignment zone, low-speed insertion zone, and safe withdrawal zone, respectively. , , , Distance threshold > > > ; The attitude error threshold is used; the acceleration of the main stroke of linear motion is planned using S-shaped acceleration and deceleration: ;in, For real-time acceleration of linear motion, The maximum acceleration for linear motion, To increase the duration of accelerometer action, The total duration of the acceleration phase; generating the rotational joint follow-up trajectory: ;in, Real-time trajectory of the joints; and These are the initial joint angle and the target joint angle, respectively. The normalized trajectory progress function is defined for the 0-1 interval; visual correction and deformation compensation are integrated to obtain the final comprehensive reference trajectory, generating initial trajectory commands for each degree of freedom; a fifth-order polynomial is used to achieve smooth transitions between different regions of the trajectory, with the transition trajectory formula as follows: ;in, For trajectory fusion segment functions; ~ The coefficients are 5th-order polynomials; t is the transition time; the position, velocity, and acceleration of the constraint switching point are continuous to eliminate trajectory switching impact; combined with joint limits and singular configuration constraints, a comprehensive safety cost function is constructed to optimize the trajectory: ;in, Optimize the objective function for safety; ~ These are the weighting coefficients; This is a singular configuration penalty term; It includes obstacle collision penalty items; outputs target trajectory instructions, zone control parameters, and trajectory safety constraint parameters.

[0015] Furthermore, the method for implementing master-slave collaborative closed-loop control and adaptive robust correction on the robotic arm based on the target trajectory commands, zone control parameters, and trajectory safety constraint parameters of each degree of freedom of the robotic arm, combined with real-time sensor feedback data of vision, force, position, and current, to enable the robotic arm to complete the target operation and provide real-time feedback of the robotic arm's end-effector pose and motion state data, includes: defining linear motion displacement error and velocity error. , ,in, This represents the linear motion displacement error. For linear motion speed error, , This represents the actual displacement during linear motion. The velocity of the target moving in a straight line. The actual velocity of the linear motion is given; a PID control law for linear motion with superimposed friction and yaw compensation is constructed: ; ,in, Output from the linear drive unit. , and For PID control parameters, , and These are compensation items for friction, straightness, and long-stroke runout. The coefficient of friction is Coulomb. The coefficient of viscous friction is... Define the single-joint angle error as a sign function. Construct a joint torque control law that superimposes gravity and friction compensation. Joint friction compensation item is ;in, For joint torque output, For gravity compensation, and For joint friction parameters; correct joint commands based on robot arm end-effector pose error. , , ,in, This refers to the pose error of the robotic arm's end effector. For correction amount, For the original reference trajectory, For the corrected control commands; total control output From basic items Adaptive terms and robust items composition, Basic items Adaptive terms Parameter update law Robust item ,in, and For feedback gain, For the regression matrix, For online parameter estimation, For adaptive gain, The robustness coefficient is... It is a saturation function. The boundary layer width is used; weighted fusion is employed to integrate multi-channel sensor feedback data. , To integrate the estimation results, These are vision, force sensing, current, and position feedback, respectively. These are the weights of each channel; determined by the formula. To determine whether an assignment is completed, use a decision function. Determine if there is an operational abnormality, and collect and output data on the end-effector pose and overall motion status of the robotic arm throughout the entire process.

[0016] Furthermore, the method for performing graded collision detection, fault identification, and longitudinal folding active fault-tolerant control on the robotic arm based on the joint limit and singular configuration constraint data, the robotic arm motion state data, and multi-sensor monitoring information, and outputting safety intervention commands, and for real-time constraint and closed-loop control of the robotic arm's motion trajectory includes: constructing a folding and contraction optimization objective function. The constraints are , ;in, To optimize the objective function for folding, These are the weighting coefficients. This refers to the envelope length of the robotic arm after folding. For the folded target joint vector, This is the length of the pullback path. The distance between the robotic arm and the obstacle. For safe distance threshold, Protect the equipment components; construct a comprehensive collision risk index. ; ;in, To assess the overall collision risk value, , , and These are risk items for position, current, force, and pose error, based on thresholds. right Divide into, It is divided into three levels: Level 1 warning, Level 2 detection, and Level 3 emergency response, and corresponding deceleration, shutdown, and emergency contraction intervention commands are output.

[0017] Furthermore, the method for performing graded collision detection, fault identification, and longitudinal folding active fault-tolerant control on the robotic arm based on the joint limit and singular configuration constraint data, the robotic arm motion state data, and multi-sensor monitoring information, and outputting safety intervention commands to perform real-time constraint and closed-loop control on the robotic arm's motion trajectory, also includes: defining a system health state vector and a comprehensive health degree. , ,in, The health status vector sequentially represents the working status of vision, force sensing, current, position, and actuator. Overall health status These are weighting coefficients. System faults are determined in real time; the fault-tolerant control law is as follows: Online self-tuning formula for parameters ,in, For fault-tolerant control output, For fault tolerance gain, For the safe target pose, For learning rate, and These are the actual observed values ​​and the model predicted values, respectively. The feature vector is used for regression; the robotic arm's trajectory is corrected in real time by combining joint limits, singular constraints, and safety intervention commands.

[0018] Furthermore, a control method for the control system of the aforementioned longitudinal six-degree-of-freedom fully automatic robotic arm includes: modeling the configuration of the longitudinal six-degree-of-freedom robotic arm, consisting of one linear translational degree of freedom and five rotational degrees of freedom; constructing a kinematic model incorporating cantilever elastic deformation compensation; solving for the end-effector spatial pose, joint target variables, link deformation compensation, joint limit, and singular configuration constraint data using the kinematic model incorporating cantilever elastic deformation compensation; and based on the end-effector spatial pose, joint target variables, link deformation compensation, joint limit, and singular configuration constraint data, combined with the narrow working space... Based on the zone rules, target pose, and control requirements of the operation stage, a longitudinally progressive continuous motion trajectory and a multi-segment smooth fusion strategy for each degree of freedom of the robotic arm are generated. The target trajectory commands, zone control parameters, and trajectory safety constraint parameters for each degree of freedom of the robotic arm are output. Based on the target trajectory commands, zone control parameters, and trajectory safety constraint parameters for each degree of freedom of the robotic arm, combined with real-time sensor feedback data of vision, force, position, and current, master-slave collaborative closed-loop control and adaptive robust correction are implemented on the robotic arm to enable the robotic arm to complete the target operation and provide real-time feedback of the end-effector pose and motion state data of the robotic arm.

[0019] The aforementioned spatial kinematics modeling strategy corresponds to the spatial kinematics modeling module, the partitioned trajectory planning strategy corresponds to the partitioned trajectory planning module, the adaptive closed-loop control strategy corresponds to the adaptive closed-loop control module, and the full-space safety protection strategy corresponds to the full-space safety protection module. That is, each strategy has the same data processing method as each module and they correspond one-to-one, thereby realizing the safe closed-loop control of the longitudinal six-degree-of-freedom fully automatic robotic arm by the control system.

[0020] The beneficial effects of this invention are: simplified and controllable configuration, balancing cost and versatility: This invention adopts a longitudinal six-degree-of-freedom serial configuration of "one linear translational degree of freedom and five rotational degrees of freedom", which simplifies the number of joints and the complexity of the mechanical structure and control system compared with high-degree-of-freedom general-purpose robotic arms, effectively reducing equipment manufacturing and maintenance costs; the end effector can be quickly replaced, compatible with various operation scenarios such as automatic vehicle charging, port material handling, and industrial flexible assembly, solving the technical problem of difficulty in balancing degrees of freedom, cost, and versatility in traditional solutions.

[0021] Precise compensation for cantilever deformation and high precision in end-effector operations: This invention establishes a spatial kinematic model that integrates cantilever elastic deformation compensation. Through a segmented link flexure deformation model, the deflection and rotation angle of each link are calculated in real time. Feedforward correction is performed on the end-effector pose error caused by the arm's own weight sag and load deformation. Combined with deformation adaptive Jacobian matrix and damping pseudo-inverse algorithm to avoid singular configurations, it effectively offsets the accumulation of long-stroke errors, significantly improves end-effector positioning accuracy and attitude control stability, and ensures the reliable execution of high-precision operations such as charging plug-in and precision docking.

[0022] Zoned progressive trajectory planning adapts to narrow and long work scenarios: For narrow and long work spaces with long longitudinal strokes, a zoned trajectory planning strategy is adopted, dividing the work process into six functional areas: standby retraction, long stroke approach, near-end coarse alignment, fine alignment, low-speed insertion, and safe withdrawal. Different speed and precision control strategies are matched for different areas. In the long stroke stage, linear guides are used as the main drive to ensure work efficiency, while in the near-end stage, joint posture fine adjustment is emphasized to ensure docking accuracy. The smooth transition of multiple trajectory segments is achieved through a fifth-order polynomial, eliminating the motion impact of area switching and balancing work efficiency and motion stability.

[0023] Master-slave collaborative closed-loop control with strong system robustness: A hierarchical collaborative control architecture of "guide rail master drive + joint slave follow" is constructed. The bottom layer control superimposes friction compensation, gravity compensation, and long stroke yaw compensation to suppress low-speed crawling and attitude drift. The upper layer control integrates feedback data from multiple sensors such as vision, force, position, and current, and combines adaptive parameter estimation and robust correction algorithms to dynamically correct joint control commands. It can adapt to load changes and external disturbances, effectively improving multi-axis synchronization accuracy and system anti-interference capability.

[0024] The longitudinal folding storage design offers excellent space adaptability: Equipped with a longitudinal folding retraction control strategy, in non-operating or abnormal working conditions, the guide rail retracts to a safe position and each rotating joint retracts sequentially, significantly reducing the longitudinal envelope size of the equipment, making it suitable for installation scenarios with limited space such as underground garages, parking lots, and narrow workstations; after retraction, the end effector enters the protection zone, which can effectively reduce the risk of bump damage in non-operating conditions and extend the service life of the equipment.

[0025] Comprehensive hierarchical safety protection with high reliability for unattended operation: A comprehensive safety protection system is established, integrating position boundaries, motor current, end effector force, and posture error to construct a comprehensive collision risk index, realizing a hierarchical intervention mechanism of first-level early warning deceleration, second-level detection shutdown, and third-level emergency contraction; equipped with full-link fault-tolerant control, when a sensor or actuator experiences a partial failure, it can perform degraded control and safety retreat, ensuring equipment safety and operational reliability under unattended conditions, and reducing the risk of accidents and damage. Attached Figure Description

[0026] To more clearly illustrate the technical solutions of the embodiments disclosed in this invention, the accompanying drawings of the embodiments will be briefly described below. These drawings are for illustrative purposes only and are not intended to limit the scope of protection of this invention.

[0027] Figure 1 The three-dimensional unfolding structure of the longitudinal six-degree-of-freedom fully automatic robotic arm in this invention. Figure 1 ;

[0028] Figure 2 The three-dimensional unfolding structure of the longitudinal six-degree-of-freedom fully automatic robotic arm in this invention. Figure 2 ;

[0029] Figure 3 The three-dimensional unfolding structure of the longitudinal six-degree-of-freedom fully automatic robotic arm in this invention. Figure 3 ;

[0030] Figure 4 This is a perspective view of the folding structure of the longitudinal six-degree-of-freedom fully automatic robotic arm in this invention;

[0031] Figure 5 This is a perspective view of the gun head in this invention;

[0032] Figure 6 This is a schematic diagram of the module connection of the control system of the longitudinal six-degree-of-freedom fully automatic robotic arm in this invention;

[0033] Figure 7 This is a flowchart of the control method for the longitudinal six-degree-of-freedom fully automatic robotic arm in this invention;

[0034] Among them, 1—linear motion assembly (1.1—linear guide rail, 1.2—lead screw, 1.3—sliding base, 1.4—lead screw mounting seat, 1.5—limit transmission nut), 2—robotic arm assembly (2.1—first rotary joint, 2.2—first connecting seat, 2.3—second rotary joint, 2.4—second connecting seat, 2.5—third rotary joint, 2.6—first connecting rod, 2.7—fourth rotary joint, 2.8—second connecting rod, 2.9—fifth rotary joint), 3—gun head assembly (3.1—gun head, 3.2—gun head mounting seat, 3.3—cable, 3.4—camera, 3.5—weighing sensor). Detailed Implementation

[0035] The technical solutions (including preferred technical solutions) of the present invention will be further described in detail below with reference to the accompanying drawings and by listing some optional embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. 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. In the description of the present invention, it should be understood that the terms "upper," "lower," "front," "rear," "left," "right," "top," "bottom," "inner," and "outer," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the present invention.

[0036] Example 1

[0037] This embodiment discloses a longitudinal six-degree-of-freedom fully automated robotic arm, whose unfolded three-dimensional structure is as follows: Figure 1 , Figure 3 As shown, the folding and storage structure is as follows Figure 4 As shown, the structure of the end-mounted gun head assembly 3 is as follows: Figure 5 As shown.

[0038] The robotic arm includes a linear motion component 1 and a robotic arm component 2 mounted on the linear motion component 1. The end of the robotic arm component 2 is connected to a device component. In this embodiment, the device component is a gun head component 3. In practical applications, it can be quickly replaced with a clamping component, a conveying suction cup, or other execution end, adapting to various operation scenarios such as automatic vehicle charging, material handling, and flexible assembly.

[0039] The linear motion assembly 1 includes a linear motion section capable of reciprocating along a linear direction and a linear drive section driving the linear motion section. Specifically, the linear motion assembly 1 includes a linear guide rail 1.1, a lead screw 1.2, a sliding base 1.3, a lead screw mounting seat 1.4, and a limiting transmission nut 1.5; wherein the sliding base 1.3 constitutes the linear motion section, and the lead screw 1.2, the lead screw mounting seat 1.4, and the drive motor (not shown in the figure) constitute the linear drive section. The sliding base 1.3 is slidably mounted on the linear guide rail 1.1, and the limiting transmission nut 1.5 is fixed to the bottom of the sliding base 1.3 and threadedly engaged with the lead screw 1.2; the lead screw mounting seat 1.4 is fixed to both ends of the linear guide rail 1.1 to support the rotation of the lead screw 1.2. When the drive motor drives the lead screw 1.2 to rotate, the limiting transmission nut 1.5 and the sliding base 1.3 are driven by threaded transmission to perform longitudinal reciprocating linear motion along the extension direction of the linear guide rail 1.1, constituting one linear translational degree of freedom of the robotic arm.

[0040] The robotic arm assembly 2 includes multiple links connected at both ends by rotary joints. These rotary joints and links are connected in series to form a linkage mechanism, comprising five rotational degrees of freedom, which, together with the one translational degree of freedom of the linear motion component, constitute a longitudinal six-degree-of-freedom configuration. Specifically, the robotic arm assembly 2 includes a first rotary joint 2.1, a first connecting seat 2.2, a second rotary joint 2.3, a second connecting seat 2.4, a third rotary joint 2.5, a first link 2.6, a fourth rotary joint 2.7, a second link 2.8, and a fifth rotary joint 2.9.

[0041] The assembly and motion relationships are as follows: The fixed end of the first rotary joint 2.1 is fixedly mounted on the sliding base 1.3, and the output end of the first rotary joint 2.1 is fixedly connected to the first connecting seat 2.2, which can drive the first connecting seat 2.2 to rotate around the vertical axis; the fixed end of the second rotary joint 2.3 is fixed to the first connecting seat 2.2, and the output end of the second rotary joint 2.3 is fixedly connected to the second connecting seat 2.4, which can drive the second connecting seat 2.4 to pitch around the horizontal axis; the fixed end of the third rotary joint 2.5 is fixed to the second connecting seat 2.4, and the third rotary joint... The output end of section 2.5 is fixedly connected to the proximal end of the first link 2.6, which can drive the first link 2.6 to rotate around its connection point with the third rotary joint 2.5; the fixed end of the fourth rotary joint 2.7 is fixed to the distal end of the first link 2.6, and the output end of the fourth rotary joint 2.7 is fixedly connected to the proximal end of the second link 2.8, which can drive the second link 2.8 to pitch and swing around the horizontal axis; the fixed end of the fifth rotary joint 2.9 is fixed to the distal end of the second link 2.8, and the output end of the fifth rotary joint 2.9 is connected to the gun head assembly 3, which can drive the gun head assembly 3 to rotate.

[0042] The gun head assembly 3, as described Figure 5 As shown, the device includes a gun head 3.1, a gun head mounting base 3.2, a cable 3.3, a camera 3.4, and a load cell 3.5. The gun head mounting base 3.2 is fixedly connected to the output end of the fifth rotary joint 2.9. The load cell 3.5 is located between the gun head mounting base 3.2 and the gun head 3.1 and is used to detect the contact force and load on the gun head 3.1 in real time. The camera 3.4 is fixedly mounted on the side of the gun head mounting base 3.2 and is used to acquire image information of the target and achieve visual positioning and attitude alignment. The cable 3.3 is used to power the gun head 3.1, the camera 3.4, and various sensors and to transmit data signals.

[0043] In this embodiment, the robotic arm can be folded and retracted longitudinally when not in operation, such as... Figure 4As shown: the sliding base 1.3 retracts to the starting end of the linear guide 1.1, and the first link 2.6 and the second link 2.8 retract sequentially through the rotation of each rotary joint, so that the gun head assembly 3 is retracted to the protection area close to the sliding base 1.3, which greatly reduces the longitudinal space occupied, making it suitable for narrow installation scenarios such as underground garages, narrow work positions, and parking lot pillars, while avoiding damage to the end effector and improving the durability of the equipment.

[0044] Example 2

[0045] This embodiment discloses a control system for controlling the longitudinal six-degree-of-freedom fully automatic robotic arm described in Embodiment 1. The system module connection relationship is as follows: Figure 6 As shown, it includes a spatial kinematics modeling module, a zoned trajectory planning module, an adaptive closed-loop control module, and a full-space safety protection module.

[0046] The spatial kinematics modeling module is used to perform kinematic modeling on a longitudinal six-DOF (one linear translational degree of freedom + five rotational degrees of freedom) robotic arm configuration, constructing a kinematic model that integrates cantilever elastic deformation compensation. The module takes as input the robotic arm hardware parameters, load parameters, and real-time joint variables, and outputs the end-effector spatial pose, joint target variables, link deformation compensation, joint limit data, and singular configuration constraint data. This module simultaneously performs forward kinematics calculation, inverse kinematics solution, deformation adaptive Jacobian matrix calculation, and singular configuration determination, providing a kinematic mathematical foundation for subsequent trajectory planning and motion control.

[0047] The partitioned trajectory planning module receives end-effector pose, joint variables, and constraint data from the spatial kinematics modeling module, as well as target pose and operation stage commands from the host computer. This module incorporates a narrow workspace partitioning rule, dividing the workspace into six functional areas: a standby retraction zone, a long-stroke approach zone, a near-end coarse alignment zone, a fine alignment zone, a low-speed insertion zone, and a safe exit zone. It generates longitudinally progressive continuous motion trajectories for each degree of freedom of the robotic arm based on the speed and precision control requirements of each area, and achieves smooth fusion of multiple trajectory segments through fifth-order polynomial interpolation. The final output includes target trajectory commands for each degree of freedom, partition control parameters, and trajectory safety constraint parameters.

[0048] The adaptive closed-loop control module receives target trajectory commands, zone control parameters, and safety constraint parameters from the zoned trajectory planning module at its input end, and simultaneously integrates real-time feedback data from multiple sensors including vision, force, position, and current sensors. This module employs a hierarchical control architecture: the bottom layer executes master-slave collaborative closed-loop control of the guide rails and joints, while the upper layer performs adaptive robust correction. Through feedforward compensation, online parameter estimation, and robust suppression terms, it achieves high-precision trajectory tracking and dynamic end-effector pose correction. The output consists of drive control commands for each degree of freedom, driving the robotic arm to complete the target operation, while simultaneously providing real-time feedback on the end-effector pose and overall motion status data.

[0049] The full-space safety protection module receives joint limit and singular configuration constraint data, real-time motion status data of the robotic arm, and multi-sensor monitoring information at its input end. This module incorporates a hierarchical collision detection mechanism, fault identification algorithm, and a longitudinally folding active fault-tolerant control strategy. It can calculate comprehensive collision risk indicators in real time, classify safety levels into three levels: Level 1 warning, Level 2 detection, and Level 3 emergency, and output corresponding intervention commands. In the event of sensor or actuator failure, it automatically triggers fault-tolerant control and safety retreat, performing real-time constraints and closed-loop corrections on the robotic arm's trajectory to ensure equipment and scene safety throughout the entire operation.

[0050] Example 3

[0051] This embodiment discloses a control method based on the control system described in Embodiment 2. The overall control flow is as follows: Figure 7 As shown. This method is based on the serial configuration of a longitudinal six-degree-of-freedom robotic arm, and sequentially completes kinematic modeling and deformation compensation, zoned trajectory planning, adaptive closed-loop control, and full-domain safety protection to achieve high-precision and high-safety fully automated operation. The specific steps, corresponding formulas, and the meanings of all symbols of the method are explained in detail below:

[0052] Step S1: Spatial kinematics modeling and constraint solution

[0053] This step establishes a kinematic model that integrates cantilever elastic deformation compensation, solves the end pose, joint target quantities and safety constraints, and provides a mathematical basis for subsequent trajectory planning and motion control.

[0054] S1.1 Establishing a kinematic coordinate system and generalized joint variables

[0055] Define a four-level coordinate system: the base coordinate system {0}, fixed at the center of the starting end face of the linear guide 1.1; the linear motion coordinate system {L}, fixed at the geometric center of the sliding base 1.3, and translated synchronously with the sliding base; the joint coordinate system {i} (i=1,2,3,4,5), set sequentially at the rotation axis of each rotary joint; and the end tool coordinate system {e}, fixed at the end center of the gun head 3.1.

[0056] Define the system's generalized joint variable vector:

[0057] Where q is the system's generalized joint variable vector, with dimensions 6×1, containing one translation and five rotations; s is the linear displacement, i.e., the longitudinal translation of the sliding base 1.3 relative to the origin of the base coordinate system, in meters. (i=1,2,3,4,5) represents the rotation angle of the i-th rotary joint, in rad.

[0058] S1.2 Constructing homogeneous transformation matrices at each level

[0059] Construct the homogeneous transformation matrix for the linear translation degrees of freedom, i.e., the pose transformation from the base coordinate system to the linear motion coordinate system:

[0060] ;in, This is the transformation matrix from the base coordinate system to the moving coordinate system of the guide rail; This represents the displacement of the linear guide rail;

[0061] Construct the DH standard homogeneous transformation matrix for each rotational joint, that is, the pose transformation from the coordinate system of joint i, 1 to the coordinate system of joint i:

[0062] ;in, Let be the homogeneous transformation matrix from coordinate system i,1 to coordinate system i; The length of the i-th link is the length of the common perpendicular of the axes of two adjacent joints, in meters. The offset of the i-th link is the distance between two adjacent common perpendiculars along the joint axis, expressed in meters (m).

[0063] The total transformation matrix from the base coordinate system to the end-tool coordinate system is the product of the transformation matrices at each level:

[0064] ;in, This is the transformation matrix from the base coordinate system to the end-tool coordinate system; This is the fixed homogeneous transformation matrix from the linear motion coordinate system to the coordinate system of the first rotary joint; These are the homogeneous transformation matrices between adjacent rotational joint coordinate systems, in sequence. This is the fixed-installation homogeneous transformation matrix from the coordinate system of rotary joint No. 5 to the coordinate system of the end tool.

[0065] S1.3 Segmented Cantilever Elastic Deformation Modeling

[0066] To address the self-weight and end-load deformation of the longitudinal cantilever structure, a segmented link deflection deformation model was established, and the deflection and rotation angle of each link were calculated segment by segment.

[0067] Formula for the lateral deflection at the end of the i-th link segment: The formula for the final rotation angle of the i-th link segment is: ;in, Let be the linear deflection value at the end of the i-th link segment, in meters (m). denoted as angular deflection at the end of the i-th link segment, in rad; The equivalent lateral load on the i-th link segment includes the equivalent components of the link's own weight and the end load, and is expressed in N. The equivalent bending moment on the i-th link segment is expressed in N·m. is the effective cantilever length of the i-th link segment, in meters; Let be the elastic modulus of the material of the i-th connecting rod segment, in Pa; Let be the moment of inertia of the cross section of the i-th connecting rod, in m4.

[0068] The deformations of each link are projected onto the end-effector coordinate system and summed to obtain the six-dimensional deformation compensation vector at the end of the robotic arm:

[0069] ;in, This is the end-point six-dimensional deformation compensation vector, with dimensions of 6×1. The first three terms are for linear displacement compensation, and the last three terms are for angular displacement compensation. , , Let be the linear deflection components of the i-th link segment in the x, y, and z coordinate axes, in meters (m). , , Let be the angular components of the deformation of the i-th link segment in the x, y, and z coordinate axes, in rad; n is the total number of link segments.

[0070] The compensated end-effector pose can be expressed as: ; The end pose is obtained from uncompensated kinematics calculation; The end pose after compensation.

[0071] Solving the forward kinematics of S1.4

[0072] Define the six-dimensional pose vector of the robotic arm's end effector: ;in, is the six-dimensional pose vector of the end effector, with dimensions of 6×1; x, y, and z are the three-dimensional position coordinates of the center point of the end effector in the base coordinate system, in meters; , , The three-dimensional attitude angles at the end point are represented by ZYX Euler angles, in rad.

[0073] From the total transformation matrix The position vector and the 3rd order rotation matrix R are separated from the original vector. The elements of the rotation matrix are denoted as... (i,j=1,2,3), the three attitude angles are solved by rotating the matrix elements:

[0074] ;in, Let be the element in the i-th row and j-th column of the rotation matrix R; (y, x) is a two-parameter arctangent function. The quadrant of the angle can be determined by the signs of x and y, and its range is (π, π). By superimposing the cantilever deformation compensation, the actual pose of the end effector after compensation is obtained: ; This is a rigid body positive kinematics mapping function, which takes joint variables as input and outputs the end pose under the rigid body assumption.

[0075] S1.5 Inverse Kinematics Solution

[0076] Given the target end pose Solve for the corresponding target joint variables. The general expression is: ; The target's end-point six-dimensional pose vector; For the target generalized joint variable vector; This is the inverse mapping of the positive kinematics function. During the solution process, the longitudinal target displacement of the linear motion is allocated first. ; The target displacement for linear motion, in meters; The x-axis coordinate of the end-target pose is in meters. The length of the i-th link participating in the longitudinal projection, in meters; Let be the spatial angle between the i-th link and the direction of linear motion, in rad; The number of links involved in the longitudinal projection is given. After obtaining the target value of linear displacement, the angle of the front boom joint is solved using the law of cosines. Then, combined with the end effector attitude constraints, the remaining joint variables are corrected through numerical iteration to finally obtain the target values ​​of all joints.

[0077] S1.6 Deformation Adaptive Jacobian Matrix and Singular Configuration Determination

[0078] Rigid body Jacobian matrix The linear mapping relationship from joint velocity to end-effector velocity is described, and the deformation adaptive Jacobian matrix is ​​obtained after superimposing the deformation compensation term:

[0079] ; The deformable adaptive Jacobian matrix has a dimension of 6×6; The Jacobian matrix under the assumption of an ideal rigid body; This is a deformation compensation correction term, which is related to the current joint posture and load parameters; This is the set of estimated load, attitude, and other parameters.

[0080] To avoid matrix non-invertibility under singular configurations, a damped pseudo-inverse form is used to calculate the Jacobian inverse matrix: ; The damped pseudo-inverse of the adaptive Jacobian matrix has a dimension of 6×6. is the damping factor, a positive real number, used to smooth the solution results in singular regions and avoid abrupt velocity changes; It is a 6th order identity matrix; the superscript T is the matrix transpose.

[0081] Define a singularity index to determine singular configurations: ; This is a singularity index; the smaller the value, the closer it is to a singular configuration. For matrix determinant operations; when When the robotic arm is determined to be near a singular configuration, singular configuration constraint data is generated, in which... This is the preset singularity threshold.

[0082] Finally, the hardware parameters of the robotic arm are read, and the upper and lower limits of joint angle, angular velocity, angular acceleration, and linear travel are set to obtain joint limit constraint data.

[0083] Step S2: Zonal Trajectory Planning

[0084] This step combines the rules of narrow working space partitioning to generate continuous motion trajectories for each degree of freedom, taking into account both long-stroke work efficiency and near-end precision operation accuracy.

[0085] S2.1 Job Space Partition Determination

[0086] Define the end-position distance and attitude error discrimination function:

[0087] ; ; This represents the Euclidean distance from the current position of the endpoint to the target position, in meters (m). The error norm from the current attitude to the target attitude is expressed in rad. , These are the current position vector of the end point and the target position vector, respectively, both with a dimension of 3×1; , These are the current attitude angle vector of the end effector and the target attitude angle vector, respectively, both with a dimension of 3×1.

[0088] Based on distance threshold With attitude error threshold Define the partition determination function: ;

[0089] ~ These correspond sequentially to the standby retraction zone, long-stroke approach zone, near-end coarse alignment zone, fine alignment zone, low-speed insertion zone, and safe withdrawal zone. The preset four-level distance threshold is satisfied. The unit is meters (m). This is the preset attitude error threshold, in rad.

[0090] S2.2 Vertical Progressive Trajectory Generation

[0091] A progressive generation logic of "guide rail main stroke pre-planning + joint posture follow-up" is adopted. The linear motion main stroke adopts S-shaped acceleration and deceleration planning, and the acceleration expression of the acceleration segment is:

[0092] ;

[0093] This refers to the real-time acceleration of linear motion; This is the maximum allowable acceleration for linear motion; This refers to the duration of the acceleration / deceleration, i.e., the transition time during the acceleration / deceleration phase. This is the total duration of the acceleration phase.

[0094] Integrating the acceleration over time yields the velocity curve, and integrating again yields the guide rail displacement curve. The follow-up trajectories of each rotary joint are generated using a normalized progress synchronization method: ; Let be the real-time angle trajectory of the j-th rotational joint. Let be the initial angle and the target angle of the j-th joint, respectively. The normalized trajectory progress function has a value range of [0,1] and is synchronized with the linear motion progress.

[0095] Fusion visual correction amount With deformation compensation amount The final composite reference trajectory is obtained:

[0096] ; This is the joint reference trajectory vector.

[0097] S2.3 Multi-segment Trajectory Smooth Fusion

[0098] A fifth-order polynomial is used to achieve a smooth transition at the point where adjacent regions switch trajectories. The trajectory expression for the transition segment is as follows:

[0099] ; This is the joint trajectory function for the transition segment; ~ These are the six undetermined coefficients of a fifth-degree polynomial; As the time variable of the transition segment, by constraining the position, velocity, and acceleration of the start and end points of the transition segment to be continuous, six coefficients are solved to ensure that the trajectory switching is shock-free and jitter-free.

[0100] Trajectory optimization under S2.4 safety constraints

[0101] Construct a comprehensive safety cost function to perform global trajectory optimization:

[0102] ; To optimize the objective function for safety, the smaller the value, the better the overall trajectory performance. ~ These are the weighting coefficients for each item, all of which are positive real numbers; This is a singular configuration penalty term, which increases rapidly as the configuration approaches a singular configuration; q is a joint constraint (which must be within the specified range). This is a penalty for obstacle collisions; its value increases rapidly as the vehicle approaches an obstacle. X represents the joint velocity (within the specified range), and X represents the actual pose of the robotic arm's end effector. The target pose of the robotic arm's end effector.

[0103] Step S3: Adaptive Closed-Loop Control

[0104] This step employs a hierarchical control architecture to achieve high-precision trajectory tracking and robust motion control.

[0105] S3.1 Bottom Layer: Master-Slave Cooperative Control of Guide Rails and Joints

[0106] (1) Main drive control for linear motion

[0107] Define displacement error and velocity error for linear motion:

[0108] ; ;in, This is the linear displacement error. For linear velocity error, These are the target displacement and actual displacement of the guide rail, respectively. These are the target speed and the actual speed of the guide rail, respectively.

[0109] Construct a PID control law that superimposes friction, straightness, and yaw compensation:

[0110] The friction compensation item is as follows: ;in, This refers to the control output of the linear drive unit. , , These are the proportional, derivative, and integral gains of the linear PID controller, respectively. , , These are friction compensation, straightness error compensation, and long-stroke runout compensation. The coefficient of friction is Coulomb. The coefficient of viscous friction is... This is a sign function; it outputs 1 when the input is positive, 1 when the input is negative, and 0 when the input is 0.

[0111] (2) Rotational joint controlled by follower

[0112] Define single joint angle error:

[0113] ; Let be the angle error of the i-th joint. and These are the target angle and the actual feedback angle of the i-th joint, respectively.

[0114] Constructing a joint torque control law that combines gravity and friction compensation:

[0115] The joint friction compensation item is: ; Let be the torque output of the i-th joint. , , These are the proportional, derivative, and integral gains of the joint PID controller, respectively. This is a gravity compensation term used to counteract the joint torque generated by the arm's own weight. This is a joint friction compensation item. The coefficient of friction of the joint is the Coulomb friction coefficient. The coefficient of viscous friction of the joint. Let be the actual angular velocity of the i-th joint.

[0116] (3) Master-slave cooperative pose correction

[0117] Define the end-effector pose error vector:

[0118] Joint corrections are calculated based on adaptive Jacobi pseudo-inverse. The revised final control command is as follows: ;in, This is the six-dimensional pose error vector at the end point. This is the joint correction vector. To coordinate the correction of the gain matrix, The joint vectors of the original reference trajectory. This is the corrected joint control command vector.

[0119] S3.2 Upper Layer: Adaptive Robust Coordination Control

[0120] The total control output consists of three parts: the basic feedback term, the adaptive compensation term, and the robust suppression term.

[0121] The basic feedback items are: ;in, Based on the closed-loop control term, , These are the position feedback gain and velocity feedback gain matrices, respectively. This is the rate of change vector of the end-effector pose error.

[0122] The adaptive compensation term is: The online parameter update law is: ;in, This is an adaptive compensation term used for online compensation of unknown parameters and load changes. For the system regression matrix, For the parameter vector estimated online, It is an adaptive gain matrix and a symmetric positive definite matrix. Let be the update rate of the parameter vector. The robust compensation term is: ; This is a robust compensation term used to suppress external disturbances and modeling errors. The robustness coefficient is a positive real number. This is a saturation function used to avoid control quantity chattering. is the boundary layer width, a positive real number.

[0123] S3.3 Multi-sensor data fusion and job determination

[0124] The system integrates visual, force, electrical, and position sensor data using a weighted fusion method.

[0125] Satisfy the weight normalization constraint: .in, The results are the merged system state estimation results; , , , These are feedback quantities from vision, force sensing, current, and position, respectively. , , , These correspond to the weighting coefficients of each sensing channel.

[0126] Task completion criteria (must be met simultaneously):

[0127] ;

[0128] Operational anomaly detection criteria (meeting any one of them is sufficient):

[0129] ;

[0130] in, For the depth of the end insertion, The threshold for determining the insertion depth. For end contact force, , These represent the upper and lower limits of normal contact force. This is the operating current of the drive motor. This is the normal operating current threshold. , , These are the abnormal thresholds for force, current, and pose error, respectively.

[0131] Step S4: Full-space safety protection and fault tolerance

[0132] S4.1 Longitudinal folding and shrinkage control

[0133] Construct the folding and shrinking optimization objective function:

[0134] ;

[0135] Constraints: ; ;in, To optimize the objective function for folding and contraction, This represents the longitudinal envelope length of the folded robotic arm. For the folded target joint vector, This represents the total length of the pullback path. , , These are the weighting coefficients for each item. The minimum distance between the robotic arm and the obstacle. For safe distance threshold, This is the pose of the end after folding. This is a set of end-protection regions.

[0136] S4.2 Graded Collision Detection

[0137] Construct a comprehensive collision risk index:

[0138] ;

[0139] in, ;

[0140] The overall collision risk value is determined by the number of collisions; a higher value indicates a higher risk. , , , These are the risk sub-items: location boundary, current anomaly, force anomaly, and pose error. , , , These are the weighting coefficients for each sub-item. This serves as a reference location for the joint boundary. Allow margin at the boundary, , , These are the normalized reference values ​​for current, force, and error, respectively.

[0141] Set three threshold levels Implement tiered intervention:

[0142] when When this occurs, a Level 1 warning is triggered, and motion deceleration is implemented;

[0143] when When this occurs, a secondary detection is triggered, resulting in deceleration and a stop or a slight reversal.

[0144] when When this occurs, a Level 3 emergency response is triggered, and an emergency shutdown and folding / retraction are performed.

[0145] S4.3 Fault-Tolerant Control

[0146] Define the system health state vector: ;

[0147] Calculate overall health score: ;

[0148] in, The health status vector consists of components that represent the health status of vision, force sensing, current, position, and actuator, with values ​​ranging from 0 to 1. For the overall health of the system; The weighting coefficients for each health component.

[0149] when When a system fault is detected, the system enters fault-tolerant control mode.

[0150] The fault-tolerant control law is: ;

[0151] Online parameter self-tuning formula: ;

[0152] in, For fault-tolerant control output; This is the fault-tolerant control gain matrix; The preset safe target pose; This represents the parameter estimate at time k. Let be the learning rate, a positive real number; This represents the actual observed value at time k. This is the model prediction value at time k; Let be the regression feature vector at time k.

[0153] By combining joint limits, singular constraints, and safety intervention commands, the robot arm's motion trajectory is corrected in real time, forming a closed-loop safety control throughout the entire process, until the robot arm folds and retracts to return to standby mode after the task is completed.

[0154] In summary, this embodiment presents a complete closed-loop control method for a longitudinal six-degree-of-freedom robotic arm, which consists of four core steps, forming a complete process of modeling, planning, control, and safety protection.

[0155] The first step is kinematic modeling and deformation compensation: a multi-level coordinate system with linear translation and five rotational joints is constructed, and a homogeneous transformation matrix is ​​built using the DH method; a piecewise cantilever deflection model is established, and the link deflection and rotation angle are calculated, and the six-dimensional deformation compensation at the end is accumulated. The end pose is calculated through forward motion, combined with deformation correction, and the damped pseudo-inverse Jacobian is used to avoid singular configurations. Constraint data such as joint stroke and singular regions are output simultaneously to solve the positioning deviation caused by the self-weight deformation of the long cantilever.

[0156] The second step is zoned trajectory planning: based on the end-effector distance and attitude error, the workspace is divided into six major areas. For long strokes, an S-shaped acceleration / deceleration guide rail is used as the main motion, with joints moving synchronously. Zone switching uses fifth-order polynomial interpolation to ensure continuous speed and acceleration without impact. A safety cost function containing singularity and obstacle avoidance penalty terms is constructed to optimize the trajectory, outputting the target trajectory for each axis and zone control parameters, balancing long-distance movement efficiency and near-end docking accuracy.

[0157] The third step is adaptive hierarchical closed-loop control: the bottom layer guide rails and joints adopt PID master-slave collaborative control with friction and gravity compensation, and correct joint commands in real time through adaptive Jacobian pseudo-inverse; the upper layer integrates multi-sensor weighted feedback of vision, force, current and position, and splits the control output into three items: basic feedback, adaptive parameter compensation and robust disturbance rejection, to suppress load and external disturbances, and continuously collect end pose to determine whether the insertion is completed or the operation is abnormal.

[0158] The fourth step is comprehensive safety and fault-tolerant management: real-time calculation of comprehensive collision risk indicators, and graded implementation of deceleration, shutdown, and folding / retraction interventions; establishing an equipment health assessment mechanism, and activating fault-tolerant control laws to drive the robotic arm to fold to a safe protection area when sensors or motors fail. Data from the entire process is fed back in a closed loop to the trajectory planning module, and the arm automatically retracts and goes into standby mode after the task is completed, adapting to narrow, unmanned operation scenarios such as garages.

[0159] It should be noted that the above description of the technical solutions is exemplary, and this specification may be embodied in different forms and should not be construed as limiting it to the technical solutions set forth herein. Rather, providing these descriptions will ensure that the disclosure of this invention is thorough and complete, and will fully convey the scope of the disclosure to those skilled in the art. Furthermore, the technical solutions of this invention are defined only by the scope of the claims. When using the terms "comprising," "having," and "including" as described in this specification, other parts may also be included, and the terms used may generally be singular or plural.

[0160] Finally, it should be noted that the above embodiments are merely representative examples of the present invention. Obviously, the present invention is not limited to the above embodiments and many variations are possible. Any simple modifications, equivalent changes, and alterations made to the above embodiments based on the technical essence of the present invention should be considered within the protection scope of the present invention.

Claims

1. A longitudinal six-degree-of-freedom fully automatic robotic arm, characterized in that: The system includes a linear motion assembly (1) and a robotic arm assembly (2) mounted on the linear motion assembly (1). The linear motion assembly (1) includes a linear motion section capable of reciprocating along a linear direction and a linear drive section that drives the linear motion section to reciprocate along a linear direction. The robotic arm assembly (2) includes multiple connecting rods whose ends are connected by rotary joints. The rotary joints can drive the connecting rods to rotate around the connection points. The multiple connecting rods and multiple rotary joints form a linkage mechanism. One end of the linkage mechanism is connected to the linear motion section through a rotary joint, and the other end of the linkage mechanism is connected to the equipment assembly through a rotary joint. The linear drive section performs linear motion control on the linear motion section using a spatial kinematics modeling strategy, a zoned trajectory planning strategy, and an adaptive closed-loop control strategy. The rotary joints drive the linkage mechanism, the connecting rods, and the equipment assembly to rotate around their respective connection points using a spatial kinematics modeling strategy, a zoned trajectory planning strategy, and an adaptive closed-loop control strategy. The linear motion assembly (1) and the robotic arm assembly (2) are protected by a full-space safety protection strategy.

2. A control system for a longitudinal six-degree-of-freedom fully automatic robotic arm as described in claim 1, characterized in that: include: The spatial kinematics modeling module is used to model the configuration of a longitudinal six-DOF manipulator consisting of one linear translational degree of freedom and five rotational degrees of freedom. It constructs a kinematic model that integrates cantilever elastic deformation compensation and solves the end-effector spatial pose, joint target variables, link deformation compensation, joint limit and singular configuration constraint data through the kinematic model that integrates cantilever elastic deformation compensation. The partitioned trajectory planning module is used to generate longitudinally progressive continuous motion trajectories of each degree of freedom of the robotic arm and a multi-segment smooth fusion strategy based on the end-effector spatial pose, joint target variables, link deformation compensation, joint limit and singular configuration constraint data, combined with the narrow work space partitioning rules, work target pose and work stage control requirements, and outputs the target trajectory commands, partition control parameters and trajectory safety constraint parameters of each degree of freedom of the robotic arm. The adaptive closed-loop control module is used to implement master-slave collaborative closed-loop control and adaptive robust correction on the robotic arm based on the target trajectory command, zone control parameters and trajectory safety constraint parameters of each degree of freedom of the robotic arm, combined with real-time sensor feedback data of vision, force, position and current, so that the robotic arm can complete the target operation action and provide real-time feedback of the robotic arm end pose and robotic arm motion state data.

3. The control system of the longitudinal six-degree-of-freedom fully automatic robotic arm as described in claim 2, characterized in that: Also includes: The full-space safety protection module is used to perform graded collision detection, fault identification operation, and longitudinal folding active fault-tolerant control on the robotic arm based on the joint limit and singular configuration constraint data, the robotic arm motion state data, and multi-sensor monitoring information, output safety intervention commands, and perform real-time constraint and closed-loop control on the robotic arm motion trajectory.

4. The control system of the longitudinal six-degree-of-freedom fully automatic robotic arm as described in claim 3, characterized in that: The method for modeling a longitudinal six-DOF manipulator configuration consisting of one linear translational degree of freedom and five rotational degrees of freedom, and constructing a kinematic model that incorporates cantilever elastic deformation compensation, includes: Establish a kinematic coordinate system including a base coordinate system, a linear motion coordinate system, a joint coordinate system, and an end-effector coordinate system; define the system's generalized joint variables as: ,in, For linear motion displacement, Let the angle be the i-th rotational joint angle; construct the homogeneous transformation matrix of the translational degrees of freedom of the linear motion component. : Construct the homogeneous transformation matrix for each rotational joint. : ;in, The length of the link. For the connecting rod torsion angle, Given the link offset; establish a segmented link deflection model and calculate the deflection and rotation angle of each link stage: , ,in, Let be the equivalent load on the i-th link segment. For equivalent bending moment, For the length of the rod, For elastic modulus, Given the moment of inertia of the cross section; generate the deformation compensation vector at the end of the robotic arm: ;in, For the first Deflection of the connecting rod segment in the x-direction; For the first Deflection of the connecting rod in the y-direction. For the first The deflection of the connecting rod in the z-direction. For the first The connecting rod rotates at an angle in the x-direction. For the first The connecting rod rotates at an angle in the y-direction. For the first For the connecting rod with rotation angle in the z-direction, the kinematic model with fused deformation compensation is obtained: ,in, Let be the transformation matrix from the base coordinate system to the tool coordinate system. This is the homogeneous coordinate transformation matrix from the linear motion unit to the first rotary joint. Let be the homogeneous coordinate transformation matrix from revolute joint 1 to revolute joint 2. Let be the homogeneous coordinate transformation matrix from revolute joint 2 to revolute joint 3. This is the homogeneous coordinate transformation matrix from revolute joint 3 to revolute joint 4. This is the homogeneous coordinate transformation matrix from revolute joint 4 to revolute joint 5. This is the homogeneous coordinate transformation matrix from the No. 5 rotary joint to the tool coordinate system.

5. The control system of the longitudinal six-degree-of-freedom fully automatic robotic arm as described in claim 4, characterized in that: The method for solving the end-effector spatial pose, joint target variables, link deformation compensation, joint limit, and singular configuration constraint data by fusing a kinematic model with cantilever elastic deformation compensation includes: Define the six-dimensional pose vector of the robotic arm's end effector: ,in, This is the pose vector of the robotic arm's end effector. The coordinates of the robotic arm's end effector position; The attitude angles of the robotic arm's end effector are calculated by separating the position vector and rotation matrix of the robotic arm's end effector from the total homogeneous transformation matrix. ; ; ;in, Using the rotation matrix elements from the base coordinate system to the tool coordinate system at the end of the robotic arm, and combining them with the cantilever deformation compensation amount, we obtain the actual pose of the end effector after compensation: ,in, Let be the rigid body kinematic mapping function from joint variables to end-effector pose. This is the compensation amount for link deformation; based on the target end pose of the robotic arm, the joint target variables are solved by inverse solution, and the inverse solution formula is: ,in, The target end effector pose of the robotic arm. The inverse mapping of the positive kinematic function; assigning longitudinal displacement of linear motion: ,in, For the displacement of the target moving in a straight line, The X-axis coordinate of the target at the end of the robotic arm. Let be the angle between the link and the direction of linear motion, and m be the number of links involved in the longitudinal projection. The cosine theorem is used to solve for the rotational joint angle at the front end of the robotic arm, and attitude constraints are used to correct the remaining joint variables. An adaptive Jacobian matrix is ​​obtained by superimposing deformation compensation terms on the rigid body Jacobian matrix. ;in, For the deformation adaptive Jacobian matrix, For an ideal rigid body, the Jacobian matrix is... This is a deformation compensation correction term. This is the set of estimated load, attitude, and other parameters; a damping pseudo-inverse is used to avoid matrix singularity, as shown in the formula: ;in, This is the damped pseudo-inverse of the Jacobian matrix. The damping factor, Let T be the identity matrix and T be the inverted form of the matrix; read the hardware parameters of the robotic arm, set the upper and lower limits of joint angles, velocities, accelerations, and linear motion strokes to obtain joint limit data; calculate the singularity index to determine singular configurations, using the formula: ;in, As a singularity index, For the determinant of a matrix, when When this occurs, it is determined to be a singular configuration, and singular configuration constraint data is generated, where, This is the threshold for determining singularity.

6. The control system of the longitudinal six-degree-of-freedom fully automatic robotic arm as described in claim 5, characterized in that: The method for generating longitudinally progressive continuous motion trajectories and multi-segment smooth fusion strategies for each degree of freedom of the robotic arm based on the end-effector spatial pose, joint target variables, link deformation compensation, joint limit and singular configuration constraint data, combined with the narrow work space zoning rules, work target pose and work stage control requirements, and outputting the target trajectory commands, zoning control parameters and trajectory safety constraint parameters for each degree of freedom of the robotic arm includes: Define the position distance discriminant function and the attitude error discriminant function: , ;in, This represents the straight-line distance from the current position of the robotic arm's end effector to the target position. This represents the attitude error between the current posture of the robotic arm's end effector and the target posture. and These represent the current position of the robotic arm's end effector and the target position of the robotic arm's end effector, respectively. and These represent the current posture and the target posture of the robotic arm's end effector, respectively. The system is divided into six working regions based on distance and posture error thresholds, with a region determination function as follows: ;in, ~ These correspond to the standby retraction zone, long-stroke approach zone, near-end coarse alignment zone, fine alignment zone, low-speed insertion zone, and safe withdrawal zone, respectively. , , , Distance threshold > > > ; The attitude error threshold is used; the acceleration of the main stroke of linear motion is planned using S-shaped acceleration and deceleration: ;in, For real-time acceleration of linear motion, The maximum acceleration for linear motion, To increase the duration of accelerometer action, The total duration of the acceleration phase; generating the rotational joint follow-up trajectory: ;in, Real-time trajectory of the joints; and These are the initial joint angle and the target joint angle, respectively. The normalized trajectory progress function is defined for the 0-1 interval; visual correction and deformation compensation are integrated to obtain the final comprehensive reference trajectory, generating initial trajectory commands for each degree of freedom; a fifth-order polynomial is used to achieve smooth transitions between different regions of the trajectory, with the transition trajectory formula as follows: ;in, For trajectory fusion segment functions; ~ The coefficients are 5th-order polynomials; t is the transition time; the position, velocity, and acceleration of the constraint switching point are continuous to eliminate trajectory switching impact; combined with joint limits and singular configuration constraints, a comprehensive safety cost function is constructed to optimize the trajectory: ;in, Optimize the objective function for safety; ~ These are the weighting coefficients; This is a singular configuration penalty term; It includes obstacle collision penalty items; outputs target trajectory instructions, zone control parameters, and trajectory safety constraint parameters.

7. The control system of the longitudinal six-degree-of-freedom fully automatic robotic arm as described in claim 6, characterized in that: The method for implementing master-slave collaborative closed-loop control and adaptive robust correction on the robotic arm based on the target trajectory commands, zone control parameters, and trajectory safety constraint parameters of each degree of freedom of the robotic arm, combined with real-time sensor feedback data of vision, force, position, and current, to enable the robotic arm to complete the target operation and to provide real-time feedback of the robotic arm's end-effector pose and motion state data includes: Define linear motion displacement error and velocity error: , ,in, This represents the linear motion displacement error. For linear motion speed error, , This represents the actual displacement during linear motion. The velocity of the target moving in a straight line. The actual velocity of the linear motion is given; a PID control law for linear motion with superimposed friction and yaw compensation is constructed: ; ,in, Output from the linear drive unit. , and For PID control parameters, , and These are compensation items for friction, straightness, and long-stroke runout. The coefficient of friction is Coulomb. The coefficient of viscous friction is... Define the single-joint angle error as a sign function. Construct a joint torque control law that superimposes gravity and friction compensation. Joint friction compensation item is ;in, For joint torque output, For gravity compensation, and For joint friction parameters; correct joint commands based on robot arm end-effector pose error. , , ,in, This refers to the pose error of the robotic arm's end effector. For correction amount, For the original reference trajectory, For the corrected control commands; total control output From basic items Adaptive terms and robust items composition, Basic items Adaptive terms Parameter update law Robust item ,in, and For feedback gain, For the regression matrix, For online parameter estimation, For adaptive gain, The robustness coefficient is... It is a saturation function. The boundary layer width is used; weighted fusion is employed to integrate multi-channel sensor feedback data. , To integrate the estimation results, These are vision, force sensing, current, and position feedback, respectively. These are the weights of each channel; determined by the formula. To determine whether an assignment is completed, use a decision function. Determine if there is an operational abnormality, and collect and output data on the end-effector pose and overall motion status of the robotic arm throughout the entire process.

8. The control system of the longitudinal six-degree-of-freedom fully automatic robotic arm as described in claim 7, characterized in that: The method for performing graded collision detection, fault identification, and longitudinal folding active fault-tolerant control on the robotic arm based on the joint limit and singular configuration constraint data, the robotic arm motion state data, and multi-sensor monitoring information, and outputting safety intervention commands, and for real-time constraint and closed-loop control of the robotic arm's motion trajectory includes: Constructing the folding and shrinking optimization objective function The constraints are , ;in, To optimize the objective function for folding, These are the weighting coefficients. This refers to the envelope length of the robotic arm after folding. For the folded target joint vector, This is the length of the pullback path. The distance between the robotic arm and the obstacle. For safe distance threshold, Protect the equipment components; construct a comprehensive collision risk index. ; ;in, To assess the overall collision risk value, , , and These are risk items for position, current, force, and pose error, based on thresholds. right Divide into, It is divided into three levels: Level 1 warning, Level 2 detection, and Level 3 emergency response, and corresponding deceleration, shutdown, and emergency contraction intervention commands are output.

9. The control system of the longitudinal six-degree-of-freedom fully automatic robotic arm as described in claim 8, characterized in that: The method for performing graded collision detection, fault identification, and longitudinal folding active fault-tolerant control on the robotic arm based on the joint limit and singular configuration constraint data, the robotic arm motion state data, and multi-sensor monitoring information, and outputting safety intervention commands, and for real-time constraint and closed-loop control of the robotic arm's motion trajectory, further includes: Define the system health state vector and overall health score: , ,in, The health status vector sequentially represents the working status of vision, force sensing, current, position, and actuator. Overall health status These are weighting coefficients. System faults are determined in real time; the fault-tolerant control law is as follows: Online self-tuning formula for parameters ,in, For fault-tolerant control output, For fault tolerance gain, For the safe target pose, For learning rate, and These are the actual observed values ​​and the model predicted values, respectively. The feature vector is used for regression; the robotic arm's trajectory is corrected in real time by combining joint limits, singular constraints, and safety intervention commands.

10. A control method for a control system based on any one of claims 2-9 of a longitudinal six-degree-of-freedom fully automated robotic arm, characterized in that: include: A longitudinal six-DOF robotic arm configuration consisting of one linear translational degree of freedom and five rotational degrees of freedom is modeled, and a kinematic model integrating cantilever elastic deformation compensation is constructed. The end-effector spatial pose, joint target variables, link deformation compensation, joint limit and singular configuration constraint data are solved through the kinematic model integrating cantilever elastic deformation compensation. Based on the end-effector spatial pose, joint target variables, link deformation compensation, joint limit and singular configuration constraint data, combined with the narrow work space partitioning rules, work target pose and work stage control requirements, the longitudinal progressive continuous motion trajectory of each degree of freedom of the robotic arm and the multi-segment smooth fusion strategy are generated, and the target trajectory command, partition control parameters and trajectory safety constraint parameters of each degree of freedom of the robotic arm are output. Based on the target trajectory commands, zone control parameters, and trajectory safety constraint parameters of each degree of freedom of the robotic arm, combined with real-time sensor feedback data of vision, force, position, and current, the robotic arm is subject to master-slave collaborative closed-loop control and adaptive robust correction, enabling the robotic arm to complete the target operation and providing real-time feedback of the robotic arm end pose and motion state data.

Images