Supporting platform leveling control method, device and equipment and computer readable medium

Abstract

The invention discloses a supporting platform leveling control method, device and equipment and a computer readable medium. The method comprises the following steps: calculating the position adjustment amount of each supporting leg for leveling the supporting platform by utilizing a D-H coordinate method; trajectory planning is conducted on the position adjustment amount of each supporting leg according to a smooth and symmetrical speed curve, and the expected position trajectory of each supporting leg is obtained; calculating a position tracking error of each supporting leg according to the expected position track of each supporting leg, and calculating an adjacent cross coupling error of each supporting leg according to the position tracking error; and calculating the position compensation amount of each supporting leg according to the position tracking error, and calculating the synchronous compensation amount of each supporting leg according to the adjacent cross coupling error. According to the leveling control method of the supporting platform, the adjacent cross coupling synchronous control principle is adopted, the virtual leg problem can be solved, the leveling efficiency is high, the anti-interference performance is high, and meanwhile the leveling process has the advantages of being stable and smooth, free of impact and high in precision.

Description

Technical Field

[0001] The present invention relates to the field of motion control technology, and in particular to a support platform leveling control method, device, equipment and computer-readable medium. Background Art

[0002] With the development of electronic technology and control theory, various advanced leveling algorithms and strategies are gradually applied to leveling systems. In order to ensure the leveling speed and accuracy, the existing leveling strategy often adopts the method of coarse and fine adjustment, that is, when the adjustment amount of the outrigger is large, a faster speed is used; when the leveling deviation is small, a smaller speed is used. However, the speed change of the above process is a step change, and the outrigger trajectory is not planned and designed, which is prone to unstable impact during the leveling process. In addition, the single-axis step-by-step action method commonly used in the existing leveling method is prone to the "virtual leg" problem, and there is a lack of coupling between the axes, resulting in poor correlation, delayed response, weak anti-interference ability, and low leveling efficiency. Summary of the invention

[0003] In order to solve at least one technical problem existing in the above-mentioned prior art, the embodiments of the present invention provide a support platform leveling control method, device, equipment and computer readable medium. The technical solution is as follows:

[0004] In a first aspect, a support platform leveling control method is provided, the method comprising:

[0005] Use the DH coordinate method to calculate the position adjustment amount of each leg to level the support platform;

[0006] Performing trajectory planning for the position adjustment amount of each leg according to a smooth and symmetrical speed curve to obtain a desired position trajectory of each leg;

[0007] Calculating a position tracking error of each leg according to a desired position trajectory of each leg, and calculating an adjacent cross-coupling error of each leg according to the position tracking error;

[0008] The position compensation amount of each leg is calculated according to the position tracking error, and the synchronization compensation amount of each leg is calculated according to the adjacent cross-coupling error.

[0009] Furthermore, performing trajectory planning for the position adjustment amount of each leg according to the smooth and symmetrical speed curve to obtain the desired position trajectory of each leg includes:

[0010] Calculate the relationship between the position and time of each leg according to the smooth symmetrical speed curve and the preset motion limit;

[0011] Calculating a synchronization proportional coefficient of each leg according to the position adjustment amount and the total displacement of each leg;

[0012] The expected position trajectory of each leg is calculated according to the synchronization proportional coefficient and the changing relationship between the position of each leg and time.

[0013] Further, the calculating the position tracking error of each leg according to the expected position trajectory of each leg, and calculating the adjacent cross-coupling error of each leg according to the position tracking error, comprises:

[0014] Obtaining the actual position trajectory of each leg during the leveling process of the support platform according to the expected position trajectory;

[0015] Calculate the difference between the expected position trajectory and the actual position trajectory of each leg as the position tracking error of each leg;

[0016] The synchronization error of each leg is calculated according to the position tracking error, and the adjacent cross-coupling error of each leg is calculated according to the synchronization error.

[0017] Further, the synchronization error is the difference between the position tracking error of a leg and the position tracking error of the next adjacent numbered leg;

[0018] The adjacent cross-coupling error is the difference between the synchronization error of a leg and the synchronization error of its last adjacent numbered leg, wherein the adjacent cross-coupling error of the leg with the smallest number is the difference between the synchronization error of the leg with the smallest number and the synchronization error of the leg with the largest number.

[0019] Further, the calculating the position compensation amount of each leg according to the position tracking error includes:

[0020] The position compensation is the product of the position control proportional coefficient and the position tracking error:

[0021] U s (k) = K s_p e s (k)

[0022] Among them, U s (k) is the position compensation, K s_p is the position control proportional control coefficient, e s (k) is the position controller input error.

[0023] Further, the calculating the synchronous compensation amount of each leg according to the adjacent cross-coupling errors includes:

[0024] The synchronous compensation amount is calculated according to the following formula:

[0025]

[0026] Among them, U c (k) is the synchronous compensation amount, K c_pis the synchronous control proportional coefficient, K c_i is the synchronous control integral coefficient e c (k) is the synchronous controller input error, e c (i) is the i-th input error.

[0027] Furthermore, the calculation of the position adjustment amount of each leg of the support platform for leveling the support platform by using the DH coordinate method includes:

[0028] Establish the inertial coordinate system and object coordinate system of the support platform;

[0029] Calculate the coordinates of the support points of each leg of the support platform in the inertial coordinate system according to the DH coordinate method;

