Six-axis full-automatic mechanical arm and control system and control method thereof

Abstract

The application discloses a six-axis full-automatic mechanical arm and a control system and a control method thereof, and comprises a mechanical arm base and a mechanical arm assembly. A lifting mechanism is connected between the mechanical arm base and the mechanical arm assembly. The mechanical arm assembly comprises six rotary joints connected in series through connecting pieces. The rotary joint comprises a rotary driving part and a rotary connecting part. The rotary driving part can drive the rotary connecting part to rotate. The connecting piece comprises a mounting seat for mounting the rotary driving part and a connecting rod for fixing two rotary connecting parts. The lifting mechanism is controlled through a global kinematics modeling strategy, a full-space trajectory planning strategy and a zero-space closed-loop control strategy. The rotary driving part drives the rotary connecting part to rotate through the global kinematics modeling strategy, the full-space trajectory planning strategy and the zero-space closed-loop control strategy. The lifting mechanism and the rotary joint 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 six-axis fully automatic robotic arm and its control system and control method. Background Technology

[0002] With the rapid iteration of industrial automation and intelligent unmanned operation technologies, fully automated robotic arm systems, as core intelligent equipment for liberating manpower and completing high-precision flexible operations and complex spatial maneuvers, have been widely applied in various scenarios such as automatic vehicle charging, material handling, flexible assembly, and port logistics. They possess core advantages such as fully automated operation, high-precision flexible docking, intelligent scheduling, and full-scenario adaptability, making them an important development direction in the fields of intelligent manufacturing and unmanned operations, with broad prospects for industrial application and market value. Currently, mainstream fully automated robotic arm systems in the industry mostly adopt traditional configurations such as suspended and fixed types, and are differentiated into two technical solutions based on operational requirements: low-degree-of-freedom and high-degree-of-freedom. However, both types of solutions have irreconcilable technical defects, severely restricting the industrialization and application expansion of robotic arm systems.

[0003] The degree of freedom cannot be balanced with structure and cost: low-degree-of-freedom robotic arms have fewer joints, simpler structure and controllable cost, but insufficient freedom of operation, cannot adapt to changing working environments and complex spatial posture adjustment needs, and the working scene is highly limited; high-degree-of-freedom robotic arms can cover multiple scene operation needs, but the excessive number of joints leads to complex mechanical structure, high processing and assembly difficulty, and high overall cost, making it difficult to achieve large-scale promotion.

[0004] Insufficient motion control precision: Existing robotic arms struggle to effectively integrate visual and force sensing information for precise motion trajectory planning and tracking, resulting in significant motion errors. In high-precision operation scenarios such as automatic vehicle charging and precision insertion, issues such as alignment deviations and insertion failures can easily occur, making it impossible to stably complete predetermined tasks.

[0005] Weak protection and storage performance: Existing robotic arms have poor self-protection capabilities and cannot be compactly stored when not in operation. They are prone to collisions and scratches during operation and standby, which can lead to equipment damage. At the same time, the overall space occupancy rate is high, making them unsuitable for box-type, column-type, and modular deployment, resulting in poor engineering adaptability and scenario compatibility.

[0006] In summary, developing a fully automated robotic arm system with a reasonable number of joints, simple structure, controllable cost, adaptability to most working environments, high-precision motion control capabilities, good protection after retraction, and resistance to collisions has become a key technical problem that urgently needs to be solved in the field of intelligent unmanned operations. Summary of the Invention

[0007] The purpose of this invention is to overcome the shortcomings of the aforementioned background technology and provide a six-axis fully automatic robotic arm with a reasonable number of joints, simple structure, controllable cost, adaptability to most working environments, high-precision motion control capability, good protection after retraction, and resistance to collisions, as well as its control system and control method.

[0008] To achieve this objective, the six-axis fully automatic robotic arm designed in this invention includes a robotic arm base and a robotic arm assembly. A lifting mechanism for raising and lowering the robotic arm assembly is connected between the robotic arm base and the robotic arm assembly. The robotic arm assembly includes six rotary joints connected in series via connectors (the six rotary joints are connected sequentially to form the robotic arm assembly; each preceding rotary joint can drive all subsequent rotary joints to rotate in tandem, and each subsequent rotary joint can also rotate relative to the preceding rotary joint). Each rotary joint includes a rotary drive component and a rotary connecting component. The rotary drive component can drive the rotary connecting component to rotate. The connecting component includes a mounting base for mounting the rotary drive component and a connecting rod for fixing and connecting two preceding and following rotary connecting components. The lifting mechanism controls the lifting motion using a global kinematics modeling strategy, a full-space trajectory planning strategy, and a zero-space closed-loop control strategy. The rotary drive component drives the rotary connecting component to rotate using the same strategy. The lifting mechanism and the rotary joints are protected by a full-space safety protection strategy.

[0009] Furthermore, the present invention provides a six-axis fully automatic robotic arm control system, comprising:

[0010] The global kinematics modeling module is used to model the configuration of a seven-DOF robotic arm consisting of one lifting degree of freedom and six rotational degrees of freedom, forming a screw kinematic model. The global pose solution, joint constraints, and configuration maneuverability data are obtained through the screw kinematic model.

[0011] The full-space trajectory planning module is used to generate continuous motion trajectories and adaptive acceleration / deceleration strategies for each degree of freedom of the robotic arm based on the global pose solution, joint constraints, and configuration operability data, combined with the target pose of the work object and the priority of the work stage. It outputs the target trajectory instructions, stage optimization targets, and trajectory constraint parameters for each degree of freedom of the robotic arm.

[0012] The zero-space (the space in which the lifting mechanism and rotary joint can move freely without changing the position of the end effector) closed-loop control module is used to perform closed-loop control and zero-space configuration optimization of the robot arm's position based on the target trajectory command, stage optimization target and trajectory constraint parameters, combined with real-time sensor feedback data of vision and force, so that the robot arm can complete the target action and provide real-time feedback of the end position of the robot arm components and the robot arm's motion status data.

[0013] Furthermore, the full-space safety protection module is used to perform graded collision detection, fault identification operations, and active fault-tolerant control on the robotic arm based on the joint constraints, 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.

[0014] Furthermore, the method for modeling the configuration of a seven-degree-of-freedom robotic arm consisting of one lifting degree of freedom and six rotational degrees of freedom to form a screw kinematic model includes: establishing a base coordinate system with the center of the robotic arm base as the origin, establishing a lifting axis coordinate system with the lifting center of the lifting mechanism as the origin, establishing a six-joint coordinate system with the rotation centers of the six rotational joints as the origin, and establishing a tool coordinate system with the end point center of the robotic arm assembly as the origin.

[0015] Based on the base coordinate system, the lifting axis coordinate system, the joint coordinate system, and the tool coordinate system, the generalized joint variable q of the robotic arm and the translation transformation matrix T of the lifting mechanism are defined. h The exponential product formula is used to define the homogeneous transformation matrix of the robot arm's end-effector pose. The generalized joint variable q of the robotic arm and the translation transformation matrix T of the lifting mechanism. h Homogeneous transformation matrix of the end effector pose of the robotic arm This constitutes the spinor kinematic model;

[0016] The expression for the generalized joint variable q of the robotic arm is:

[0017]

[0018] The expression for the translation transformation matrix Th of the lifting mechanism is:

[0019] The homogeneous transformation matrix of the end effector pose of the robotic arm The expression is:

[0020]

[0021] in, This refers to the lifting displacement of the lifting mechanism. For the first The rotation angle of each rotational joint, where T is the vector transpose and e is the base of the natural logarithm. For the first Rotational power of a rotating joint spinor The antisymmetric matrix form, This is the initial pose matrix of the end effector of the robotic arm in the zero-position configuration.

