Large-load posture-adjusting docking mobile parallel compound robot and control method thereof

Abstract

This invention discloses a high-load, attitude-adjusting, docking, and mobile parallel composite robot and its control method, belonging to the field of robotics technology. The high-load, attitude-adjusting, docking, and mobile parallel composite robot includes a moving platform, an AGV (Automated Guided Vehicle) cart, and a control system. Six parallel branches are arranged between the moving platform and the AGV cart. The upper and lower ends of the branches are hinged to the lower end face of the moving platform and the upper surface of the AGV cart, respectively, via Hooke's joints. The branches are symmetrically arranged axially between the moving platform and the AGV cart. The control system includes a network master control layer, a master-slave drive control layer, and an execution component layer. The network master control layer and the master-slave drive control layer are connected via a wireless local area network. The master-slave drive control layer is communicatively connected to the execution component layer. The robot of this invention has the advantages of high load capacity, low cost, and large space requirements. Its control method has high mobility and high real-time performance.

Description

Technical Field

[0001] This invention belongs to the field of robotics technology, specifically relating to a high-load attitude adjustment docking mobile parallel composite robot and its control method. Background Technology

[0002] In complex scenarios such as the assembly and processing of large parts, traditional manual high-precision orientation adjustment has obvious limitations, such as high skill requirements and strong repetition, which leads to heavy workload and fatigue of operators, making it difficult to ensure the stability and consistency of the operation, thus affecting production efficiency and processing accuracy.

[0003] With the development of intelligent manufacturing, mobile robot systems integrating AGVs and robotic arms have become a research hotspot. Compared with serial robotic arms, parallel robots have simpler inverse kinematics solutions due to their symmetrical structure, and superior structural stiffness and load capacity. The system formed by the integration of AGVs and parallel robots has the ability to move over a wide range of distances and adjust posture with high load and precision. It can meet the needs of long-distance transportation of large parts and millimeter-level and sub-millimeter-level posture adjustment, and has broad application prospects in high-end equipment manufacturing fields such as aerospace and rail transportation.

[0004] Currently, while six-DOF parallel robots are widely used in many fields, research on ball joints and prismatic joints as motion joints is relatively mature. However, there are still obvious drawbacks. For example, ball joints are expensive and have a small rotation range, which limits the workspace. Furthermore, for the same lead screw radius, prismatic joints have a smaller extension ratio and lower structural stiffness than helical joints, making it difficult to meet the needs of heavy-duty, large-scale posture adjustment.

[0005] Existing research is mostly based on ideal models. In practice, control accuracy is limited by factors such as kinematic pair clearance, friction, and load variations. In particular, parallel robots with Hooke joints and helical pairs have unique kinematic and inverse dynamic models and complex motion coupling relationships. Existing high-precision control algorithms are imperfect and cannot fully utilize the advantages of the structure. Furthermore, existing control strategies lack anti-interference and adaptive capabilities, making it difficult to meet the speed, accuracy, and stability requirements of heavy-load, high-precision attitude adjustment scenarios. Summary of the Invention

[0006] This invention is proposed to solve the problems existing in the prior art, and its purpose is to provide a high-load attitude adjustment docking mobile parallel composite robot and its control method.

[0007] The technical solution of the present invention is: a high-load attitude adjustment docking mobile parallel composite robot, the main body of which includes a moving platform, an AGV trolley, and a control system;

[0008] Six parallel branches are set between the moving platform and the AGV trolley; the upper and lower ends of the branches are respectively connected to the lower end face of the moving platform and the upper surface of the AGV trolley through Hooke joints; in the initial pose, the branches are symmetrically arranged in the axis plane between the moving platform and the AGV trolley.

[0009] The branch chain includes a moving platform Hooke hinge that is hinged to the lower end face of the moving platform and a stationary platform Hooke hinge that is hinged to the upper surface of the AGV trolley; a telescopic mechanism is provided between the moving platform Hooke hinge and the stationary platform Hooke hinge.

[0010] The moving platform Hooke hinge and the static platform Hooke hinge have the same structure. Both include a lower Hooke hinge connected to the plate. The lower Hooke hinge has a movable upper Hooke hinge. The two upper Hooke hinges are connected to the telescopic mechanism.

[0011] The telescopic mechanism includes a sleeve, a force sensor, a force sensor base plate, and a support screw. The sleeve is connected to the force sensor, the force sensor is connected to the force sensor base plate, the force sensor base plate is connected to the upper Hooke hinge on the upper surface of the AGV trolley, the inner cavity of the sleeve accommodates the support screw, and the telescopic end of the support screw is connected to the upper Hooke hinge at the lower end of the moving platform.

[0012] The control system includes a network master control layer, a master-slave drive control layer, and an execution component layer. The network master control layer and the master-slave drive control layer are connected via a wireless local area network. The master-slave drive control layer can receive and execute control commands from the rugged industrial tablet in the network master control layer and collect and feed back data information from the execution component layer. The master-slave drive control layer is communicatively connected to the execution component layer. The execution component layer can complete the overall motion control of the assembly robot. At the same time, the execution component layer can feed back data information to the master-slave drive control layer.

[0013] Furthermore, the network main control layer includes a function selection module, a parameter preset module, a mechanism kinematics module, a PLC main control module, and a motion control algorithm module;

[0014] The function selection module is connected to the parameter preset module and the mechanism kinematics module. The parameter preset module is connected to the PLC main control module. The PLC main control module can realize the communication connection configuration and data interaction of the system hardware, as well as the real-time control of the hardware. The mechanism kinematics module is connected to the motion control algorithm module.

[0015] Furthermore, the master-slave control layer includes a communication coupling module, a motor drive module, an input / output module, a multi-source sensing module, and a sensor module;

[0016] The communication coupling module and the input / output module can interact with each other. The input / output module can connect external input / output variables to the EtherCAT fieldbus network. The input / output module is connected to the multi-source sensing module, sensor module, motor drive module, and control panel module of the master-slave drive layer. The motor drive module is connected to the execution component layer, which can realize high-precision positioning and movement of servo motors, receive and parse relevant instructions for external motor control, and send control signals to the corresponding motors.

[0017] Furthermore, the execution component layer includes a driver module and a third servo motor; the third servo motor can receive control information from the driver module and execute it; the third servo motor is connected to the servo motor module of the execution component layer, and the servo motor module is connected to the parallel assembly robot and can drive the assembly robot.

[0018] Furthermore, the motor drive module is used for high-precision positioning of the servo motor. The input port of the motor drive module is used to receive and parse relevant instructions from external motor control, and the output port of the motor drive module is used to send control signals to the corresponding driver module.

[0019] This invention also provides a control method for a high-load attitude-adjusting docking mobile parallel composite robot, comprising the following steps:

[0020] Step (i) The system starts up and completes the initialization self-test. The operator selects the control mode through the function selection module and inputs specific control commands through the control panel module.

[0021] Step (ii): After receiving the instruction, the network master control layer combines the selected control mode and calls the corresponding control algorithm to calculate the target motion information of the end-effector.

[0022] Step (iii): The length of each branch rod is obtained through the inverse kinematics control algorithm, converted into the target value of the servo motor encoder code disk and transmitted to the master-slave drive control layer;

[0023] Step (iv): The motor drive module receives the target value and sends a control signal to the motor according to the current control mode to drive the moving platform to move.

[0024] Step (v): During the movement, the multi-source sensing module provides real-time feedback of data from the interactive force sensing module and the position information feedback module. After the operator moves the moving platform to the vicinity of the target position, they switch to the fine-tuning mode to precisely adjust the pose until the task is completed.

[0025] Step (vi): During the operation, if the monitoring data of the network master control layer is abnormal, the operator can press the emergency stop button to stop the robot immediately.

[0026] Step (vii): After the task is completed, the operator issues a reset command, the robot returns to its initial position, and the system records the task data.

[0027] Furthermore, in step (iii), the inverse kinematics control algorithm includes the following steps:

[0028] Step (a1): Based on the preset high-load docking operation trajectory planning, the desired target pose of the end-effector dynamic platform in the static platform base coordinate system is obtained in real time;

[0029] Step (a2): Based on the closed-loop vector method and spatial geometric transformation theory, establish the ideal position inverse kinematic model of the parallel robot. Substitute the desired target pose from step (a1) into the model to calculate the theoretical target extension length of the six parallel branches under the error-free ideal state.

[0030] Step (a3): Combine the mechanical transmission parameters of the parallel robot to establish a physical mapping model, and directly convert the theoretical target extension length of each branch calculated in step (a2) into the theoretical target drive pulse number of the corresponding underlying servo motor.

[0031] In step (a4), the motion control algorithm module uses the theoretical target driving pulse number obtained in step (a3) ​​as an absolute position command and sends it to the servo drivers of each branch in the execution component layer in real time, driving the motors of each branch to move in coordination, and controlling the end effector to accurately reach the desired target pose.

[0032] Furthermore, in step (v), the interactive force sensing module in the multi-source sensing module includes the following steps:

[0033] Step (b1): Based on the finite instantaneous spinor theory and the principle of virtual work, an inverse dynamics model of the parallel robot is established to predict the theoretical driving force in the case of no interaction force.

[0034] Step (b2) compensates for the inverse dynamics model in step (b1) and establishes a friction force model;

[0035] Step (b3) involves identifying the parameters of the friction model in step (b2), and then using the identified friction model to compensate the inverse dynamics model in step (b1) to obtain the compensated theoretical driving force.

[0036] Step (b4): Based on the compensated inverse dynamics model and motor current feedback, an external force observer is constructed to estimate the external interaction force at the end in real time.

[0037] Furthermore, in step (v), the location information feedback module in the multi-source sensing module includes the following steps:

[0038] Step (c1): Based on the feedback signal of the absolute encoder of the underlying servo motor, establish a joint space physical mapping model and obtain the actual extension length of each parallel branch in real time.

[0039] Step (c2) establishes the position inverse constraint equation of the parallel robot based on the closed-loop vector method and spatial geometric constraints, and constructs a nonlinear objective function for solving the forward kinematics by combining the actual extension length in step (c1).

[0040] Step (c3): For the nonlinear objective function constructed in step (c2), the pose of the previous control cycle is introduced as the initial value, and the Newton-Raphson iterative algorithm is used to solve it in real time to obtain the estimated spatial pose of the end effector in the base coordinate system.

[0041] Step (c4): Based on the spatial pose of the moving platform obtained in step (c3), and combined with the local motion conversion of the seventh axis, an extended Kalman filter (EKF) state observer is constructed to filter and fuse high-frequency noise, and output the absolute global pose of the end effector of the composite robot in real time.

[0042] The beneficial effects of this invention are as follows:

[0043] This invention discloses a high-load attitude adjustment and docking mobile parallel composite robot. This parallel attitude adjustment robot combines the advantages of AGV carts and parallel robots, and adopts a combination of Hooke joints and screw pairs. Compared with traditional robots, this invention improves load capacity, reduces costs, expands workspace, can meet the needs of large component assembly, etc., and has outstanding operation performance and wide applicability, especially in aerospace, rail transportation and other fields. It solves the problems of high intensity, poor consistency and low efficiency of manual attitude adjustment in the prior art.

[0044] The present invention also has the following technical effects:

[0045] First, it has low development and usage costs and is highly practical: it adopts an open structure and modular design, which reduces the difficulty of development, eliminates the need for motion control cards, and saves space and costs; it has real-time computing capabilities and improves portability by transmitting data through a local area network; it is easy and safe to operate, reducing manpower and time costs.

[0046] Second, the system has high real-time performance and reliability: Compared with traditional fieldbus, the control system has better real-time performance and time determinism, frequent short frame information exchange, strong fault tolerance, and can ensure the stable operation of high-precision and high-safety operations.

[0047] Third, the overall benefits are significant: while meeting the requirements of heavy-duty and high-precision operations, it can improve production efficiency, reduce costs, and promote the development of industrial automation and intelligence by virtue of its advantages such as low cost and high reliability.

[0048] Fourth, in terms of control: Six-DOF parallel robots have complex structures and strong couplings, making them difficult to control. Six-DOF parallel posture adjustment and positioning robots with Hooke joints and helical pairs as core motion joints are integrated with AGV systems to improve adaptability. Attached Figure Description

[0049] Figure 1 This is a schematic diagram of the overall structure of a high-load attitude adjustment docking mobile parallel composite robot provided in an embodiment of the present invention;

[0050] Figure 2 This is a structural block diagram of the control system provided in an embodiment of the present invention;

[0051] Figure 3 This is a schematic diagram of the hinge point layout of the static platform provided in an embodiment of the present invention;

[0052] Figure 4 This is a schematic diagram of the hinge point layout of the moving platform provided in an embodiment of the present invention;

[0053] Figure 5 This is a schematic diagram of the branch structure of the parallel robot provided in an embodiment of the present invention;

[0054] Figure 6 This is a schematic diagram of the Hooke hinge structure of the parallel robot's static and dynamic platform provided in an embodiment of the present invention;

[0055] Figure 7 This is a front view structural diagram of the parallel robot motion platform provided in an embodiment of the present invention;

[0056] Figure 8 This is a top view structural diagram of the parallel robot motion platform provided in an embodiment of the present invention;

[0057] Figure 9 This is a schematic diagram of the posture adjustment and assembly of a mobile parallel robot provided in an embodiment of the present invention;

[0058] Figure 10 This is a schematic diagram of the inverse kinematics solution of a mobile parallel robot provided in an embodiment of the present invention;

[0059] Figure 11 This is a schematic diagram of the hinge point arrangement of a mobile parallel robot provided in an embodiment of the present invention;

[0060] Figure 12 This is a flowchart of a control method for a high-load attitude adjustment docking mobile parallel composite robot provided in an embodiment of the present invention.

[0061] The components include: 1. Moving platform; 2. Branch chain; 3. AGV trolley; 4. Network master control layer; 5. Master-slave drive control layer; 6. Execution component layer; 10. Moving platform main body; 101. Moving platform Hooke hinge adapter plate; 11. Photoelectric switch; 12. Tooling connection plate; 13. Slider adapter; 14. Mechanical limit bracket; 15. Mechanical limit block; 16. Guide rail slider; 17. Seventh axis assembly; 18. First servo motor; 19. Lead screw fixed end; 20. Lead screw nut; 21. Seventh axis lead screw; 22. Lead screw moving end; 23. Motor connection plate; 24. Second servo motor; 25. Static platform Hooke hinge; 26. Force sensor base plate; 27. Force sensor; 28. Sleeve; 29. ​​Branch chain lead screw; 30. 31. Moving platform Hooke hinge; 32. AGV trolley body; 251. Static platform Hooke hinge adapter plate; 252. Lower Hooke hinge; 453. Upper Hooke hinge; 41. Parameter preset module; 42. PLC main control module; 43. PLC main control system; 44. Enable module; 45. Function selection module; 46. Mechanism kinematics module; 47. Motion control algorithm module; 51. Control panel module; 52. Motor drive module; 53. Communication coupling module; 54. Input / output module; 55. Multi-source sensing module; 56. Sensor module; 61. Servo motor module; 62. Assembly robot; 63. Third servo motor; 64. Driver module. Detailed Implementation

[0062] It should be noted that in the following embodiments, the terms "first", "second", "third", etc. are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated.

[0063] The present invention will be described in detail with reference to the accompanying drawings and embodiments:

[0064] like Figures 1 to 12 As shown, a high-load attitude adjustment docking mobile parallel composite robot mainly includes a moving platform 1, an AGV trolley 3, and a control system.

[0065] like Figure 1 As shown, six parallel branches 2 are arranged between the moving platform 1 and the AGV trolley 3; the upper and lower ends of the branches 2 are respectively connected to the lower end face of the moving platform 1 and the upper surface of the AGV trolley 3 through Hooke joints; in the initial position, the branches 2 are symmetrically arranged in the axial plane between the moving platform 1 and the AGV trolley 3.