[0030] The position adjustment amount of each leg is calculated based on the highest point chasing principle.

[0031] In a second aspect, a support platform leveling control device is provided, the device comprising:

[0032] A position adjustment amount calculation module, used to calculate the position adjustment amount of each leg of the support platform for leveling the support platform using the DH coordinate method;

[0033] A trajectory planning module is used to plan the trajectory of the position adjustment of each leg according to a smooth and symmetrical speed curve to obtain the desired position trajectory of each leg;

[0034] A synchronous error coupling module is used to calculate the position tracking error of each leg according to the expected position trajectory of each leg, and calculate the adjacent cross-coupling error of each leg according to the position tracking error;

[0035] A position compensation amount calculation module is used to calculate the position compensation amount of each leg according to the position tracking error;

[0036] The synchronous compensation amount calculation module is used to calculate the synchronous compensation amount of each leg according to adjacent cross-coupling errors.

[0037] Furthermore, the trajectory planning module includes:

[0038] A smooth and symmetrical speed curve calculation module is used to calculate the relationship between the position and time change of each leg according to the smooth and symmetrical speed curve and the preset motion limit;

[0039] A synchronization proportional coefficient calculation module, used to calculate the synchronization proportional coefficient of each leg according to the position adjustment amount and the total displacement of each leg;

[0040] The expected trajectory calculation module is used to calculate the expected position trajectory of each leg according to the synchronization proportional coefficient and the relationship between the position and time of each leg.

[0041] Furthermore, the synchronous error coupling module includes:

[0042] An actual position trajectory acquisition module is used to obtain the actual position trajectory of each leg during the leveling process of the support platform according to the expected position trajectory;

[0043] A position tracking error calculation module is used to calculate the difference between the expected position trajectory and the actual position trajectory of each leg;

[0044] A synchronization error calculation module, used to calculate the synchronization error of each leg according to the position tracking error;

[0045] The adjacent cross-coupling error calculation module is used to calculate the adjacent cross-coupling error of each leg according to the synchronization error.

[0046] Furthermore, the synchronization error calculation module is specifically used for:

[0047] Calculate the difference between the position tracking error of a leg and the position tracking error of the next adjacent numbered leg as a synchronization error;

[0048] The difference between the synchronization error of a leg and the synchronization error of the leg with the previous adjacent number is taken as the adjacent cross-coupling error, wherein the adjacent cross-coupling error of the leg with the smallest number is the difference between the synchronization error of the leg with the smallest number and the synchronization error of the leg with the largest number.

[0049] Furthermore, the position compensation amount calculation module is specifically used to calculate the position compensation amount of each leg according to the following formula:

[0050] U s (k) = K s_p e s (k)

[0051] Among them, U s (k) is the position compensation, K s_p is the position control proportional control coefficient, e s (k) is the position controller input error.

[0052] Furthermore, the synchronous compensation amount calculation module is specifically used to calculate the synchronous compensation amount of each leg according to the following formula:

[0053]

[0054] Among them, U c (k) is the synchronous compensation amount, K c_p is the synchronous control proportional coefficient, K c_i is the synchronous control integral coefficient e c (k) is the synchronous controller input error, e c (i) is the i-th input error.

[0055] Furthermore, the position adjustment amount calculation module includes:

[0056] A coordinate system establishment module is used to establish the inertial coordinate system and object coordinate system of the support platform;

[0057] A coordinate conversion module, used to calculate the coordinates of the support points of each leg of the support platform in the inertial coordinate system according to the DH coordinate method;

[0058] The adjustment amount calculation module is used to calculate the position adjustment amount of each leg based on the highest point chasing principle.

[0059] In a third aspect, an electronic device is provided, including:

[0060] one or more processors; and

[0061] A memory associated with the one or more processors, the memory being used to store program instructions, wherein the program instructions, when read and executed by the one or more processors, execute any method described in the first aspect.

[0062] According to a fourth aspect, a computer-readable medium is provided, on which a computer program is stored, wherein when the program is executed by a processor, the method described in any one of the first aspects is implemented.

[0063] The technical solution provided by the embodiment of the present invention has the following beneficial effects:

[0064] (1) The leveling control method of the support platform proposed in the present invention adopts the adjacent cross-coupling synchronous control principle, and synchronizes the posture adjustment according to a certain proportional relationship, which can solve the virtual leg problem and has high leveling efficiency and strong anti-interference ability.

[0065] (2) The leveling control method of the support platform proposed in the present invention carries out trajectory planning and design for the multi-leg posture adjustment process, and has the advantages of a stable and smooth leveling process without impact and high precision. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0067] Figure 1 A schematic diagram of a support platform leveling control method disclosed in an example of the present invention;

[0068] Figure 2The v(t) curve for adjusting the outrigger trajectory planning disclosed in the example of the present invention;

[0069] Figure 3 The a(t) curve for adjusting the outrigger trajectory planning disclosed in the example of the present invention;

[0070] Figure 4 A synchronous controller and a synchronous coupler model disclosed in an example of the present invention;

[0071] Figure 5 To simulate the displacement curve of leveling of a four-point support platform - no synchronization controller was added;

[0072] Figure 6 Added a synchronous controller to simulate the displacement curve of the four-point support platform leveling;

[0073] Figure 7 The invention discloses an example of a method for disposing the outriggers during the leveling process.

