A multi-point leveling control method

Abstract

The present invention discloses a multi-point leveling control method. In the field of weapon equipment manufacturing or aircraft manufacturing, in order to meet the requirements of rapid transfer of components, it has become a trend to use mobile robot transportation to replace the original hoisting transportation; due to the influence of terrain and multi-point workstations, the mobile robot needs to be lifted and leveled after the movement is completed. For example, in the docking process of large spacecraft cabins and the hole making process of the aircraft trailing edge wing surface, the electric support mechanism on the robot needs to be used to lift the vehicle body to a specified height and level the vehicle body so that the vehicle body state meets the requirements of subsequent processes. The existing leveling methods mostly limit the installation posture of the inclination sensor to be consistent with the support plane coordinates and limit the support point position coordinates, and the versatility is poor; some leveling methods perform two-way split adjustment, which is insufficient in flexibility; in addition, some leveling methods approximate the moving speed of each point, and the vehicle body will be affected by a large additional force during the moving process. In view of the above problems, the present invention proposes a high-versatility, high-flexibility multi-point leveling control method, which fully considers the rigid constraints in the same plane, and can convert the output of the inclination sensor installed on the plane in any posture into the moving speed of the support leg; the adjustment process is a two-way adjustment of the x and y axes, which improves the leveling efficiency. Experiments have shown that this method can achieve a leveling accuracy of no more than 0.05° in both directions.

Description

Technical field:

[0001] The invention relates to the technical field of intelligent industrial logistics, in particular to a multi-point leveling control method. Background technology:

[0002] In the field of weapons and equipment manufacturing or aircraft manufacturing, in order to meet the requirements of rapid equipment transfer, it has become a trend to use mobile robots to replace the original lifting and transportation. Due to the influence of terrain and multi-point workstations, the transfer mobile robot needs to be lifted and leveled after the movement is completed. For example, in the docking process of large spacecraft cabins, after the mobile robot loads the mobile cabin to the docking station, the electric support mechanism on the robot will lift the vehicle body to the specified height and level the vehicle body, so that the loaded cabin meets the docking requirements with the fixed cabin; for example, in the process of drilling holes on the trailing edge of the aircraft, the mobile robot loads the drilling mechanical arm to move at multiple points. In order to ensure that the front-end camera of the mechanical arm can be aligned with the hole position and ensure the accuracy of the drilling position, the electric support mechanism on the robot needs to be used to lift the vehicle body to the specified height and level the vehicle body.

[0003] Existing leveling methods mostly limit the installation posture of the inclination sensor to be consistent with the support plane coordinates and limit the support point position coordinates, which have poor versatility; or they perform two-way adjustment, which lacks flexibility; in addition, the algorithm approximates the moving speed of each point, and the vehicle body will be affected by a large additional force during the movement process. Summary of the invention:

[0004] In view of the above problems, the present invention proposes a highly versatile and flexible multi-point leveling control method, which fully considers the rigid constraints in the same plane and can convert the plane's inclination sensor output into the supporting leg moving speed.

[0005] This application achieves the above effects through the following scheme:

[0006] A multi-point leveling control method, the method comprising the following steps:

[0007] S1, after the mobile robot arrives at the workstation and stops, it controls the supporting legs of the vehicle body to move downward until each supporting leg is in contact with the ground. The position of each supporting leg is the supporting point;

[0008] S2, numbering each supporting leg of the mobile robot and establishing a supporting plane reference coordinate system, determining the installation posture of the inclination sensor in the supporting plane reference coordinate system and the conversion matrix between the inclination sensor and the supporting plane reference coordinate system, and calculating the coordinates of each supporting point in the inclination sensor coordinate system;

[0009] S3, reading the value of the inclination sensor, and determining the conversion matrix between the tilt coordinate system and the support plane reference coordinate system according to the value of the inclination sensor;

[0010] S4, calculating the reference height of each support point, selecting the highest point as the reference height and calculating the moving speed of each support leg; if it is the first calculation, recording the highest point number of the first calculation;

[0011] S5, each supporting leg moves according to the speed command, and the inclination sensor feedback value changes; if the inclination sensor value is within the set inclination range after adjustment, the leveling action is stopped; if it does not enter the set range, the new inclination sensor measurement value is used to continue the S4 cycle; if the highest point is different from the previous cycle under the new inclination sensor measurement value, the height value corresponding to the highest point number during the first calculation is used as the reference height to calculate the moving speed.

[0012] Furthermore, the S1 is:

[0013] The current feedback value of each supporting leg motor is read to determine whether each supporting leg is in contact with the ground. The current feedback value judgment condition is related to the load of the mobile robot and is determined through experiments based on actual conditions.

[0014] Further, the S2 is:

[0015] The support plane reference coordinate system {P} is established on the plane to be adjusted. The coordinates of each support point in the support plane reference coordinate system {P} are expressed as follows: The coordinate values ​​of each support point in the coordinate system are determined by the structural design, where i is the support point number. are the x-axis and y-axis coordinates of the support point in the support plane reference coordinate system {P} respectively; the position coordinates of the origin of the sensor coordinate system {O} in the support plane reference coordinate system {P} are expressed as T = [x t0 y t0 0] T , the angle between the x-axis of the sensor coordinate system {O} and the x-axis of the support plane reference coordinate system {P} is δ, where x t0 ,y t0 are the x-axis and y-axis coordinates of the origin in the support plane reference coordinate system {P}, respectively. t0 ,y t0 , δ are determined by the structural design; δ ​​is used to represent the rotation matrix of the two coordinate systems P R o =f(δ); the position coordinates of each support point in the sensor coordinate system {O} are expressed as P i O =( P R o ) -1 (P i -T).

[0016] Further, the S3 is:

[0017] Set the sensor coordinate system in the tilt state to {S}. The inclination sensor feedback coordinate system {S} contains two-way inclination values ​​α and β, where α is the inclination of the coordinate system {S} x-axis, and β is the inclination of the coordinate system {S} y-axis; use the inclination sensor measurement value to represent the rotation matrix of the coordinate system {S} and {O} o R S =f(α, β).

[0018] Further, the S4 is:

[0019] Each support point has a height difference with the tilted plane, which is manifested as the different heights of the z-axis coordinates of each point in the support plane reference coordinate system {P}. After tilting, the coordinates of each support point in the support plane reference coordinate system {P} are expressed as: P i r = P R o o R S P i O +T;

[0020] Select the maximum value of the z-axis coordinate of each support point as the reference point and record the point number as i max , the z-axis coordinate value of this point is The moving speed of each support point is expressed as Where P i r (3) is the z-axis coordinate of each support point in the support plane reference coordinate system {P}, and t is the length of a control cycle.

[0021] Further, the S5 is:

[0022] In the new adjustment cycle, if the calculated maximum value of the z-axis coordinate is a reference point number that is inconsistent with the previous cycle, use the previous cycle record number i max Calculate the moving speed of each support point for the new cycle reference point.

[0023] Beneficial Effects

[0024] Compared with the conventional leveling method, this solution does not limit the installation position and posture of the inclination sensor. The installation position and posture of the inclination sensor can conform to the mechanical structure design of the vehicle body, which is more flexible. Secondly, this solution uses the idea of ​​matrix transformation to solve the transformation relationship between the plane reference coordinate system, the sensor coordinate system, and the sensor feedback coordinate system. Therefore, the design position of the support point does not need to be symmetrical. If necessary, it can be arranged at any position in the plane reference coordinate system. In addition, the matrix relationship can be used to solve the precise reference height of each support point, which is more accurate than the common proportional approximation method. Finally, the adjustment process is a bidirectional adjustment of the x and y axes, which effectively improves the leveling efficiency. Description of the drawings:

[0025] Figure 1 The overall flow chart for the implementation of this application method;

[0026] Figure 2 Create a schematic diagram for the coordinate system;

[0027] Figure 3 Schematic diagram of the sensor tilt state. Specific implementation method:

[0028] The technical solution of the present invention is further described in detail below in conjunction with the accompanying drawings:

[0029] like Figure 1 As shown, this embodiment is a multi-point leveling control method, and the method includes the following steps:

[0030] S1, after the mobile robot arrives at the workstation and stops, it controls the supporting legs of the vehicle body to move downward until each supporting leg is in contact with the ground. The position of each supporting leg is the supporting point;

[0031] S2, numbering each supporting leg of the mobile robot and establishing a supporting plane reference coordinate system, determining the installation posture of the inclination sensor in the supporting plane reference coordinate system and the conversion matrix between the inclination sensor and the supporting plane reference coordinate system, and calculating the coordinates of each supporting point in the inclination sensor coordinate system;

[0032] S3, reading the value of the inclination sensor, and determining the conversion matrix between the tilt coordinate system and the support plane reference coordinate system according to the value of the inclination sensor;

[0033] S4, calculating the reference height of each support point, selecting the highest point as the reference height and calculating the moving speed of each support leg; if it is the first calculation, recording the highest point number of the first calculation;