[0066] like Figure 5 As shown, the branch 2 includes a moving platform Hooke hinge 30 hinged to the lower end face of the moving platform 1 and a stationary platform Hooke hinge 25 hinged to the upper surface of the AGV trolley 3; a telescopic mechanism is provided between the moving platform Hooke hinge 30 and the stationary platform Hooke hinge 25.

[0067] like Figure 6As shown, the moving platform Hooke hinge 30 and the stationary platform Hooke hinge 25 have the same structure, both including a lower Hooke hinge 251 connected to the plate, and a movable upper Hooke hinge 252 is provided in the lower Hooke hinge 251. The two upper Hooke hinges 252 are connected to the telescopic mechanism.

[0068] like Figure 5 As shown, the telescopic mechanism includes a sleeve 28, a force sensor 27, a force sensor base plate 26, and a support screw 29. The sleeve 28 is connected to the force sensor 27, the force sensor 27 is connected to the force sensor base plate 26, the force sensor base plate 26 is connected to the upper Hooke hinge 252 on the upper surface of the AGV trolley 3, the inner cavity of the sleeve 28 accommodates the support screw 29, and the telescopic end of the support screw 29 is connected to the upper Hooke hinge 252 at the lower end of the moving platform 1.

[0069] like Figure 2 As shown, the control system includes a network master control layer 4, a master-slave drive control layer 5, and an execution component layer 6. The network master control layer 4 and the master-slave drive control layer 5 are connected via a wireless local area network. The master-slave drive control layer 5 can receive and execute the control commands of the rugged industrial tablet in the network master control layer 4 and collect and feed back data information from the execution component layer. The master-slave drive control layer 5 is communicatively connected to the execution component layer 6. The execution component layer 6 can complete the overall motion control of the assembly robot 62. At the same time, the execution component layer 6 can feed back data information to the master-slave drive control layer 5.

[0070] Specifically, the lower Hooke hinge 251 in the moving platform Hooke hinge 30 is connected to the moving platform Hooke hinge adapter plate 101, whose lower end face is precision machined. The adapter plate is a precision-machined flat surface, ensuring accuracy. Each moving platform Hooke hinge adapter plate 101 is evenly distributed on the moving platform body 10, and the installation height of the six branches remains consistent. Figure 3 , Figure 4 As shown. The lower Hooke hinge 251 of the static platform Hooke hinge 25 is connected to the static platform Hooke hinge adapter plate 32, which is precision machined on the upper surface of the AGV trolley 3. The adapter plate is a precision-machined flat surface to ensure accuracy. The adapter plate is fixed to the static and moving platforms. Each static platform Hooke hinge adapter plate 32 is evenly distributed on the AGV trolley body 31, and the installation height of the six branches is consistent. Figure 4 As shown.

[0071] Specifically, a motor connecting plate 23 is provided at the end of the sleeve 28. A second servo motor 24 is externally connected to one side of the motor connecting plate 23, and a drive mechanism for driving the lead screw is provided in the motor packaging block on the other side of the motor connecting plate 23. The second servo motor 24 is parallel to the axis of the branch lead screw 29, and the drive mechanism is a synchronous drive mechanism, which drives the seventh axis lead screw 21. The moving platform 1 includes a moving platform body 10 connected to the upper Hooke hinge 252 of the moving platform Hooke hinge 30 in the parallel branch 2. The moving platform body has guide rail sliders 16 on the upper and lower sides, and slider adapters 13 are mounted on the guide rail sliders 16.

[0072] Specifically, the slider adapter 13 is equipped with a mechanical limit block 15, which restricts movement between the mechanical limit brackets 14 connected to the moving platform body 10. The slider adapter 13 is restricted to move within the range of the photoelectric switch 11. The photoelectric switch 11 is connected to the moving platform body 10. A tooling connection plate 12 is connected to the slider adapter 13 for placing different tooling, thereby driving the tooling to move on the guide rail slider 16. A seventh axis assembly 17 is connected in the middle of the slider adapter 13. The seventh axis assembly 17 includes a first servo motor 18, which is connected to the seventh axis lead screw 21. The seventh axis lead screw 21 is fixed on both sides to the lead screw fixed end 19 and the lead screw moving end 22, respectively, and drives the lead screw nut 20. The first servo motor 18 drives the seventh axis lead screw 21 to rotate between the fixed end 19 and the moving end 22 of the lead screw. The seventh axis lead screw 21 and the lead screw nut 20 are in a helical fit. The lead screw nut 20 moves on the seventh axis lead screw 21, and the lead screw nut 20 drives the slider adapter 13 to move on the moving platform body 10.

[0073] like Figure 3 , Figure 4 As shown, the six stationary platforms Hooke hinges 25 on the AGV trolley 3 are divided into three groups in the following manner:

[0074] The first group of two stationary platform Hooke hinges 25 and the second group of two stationary platform Hooke hinges 25 are both arranged inward at a certain angle to the long side of the AGV trolley 3, and the two groups are symmetrically distributed along a plane passing through the Y-axis and perpendicular to the AGV trolley 3; the third group of two stationary platform Hooke hinges 25 are arranged along the short side of the upper surface of the AGV trolley 3, and the short side of the lower Hooke hinge 251 is parallel to the short side of the upper surface of the AGV trolley 3. In the first and second groups of stationary platform Hooke hinges 25, the center points of the bottom surfaces of the two stationary platform Hooke hinges 25 in each group are collinear, and the connecting line is parallel to the short side of the stationary platform Hooke hinge 25 in this group; in the third group of stationary platform Hooke hinges 25, the center points of the bottom surfaces of the two stationary platform Hooke hinges 25 are collinear, and the connecting line is parallel to the short side of the AGV trolley 3. Figure 3As shown, the extensions of the lines connecting the center points of the bottom surfaces of the static platform Hooke hinges 25 intersect each other, forming an isosceles triangle structure and constituting an inwardly symmetrical arrangement. Correspondingly, the six moving platform Hooke hinges 30 on the moving platform 1 are also paired up, forming three groups of moving platform Hooke hinges: the first group of two moving platform Hooke hinges 30 and the second group of two moving platform Hooke hinges 30 are both arranged inward at a certain angle to the long side of the moving platform 1, and the two groups are symmetrically distributed along a plane passing through the Y-axis and perpendicular to the moving platform 1; the third group of two moving platform Hooke hinges 30 are arranged along the short side of the lower surface of the moving platform 1, and the short side of the lower Hooke hinge 251 is parallel to the short side of the lower surface of the moving platform 1. In the first and second groups of moving platform Hooke hinges 30, the center points of the bottom surfaces of the two moving platform Hooke hinges 30 in each group are collinear, and the connecting line is parallel to the short side of the moving platform Hooke hinge 30 in that group; in the third group of moving platform Hooke hinges 30, the center points of the bottom surfaces of the two moving platform Hooke hinges 30 are collinear, and the connecting line is parallel to the short side of moving platform 1. For example... Figure 4 As shown, the extensions of the lines connecting the center points of the bottom surfaces of the Hooke hinges 30 on each group of moving platforms intersect in pairs, forming an isosceles triangle structure and constituting an inwardly symmetrical arrangement. The six parallel branches 2 are grouped in pairs, corresponding to the Hooke hinges connecting the static and moving platforms, presenting an overall arrangement of three groups of isosceles triangle supports.

[0075] Furthermore, such as Figure 2 As shown, the network main control layer 4 includes a function selection module 45, a parameter preset module 41, a mechanism kinematics module 46, a PLC main control module 42, and a motion control algorithm module 47. The function selection module 45 is connected to the parameter preset module 41 and the mechanism kinematics module 46. The parameter preset module 41 is connected to the PLC main control module 42. The PLC main control module 42 can realize the communication connection configuration and data interaction of the system hardware, as well as the real-time control of the hardware. The mechanism kinematics module 46 is connected to the motion control algorithm module 47.