[0074] Figure 8 The examples disclosed in the present invention Support platform Schematic diagram of the leveling control device;

[0075] Fig. 9 Computer readable media disclosed for examples of the present invention. DETAILED DESCRIPTION

[0076] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution in the embodiment of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiment of the present invention. Obviously, the described embodiment is only a part of the embodiment of the present invention, not all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0077] A support platform leveling control method, device, equipment and computer readable medium, the specific technical solution is as follows:

[0078] like Figure 1 As shown, a support platform leveling control method includes:

[0079] S1. Use the DH coordinate method to calculate the position adjustment amount of each leg to level the support platform.

[0080] In the above, the support platform is a support platform with multiple legs, such as a four-point support platform. The DH coordinate method is a The robot kinematic modeling method describes the relative relationship between the robot joints by defining the link parameters and coordinate system transformation. Position and posture relationship. The position adjustment amount is expressed as the distance that each leg of the support platform needs to be extended or shortened to level the support platform. In some cases, the support legs of the supporting platform can be extended by adjusting one or more legs to a certain distance. In other cases, the support platform can be shortened by adjusting one or more legs to make the support platform The pivot points of the legs are located in the same plane; in other cases, one or more legs can be extended and one or more legs can be adjusted. The supporting points of the legs of the shortened support platform are located in the same plane. Install the inclination sensor, which can measure the inclination of the platform in the coordinate system and calculate the leveling (inclination) The amount of position adjustment required for the outrigger when the angle approaches 0.

[0081] In one embodiment, step S1 includes:

[0082] Establish the inertial coordinate system and object coordinate system of the support platform;

[0083] Calculate the coordinates of the supporting points of each leg of the supporting platform in the inertial coordinate system according to the DH coordinate method;

[0084] The position adjustment of each leg is calculated based on the highest point chasing principle.

[0085] Above, The object coordinate system is bound to the object. When the object moves or rotates, the object coordinate system changes in the same way. The origin of the inertial coordinate system is the same as the origin of the object coordinate system. The points coincide and do not move synchronously with the object. The conversion from the object coordinate system to the inertial coordinate system only requires a certain angle of rotation. The highest point pursuit principle is expressed as: Based on the highest point pursuit principle, the highest point is fixed and the postures of the other legs are adjusted to The extension mode enables the support platform to be leveled.

[0086] Take the four-point support platform as an example. Take the support point of the adjustment leg 4 and the four-point support platform as the origin O of the space coordinate system, and establish the coordinate system {0} and the coordinate system {1} respectively. Among them, the coordinate system {0} is the inertial coordinate system O-xyz, and the coordinate system {1} is the object coordinate system Ox′y′z′. The inertial coordinate system O-xyz serves as the reference plane for leveling, and its x-axis and y-axis are parallel to the world coordinate system. The object coordinate system Ox′y′z′ is associated with the four-point support platform and rotates with it.

[0087] According to the DH coordinate method, the object coordinate system can be rotated to the inertial coordinate system, and the four-point support platform rotates around the x-axis with the origin O as the center by an offset angle θ in the x direction. x , rotate around the y axis in the y direction by an angle θ y The reference plane can be reached to the leveling state, and the rotation matrix R can be derived based on the matrix. x is the rotation matrix around the x-axis, R y is the rotation matrix around the y-axis, the matrix is ​​expressed as follows:

[0088]

[0089] The rotation matrix R can be calculated according to R x and R y The calculation formula is as follows:

[0090] R=RR

[0091] R=R x R y

[0092]

[0093] The coordinates of the four support points P1, P2, P3, and P4 in the coordinate system {1} can be expressed as follows 1 P i =( 1 X i 1 Y i 1 Z i ) T , the coordinates in the coordinate system {0} can be expressed as follows 0 P i =( 0 X i 0 Y i 0 Z i ) T , where (i is the support point number, 1, 2, 3, 4) According to the DH coordinate method, the transformation relationship between the four vertices in the coordinate system {1} and the coordinate system {0} is as follows:

[0094] 0 P i =R 1 P i

[0095] The details are as follows:

[0096]

[0097] Get the coordinate values ​​of each support point in the coordinate system {0} 0 Z i :

[0098] 0 Z i = -cosθ y sinθ x 1 X i +sinθ y 1 Y i +cosθ x cosθ y 1 Z i

[0099] The specific four support points are in the coordinate system {0} 0 Z i They are as follows:

[0100] 0 Z1=-acosθ y sinθ x

[0101] 0Z2=-acosθ x sinθ y +bsinθ y

[0102] 0 Z3=bsinθ y

[0103] 0 Z4=0

[0104] Based on the highest point pursuit principle, the highest point is fixed and the postures of the other three legs are adjusted to be extended. Each leg is compensated by the offset value z0. After compensation, the extension of the adjusted leg of the highest support point is 0. The offset value z0 is as follows:

[0105] z0=-|{ 0 Z1, 0 Z2, 0 Z3, 0 Z4} max |

[0106] After compensation of the offset z0, the adjustment amount ΔZ of the adjustment leg i is obtained i (i = 1, 2, 3, 4), where ΔZ i The negative sign indicates that the adjustment leg is extended, and the position adjustment amount of each adjustment leg is as follows:

[0107] ΔZ1=-acosθ y sinθ x +z0

[0108] ΔZ2=-acosθ x sinθ y +bsinθ y +z0