[0022] Furthermore, the method for obtaining the global pose solution, joint constraints, and configuration maneuverability data through the screw kinematic model includes: using the generalized joint variable q of the robotic arm as input, and calculating the homogeneous transformation matrix of the robotic arm's end-effector pose using the exponential product formula. Output the end effector pose of the robotic arm The global pose solution is obtained, and the end effector pose of the robotic arm is obtained. The expression is: ,in, For positive kinematic mapping function, This is the compensation amount for the straightness error of the lifting mechanism. This is the amount of backlash compensation for the rotary joint. This is the amount to compensate for installation deviation. This is the amount of elastic deformation compensation for the connecting rod;

[0023] In the process of solving the inverse kinematics and multi-objective optimization of the robotic arm, the physical and soft constraint boundaries of the rotary joint are clearly defined and incorporated into the optimization algorithm as constraint penalties for the rotary joint to solve for the optimal configuration of the rotary joint. The joint constraints include the constraint penalties for the rotary joint. The expression is:

[0024] ,in, This is the minimum permissible rotation angle of the rotary joint. This represents the maximum permissible rotation angle of the rotary joint.

[0025] An analytical Jacobian matrix containing a lifting mechanism and six rotary joints is constructed to quantify the motion flexibility and redundancy of the robotic arm, thereby obtaining the operability data of the configuration. , The expression is:

[0026] Where J is the Jacobian matrix, This is the transpose of the Jacobian matrix. It is a determinant.

[0027] Furthermore, based on the global pose solution, joint constraints, and configuration operability data, and combined with the target pose of the work object and the priority of the work stage, the method for generating continuous motion trajectories and adaptive acceleration / deceleration strategies for each degree of freedom of the robotic arm, and outputting target trajectory commands, stage optimization targets, and trajectory constraint parameters for each degree of freedom of the robotic arm includes: dynamically adjusting the weight distribution among the position accuracy, attitude accuracy, motion stability, and force safety of the robotic arm according to different stages of the robotic arm's work task; generating a continuous target pose trajectory at the end of the robotic arm component in Cartesian space, and simultaneously planning the motion trajectories of the lifting mechanism and rotary joints in joint space, with the Cartesian space reference trajectory of the end of the robotic arm component. The expression is: Generalized joint reference trajectory The expression is: ,in, This is the reference position trajectory for the end effector of the robotic arm component. This serves as the reference attitude trajectory for the end effector of the robotic arm component. This is the initial configuration for the robotic arm. For the target configuration of the robotic arm, For time scaling functions, The null projection matrix, To optimize the trajectory for zero space, during the motion trajectory planning of the lifting mechanism and rotary joints, the operability index of the current robotic arm configuration is evaluated in real time. Zero-space motion is used to adjust the joint configuration of the robotic arm without changing the target trajectory of the end effector components. The expression for the zero-space optimization objective function is:

[0028] ,in, To optimize the overall configuration objective function, Assigning weights to each optimization term, For the singularity avoidance indicator, To limit and avoid indicators, To optimize torque performance, For a pre-defined ideal joint combination of robotic arms, It is the Euclidean norm.

[0029] Furthermore, based on the target trajectory command, the stage optimization target and trajectory constraint parameters, and combined with real-time sensor feedback data from vision and force, the method for closed-loop control and zero-space configuration optimization of the robotic arm's pose, enabling the robotic arm to complete the target action, and providing real-time feedback of the robotic arm component end-effector pose and robotic arm motion state data, includes: taking the target position and posture of the robotic arm component end-effector as the control object, generating control quantities for correcting the displacement of the lifting guide rail and the motion of each rotary joint based on the deviation between the current end-effector pose and the target end-effector pose, and the robotic arm component end-effector pose error. The expression is:

[0030] Generalized joint velocity commands corresponding to the task space The expression is: ,in, Position error Attitude error The target position of the end effector of the robotic arm component. This represents the current position of the end effector of the robotic arm assembly. Rotate the end effector of the robotic arm assembly from its current pose R to the target pose. Required relative rotation matrix, For the target posture of the robotic arm component end effector Rotate to current pose Required relative rotation matrix, It is the Jacobian pseudo-inverse matrix. The target velocity at the end effector of the robotic arm component. This is the task space error feedback gain matrix. Here, T is the vectorization operator, and T is the matrix transpose.

[0031] Furthermore, based on the joint constraints, the robotic arm motion state data, and multi-sensor monitoring information, the method for performing graded collision detection, fault identification operations, and active fault-tolerant control on the robotic arm, outputting safety intervention commands, and performing real-time constraint and closed-loop control on the robotic arm's motion trajectory includes: constructing a comprehensive collision risk index. Assessment system overall health When the overall health of the system Less than the threshold When the robot arm malfunctions or finishes its work, it enters the active fault-tolerant control mode. When the robot arm malfunctions or finishes its work, it performs minimum envelope containment optimization.

[0032] Furthermore, the comprehensive collision risk index The expression is:

[0033] System overall health The expression is: The objective function for optimizing the minimum envelope of a robotic arm. The expression is: ;in, These are the weights of each risk item. For environmental proximity risk items, This is a risk item related to abnormal current. This is a risk item related to abnormal force. This is a pose error risk item. As a weighted coefficient for health, Weighted by health score The corresponding system health statuses, These are the weighting coefficients. This refers to the envelope volume after the robotic arm is stowed away. This represents the total motion of the robotic arm's rotary joints. This represents the length of the robotic arm's retraction path.

[0034] Furthermore, this invention also provides a six-axis fully automatic robotic arm control method, comprising: modeling a seven-DOF robotic arm configuration consisting of one lifting degree of freedom and six rotational degrees of freedom to form a screw kinematic model; obtaining global pose solution, joint constraint conditions, and configuration operability data through the screw kinematic model; based on the global pose solution, joint constraint conditions, and configuration operability data, and combining the target pose of the work object and the priority of the work stage, generating continuous motion trajectories and adaptive acceleration / deceleration strategies for each degree of freedom of the robotic arm, and outputting target trajectory commands, stage optimization targets, and trajectory constraint parameters for each degree of freedom of the robotic arm; based on the target trajectory commands, stage optimization targets, and trajectory constraint parameters, and combining real-time sensor feedback data of vision and force, performing closed-loop control and zero-space configuration optimization of the robotic arm pose, enabling the robotic arm to complete the target action, and providing real-time feedback of the end-effector pose and motion state data of the robotic arm.

[0035] The above-mentioned global kinematics modeling strategy corresponds to the global kinematics modeling module, the full-space trajectory planning strategy corresponds to the full-space trajectory planning module, the zero-space closed-loop control strategy corresponds to the zero-space closed-loop control module, and the full-space safety protection strategy corresponds to the full-space safety protection module. That is, each strategy is the same as the data processing method of each module and corresponds one-to-one, thereby realizing the safe closed-loop control of the six-axis fully automatic robotic arm by the control system.

