A control method and control system for a planar double five-link mechanism

Abstract

The present application provides a control method and control system for a planar double five-bar linkage, the method comprising: establishing a mechanical decoupling matrix from the logical axis to the physical axis of the center of mass of the planar double five-bar linkage; determining the current position of the robot arm based on the rotation angle, the motor shaft radius, and the crank length; determining the current position of the center of mass of the mechanism using the current position of the robot arm and the physical relationship between the robot arm and the center of mass; determining the logical axis force of the center of mass of the mechanism based on the current position and a reference position of the center of mass of the mechanism; inputting the logical axis force and the current position of the robot arm into the mechanical decoupling matrix to determine the motor torque of the rotating motor, controlling the rotating motor according to the motor torque, and driving the planar double five-bar linkage to operate through the rotating motor. By adopting the above-mentioned control method and control system for the planar double five-bar linkage, the problems of slow response speed, low control freedom, and limited rotation angle range in the control method of the existing motion platform are solved.

Description

Technical Field

[0001] The present application relates to the field of automatic control technology, and in particular to a control method and control system for a planar double five-bar linkage mechanism. Background Art

[0002] In the field of industrial production, the control accuracy of the motion table directly determines the processing accuracy of industrial equipment. The motion table has a strong impact on its non-moving parts during the movement process, which seriously affects the feedback control accuracy of the motion table. In order to improve the control accuracy of the motion table, a balancing mechanism connected to the motion table is set up. The motion table is controlled by the balancing mechanism to eliminate the impact effect. Therefore, how to effectively control the balancing mechanism has become the key to improving the control accuracy of the motion table. At present, there are usually two types of balancing mechanisms: one is to use shock absorbers and linear motor drives as balancing mechanisms, and add shock absorbers between the motion table and the base frame to reduce vibration. The other is a balancing mechanism driven by a linear motor, which requires the arrangement of multiple linear motors to control the planar degrees of freedom. Its control strategy is relatively simple, and output decoupling is easy to achieve.

[0003] However, the first balancing mechanism control method primarily uses shock absorbers to mitigate impacts. Since shock absorbers only operate on a single degree of freedom, this leads to a single degree of control and a slow response speed. The second balancing mechanism control method, however, suffers from a limited range of rotation angles due to the linear motor's single drive direction. Summary of the Invention

[0004] In view of this, the purpose of this application is to provide a control method and control system for a planar double five-bar linkage mechanism to solve the problems of slow response speed, low control freedom and limited rotation angle range in the existing motion platform control method.

[0005] In a first aspect, an embodiment of the present application provides a control method for a planar double five-bar linkage mechanism, the planar double five-bar linkage mechanism including a rotary motor, a robotic arm, and a crank, the control method comprising:

[0006] Based on the dimensional parameters of the planar double five-bar linkage mechanism and the planar mechanical relationship between the robot arm and the crank, a mechanical decoupling matrix from the logical axis to the physical axis of the center of mass of the planar double five-bar linkage mechanism is established.

[0007] Obtain the rotation angle of the rotary motor and determine the current position of the robotic arm based on the rotation angle, the motor shaft radius, and the crank length;

[0008] Using the current position of the manipulator and the physical relationship between the manipulator and the center of mass, the current position of the center of mass of the planar double five-bar linkage is determined;

[0009] Based on the current position and reference position of the center of mass of the planar double five-bar linkage mechanism, a position error and a logical axial force of the center of mass of the planar double five-bar linkage mechanism corresponding to the position error are determined;

[0010] The logical axis force and the current position of the robot arm are input into the mechanical decoupling matrix to determine the motor torque of the rotating motor. The rotating motor is then controlled according to the motor torque, and the planar double five-bar linkage is driven by the rotating motor.

[0011] Optionally, the planar double five-bar linkage includes a first five-bar linkage and a second five-bar linkage, the first five-bar linkage includes a first rotary motor, a second rotary motor, a first robotic arm, a first crank, a second crank, a first connecting rod and a second connecting rod, and the second five-bar linkage includes a third rotary motor, a fourth rotary motor, a second robotic arm, a third crank, a fourth crank, a third connecting rod and a fourth connecting rod; based on the size parameters of the planar double five-bar linkage and the planar mechanical relationship between the robotic arm and the crank, a mechanical decoupling matrix from the logical axis to the physical axis of the center of mass of the planar double five-bar linkage is established, including: according to the size parameters of the planar double five-bar linkage, The first mapping matrix is ​​established based on the inch parameters. The first mapping matrix refers to the mechanical mapping matrix between the center of mass of the planar double five-bar linkage and the robotic arm; according to the planar mechanical relationship between the robotic arm and the crank, the second mapping matrix and the angle solution relationship are established. The second mapping matrix refers to the mechanical mapping matrix between the robotic arm and the connecting rod, and the angle solution relationship refers to the solution relationship between the connecting rod angle and the rotation angle of the rotary motor; the crank end is subjected to radial and tangential mechanical analysis to determine the motor torque calculation formula; based on the first mapping matrix, the second mapping matrix, the angle solution relationship and the motor torque calculation formula, a mechanical decoupling matrix is ​​established.

[0012] Optionally, a first mapping matrix is ​​established according to the size parameters of the planar double five-bar linkage, including: establishing a first center-of-mass force equation based on the equal relationship between the sum of the forces on the first and second robotic arms in the X direction and the force on the center of mass of the planar double five-bar linkage in the X direction; establishing a second center-of-mass force equation based on the equal relationship between the sum of the forces on the first and second robotic arms in the Y direction and the force on the center of mass of the planar double five-bar linkage in the Y direction; taking the product of the difference between the forces on the second robotic arm and the first robotic arm in the Y direction and the first center-of-mass distance as the first center-of-mass torque, where the first center-of-mass distance refers to the first mechanical arm in the X direction. The invention relates to a method for determining the distance between the origin of the first manipulator arm or the origin of the second manipulator arm and the center of mass origin of the planar double five-bar linkage mechanism; multiplying the difference between the forces exerted on the second manipulator arm and the first manipulator arm in the X direction by the second center of mass distance as the second center of mass torque, where the second center of mass distance refers to the distance between the origin of the first manipulator arm or the origin of the second manipulator arm and the center of mass origin of the planar double five-bar linkage mechanism in the Y direction; constructing a center of mass torque equation based on the equality between the sum of the first center of mass torque and the second center of mass torque and the center of mass torque of the planar double five-bar linkage mechanism; and performing matrix transformation on the first center of mass force equation, the second center of mass force equation, and the center of mass torque equation to generate a first mapping matrix.

[0013] Optionally, according to the planar mechanical relationship between the robotic arm and the crank, a second mapping matrix and an angle solution relationship are established, including: taking the product of the cosine value of the first connecting rod angle and the force of the first connecting rod on the first robotic arm as the first component force in the X direction of the first robotic arm, and taking the product of the cosine value of the second connecting rod angle and the force of the second connecting rod on the first robotic arm as the second component force in the X direction of the first robotic arm, the first connecting rod angle refers to the counterclockwise angle between the first connecting rod and the positive direction of the X axis, and the second connecting rod angle refers to the counterclockwise angle between the second connecting rod and the positive direction of the X axis; based on the first component force in the X direction of the first robotic arm and the product of the cosine value of the second connecting rod angle and the force of the second connecting rod on the first robotic arm, the first component force in the X direction of the first robotic arm is calculated. The first force equation of the first robotic arm is established by taking the product of the sine value of the first link angle and the force exerted by the first link on the first robotic arm as the first component of the force in the Y direction of the first robotic arm, and the product of the sine value of the second link angle and the force exerted by the second link on the first robotic arm as the second component of the force in the Y direction of the first robotic arm; based on the equal relationship between the sum of the first component of the force in the Y direction of the first robotic arm and the second component of the force in the Y direction of the first robotic arm and the force exerted on the first robotic arm in the Y direction, the second force equation of the first robotic arm is established; taking the cosine value of the third link angle and the product of the sine value of the third link angle and the force exerted by the first robotic arm on the first robotic arm as the second component of the force in the Y direction of the first robotic arm The product of the forces exerted by the three links on the second robotic arm is used as the first component of the second robotic arm in the X direction, and the product of the cosine value of the fourth link angle and the force exerted by the fourth link on the second robotic arm is used as the second component of the second robotic arm in the X direction. The third link angle refers to the counterclockwise angle between the third link and the positive direction of the X axis, and the fourth link angle refers to the counterclockwise angle between the fourth link and the positive direction of the X axis. Based on the equal relationship between the sum of the first component of the second robotic arm in the X direction and the second component of the second robotic arm in the X direction and the force on the second robotic arm in the X direction, the first force equation of the second robotic arm is established; the sine value of the third link angle is used as The product of the sine value of the fourth link angle and the force exerted by the third link on the second robotic arm is used as the first component force of the second robotic arm in the Y direction, and the product of the sine value of the fourth link angle and the force exerted by the fourth link on the second robotic arm is used as the second component force in the Y direction of the second robotic arm; based on the equal relationship between the sum of the first component force in the Y direction of the second robotic arm and the second component force in the Y direction of the second robotic arm and the force on the second robotic arm in the Y direction, the second force equation of the second robotic arm is established; the first force equation of the first robotic arm, the second force equation of the first robotic arm, the first force equation of the second robotic arm and the second force equation of the second robotic arm are matrix transformed to generate a second mapping matrix.