[0109] ΔZ3=bsinθ y +z0

[0110] ΔZ4=z0

[0111] S2. Plan the trajectory of the position adjustment of each leg according to the smooth and symmetrical speed curve to obtain the expected position trajectory of each leg.

[0112] As mentioned above, trajectory planning means designing the changes in speed and / or acceleration of each leg during the movement of the position adjustment amount, so that each leg meets the speed and / or acceleration restrictions during the movement and makes the change process smooth and imperceptible.

[0113] Step S2 includes:

[0114] Calculate the relationship between the position and time of each leg according to the smooth symmetrical speed curve and the preset motion limit;

[0115] Calculate the synchronization proportional coefficient of each leg according to the position adjustment amount and the total displacement of each leg;

[0116] The expected position trajectory of each leg is calculated based on the synchronization proportional coefficient and the relationship between the position and time of each leg.

[0117] In the above, a smooth and symmetrical speed curve is used to plan the trajectory of each leg position adjustment. v(t) is the speed change function of the leg adjusted with the movement time t, as follows:

[0118]

[0119] Among them, v max is the maximum speed of the outrigger, a max is the absolute value of the maximum acceleration in the acceleration phase and the maximum deceleration in the deceleration phase (i.e., a max >0), t s is the total running time. The v(t) curve is as follows Figure 2 As shown, the acceleration curve a(t) can be obtained by differentiating the velocity curve v(t), as Figure 3 shown.

[0120] With t s Taking the time / 2 as the dividing point as an example, the leg adjustment movement process can be divided into two sections. s / 2 is the variable acceleration-constant speed section, and the position adjustment amount is S a When t s / 2≤t≤t s For the uniform speed-variable deceleration stage, the position adjustment amount is related to S a same.

[0121] By integrating the v(t) curve, we can get the equation S(t) for adjusting the position of the leg with respect to time as follows:

[0122]

[0123] Adjust the total displacement of the outriggers during the movement process to 2S a The legs are adjusted to adjust their positions according to the trajectory planning process, and the motion process is smooth and stable, and meets the requirements of the leveling system for time, maximum speed and maximum acceleration.

[0124] During the leveling process, the i-th adjustment leg is synchronized with the proportional coefficient λ i , the adjustment position required to complete the synchronous motion, the synchronous proportional coefficient λ i It can be expressed as follows:

[0125]

[0126] Adjust the desired position trajectory S of leg i i The equation of (t) versus time is as follows:

[0127] S i (t) = λ i S(t)

[0128] S3. Calculate the position tracking error of each leg according to the expected position trajectory of each leg, and calculate the adjacent cross-coupling error of each leg according to the position tracking error.

[0129] As described above, based on the principle of adjacent cross-coupling control structure, the states of each leg and the adjacent legs are considered, and the difference between the feedback signal of the controlled leg and the feedback signal of the adjacent leg is coupled into the compensation signal of the leg. The coupling between the legs is strong and the synchronization performance is good.

[0130] Step S3 includes:

[0131] Obtaining the actual position trajectory of each leg during the leveling process of the support platform according to the expected position trajectory;

[0132] The difference between the expected position trajectory and the actual position trajectory of each leg is calculated as the position tracking error of each leg;

[0133] The synchronization error of each leg is calculated according to the position tracking error, and the adjacent cross-coupling error of each leg is calculated according to the synchronization error.

[0134] As mentioned above, the adjacent cross-coupling error e of the i-th axis can be obtained based on the synchronous coupling error * i It includes the synchronization errors of the i-th axis and the i-1 axis. According to the definition of adjacent cross coupling, each adjustment leg can be designed with a synchronization controller to calculate the synchronization error and control multiple adjustment legs to move synchronously according to a certain proportional coefficient. For example, based on the adjacent cross coupling principle, the four-point support platform synchronization controller and synchronization coupler model are as follows: Figure 4 As shown, S fi To adjust the true feedback position signal of leg i, ω T i (t) is the output of the position controller for adjusting leg i, ω S i (t) is the synchronous controller output for adjusting leg i.

[0135] Further, the synchronization error is the difference between the position tracking error of one leg and the position tracking error of its next adjacent numbered leg;

[0136] The adjacent cross-coupling error is the difference between the synchronization error of a leg and the synchronization error of its last adjacent numbered leg, wherein the adjacent cross-coupling error of the leg with the smallest number is the difference between the synchronization error of the leg with the smallest number and the synchronization error of the leg with the largest number.

[0137] As mentioned above, for a synchronous control system with n legs, the legs are numbered sequentially in advance, and the position tracking error e of the i-th axis is defined as i (t) is the command input value S of the leg i (t) and actual feedback value S f The difference in (t) is expressed as follows:

[0138] e i (t) = S i (t)-S f (t)

[0139] The synchronization error of each leg can be defined as the difference between the following error of the axis and the following error of the next leg, taking into account the synchronization ratio coefficient λ between the adjustment legs of the leveling platform i , then the synchronization error ε between the adjustment legs i The specific expressions are as follows:

[0140]

[0141] To ensure that each leg follows the error e i (t)→0, if we want to ensure that the adjustment legs are synchronized in proportion, we also need to make ε i →0. According to the synchronization error ε between each leg i It can be seen that the control of the adjustment leg i needs to consider the synchronization of the two adjacent adjustment legs, so that the synchronization error ε i-1 →0 and ε i → 0. Define the adjacent cross-coupling error of leg i The details are as follows:

[0142]

[0143] ε i Bring in The synchronous error coupler for adjusting leg i is obtained as follows:

[0144]

[0145] S4. Calculate the position compensation amount of each leg according to the position tracking error, and calculate the synchronization compensation amount of each leg according to the adjacent cross-coupling error.

[0146] The above, for example, Figure 4As shown in the figure, the control module of each adjustment leg includes a position controller, a synchronous controller, a synchronous error coupler, and a driver. The position controller is used to improve the responsiveness of the single axis of the adjustment leg to the planned position command and reduce the position following error; the synchronous controller is responsible for reducing the synchronization error between the axes; the synchronous error coupler converts the following error e i The following error between the two adjacent legs is e i-1 and e i+1 After coupling calculation, the adjacent coupling error e is obtained. * i , as the input of the synchronous controller, after being processed by the synchronous controller, the synchronous compensation signal is output, thereby realizing multi-axis synchronous control according to the synchronous proportional coefficient. The position controller adopts P control, K s_p is the proportional control coefficient. The synchronous controller adopts PI control, K c_p is the proportional coefficient of the synchronous controller, K c_i is the integral coefficient of the synchronous controller, and the specific transfer function G(s) is as follows:

[0147]

[0148] The position compensation is the product of the position control proportional coefficient and the position tracking error:

[0149] U s (k) = K s_p e s (k)

[0150] Among them, U s (k) is the position compensation, K s_p is the position control proportional control coefficient, e s (k) is the position controller input error.

[0151] The synchronous compensation amount is calculated according to the following formula:

[0152]

[0153] Among them, U c (k) is the synchronous compensation amount, K c_p is the synchronous control proportional coefficient, K c_i is the synchronous control integral coefficient e c (k) is the synchronous controller input error, e c (i) is the i-th input error.

[0154] The method disclosed in the present invention is described in detail below using a specific scenario as an example:

[0155] The specific steps of a four-point support platform leveling control method are as follows:

[0156] S1. Use the DH coordinate method to calculate the position adjustment amount of each leg to level the support platform:

[0157] S1.1. Establish the inertial coordinate system and object coordinate system of the four-point support platform:

[0158] Platform a = 2m, b = 3m, initial platform height 1m, initial inclination angle θ x =2°,θ y =1°. Take the support point of the adjustment leg 4 and the four-point support platform as the origin O of the space coordinate system, and establish the coordinate system {0} and the coordinate system {1} respectively. The x-axis of the object coordinate system is parallel to the wide side of the rectangle, and the y-axis is parallel to the long side of the rectangle. The expression of the four support points in the coordinate system {0} is 0 P1, 0 P2, 0 P3, 0 P4. The coordinates of the four support points in the coordinate system {1} are 1 P1=(2 0 0) T , 1 P2=(2 3 0) T , 1 P3=(0 3 0) T , 1 P4=(0 0 0) T .

[0159] S1.2, according to the DH coordinate method, solve and calculate the displacement required to adjust the supporting legs.

[0160] According to the initial inclination angle of the platform, R x and R y as follows:

[0161]

[0162] The rotation matrix R is calculated as follows:

[0163]

[0164] The coordinates of the four support points in the coordinate system {1} can be expressed as 1 P i =( 1 X i 1 Y i 1 Z i ) T , the coordinates of the four support points in the coordinate system {0} can be expressed as 0 P i =( 0 X i 0 Yi 0 Z i ) T , where (i = 1, 2, 3, 4) According to the DH coordinate method, the transformation relationship between the four vertices in the coordinate system {1} and the coordinate system {0} is as follows:

[0165]

[0166] Get four adjustment legs 0 Z i They are as follows:

[0167] 0 Z1=-0.0698

[0168] 0 Z2=0.0175

[0169] 0 Z3=0.0524

[0170] 0 Z4=0

[0171] According to the calculation formula, the z0 value can be obtained as 0.0524. After the offset z0 is compensated, the adjustment amount ΔZ of the adjustment leg i is obtained. i (i=1,2,3,4), the position adjustment amount of each adjustment leg is as follows, it can be seen that the adjustment leg 3 is the highest support point and remains fixed:

[0172] ΔZ1=-0.0174

[0173] ΔZ2=-0.0349

[0174] ΔZ3=0

[0175] ΔZ4=-0.0524

[0176] Step 2: Trajectory planning and design for adjusting the outrigger position adjustment amount:

[0177] Select the leg v max =10mm / s, a max =5mm / s 2 , t s = 15s, we can get v(t) as the function of adjusting the speed of the legs with the movement time t, as follows:

[0178]

[0179] By integrating v(t), we can get the total motion adjustment as 2S a=0.1223, and then the synchronization proportional coefficient of the i-th adjustment leg is calculated as follows, and the equation S of the change of the position of the i-th adjustment leg over time is obtained i (t) = λ i S(t).

[0180] λ1=0.155

[0181] λ2=0.2854

[0182] λ3=0

[0183] λ4=0.4285

[0184] Step 3: Based on the adjacent cross-coupling control principle, the synchronous error coupler of the adjustment leg is designed. For the synchronous leveling control system of the four-point support platform, the position tracking error e of the i-th adjustment leg is defined as i (t) is the command input value S of the axis i (t) and actual feedback value S f The difference in (t) is expressed as follows:

[0185] e i (t) = S i (t)-S f (t)

[0186] According to the synchronous error coupler of the i-th adjustment leg The calculation formula can be used to obtain the synchronous error coupler of the four adjustment legs of the four-point support platform as follows:

[0187]

[0188] Step 4: Design and adjust the outrigger synchronization controller and position controller:

[0189] The position controller for adjusting the outriggers adopts P control, and the synchronous controller adopts PI control. The control algorithm runs in the leveling control device, and the proportional control algorithm of the position controller is discretized and implemented as follows:

[0190] U s (k) = K s_p e s (k)

[0191] U s (k) is the output of the position controller, K s_p is the proportional control coefficient of the position controller, e s (k) is the position controller input error. The PI control algorithm of the synchronous controller is discretized as follows:

[0192]

[0193] Among them U c (k) is the synchronous controller output, e c (k) is the synchronous controller input error, e c (i) is the i-th input error.

[0194] Based on SimMechanics dynamic modeling, a physical model of the four-point leveling system is established, and the leveling control method is simulated and verified. The structural parameters of the leveling system are set as follows in the simulation: leveling workbench a = 2m, b = 3m, initial platform height 1m, initial inclination angle θ x =2°,θ y =1°. Figure 5 The following is the simulated leveling displacement curve without adding a synchronous controller. The results show that the maximum synchronous displacement error is 0.007mm. Figure 6 The figure shows the simulated leveling displacement curve with the addition of a synchronous controller. The simulation results show that the maximum synchronous displacement error is about 0.025 mm. During the entire leveling process, the position error between the actual displacement of each leg and the expected displacement is as follows: Figure 7 As shown, the above results show that the synchronization error is reduced by about 64% by the method disclosed in the example of the present invention, and the leg position error approaches zero after the adjustment.

[0195] Second, as Figure 8 As shown, based on the above-mentioned support platform leveling control method, the present invention also discloses a support platform leveling control device, including:

[0196] The position adjustment amount calculation module 201 is used to calculate the position adjustment amount of each leg of the support platform for leveling the support platform by using the DH coordinate method;

[0197] The trajectory planning module 202 is used to perform trajectory planning for the position adjustment amount of each leg according to the smooth and symmetrical speed curve to obtain the desired position trajectory of each leg.

[0198] The synchronous error coupling module 203 is used to calculate the position tracking error of each leg according to the expected position trajectory of each leg, and calculate the adjacent cross-coupling error of each leg according to the position tracking error.

[0199] The position compensation amount calculation module 204 is used to calculate the position compensation amount of each leg according to the position tracking error.

[0200] The synchronous compensation amount calculation module 205 is used to calculate the synchronous compensation amount of each leg according to adjacent cross-coupling errors.

[0201] Furthermore, the position adjustment amount calculation module 201 includes:

[0202] A coordinate system establishment module is used to establish the inertial coordinate system and object coordinate system of the support platform;

[0203] A coordinate conversion module, used to calculate the coordinates of the support points of each leg of the support platform in the inertial coordinate system according to the DH coordinate method;

[0204] The adjustment amount calculation module is used to calculate the position adjustment amount of each leg based on the highest point chasing principle.

[0205] The trajectory planning module 202 includes:

[0206] A smooth and symmetrical speed curve calculation module is used to calculate the relationship between the position and time change of each leg according to the smooth and symmetrical speed curve and the preset motion limit;

[0207] A synchronization proportional coefficient calculation module, used to calculate the synchronization proportional coefficient of each leg according to the position adjustment amount and the total displacement of each leg;

[0208] The expected trajectory calculation module is used to calculate the expected position trajectory of each leg according to the synchronization proportional coefficient and the relationship between the position and time of each leg.

[0209] The synchronization error coupling module 203 includes:

[0210] An actual position trajectory acquisition module is used to obtain the actual position trajectory of each leg during the leveling process of the support platform according to the expected position trajectory;

[0211] A position tracking error calculation module is used to calculate the difference between the expected position trajectory and the actual position trajectory of each leg;

[0212] A synchronization error calculation module, used to calculate the synchronization error of each leg according to the position tracking error;

[0213] The adjacent cross-coupling error calculation module is used to calculate the adjacent cross-coupling error of each leg according to the synchronization error.

[0214] Furthermore, the synchronization error calculation module is specifically used for:

[0215] Calculate the difference between the position tracking error of a leg and the position tracking error of the next adjacent numbered leg as a synchronization error;

[0216] The difference between the synchronization error of a leg and the synchronization error of the leg with the previous adjacent number is taken as the adjacent cross-coupling error, wherein the adjacent cross-coupling error of the leg with the smallest number is the difference between the synchronization error of the leg with the smallest number and the synchronization error of the leg with the largest number.

[0217] The position compensation amount calculation module 204 is specifically used to calculate the position compensation amount of each leg according to the following formula:

[0218] U s(k) = K s_p e s (k)

[0219] Among them, U s (k) is the position compensation, K s_p is the position control proportional control coefficient, e s (k) is the position controller input error.

[0220] The synchronous compensation amount calculation module 205 is specifically used to calculate the synchronous compensation amount of each leg according to the following formula:

[0221]

[0222] Among them, U c (k) is the synchronous compensation amount, K c_p is the synchronous control proportional coefficient, K c_i is the synchronous control integral coefficient e c (k) is the synchronous controller input error, e c (i) is the i-th input error.

