Self-owned momentum target horizontal servo system control method and device and electronic equipment

Abstract

The invention discloses a self-owned momentum target horizontal follow-up system control method and device and electronic equipment. The control method comprises the steps that S1, a crown block and mechanical arm kinematics model is constructed in an off-line mode; s2, an optical motion capture system is adopted to measure the target position in real time, and the horizontal deviation of the target hung by the left and right double-shaft mechanical arms after the rope swings is obtained; s3, an orthogonal displacement measuring device of the magnetic railing ruler is installed on the upper end point of the suspension rope, and horizontal direction deviation speed measurement in the swing process of the suspension rope is continuously achieved; and S4, a follow-up device decoupling controller is designed, a decoupling control method is adopted, and independent control over the crown block and the mechanical arm is achieved according to the optimal parking point and the measured horizontal deviation and horizontal direction deviation speed. According to the invention, the synchronous measurement of the swinging position and speed of the target is realized by fusing the information of the motion capture system and the magnetic railing ruler, the independent control of the crown block and the mechanical arm is realized by adopting a decoupling control method, and the control complexity is reduced.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of target horizontal follow-up tracking, in particular, to a self-momentum target horizontal follow-up system control method and device and electronic equipment. BACKGROUND

[0002] Micro / low gravity is one of the most typical characteristics of outer space environment, and the force state and dynamic characteristics of a spacecraft or an astronaut are obviously different from those in the gravity environment on the earth, which has an important influence on the design and execution of a space mission. Simulating the micro / low gravity environment in space on the ground is very important for astronaut training and spacecraft testing, and can help astronauts or spacecraft to adapt to the space environment in advance. The suspension method is a commonly used method for simulating the micro / low gravity environment, which uses a pulley and a cable mechanism and utilizes the tension of the cable to offset the gravity of the target object. Among them, the horizontal follow-up system is one of the core subsystems of the micro / low gravity suspension device, and the accuracy thereof determines the micro / low gravity simulation effect.

[0003] The self-momentum target horizontal follow-up system requires very high dynamic response performance, and the existing control method needs to rely on a very accurate dynamic model to ensure the response speed of the control. In addition, the control system has very strong nonlinearity, and the traditional PID control method is not easy to debug the control parameters. When the target is replaced, the control parameters need to be reset, and the robustness is poor. In addition, the follow-up system is relatively complex and includes multiple driving periods. The use of a coupled control strategy has a large number of redundant degrees of freedom, and the motion control is difficult. The existing method has insufficient target horizontal follow-up tracking. SUMMARY

[0004] The present application provides a self-momentum target horizontal follow-up system control method to solve the technical problems of poor robustness and difficult motion control of the existing control method.

[0005] The technical scheme adopted by the present application is as follows:

[0006] A self-momentum target horizontal follow-up system control method, the horizontal follow-up system includes a crown block and two left and right double-shaft mechanical arms, the crown block can move horizontally, the two left and right double-shaft mechanical arms are rotationally arranged at both ends of a waist part, the two left and right double-shaft mechanical arms are provided with a cable hanging point at the end thereof and suspend a hanging object through a suspension cable, the waist part is installed at the center of a hoisting bin of the crown block, the two left and right double-shaft mechanical arms are horizontally rotated, and the method comprises the following steps:

[0007] S1, constructing an off-line kinematics model of the crown block and the mechanical arms;

[0008] S2, using an optical motion capture system to measure the target position in real time to obtain the horizontal deviation of the hanging target of the two left and right double-shaft mechanical arms after the cable swings;

[0009] S3, install the orthogonal displacement measuring device of the magnetic grating ruler to the upper end point of the suspension rope, continuously realize the horizontal deviation speed measurement in the suspension rope swing process;

[0010] S4, design a servo decoupling controller, adopt a decoupling control method, and realize independent control of the crown block and the mechanical arm according to the optimal parking point and the measured horizontal deviation and horizontal deviation speed.

[0011] Further, the step S1 specifically comprises the steps of:

[0012] S11, the waist is installed at the center of the hoist bin of the crown block, the center position of the crown block is represented as (x0, y0), and the joint angle of the mechanical arm is represented as θ ij , wherein the subscript i represents the left or right mechanical arm mark, wherein 1 represents right and 2 represents left, the subscript j represents the joint number corresponding to the mechanical arm, and the angle is positive when counterclockwise rotation; A1 and A2 respectively represent the first joints of the mechanical arms; B1 and B2 respectively represent the second joints of the mechanical arms; C1 and C2 respectively represent the upper end points of the suspension ropes at the end rope hanging points of the left and right mechanical arms, A1A2 represents the connecting line of the first joints of the two double-shaft mechanical arms, and the waist is located on the A1A2 straight line;

[0013] S12, when horizontally following, the horizontal coordinates of C1 and C2 are determined, which can be represented as:

[0014]

[0015] θ 21 = θ 11 + π

[0016] , wherein O represents the center point of the suspension device of the crown block, the waist of the mechanical arm, and the base positions of the left and right mechanical arms, L1, L2 and L3 represent the lengths of connecting rods OA1, A1B1 and B1C1, θ 11 represents the rotation joint angle of the waist, θ 12 represents the joint angle of the first joint of the right mechanical arm, θ 13 represents the joint angle of the second joint of the right mechanical arm, θ 21 represents the rotation angle of the waist, which is different from θ 12 by an angle of π, θ 22 represents the joint angle of the first joint of the left mechanical arm, θ 23 represents the joint angle of the second joint of the left mechanical arm, x0 represents the deviation of the crown block in the x direction, and y0 represents the deviation of the crown block in the y direction.