[0014] Optionally, the current position of the first robotic arm includes the X-axis coordinate of the first robotic arm and the Y-axis coordinate of the first robotic arm, and the current position of the second robotic arm includes the X-axis coordinate of the second robotic arm and the Y-axis coordinate of the second robotic arm; according to the planar mechanical relationship between the robotic arm and the crank, a second mapping matrix and an angle solution relationship are established, which also includes: taking the Y-axis coordinate of the first robotic arm as the minuend, taking the product of the first rotation shaft radius and the sine value of the first rotation angle and the sum of the distance between the first rotating motor and the origin of the first robotic arm in the Y direction as the subtrahend, taking the difference between the minuend and the subtrahend as the first longitudinal difference, the first rotation shaft radius is the rotation shaft radius of the first rotating motor, and the first rotation angle is the rotation angle of the first rotating motor; taking the ratio of the first longitudinal difference to the length of the first connecting rod as the first longitudinal ratio, and based on the first longitudinal ratio and the sine value of the first connecting rod angle, establish a first angle solution equation corresponding to the first connecting rod; take the X-axis coordinate of the first robotic arm as the minuend, take the product of the first rotation axis radius and the cosine value of the first rotation angle and the sum of the distance between the first rotating motor and the origin of the first robotic arm in the X direction as the subtrahend, and use the difference between the minuend and the subtrahend as the first lateral difference; take the ratio of the first lateral difference to the length of the first connecting rod as the first lateral ratio, and establish a second angle solution equation corresponding to the first connecting rod based on the equal relationship between the first lateral ratio and the cosine value of the first connecting rod angle; according to the above process, establish the first angle solution equation and the second angle solution equation corresponding to the second connecting rod, the third connecting rod and the fourth connecting rod respectively; establish the angle solution relationship from the eight angle solution equations corresponding to the four connecting rods.

[0015] Optionally, a radial and tangential mechanical analysis is performed on the crank end to determine the motor torque calculation formula, including: for each rotating motor, calculating the product of the connecting rod force corresponding to the rotating motor and the radius of the rotating motor's shaft, and calculating the product of the motor torque of the rotating motor and the cosine value of the corresponding crank-connecting rod angle, where the connecting rod force refers to the force of the connecting rod on the robotic arm, and the crank-connecting rod angle refers to the angle between the tangential force direction of the crank and the connecting rod; constructing a torque equation based on the equality between the two products; for each rotating motor, determining the motor torque calculation formula of the rotating motor based on the torque equation corresponding to the rotating motor.

[0016] Optionally, the current position of the robotic arm is determined based on the rotation angle, the motor shaft radius and the crank length, including: for each crank, based on the coordinates of the rotating motor corresponding to the crank, the shaft radius and the rotation angle, determining the end coordinates of the crank; based on the end coordinates of each crank, determining the first crank line connecting rod angle, the second crank line connecting rod angle, the first double crank angle and the second double crank angle, the first crank line connecting rod angle is the angle between the line connecting the end of the first crank and the end of the second crank and the first connecting rod, the second crank line connecting rod angle is the angle between the line connecting the end of the third connecting rod and the end of the fourth connecting rod and the third connecting rod The angle between the rods, the first double crank angle is the angle between the line connecting the end of the first crank and the end of the second crank and the X-axis, and the second double crank angle is the angle between the line connecting the end of the third connecting rod and the end of the fourth connecting rod and the X-axis; the sum of the first crank line connecting rod angle and the first double crank angle is used as the first connecting rod angle, and the sum of the second crank line connecting rod angle and the second double crank angle is used as the third connecting rod angle; based on the first crank coordinate, the first connecting rod length and the first connecting rod angle, the first robotic arm coordinate is determined; based on the third crank coordinate, the third connecting rod length and the third connecting rod angle, the second robotic arm coordinate is determined.

[0017] Optionally, the current position of the center of mass of the planar double five-bar linkage is determined using the current position of the robotic arm and the physical relationship between the robotic arm and the center of mass, including: taking the weighted sum of the coordinates of the first robotic arm and the second robotic arm in the X direction as the X-axis coordinate of the center of mass position of the planar double five-bar linkage; taking the weighted sum of the coordinates of the first robotic arm and the second robotic arm in the Y direction as the Y-axis coordinate of the center of mass position of the planar double five-bar linkage; determining multiple center of mass position coefficients based on the distances in the X and Y directions between the robotic arm and the center of mass of the planar double five-bar linkage; calculating the product of each center of mass position coefficient and the corresponding robotic arm coordinate, and taking the sum of the multiple products as the rotation angle of the center of mass of the planar double five-bar linkage; and taking the X-axis coordinate, Y-axis coordinate and rotation angle of the center of mass position of the planar double five-bar linkage as the current position of the center of mass of the planar double five-bar linkage structure.

[0018] Optionally, before determining the position error and the logical axial force of the center of mass of the planar double five-bar linkage mechanism corresponding to the position error based on the current position and reference position of the center of mass of the planar double five-bar linkage mechanism, it also includes: obtaining the reference position of the motion platform, taking the quotient of the mass of the planar double five-bar linkage mechanism and the mass of the motion platform as the position coefficient; and taking the product of the reference position of the motion platform and the position coefficient as the reference position.

[0019] In a second aspect, an embodiment of the present application further provides a control system for a planar double five-bar linkage mechanism, the control system comprising a comparison module, a controller, an actuator, a motor angle measurement module, and a position measurement module;

[0020] a comparison module, configured to receive a reference position and a current position of the center of mass of the planar double five-bar linkage fed back by the position measurement module, and determine a position error based on the reference position and the current position;

[0021] a controller, configured to receive a position error output by the comparison module and determine a logical axis force based on the position error;

[0022] an actuator, configured to receive the logical shaft force output by the controller and determine the motor torque of the rotating motor based on the logical shaft force;

[0023] The motor angle measurement module is used to obtain the current rotation angle of the rotating motor;

[0024] The position measurement module is used to determine the current position of the center of mass of the planar double five-bar linkage based on the current rotation angle of the rotating motor, and feed the current position of the center of mass of the planar double five-bar linkage as feedback information to the comparison module.

[0025] The embodiments of the present application bring the following beneficial effects:

[0026] The present invention provides a control method and control system for a planar double-five-bar linkage mechanism. The control method and control system utilize the planar double-five-bar linkage mechanism to control the movement of a motion platform, enabling multi-degree-of-freedom control of the motion platform. By performing dynamic analysis on the planar double-five-bar linkage mechanism, a mechanical decoupling matrix for controlling the center of mass of the planar double-five-bar linkage mechanism is determined, thereby achieving the desired effect of accurately and rapidly determining the motor torque of the rotary motor. Simultaneously, the current position of the robotic arm and the current position of the center of mass of the planar double-five-bar linkage mechanism are determined based on parameters such as the rotation angle of the rotary motor and the motor shaft radius. The determined current position of the center of mass of the planar double-five-bar linkage mechanism and a feedback loop control strategy are used to perform feedback control of the planar double-five-bar linkage mechanism. This method effectively mitigates the high impact and strong residual vibration of the motion platform on the base frame during high-speed and high-acceleration motion, ensuring the required control accuracy of the motion platform. Compared with existing motion platform control methods, this method addresses the issues of slow response speed, low control degrees of freedom, and limited rotation angle range.