[0076] Specifically, the network main control layer 4 also includes a PLC main control system 43 that implements PLC hardware self-testing and power-on enable. The PLC main control module 42 is connected to the parameter preset module 41. The PLC main control module 42 implements communication connection configuration, data interaction, and power-on enable for the system hardware, thereby enabling real-time hardware control. The parameter preset module in the network main control layer 4 is connected to the function selection module 45. The function selection module 45 is connected to the mechanism kinematics module 46, and the mechanism kinematics module 46 is connected to the motion control algorithm module. The PLC main control module 42 includes the PLC main control system 43 used to build the entire hardware control system and the power-on enable module 44 for the entire machine. The PLC main control system 43 is connected to the enable module 44, the parameter setting module, and the user management module, implementing communication connection configuration, data interaction, and real-time hardware control. The parameter preset module 41 is used to change relevant setting parameters and access related permissions, including parameters such as running speed, acceleration / deceleration distance, compliance speed, and parameters preset by different users. The function selection module 45 includes four main functions: constant position jogging, constant velocity jogging, human-machine interaction, and spatial positioning. After completing the function selection module 45, the system proceeds to the mechanism kinematics module 46. The mechanism kinematics module 46 includes a forward kinematics module for real-time feedback of the robot's trajectory and an inverse kinematics module for controlling the robot's motion. The mechanism kinematics module 46 sends commands to the motion control algorithm module 47. The PLC main control system 43 is connected to the motion control algorithm module 47. The motion control algorithm module 47 includes a trajectory control algorithm module based on fifth-order polynomial acceleration and deceleration and a compliant control algorithm based on admittance control, such as... Figure 7 As shown. The network master control layer 4 includes a rugged industrial panel, which has a function selection area covering functions such as constant position jogging, constant speed jogging, human-machine interaction, and spatial positioning. The rugged industrial panel is an EtherCAT master station and is equipped with the Huichuan Autostudio automation control software platform. The master-slave drive control layer 5 consists of EtherCAT slave stations.

[0077] Furthermore, such as Figure 2As shown, the master-slave control layer 5 includes a communication coupling module 53, a motor drive module 52, an input / output module 54, a multi-source sensing module 55, and a sensor module 56. The communication coupling module 53 is connected to the input / output module 54 and can perform data interaction. The input / output module 54 can connect external input / output variables to the EtherCAT fieldbus network. The input / output module 54 is connected to the multi-source sensing module 55, the sensor module 56, the motor drive module 52, and the control panel module 51 of the master-slave control layer 5. The motor drive module 52 is connected to the execution component layer 6 and can realize high-precision positioning motion of the servo motor. Its input port receives and parses relevant instructions for external motor control, and its output sends control signals to the corresponding motor to accurately control the motor motion. The multi-source sensing module 55 includes a position information feedback module and an interactive force sensing module. The position information feedback module and the interactive force sensing module are used to acquire robot motion position information and force / torque information during human-machine collaboration. Its output transmits relevant information to the input / output module to provide data support for control decisions.

[0078] like Figure 2 As shown, the execution component layer 6 includes a driver module 64 and a third servo motor 63; the third servo motor 63 can receive control information from the driver module 64 and execute it; the third servo motor 63 is connected to the servo motor module 61 of the execution component layer 6, and the servo motor module 61 is connected to the parallel assembly robot 62 and can drive the assembly robot 62.

[0079] The execution component layer 6 includes seven drivers and seven motors. The motors receive control information from the drivers and execute it. The motors are connected to the servo motor module 61, which is connected to the assembly robot, namely, the mobile parallel attitude adjustment and positioning composite robot, to realize the motion control of the robot.

[0080] The motor drive module 52 is used for high-precision positioning of the servo motor. The input port of the motor drive module 52 is used to receive and parse relevant instructions for external control of the motor. The output port of the motor drive module 52 is used to send control signals to the corresponding driver module 64.

[0081] Specifically, the driver module 64 includes seven servo motor drivers, namely six parallel-chain servo motors and servo motors mounted on the motion platform.

[0082] The control panel module 51 receives operator commands, including self-locking / self-resetting buttons, two-speed / three-speed knobs, emergency stop, charging, and network debugging serial port. The communication coupling module 53 is the core connecting component for data interaction between the master-slave control layer, the network master control layer, and the execution component layer 6. The network master control layer 4 and the master-slave control layer 5 establish a communication system via a TCP / IP LAN to achieve data transmission and command interaction. Specifically, the communication coupling module 53 performs data relay and protocol conversion functions. It receives control commands transmitted by the network master control layer 4 via the TCP / IP LAN, such as motion mode switching and target pose parameters, and converts them into signals conforming to the EtherCAT fieldbus protocol, transmitting them to the input / output module 54 of the master-slave control layer 5. Simultaneously, it converts data collected by the input / output module 54 from the execution component layer 6, such as motor operating status and sensor detection values, into a format recognizable by the network master control layer 4, achieving bidirectional data transmission across protocols and ensuring communication compatibility between the three layers. The input / output module 54 connects external input / output variables to the EtherCAT fieldbus network. Specifically, the input / output module 54 includes analog and digital input / output modules. The sensor module 56 acquires force / torque information from force sensors installed in the parallel branch 2, as well as position information from IMUs, PGVs, etc., installed around the vehicle body. The output of the sensor module 56 transmits information to the input / output module 54 and the multi-source sensing module 55. The multi-source sensing module 55 is a key module for the sensor module and the actuator layer 6 to acquire information about the robot's operating status and working environment. The multi-source sensing module 55 performs multi-dimensional information acquisition. The position information feedback module, through encoders, acquires the extension and retraction of the parallel branch helical joint and the Hooke hinge angle in real time, and obtains the spatial position and attitude of the moving platform based on the forward kinematics module 46. The interactive force sensing module, using the force sensor base plate 26 at the branch end, captures force and torque information between the moving platform and the external environment based on the inverse dynamics model and the Jacobian matrix, providing data for compliant control and achieving comprehensive perception.

[0083] Servo motor module 61 connects to parallel branch 2, and has a built-in encoder to monitor the rotation angle and speed, feeding back to driver module 64. It can adjust torque and speed according to the signal from driver module 64, driving the lead screw to extend and retract, providing power support for assembly robot 62 to achieve platform posture adjustment. Third servo motor 63 powers servo motor module 61, receiving pulse signals from driver module 64 and generating rotational torque to drive its operation. Power and torque can be adjusted according to load to ensure smooth movement. Driver module 64 connects master-slave control layer 5 and execution component layer 6, receiving and parsing control commands, and adjusting the signal output to third servo motor 63 based on feedback information from servo motor module 61. It also has overcurrent protection functions to ensure safe operation. Assembly robot 62 is a six-degree-of-freedom parallel robot, specifically composed of moving platform 1, AGV trolley 3, and parallel branch 2, as shown below. Figure 1 As shown.

[0084] like Figure 12 As shown, the present invention provides a control method for a high-load attitude-adjusting docking mobile parallel composite robot, comprising the following steps:

[0085] Step (i) The system starts up and completes the initialization self-test. The operator selects the control mode through the function selection module 45 and inputs specific control commands through the control panel module 51.

[0086] Step (ii): After receiving the instruction, the network master control layer 4, in conjunction with the selected control mode, calls the corresponding control algorithm to calculate the target motion information of the end-effector platform.

[0087] Step (iii): The rod length of each branch 2 is obtained through the inverse kinematics control algorithm, converted into the target value of the servo motor encoder code disk and transmitted to the master-slave drive control layer 5;

[0088] Step (iv): The motor drive module receives the target value and sends a control signal to the motor according to the current control mode to drive the moving platform 1 to move.

[0089] Step (v): During the movement, the multi-source sensing module 55 provides real-time feedback of data from the interactive force sensing module and the position information feedback module. After the operator moves the moving platform 1 to the vicinity of the target position, they switch to the fine-tuning mode to precisely adjust the pose until the task is completed.