[0034] S5, each supporting leg moves according to the speed command, and the inclination sensor feedback value changes; if the inclination sensor value is within the set inclination range after adjustment, the leveling action is stopped; if it does not enter the set range, the new inclination sensor measurement value is used to continue the S4 cycle; if the highest point is different from the previous cycle under the new inclination sensor measurement value, the height value corresponding to the highest point number during the first calculation is used as the reference height to calculate the moving speed.

[0035] Furthermore, the S1 is:

[0036] The current feedback value of each supporting leg motor is read to determine whether each supporting leg is in contact with the ground. The current feedback value judgment condition is related to the load of the mobile robot and is determined through experiments based on actual conditions.

[0037] Further, the S2 is:

[0038] The support plane reference coordinate system {P} is established on the plane to be adjusted. The coordinates of each support point in the support plane reference coordinate system {P} are expressed as The coordinate values ​​of each support point in the coordinate system are determined by the structural design, where i is the support point number. are the x-axis and y-axis coordinate values ​​of the support point in the support plane reference coordinate system {P} respectively; the position coordinate of the origin of the sensor coordinate system {O} in the support plane reference coordinate system {P} is expressed as T = [x t0 y t0 0] T , the angle between the x-axis of the sensor coordinate system {O} and the x-axis of the support plane reference coordinate system {P} is δ, where x t0 ,y t0 are the x-axis and y-axis coordinates of the origin in the support plane reference coordinate system {P}, respectively. t0 ,y t0 , δ are determined by the structural design; δ ​​is used to represent the rotation matrix of the two coordinate systems P R o =f(δ); the position coordinates of each support point in the sensor coordinate system {O} are expressed as P i O =( P R o ) -1 (P i -T).x t0 ,y t0 , δ represent the installation position and posture, which are determined by the vehicle structure design. You can design and determine the value according to your needs. After the above parameters are determined, the conversion relationship is also determined. The specific algorithm for converting the coordinate system is described below.

[0039] Further, the S3 is:

[0040] like Figure 2 As shown, the sensor coordinate system in the tilt state is set to {S}, and the inclination sensor feedback coordinate system {S} contains two-way inclination values ​​α and β, where α is the inclination of the coordinate system {S} x-axis, and β is the inclination of the coordinate system {S} y-axis; the inclination sensor measurement value is used to represent the rotation matrix of the coordinate system {S} and {O} o R S =f(α, β).

[0041] Further, the S4 is:

[0042] Each support point has a height difference with the tilted plane, which is manifested as the different heights of the z-axis coordinates of each point in the support plane reference coordinate system {P}. After tilting, the coordinates of each support point in the support plane reference coordinate system {P} are expressed as: P i r = P Ro o R S P i O +T;

[0043] Select the maximum value of the z-axis coordinate of each support point as the reference point and record the point number as i max , record the z-axis coordinate value of this point The moving speed of each support point is expressed as Where P i r (3) is the z-axis coordinate of each support point in the support plane reference coordinate system {P}, and t is the length of a control cycle.

[0044] Further, the S5 is:

[0045] In the new adjustment cycle, if the calculated maximum value of the z-axis coordinate is a reference point number that is inconsistent with the previous cycle, use the previous cycle record number i max Calculate the moving speed of each support point for the new cycle reference point.

[0046] Figure 2 The figure shows a four-point support leveling state. Figure 2 The stress analysis is performed in the state shown, such as Figure 3 As shown, the origin of the support plane reference coordinate system {P} is the geometric center of the rectangle formed by the four-point line, and the coordinates of each point are In actual use, there is no restriction on the four points, and the arrangement status of each point is also not restricted.

[0047] 1) The sensor is installed on the reference plane, denoted as sensor {O}, and its origin is located at T = [x t0 y t0 0] T , the angle between the sensor {O}x-axis and the coordinate system {P}x-axis is δ, and the transformation matrix of the two coordinate systems is

[0048] 2) Each support point can be expressed as P in the coordinate system {O} i O =( P R o ) -1 (P i -T)(i=1~4).

[0049] 3) The outputs of the tilt sensor are α and β, and the sensor coordinate system in the tilt state is set to {S}. The rotation matrix of the coordinate system {S} and the coordinate system {O} can be expressed as o R S =[n x n y nz ], where n x =[cosα 0 sinα] T ,

[0050] 4) Each support point has a height difference with the inclined plane, which is manifested as the different heights of the z-axis coordinates of each point in the coordinate system {P}. The coordinates of each point in the coordinate system {P} can be expressed as: P i r = P R o o R S P i O +T(i=1~4)