[0017] S13, construct the velocity Jacobian matrix of the two double-shaft mechanical arms, so that the system can adopt a velocity control strategy to improve the response speed, and the derivative of the above formula is simplified as follows:

[0018]

[0019] wherein,

[0020] a1 = -L1 sin θ 11 ,

[0021] b1 = -L2 sin(θ 11 + θ 12 ),

[0022] d1 = -L3 sin(θ 11 + θ 12 + θ 13 ),

[0023] a2 = L1 cos θ 11 ,

[0024] b2 = L2 cos(θ 11 + θ 12 ),

[0025] d2 = L3 sin(θ 11 + θ 12 + θ 13 ),

[0026] A1 = -L1 sin θ 21 ,

[0027] B1 = -L2 sin(θ 21 + θ 22 ),

[0028] D1 = -L3 sin(θ 21 + θ 22 + θ 23 ),

[0029] A2 = L1 cos θ 21 ,

[0030] B2 = L2 cos(θ 21 + θ 22 ),

[0031] D2 = L3 cos(θ 21 + θ 22 + θ 23 ).

[0032] k1, k2, k3, k4 are intermediate variables;

[0033] Mechanical arm joint angular velocity The Jacobian matrix can be expressed as:

[0034]

[0035] Further, the step S2 specifically comprises steps of:

[0036] S21, fixing a mark point on the endpoint of the mechanical arm, and fixing a second mark point above the mark point, at this time, the suspension ropes pass through the centers of the mark points, and the rope swing is determined by estimating the positions of the two mark points, and the horizontal deviations of the left and right double-shaft mechanical arms after the rope swing are measured by using an optical motion capture system

[0037] Further, the step S4 specifically comprises steps of:

[0038] S41, simplifying the control system, pre-designing a control criterion, and preferentially setting x0, y0, and θ 11 control law, and adding constraint equations;

[0039] S42, according to the pre-set control criterion, respectively calculating a crane expected motion speed to control the crane motion, a waist joint expected angular velocity control amount to control the waist rotation, and an angular velocity of each joint to control each joint controller to drive the mechanical arm motion.

[0040] Further, in the step S41, the control criterion comprises:

[0041] (1) the midpoint of the line segment C1C2 tends to the optimal parking point P of the mechanical arm body coordinate system, wherein the optimal parking point P is defined as being located in front of the waist of the mechanical arm, and the distance from the waist center is set as L, at this time, the left and right double-shaft mechanical arms are balanced in force, the two double-shaft mechanical arms and the crane motion adjustment meet the requirements, and adapt to the horizontal motion of the unloading target;

[0042] (2) the control strategy makes the straight line A1A2 and the straight line C1C2 meet the pre-set parallelism requirement;

[0043] (3) the crane and the mechanical arm adopt a decoupling control strategy to reduce the control complexity.

[0044] Further, the step S42 specifically comprises steps of:

[0045] S4201, the crane can realize large-range rapid motion, and the control target is to track the optimal parking point P, at the current time, the actual optimal parking point P tends to the midpoint of the line segment C1C2, and is represented as:

[0046]

[0047] S4202, the distance between the optimal parking point P and the waist is L, and the coordinates of the optimal parking point P can be represented as:

[0048] x p =x0+Lcos(θ11 + π / 2) = x0- L sin θ 11

[0049] y p + π / 2) = y0+ L cos θ 11 11 ;

[0050] S4203, the deviation of the optimal parking point P can be represented as Δx0, Δy0, as shown in the following formula:

[0051] 2Δx0= L2cos(θ 11 + θ 12 )+ L3cos(θ 11 + θ 12 + θ 13 )+ L2cos(θ 21 + θ 22 )+ L3cos(θ 21 + θ 22 + θ 23 )+ 2L sin θ 11

[0052] 2Δy0= L2sin(θ 11 + θ 12 )+ L3sin(θ 11 + θ 12 + θ 13 )+ L2sin(θ 21 + θ 22 )+ L3sin(θ 21 + θ 22 + θ 23 )- 2L cos θ 11 ;

[0053] S4204, the proportional controller is used to control the motion of the crown block to eliminate the above deviation, and the expected motion speed of the crown block is obtained:

[0054]

[0055] wherein, is a proportional control parameter, the control of the crown block is decoupled from the control of the robot arm, and the control target is to track the optimal parking point so that the parking point is always located in front of the waist of the crown block.

[0056] Further, the step S42 specifically further comprises the step of:

[0057] S4211, the waist rotation is controlled by using the criterion that the straight line A1A2 and the straight line C1C2 meet the preset parallel degree requirement, and in an ideal case, θ 11 ​Tend to zero, left and right two double-axis mechanical arm suspension rope hanging point C1 and C2 coordinates can be expressed as:

[0058]

[0059] S4212, using C1 and C2 coordinates to calculate the waist angle deviation is:

[0060]