[0027] In order to make the above-mentioned objects, features and advantages of the present application more obvious and easy to understand, preferred embodiments are given below and described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following is a brief introduction to the drawings required for use in the embodiments. It should be understood that the following drawings only show certain embodiments of the present application and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other relevant drawings can be obtained based on these drawings without paying any creative work.

[0029] Figure 1 A flow chart showing a control method for a planar double five-bar linkage mechanism provided in an embodiment of the present application is shown;

[0030] Figure 2a A schematic structural diagram of a first five-bar linkage mechanism provided in an embodiment of the present application is shown;

[0031] Figure 2b A schematic structural diagram of a second five-bar linkage mechanism provided in an embodiment of the present application is shown;

[0032] Figure 3 A schematic diagram showing the mechanical relationship of the first five-bar linkage provided in an embodiment of the present application is shown;

[0033] Figure 4 A schematic diagram showing the mechanical relationship of the second five-bar linkage provided in an embodiment of the present application is shown;

[0034] Figure 5 A schematic diagram showing the mechanical relationship between the first rotating motor, the first crank, and the first connecting rod provided in an embodiment of the present application is shown;

[0035] Figure 6 A schematic diagram showing the angular relationship between the first connecting rod angle, the first crank connecting rod angle, and the first double crank angle provided in an embodiment of the present application is shown;

[0036] Figure 7 A schematic diagram showing the angular relationship between the third connecting rod angle, the second crank connecting rod angle, and the second double crank angle provided in an embodiment of the present application is shown;

[0037] Figure 8 A structural schematic diagram of a control system of a planar double five-bar linkage mechanism provided in an embodiment of the present application is shown. DETAILED DESCRIPTION

[0038] In order to make the purpose, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all of the embodiments. The components of the embodiments of the present application generally described and shown in the drawings here can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the application for which protection is claimed, but merely represents the selected embodiments of the present application. Based on the embodiments of the present application, each other embodiment obtained by those skilled in the art without making creative work falls within the scope of protection of the present application.

[0039] It is worth noting that, prior to the present application, in the field of industrial production, the control accuracy of the motion platform directly determined the processing accuracy of industrial equipment. The motion platform exerts a strong impact on its non-moving parts during movement, seriously affecting the feedback control accuracy of the motion platform. To improve the control accuracy of the motion platform, a balancing mechanism connected to the motion platform is provided. The balancing mechanism is used to control the motion platform and eliminate the impact effect. Therefore, how to effectively control the balancing mechanism has become the key to improving the control accuracy of the motion platform. Currently, balancing mechanisms are generally of the following two types: one uses a shock absorber and a linear motor drive as a balancing mechanism, adding a shock absorber between the motion platform and the base frame to reduce vibration. The other is a balancing mechanism driven by a linear motor, which requires the arrangement of multiple linear motors to control the planar degrees of freedom. Its control strategy is relatively simple, and output decoupling is easy to achieve. However, in the control method for the first balancing mechanism, the shock absorber is mainly used to reduce the impact effect. Since the shock absorber can only work on a single degree of freedom, it leads to the problem of controlling a single degree of freedom and a slow response speed during the control process. In the control method for the second balancing mechanism, since the driving direction of the linear motor is relatively single, there is a problem of limited rotation angle range during rotation control.

[0040] Based on this, an embodiment of the present application provides a control method for a planar double five-bar linkage mechanism to improve the response speed, control freedom and rotation angle range when controlling a motion platform.

[0041] See also Figure 1 , Figure 1 This is a flow chart of a control method for a planar double five-link mechanism provided in an embodiment of the present application. Figure 1 As shown, the control method of the planar double five-bar linkage provided in the embodiment of the present application includes a rotating motor, a robotic arm, and a crank. The control method includes:

[0042] Step S101 : Based on the dimensional parameters of the planar double five-bar linkage and the planar mechanical relationship between the robot arm and the crank, a mechanical decoupling matrix from the logical axis to the physical axis of the center of mass of the planar double five-bar linkage is established.

[0043] In this step, the mechanical decoupling matrix may refer to a transfer decoupling matrix, and the mechanical decoupling matrix is ​​used to determine the motor torque of the rotating motor according to the input logical shaft force.

[0044] In an embodiment of the present application, the method can be performed by a control system of a planar double five-bar linkage. First, the control system of the planar double five-bar linkage obtains the dimensional parameters of the planar double five-bar linkage, which are determined before constructing the mechanical decoupling matrix and can be obtained by measurement. The dimensional parameters include but are not limited to: the first center of mass distance, the second center of mass distance, the first connecting rod length, the second connecting rod length, the third connecting rod length, the fourth connecting rod length, the distance between the center of each rotating motor in the X direction and the corresponding robot arm origin, the distance between the center of each rotating motor in the Y direction and the corresponding robot arm origin, and the motor shaft radius of each rotating motor.

[0045] The control system for the planar double five-bar linkage then uses these dimensional parameters and the planar mechanical relationship between the arm and crank to construct a mechanical decoupling matrix. This matrix can be used as a calculation formula, and the motor torque of the rotary motor can be calculated by inputting the logical axial force into the matrix.

[0046] In an optional embodiment, the planar double five-bar linkage includes a first five-bar linkage and a second five-bar linkage, the first five-bar linkage includes a first rotary motor, a second rotary motor, a first robotic arm, a first crank, a second crank, a first connecting rod and a second connecting rod, and the second five-bar linkage includes a third rotary motor, a fourth rotary motor, a second robotic arm, a third crank, a fourth crank, a third connecting rod and a fourth connecting rod; based on the size parameters of the planar double five-bar linkage and the planar mechanical relationship between the robotic arm and the crank, a mechanical decoupling matrix of the logical axis to the physical axis of the center of mass of the planar double five-bar linkage is established, including: according to the planar double five-bar linkage The first mapping matrix is ​​established based on the size parameters. The first mapping matrix refers to the mechanical mapping matrix between the center of mass of the planar double five-bar linkage and the robotic arm. According to the planar mechanical relationship between the robotic arm and the crank, the second mapping matrix and the angle solution relationship are established. The second mapping matrix refers to the mechanical mapping matrix between the robotic arm and the connecting rod. The angle solution relationship refers to the solution relationship between the connecting rod angle and the rotation angle of the rotary motor. The crank end is subjected to radial and tangential mechanical analysis to determine the motor torque calculation formula. Based on the first mapping matrix, the second mapping matrix, the angle solution relationship and the motor torque calculation formula, a mechanical decoupling matrix is ​​established.

[0047] Refer to the following Figure 2a and Figure 2b Let’s introduce the five-bar linkage.

[0048] Figure 2a A structural schematic diagram of the first five-bar linkage provided in an embodiment of the present application is shown.

[0049] Figure 2b A structural schematic diagram of a second five-bar linkage provided in an embodiment of the present application is shown.

[0050] like Figure 2a As shown, the first five-bar linkage includes a first rotary motor 211, a second rotary motor 212, a first robotic arm 213, a first connecting rod 214, a second connecting rod 215, a first crank 216, and a second crank 217. The first rotary motor 211 drives the first crank 216 to rotate about the axis of the first rotary motor. The first crank 216 is connected to the first end of the first connecting rod 214 and drives the first connecting rod 214 to move. The second end of the first connecting rod 214 intersects with the other end of the second connecting rod 215. The intersection is connected to the first robotic arm 213, which is connected to the motion stage to control the movement of the motion stage. Here, the line segment between the first end of the first connecting rod 214 and the axis of the first rotary motor represents the position of the first crank 216. The motion plane of the motion stage is parallel to the motion planes of the connecting rods and the robotic arm.

[0051] like Figure 2b As shown, the second five-bar linkage includes a third rotary motor 221, a fourth rotary motor 222, a second robotic arm 223, a third connecting rod 224, a fourth connecting rod 225, a third crank 226, and a fourth crank 227. The second five-bar linkage has the same motion as the first five-bar linkage and will not be described in detail.

[0052] Here, the first mapping matrix refers to the force matrix between the center of mass of the planar double five-bar linkage and the robotic arm, i.e., the mechanical mapping matrix. The second mapping matrix refers to the force matrix between the robotic arm and the connecting rod. The angle settlement relationship refers to the calculated relationship between the connecting rod angle and the rotation angle of the rotary motor.

[0053] Specifically, the first mapping matrix, the second mapping matrix, the angle solution relationship and the motor torque calculation formula are determined respectively, and then the above four formulas are combined to establish a mechanical decoupling matrix.