[0036] The beneficial effects of this invention are as follows: First, this invention, through the lifting mechanism connecting the robotic arm base and the robotic arm assembly, and in conjunction with the robotic arm assembly consisting of six rotary joints (including rotary drive components and rotary connection components) connected in series via mounting bases and connecting rods, constitutes a seven-degree-of-freedom redundant configuration of "one vertical lifting guide axis and six spatially connected rotary joints". This configuration, while satisfying the requirements of full-space wide-range coverage and end-effector multi-pose adjustment capabilities, effectively controls the number of joints, avoiding structural complexity and high costs caused by excessive degrees of freedom, and overcoming the shortcomings of low-degree-of-freedom robotic arms that cannot adapt to changing environments, achieving the design goals of simple structure, controllable cost, and adaptability to most complex working environments. Second, the control system of this invention uses a full-domain kinematics modeling module for accurate spatial pose calculation, and in conjunction with a full-space trajectory planning module, can adaptively match the priority weights of position, attitude, stability, and force according to the operation stage (such as approach, alignment, insertion, etc.). With the help of the zero-space closed-loop control module, combined with visual and force sensor feedback such as the camera installed on the gun head assembly, the system can ensure high-precision tracking of the target pose by the end effector while utilizing redundant degrees of freedom for singularity avoidance, joint limit avoidance, and torque balancing. This mechanism greatly improves the all-space adaptability and control accuracy of the robotic arm under complex working conditions, and is especially suitable for high-precision docking scenarios such as automatic vehicle charging. Finally, by configuring an all-space safety protection strategy, this invention can perform graded collision detection and system health assessment in real time based on joint constraints and multi-sensor monitoring information, and output safety intervention commands or enter an active fault-tolerant control mode in case of anomalies. In addition, in the non-working state or when the work is finished, the system can drive the lifting mechanism and each rotary joint to perform minimum envelope storage optimization, so that the robotic arm assembly retracts to a safe area near the robotic arm base. This not only effectively avoids accidental collisions during equipment standby or movement, but also greatly enhances the robotic arm's own protection capabilities and extends the service life of the equipment. Attached Figure Description

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

[0038] Figure 1 This is a front view of the six-axis fully automatic robotic arm designed according to the present invention;

[0039] Figure 2 A perspective view of the six-axis fully automatic robotic arm designed for this invention;

[0040] Figure 3 This is a three-dimensional structural view of the joint module in this invention;

[0041] Figure 4This is a schematic diagram of the module connection of the six-axis fully automatic robotic arm control system designed in this invention;

[0042] Figure 5 A flowchart of the six-axis fully automatic robotic arm control method designed for this invention;

[0043] Among them, 1—lifting mechanism (1.1—robotic arm base, 1.2—lifting drive motor, 1.3—lifting guide rail module), 2—robotic arm assembly (2.1—first motor mounting base, 2.2—first joint motor, 2.3—first rotating part, 2.4—second joint motor, 2.5—second rotating part, 2.6—second motor mounting base, 2.7—connecting rod, 2.8—third motor mounting base, 2.9—third joint motor, 2.10—third rotating part, 2.11—fourth joint motor, 2.12—fourth rotating part, 2.13—fourth motor mounting base, 2.14—fifth joint motor, 2.15—fifth rotating part), 3—gun head assembly (3.1—fifth motor mounting base, 3.2—gun head, 3.3—sixth joint motor, 3.4—sixth rotating part, 3.5—camera), 4—joint module rotating part, 5—joint module fixing part. Detailed Implementation

[0044] 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 way of 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 the embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0045] Example 1

[0046] This embodiment provides a specific physical structure of a six-axis fully automated robotic arm, such as... Figures 1 to 3 As shown, the robotic arm mainly consists of three core parts: a lifting mechanism 1, a robotic arm assembly 2, and a gun head assembly 3.

[0047] The lifting mechanism 1, serving as the base and power source for the robotic arm, specifically includes: a robotic arm base 1.1, a lifting drive motor 1.2, and a lifting guide rail module 1.3. The lifting drive motor 1.2 is bolted to the robotic arm base 1.1. The lifting guide rail module 1.3 is vertically mounted on the robotic arm base 1.1 and is driven by the lifting drive motor 1.2 to achieve vertical linear motion. The lifting guide rail module 1.3 is a pre-assembled component, operating on the principle of a screw and nut mechanism. The lifting drive motor 1.2 drives the screw of the lifting guide rail module 1.3 to rotate, causing the guide rail of the lifting guide rail module 1.3 to move up and down. The specific structure is not described in detail here, as it is a commonly used component for those skilled in the art.

[0048] The robotic arm assembly 2 is connected in series to the upper part of the lifting mechanism 1 via connectors, and specifically includes six rotary joints connected in series. For clarity, the definitions of each rotary joint are as follows:

[0049] The first rotary joint consists of a first motor mounting base 2.1, a first joint motor 2.2, and a first rotating part 2.3. The first motor mounting base 2.1 is bolted to the lifting guide rail of the lifting guide rail module 1.3, allowing the entire robotic arm assembly 2 to move vertically with the lifting mechanism 1. The first joint motor 2.2 is mounted inside the first motor mounting base 2.1 and drives the first rotating part 2.3. A second motor mounting base 2.6 is fixed to the first rotating part 2.3, and the first rotating part 2.3 drives the second motor mounting base 2.6 to rotate.

[0050] The second rotary joint consists of a second motor mounting base 2.6, a second joint motor 2.4, and a second rotating part 2.5. The second joint motor 2.4 is mounted inside the second motor mounting base 2.6 and drives the second rotating part 2.5 to move. The second rotating part 2.5 is fixed to one end of the connecting rod 2.7 and drives the connecting rod 2.7 to rotate.

[0051] The third rotary joint consists of a third motor mounting base 2.8, a third joint motor 2.9, and a third rotating part 2.10. The third joint motor 2.9 is mounted inside the third motor mounting base 2.8 and drives the third rotating part 2.10. The third rotating part 2.10 is fixed to the other end of the connecting rod 2.7. The third rotating part 2.10 does not drive the connecting rod 2.7 to rotate, but it drives the third joint motor 2.9 and the third motor mounting base 2.8 to rotate.

[0052] The fourth rotary joint consists of a third motor mounting base 2.8, a fourth joint motor 2.11, and a fourth rotating part 2.12. The fourth joint motor 2.11 is mounted below the third motor mounting base 2.8 and drives the fourth rotating part 2.12. A fourth motor mounting base 2.13 is fixed to the fourth rotating part 2.12, which in turn drives the fourth motor mounting base 2.13 to rotate.

[0053] The fifth rotary joint consists of a fourth motor mounting base 2.13, a fifth joint motor 2.14, and a fifth rotating part 2.15. The fifth joint motor 2.14 is mounted inside the fourth motor mounting base 2.13 and drives the fifth rotating part 2.15. A sixth motor mounting base 3.1 is fixed to the fifth rotating part 2.15, which in turn drives the sixth motor mounting base 3.1 to rotate.

[0054] The sixth rotary joint consists of a sixth motor mounting base 3.1, a sixth joint motor 3.3, and a sixth rotating part 3.4. The sixth joint motor 3.3 is mounted inside the sixth motor mounting base 3.1 and drives the sixth rotating part 3.4. A gun head 3.2 and a camera 3.5 are fixed on the sixth rotating part 3.4, which in turn rotates the gun head 3.2 and the camera 3.5.

[0055] All six rotary joints described above utilize joint modules for rotation. Since these modules are pre-assembled parts, their specific structures will not be detailed here. Each joint module includes, as follows: Figure 3 The joint module rotating part 4 (i.e., the first to sixth rotating parts mentioned above) and the joint module fixing part 5 (i.e., the first to sixth joint motors mentioned above) are shown.

[0056] The gun head assembly 3 is mounted at the end of the robotic arm assembly 2, and specifically includes: a sixth motor mount 3.1, a gun head 3.2, a sixth joint motor 3.3, a sixth rotating part 3.4, and a camera 3.5. The sixth motor mount 3.1 is fixed to the fifth rotating part 2.15, which drives the sixth motor mount 3.1 to rotate. The sixth joint motor 3.3 drives the sixth rotating part 3.4 to perform precise end-effector rotation of the gun head 3.2. The camera 3.5 is fixed near the gun head 3.2 by a bracket and is used to acquire images of the working environment.