[0061] S4213, using the proportion controller to get the waist joint expected angular velocity control quantity:

[0062]

[0063] For the control parameters of the waist joint.

[0064] Further, step S42 further comprises the following steps:

[0065] S4221, through the optical motion capture system and the orthogonal magnetic grating ruler, the mechanical arm end point C1 and C2 offset position And the speed of the mounting point offset motion By controlling the mechanical arm to follow the target motion, eliminating the offset and ensuring that the rope remains vertical, the difference equation of the error evolution of the control system is called error dynamics, the purpose of designing the controller is to make the error tend to zero with time, the dynamic change of the error can be described by a second-order difference equation, which can be analogized as a typical mass-spring-damper model, which can be specifically expressed as:

[0066]

[0067] Wherein is the natural frequency, is the damping ratio, when all roots of the second-order equation are negative real parts, the error will converge to zero with time, adjust The value ensures the stability of the system and adjusts the tracking performance of the mechanical arm;

[0068] S4222, determine the acceleration value of the mounting point offset adjustment according to the above formula, based on the control instruction of the last time, the end speed control instruction at the current time can be further obtained, expressed as:

[0069]

[0070] Wherein is the mechanical arm end control instruction at t-T time, T is the control period length, and Indicates the control instruction of the mechanical arm end at t time;

[0071] S4223、After obtaining the end speed control instruction, a speed Jacobian model of the crown block and the mechanical arm can be used to calculate and output these values to each joint controller to drive the movement of the mechanical arm.

[0072] Another aspect of the present application also provides a self-momentum target horizontal follow-up system control device, the horizontal follow-up system comprising a crown block and two left and right double-shaft mechanical arms, the crown block being horizontally movable, the two left and right double-shaft mechanical arms being rotatably arranged at both ends of a waist part, the two left and right double-shaft mechanical arms being provided with a rope hanging point at the end and suspending a hanging object through a suspension rope, the waist part being mounted to the center of a hoisting bin of the crown block, the two left and right double-shaft mechanical arms being horizontally rotatable, and comprising:

[0073] a kinematic model establishment module for offline constructing a kinematic model of the crown block and the mechanical arm;

[0074] a position measurement module for measuring the position of the target in real time by using an optical motion capture system to obtain the horizontal deviation of the hanging target of the two left and right double-shaft mechanical arms after the rope swings;

[0075] a deflection speed measurement module for mounting a quadrature displacement measurement device of a magnetic scale to the upper end point of the suspension rope to continuously measure the horizontal deflection speed during the swing of the suspension rope;

[0076] a decoupling control module for designing a follow-up device decoupling controller, using a decoupling control method, and realizing independent control of the crown block and the mechanical arm according to the optimal parking point and the measured horizontal deviation and horizontal deflection speed.

[0077] Another aspect of the present application also provides an electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the self-momentum target horizontal follow-up system control method when executing the computer program.

[0078] Another aspect of the present application also provides a storage medium comprising a stored program, wherein the program controls the device where the storage medium is located to implement the steps of the self-momentum target horizontal follow-up system control method when the program is executed.

[0079] The present application has the following beneficial effects:

[0080] The horizontal follow-up system targeted by the present application has redundancy in the degree of freedom in the horizontal direction, and there are many solutions by using traditional kinematics solving, therefore, the present application realizes synchronous measurement of target swing position and speed by fusing motion capture system and magnetic scale information, adopts decoupling control strategy, so that the control of the crown, waist and mechanical arm has certain independence, realizes independent control of the crown and mechanical arm, and can reduce the control complexity of the system. The present application learns from the operation mode of human arm, uses the concept of optimal parking point, so that the stress on both sides of the system left and right mechanical arms is more balanced, and the margin of mechanical arm and crown movement adjustment is larger. By integrating the position and speed decoupling measurement results, introducing second-order error dynamics, the requirement of high-precision dynamics modeling is avoided, and the dynamic performance of tracking is also ensured.

[0081] In addition to the objects, features and advantages described above, the present application has other objects, features and advantages. The present application will be further described below with reference to the drawings. BRIEF DESCRIPTION OF DRAWINGS

[0082] The accompanying drawings, which form a part of the present application, are included to provide a further understanding of the application, and are incorporated in and constitute a part of this specification, illustrate embodiments of the present application and serve to explain the principles of the present application. In the drawings:

[0083] Figure 1 It is a self-momentum target horizontal follow-up system control method flowchart of the preferred embodiment of the present application.

[0084] Figure 2 It is a structure principle schematic diagram of the self-momentum target horizontal follow-up system of the preferred embodiment of the present application.

[0085] Figure 3 It is a structure principle schematic diagram of the left and right two double-shaft mechanical arms of the preferred embodiment of the present application.

[0086] Figure 4 It is a motion model schematic diagram of the crown and mechanical arm system of the preferred embodiment of the present application.

[0087] Figure 5 It is a definition schematic diagram of the end optimal parking point P of the preferred embodiment of the present application.

[0088] Figure 6 It is a self-momentum target horizontal follow-up system control device module schematic diagram of the preferred embodiment of the present application.

[0089] Figure 7 It is an electronic equipment entity schematic diagram of the preferred embodiment of the present application.

[0090] Figure 8 It is an internal structure diagram of the computer equipment of the preferred embodiment of the present application.