[0054] In an optional embodiment, a first mapping matrix is ​​established based on the dimensional parameters of the planar double five-bar linkage, including: establishing a first center of mass force equation based on the equal relationship between the sum of the forces applied to the first and second robotic arms in the X direction and the force applied to the center of mass of the planar double five-bar linkage in the X direction; establishing a second center of mass force equation based on the equal relationship between the sum of the forces applied to the first and second robotic arms in the Y direction and the force applied to the center of mass of the planar double five-bar linkage in the Y direction; taking the product of the difference between the forces applied to the second robotic arm and the first robotic arm in the Y direction and the first center of mass distance as the first center of mass torque, where the first center of mass distance refers to the first center of mass torque in the X direction. The distance between the origin of the robot arm or the origin of the second robot arm and the origin of the center of mass of the planar double five-bar linkage mechanism is determined; the product of the difference in force between the second robot arm and the first robot arm in the X direction and the second center of mass distance is used as the second center of mass torque, and the second center of mass distance refers to the distance in the Y direction between the origin of the first robot arm or the origin of the second robot arm and the origin of the center of mass of the planar double five-bar linkage mechanism; a center of mass torque equation is constructed based on the equality relationship between the sum of the first center of mass torque and the second center of mass torque and the center of mass torque of the planar double five-bar linkage mechanism; and matrix transformation is performed on the first center of mass force equation, the second center of mass force equation, and the center of mass torque equation to generate a first mapping matrix.

[0055] Here, the force on the first robot arm in the X direction is recorded as F AX , the force on the second robot arm in the X direction is recorded as F BX , the force on the first robot arm in the Y direction is recorded as F AY , the force on the second robot arm in the Y direction is recorded as F BY The force on the center of mass of the planar double five-bar linkage in the X direction is recorded as F X The force on the center of mass of the planar double five-bar linkage in the Y direction is recorded as F Y , the torque at the center of mass of the planar double five-bar linkage is denoted as T Z , the first centroid distance is recorded as L X , the second centroid distance is recorded as L Y . The first robotic arm origin may refer to a point in the coordinate system corresponding to the first robotic arm when the first robotic arm is in the initial position, and the second robotic arm origin may refer to a point in the coordinate system corresponding to the second robotic arm when the second robotic arm is in the initial position.

[0056] Specifically, the force equation for the first center of mass is: F X =F AX +F BX , the force equation for the second center of mass is: F Y =F AY +F BY , the first center of mass torque is: (F BY -F AY )×L X , the second center of mass torque is: (F BX -F AX)×L Y , the center of mass torque equation is: T Z =(F BY -F AY )×L X +(F BX -F AX )×L Y .

[0057] The first center-of-mass force equation, the second center-of-mass force equation, and the center-of-mass torque equation are converted into matrix form:

[0058]

[0059] The above formula is expressed as: F b =GF m , where F b The force matrix of the center of mass of the planar double five-bar linkage, F m Represents the force matrix of the first and second robotic arms in the planar double five-bar linkage.

[0060] F b =GF m Transform the form to get: F m =G -1 F b , then the first mapping matrix G can be determined -1 for:

[0061]

[0062] In an optional embodiment, according to the planar mechanical relationship between the robot arm and the crank, a second mapping matrix and an angle solution relationship are established, including: taking the product of the cosine value of the first connecting rod angle and the force of the first connecting rod on the first robot arm as the first component force in the X direction of the first robot arm, and taking the product of the cosine value of the second connecting rod angle and the force of the second connecting rod on the first robot arm as the second component force in the X direction of the first robot arm, the first connecting rod angle refers to the counterclockwise angle between the first connecting rod and the positive direction of the X axis, and the second connecting rod angle refers to the counterclockwise angle between the second connecting rod and the positive direction of the X axis; based on the first component force in the X direction of the first robot arm and the first component force in the X direction of the first robot arm, the first connecting rod angle refers to the counterclockwise angle between the first connecting rod and the positive direction of the X axis. The first force equation of the first robotic arm is established based on the equal relationship between the sum of the second component of the force in the X direction of the robotic arm and the force of the first robotic arm in the X direction; the product of the sine value of the first link angle and the force of the first link on the first robotic arm is used as the first component of the force in the Y direction of the first robotic arm, and the product of the sine value of the second link angle and the force of the second link on the first robotic arm is used as the second component of the force in the Y direction of the first robotic arm; based on the equal relationship between the sum of the first component of the force in the Y direction of the first robotic arm and the second component of the force in the Y direction of the first robotic arm and the force of the first robotic arm in the Y direction, the second force equation of the first robotic arm is established; the cosine of the angle of the third link is used as the product of the sine value of the first link angle and the force of the second link on the first robotic arm The product of the value and the force exerted by the third link on the second robotic arm is taken as the first component of the second robotic arm in the X direction, and the product of the cosine value of the fourth link angle and the force exerted by the fourth link on the second robotic arm is taken as the second component of the second robotic arm in the X direction. The third link angle refers to the counterclockwise angle between the third link and the positive direction of the X axis, and the fourth link angle refers to the counterclockwise angle between the fourth link and the positive direction of the X axis. Based on the equal relationship between the sum of the first component of the second robotic arm in the X direction and the second component of the second robotic arm in the X direction and the force exerted on the second robotic arm in the X direction, the first force equation of the second robotic arm is established. The sine of the third link angle is taken as The product of the sine value of the fourth link angle and the force exerted by the third link on the second robotic arm is used as the first component force of the second robotic arm in the Y direction, and the product of the sine value of the fourth link angle and the force exerted by the fourth link on the second robotic arm is used as the second component force in the Y direction of the second robotic arm; based on the equal relationship between the sum of the first component force in the Y direction of the second robotic arm and the second component force in the Y direction of the second robotic arm and the force on the second robotic arm in the Y direction, a second force equation for the second robotic arm is established; the first force equation of the first robotic arm, the second force equation of the first robotic arm, the first force equation of the second robotic arm and the second force equation of the second robotic arm are matrix transformed to generate a second mapping matrix.

[0063] Refer to the following Figure 3 and Figure 4 Let's introduce the process of determining the second mapping matrix.

[0064] Figure 3 A schematic diagram of the mechanical relationship of the first five-bar linkage provided in an embodiment of the present application is shown.

[0065] like Figure 3 As shown, point O represents the origin position of the center of mass of the planar double five-bar linkage mechanism, point a represents the offset position of the center of mass of the planar double five-bar linkage mechanism, the current position of the first manipulator is point A, and the origin of the first manipulator is point O a , the length of the first connecting rod is L1, the length of the second connecting rod is L2, the motor shaft radius of the first rotary motor is R1, the motor shaft radius of the second rotary motor is R2, the rotation angle of the first rotary motor is θ1, the rotation angle of the second rotary motor is θ2, the first connecting rod angle is φ1, the second connecting rod angle is φ2, the distance between the first rotary motor and the origin of the first robotic arm in the X direction is X1, the distance between the second rotary motor and the origin of the first robotic arm in the X direction is X2, the distance between the first rotary motor and the origin of the first robotic arm in the Y direction is Y1, and the distance between the second rotary motor and the origin of the first robotic arm in the Y direction is Y2.

[0066] Depend on Figure 3 The force formula of the first robotic arm is:

[0067] F AX =F L1 cosφ1+F L2 cosφ2;

[0068] F AY =F L1 sinφ1+F L2 sinφ2;

[0069] In the above formula, F L1 Indicates the force exerted by the first link on the first robotic arm, F L2 Represents the force exerted by the second link on the first robotic arm.

[0070] Convert the force formula of the first manipulator into a matrix, and we can get:

[0071]

[0072] Figure 4 A schematic diagram of the mechanical relationship of the second five-bar linkage provided in an embodiment of the present application is shown.

[0073] like Figure 4 As shown, point O represents the origin position of the center of mass of the planar double five-bar linkage mechanism, point a represents the offset position of the center of mass of the planar double five-bar linkage mechanism, the current position of the second manipulator is point B, and the origin position of the second manipulator is point O b, the length of the third connecting rod is L3, the length of the fourth connecting rod is L4, the motor shaft radius of the third rotary motor is R3, the motor shaft radius of the fourth rotary motor is R4, the rotation angle of the third rotary motor is θ3, the rotation angle of the fourth rotary motor is θ4, the angle of the third connecting rod is φ3, the angle of the fourth connecting rod is φ4, the distance between the third rotary motor and the origin of the second robotic arm in the X direction is X3, the distance between the fourth rotary motor and the origin of the second robotic arm in the X direction is X4, the distance between the third rotary motor and the origin of the second robotic arm in the Y direction is Y3, and the distance between the fourth rotary motor and the origin of the second robotic arm in the Y direction is Y4.

