Chain type docking system based on mobile six-degree-of-freedom platform and control method

Abstract

According to the chain type docking system based on the movable six-degree-of-freedom platform and the control method, the chain type docking system is rapidly arranged on the movable platform, adapts to different sites and terrains, does not need base coordinate system calibration, and completes position and posture adjustment based on forward and inverse kinematics calculation coordinate transformation of a mechanism; the technical effect of improving maneuverability, expandability and environmental adaptability of the pose adjusting system is achieved, the more flexible docking task requirement is met, the number of chain type docking systems is increased, and the system is used for docking and assembling of larger equipment.

Description

Technical Field

[0001] The present invention belongs to the technical field of mechanical measurement, and in particular relates to a six-degree-of-freedom docking and control technology. Background Art

[0002] With the advancement of science and technology and the development of infrastructure, the demand for large and heavy component docking and assembly technology is increasing. This technology has been widely used in the assembly and docking of large satellite components, spacecraft modules, and large aircraft.

[0003] Patent CN101907893B discloses an aircraft component attitude adjustment assembly system and debugging method based on a six-degree-of-freedom parallel mechanism, and patent CN112454245B discloses a six-degree-of-freedom parallel attitude adjustment platform with a two-stage travel and overlapping installation space. These attitude adjustment systems and their associated measurement equipment require fixed locations, requiring high site requirements and failing to meet the demands for flexible and maneuverable docking of large components. Summary of the Invention

[0004] In order to solve the technical problem that large and heavy components cannot move freely during docking and assembly, a technical solution of chain docking and control based on a mobile six-degree-of-freedom platform is adopted. It can be quickly deployed on the mobile platform and adapt to different sites and terrains. It does not require calibration of the base coordinate system. Instead, the posture adjustment is completed by calculating the coordinate transformation based on the forward and inverse kinematics of the mechanism, which produces the technical effect of improving the maneuverability, scalability and environmental adaptability of the posture adjustment system, meeting the needs of more flexible docking tasks, and increasing the number of chain docking systems for the docking and assembly of larger equipment.

[0005] The system includes more than three docking subsystems; the docking subsystem includes a six-degree-of-freedom attitude adjustment platform and a posture measurement device; the six-degree-of-freedom attitude adjustment platform includes a movable platform and four PPPS attitude adjustment mechanisms. The four PPPS attitude adjustment mechanisms are respectively fixed on both sides of the movable platform, serving as four motion branches of the attitude adjustment platform, forming a six-degree-of-freedom parallel mechanism based on four branches; the posture measurement device includes a camera, a target, and a spirit level. The target is installed on one side of the movable platform, the camera is installed on the other side of the movable platform, and the spirit level is installed in the middle of the movable platform.

[0006] Each PPPS attitude adjustment mechanism includes a ball joint base, a support leg, a three-degree-of-freedom skeleton, and a frame. The ball joint base is fixed to the bottom of the support leg; the three-degree-of-freedom skeleton has three directions, XYZ, and each of the three directions includes a motor, a reducer, a pulley, a synchronous belt, a screw, a nut, a base, a slider, and a guide rail to achieve single-degree-of-freedom movement; the Z-direction slider is fixed to the top of the support leg, the guide rail and the base are fixed to the frame, the X-direction guide rail and the base are fixed to the frame, the Y-direction slider is fixed to the movable platform, and the guide rail and the base are fixed to the X-direction slider; the motor drives the pulley through the reducer, and the synchronous belt drives the screw to rotate, which has an accurate transmission ratio, high efficiency, good buffering and shock absorption effect, and a small installation space and compact structure; the screw passes through the base and drives the nut to rotate, so that the slider moves along the T-shaped dovetail guide rail, sliding smoothly and with good load-bearing effect; the four PPPS attitude adjustment mechanisms can be retracted off the ground in the Z direction and can also move relative to the movable platform in the X and Y directions.

[0007] Step 1: Establish four pose coordinate systems with the point directly below the posture adjustment platform No. i, the center of the posture adjustment platform No. i, the point directly below the posture adjustment platform No. i+1, and the center of the posture adjustment platform No. i+1 as the origins.