[0091] As shown in the figure: 1, headstock; 2, hoist bin; 3, constant force assembly; 4, double-shaft mechanical arm; 5, mechanical arm device; 6, hanging object; 7, suspension rope; 8, crossbeam; 9, waist; 10, waist pivot; 11, first mechanical arm shaft; 12, upper end point of suspension rope; 13, second mechanical arm shaft; 14, mounting tray. DETAILED DESCRIPTION

[0092] The embodiments of the present application are described in detail below with reference to the accompanying drawings, but the present application can be implemented in various different ways as defined and covered by the following.

[0093] Reference Figures 1 to 3 The preferred embodiment of the present application provides a self-momentum target horizontal follow-up system control method, the horizontal follow-up system comprising a headstock 1 and a mechanical arm device 5, the mechanical arm device 5 comprising a waist 9 and two double-shaft mechanical arms 4 on the left and right, the double-shaft mechanical arm 4 comprising a first mechanical arm shaft 11, a second mechanical arm shaft 13, a mounting tray 14 on the second mechanical arm shaft 13, a constant force assembly 3 on the mounting tray 14, and an upper end point 12 of a suspension rope on the constant force assembly 3, the headstock 1 being horizontally movable, the two double-shaft mechanical arms 4 on the left and right being rotatably arranged at both ends of the waist 9, the two double-shaft mechanical arms 4 on the left and right having rope hanging points at the ends and suspending a hanging object 6 through a suspension rope 7, the waist 9 being mounted to the center of the hoist bin 2 of the headstock 1 through a waist pivot 10, the hoist bin 2 being horizontally movable along a crossbeam 8, and the two double-shaft mechanical arms 4 on the left and right being horizontally rotatable, comprising the steps of:

[0094] S1, constructing a kinematic model of the headstock and the mechanical arm offline;

[0095] S2, using an optical motion capture system to measure the target position in real time to obtain the horizontal deviation of the hanging target of the two double-shaft mechanical arms on the left and right after the rope swings;

[0096] S3, installing a quadrature displacement measuring device of a magnetic scale to the upper end point of the suspension rope to continuously measure the horizontal direction deviation speed in the swinging process of the suspension rope;

[0097] S4, designing a follow-up device decoupling controller, using a decoupling control method, and realizing independent control of the headstock and the mechanical arm according to the optimal parking point and the measured horizontal deviation and horizontal direction deviation speed.

[0098] This embodiment addresses a horizontal servo system with redundant degrees of freedom in the horizontal direction. Traditional kinematic solutions yield numerous results. Therefore, this application integrates information from the motion capture system and the magnetic scale to achieve synchronous measurement of the target's swing position and velocity. A decoupled control strategy is employed to give the crane, waist support, and robotic arm a degree of independence, enabling independent control of the crane and robotic arm and reducing system control complexity. This embodiment borrows from the operating mode of a human arm, using the concept of an optimal stopping point to achieve relatively balanced forces on both sides of the robotic arm, resulting in greater margin for motion adjustment for both the robotic arm and the crane. By integrating the decoupled position and velocity measurement results and introducing second-order error dynamics, the requirement for high-precision dynamic modeling is avoided, while ensuring the dynamic performance of the tracking.

[0099] Preferably, step S1 specifically includes the following steps:

[0100] S11. The waist section is installed at the center of the overhead crane's pod. The center position of the overhead crane is denoted as (x0, y0), and the joint angle of the robotic arm is denoted as θ. ij The subscript i represents the left or right robotic arm symbol, where 1 represents right and 2 represents left; the subscript j represents the joint number corresponding to the robotic arm, with the angle being positive when rotating counterclockwise; A1 and A2 represent the first joint of the robotic arm; B1 and B2 represent the second joint of the robotic arm; C1 and C2 represent the upper end point 12 of the suspension rope at the end rope attachment point of the left and right robotic arms; A1A2 represents the line connecting the first joints of the two dual-axis robotic arms, with the waist located on the straight line A1A2;

[0101] S12. During horizontal tracking, the horizontal coordinates of points C1 and C2 can be determined as follows:

[0102]

[0103] θ 21 =θ 11 +π

[0104] Among them, such as Figure 4 As shown, point O represents the center point of the overhead crane suspension device, the waist of the robotic arm, and also the position of the base of the left and right robotic arms. L1, L2, and L3 represent the lengths of connecting rods OA1, A1B1, and B1C1. θ 11 θ represents the rotational joint angle of the waist. 12 θ represents the joint angle of the first joint of the right robotic arm. 13 θ represents the joint angle of the second joint of the right robotic arm. 21 This represents the waist rotation angle, and is related to θ. 12 The phase difference is π, θ 22 θ represents the joint angle of the first joint of the left robotic arm. 23θ2 represents a joint angle of a second joint of the left robot arm, x0 represents a displacement of the crown in the x direction, and y0 represents a displacement of the crown in the y direction;

[0105] S13, construct two double-axis robot arm velocity Jacobian matrices, so that the system can adopt a velocity control strategy, improve the response speed, and after derivation of the above two sides, the following is obtained:

[0106]

[0107] wherein,

[0108] a1=-L1sinθ 11 ,

[0109] b1=-L2sin(θ 11 +θ 12 ),