[0074] F BX =F L3 cosφ3+F L4 cosφ4;

[0075] F BY =F L3 sinφ3+F L4 sinφ4;

[0076] In the above formula, F L3 Indicates the force exerted by the third link on the second robotic arm, F L4 Represents the force exerted by the fourth link on the second robotic arm.

[0077] Convert the force formula of the second manipulator into a matrix and we can get:

[0078]

[0079] The matrix converted from the force formula of the first manipulator and the matrix converted from the force formula of the second manipulator are combined to obtain:

[0080]

[0081] The above formula is expressed as: F m =TF L , where F L It represents the force matrix exerted by the connecting rod on the robot arm, F m Represents the force matrix of the first and second robotic arms in the planar double five-bar linkage.

[0082] F m =TF L Transform the form to get: F L =T -1 F m , then the second mapping matrix T can be determined -1 for:

[0083]

[0084] In the above formula, the calculation formulas for t1, t2, t3, t4, t5, t6, t7, and t8 are:

[0085]

[0086]

[0087]

[0088]

[0089] In an optional embodiment, the current position of the first robotic arm includes the X-axis coordinate of the first robotic arm and the Y-axis coordinate of the first robotic arm, and the current position of the second robotic arm includes the X-axis coordinate of the second robotic arm and the Y-axis coordinate of the second robotic arm; according to the planar mechanical relationship between the robotic arm and the crank, a second mapping matrix and an angle solution relationship are established, and it also includes: taking the Y-axis coordinate of the first robotic arm as the minuend, taking the product of the first rotation shaft radius and the sine value of the first rotation angle and the sum of the distance between the first rotating motor and the origin of the first robotic arm in the Y direction as the subtrahend, and taking the difference between the minuend and the subtrahend as the first longitudinal difference, the first rotation shaft radius is the rotation shaft radius of the first rotating motor, and the first rotation angle is the rotation angle of the first rotating motor; taking the ratio of the first longitudinal difference to the length of the first connecting rod as the first longitudinal ratio, based on the first longitudinal The first angle solution equation corresponding to the first link is established based on the equality between the ratio and the sine value of the first link angle; the X-axis coordinate of the first robotic arm is taken as the minuend, the product of the first rotation axis radius and the cosine value of the first rotation angle and the sum of the distance between the first rotating motor and the origin of the first robotic arm in the X direction are taken as the subtrahend, and the difference between the minuend and the subtrahend is taken as the first lateral difference; the ratio of the first lateral difference to the length of the first link is taken as the first lateral ratio, and the second angle solution equation corresponding to the first link is established based on the equality between the first lateral ratio and the cosine value of the first link angle; according to the above process, the first angle solution equation and the second angle solution equation corresponding to the second link, the third link and the fourth link are respectively established; the angle solution relationship is established from the eight angle solution equations corresponding to the four links.

[0090] Specifically, the coordinates of the current position of the first robotic arm are denoted as (x S1 ,y S1 ), the coordinates of the current position of the second robot are marked as (x S2 ,y S2 ).

[0091] According to the plane relationship, we can get:

[0092]

[0093]

[0094]

[0095]

[0096] In the above formula, the first longitudinal difference is: y S1 -R1sinθ1-Y1, the first lateral difference is: x S1 -R1cosθ1-X1.

[0097] The first longitudinal ratio is: The first lateral ratio is

[0098] The first angle solution equation is:

[0099] The second angle solution equation is:

[0100] The above eight angle solution equations are the angle solution relationships.

[0101] In an optional embodiment, a radial and tangential mechanical analysis is performed on the crank end to determine the motor torque calculation formula, including: calculating, for each rotating motor, the product of the connecting rod force corresponding to the rotating motor and the radius of the rotating motor, and calculating the product of the motor torque of the rotating motor and the cosine value of the corresponding crank-connecting rod angle, where the connecting rod force refers to the force of the connecting rod on the robotic arm, and the crank-connecting rod angle refers to the angle between the tangential force direction of the crank and the connecting rod; constructing a torque equation based on the equality between the two products; and determining, for each rotating motor, the motor torque calculation formula of the rotating motor based on the torque equation corresponding to the rotating motor.

[0102] Here, the radial force of the crank is provided by the bearing at the end of the crank, and the tangential force of the crank is provided by the rotating motor corresponding to the crank.

[0103] Refer to the following Figure 5 Let's introduce the crank torque calculation process.

[0104] Figure 5 A schematic diagram of the mechanical relationship between the first rotating motor, the first crank, and the first connecting rod provided in an embodiment of the present application is shown.

[0105] like Figure 5 As shown, angle δ1 is the first crank-connecting rod included angle, which is the angle between the tangential force direction of the first crank end and the first connecting rod.

[0106] Similarly, angle δ2 is the second crank-connecting rod angle, which is the angle between the tangential force direction of the second crank end and the second connecting rod. Angle δ3 is the third crank-connecting rod angle, which is the angle between the tangential force direction of the third crank end and the third connecting rod. Angle δ4 is the fourth crank-connecting rod angle, which is the angle between the tangential force direction of the fourth crank end and the fourth connecting rod.

[0107] According to the torque calculation formula, the torque of the first crank can be expressed as F L1 R1, which is one of the products. In addition, the torque of the first crank can also be expressed as τ1cosδ1, which is another product, where τ1 represents the motor torque of the first rotating motor. Combining the above two products, we can get the torque equation corresponding to the first crank:

[0108] τ1cosδ1=F L1 R1.

[0109] Similarly, the torque equations corresponding to the second crank, the third crank, and the fourth crank can be obtained respectively:

[0110] τ2cosδ2=F L2 R2;τ3cosδ3=F L3 R3;τ4cosδ4=F L4 R4.

[0111] In the above formula, τ2 represents the motor torque of the second rotating electric machine, τ3 represents the motor torque of the third rotating electric machine, and τ4 represents the motor torque of the fourth rotating electric machine.

[0112] At the same time, according to Figure 5 The relationship between the various angles in the equation can be obtained as follows:

[0113]

[0114]

[0115] Substituting the above angle calculation formula into the various torque equations, we can obtain:

[0116]

[0117]

[0118]

[0119]

[0120] The four calculation formulas obtained above are the motor torque calculation formulas.

[0121] Step S102 : obtaining the rotation angle of the rotary motor, and determining the current position of the robot arm based on the rotation angle, the motor shaft radius, and the crank length.

[0122] In this step, the rotating motor may refer to a torque motor.

[0123] The rotation angle of the rotating motor can be measured by an angle measuring device. For example, the angle measuring device can be a ring grating scale or a circular arc grating scale.

[0124] In an optional embodiment, the current position of the robot arm is determined based on the rotation angle, the motor shaft radius and the crank length, including: for each crank, for each crank, based on the coordinates of the rotating motor corresponding to the crank, the shaft radius and the rotation angle, determining the end coordinates of the crank; based on the end coordinates of each crank, determining the first crank line connecting rod angle, the second crank line connecting rod angle, the first double crank angle and the second double crank angle, the first crank line connecting rod angle is the angle between the line connecting the end of the first crank and the end of the second crank and the first connecting rod, the second crank line connecting rod angle is the angle between the end of the third connecting rod and the end of the fourth connecting rod The angle between the connecting line and the third connecting rod, the first double crank angle is the angle between the connecting line of the first crank end and the second crank end and the X-axis, and the second double crank angle is the angle between the connecting line of the third connecting rod end and the fourth connecting rod end and the X-axis; the sum of the first crank line connecting rod angle and the first double crank angle is used as the first connecting rod angle, and the sum of the second crank line connecting rod angle and the second double crank angle is used as the third connecting rod angle; based on the first crank coordinate, the first connecting rod length and the first connecting rod angle, the first robotic arm coordinate is determined; based on the third crank coordinate, the third connecting rod length and the third connecting rod angle, the second robotic arm coordinate is determined.