[0051] 5) Compare the z-axis coordinates P of each point i r (3), let the maximum point number of the z-axis coordinate be i max , then the moving speed of each point should be

[0052] 6) In the new adjustment cycle, if the calculated maximum value of the z-axis coordinate is a reference point number that is inconsistent with the previous cycle, the previous cycle record number i should be used. max Calculate the moving speed of each support point for the new cycle reference point.

[0053] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. A person skilled in the art can make several changes and adjustments within the technical scope disclosed by the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A multi-point leveling control method, characterized in that: The method comprises the following steps: S1, after the mobile robot arrives at the workstation and stops, it controls the supporting legs of the vehicle body to move downward until each supporting leg is in contact with the ground. The position of each supporting leg is the supporting point; S2, numbering each supporting leg of the mobile robot and establishing a supporting plane reference coordinate system, determining the installation posture of the inclination sensor in the supporting plane reference coordinate system and the conversion matrix between the inclination sensor and the supporting plane reference coordinate system, and calculating the coordinates of each supporting point in the inclination sensor coordinate system; S3, reading the value of the inclination sensor, and determining the conversion matrix between the tilt coordinate system and the support plane reference coordinate system according to the value of the inclination sensor; S4, calculate the reference height of each support point, select the highest point as the reference height and calculate the moving speed of each support leg; if it is the first calculation, record the highest point number when calculating for the first time; S4 is: Each support point has a height difference with the tilted plane, which is manifested as the different heights of the z-axis coordinates of each point in the support plane reference coordinate system {P}. After tilting, the coordinates of each support point in the support plane reference coordinate system {P} are expressed as: P i r = P R o o R S P i O +T; Select the maximum value of the z-axis coordinate of each support point as the reference point and record the point number as i max , the z-axis coordinate value of this point is The moving speed of each support point is expressed as Where P i r (3) is the z-axis coordinate of each support point in the support plane reference coordinate system {P}, and t is the length of a control cycle; S5, each supporting leg moves according to the speed command, and the inclination sensor feedback value changes; if the inclination sensor value is within the set inclination range after adjustment, the leveling action is stopped; if it does not enter the set range, the new inclination sensor measurement value is used to continue the S4 cycle; if the highest point is different from the previous cycle under the new inclination sensor measurement value, the height value corresponding to the highest point number during the first calculation is used as the reference height to calculate the moving speed.

2. A multi-point leveling control method according to claim 1, characterized in that: The S1 is: The current feedback value of each supporting leg motor is read to determine whether each supporting leg is in contact with the ground. The current feedback value judgment condition is related to the load of the mobile robot and is determined through experiments based on actual conditions.

3. A multi-point leveling control method according to claim 1, characterized in that: The S2 is: The support plane reference coordinate system {P} is established on the plane to be adjusted. The coordinates of each support point in the support plane reference coordinate system {P} are expressed as The coordinate values ​​of each support point in the coordinate system are determined by the structural design, where i is the support point number. are the x-axis and y-axis coordinate values ​​of the support point in the support plane reference coordinate system {P} respectively; the position coordinate of the origin of the sensor coordinate system {O} in the support plane reference coordinate system {P} is expressed as T = [x t0 y t0 0] T , the angle between the x-axis of the sensor coordinate system {O} and the x-axis of the support plane reference coordinate system {P} is δ, where x t0 ,y t0 are the x-axis and y-axis coordinates of the origin in the support plane reference coordinate system {P}, respectively. t0 ,y t0 , δ are determined by the structural design; δ ​​is used to represent the rotation matrix of the two coordinate systems P R o =f(δ); the position coordinates of each support point in the sensor coordinate system {O} are expressed as P i O =( P R o ) -1 (P i -T).

4. A multi-point leveling control method according to claim 1, characterized in that: The S3 is: Set the sensor coordinate system in the tilt state to {S}. The inclination sensor feedback coordinate system {S} contains two-way inclination values ​​α and β, where α is the inclination of the coordinate system {S} x-axis, and β is the inclination of the coordinate system {S} y-axis; use the inclination sensor measurement value to represent the rotation matrix of the coordinate system {S} and {O} o R S =f(α,β).

5. The multi-point leveling control method according to claim 1, characterized in that: The S5 is: In the new adjustment cycle, if the calculated maximum value of the z-axis coordinate is a reference point number that is inconsistent with the previous cycle, use the previous cycle record number i max Calculate the moving speed of each support point for the new cycle reference point.

Images