[0090] Step (vi): During the operation, if the monitoring data of the network master control layer 4 is abnormal, the operator can press the emergency stop button to stop the robot immediately.

[0091] Step (vii): After the task is completed, the operator issues a reset command, the robot returns to its initial position, and the system records the task data.

[0092] Furthermore, in step (iii), the inverse kinematics control algorithm includes the following steps:

[0093] Step (a1): Based on the preset high-load docking operation trajectory planning, the desired target pose of the end-effector dynamic platform in the static platform base coordinate system is obtained in real time.

[0094] Step (a2): Based on the closed-loop vector method and spatial geometric transformation theory, establish the ideal position inverse kinematic model of the parallel robot. Substitute the desired target pose from step (a1) into the model to calculate the theoretical target extension length of the six parallel branches under the error-free ideal state.

[0095] Step (a3) ​​involves establishing a physical mapping model based on the mechanical transmission parameters of the parallel robot, and directly converting the theoretical target extension length of each branch calculated in step (a2) into the theoretical target drive pulse number of the corresponding underlying servo motor.

[0096] In step (a4), the motion control algorithm module uses the theoretical target driving pulse number obtained in step (a3) ​​as an absolute position command and sends it to the servo drivers of each branch in the execution component layer in real time, driving the motors of each branch to move in coordination, and controlling the end effector to accurately reach the desired target pose.

[0097] Specifically, in step (iii) above, the establishment of the inverse kinematic model includes the following process:

[0098] Figure 10 As shown, it is 6-U H Schematic diagram of the coordinate system for the RU parallel mechanism. This parallel mechanism is relative to the plane... Symmetry. The hinge points of the Hooke's hinge of the i-th branch with the mounting surface and the stationary plane are defined as points. and points The distance between the two sides is It will be from the point Zhang Cheng's plane is defined as a static plane, which is also parallel to the upper surface of the AGV. The center point of this plane is defined as point. O。 Taking the direction of the normal vector of this plane as the z-axis, we can establish... A static coordinate system. The x-axis direction originates from the origin. O point to The midpoint is used to determine the y-axis direction using the right-hand rule. Similarly, the point... and points Let represent the hinge points of the Hooke's hinge of the i-th branch with the mounting surface and the moving platform, respectively. The distance between the two surfaces is . It will be from the point Zhang Cheng's surface is defined as a moving plane, and the center of this surface is defined as a point. P Taking the direction of the normal vector of this plane as... The axis can be used to establish a moving coordinate system. .in The direction of the axis is from point P to The midpoints are in the same direction, so we can use the right-hand rule to determine them. Axial direction. Point This indicates the center position of the helical pair.

[0099] Figure 11 This is a top view to demonstrate the geometric distribution of Hooke's hinge points on the surface of a parallel mechanism. yes The projection onto the static plane. In a Hooke's hinge, the axis of rotation closer to the static platform is parallel to the axis of rotation closer to the moving platform. The angle between this axis of rotation and the x-axis is defined as... .in and The values ​​are equal. Point With point Representing line segments respectively and The midpoint of the line segment. and The length is denoted as and Midpoint Located on the x-axis, its x-coordinate is Surface I is composed of straight lines. and Zhang Cheng, plane II is composed of straight lines and z The axis is stretched. The included angle between the two surfaces is defined as... When the projection point Online segment The inner side is considered positive. (Face) From a straight line and Zhang Cheng. Noodles From a straight line and z The axes are stretched. The included angle is defined as... When the projection point Located on line segment Take a positive value when it is inside.

[0100] ;

[0101] Specifically, in step (a2) above, based on the finite instantaneous spinor theory and the principle of virtual work, an inverse dynamics model of the parallel robot is established to obtain the branch extension length. The expression for this is given. Therefore, when the position vector of point P in the static coordinate system is given... Then the branch stretch length of the i-th branch is:

[0102] ;

[0103] in, This indicates the active sub-position of the i-th branch. This represents the coordinates of the Hooke's hinge connected to the moving platform in the static frame. It is a position vector. Indicates the attitude angle The rotation matrix from the local coordinate system to the base coordinate system of the driven platform is determined. This represents the position vector of the Hooke hinge connected to the moving platform in the moving frame. It is the position vector of the Hooke's hinge connected to the stationary frame. This represents the position vector of the origin of the moving coordinate system in the static coordinate system.

[0104] Transformation matrix The expression is as follows:

[0105] ;

[0106] In the formula and They represent and .horn These represent the lower axis of the moving system. about the stationary axis The rotation angle. Therefore, the joint position and unit vector, and the position and unit direction vector of the i-th branch active joint are obtained. Represented as:

[0107] ;

[0108] In the formula, This indicates the active sub-position of the i-th branch. This represents the direction vector of the active secondary. This represents the position vector of the origin of the moving coordinate system in the static coordinate system. Indicates the attitude angle The rotation matrix from the local coordinate system to the base coordinate system of the driven platform is determined. This represents the position vector of the Hooke hinge connected to the moving platform in the moving frame. It is the position vector of the Hooke's hinge connected to the stationary frame. It is a position vector.

[0109] The unit direction vector corresponding to the branch and the driving force of the active joint can be obtained from the closed-loop vectors described above in the static coordinate system at any pose. The rotation angle of each joint... It can be obtained through the mutual transformation between dynamic and static coordinate systems and joint coordinate systems. The 6-U coordinate system under study can be established using the finite screw method. HThe mapping relationship between the joint movements of the RU robot and the posture of the mobile platform is then a branch. unit direction vector It can also be expressed as:

[0110] ;

[0111] In the formula, Indicates the attitude angle The rotation matrix from the local coordinate system to the base coordinate system of the driven platform is determined. Let represent the rotation matrix of the first revolute joint coordinate system relative to the moving coordinate system in the Hooke joint decomposition. Let represent the rotation matrix of the second revolute joint coordinate system relative to the first revolute joint coordinate system in the Hooke joint decomposition. Let represent the rotation matrix of the helical joint coordinate system relative to the second revolute joint coordinate system of the Hooke joint decomposition. This represents the unit direction vector corresponding to the branch axis in its joint coordinate system.

[0112] Furthermore, in step (v), the interactive force sensing module in the multi-source sensing module includes the following steps:

[0113] Step (b1) establishes an inverse dynamics model of the parallel robot based on the finite instantaneous spinor theory and the principle of virtual work, in order to predict the theoretical driving force in the case of no interaction force.

[0114] Specifically, the established inverse dynamics model includes: establishing the robot's position inverse solution model based on the closed-loop vector method; establishing the robot's velocity mapping model, force mapping model, and acceleration model based on the finite instantaneous screw theory; and establishing rigid body dynamics equations based on the principle of virtual work to obtain the theoretical driving force expressions for each drive joint.

[0115] Based on the finite instantaneous spinor theory and the principle of virtual work, an inverse dynamics model of a parallel robot is established to obtain the branch driving force. The expression is:

[0116] ;

[0117] In the formula: The Jacobian matrix represents the velocity mapping between the drive joints and moving platform of a parallel assembly robot. and Let represent the transformation matrices of the centroids of the lower and upper links in the i-th branch, respectively. It represents the angular velocity of the j-th joint in the i-th branch rotating about the corresponding instantaneous motion axis. It represents the unit instantaneous kinetic spin of the j-th joint in the i-th branch. This represents the coordinate transformation matrix from point O to the center of mass of the moving platform. and Let represent the inertial force and gravitational helical force acting at the center of mass of the lower link in the i-th branch, respectively. and These represent the inertial force, gravity, and external helical force acting at the center of mass of the moving platform, respectively.

[0118] Step (b2) compensates for the inverse dynamics model in step (b1) and establishes a friction force model.

[0119] Specifically, in step (b2), considering the effects of speed and load changes, a speed-load-friction model is established:

[0120] ;