[0125] Specifically, the first crank connecting rod angle is recorded as ɑ1, the second crank connecting rod angle is recorded as ɑ1, the first double crank angle is recorded as β1, the second double crank angle is recorded as β2, and the coordinates of the first rotating motor are recorded as (x o1 ,y o1 ), the coordinates of the second rotating motor are denoted as (x o2 ,y o2 ), the coordinates of the third rotating motor are denoted as (x o3 ,y o3 ), the coordinates of the fourth rotating motor are denoted as (x o4 ,y o4 ), the first crankshaft end coordinate is expressed as (x a ,y a ), the coordinate of the second crankshaft end is expressed as (x b ,y b ), the third crankshaft end coordinate is expressed as (x c ,y c), the fourth crankshaft end coordinate is expressed as (x d ,y d ).

[0126] Among them, the first crank connecting rod angle, the second crank connecting rod angle, the first double crank angle and the second double crank angle are all positive in the counterclockwise direction.

[0127] Refer to the following Figure 6 and Figure 7 Let's introduce the relationship between the connecting rod angle, the crank line connecting rod angle and the double crank angle.

[0128] Figure 6 A schematic diagram showing the angular relationship between the first connecting rod angle, the first crank line connecting rod angle, and the first double crank angle provided in an embodiment of the present application is shown.

[0129] like Figure 6 As shown, the first connecting rod angle Φ1 can be calculated from the first crank connecting rod angle β1 and the first double crank angle ɑ1, that is, Φ1 = β1 + ɑ1. At the same time, according to the position relationship, we can get:

[0130] x a =R1cosθ1+x o1 ;y a =R1sinθ1+y o1 ;

[0131] x b =R2cosθ2+x o2 ;y b =R2sinθ2+y o2 ;

[0132]

[0133] Figure 7 A schematic diagram showing the angular relationship between the third connecting rod angle, the second crank line connecting rod angle, and the second double crank angle provided in an embodiment of the present application is shown.

[0134] like Figure 7 As shown, the third connecting rod angle Φ3 can be calculated from the second crank connecting rod angle β2 and the second double crank angle ɑ2, that is, Φ3 = β2 + ɑ2. At the same time, according to the position relationship, we can get:

[0135] x c =R3cosθ3+x o3 ;y c =R3sinθ3+y o3 ;

[0136] x d =R4cosθ4+x o4;y d =R4sinθ4+y o4 ;

[0137]

[0138] According to physical reality, ɑ1 and ɑ1 take positive values. β1 and β2 can be expressed as:

[0139]

[0140]

[0141] In this way, the coordinates of the first robotic arm and the second robotic arm in the X and Y directions can be expressed as:

[0142] x S1 =x a +L1×cosΦ1;

[0143] y S1 =y a +L1×sinΦ1;

[0144] x S2 =x c +L3×cosΦ3;

[0145] y S2 =y c +L3×sinΦ3.

[0146] Step S103 : determining the current position of the center of mass of the planar double five-bar linkage by using the current position of the robotic arm and the physical relationship between the robotic arm and the center of mass.

[0147] In this step, the current position of the center of mass of the planar double five-bar linkage mechanism changes with the position of the robotic arm. Therefore, the current position of the center of mass of the planar double five-bar linkage mechanism can be determined by the current position of the robotic arm and the physical relationship between the robotic arm and the center of mass.

[0148] In an optional embodiment, the current position of the center of mass of the planar double five-bar linkage is determined using the current position of the robotic arm and the physical relationship between the robotic arm and the center of mass, including: taking the weighted sum of the coordinates of the first robotic arm and the second robotic arm in the X direction as the X-axis coordinate of the center of mass position of the planar double five-bar linkage; taking the weighted sum of the coordinates of the first robotic arm and the second robotic arm in the Y direction as the Y-axis coordinate of the center of mass position of the planar double five-bar linkage; determining multiple center of mass position coefficients based on the distances in the X and Y directions between the robotic arm and the center of mass of the planar double five-bar linkage; calculating the product of each center of mass position coefficient and the corresponding robotic arm coordinate, and taking the sum of the multiple products as the rotation angle of the center of mass of the planar double five-bar linkage; and taking the X-axis coordinate, Y-axis coordinate and rotation angle of the center of mass position of the planar double five-bar linkage as the current position of the center of mass of the planar double five-bar linkage structure.

[0149] Specifically, the X-axis coordinate of the center of mass of the planar double five-link is marked as X, the Y-axis coordinate of the center of mass of the planar double five-link is marked as Y, and the weight is 0.5, then we can get: X = 0.5x S1 +0.5x S2 , Y=0.5y S1 +0.5y S2 .

[0150] The center of mass position coefficients include a first center of mass position coefficient, a second center of mass position coefficient, a third center of mass position coefficient, and a fourth center of mass position coefficient. The calculation formulas for the above four center of mass position coefficients are:

[0151]

[0152] In the above formula, the first centroid distance is recorded as L X , the second centroid distance is recorded as L Y , then the calculation formula for the rotation angle of the center of mass of the planar double five-bar linkage is:

[0153]

[0154] Convert the above three formulas into matrix form:

[0155]

[0156] Step S104 : determining a position error and a logical axial force of the center of mass of the planar double five-bar linkage mechanism corresponding to the position error based on the current position and the reference position of the center of mass of the planar double five-bar linkage mechanism.

[0157] In this step, the control system of the planar double five-bar linkage receives the input reference position and subtracts the current position from the reference position to obtain the position error. Based on the position error, the controller in the control system of the planar double five-bar linkage determines the logical axial force of the center of mass of the planar double five-bar linkage. The logical axial force includes FX 、F y 、T rz The force matrix F of the center of mass of the planar double five-bar linkage is formed by the logical axial force b .

[0158] In an optional embodiment, before determining the position error and the logical axial force of the center of mass of the planar double five-bar linkage mechanism corresponding to the position error based on the current position and reference position of the center of mass of the planar double five-bar linkage mechanism, it also includes: obtaining the reference position of the motion platform, taking the quotient of the mass of the planar double five-bar linkage mechanism and the mass of the motion platform as the position coefficient; and taking the product of the reference position of the motion platform and the position coefficient as the reference position.

[0159] Here, the mass of the planar double five-bar linkage is denoted as M1, and the mass of the motion platform is denoted as M2.

[0160] Specifically, the control system of the planar double five-bar linkage mechanism receives the reference position of the motion platform, and uses M1 / M2 as the position coefficient, and the product of the position coefficient and the reference position of the motion platform as the reference position of the planar double five-bar linkage mechanism. This can balance the impact of the motion platform on the basic frame. If the motion platform moves in the forward direction, the planar double five-bar linkage mechanism moves in the reverse direction.

[0161] In step S105 , the logical axis force and the current position of the robot arm are input into the mechanical decoupling matrix to determine the motor torque of the rotating motor, and the rotating motor is controlled according to the motor torque to drive the planar double five-bar linkage to operate.

[0162] In this step, after determining the logical axis force, multiply the logical axis force by the first mapping matrix to determine F m The value of F is used to determine the force of each manipulator. m Multiplying with the second mapping matrix can determine F L That is, determine the force exerted by the connecting rod on the robotic arm, input the force exerted by the connecting rod on the robotic arm and the calculated current position of the robotic arm into the motor torque calculation formula, and calculate the motor torque τ of each rotary motor. The rotary motor operates according to the motor torque to drive the planar double five-bar linkage to the specified reference position, thereby completing the precise control of the center of mass of the planar double five-bar linkage.

[0163] Compared with the control method of the planar double five-bar linkage in the prior art, the present application can use the planar double five-bar linkage to control the movement of the motion platform, and can perform multi-degree-of-freedom control on the motion platform. By performing a dynamic analysis on the planar double five-bar linkage, the mechanical decoupling matrix of the rotary motor controlling the center of mass of the planar double five-bar linkage is determined, so as to achieve the desired effect of accurately and quickly determining the motor torque of the rotary motor. At the same time, the current position of the manipulator arm and the current position of the center of mass of the planar double five-bar linkage are determined according to parameters such as the rotation angle of the rotary motor and the radius of the motor shaft. The planar double five-bar linkage is feedback-controlled using the determined current position of the center of mass of the planar double five-bar linkage and the feedback loop control strategy. This can effectively improve the large impact and strong residual vibration of the motion platform on the base frame during high-speed and high-acceleration motion, ensure the control accuracy requirements of the motion platform, and solve the problems of slow response speed, low control freedom and limited rotation angle travel.