[0110] d1=-L3sin(θ 11 +θ 12 +θ 13 ),

[0111] a2=L1cosθ 11 ,

[0112] b2=L2cos(θ 11 +θ 12 ),

[0113] d2=L3sin(θ 11 +θ 12 +θ 13 ),

[0114] A1=-L1sinθ 21 ,

[0115] B1=-L2sin(θ 21 +θ 22 ),

[0116] D1=-L3sin(θ 21 +θ 22 +θ 23 ),

[0117] A2=L1cosθ 21 ,

[0118] B2=L2cos(θ 21 +θ 22 ),

[0119] D2=L3cos(θ 21 +θ 22 +θ 23 ).

[0120] k1, k2, k3, k4 are intermediate variables;

[0121] Joint angular velocity of the mechanical arm The Jacobian matrix can be expressed as:

[0122]

[0123] Preferably, step S2 specifically comprises the steps of:

[0124] S21, fixing a mark point on the upper end of the mechanical arm, and fixing a second mark point above the mark point, at this time the suspension rope passes through the center of the mark point, determining the rope swing by estimating the positions of the two mark points, and measuring the horizontal deviation of the left and right double-shaft mechanical arm hanging targets after the rope swing by using an optical motion capture system

[0125] Preferably, step S4 specifically comprises the steps of:

[0126] S41, simplifying the control system, pre-designing the control criterion, and preferentially setting x0, y0, θ 11 control law, and adding constraint equations;

[0127] S42, according to the pre-set control criterion, respectively calculating the expected motion speed of the crown block to control the motion of the crown block, the expected angular velocity of the waist joint to control the waist rotation, and the angular velocity of each joint to control each joint controller to drive the motion of the mechanical arm.

[0128] Preferably, in step S41, the control criterion comprises:

[0129] (1) the midpoint of the line segment C1C2 tends to the optimal parking point P of the mechanical arm body coordinate system, wherein the optimal parking point P is defined as being located in front of the waist of the mechanical arm and having a distance L (see Figure 5 ) from the center of the waist, at this time the left and right double-shaft mechanical arms are balanced in force, the margins of the motion adjustment of the two double-shaft mechanical arms and the crown block meet the requirements, and the horizontal motion of the unloading target is adapted;

[0130] (2) the control strategy makes the straight line A1A2 and the straight line C1C2 meet the pre-set parallelism requirement;

[0131] (3) the crown block and the mechanical arm adopt a decoupling control strategy to reduce the control complexity.

[0132] Preferably, step S42 specifically comprises the steps of:

[0133] S4201, the overhead crane can achieve large-scale rapid movement. The control objective is to track the optimal stopping point P. At the current moment, the actual optimal stopping point P approaches the midpoint of line segment C1C2, expressed as:

[0134]

[0135] S4202, the optimal mooring point P is L away from the waist, and its coordinates can be expressed as:

[0136] x p =x0+Lcos(θ) 11 +π / 2)=x0-Lsinθ 11

[0137] y p =y0+Lsin(θ) 11 +π / 2)=y0+Lcosθ 11 ;

[0138] S4203. The deviation from the optimal mooring point P can be expressed as Δx0, Δy0, as shown in the following formula:

[0139] 2Δx0=L2cos(θ 11 +θ 12 )+L3cos(θ 11 +θ 12 +θ 13 )+L2cos(θ 21 +θ 22 )+L3cos(θ 21 +θ 22 +θ 23 )+2Lsinθ 11

[0140] 2Δy0=L2sin(θ 11 +θ 12 )+L3sin(θ 11 +θ 12 +θ 13 )+L2sin(θ 21 +θ 22 )+L3sin(θ 21 +θ 22 +θ 23 )-2Lcosθ 11 ;

[0141] S4204. A proportional controller is used to control the crane's movement to eliminate the above deviations, and the desired crane speed is obtained:

[0142]

[0143] in, Using proportional control parameters, the control of the overhead crane and the control of the robotic arm are decoupled. The control objective is to track the optimal parking point so that the parking point is always located directly in front of the waist of the overhead crane.

[0144] Preferably, step S42 further includes the following steps:

[0145] S4211. Using the criterion that lines A1A2 and C1C2 satisfy the preset parallelism requirement, the waist rotation is controlled. Ideally, θ 11 The coordinates of the suspension rope attachment points C1 and C2 of the two dual-axis robotic arms tending towards zero can be expressed as follows:

[0146]

[0147] S4212. Calculate the waist angle deviation using coordinates C1 and C2:

[0148]

[0149] S4213. The desired angular velocity control value of the lumbar joint is obtained by using a proportional controller:

[0150]

[0151] These are the control parameters for the lumbar joints.

[0152] Preferably, step S42 further includes the following steps:

[0153] S4221. Obtain the offset positions of the robotic arm's end points C1 and C2 using an optical motion capture system and an orthogonal magnetic grating ruler. and the speed of the mount point offset movement By controlling the robotic arm to follow the target's movement, eliminating deviation, and ensuring the rope remains vertical, the difference equation governing the evolution of system error is called error dynamics. The purpose of designing the controller is to make the error tend to zero over time. The dynamic change of the error is described using a second-order difference equation, which can be analogized to a typical mass-spring-damped model, specifically expressed as:

[0154]