[0121] In the formula, , This indicates the linear velocity of the driving joint. Indicates the first i The linear velocity of P in a branch chain. This represents the Coulomb friction coefficient. This represents the static friction coefficient. This represents the coefficient of viscous friction. This represents the offset from the origin. Stribeck velocity represents the characteristic velocity that determines the transition of friction from static friction to Coulomb friction. The smaller the size, the steeper the transition. This represents a normalized velocity, used to determine which stage of the Stribeck effect the current velocity is in. The smaller the ratio, the greater the effect of static friction; The larger the ratio, the closer the system is to pure coulombic + viscous friction.

[0122] The frictional force related to the external load can be expressed as:

[0123] ;

[0124] In the formula, This indicates the magnitude of the external load force. and These represent two parameters of the friction force model related to the load.

[0125] Step (b3) involves identifying the parameters of the friction model in step (b2), and then using the identified friction model to compensate the inverse dynamics model in step (b1) to obtain the compensated theoretical driving force.

[0126] Specifically, all unknown parameters in the velocity-load-friction model established in step (b2) were identified experimentally. The identified friction model was then used to compensate for the inverse dynamics model in step (b1). The corrected expression for the branch driving force is as follows:

[0127] ;

[0128] In the formula, This represents the frictional force of each branch of a parallel assembly robot.

[0129] Step (b4): Based on the compensated inverse dynamics model and motor current feedback, an external force observer is constructed to estimate the external interaction force at the end in real time.

[0130] Specifically, step (b4) involves real-time acquisition of the actual driving force of each drive joint. Based on the difference between the actual driving force and the compensated theoretical driving force, an external force observer is constructed as follows:

[0131] ;

[0132] In the formula, The external interaction force is at the center point of the moving platform plane. P The equivalent force / torque at a point in the moving system is expressed as follows. This represents the external force at the origin in the static coordinate system. This indicates the actual driving force of each branch. This represents the theoretical driving force of each branch drive joint P of a parallel assembly robot.

[0133] Before collecting the actual driving force of each drive joint in step (b4), the following steps are also included: using a filtering algorithm to filter and reduce the noise of the current feedback signal of the force sensor, and using the filtered signal as the basis for calculating the actual driving force.

[0134] The interactive force perception module algorithm is implemented using the following process:

[0135] UR H The finite motion of the U-branch can be obtained by synthesizing the finite motions of each single-degree-of-freedom joint. Based on the description of finite instantaneous spinors and the algorithm side, the first... i UR H The finite motion of the U-branch can be described as follows:

[0136] ;

[0137] In the formula, Indicates the initial pose, from the static frame To the dynamic system The finite spinor of motion, symbolized as " "" represents the trigonometric product operation of finite spinors, which is a custom spinor system recursion / combination / intersection operator used to synthesize the force spinor system contributed by each joint along the branch from the end to the base, and finally obtain the force spinor system of the entire branch.

[0138] In the initial pose, the axes of the dynamic and static coordinate systems are parallel and have no rotation angle, so it can be represented as:

[0139] ;

[0140] In the formula, and These represent the linear displacement from the origin of the moving system to the origin of the stationary system and the unit direction vector of the finite motion axis, respectively, under the initial pose. Indicates the first The first branch The finite rotation of motion of a joint from its initial pose to its end pose can be expressed as:

[0141] ;

[0142] In the formula, Indicates the first The first branch The angular displacement of a joint about its axis from its initial pose to its desired pose. and They represent the first The first branch The unit direction vector and position vector of the finite motion axes of a joint; , This is the lead of the H sub-pair.

[0143] The instantaneous spinor of the robot's branches and the velocity model can be obtained:

[0144] ;

[0145] In the formula, Indicates the moving speed of the platform. Indicates the first i UR H The unit instantaneous spinor of the j-th kinematic pair in the U-branch; denoted by ω, t represents the magnitude of the angular velocity about the corresponding instantaneous motion axis, t represents the end effector of the parallel robot, i represents the i-th branch of the six branches in the parallel robot, and j represents the j-th instantaneous motion axis in the branch.

[0146] Due to a single UR H U-branch is a fully free and unconstrained branch, therefore each branch has no constraint force spinor, only driving force spinor. According to the finite spinor theory, the spinor of this driving force can be obtained from the anti-rotation of other passive joints after locking the active joint. Therefore, this force spinor passes through the point... And parallel to Then the spin of the unit driving force of each branch It can be represented as:

[0147] ;

[0148] in Let H be the unit vector of the sub-axis.

[0149] Taking advantage of the fact that the reciprocal product of the driving force spinor and the instantaneous spinor of the passive joint is zero, the mapping relationship between the generalized velocity of the end-effector reference point and the velocity of the active joint can be established as follows:

[0150] ;

[0151] After simplification, we get:

[0152] ;

[0153] In the formula:

[0154] ;

[0155] like If the rank is full, the above formula can be written as:

[0156] ;

[0157] Therefore, the speed mapping model of the parallel mechanism can be obtained. Let be the Jacobian matrix of the institution. This indicates the speed of the driving joint of the branch. This represents the driving joint velocity of the i-th branch.

[0158] The terminal acceleration model can be obtained by differentiating the velocity model:

[0159] ;

[0160] In the formula, This represents the acceleration of all driven joints. This represents the velocity vector of the driving joint. The driving Jacobian matrix of the system is represented, and the mapping relationship between the driving joint acceleration and the acceleration is established. It is obtained by combining the unit driving force spinor at each joint and the Coriolis term of the acceleration of each branch through reciprocity product.

[0161] This allows us to obtain the mapping relationship between the driving joint acceleration and the acceleration of each joint in the branch:

[0162] ;

[0163] In the formula, The mapping matrix represents the i-th branch. Let the inertia matrix of the i-th branch be represented. The velocity Jacobian matrix from the driving joint velocity to the end effector velocity.

[0164] Then the rotational acceleration of each component in the branch at its center of mass can be expressed as:

[0165] ;

[0166] In the formula, Let U represent the four components in the i-th branch of the robot: the U-shaped sub-center cross shaft, the AH link, the HB link, and another U-shaped sub-center cross shaft. , , represent the transformation matrices of the centroid velocities of each component in the i-th branch. Let represent the accelerations at the centroids of the 1st, 2nd, and 3rd joints in the i-th branch, respectively. The front of the Hessian matrix Hi Layer matrix module.

[0167] According to the principle of virtual work, if a robot is in equilibrium under the action of external forces, then the work done by any force with virtual velocity must be zero. That is, the work done by gravity, inertial force, driving force, and external forces is zero.

[0168] ;

[0169] In the formula, This represents the driving force in joint theory. This represents the resultant force acting on the Kth component of the i-th branch. This represents the inertial force acting on the Kth component of the i-th branch. This represents the gravitational force acting on the Kth component of the i-th branch. Let represent the virtual work displacement spindle experienced by the i-th branch moving platform. Rotation of inertial force on moving platform. Gravity rotation of the moving platform. The total displacement rotation of the moving platform. External force spinor. δ represents the total number of rigid bodies in the i-th branch; δ acts on a certain motion quantity (such as position or velocity spinor) and represents taking a "variation" on that quantity, that is, changing the configuration of the system infinitesimally within the constraints. This represents the transpose of the driving joint velocity vector.

[0170] Therefore, the theoretical driving force of the joint can be obtained:

[0171] ;

[0172] In the formula, This represents the force transformation matrix between point O and point P. These respectively represent the action on the component Gravitational spin on the moving platform; These respectively represent the action on the component The inertial spinor on the moving platform.

[0173] get:

[0174] ;

[0175] In the formula, This represents the estimated external force at reference point P.

[0176] Furthermore, in step (v), the location information feedback module in the multi-source sensing module includes the following steps:

[0177] Step (c1): Based on the feedback signal of the absolute encoder of the underlying servo motor, establish a joint space physical mapping model and obtain the actual extension length of each parallel branch in real time.

[0178] Specifically, in step (c1), a joint space physical mapping model is established to obtain the actual extension length of the i-th branch. The expression is:

[0179] ;

[0180] In the formula, This represents the initial zero-position length of the i-th branch after calibration. This represents the real-time pulse count fed back by the absolute encoder of the i-th branch servo motor at the current moment. This indicates the number of pulses corresponding to the initial zero position. This indicates the single-turn resolution of the servo motor encoder. This indicates the reduction ratio of the branch mechanical transmission system. This indicates the lead of the lead screw inside the branch chain.

[0181] Step (c2) establishes the position inverse constraint equation of the parallel robot based on the closed-loop vector method and spatial geometric constraints, and constructs a nonlinear objective function for solving the forward kinematics by combining the actual extension length in step (c1).

[0182] Specifically, in step (c2), the inverse position constraint equation is established based on the closed-loop vector method, and the relationship model between the position vector of the center point of the moving platform and the vectors of each branch is established, i.e., the nonlinear objective function matrix. The i-th component The expression is:

[0183] ;

[0184] In the formula, The three-dimensional position and Euler angle attitude variables of the moving platform in the base coordinate system of the static platform. This represents the position vector of the center point of the moving platform in the base coordinate system. Indicates the attitude angle The rotation matrix from the local coordinate system to the base coordinate system of the driven platform is determined. Let represent the position vector of the Hooke hinge on the i-th branch in the local coordinate system of the moving platform. Let represent the position vector of the Hooke hinge under the i-th branch in the static platform base coordinate system. Let be the actual stretch length of the i-th branch obtained in step i at the current moment.

[0185] Before establishing the nonlinear objective function matrix in step (c2), the following steps are also included: using a laser tracker to perform kinematic calibration on the actual structural parameters of the heavy-load parallel robot, and obtaining and updating the true position vectors of the upper and lower Hooke hinges of each branch. , and the actual initial zero length The calibrated and compensated structural parameters are substituted into the objective function to eliminate the influence of machining and assembly errors on the accuracy of the forward position calculation.

[0186] Specifically, the location feedback module algorithm implementation includes the following steps:

[0187] First, let's set For the equation The approximate solution, in the approximate solution Taylor expansion function We can obtain the following equation:

[0188] ;

[0189] Since terms of second order and above are very small, they can be ignored, and therefore can be written as:

[0190] ;

[0191] Will Substituting into the above equation, the solution can be written as:

[0192] ;

[0193] The above formula can be used as an equation. Newton's iterative formula;

[0194] Then, let's assume For Newton's iterative true solution x *'s neighborhood:

[0195] ;

[0196] In the formula, X * The asterisk (*) represents the true value. It is a common mathematical notation that indicates the "true value," "ideal value," or "optimal value." This represents the neighborhood radius (a small positive real number), indicating the allowable error range. x* neighborhood Within, for a given initial value x The method involves numerical iteration to obtain the true solution.

[0197] Furthermore, during the solution process, the lengths of each branch of the parallel mechanism are known. The goal is to obtain the position of the end effector of the moving platform. The following transformation can be obtained:

[0198] ;

[0199] In the formula, , This represents the spatial coordinates of each hinge point in the actual installation of the robot. This represents the position vector of the center coordinates of the moving platform in the static coordinate system.

[0200] Because the drive motor can only read the actual amount of movement of the branch. It is necessary to use the inverse kinematics module to calculate the rotation angle of the branched helical pair around the axis under the corresponding pose from the iterative pose calculation. And obtain the displacement deviation it produces. Therefore, the relationship between the length of each branch and the pose of the end-effector can be obtained:

[0201] ;

[0202] Each branch By forming a system of equations, we can obtain:

[0203] ;

[0204] Using iterative methods Solve for the initial solution of the positioning pose. To make the iteration more efficient, the initial pose of the parallel mechanism is used as the initial solution, and the following formula can be obtained:

[0205] ;

[0206] right Find the Jacobian matrix and obtain its partial derivative with respect to each pose variable:

[0207] ;

[0208] In the k In the next iteration, we can obtain:

[0209] ;

[0210] When the iterative process meets the iterative accuracy At that time, that is:

[0211] ;

[0212] but It can be used as an equation An approximate solution can be obtained. Therefore, given the driving quantities corresponding to the six branches, the end-effector pose of the moving platform can be obtained using the forward kinematics solution method.

[0213] In step (c3), for the nonlinear objective function constructed in step (c2), the pose of the previous control cycle is introduced as the initial value, and the Newton-Raphson iterative algorithm is used to solve it in real time to obtain the estimated spatial pose of the end effector platform in the base coordinate system.

[0214] Specifically, step (c3) uses the Newton-Raphson algorithm to solve for the nonlinear objective function matrix:

[0215] ;

[0216] The expression for its pose iteration update is:

[0217] ;

[0218] In the formula, This represents the pose update vector for the (k+1)th iteration. Let represent the pose estimation vector of the moving platform in the k-th iteration. Describe the objective function exist The analytic Jacobian matrix at a given location is defined by its matrix elements as follows: .

[0219] when The iteration terminates at time. The preset pose convergence accuracy threshold will be used to determine the convergence accuracy at the time of convergence. The output is used as a real-time spatial pose estimate of the moving platform.

[0220] Step (c4): Based on the spatial pose of the moving platform obtained in step (c3), and combined with the local motion conversion of the seventh axis, an extended Kalman filter (EKF) state observer is constructed to filter and fuse high-frequency noise, and output the absolute global pose of the end effector of the composite robot in real time.

[0221] This invention discloses a control method for a high-load attitude-adjusting docking parallel composite robot. The motion control algorithm module includes a joint space trajectory planning control algorithm and a compliant control algorithm. The joint space trajectory planning control algorithm is implemented based on a fifth-order polynomial acceleration and deceleration trajectory planning control algorithm. This algorithm can generate a smooth and continuous motion trajectory based on the target pose, motion velocity, and acceleration constraints of the robot's end effector platform. This effectively avoids impacts and vibrations during motion and improves the stability of the robot's motion. The compliant control algorithm is implemented based on the admittance control algorithm of the robot's end effector. This algorithm establishes a force-position relationship model between the robot's end effector platform and the environment, and converts the end effector contact force information collected by the force sensing module into position or velocity adjustment commands.

[0222] Specifically, the trajectory planning and control algorithm based on fifth-order polynomial acceleration and deceleration includes the following steps:

[0223] Step (d1): Based on the currently selected working mode, the control system receives the control command issued by the operator and obtains the key point pose information of the center point of the assembly robot's end-effector platform, which is expected to reach the target point, including position and attitude parameters.

[0224] Step (d2) uses the inverse kinematics control algorithm built into the PLC main control system module to solve the pose information of the key points in the end-effector operation space into the joint space path point information of the six motors one by one, ensuring that the motor operation is smooth and shock-free, laying the foundation for subsequent trajectory planning.

[0225] Step (d3): Each motor uses a fifth-order polynomial acceleration and deceleration trajectory planning algorithm to perform trajectory interpolation based on the corresponding joint space critical path point information, generating the position, velocity, and acceleration curves of the motor during this motion process, ensuring smooth acceleration and deceleration and avoiding sudden motion changes;

[0226] In step (d4), the servo motor operates in position control mode. The PLC main control system module sends position commands to each motor in real time according to its own clock cycle, until the end-effector moves to the desired target point.

[0227] With precise control commands issued through structural design, this mobile parallel posture adjustment robot can achieve a series of preset movements, such as six-degree-of-freedom posture adjustment and forward extension on the seventh axis. The final specific motion state is shown in Figure 9.

Claims