[0057] In this embodiment, the robotic arm controls the spatial pose of the gun head assembly 3 through the coordinated movement of the lifting mechanism 1 and the robotic arm assembly 2. The specific action is as follows:

[0058] Vertical lifting and coarse positioning: When operation is required, the lifting drive motor 1.2 starts, driving the lifting guide rail module 1.3 to raise the entire robotic arm assembly 2 to the preset working height (such as the height of the vehicle charging port). At this time, the robotic arm is in the unfolded ready state.

[0059] Spatial posture deployment (six-axis linkage): After the lifting position is reached, the first joint motor 2.2 to the fifth joint motor 2.14 work together. By controlling the rotation angle of each rotary drive component (motor), the rotary connecting components (joint modules) are driven to produce pitch, yaw, or rotational movements. For example, the first rotary joint is responsible for overall rotation, the second and third rotary joints are responsible for the pitch of the upper arm to change the working radius, and the fourth and fifth joints are responsible for wrist posture adjustment.

[0060] Precise alignment and operation at the end: When the nozzle 3.2 approaches the target (such as a charging port), the sixth joint motor 3.3 starts, driving the nozzle 3.2 to perform fine-tuning rotation. Combined with visual feedback from the camera 3.5, millimeter-level precise alignment is achieved. Then, the insertion or gripping action is performed.

[0061] Retraction and Storage: After the operation is completed, the motors of each joint of the robotic arm assembly 2 reverse, causing each link to fold and retract, and the lifting mechanism 1 drives the entire assembly to descend to its lowest position. Finally, the gun head assembly 3 is stored in the protective area near the robotic arm base 1.1, forming a minimum envelope to prevent collision.

[0062] Example 2

[0063] like Figure 4 As shown, this embodiment provides a system for controlling a robotic arm based on Embodiment 1. The system includes four main modules, as detailed below:

[0064] The global kinematics modeling module is used to model the configuration of a seven-DOF robotic arm consisting of one lifting degree of freedom and six rotational degrees of freedom, forming a screw kinematic model. The global pose solution, joint constraints, and configuration maneuverability data are obtained through the screw kinematic model.

[0065] The full-space trajectory planning module is used to generate continuous motion trajectories and adaptive acceleration / deceleration strategies for each degree of freedom of the robotic arm based on the global pose solution, joint constraints, and configuration operability data, combined with the target pose of the work object and the priority of the work stage. It outputs the target trajectory instructions, stage optimization targets, and trajectory constraint parameters for each degree of freedom of the robotic arm.

[0066] The zero-space closed-loop control module is used to perform closed-loop control and zero-space configuration optimization of the robotic arm's posture based on the target trajectory command, stage optimization target and trajectory constraint parameters, combined with real-time sensor feedback data of vision and force, so that the robotic arm can complete the target action and provide real-time feedback of the end-effector posture and motion state data of the robotic arm.

[0067] The full-space safety protection module is used to perform graded collision detection, fault identification, and active fault-tolerant control on the robotic arm based on the joint constraints, 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.

[0068] The full-space safety protection module interacts with the full-space trajectory planning module and the zero-space closed-loop control module to ensure the real-time design of the robotic arm's motion trajectory and the monitoring of the motion process. This ensures that the robotic arm can always move the end effector of the robotic arm components to the target position in the optimal posture to complete the preset task, while ensuring the safety of the robotic arm throughout the entire motion process.

[0069] Example 3

[0070] This embodiment is based on the control system of Embodiment 2, combined with Figure 5This paper details a closed-loop control method for a six-axis fully automated robotic arm in the automatic charging scenario of new energy vehicles. The method deeply integrates global kinematic modeling, priority trajectory planning, zero-space decoupling control, and three-dimensional safety protection to ensure that the gun head assembly 3 can accurately, smoothly, and safely complete the entire process from standby, identification, alignment, insertion, and storage.

[0071] Step S1: Standby Monitoring and Environmental Sensing

[0072] When the system is in standby mode, the controller continuously monitors the vehicle's location via a camera 3.5 installed near the charging head assembly 3. When the system detects that the vehicle has entered the preset charging area and there are no obvious obstacles or personnel interference in the working area, the system enters the automatic charging preparation state.

[0073] The controller triggers camera 3.5 to acquire an image of the vehicle's charging port area. To improve recognition accuracy and anti-interference capability, the controller performs preprocessing on the image, which includes one or more of the following: region of interest (ROI) cropping, filtering and noise reduction, brightness compensation, edge enhancement, and feature enhancement.

[0074] Step S2: Charging port target recognition and pose conversion

[0075] The controller analyzes the preprocessed image, extracts the charging port contour features, center position, opening direction and attitude features, and obtains the position parameters and orientation parameters of the charging port in the camera coordinate system {C}.

[0076] Subsequently, the controller, based on the camera calibration parameters, the installation offset parameters between the camera and the head assembly 3, the current position of the lifting guide rail, and the current joint pose of the robotic arm, transforms the identified charging port pose into the robotic arm's base coordinate system {0}, forming the target position and target pose for motion control. The coordinate transformation formula is as follows: ,in, : The target pose of the charging port in the robotic arm base coordinate system {0} (i.e. the ideal pose that the head assembly 3 needs to reach). : The current pose of the robotic arm end effector (tool coordinate system) relative to the base coordinate system {0}. : The installation transformation matrix (including translation and rotation) of the camera 3.5 relative to the coordinate system of the robotic arm end effector. The pose of the charging port as identified in the camera coordinate system {C}.

[0077] Step S3: Global kinematic modeling and inverse kinematics solution based on spinor theory

[0078] The controller establishes a seven-DOF redundant robotic arm configuration, including one lifting and translational degree of freedom and six rotational joint degrees of freedom.

[0079] S3.1: Establishing a Coordinate System and Defining Generalized Variables

[0080] The base coordinate system is set at the center of the robotic arm base 1.1; the lifting axis coordinate system is set at the center of the lifting guide slide; the coordinate systems of each joint are set sequentially at the axis of each rotary joint; and the tool coordinate system is set at the center of the end of the gun head assembly 3.

[0081] The generalized joint variables of a robotic arm are defined as follows:

[0082] ,in, : Generalized joint variable vector of the robotic arm. h: Lifting displacement of lifting mechanism 1. θi: Rotation angle of the i-th rotary joint (driven by the motors of the first to sixth joints).

[0083] S3.2: Screw Kinematic Modeling

[0084] A global kinematic model of the robotic arm is established using a screw kinematic modeling method based on the product of exponential functions (POE). The translation transformation matrix of the lifting guide rail is also included. for:

[0085] The pose transformation of the tool coordinate system relative to the base coordinate system is expressed as:

[0086] , where T(q): end pose homogeneous transformation matrix. e: base of natural logarithm. : The screw quantity corresponding to the i-th rotational joint. : antisymmetric matrix form ( (Should be lifting axis). M: The initial pose matrix of the end-effector coordinate system of the robotic arm assembly relative to the base coordinate system when the robotic arm is in zero pose.

[0087] S3.3: Solving Forward Kinematics and Error Compensation

[0088] The controller calculates the spatial position and orientation of the gun head assembly 3 in the base coordinate system in real time based on the lifting displacement and the rotation angles of the six rotary joints. The end effector pose of the robotic arm assembly after error compensation is as follows:

[0089] The end effector pose vector of the robotic arm assembly can be represented as: If Euler angles are used, the end effector orientation of the robotic arm component can be obtained by the following formula:

[0090] ; ; .

[0091] in, : The end-effector pose vector of the compensated robotic arm assembly. : Forward kinematic mapping function. : Compensation amount for straightness error of lifting guide rail. : Joint reverse clearance compensation amount. Installation deviation compensation amount. : Elastic deformation compensation amount of the connecting rod. x, y, z: Position coordinates of the end effector of the robotic arm assembly in a spatial rectangular coordinate system. θ, ψ: Euler angles (roll, pitch, yaw) of the end effector of the robotic arm component. : Elements in the end-effector rotation matrix R(q) of the robotic arm component. .

[0092] S3.4: Solving the inverse kinematics of redundant degrees of freedom

[0093] Because the robotic arm has seven degrees of freedom redundancy, the controller uses a multi-objective optimization approach to solve the inverse kinematics. This addresses the target end-effector pose of the robotic arm components. Define task error Construct the optimization objective function:

[0094] The joint limitation penalty items are: The torque balance penalty term is:

[0095] In the iterative solution, the correction amount for the revolute joint is:

[0096] ;in, Analyze the Jacobian matrix to establish the mapping relationship between the end-effector Cartesian space velocity and the joint space velocity of the robotic arm component. Jacobi pseudo-inverse matrix. : Singularity penalty term, used to measure and mitigate the risk of the robotic arm approaching a singular state. : Soft limit penalty for rotational joints and These are the minimum and maximum allowable rotation angles for the i-th rotary joint, respectively. : Torque distribution penalty term for rotary joints : The driving torque of the i-th rotary joint. : A pre-defined reference configuration (such as the most relaxed or least strenuous posture). ~ : Weight coefficients for each optimization objective. I: Identity matrix. Zero-space optimization objective function.

[0097] Step S4: Full-space continuous trajectory planning based on task scenario priority

[0098] The controller dynamically adjusts the trajectory planning priority according to different stages of the vehicle's automatic charging operation.

[0099] S4.1: Adaptive matching of job scenario priorities

[0100] Define the current task phase priority weight vector as follows: The objective function for trajectory optimization is: ;

[0101] For different stages:

[0102] Approach phase (lifting approach zone, spatial attitude deployment zone): The main focus is on rapid approach and smooth movement. The priority is to make the gun head 3.2 quickly approach the target area, maintain smooth movement, and avoid excessive detours, so that the gun head 3.2 can reach the vicinity of the vehicle charging port as soon as possible.

[0103] objective function ;

[0104] Alignment Phase (Coarse Alignment Zone, Fine Alignment Zone): The objectives are to minimize end-effector pose error, prioritize attitude matching, and prioritize configuration stability, ensuring the gun head 3.2 maintains a suitable attitude and high control precision before approaching the charging port. The primary objective function of the alignment phase is to minimize the end-effector position and attitude error of the robotic arm components.

[0105] ;

[0106] Attitude error: .

[0107] Interlocking Phase (Low-Speed ​​Interlocking Zone): In the low-speed interlocking zone, the priority is "compliant propulsion, stable force distribution, and minimal collision risk." The system automatically reduces speed and acceleration, and minimizes abrupt attitude changes. The objective function is: [Objective Function to be inserted here].

[0108] .

[0109] Exit Phase (Safe Exit Zone): Prioritizing "safe retraction path, minimum storage envelope, and reasonable joint load," the robotic arm quickly leaves the work area and enters a safe storage state. The primary objectives are safe retraction and minimum storage envelope, with the objective function... .

[0110] In the above function expression, , , Weights for position accuracy, attitude accuracy, motion stability, and force safety. Target location and current location. Target attitude and current attitude. End contact force or equivalent force. , , , Weighting coefficients for each stage. : Rotational joint acceleration vector. : Optimize the configuration. Expected contact force. : Length of the pullback path. : Envelope volume after storage.

[0111] S4.2: Generation of Continuous Trajectories in Full Space

[0112] Generate a continuous trajectory of the target pose at the end of the gun head in Cartesian space:

[0113]

[0114] Simultaneously plan the motion trajectories of the lifting guide rail and six rotary joints in the joint space:

[0115]

[0116] in Let be the time-scaling function, and let the null term be used for redundant configuration synchronization optimization. If a fifth-order polynomial time scale is used, then: . End-point reference trajectory. Reference location trajectory. : Reference attitude trajectory. : Generalized joint reference trajectory, including the reference displacement of the lifting guide axis and the reference angles of the six rotary joints. The initial configuration and the optimal target configuration obtained through step S3. : Time scaling function. : Null space projection matrix. Zero-space optimized trajectory. T: Total trajectory duration.

[0117] S4.3: Adaptive Acceleration / Deceleration Planning

[0118] The controller adaptively adjusts the acceleration and deceleration parameters at each stage based on the trajectory curvature, end-load status, current configuration maneuverability, and distance to the vehicle's charging port.

[0119] ;

[0120] ;

[0121] The operability index is as follows: .in, , Adaptive reference velocity and acceleration. , Maximum speed and maximum acceleration. : Distance correction function (d is the target distance). Curvature correction function ( (For trajectory curvature). : Operability correction function (μ is the operability index). : Load correction function (m is the load estimate).

[0122] S4.4: Global Singular Configuration Avoidance and Zero-Space Configuration Optimization

[0123] The controller continuously evaluates the operability metrics of the current configuration. When it detects that the robotic arm is approaching a singular region or a joint is approaching a soft limit boundary, it adjusts the joint configuration using zero-space motion. This adjustment avoids dangerous areas and maintains configuration stability without altering the end-effector trajectory. During automatic vehicle charging, this mechanism ensures that the charging head assembly maintains high accessibility and execution stability during complex attitude adjustments and close-range precision alignment. The zero-space optimization objective function is:

[0124]

[0125] The singularity avoidance index is:

[0126] Limit avoidance indicators are:

[0127] Zero-space optimization speed is:

[0128] in, Weights of each optimization item. : To prevent small positive numbers with a denominator of zero. : Zero-space optimized gain matrix.

[0129] Step S5: Adaptive Closed-Loop Motion Control Based on Zero-Space Decoupling

[0130] The controller performs hierarchical closed-loop control to ensure the insertion accuracy of the nozzle 3.2.

[0131] S5.1: Task Space Closed-Loop Control Layer

[0132] Using the target position and attitude at the end of the gun head 3.2 as the control objects, calculate the end-effector pose error:

[0133] Generate the generalized joint velocity commands corresponding to the task space: ;in, : Task space error vector. : Position error vector. : Attitude error vector. : Generalized joint velocity command corresponding to the task space. Target terminal velocity. : Task space error feedback gain matrix. Rotate the end effector of the robotic arm assembly from its current pose R to the target pose. Required relative rotation matrix, For the target posture of the robotic arm component end effector Rotate to current pose The required relative rotation matrix. : A mapping operation from an antisymmetric matrix to a vector, where T is the matrix transpose.

[0134] S5.2: Zero-space optimization control layer

[0135] Without altering the pose of the end-target, configuration optimization is performed on redundant degrees of freedom to generate zero-space optimization velocity. The generalized joint speed control command is as follows:

[0136] .

[0137] S5.3: The bottom-level single-axis drive control layer performs independent closed-loop control on the lifting guide shaft and each rotary joint. For the lifting guide shaft, due to its large inertia, long stroke, and significant load-bearing capacity, the controller adopts a layered closed-loop control method, combined with gravity compensation, friction compensation, and inertial feedforward, to reduce the following error and jitter of the guide shaft at different speed ranges. For the six rotary joints, the controller performs high-precision joint control based on the target angle and feedback angle, and superimposes torque feedforward and nonlinear friction compensation to improve the response accuracy and stability when making small angle corrections.

[0138] The lifting guide rail axis is controlled as follows:

[0139] Guide rail displacement error ;

[0140] The control law is: ;

[0141] Gravity compensation term .

[0142] Rotary joint control is as follows:

[0143] The angle error of the i-th rotational joint ;

[0144] The control law is: .

[0145] The collaborative execution control is as follows:

[0146] The underlying integrated control input is ,in , .in, Target displacement and actual displacement of the guide rail. The proportional, derivative, and integral control parameters of the sliding guide rail. Friction compensation item. : Inertial feedforward term. : Gravitational acceleration. : The target angle and the actual angle of the i-th rotational joint. : Proportional, derivative, and integral control parameters of the i-th rotary joint. : Torque feedforward term. : Nonlinear friction compensation term. , : Control inputs for task space and null space. , : Control mapping functions corresponding to the task space and null space.

[0147] S5.4: Multi-sensor fusion feedback

[0148] During the movement and insertion of the robotic arm, the controller continuously collects multi-source feedback information for weighted fusion estimation to determine whether the gun head assembly is correctly aligned with the charging port, whether there is lateral interference, whether jamming occurs, and whether fine-tuning, retraction, or retry actions are required.

[0149] And satisfy .in, : Fusion estimation results. Visual, force, drive current, and position feedback information. : The weight of each feedback channel.

[0150] Step S6: Full-space three-dimensional protection, active fault-tolerant control and safe storage, with the controller performing safety protection throughout the entire operation process.

[0151] S6.1: Graded Collision Detection and Emergency Response

[0152] Construct a comprehensive collision risk index:

[0153] ;

[0154] .

[0155] The grading and testing rules are as follows:

[0156] Level 1 warning (based on environmental sensor information to identify personnel or obstacles entering the work area in advance, and to execute deceleration and pre-avoidance actions to achieve non-contact active protection): ;

[0157] Secondary detection (based on the deviation between joint current feedback and the desired drive state, it detects whether the robotic arm experiences minor collisions, abnormal resistance, or local interference, and performs deceleration or pause actions when the deviation exceeds a preset threshold): ;

[0158] Level 3 detection (based on end force information and position error changes, detects whether the gun head assembly experiences lateral collision, misaligned contact, or jamming during insertion, and immediately stops advancement and performs reverse retraction when the safety threshold is exceeded):

[0159] Level 4 Emergency Response (When a severe abnormal deviation in joint position, a continuous increase in abnormal force, or the system enters a dangerous state is detected, the controller immediately stops the current propulsion action; if the actuator is still under control, an emergency safety retraction action is performed; if the safety retraction conditions are not met, the brakes are maintained and an alarm signal is output to prevent equipment damage or impact on the vehicle): .

[0160] .

[0161] in, Comprehensive collision risk index. Weights of each risk item. Environmental proximity risk items ( As an environmental distance indicator, (This refers to the safe distance threshold). Current anomaly risk item ( For driving current, (Reference current). : Risk item of abnormal force ( For end contact force, (For reference force). Position error risk item ( For end-effector pose error, (This refers to the end reference error).

[0162] ~ : Classification threshold.

[0163] S6.2: Multimodal perception fusion active fault-tolerant control, including sensor fault tolerance, joint fault tolerance, and adaptive parameter updates. Sensor fault tolerance: When the vision sensor identifies an anomaly, the target is lost, or the data is unreliable, the system can maintain its current safe state and execute conservative actions or enter a safe exit process based on other available feedback information, thereby preventing the robotic arm from continuing to perform dangerous actions under abnormal perception conditions. Joint fault tolerance: When a rotary joint malfunctions, the controller can lock the abnormal joint and reconstruct the current motion control strategy based on the remaining controllable degrees of freedom, allowing the robotic arm to prioritize safe exit or reset actions, avoiding lingering in high-risk postures. Adaptive parameter updates: After power-on, the system can execute initialization parameter identification and state detection processes, updating friction compensation parameters, load estimation parameters, and control gain based on the current structural state, load state, and operating history, enabling the robotic arm to maintain good control consistency and adaptability during long-term operation. Specific operations are as follows:

[0164] Evaluation system health status vector ;

[0165] Overall health level:

[0166] When satisfied At this time, the system enters fault-tolerant control mode.

[0167] The fault-tolerant control input is:

[0168] The adaptive update law for parameters is:

[0169] System health state vector ( , , , , These are health indicators for vision, force, electrical current, position, and actuators, respectively. Overall health status. Health weighting coefficient. Fault switching threshold. : Fault-tolerant control input. : Safe target pose. Additional safety control parameters. Online parameter estimation. Update gain. : Regression feature vector. Actual observations. Model predictions.

[0170] S6.3: Minimum Envelope Retraction Reset. When the task is completed, a system malfunction occurs, or a stop command is received, the controller automatically generates a minimum envelope retraction trajectory. This causes the lifting guide rail and six rotary joints to work together to retract the robotic arm into a safe posture with a smaller envelope space, lower joint load, and lower collision risk. This improves the spatial adaptability and shutdown safety of the equipment in applications such as parking lots and underground garages. After charging is complete, the controller controls the gun head assembly 3 to smoothly retract in the reverse direction along the insertion direction. Then, minimum envelope retraction optimization is performed. Simultaneously satisfying safety distance constraints: ;in, Minimum envelope storage optimization objective. : Envelope volume after storage. Total motion of the rotational joint. : Length of the pullback path. , , Weighting coefficient. : Target configuration for storage. : The minimum distance between the robotic arm and the obstacle.

[0171] Through the above steps, the robotic arm completes the entire automated operation from standby, identification, alignment, insertion, charging to safe storage, achieving high-precision, high-safety and high-reliability automatic vehicle charging.

[0172] In summary, this invention achieves fully unmanned operation of automatic vehicle charging in parking lot scenarios through the coordinated control of the lifting mechanism 1, the robotic arm assembly 2, and the gun head assembly 3, combined with multi-sensor fusion and intelligent algorithms. The specific process is as follows:

[0173] Standby storage: The robotic arm is initially in the minimum envelope storage state: the lifting guide rail is at the initial safe height (e.g., 30cm above the ground), the six rotating joints (driven by motors from the first to the sixth joints) are folded to the preset storage angle, and the gun head assembly 3 (including the gun head 3.2 and camera 3.5) is retracted into the protection area near the robotic arm base 1.1 to avoid collisions with pedestrians or vehicles.

[0174] Vehicle arrival confirmation: The system continuously monitors the charging station through a 3.5 camera. When the vehicle enters the preset area and stops stably, the controller verifies that there are no obstacles or personnel interference in the working space and triggers the automatic charging process.

[0175] Lifting and Approaching: The controller drives the lifting drive motor 1.2 of the lifting mechanism 1 to move, which drives the lifting guide rail module 1.3 to rise, so that the entire robotic arm assembly 2 reaches the height range that matches the vehicle charging port (such as 80-120cm above the ground); at the same time, the first joint motor 2.2 initially adjusts the spatial unfolding posture of the robotic arm assembly 2 to prepare for subsequent alignment.

[0176] Spatial posture deployment: Based on the initial rough pose of the charging port identified by camera 3.5, the controller controls the coordinated movement of six rotary joints: the first joint motor 2.2 drives the first rotating part 2.3 to rotate, adjusting the overall orientation of the robotic arm; the second joint motor 2.4 and the third joint motor 2.9 drive the second rotating part 2.5 and the third rotating part 2.10 respectively, and through the linkage of the connecting rod 2.7, make the gun head assembly 3 gradually approach the area in front of the charging port (such as 50cm away from the charging port); the fourth joint motor 2.11 and the fifth joint motor 2.14 adjust the wrist posture, so that the direction of the gun head 3.2 is initially aligned with the direction of the charging port.

[0177] Proximal coarse alignment: When the gun head assembly 3 enters the preset area in front of the charging port (e.g., within 20cm), the controller reduces the overall movement speed and corrects the lifting height and the posture of each joint according to the target pose error, so that the gun head 3.2 reaches the pre-insertion position (e.g., 5cm away from the charging port).

[0178] Precise alignment: The camera 3.5 recaptures close-up images, and the controller obtains the precise pose of the charging port through secondary recognition (such as sub-pixel level edge detection). Combined with the zero-space optimization algorithm, the joint configuration is adjusted to ensure that the axis of the gun head 3.2 is completely aligned with the axis of the charging port, and the end pose error is controlled within the set value.

[0179] Low-speed insertion: The controller controls the gun head assembly 3 to advance along the charging port direction at a low speed (e.g., 5mm / s), while the force sensor monitors the axial thrust (within normal range) and drive current (within normal range) in real time. If the advancement depth reaches the set value and the force is stable, the insertion is considered successful.

[0180] Abnormal Handling: If abnormal force, abnormal current, or abnormal position error is detected, the controller will immediately stop advancing, control the gun head assembly 3 to retract a preset distance (e.g., 10cm), and re-execute the fine alignment and low-speed insertion action.

[0181] Connection and charging hold: After successful connection, the system locks the current configuration of the robotic arm and enters the charging hold phase, monitoring the charging status (such as voltage and current) in real time until charging is complete.

[0182] Pull-out and retraction reset: After charging is completed, the controller controls the gun head assembly 3 to smoothly retract in the reverse direction of the insertion direction (e.g., speed ≤10mm / s), and then drives the lifting mechanism 1 to descend. The six rotating joints fold and reset according to the minimum envelope storage trajectory, and the gun head assembly 3 returns to the protection area near the robotic arm base 1.1, waiting for the next charging task.

[0183] This invention achieves the following core technical effects in the automatic vehicle charging scenario in parking lots through the deep integration of algorithmic innovation and configuration design: It adopts a seven-degree-of-freedom redundant configuration of "one lifting guide axis and six spatially connected rotary joints," combined with global kinematic modeling based on spinor theory, breaking through the posture limitations of traditional six-axis robotic arms. Even under complex conditions such as vehicle parking errors, charging port height deviations, or posture tilts, a better joint configuration can still be selected through redundant degree-of-freedom optimization (zero-space motion), improving the adaptability of the gun head 3.2 to the charging port positions of different vehicle models. Through multi-feedback fusion closed-loop control (vision + force + position), it can improve the automatic alignment success rate and reduce the insertion failure rate when the vehicle has lateral deviation; low-speed compliant insertion (e.g., speed ≤ 5mm / s) effectively reduces the insertion failure rate. This invention proposes redundant configuration optimization control based on zero-space decoupling, separating the end-effector pose control in the task space from the zero-space configuration optimization. Without changing the end-effector trajectory, it avoids singular configurations and soft joint limitations through zero-space motion. The robotic arm exhibits minimal joint acceleration fluctuations and end-effector vibration amplitude when approaching the charging port, preventing damage to the charging port caused by sudden, large joint movements. Long-term operation reduces joint wear and improves structural stability. This invention integrates full-space three-dimensional protection and active fault-tolerant control algorithms, including graded collision detection (level 1 warning to level 4 emergency response) and minimum envelope containment optimization. In non-working states, the robotic arm's containment envelope volume is small, adapting to confined parking spaces. During operation, if personnel are detected approaching, a level 1 warning is immediately triggered, and the arm decelerates, effectively reducing the risk of collision. In the event of an abnormal power outage, minimum envelope containment is automatically executed to prevent the equipment from falling. Furthermore, this invention also designs multimodal perception fusion active fault-tolerant control, supporting sensor fault tolerance (e.g., switching to laser ranging backup when camera 3.5 fails), joint fault tolerance (locking abnormal joints and reconstructing the control strategy), and adaptive parameter updates. Even when the camera is briefly obstructed or data is abnormal, the system can still maintain charging operations through force feedback, resulting in a low task interruption rate. After long-term operation, the control accuracy decays little, and there is no need for frequent calibration, making it suitable for unattended parking lot scenarios.

[0184] In summary, this invention, through its technical system of "seven-degree-of-freedom redundant configuration, screw kinematic modeling, priority trajectory planning, zero-space decoupling control, and full-space protection," achieves high-precision, high-safety, and high-reliability fully automated operation in the automatic vehicle charging scenario in parking lots. It solves industry pain points such as insufficient degrees of freedom, low control precision, weak protection capabilities, and poor adaptability of traditional robotic arms, and has significant engineering application value.

[0185] 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 six-axis fully automatic robotic arm, comprising a robotic arm base and robotic arm components, characterized in that: A lifting mechanism for raising and lowering the robotic arm assembly is connected between the robotic arm base and the robotic arm assembly. The robotic arm assembly includes six rotary joints connected in series via connectors. Each rotary joint includes a rotary drive component and a rotary connecting component. The rotary drive component can drive the rotary connecting component to rotate. The connector includes a mounting base for mounting the rotary drive component and a connecting rod for fixing the front and rear rotary connecting components. The lifting mechanism controls the lifting motion using a global kinematics modeling strategy, a full-space trajectory planning strategy, and a zero-space closed-loop control strategy. The rotary drive component drives the rotary connecting component to rotate using the same strategy. The lifting mechanism and the rotary joints are protected by a full-space safety protection strategy.

2. A six-axis fully automatic robotic arm control system based on the six-axis fully automatic robotic arm according to claim 1, characterized in that: include: The global kinematics modeling module is used to model the configuration of a seven-DOF robotic arm consisting of one lifting degree of freedom and six rotational degrees of freedom, forming a screw kinematic model. The global pose solution, joint constraints, and configuration maneuverability data are obtained through the screw kinematic model. The full-space trajectory planning module is used to generate continuous motion trajectories and adaptive acceleration / deceleration strategies for each degree of freedom of the robotic arm based on the global pose solution, joint constraints, and configuration operability data, combined with the target pose of the work object and the priority of the work stage. It outputs the target trajectory instructions, stage optimization targets, and trajectory constraint parameters for each degree of freedom of the robotic arm. The zero-space closed-loop control module is used to perform closed-loop control and zero-space configuration optimization of the robotic arm's posture based on the target trajectory command, stage optimization target and trajectory constraint parameters, combined with real-time sensor feedback data of vision and force, so that the robotic arm can complete the target action and provide real-time feedback of the end-effector posture and motion state data of the robotic arm.

3. The six-axis fully automatic robotic arm control system 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, and active fault-tolerant control on the robotic arm based on the joint constraints, 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 six-axis fully automatic robotic arm control system as described in claim 3, characterized in that: The method for modeling a seven-degree-of-freedom robotic arm configuration consisting of one lifting degree of freedom and six rotational degrees of freedom to form a screw kinematic model includes: establishing a base coordinate system with the center of the robotic arm base as the origin, establishing a lifting axis coordinate system with the lifting center of the lifting mechanism as the origin, establishing a six-joint coordinate system with the rotation centers of the six rotational joints as the origin, and establishing a tool coordinate system with the end point center of the robotic arm assembly as the origin. Based on the base coordinate system, the lifting axis coordinate system, the joint coordinate system, and the tool coordinate system, the generalized joint variable q of the robotic arm and the translation transformation matrix T of the lifting mechanism are defined. h The exponential product formula is used to define the homogeneous transformation matrix of the robot arm's end-effector pose. The generalized joint variable q of the robotic arm and the translation transformation matrix T of the lifting mechanism. h Homogeneous transformation matrix of the end effector pose of the robotic arm This constitutes the spinor kinematic model; The expression for the generalized joint variable q of the robotic arm is: The expression for the translation transformation matrix Th of the lifting mechanism is: The homogeneous transformation matrix of the end effector pose of the robotic arm The expression is: in, This refers to the lifting displacement of the lifting mechanism. For the first The rotation angle of each rotational joint, where T is the vector transpose and e is the base of the natural logarithm. For the first Rotational power of a rotating joint spinor The antisymmetric matrix form, This is the initial pose matrix of the end effector of the robotic arm in the zero-position configuration.

5. The six-axis fully automatic robotic arm control system as described in claim 4, characterized in that: The method for obtaining the global pose solution, joint constraints, and configuration maneuverability data through a screw kinematic model includes: using the generalized joint variable q of the robotic arm as input, and calculating the homogeneous transformation matrix of the robotic arm's end-effector pose using the exponential product formula. Output the end effector pose of the robotic arm The global pose solution is obtained, and the end effector pose of the robotic arm is obtained. The expression is: ,in, For positive kinematic mapping function, This is the compensation amount for the straightness error of the lifting mechanism. This is the amount of backlash compensation for the rotary joint. This is the amount to compensate for installation deviation. This is the amount of elastic deformation compensation for the connecting rod; In the process of solving the inverse kinematics and multi-objective optimization of the robotic arm, the physical and soft constraint boundaries of the rotary joint are clearly defined and incorporated into the optimization algorithm as constraint penalties for the rotary joint to solve for the optimal configuration of the rotary joint. The joint constraints include the constraint penalties for the rotary joint. The expression is: ,in, This is the minimum permissible rotation angle of the rotary joint. This represents the maximum permissible rotation angle of the rotary joint. An analytical Jacobian matrix containing a lifting mechanism and six rotary joints is constructed to quantify the motion flexibility and redundancy of the robotic arm, thereby obtaining the operability data of the configuration. , The expression is: Where J is the Jacobian matrix, This is the transpose of the Jacobian matrix. It is a determinant.

6. The six-axis fully automatic robotic arm control system as described in claim 5, characterized in that: Based on the global pose solution, joint constraints, and configuration operability data, and combined with the target pose of the work object and the priority of the work stage, the method generates continuous motion trajectories and adaptive acceleration / deceleration strategies for each degree of freedom of the robotic arm, and outputs target trajectory commands, stage optimization targets, and trajectory constraint parameters for each degree of freedom of the robotic arm. This includes: dynamically adjusting the weight distribution among the position accuracy, attitude accuracy, motion stability, and force safety of the robotic arm according to different stages of the robotic arm's work task; generating a continuous target pose trajectory at the end of the robotic arm component in Cartesian space, and simultaneously planning the motion trajectories of the lifting mechanism and rotary joints in joint space; and using the Cartesian space reference trajectory of the end of the robotic arm component. The expression is: Generalized joint reference trajectory The expression is: ,in, This is the reference position trajectory for the end effector of the robotic arm component. This serves as the reference attitude trajectory for the end effector of the robotic arm component. This is the initial configuration for the robotic arm. For the target configuration of the robotic arm, For time scaling functions, The null projection matrix, To optimize the trajectory for zero space, during the motion trajectory planning of the lifting mechanism and rotary joints, the operability index of the current robotic arm configuration is evaluated in real time. Zero-space motion is used to adjust the joint configuration of the robotic arm without changing the target trajectory at the end of the robotic arm components. The expression for the zero-space optimization objective function is: ,in, To optimize the overall configuration objective function, Assigning weights to each optimization term, For the singularity avoidance indicator, To limit and avoid indicators, To optimize torque performance, For a pre-defined ideal joint combination of robotic arms, It is the Euclidean norm.

7. The six-axis fully automatic robotic arm control system as described in claim 6, characterized in that: Based on the target trajectory command, stage optimization target, and trajectory constraint parameters, combined with real-time sensor feedback data from vision and force, the robot arm's pose is subjected to closed-loop control and zero-space configuration optimization to enable the robot arm to complete the target action. The method for real-time feedback of the robot arm component's end-effector pose and motion state data includes: using the target position and posture of the robot arm component's end-effector as the control object, generating control quantities to correct the displacement of the lifting guide rail and the motion of each rotary joint based on the deviation between the current end-effector pose and the target end-effector pose; and the robot arm component end-effector pose error. The expression is: Generalized joint velocity commands corresponding to the task space The expression is: ,in, Position error Attitude error The target position of the end effector of the robotic arm component. This represents the current position of the end effector of the robotic arm assembly. Rotate the end effector of the robotic arm assembly from its current pose R to the target pose. Required relative rotation matrix, For the target posture of the robotic arm component end effector Rotate to current pose Required relative rotation matrix, It is the Jacobian pseudo-inverse matrix. The target velocity at the end effector of the robotic arm component. This is the task space error feedback gain matrix. Here, T is the vectorization operator, and T is the matrix transpose.

8. The six-axis fully automatic robotic arm control system as described in claim 7, characterized in that: Based on the joint constraints, the robotic arm motion state data, and multi-sensor monitoring information, a method for performing graded collision detection, fault identification, and active fault-tolerant control on the robotic arm, outputting safety intervention commands, and real-time constraint and closed-loop control of the robotic arm's motion trajectory includes: constructing a comprehensive collision risk index. Assessment system overall health When the overall health of the system Less than the threshold When the robot arm malfunctions or finishes its work, it enters the active fault-tolerant control mode. When the robot arm malfunctions or finishes its work, it performs minimum envelope containment optimization.

9. The six-axis fully automatic robotic arm control system as described in claim 8, characterized in that: The comprehensive collision risk index The expression is: ; System overall health The expression is: The objective function for optimizing the minimum envelope of a robotic arm. The expression is: ;in, These are the weights for each risk item. For environmental proximity risk items, This is a risk item related to abnormal current. This is a risk item related to abnormal force. This is a pose error risk item. As a weighted coefficient for health, Weighted by health score The corresponding system health statuses, These are the weighting coefficients. This refers to the envelope volume after the robotic arm is stowed away. This represents the total motion of the robotic arm's rotary joints. This represents the length of the robotic arm's retraction path.

10. A six-axis fully automatic robotic arm control method based on the six-axis fully automatic robotic arm control system according to any one of claims 2-9, characterized in that: include: A seven-DOF robotic arm configuration consisting of one lifting degree of freedom and six rotational degrees of freedom is modeled to form a screw kinematic model. The global pose solution, joint constraints, and configuration maneuverability data are obtained through the screw kinematic model. Based on the global pose solution, joint constraints, and configuration maneuverability data, combined with the target pose of the work object and the priority of the work stage, continuous motion trajectories and adaptive acceleration / deceleration strategies for each degree of freedom of the robotic arm are generated. The target trajectory commands, stage optimization targets, and trajectory constraint parameters for each degree of freedom of the robotic arm are output. Based on the target trajectory commands, stage optimization targets, and trajectory constraint parameters, combined with real-time sensor feedback data from vision and force, closed-loop control and zero-space configuration optimization are performed on the robotic arm's pose to enable the robotic arm to complete the target action. Real-time feedback of the end-effector pose and motion state data of the robotic arm is also provided.

Images