[0155] in For natural frequency, When the damping ratio is given, and all roots of the second-order equation have negative real parts, the error will converge to zero over time. Adjustment... Adjust the value of the value to ensure system stability and regulate the tracking performance of the robotic arm;

[0156] S4222, determine the mounting point offset adjustment acceleration value according to the above formula, based on the control instruction of the last moment, the end speed control instruction of the current moment can be further obtained, denoted as:

[0157]

[0158] Wherein is the control instruction of the end of the robot arm at t-T moment, T is the control period length, and denotes the control instruction of the end of the robot arm at t moment.

[0159] S4223, after obtaining the end speed control instruction, the speed Jacobian model of the crown block and the robot arm can be used to calculate and output these values to each joint controller to drive the robot arm movement.

[0160] As Figure 6 shown, another aspect of the present application also provides a self-momentum target horizontal follow-up system control device, the horizontal follow-up system includes a crown block and two left and right double-shaft robot arms, the crown block can move horizontally, the left and right double-shaft robot arms are rotatably arranged at both ends of the waist, the left and right double-shaft robot arms are provided with a rope mounting point at the end and a suspended object is suspended by a suspension rope, the waist is installed at the center of the hoist warehouse of the crown block, the left and right double-shaft robot arms rotate horizontally, comprising:

[0161] A kinematic model establishment module is used to construct the kinematic model of the crown block and the robot arm offline.

[0162] A position measurement module is used to measure the target position in real time by using an optical motion capture system to obtain the horizontal deviation of the left and right double-shaft robot arms after the rope swings.

[0163] An offset speed measurement module is used to install the orthogonal displacement measurement device of the magnetic grating ruler on the upper end point of the suspension rope to continuously measure the horizontal offset speed during the swing of the suspension rope.

[0164] A decoupling control module is used to design a follow-up device decoupling controller, adopt a decoupling control method, and realize independent control of the crown block and the robot arm according to the optimal parking point and the measured horizontal deviation and horizontal offset speed.

[0165] As Figure 7 shown, the preferred embodiment of the present application also provides an electronic device, which includes a memory, a processor, and a computer program stored on the memory and executable on the processor, and the processor implements the steps of the self-momentum target horizontal follow-up system control method in the above embodiment when executing the computer program.

[0166] As Figure 8As shown, a preferred embodiment of this application also provides a computer device, which may be a terminal or a liveness detection server, and its internal structure diagram may be as follows. Figure 8 As shown. The computer device includes a processor, memory, and a network interface connected via a system bus. The processor provides computational and control capabilities. The memory includes a non-volatile storage medium and internal memory. The non-volatile storage medium stores the operating system and computer programs. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage medium. The network interface is used to communicate with other external computer devices via a network connection. When the computer program is executed by the processor, it implements the steps of the aforementioned self-momentum target level servo system control method.

[0167] Those skilled in the art will understand that Figure 8 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0168] A preferred embodiment of this application also provides a storage medium, the storage medium including a stored program, which, when the program is executed, controls the device where the storage medium is located to perform the steps of the self-momentum target level servo system control method in the above embodiments.

[0169] It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0170] If the functions described in this embodiment are implemented as software functional units and sold or used as independent products, they can be stored in one or more computing device-readable storage media. Based on this understanding, the parts of this application's embodiments that contribute to the prior art or the technical solutions can be embodied in the form of a software product. This software product is stored in a storage medium and includes several instructions to cause a computing device (which may be a personal computer, server, mobile computing device, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned storage media include: USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, optical disks, and other media capable of storing program code.

[0171] Those skilled in the art will appreciate that embodiments of the present application can be readily used as software, hardware, or a combination of software and hardware. In one embodiment, the present application can be implemented in software and / or firmware. In this embodiment, the software implementation can include computer readable code stored in a computer readable storage medium (alternatively referred to as computer readable media, computer readable storage media, or computer readable storage device) that, when taken in whole or in part, can program one or more computing devices to implement the functions of the present application. The computer readable storage medium can consist of one or more computer readable storage media including, but not limited to, magnetic storage media (e.g., magnetic disks), optical storage media (e.g., optical disks), solid state memory (e.g., flash memory, etc.), or any suitable combination of the foregoing. Such computer readable storage media can store the instructions for execution by one or more processors of an apparatus.

[0172] The present application is described in reference to the drawings, which are as follows. Figure 1 The processes and displays presented herein are not inherently related to any particular computer or other apparatus. Various Figure 1 The processes and displays presented herein are not inherently related to any particular computer or other apparatus. Various

[0173] The processes and displays presented herein are not inherently related to any particular computer or other apparatus. Various Figure 1 The processes and displays presented herein are not inherently related to any particular computer or other apparatus. Various Figure 1 The processes and displays presented herein are not inherently related to any particular computer or other apparatus. Various

[0174] The processes and displays presented herein are not inherently related to any particular computer or other apparatus. Various Figure 1 The processes and displays presented herein are not inherently related to any particular computer or other apparatus. Various Figure 1 The processes and displays presented herein are not inherently related to any particular computer or other apparatus. Various

[0175] While the preferred embodiments of the application have been described, additional variations and modifications can be employed. Therefore, the terms and expressions used throughout the description are intended to be illustrative, rather than limiting, of the claimed application. Accordingly, all changes coming within the spirit and scope of the application are intended to be embraced by the appended claims.