[0223] In the second aspect, a leveling control device for a four-point support platform is provided, wherein all components used in the leveling control device are 100% domestically produced components, including chips, capacitors, resistors, inductors, optocouplers, crystal oscillators, relays, etc. The leveling control device includes a DSP minimum system, an EEPROM storage unit, a sensor signal acquisition unit, a digital quantity acquisition unit, a digital quantity output unit, a CAN communication unit, an RS485 communication unit, and an RS232 communication unit.

[0224] Furthermore, the DSP minimum system uses a 100% domestically produced JDSPF28335 high-performance, low-power 32-bit floating-point digital signal processor produced by the 58th Institute of China Electronics Technology Group Corporation, as well as a crystal oscillator, power supply, etc., to provide a minimum executable configuration for the leveling control algorithm.

[0225] Furthermore, the data and clock pins of the SPI unit of JDSPF28335 are connected to the storage medium EEPROM chip to form the EEPROM storage unit, which is used to store the key parameters of the leveling system.

[0226] Furthermore, the external current and voltage acquisition interface is connected to the AD acquisition unit of JDSPF28335 via a conditioning circuit composed of resistors, capacitors, operational amplifiers, etc., to form the sensor signal acquisition unit, which is used to collect the current of the four leveling leg locking mechanisms of the four-point support platform.

[0227] Furthermore, the external digital signal output and input are connected to the GPIO pin of JDSPF28335 after optical coupling isolation, forming the digital acquisition unit and digital logic output unit. The digital acquisition unit is used to collect emergency stop signals and action button signals, etc. The digital logic output unit is used to control signal lights, alarms, leveling leg locking mechanisms, etc.

[0228] Furthermore, the ECAN module pins of JDSPF28335 are connected to the isolated CAN driver chip to form the CAN communication unit for connecting four motor drivers.

[0229] Furthermore, the SCI module of JDSPF28335 is connected with the isolated RS485 differential communication chip and the isolated RS232 differential communication chip to form the RS485 communication unit and the RS232 communication unit, which are used to connect the inclination sensor and the human-computer interaction software.

[0230] Based on the supporting platform leveling control method disclosed in the above embodiment of the present invention, Figure 3 As shown, an embodiment of the present invention further provides a support platform leveling control device, including.

[0231] In addition, an embodiment of the present invention further provides an electronic device, including:

[0232] one or more processors; and

[0233] A memory associated with the one or more processors, the memory being used to store program instructions, and the program instructions, when read and executed by the one or more processors, execute the support platform leveling control method disclosed in the above embodiment.

[0234] Among them, Fig. 9 As shown, the computer device 12 is in the form of a general-purpose computing device. The components of the computer device 12 may include, but are not limited to, one or more processors or processing units 16, a system memory 28, and a bus 18 connecting different system components (including the system memory 28 and the processing unit 16). The bus 18 represents one or more of several types of bus structures, including a memory bus or memory controller, a peripheral bus, a graphics acceleration port, a processor or a local bus using any of a variety of bus structures. For example, these architectures include, but are not limited to, an Industry Standard Architecture (ISA) bus, a Micro Channel Architecture (MAC) bus, an Enhanced ISA bus, a Video Electronics Standards Association (VESA) local bus, and a Peripheral Component Interconnect (PCI) bus.

[0235] The computer device 12 typically includes a variety of computer system readable media. These media can be any available media that can be accessed by the computer device 12, including volatile and non-volatile media, removable and non-removable media.

[0236] The system memory 28 may include computer system readable media in the form of volatile memory, such as random access memory (RAM) 30 and / or cache memory 32. The computer device 12 may further include other removable / non-removable, volatile / non-volatile computer system storage media. By way of example only, the storage system 34 may be used to read and write non-removable, non-volatile magnetic media (not shown in the figure, usually referred to as a "hard drive"). Although not shown in the figure, a disk drive for reading and writing to a removable non-volatile disk (such as a "floppy disk"), and an optical disk drive for reading and writing to a removable non-volatile optical disk (such as a CD-ROM, DVD-ROM or other optical media) may be provided. In these cases, each drive may be connected to the bus 18 via one or more data media interfaces. The memory 28 may include at least one program product having a set (e.g., at least one) of program modules that are configured to perform the functions of various embodiments of the present invention.

[0237] A program / utility 40 having a set (at least one) of program modules 42 may be stored, for example, in the memory 28, such program modules 42 including, but not limited to, an operating system, one or more application programs, other program modules, and program data, each of which or some combination may include an implementation of a network environment. The program modules 42 generally perform the functions and / or methods of the embodiments described herein.

[0238] The computer device 12 may also communicate with one or more external devices 14 (e.g., keyboard, pointing device, display 24, etc.), may also communicate with one or more devices that enable a user to interact with the computer device 12, and / or communicate with any device that enables the computer device 12 to communicate with one or more other computing devices (e.g., network card, modem, etc.). Such communication may be performed through an input / output (I / O) interface 22. In addition, the computer device 12 in this embodiment, the display 24 does not exist as an independent individual, but is embedded in the mirror surface, and when the display surface of the display 24 is not displayed, the display surface of the display 24 and the mirror surface are visually integrated. In addition, the computer device 12 may also communicate with one or more networks (e.g., local area network (LAN), wide area network (WAN) and / or public network, such as the Internet) through a network adapter 20. As shown in the figure, the network adapter 20 communicates with other modules of the computer device 12 through a bus 18. It should be understood that although not shown in the figures, other hardware and / or software modules may be used in conjunction with the computer device 12, including but not limited to: microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, and data backup storage systems.