[0164] Based on the same inventive concept, the embodiment of the present application also provides a control system for a planar double five-bar linkage mechanism corresponding to the control method of the planar double five-bar linkage mechanism. Since the principle of solving the problem by the system in the embodiment of the present application is similar to the control method of the planar double five-bar linkage mechanism in the embodiment of the present application, the implementation of the system can refer to the implementation of the method, and the repeated parts will not be repeated.

[0165] See also Figure 8 , Figure 8 This is a schematic diagram of the control system of a planar double five-link mechanism provided in an embodiment of the present application. Figure 8 As shown in , the control system 300 of the planar double five-bar linkage mechanism includes a comparison module 301, a controller 302, an actuator 303, a motor angle measurement module 304, and a position measurement module 305;

[0166] A comparison module 301 is configured to receive a reference position and a current position of the center of mass of the planar double five-bar linkage fed back by the position measurement module, and determine a position error based on the reference position and the current position;

[0167] The controller 302 is configured to receive the position error output by the comparison module and determine the logical axis force based on the position error;

[0168] An actuator 303 is configured to receive the logical axis force output by the controller and determine the motor torque of the rotating motor based on the logical axis force;

[0169] The motor angle measurement module 304 is used to obtain the current rotation angle of the rotating motor;

[0170] The position measurement module 305 is used to determine the current position of the center of mass of the planar double five-bar linkage mechanism based on the current rotation angle of the rotary motor, and feed the current position of the center of mass of the planar double five-bar linkage mechanism as feedback information to the comparison module.

[0171] In an embodiment of the present application, the actuator includes a mechanical decoupling matrix, which performs the calculation process from the logical shaft force to the motor torque. The position measurement module is used to determine the current position of the crank, the current position of the robotic arm, and the current position of the center of mass of the planar double five-bar linkage. The motor angle measurement module can be a ring grating ruler. The comparison module is also used to determine the reference position of the center of mass of the planar double five-bar linkage based on the reference position of the motion stage.

[0172] Those skilled in the art will clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0173] In the several embodiments provided in this application, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. The device embodiments described above are merely schematic. For example, the division of the units is merely a logical function division. There may be other division methods in actual implementation. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed may be through some communication interface, indirect coupling or communication connection of devices or units, which may be electrical, mechanical or other forms.

[0174] The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of these units may be selected to achieve the purpose of this embodiment according to actual needs.

[0175] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit.

[0176] If the functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a non-volatile computer-readable storage medium that is executable by a processor. Based on this understanding, the technical solution of the present application, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0177] Finally, it should be noted that the above-described embodiments are only specific implementation methods of the present application, which are used to illustrate the technical solutions of the present application, rather than to limit them. The scope of protection of the present application is not limited thereto. Although the present application has been described in detail with reference to the above-mentioned embodiments, those skilled in the art should understand that any person skilled in the art can modify or easily conceive of changes to the technical solutions described in the above-mentioned embodiments within the technical scope disclosed in the present application, or perform equivalent replacements for some of the technical features thereof. These modifications, changes, or replacements do not deviate from the spirit and scope of the technical solutions of the embodiments of the present application, and should be included in the scope of protection of the present application. Therefore, the scope of protection of the present application should be based on the scope of protection of the claims.

Claims

1. A control method for a planar double five-bar linkage, characterized in that: The planar double five-bar linkage includes a rotary motor, a robotic arm, and a crank, and the control method includes: Based on the dimensional parameters of the planar double five-bar linkage and the planar mechanical relationship between the robotic arm and the crank, a mechanical decoupling matrix from the logical axis to the physical axis of the center of mass of the planar double five-bar linkage is established; Obtaining a rotation angle of the rotating motor, and determining a current position of the robotic arm based on the rotation angle, a motor shaft radius, and a crank length; Determining the current position of the center of mass of the planar double five-bar linkage using the current position of the robotic arm and the physical relationship between the robotic arm and the center of mass; Determining a position error and a logical axial force of the center of mass of the planar double five-bar linkage mechanism corresponding to the position error based on a current position of the center of mass of the planar double five-bar linkage mechanism and a reference position; Inputting the logical axial force and the current position of the robotic arm into the mechanical decoupling matrix, determining the motor torque of the rotary motor, and controlling the rotary motor according to the motor torque, thereby driving the planar double five-bar linkage to operate; The planar double five-bar linkage includes a first five-bar linkage and a second five-bar linkage, wherein the first five-bar linkage includes a first rotary motor, a second rotary motor, a first robotic arm, a first crank, a second crank, a first connecting rod and a second connecting rod, and the second five-bar linkage includes a third rotary motor, a fourth rotary motor, a second robotic arm, a third crank, a fourth crank, a third connecting rod and a fourth connecting rod; The mechanical decoupling matrix from the logical axis to the physical axis of the center of mass of the planar double five-bar linkage mechanism is established based on the dimensional parameters of the planar double five-bar linkage mechanism and the planar mechanical relationship between the robot arm and the crank, including: Establishing a first mapping matrix according to the size parameters of the planar double five-bar linkage mechanism, wherein the first mapping matrix refers to a mechanical mapping matrix between the center of mass of the planar double five-bar linkage mechanism and the robotic arm; Establishing a second mapping matrix and an angle solution relationship based on a planar mechanical relationship between the robotic arm and the crank, wherein the second mapping matrix refers to a mechanical mapping matrix between the robotic arm and the connecting rod, and the angle solution relationship refers to a solution relationship between the connecting rod angle and the rotation angle of the rotary motor; Perform radial and tangential mechanical analysis on the crank end to determine the motor torque calculation formula; A mechanical decoupling matrix is ​​established based on the first mapping matrix, the second mapping matrix, the angle solution relationship and the motor torque calculation formula.

2. The control method according to claim 1, characterized in that: The step of establishing a first mapping matrix according to the size parameters of the planar double five-bar linkage comprises: Based on the equality relationship between the sum of the forces acting on the first and second robotic arms in the X direction and the force acting on the center of mass of the planar double five-bar linkage in the X direction, a first center of mass force equation is established; Based on the equality relationship between the sum of the forces acting on the first and second robotic arms in the Y direction and the force acting on the center of mass of the planar double five-bar linkage in the Y direction, a second center of mass force equation is established; The product of the difference between the forces on the second robotic arm and the first robotic arm in the Y direction and the first mass center distance is used as the first mass center torque, where the first mass center distance refers to the distance between the origin of the first robotic arm or the origin of the second robotic arm and the mass center origin of the planar double five-bar linkage in the X direction; The product of the difference in force between the second robotic arm and the first robotic arm in the X direction and the second mass center distance is used as the second mass center torque, where the second mass center distance refers to the distance between the origin of the first robotic arm or the origin of the second robotic arm and the mass center origin of the planar double five-bar linkage in the Y direction; Constructing a center-of-mass torque equation based on an equality relationship between the sum of the first center-of-mass torque and the second center-of-mass torque and the center-of-mass torque of the planar double five-bar linkage; Performing matrix transformation on the first center-of-mass force equation, the second center-of-mass force equation, and the center-of-mass torque equation to generate a first mapping matrix.

3. The control method according to claim 1, wherein: The second mapping matrix and the angle calculation relationship are established according to the planar mechanical relationship between the robot arm and the crank, including: The product of the cosine value of the first link angle and the force exerted by the first link on the first robotic arm is used as the first component force of the first robotic arm in the X direction, and the product of the cosine value of the second link angle and the force exerted by the second link on the first robotic arm is used as the second component force of the first robotic arm in the X direction. The first link angle refers to the counterclockwise angle between the first link and the positive direction of the X axis, and the second link angle refers to the counterclockwise angle between the second link and the positive direction of the X axis. Establishing a first force equation for the first robotic arm based on an equality relationship between the sum of the first component force of the first robotic arm in the X direction and the second component force of the first robotic arm in the X direction and the force applied to the first robotic arm in the X direction; The product of the sine value of the first link angle and the force exerted by the first link on the first robotic arm is used as the first component force of the first robotic arm in the Y direction, and the product of the sine value of the second link angle and the force exerted by the second link on the first robotic arm is used as the second component force of the first robotic arm in the Y direction; Establishing a second force equation for the first robotic arm based on an equality relationship between the sum of the first component force of the first robotic arm in the Y direction and the second component force of the first robotic arm in the Y direction and the force applied to the first robotic arm in the Y direction; The product of the cosine value of the third link angle and the force exerted by the third link on the second robotic arm is used as the first component force of the second robotic arm in the X direction, and the product of the cosine value of the fourth link angle and the force exerted by the fourth link on the second robotic arm is used as the second component force of the second robotic arm in the X direction. The third link angle refers to the counterclockwise angle between the third link and the positive direction of the X axis, and the fourth link angle refers to the counterclockwise angle between the fourth link and the positive direction of the X axis. Establishing a first force equation for the second robotic arm based on an equality relationship between the sum of the first component force of the second robotic arm in the X direction, the second component force of the second robotic arm in the X direction, and the force applied to the second robotic arm in the X direction; The product of the sine value of the third link angle and the force exerted by the third link on the second robotic arm is used as the first component force of the second robotic arm in the Y direction, and the product of the sine value of the fourth link angle and the force exerted by the fourth link on the second robotic arm is used as the second component force of the second robotic arm in the Y direction; Establishing a second force equation for the second robotic arm based on an equality relationship between the sum of the first component force of the second robotic arm in the Y direction and the second component force of the second robotic arm in the Y direction and the force applied to the second robotic arm in the Y direction; A first force equation of the first robotic arm, a second force equation of the first robotic arm, a first force equation of the second robotic arm, and a second force equation of the second robotic arm are transformed into a matrix to generate a second mapping matrix.