1. A control method for a high-load attitude-adjusting docking mobile parallel composite robot, characterized in that: Includes the following steps: Step (i) The system starts and completes the initialization self-test. The operator selects the control mode through the function selection module (45) and inputs specific control commands through the control panel module (51). Step (ii), after receiving the instruction, the network master control layer (4) combines the selected control mode and calls the corresponding control algorithm to calculate the target motion information of the end-effector platform; Step (iii): The rod length of each branch (2) is obtained through the inverse kinematics control algorithm, converted into the target value of the servo motor encoder disk, and transmitted to the master-slave drive control layer (5). Step (iv): The motor drive module receives the target value and sends a control signal to the motor according to the current control mode to drive the moving platform (1) to move. Step (v), during the movement, the multi-source sensing module (55) provides real-time feedback of data from the interactive force sensing module and the position information feedback module. After the operator moves the moving platform (1) to the vicinity of the target position, they switch to the fine-tuning mode to precisely adjust the pose until the task is completed. In step (v), the interactive force sensing module in the multi-source sensing module (55) includes the following steps: Step (b1): Based on the finite instantaneous spinor theory and the principle of virtual work, establish the inverse dynamics model of the parallel robot and predict the theoretical driving force in the case of no interaction force. Step (b2) compensates for the inverse dynamics model in step (b1) and establishes a friction force model; Step (b3) involves identifying the parameters of the friction model in step (b2), and using the identified error model to compensate the inverse dynamics model in step (b1) to obtain the compensated theoretical driving force. Step (b4): Based on the compensated inverse dynamics model and motor current feedback, an external force observer is constructed to estimate the external interaction force at the end in real time. Step (vi): During the movement, if the network master control layer (4) monitors abnormal data, the operator presses the emergency stop button to stop the robot immediately. Step (vii): After the task is completed, the operator issues a reset command, the robot returns to its initial position, and the system records the task data.

2. The control method for a high-load attitude-adjusting docking mobile parallel composite robot according to claim 1, characterized in that: In step (iii), the inverse kinematics control algorithm includes the following steps: Step (a1): Based on the preset high-load docking operation trajectory planning, the desired target pose of the end-effector dynamic platform in the static platform base coordinate system is obtained in real time; Step (a2): Based on the closed-loop vector method and spatial geometric transformation theory, establish the ideal position inverse kinematic model of the parallel robot. Substitute the desired target pose from step (a1) into the model to calculate the theoretical target extension length of the six parallel branches under the error-free ideal state. Step (a3): Combine the mechanical transmission parameters of the parallel robot to establish a physical mapping model, and directly convert the theoretical target extension length of each branch calculated in step (a2) into the theoretical target drive pulse number of the corresponding underlying servo motor. In step (a4), the motion control algorithm module uses the theoretical target driving pulse number obtained in step (a3) ​​as an absolute position command and sends it to the servo drivers of each branch in the execution component layer in real time, driving the motors of each branch to move in coordination, and controlling the end effector to accurately reach the desired target pose.

3. The control method for a high-load attitude-adjusting docking mobile parallel composite robot according to claim 1, characterized in that: In step (v), the location information feedback module in the multi-source sensing module (55) includes the following steps: Step (c1): Based on the feedback signal of the absolute encoder of the underlying servo motor, establish a joint space physical mapping model and obtain the actual extension length of each parallel branch in real time. Step (c2) establishes the position inverse constraint equation of the parallel robot based on the closed-loop vector method and spatial geometric constraints, and constructs a nonlinear objective function for solving the forward kinematics by combining the actual extension length in step (c1). Step (c3): For the nonlinear objective function constructed in step (c2), the pose of the previous control cycle is introduced as the initial value, and the Newton-Raphson iterative algorithm is used to solve it in real time to obtain the estimated spatial pose of the end effector in the base coordinate system. Step (c4): Based on the spatial pose of the moving platform obtained in step (c3), and combined with the local motion conversion of the seventh axis, an extended Kalman filter (EKF) state observer is constructed to filter and fuse high-frequency noise, and output the absolute global pose of the end effector of the composite robot in real time.

4. A control method for a high-load attitude-adjusting docking mobile parallel composite robot according to any one of claims 1-3, characterized in that: The high-load attitude adjustment docking mobile parallel composite robot mainly includes a moving platform (1), an AGV trolley (3), and a control system; Six parallel branches (2) are set between the moving platform (1) and the AGV car (3); the upper and lower ends of the branches (2) are respectively connected to the lower end face of the moving platform (1) and the upper surface of the AGV car (3) through Hooke joints; in the initial pose, the branches (2) are symmetrically arranged in the axis between the moving platform (1) and the AGV car (3); The branch (2) includes a moving platform Hooke hinge (30) hinged to the lower end face of the moving platform (1) and a stationary platform Hooke hinge (25) hinged to the upper surface of the AGV trolley (3); a telescopic mechanism is provided between the moving platform Hooke hinge (30) and the stationary platform Hooke hinge (25). The moving platform Hooke hinge (30) and the stationary platform Hooke hinge (25) have the same structure, both including a lower Hooke hinge (251) connected to the plate, and a movable upper Hooke hinge (252) is provided in the lower Hooke hinge (251). The two upper Hooke hinges (252) are connected to the telescopic mechanism. The telescopic mechanism includes a sleeve (28), a force sensor (27), a force sensor base plate (26), and a branch screw (29); the sleeve (28) is connected to the force sensor (27), the force sensor (27) is connected to the force sensor base plate (26), the force sensor base plate (26) is connected to the upper Hooke hinge (252) on the upper surface of the AGV trolley (3), the inner cavity of the sleeve (28) accommodates the branch screw (29), and the telescopic end of the branch screw (29) is connected to the upper Hooke hinge (252) at the lower end of the moving platform (1); The control system includes a network master control layer (4), a master-slave drive control layer (5), and an execution component layer (6). The network master control layer (4) and the master-slave drive control layer (5) are connected through a wireless local area network. The master-slave drive control layer (5) can receive and execute the control commands of the rugged industrial tablet in the network master control layer (4) and collect and feedback data information from the execution component layer. The master-slave drive control layer (5) is connected to the execution component layer (6) through communication. The execution component layer (6) can complete the overall motion control of the assembly robot (62). At the same time, the execution component layer (6) can feed back data information to the master-slave drive control layer (5).

5. The control method for a high-load attitude-adjusting docking mobile parallel composite robot according to claim 4, characterized in that: The network main control layer (4) includes a function selection module (45), a parameter preset module (41), a mechanism kinematics module (46), a PLC main control module (42), and a motion control algorithm module (47). The function selection module (45) is connected to the parameter preset module (41) and the mechanism kinematics module (46). The parameter preset module (41) is connected to the PLC main control module (42). The PLC main control module (42) can realize the communication connection configuration and data interaction of the system hardware, as well as the real-time control of the hardware. The mechanism kinematics module (46) is connected to the motion control algorithm module (47).

6. The control method for a high-load attitude-adjusting docking mobile parallel composite robot according to claim 4, characterized in that: The master-slave drive layer (5) includes a communication coupling module (53), a motor drive module (52), an input / output module (54), a multi-source sensing module (55), and a sensor module (56); The communication coupling module (53) and the input / output module (54) can interact with each other. The input / output module (54) can connect external input / output variables to the EtherCAT fieldbus network. The input / output module (54) is connected to the control panel module (51) of the multi-source sensing module (55), sensor module (56), motor drive module (52), and master-slave drive layer (5). The motor drive module (52) is connected to the execution component layer (6). It can realize high-precision positioning and movement of servo motor, receive and parse relevant instructions of external control motor, and send control signals to the corresponding motor.

7. The control method for a high-load attitude adjustment docking mobile parallel composite robot according to claim 4, characterized in that: The execution component layer (6) includes a driver module (64) and a third servo motor (63); the third servo motor (63) can receive control information from the driver module (64) and execute it; the third servo motor (63) is connected to the servo motor module (61) of the execution component layer (6), and the servo motor module (61) is connected to the parallel assembly robot (62) and can drive the assembly robot (62).

8. The control method for a high-load attitude adjustment docking mobile parallel composite robot according to claim 4, characterized in that: The motor drive module (52) is used for high-precision positioning of the servo motor. The input port of the motor drive module (52) is used to receive and parse relevant instructions for external control of the motor. The output port of the motor drive module (52) is used to send control signals to the corresponding driver module (64).

Images