[0008] In step 1, the forward direction of the posture adjustment platform is the x direction, the vertical direction is the z direction, and the y direction is determined according to the right-hand rule; let the point directly below the posture adjustment platform No. i be the origin of the global base coordinate system No. i, O bi , establish the coordinate system {B i}; Let the center of the posture adjustment platform No. i be the origin O of the local moving coordinate system No. i vi , establish the coordinate system {V i}; Let the origin O of the i+1 global base coordinate system just below the i+1 attitude adjustment platform be bi+1 , establish the coordinate system {B i+1}; Let the center of the i+1 attitude adjustment platform be the origin O of the i+1 local moving coordinate system vi+1 , establish the coordinate system {V i+1 Assume that the intersection of the vertical direction of the center of each ball joint base and the parallel direction of the axis of the X-axis motor is the origin of the PPPS attitude adjustment mechanism O Sj .

[0009] Furthermore, {x, y, z} is used to represent the local moving coordinate system {V i}In the global base coordinate system {B i}, change {V i}First go around {B i} is rotated by γ around the Z axis, then by β around the Y axis, and then by α around the X axis. The local moving coordinate system {V i}In the global base coordinate system {B i}, and the posture of the attitude adjustment platform is represented by {x, y, z, α, β, γ}.

[0010] Step 2: Measure the initial state and movement of each axis of the attitude adjustment platform No. i to obtain the position of the origin of the PPPS attitude adjustment mechanism in the coordinate system.

[0011] In step 2, j = a, b, c, d represents the four PPPS posture adjustment mechanisms, and the origin of the PPPS posture adjustment mechanism is set to O Sj The position in base coordinate system i is The origin position of the local moving coordinate system i+1 is

[0012] Step 3: According to the principle of forward kinematics, calculate the change matrix from the initial base coordinate system i+1 to the initial moving coordinate system i+1.

[0013] In step three, use n x , n y , n z , o x , o y , o z , a x , a y , a z represents the rotation component of the rotation matrix, then

[0014] [a x a y a z ]=[n x n y n z ]×[o x o y o z ], the positive kinematic posture is The positive kinematic position is The rotation matrix of local moving coordinate system i in global base coordinate system i is The homogeneous matrix of local moving coordinate system i in global base coordinate system i is The homogeneous matrix is ​​the transformation matrix from the initial base coordinate system No. i to the initial moving coordinate system No. i.

[0015] Step 4: Multiply the pose change matrix of step 3 with the change matrix from the i+1 moving coordinate system to the i+1 camera obtained by hand-eye calibration to obtain the pose change matrix of the camera of the i+1 attitude adjustment platform in its base coordinate system.

[0016] In step 4, let the transformation matrix from the i+1 moving coordinate system to the i+1 camera obtained by hand-eye calibration be The change matrix from step 3 By multiplying, we can get the pose change matrix of the camera of the i+1 attitude adjustment platform in its base coordinate system:

[0017]

[0018] Step 5: Multiply the pose change matrix of step 4 by the pose of the local moving coordinate system i recognized by the camera of the i+1 attitude adjustment platform to obtain the change matrix from the i+1 initial base coordinate system to the i initial moving coordinate system.

[0019] In step 5, let the pose matrix of the camera of the i+1 attitude adjustment platform recognize the local moving coordinate system i as Multiplying it with the pose change matrix in step 4, we get the change matrix from the initial base coordinate system i+1 to the initial moving coordinate system i, that is

[0020]

[0021] Step 6: Multiply the pose change matrix of step 5 by the homogeneous matrix of the i+1 moving platform coordinate system in the i moving coordinate system to obtain the change matrix from the i+1 initial base coordinate system to the i+1 target moving coordinate system.

[0022] In step 6, let the homogeneous matrix of the i+1 moving platform coordinate system in the i moving coordinate system be Multiply it with the pose change matrix in step 5 to get the change matrix from the initial base coordinate system i+1 to the target moving coordinate system i+1

[0023] Step 7: Multiply the pose matrix of step 6 by the origin position of the PPPS attitude adjustment mechanism in the i+1 local moving coordinate system of step 2 to obtain the target position of each PPPS attitude adjustment mechanism in the i+1 base coordinate system.

[0024] Step 8: Subtract the target position of the origin of the PPPS attitude adjustment mechanism in the i+1 base coordinate system in step 7 from the initial position in step 2 to obtain the displacement of the PPPS attitude adjustment mechanism. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 This is the Z-axis skeleton structure diagram.

[0026] Figure 2 It is an XY skeleton structure diagram.

[0027] Figure 3 This is a schematic diagram of the posture adjustment mechanism installation.

[0028] Figure 4 It is a schematic diagram of pose coordinates. DETAILED DESCRIPTION

[0029] The technical solution of the present invention is described in detail below with reference to the accompanying drawings.

[0030] Each PPPS posture adjustment mechanism includes a ball joint base, legs, a three-degree-of-freedom skeleton, and a frame. The three-degree-of-freedom skeleton has XYZ directions. The three-direction skeleton includes a motor, a reducer, a pulley, a synchronous belt, a screw, a nut, a base, a slider, and a guide rail to achieve single-degree-of-freedom motion.

[0031] The bottom of the ball joint base is a flat plate and the top is a ball socket. The ball head is fixed to the bottom of the leg and installed in the ball socket through the split end cover. The Z-direction slider is fixed to the base on the top of the leg. The guide rail and the base are fixed to the frame. The motor drives the pulley through the reducer, and drives the screw to rotate through the synchronous belt. The screw passes through the base and drives the nut to rotate, so that the slider moves along the T-shaped dovetail guide rail, so that the PPPS posture adjustment mechanism retracts along the Z direction and leaves the ground. Figure 1 shown.

[0032] The X-direction guide rail and base are fixed to the frame, the Y-direction slider is fixed to the movable platform, and the guide rail and base are fixed to the X-direction slider, so that the PPPS attitude adjustment mechanism moves along the X and Y directions relative to the movable platform, such as Figure 2 shown.

[0033] Four PPPS attitude adjustment mechanisms are fixed on both sides of the movable platform, serving as the four motion branches of the attitude adjustment platform, forming a six-degree-of-freedom parallel mechanism based on four branches. The target is installed on one side of the movable platform, the camera is installed on the other side of the movable platform, and the level is installed in the middle of the movable platform. Figure 3 shown.

[0034] The movable platform and four PPPS attitude adjustment mechanisms constitute a six-degree-of-freedom attitude adjustment platform, the camera, target, and level constitute a posture measurement device, the six-degree-of-freedom attitude adjustment platform and the posture measurement device constitute a docking subsystem, and more than three docking subsystems constitute a chain docking system for the system's mobile six-degree-of-freedom platform.

[0035] Step 1: Take the forward direction of the posture adjustment platform as the x direction, the vertical direction as the z direction, determine the y direction according to the right-hand rule, and establish the posture coordinate system, as shown in the following example: Figure 4 shown.

[0036] The mobile platform is L = 3m long, K = 1m wide, and H = 2m high. The PPPS attitude adjustment mechanism is initially in a tightened state in the x and y directions, close to the mobile platform, and the angle of the level is recorded.

[0037] Assume that the point directly below the posture adjustment platform No. i is the origin O of the global base coordinate system No. i bi , establish the coordinate system {B i}, let the center of the posture adjustment platform No. i be the origin O of the local moving coordinate system No. i vi , establish the coordinate system {V i}, let the origin O of the i+1 global base coordinate system just below the i+1 posture adjustment platform be bi+1 , establish the coordinate system {Bi+1}, let the center of the i+1 attitude adjustment platform be the origin O of the i+1 local moving coordinate system vi+1 , establish the coordinate system {V i+1}.

[0038] Assume that the intersection of the vertical direction of the center of each ball joint base and the parallel direction of the axis of the X-axis motor is the origin O of the PPPS attitude adjustment mechanism. Sj .

[0039] Use {x, y, z} to represent the local moving coordinate system {V i}In the global base coordinate system {B i}, change {V i}First go around {B i} is rotated by γ around the Z axis, then by β around the Y axis, and then by α around the X axis. The local moving coordinate system {V i}In the global base coordinate system {B i}, and the posture of the attitude adjustment platform is represented by {x, y, z, α, β, γ}.

[0040] Step 2: Measure the initial state and movement of each axis of the i+1 attitude adjustment platform, and use j=a, b, c, d to represent the four PPPS attitude adjustment mechanisms. Let the origin of the PPPS attitude adjustment mechanism be O Sj+1 The position in the i+1 base coordinate system is At the origin of the local moving coordinate system i+1

[0041] In the initial state, the PPPS attitude adjustment mechanism is in the tightened state in the x and y directions, close to the mobile platform, and the angle of the level is recorded. Si+1 The position in the i+1 moving coordinate system, After the i+1 mobile platform is parked, adjust the z-direction motor of each PPPS attitude adjustment mechanism to make the mobile platform stand off the ground, and then level it according to the level. According to the movement distance of the z-direction motor, map the origin O of the i+1 PPPS attitude adjustment mechanism. Si+1 The position of the base coordinate system of mobile platform i+1 is

[0042]

[0043] Step 3: According to the principle of positive kinematics, calculate the change matrix from the initial base coordinate system i+1 to the initial moving coordinate system i+1

[0044] Use n x , n y , n z , o x, o y , o z , a x , a y , a z represents the rotation component of the rotation matrix, then

[0045]

[0046] [a x a y a z ]=[n x n y n z ]×[o x o y o z ], the positive kinematic posture is The positive kinematic position is The rotation matrix of local moving coordinate system i in global base coordinate system i is The homogeneous matrix of local moving coordinate system i in global base coordinate system i is The homogeneous matrix is ​​the transformation matrix from the initial base coordinate system No. i to the initial moving coordinate system No. i.

[0047] According to the principle of positive kinematics, the change matrix from the initial base coordinate system i+1 to the initial moving coordinate system i+1 is calculated, and α i+1 =1.9515,β i+1 =0.2473,γ i+1 =0.4375,

[0048]

[0049] Step 3: Let the homogeneous matrix of the local coordinate system i+1 in the local moving coordinate system i be From the change matrix of step 2, the change matrix from the initial base coordinate system i to the target coordinate system i+1 is

[0050] Let the i+1th mobile platform move -5cm relative to the y-axis of the ith mobile platform, and calculate the change matrix from the ith initial base coordinate system to the i+1th target coordinate system, and we can get

[0051]

[0052] Step 4: Assume that the change matrix from the i+1 moving coordinate system obtained by hand-eye calibration to the i+1 camera is The change matrix of step 3 By multiplying it, we can get the pose change matrix of the camera of the i+1 attitude adjustment platform in its base coordinate system:

[0053]

[0054] Calibration by hand and eye but

[0055]

[0056] Step 5: Assume that the pose matrix of the camera of the i+1 attitude adjustment platform recognizes the local moving coordinate system i as Multiply the pose change matrix of step 4 with it to obtain the change matrix from the initial base coordinate system i+1 to the initial moving coordinate system i, that is

[0057]

[0058] According to the camera recognition but

[0059]

[0060] Step 6: Let the homogeneous matrix of the i+1 moving platform coordinate system in the i moving coordinate system be Multiply the pose change matrix in step 5 by it to get the change matrix from the initial base coordinate system i+1 to the target moving coordinate system i+1

[0061] Let the i+1th mobile platform move -5cm relative to the y-axis of the ith mobile platform, and calculate the change matrix from the ith initial base coordinate system to the i+1th target coordinate system, and we can get but

[0062]

[0063] Step 7: Multiply the pose matrix of step 6 by the origin position of the PPPS attitude adjustment mechanism in the i+1 local moving coordinate system of step 2 to obtain the target position of each PPPS attitude adjustment mechanism in the i+1 base coordinate system.

[0064] Available

[0065] Taking the first three columns, we get

[0066] Step 8: Subtract the target position of the origin of the PPPS posture adjustment mechanism in the i+1 base coordinate system in step 7 from the initial position in step 2 to calculate the displacement △P of each PPPS posture adjustment mechanism. j ,Right now Available

[0067] △p a=(0.00894,-0.0624,-0.0550), △p b =(0.0358,-0.0445,-0.0402),

[0068] △p c =(0.0193,0.0950,-0.0111), △p d =(0.0909,0.0951,-0.0119).

[0069] The above are embodiments of the present invention and do not limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention are included in the protection scope of the present invention.

Claims

1. A chain docking system based on a mobile six-degree-of-freedom platform, characterized in that: include: More than three docking subsystems; the docking subsystem includes a six-degree-of-freedom attitude adjustment platform and a posture measurement device; The six-degree-of-freedom attitude adjustment platform includes a movable platform and four PPPS attitude adjustment mechanisms. The four PPPS attitude adjustment mechanisms are fixed on both sides of the movable platform, serving as the four motion branches of the attitude adjustment platform, forming a six-degree-of-freedom parallel mechanism based on four branches; the posture measurement device includes a camera, a target, and a spirit level. The target is installed on one side of the movable platform, the camera is installed on the other side of the movable platform, and the spirit level is installed in the middle of the movable platform.

2. The chain docking system based on a mobile six-degree-of-freedom platform according to claim 1, characterized in that: The PPPS posture adjustment mechanism includes: a ball joint base, a support leg, a three-degree-of-freedom skeleton, and a frame. The ball joint base is fixed to the bottom of the support leg. The three-degree-of-freedom skeleton has three XYZ directions, and each of the three directions includes a motor, a reducer, a pulley, a synchronous belt, a screw, a nut, a base, a slider, and a guide rail to achieve single-degree-of-freedom movement. The slider in the Z direction is fixed to the top of the support leg, the guide rail and the base are fixed to the frame, the guide rail and the base in the X direction are fixed to the frame, the slider in the Y direction is fixed to the movable platform, and the guide rail and the base are fixed to the slider in the X direction. The motor drives the pulley through the reducer, and drives the screw to rotate via the synchronous belt. The transmission ratio is accurate, the efficiency is high, the buffering and shock absorption effect is good, the installation space is small, and the structure is compact. The screw passes through the base and drives the nut to rotate, so that the slider moves along the T-shaped dovetail guide rail, sliding smoothly and having a good load-bearing effect. The four PPPS posture adjustment mechanisms can be retracted off the ground in the Z direction and can also move relative to the movable platform in the X and Y directions.

3. A control method based on a mobile six-degree-of-freedom platform, according to the chain docking system based on a mobile six-degree-of-freedom platform according to claim 1, characterized in that: The method comprises the following steps: Step 1: establishing four posture coordinate systems with the position directly below the posture adjustment platform No. i, the center of the posture adjustment platform No. i, the position directly below the posture adjustment platform No. i+1, and the center of the posture adjustment platform No. i+1 as the origins respectively; Step 2: Measure the initial state and movement of each axis of the attitude adjustment platform No. i to obtain the position of the origin of the PPPS attitude adjustment mechanism in the coordinate system; Step 3: According to the principle of forward kinematics, calculate the change matrix from the initial base coordinate system i+1 to the initial moving coordinate system i+1; Step 4: Multiply the pose change matrix of step 3 with the change matrix from the i+1 moving coordinate system to the i+1 camera obtained by hand-eye calibration to obtain the pose change matrix of the camera of the i+1 attitude adjustment platform in its base coordinate system; Step 5: Multiply the pose change matrix of step 4 by the pose of the local moving coordinate system i recognized by the camera of the i+1 attitude adjustment platform to obtain the change matrix from the i+1 initial base coordinate system to the i initial moving coordinate system; Step 6: Multiply the pose change matrix of step 5 by the homogeneous matrix of the i+1 moving platform coordinate system in the i moving coordinate system to obtain the change matrix from the i+1 initial base coordinate system to the i+1 target moving coordinate system; Step 7: Multiply the pose matrix of step 6 by the origin position of the PPPS attitude adjustment mechanism in the i+1 local moving coordinate system of step 2 to obtain the target position of each PPPS attitude adjustment mechanism in the i+1 base coordinate system; Step 8: Subtract the target position of the origin of the PPPS attitude adjustment mechanism in the i+1 base coordinate system in step 7 from the initial position in step 2 to obtain the displacement of the PPPS attitude adjustment mechanism.

4. The control method based on a mobile six-degree-of-freedom platform according to claim 3, characterized in that: The step 1 includes: taking the forward direction of the posture adjustment platform as the x direction, the vertical direction as the z direction, and determining the y direction according to the right-hand rule; setting the point directly below the posture adjustment platform No. i as the origin of the global base coordinate system No. i bi , establish the coordinate system {B i }; Let the center of the posture adjustment platform No. i be the origin O of the local moving coordinate system No. i vi , establish the coordinate system {V i }; Let the origin O of the i+1 global base coordinate system just below the i+1 attitude adjustment platform be bi+1 , establish the coordinate system {B i+1 }; Let the center of the i+1 attitude adjustment platform be the origin O of the i+1 local moving coordinate system vi+1 , establish the coordinate system {V i+1 Assume that the intersection of the vertical direction of the center of each ball joint base and the parallel direction of the axis of the X-axis motor is the origin of the PPPS attitude adjustment mechanism O Sj .

5. The control method based on a mobile six-degree-of-freedom platform according to claim 4, characterized in that: The step 1 further includes: using {x, y, z} to represent the local moving coordinate system {V i }In the global base coordinate system {B i }, change {V i }First go around {B i } is rotated by γ around the Z axis, then by β around the Y axis, and then by α around the X axis. The local moving coordinate system {V i }In the global base coordinate system {B i }, and the posture of the attitude adjustment platform is represented by {x, y, z, α, β, γ}.

6. The control method based on a mobile six-degree-of-freedom platform according to claim 5, characterized in that: The second step includes: using j = a, b, c, d to represent four PPPS posture adjustment mechanisms, setting the origin of the PPPS posture adjustment mechanism O Sj The position in base coordinate system i is The origin position of the local moving coordinate system i+1 is 7. The control method based on a mobile six-degree-of-freedom platform according to claim 6, characterized in that: The step three includes: using n x , n y , n z , o x , o y , o z , a x , a y , a z represents the rotation component of the rotation matrix, then [a x a y a z ]=[n x n y n z ]×[o x o y o z ], the positive kinematic posture is The positive kinematic position is The rotation matrix of local moving coordinate system i in global base coordinate system i is The homogeneous matrix of local moving coordinate system i in global base coordinate system i is The homogeneous matrix is ​​the transformation matrix from the initial base coordinate system No. i to the initial moving coordinate system No. i.

8. The control method based on a mobile six-degree-of-freedom platform according to claim 7, characterized in that: The fourth step includes: assuming that the change matrix from the i+1 moving coordinate system obtained by hand-eye calibration to the i+1 camera is The change matrix from step 3 By multiplying, we can get the pose change matrix of the camera of the i+1 attitude adjustment platform in its base coordinate system: The step 5 includes: assuming that the camera of the i+1 attitude adjustment platform recognizes the pose matrix of the i-th local moving coordinate system as Multiplying it with the pose change matrix in step 4, we get the change matrix from the initial base coordinate system i+1 to the initial moving coordinate system i, that is The step six includes: assuming that the homogeneous matrix of the i+1 moving platform coordinate system in the i moving coordinate system is Multiply it with the pose change matrix in step 5 to get the change matrix from the initial base coordinate system i+1 to the target moving coordinate system i+1 Calculate the displacement of each PPPS posture adjustment mechanism △P j ,Right now

Images