4. The control method according to claim 1, wherein: The current position of the first robotic arm includes the first robotic arm X-axis coordinate and the first robotic arm Y-axis coordinate, and the current position of the second robotic arm includes the second robotic arm X-axis coordinate and the second robotic arm Y-axis coordinate; The step of establishing a second mapping matrix and an angle calculation relationship based on the planar mechanical relationship between the robotic arm and the crank further includes: The Y-axis coordinate of the first robotic arm is used as a minuend, the product of the first rotation axis radius and the sine value of the first rotation angle and the sum of the distance between the first rotary motor and the origin of the first robotic arm in the Y direction is used as a subtrahend, and the difference between the minuend and the subtrahend is used as a first longitudinal difference, the first rotation axis radius is the rotation axis radius of the first rotary motor, and the first rotation angle is the rotation angle of the first rotary motor; Taking the ratio of the first longitudinal difference to the length of the first connecting rod as a first longitudinal ratio, and establishing a first angle solution equation corresponding to the first connecting rod based on the equality between the first longitudinal ratio and the sine value of the first connecting rod angle; The X-axis coordinate of the first robotic arm is used as a minuend, the product of the first rotation axis radius and the cosine value of the first rotation angle and the sum of the distance between the first rotary motor and the origin of the first robotic arm in the X direction are used as a subtrahend, and the difference between the minuend and the subtrahend is used as a first lateral difference; Taking the ratio of the first lateral difference to the length of the first connecting rod as a first lateral ratio, and establishing a second angle solution equation corresponding to the first connecting rod based on the equality between the first lateral ratio and the cosine value of the first connecting rod angle; According to the above process, the first angle solution equation and the second angle solution equation corresponding to the second connecting rod, the third connecting rod and the fourth connecting rod are respectively established; The angle solution relationship is established by the eight angle solution equations corresponding to the four connecting rods.

5. The control method according to claim 1, characterized in that: The radial and tangential mechanical analysis of the crank end is performed to determine the motor torque calculation formula, including: For each rotating motor, calculate the product of the connecting rod force corresponding to the rotating motor and the rotation shaft radius of the rotating motor, and calculate the product of the motor torque of the rotating motor and the cosine value of the corresponding crank-connecting rod angle. The connecting rod force refers to the force applied by the connecting rod to the robotic arm, and the crank-connecting rod angle refers to the angle between the tangential force direction of the crank and the connecting rod. Construct the torque equation based on the equality between the two products; For each rotating electrical machine, a motor torque calculation formula of the rotating electrical machine is determined based on a torque equation corresponding to the rotating electrical machine.

6. The control method according to claim 1, characterized in that: The determining the current position of the robotic arm based on the rotation angle, the motor shaft radius and the crank length includes: For each crank, determine the end coordinates of the crank based on the coordinates, shaft radius, and rotation angle of the rotating motor corresponding to the crank; Determine a first crank-line connecting rod angle, a second crank-line connecting rod angle, a first double crank angle, and a second double crank angle based on the end coordinates of each crank, wherein the first crank-line connecting rod angle is the angle between the line connecting the first crank end and the second crank end and the first connecting rod, the second crank-line connecting rod angle is the angle between the line connecting the third connecting rod end and the fourth connecting rod end and the third connecting rod, the first double crank angle is the angle between the line connecting the first crank end and the second crank end and the X-axis, and the second double crank angle is the angle between the line connecting the third connecting rod end and the fourth connecting rod end and the X-axis; The sum of the connecting rod angle of the first crank line and the first double crank angle is used as the first connecting rod angle, and the sum of the connecting rod angle of the second crank line and the second double crank angle is used as the third connecting rod angle; Determining a first robotic arm coordinate based on the first crank coordinate, the first connecting rod length, and the first connecting rod angle; The second robot arm coordinates are determined based on the third crank coordinate, the third connecting rod length, and the third connecting rod angle.

7. The control method according to claim 1, characterized in that: Determining the current position of the center of mass of the planar double five-bar linkage by utilizing the current position of the robotic arm and the physical relationship between the robotic arm and the center of mass includes: The weights of the coordinates of the first and second robotic arms in the X direction are used as the X-axis coordinate of the center of mass of the planar double five-link; The weights of the coordinates of the first and second robotic arms in the Y direction are used as the Y-axis coordinate of the center of mass of the planar double five-link; Determining a plurality of center-of-mass position coefficients based on the distances in the X and Y directions between the robot arm and the center of mass of the planar double five-bar linkage; Calculate the product of each center of mass position coefficient and the corresponding robot arm coordinate, and use the sum of multiple products as the rotation angle of the center of mass of the planar double five-bar linkage; The X-axis coordinate, Y-axis coordinate and rotation angle of the center of mass of the planar double five-bar linkage are used as the current position of the center of mass of the planar double five-bar linkage.

8. The control method according to claim 1, characterized in that: Before determining the position error and the logical axial force of the center of mass of the planar double five-bar linkage mechanism corresponding to the position error based on the current position and the reference position of the center of mass of the planar double five-bar linkage mechanism, the method further includes: Obtain the reference position of the motion platform, and use the quotient of the mass of the planar double five-bar linkage mechanism and the mass of the motion platform as the position coefficient; The product of the motion stage reference position and the position coefficient is used as the reference position.

9. A control system for a planar double five-bar linkage, characterized in that: The control system includes a comparison module, a controller, an actuator, a motor angle measurement module, and a position measurement module; The comparison module is configured to receive a reference position and a current position of the center of mass of the planar double five-bar linkage fed back by the position measurement module, and determine a position error based on the reference position and the current position; The controller is configured to receive the position error output by the comparison module and determine a logical axis force based on the position error; The actuator is configured to receive the logical axis force output by the controller and determine the motor torque of the rotating motor based on the logical axis force; The motor angle measurement module is used to obtain the current rotation angle of the rotating motor; The position measurement module is used to determine the current position of the center of mass of the planar double five-bar linkage based on the current rotation angle of the rotating motor, and feed the current position of the center of mass of the planar double five-bar linkage as feedback information to the comparison module; The planar double five-bar linkage includes a first five-bar linkage and a second five-bar linkage, wherein the first five-bar linkage includes a first rotary motor, a second rotary motor, a first robotic arm, a first crank, a second crank, a first connecting rod and a second connecting rod, and the second five-bar linkage includes a third rotary motor, a fourth rotary motor, a second robotic arm, a third crank, a fourth crank, a third connecting rod and a fourth connecting rod; The controller is further configured to: Establishing a first mapping matrix according to the size parameters of the planar double five-bar linkage mechanism, wherein the first mapping matrix refers to a mechanical mapping matrix between the center of mass of the planar double five-bar linkage mechanism and the robotic arm; Establishing a second mapping matrix and an angle solution relationship based on a planar mechanical relationship between the robotic arm and the crank, wherein the second mapping matrix refers to a mechanical mapping matrix between the robotic arm and the connecting rod, and the angle solution relationship refers to a solution relationship between the connecting rod angle and the rotation angle of the rotary motor; Perform radial and tangential mechanical analysis on the crank end to determine the motor torque calculation formula; A mechanical decoupling matrix is ​​established based on the first mapping matrix, the second mapping matrix, the angle solution relationship and the motor torque calculation formula.

Images