[0176] Obviously, numerous modifications and variations of the present application are possible in light of the above teachings. It is therefore to be understood that within the scope of the appended claims, the application can be practiced otherwise than as specifically described herein.

Claims

1. A control method for a self-momentum target horizontal servo system, the horizontal servo system comprising an overhead crane and two left and right dual-axis robotic arms, the overhead crane being capable of horizontal movement, the two left and right dual-axis robotic arms being rotatably mounted at both ends of a waist section, the ends of the two left and right dual-axis robotic arms being provided with rope attachment points and suspended objects via suspension ropes, the waist section being installed at the center of the overhead crane's hoisting compartment, the two left and right dual-axis robotic arms rotating horizontally, characterized in that... Including the following steps: S1. Offline construction of kinematic models of overhead crane and robotic arm; S2. The target position is measured in real time using an optical motion capture system to obtain the horizontal deviation of the target suspended by the two dual-axis robotic arms after the rope swings. S3. Install the orthogonal displacement measuring device of the magnetic scale onto the upper end of the suspension rope to continuously measure the horizontal offset speed during the swing of the suspension rope. S4. Design a decoupling controller for the servo device, adopt a decoupling control method, and realize independent control of the crane and the robotic arm based on the optimal parking point and the measured horizontal deviation and horizontal offset speed.

2. The control method for a self-momentum target horizontal servo system according to claim 1, characterized in that, Step S1 specifically includes the following steps: S11. The waist section is installed at the center of the overhead crane's pod. The center position of the overhead crane is denoted as (x0, y0), and the joint angle of the robotic arm is denoted as θ. ij The subscript i represents the left or right robotic arm symbol, where 1 represents right and 2 represents left. The subscript j represents the joint number corresponding to the robotic arm, with the angle being positive when rotating counterclockwise. A1 and A2 represent the first joint of the robotic arm, respectively. B1 and B2 represent the second joint of the robotic arm, respectively. C1 and C2 represent the upper end points of the suspension ropes at the end rope attachment points of the left and right robotic arms, respectively. A1A2 represents the line connecting the first joints of the two dual-axis robotic arms, with the waist located on the straight line A1A2. S12. During horizontal tracking, the horizontal coordinates of points C1 and C2 can be determined as follows: i 21 =θ 11 +p Where point O represents the center point of the overhead crane suspension device, the waist of the robotic arm, and also the position of the base of the left and right robotic arms; L1, L2, and L3 represent the lengths of connecting rods OA1, A1B1, and B1C1; and θ represents the length of the connecting rods OA1, A1B1, and B1C1. 11 θ represents the rotational joint angle of the waist. 12 θ represents the joint angle of the first joint of the right robotic arm. 13 θ represents the joint angle of the second joint of the right robotic arm. 21 Indicates the waist rotation angle, relative to θ 12 The phase difference is π, θ 22 θ represents the joint angle of the first joint of the left robotic arm. 23 Let x0 represent the joint angle of the second joint of the left robotic arm, and y0 represent the offset of the crane in the x-direction and y0 represent the offset of the crane in the y-direction. S13. Construct two dual-axis robotic arm velocity Jacobian matrices so that the system can adopt a speed control strategy to improve the response speed. The simplified result after differentiating both sides of the above equation is as follows: in, a1=-L1sinθ 11 , b1=-L2sin(θ 11 +θ 12 ), d1=-L3sin(θ 11 +θ 12 +θ 13 ), a2=L1cosθ 11 , b2=L2cos(θ 11 +θ 12 ), d2=L3sin(θ 11 +θ 12 +θ 13 ), A1=-L1sinθ 21 , B1=-L2sin(θ 21 +θ 22 ), D1=-L3sin(θ 21 +θ 22 +θ 23 ), A2=L1cosθ 21 , B2=L2cos(θ 21 +θ 22 ), D2=L3cos(θ 21 +θ 22 +θ 23 ). k1, k2, k3, k4 are intermediate variables; Robotic arm joint angular velocity The Jacobian matrix can be represented as:

3. The control method for a self-momentum target horizontal servo system according to claim 2, characterized in that, Step S2 specifically includes the following steps: S21. Fix a marker point on the suspension rope at the upper end of the robotic arm, and fix a second marker point on the suspension rope above the first marker point. At this time, the suspension rope passes through the center of the marker point. Determine the rope swing by estimating the positions of the two marker points, and use an optical motion capture system to measure the horizontal deviation of the target suspended by the left and right dual-axis robotic arms after the rope swings.

4. The control method for a self-momentum target horizontal servo system according to claim 2, characterized in that, Step S4 specifically includes the following steps: S41. Simplify the control system, pre-design control criteria, and prioritize setting x0, y0, and θ. 11 To improve the control laws, additional constraint equations are needed. S42. Based on the preset control criteria, calculate the desired motion speed of the overhead crane to control the crane's motion, calculate the desired angular velocity of the lumbar joint to control the lumbar rotation, and calculate the angular velocity of each joint to control each joint controller to drive the robotic arm's motion.