[0239] The processing unit 16 executes various functional applications and data processing by running programs stored in the system memory 28 .

[0240] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can refer to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the system or system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can refer to the partial description of the method embodiment. The system and system embodiments described above are merely schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without creative work.

[0241] The technical solution provided by the present application is described in detail above. The principle and implementation method of the present application are described in detail using specific examples. The description of the above embodiments is only used to help understand the method and core idea of ​​the present application. At the same time, for those skilled in the art, according to the idea of ​​the present application, there will be changes in the specific implementation method and application scope. In summary, the content of this specification should not be understood as limiting the present application.

[0242] All the above optional technical solutions can be arbitrarily combined to form optional embodiments of the present invention, which will not be described in detail here.

[0243] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A support platform leveling control method, characterized in that: include: Use the DH coordinate method to calculate the position adjustment amount of each leg to level the support platform; Performing trajectory planning for the position adjustment amount of each leg according to a smooth and symmetrical speed curve to obtain a desired position trajectory of each leg; Calculating a position tracking error of each leg according to a desired position trajectory of each leg, and calculating an adjacent cross-coupling error of each leg according to the position tracking error; The position compensation amount of each leg is calculated according to the position tracking error, and the synchronization compensation amount of each leg is calculated according to the adjacent cross-coupling error.

2. The method according to claim 1, characterized in that The step of performing trajectory planning for the position adjustment amount of each leg according to the smooth and symmetrical speed curve to obtain the desired position trajectory of each leg includes: Calculate the relationship between the position and time of each leg according to the smooth symmetrical speed curve and the preset motion limit; Calculating a synchronization proportional coefficient of each leg according to the position adjustment amount and the total displacement of each leg; The expected position trajectory of each leg is calculated according to the synchronization proportional coefficient and the changing relationship between the position of each leg and time.

3. The method according to claim 1, characterized in that The step of calculating the position tracking error of each leg according to the expected position trajectory of each leg, and calculating the adjacent cross-coupling error of each leg according to the position tracking error comprises: Obtaining the actual position trajectory of each leg during the leveling process of the support platform according to the expected position trajectory; Calculate the difference between the expected position trajectory and the actual position trajectory of each leg as the position tracking error of each leg; The synchronization error of each leg is calculated according to the position tracking error, and the adjacent cross-coupling error of each leg is calculated according to the synchronization error.

4. The method according to claim 3, characterized in that The synchronization error is the difference between the position tracking error of a leg and the position tracking error of the next adjacent numbered leg; The adjacent cross-coupling error is the difference between the synchronization error of a leg and the synchronization error of its last adjacent numbered leg, wherein the adjacent cross-coupling error of the leg with the smallest number is the difference between the synchronization error of the leg with the smallest number and the synchronization error of the leg with the largest number.

5. The method according to claim 1, characterized in that Calculating the position compensation amount of each leg according to the position tracking error includes: The position compensation is the product of the position control proportional coefficient and the position tracking error: YOU s (k)=K s_p and s (k) Among them, U s (k) is the position compensation, K s_p is the position control proportional control coefficient, e s (k) is the position controller input error.

6. The method according to claim 1, characterized in that The step of calculating the synchronous compensation amount of each leg according to the adjacent cross-coupling errors comprises: The synchronous compensation amount is calculated according to the following formula: Among them, U c (k) is the synchronous compensation amount, K c_p is the synchronous control proportional coefficient, K c_i is the synchronous control integral coefficient e c (k) is the synchronous controller input error, e c (i) is the i-th input error.

7. The method according to claim 1, characterized in that The method of using the DH coordinate method to calculate the position adjustment amount of each leg of the support platform for leveling the support platform includes: Establish the inertial coordinate system and object coordinate system of the support platform; Calculate the coordinates of the support points of each leg of the support platform in the inertial coordinate system according to the DH coordinate method; The position adjustment amount of each leg is calculated based on the highest point chasing principle.

8. A support platform leveling control device, characterized in that: include: A position adjustment amount calculation module, used to calculate the position adjustment amount of each leg of the support platform for leveling the support platform using the DH coordinate method; A trajectory planning module, used to perform trajectory planning on the position adjustment amount of each leg according to a smooth and symmetrical speed curve to obtain a desired position trajectory of each leg; A synchronous error coupling module is used to calculate the position tracking error of each leg according to the expected position trajectory of each leg, and calculate the adjacent cross-coupling error of each leg according to the position tracking error; A position compensation amount calculation module, used to calculate the position compensation amount of each leg according to the position tracking error; The synchronous compensation amount calculation module is used to calculate the synchronous compensation amount of each leg according to the adjacent cross-coupling errors.

9. An electronic device, characterized in that: include: one or more processors; as well as A memory associated with the one or more processors, the memory being used to store program instructions, wherein when the program instructions are read and executed by the one or more processors, the method according to any one of claims 1 to 7 is executed.

10. A computer readable medium having a computer program stored thereon, wherein: When the program is executed by a processor, the method according to any one of claims 1 to 7 is implemented.

Images