5. The control method for a self-momentum target horizontal servo system according to claim 4, characterized in that, In step S41, the control criteria include: (1) Make the midpoint of line segment C1C2 approach the optimal parking point P of the robot arm body coordinate system. The optimal parking point P is defined as: located in front of the waist of the robot arm, with the distance from the waist center set to L. At this time, the two dual-axis robots on the left and right sides are balanced, and the margin of motion adjustment of the two dual-axis robots and the crane meets the requirements, adapting to the horizontal movement of the unloading target. (2) The control strategy ensures that lines A1A2 and C1C2 meet the preset parallelism requirements; (3) The overhead crane and the robotic arm adopt a decoupled control strategy to reduce control complexity.

6. The control method for a self-momentum target horizontal servo system according to claim 5, characterized in that, Step S42 specifically includes the following steps: S4201, the overhead crane can achieve large-scale rapid movement. The control objective is to track the optimal stopping point P. At the current moment, the actual optimal stopping point P approaches the midpoint of line segment C1C2, expressed as: S4202, the optimal mooring point P is L away from the waist, and its coordinates can be expressed as: x p =x0+L cos(θ 11 +π / 2)=x0-L sinθ 11 y p =y0+L sin(θ 11 +π / 2)=y0+L cosθ 11 ; S4203. The deviation from the optimal mooring point P can be expressed as Δx0, Δy0, as shown in the following formula: 2Δx0=L2cos(θ 11 +θ 12 )+L3cos(θ 11 +θ 12 +θ 13 )+L2cos(θ 21 +θ 22 )+L3cos(θ 21 +θ 22 +θ 23 )+2L sinθ 11 2Δy0=L2sin(θ 11 +θ 12 )+L3sin(θ 11 +θ 12 +θ 13 )+L2sin(θ 21 +θ 22 )+L3sin(θ 21 +θ 22 +θ 23 )-2L cosθ 11 ; S4204. A proportional controller is used to control the crane's movement to eliminate the above deviations, and the desired crane speed is obtained: in, Using proportional control parameters, the control of the overhead crane and the control of the robotic arm are decoupled. The control objective is to track the optimal parking point so that the parking point is always located directly in front of the waist of the overhead crane.

7. The control method for a self-momentum target horizontal servo system according to claim 5, characterized in that, Step S42 further includes the following steps: S4211. Using the criterion that lines A1A2 and C1C2 satisfy the preset parallelism requirement, the waist rotation is controlled. Ideally, θ 11 The coordinates of the suspension rope attachment points C1 and C2 of the two dual-axis robotic arms tending towards zero can be expressed as follows: S4212. Calculate the waist angle deviation using coordinates C1 and C2: S4213. The desired angular velocity control value of the lumbar joint is obtained by using a proportional controller: These are the control parameters for the lumbar joints.

8. The control method for a self-momentum target horizontal servo system according to claim 5, characterized in that, Step S42 further includes the following steps: S4221. Obtain the offset positions of the robotic arm's end points C1 and C2 using an optical motion capture system and an orthogonal magnetic grating ruler. and the speed of the mount point offset movement By controlling the robotic arm to follow the target's movement, eliminating deviation, and ensuring the rope remains vertical, the difference equation governing the evolution of system error is called error dynamics. The purpose of designing the controller is to make the error tend to zero over time. The dynamic change of the error is described using a second-order difference equation, which can be analogized to a typical mass-spring-damped model, specifically expressed as: in For natural frequency, When the damping ratio is given, and all roots of the second-order equation have negative real parts, the error will converge to zero over time. Adjusting K... p r K d r Adjust the value of the value to ensure system stability and regulate the tracking performance of the robotic arm; S4222. Based on the above formula, determine the acceleration value for the mount point offset adjustment. Based on the control command from the previous moment, the end-vehicle speed control command for the current moment can be obtained, expressed as: in It is the control command at the end effector of the robotic arm at time tT, where T is the duration of the control cycle. This represents the control command given to the end effector of the robotic arm at time t. S4223. After receiving the end-effector speed control command, the speed Jacobian model of the overhead crane and the robotic arm can be used to calculate the speed. These values ​​are then output to each joint controller to drive the robotic arm's movement.

9. A control device for a self-momentum target horizontal servo system, the horizontal servo system comprising a crane and two left and right dual-axis robotic arms, the crane being capable of horizontal movement, the two left and right dual-axis robotic arms being rotatably mounted at both ends of a waist section, the ends of the two left and right dual-axis robotic arms being provided with rope attachment points and suspended objects via suspension ropes, the waist section being installed at the center of the crane's hoisting compartment, the two left and right dual-axis robotic arms rotating horizontally, characterized in that... include: The kinematic model building module is used to build kinematic models of overhead cranes and robotic arms offline. The position measurement module is used to measure the target position in real time using an optical motion capture system, and to obtain the horizontal deviation of the target suspended by the two dual-axis robotic arms after the rope swings. The offset speed measurement module is used to install the orthogonal displacement measuring device of the magnetic grating ruler onto the upper end of the suspension rope to continuously measure the horizontal offset speed during the swing of the suspension rope. The decoupling control module is used to design a decoupling controller for the servo device. It adopts a decoupling control method and realizes independent control of the crane and the robotic arm based on the optimal parking point and the measured horizontal deviation and horizontal offset speed.

10. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the self-momentum target level servo system control method as described in any one of claims 1 to 8.

Images