A blind search method for the hypotenuse of a right triangle in aerospace shaft-hole assembly

Abstract

The present invention discloses a blind search method for the hypotenuse of a right triangle in aerospace shaft-hole assembly. The rough positioning position is determined by visual information, and a search coordinate system, a robotic arm base coordinate system and an end tool coordinate system are established. The robotic arm is compliantly controlled to move to the rough positioning position, and a blind search is performed until a collision occurs, and the collision point is recorded. The robotic arm performs a light-touch movement in all four directions, and obtains the collision force in real time, and determines the position of the collision point by the change in force. Then, a corresponding blind search shaft-hole alignment strategy is set to achieve alignment of the outer circle contours of the shaft and hole plug-in, and compliant docking of the shaft and hole. The present invention realizes an efficient blind search capability for high-precision shaft-hole assembly for aerospace, has high search accuracy, especially considering the side collision phenomenon, and has good engineering applicability and universality. It is easy to implement and has high accuracy. It can be widely used in the field of on-orbit assembly of aerospace shaft holes, and greatly improves the accuracy and efficiency of shaft hole alignment during on-orbit assembly of shaft holes.

Description

Technical Field

[0001] The invention belongs to the field of on-orbit fine operation of space robots, and in particular relates to a blind search method for the hypotenuse of a right triangle in an aerospace shaft-hole assembly. Background Art

[0002] On-orbit service and maintenance missions involve a large number of pin and electrical connector assembly operations, and shaft-hole assembly is the core technology for completing this on-orbit assembly. Some shaft holes are located in confined spaces, have small geometric assembly tolerances, and feature abrupt interface changes, making them extremely difficult for astronauts to operate. There is an urgent need to utilize the precise manipulation of space robots to automatically align and assemble the shaft holes. On the one hand, due to the complex lighting conditions in space and the obstruction of vision caused by the confined operating space, most operating objects are not clearly marked, resulting in a lack of reliability in video information. Often, only auxiliary functions such as coarse positioning can be provided. After coarse positioning, the space robot's fine manipulation requires a blind search strategy. On the other hand, the small tolerances, high precision requirements, and difficult alignment of shaft holes used in aerospace applications place high demands on the accuracy and effectiveness of blind search. For example, the on-orbit assembly of the J599 electrical connector at a certain location on a satellite is a typical shaft-hole assembly, and the assembly accuracy directly affects the normal operation of the satellite's power system.

[0003] The blind search of shaft-hole assembly has attracted the attention of scholars. Earlier literature [Jokesch M, J, Winkler A, Fross A, Thomas U. Generic algorithm for peg-in hole assembly tasks for pin alignments with impedance controlled robots [C] / / Second Iberian Robotics Conference, Springer, Cham, 2016, 418: 105-117]. They proposed a helical-based blind search strategy to address the flexible assembly of multi-pole charging plugs for electric vehicles. This strategy enabled blind search and insertion / removal of a five-pole charging plug through force and position compensation. The article [Zhang Y, Lu H, Pham DT, Wang Y, Qu M, Lim J, Su S. 2019 Peg–hole disassembly using active compliance. R. Soc. Open Sci. 6: 190476] discloses a method for adjusting assembly errors of a robotic arm based on contact states. This method categorizes contact states into single-point, multi-point, and line contact, analyzes the dynamic changes in force under different contact states, and uses this as a criterion for adjusting the end-arm position. The invention patent application with publication number CN102218652A, which utilizes the flexibility of a robot to achieve shaft-hole assembly, discloses a device and method for installing a shaft into a hole on a workpiece when the position and orientation of the workpiece cannot be precisely fixed. The robot adjusts the shaft and the hole to at least a three-point contact state, and then precisely adjusts the position of the hole.

[0004] The above-mentioned blind operation methods and devices are all aimed at blind searches with a fixed base position, and the shaft-hole contact end face range is wide, and the shaft-hole tolerance is large. At the same time, there is currently no literature that considers the possibility of side collisions between shaft holes, which is not suitable for solving the problem of precise assembly of high-precision shaft holes for aerospace use.

[0005] At present, the in-orbit assembly of high-precision shaft holes for aerospace is performed by astronauts outside the cabin, which is time-consuming and difficult. There is an urgent need for a blind search method for the hypotenuse of a right triangle in aerospace shaft hole assembly that takes side collisions into consideration and is suitable for the precise operation of space robots. Summary of the Invention

[0006] The technical purpose of the present invention is to provide a blind search method for the hypotenuse of a right triangle in aerospace shaft-hole assembly, so as to solve the problem of low efficiency and high difficulty in assembling on-orbit shaft and hole plug-ins.

[0007] In order to solve the above problems, the technical solution of the present invention is:

[0008] A blind search method for the hypotenuse of a right triangle in an aerospace shaft-hole assembly includes the following steps:

[0009] S1: Determine the rough positioning position A based on the visual information, which is used as the origin and search starting position of the search coordinate system, and establish the search coordinate system O at point A. A x A y A z A ; At the same time, establish the robot arm base coordinate system O b x b y b z b and the end tool coordinate system O with the tool end center as the origin e x e y e z e ;

[0010] S2: The compliant control robot arm moves from the initial position to the rough positioning position A, and then moves along the blind search trajectory until the shaft and the hole plug-in have an initial contact collision, and the contact collision point C1 of the shaft and hole is recorded;

[0011] S3: Control the robotic arm to perform light-touch movements around the perimeter and obtain the contact collision force in real time. The specific position of the contact collision point C1 between the shaft and the hole on the hole plug-in is determined based on the changes in the collected contact collision force.

[0012] S4: According to the position of the contact collision point C1 between the shaft and the hole, a corresponding blind search shaft-hole alignment strategy is set to achieve the alignment of the outer circle contours of the shaft and hole plug-in;

[0013] S5: Perform smooth docking of the shaft and hole, determine the end point of the electrical connector assembly, record the changes in contact force and torque, contact state, and robot arm joint displacement throughout the entire process, and output the assembly time.

[0014] In step S1, the rough positioning position A is located above the end face of the hole insert, and the end tool coordinate system O e x e y e z e z e The axis is the position direction of the tool pointing to the hole along the end of the robot arm, and the search coordinate system O A x A y A z A z A Axis direction and z e The axis direction is consistent.

[0015] Among them, in step S2, the trajectory planning of the compliant control manipulator from the initial position to the rough positioning position A is realized by quintic polynomial interpolation, and the formula is as follows:

[0016] θ(t)=a0+a1t+a2t2 +a3t 3 +a4t 4 +a5t 5

[0017] θ & (t) = a1 + 2a2t + 3a3t 2 +4a4t 3 +5a4t 4

[0018] θ && (t) = 2a2 + 6a3t + 12a4t 2 +20a5t 3

[0019] Among them, θ is the joint angle, t is the time, and a0, a1, a2, a3, a4, and a5 are coefficients.

[0020] In step S2, the input of the compliance control is the six-dimensional force sensor measurement value F (f x , f y , f z ), and the rotation angles of the six joints of the manipulator (θ1, θ2, θ3, θ4, θ5, θ6). The output of the compliant control is the displacement deviation (Δx, Δy, Δz). The corresponding impedance dynamics control equation is:

[0021]

[0022]

[0023]

[0024] Among them, M, s, and K are the equivalent mass, damping, and stiffness of each degree of freedom, respectively.

[0025] Among them, in step S2, the blind search trajectory adopts spiral search, which is realized based on the advancing spiral line. The starting radius of the spiral line trajectory can cover the end face range of the hole insert, and the spiral line radius gradually decreases. The spiral line trajectory S0 is:

[0026] r=|r0-λwt / (2*pi)|

[0027] x=r*cos(wt1)-x0

[0028] y=r*sin(wt1)-y0

[0029] z=z0-v s *t1 / (2*pi)

[0030] Among them, r0 is the starting radius of the Archimedean spiral, λ is the pitch of the Archimedean spiral, t1 is the search time, v s is the speed in the z direction;

[0031] The spiral trajectory S0 is based on the search coordinate system O A x A y A z A , the coordinates need to be transformed to the base coordinate system O b x b y b z b , the coordinate transformation relationship is:

[0032]

[0033] in, is the trajectory expression in the base coordinate system, The coordinate transformation matrix from the search coordinate system to the base coordinate system.

[0034] In step S3, obtaining the contact collision point C1 between the shaft and the hole specifically includes the following steps:

[0035] S301: Record the direction r0 of the resultant force collected by the six-dimensional force sensor, and the direction r0 and the end tool coordinate system O e x e y e z e z e The axes form a plane M0;

[0036] S302: Record z in plane M0 e Normal to the axis r e , normal r e Point to the base of the robotic arm;

[0037] S303: Make the end of the robot arm move along the normal direction r e Make a small movement in the direction to collect force data F e1 And judge whether it is 0. If it is 0, it means that the contact collision point C1 between the shaft and the hole is at r e There is no constraint on the direction, that is, the initial contact point is located at the end face of the hole plug; if it is not 0, the end of the robot arm is moved along -r e Make a small movement in the direction to collect force data F e2 And judge whether it is 0. If it is 0, it means that the contact collision point C1 of the shaft and hole is at -r e There is no constraint on the direction, that is, the initial contact point is located on the side of the hole plug-in. If it is not 0, it means that there are constraints around the shaft plug-in. At this time, the shaft and hole plug-in are in the center alignment state.

[0038] Among them, in step S4, when the contact collision point C1 of the shaft and hole is on the end face of the hole insert, the corresponding blind search shaft and hole alignment strategy is as follows:

[0039] S401: Obtain the current search coordinate system O according to the joint angle, the size of the robot arm and the end tool A x A y A z A The coordinates in the search coordinate system O A x A y A z A Set the end face search strategy F1 on the end face of the hole insert;

[0040] S402: Keep the contact force between the shaft and the hole plug unchanged, adjust the yaw angle, and search for the yaw angle. After searching for one week, the F value collected in real time by the force sensor is z Change calculation F z The position P1 corresponding to the maximum value point of , returns the robot arm posture to point P1;

[0041] S403: Keep the contact force between the shaft and the hole plug unchanged, adjust the pitch angle, and search for the pitch angle. After searching for one week, the F value collected in real time by the force sensor is z Change calculation F z The position P1 corresponding to the maximum value point of , returns the robot arm posture to point P1;

[0042] S405: searching for a critical point B on the outer edge of the hole insert from the initial position along the end face r1 direction according to a judgment strategy for the end face critical point;

[0043] S405: searching from critical point B along the end surface -r1 direction to the critical point C on the outer edge of the hole insert according to the judgment strategy of the end surface critical point;

[0044] S406: searching for a critical point D on the outer edge of the hole insert from the critical point C along the normal direction of the end face r1 according to the end face critical point judgment strategy;

[0045] S407: Perform a light touch search along the oblique side BD direction, and move to P1 along the oblique side BD in the first action. The second action is based on impedance control and performs a light touch movement along the normal direction of the end face to P1+dz. If there is no contact force in the normal direction, it indicates that the center of the shaft hole is aligned. At this time, return to P1 in the third action. Otherwise, it indicates that the center of the shaft hole is aligned.

[0046] Among them, the judgment strategy of the end face critical point is as follows:

[0047] A1: Get the current search coordinate system O based on the joint angle, the size of the robot arm and the end tool A x A yA z A The coordinates in the search coordinate system O A x A y A z A Set up a right triangle hypotenuse search strategy on the end face of the hole plug;

[0048] A2: Move along the specified xy plane direction by the corresponding step length;

[0049] A3: Move along the positive z direction to achieve light contact between the shaft and the hole, and collect the contact collision force. If there is contact force in the z direction but no contact force in the x and y directions, return to step A2; otherwise, perform the following steps.

[0050] A4: Perform a light touch judgment around the perimeter. If the contact point is at the critical point of the hole plug-in edge, the search ends. Otherwise, return to the initial contact point and readjust the search direction.

[0051] Among them, the four-sided light touch judgment is specifically:

[0052] B1: Determine the coordinate position of the initial contact point, keep the z direction unchanged, and gradually move along the positive x and y directions until collision contact occurs, and collect the force data of the contact state;

[0053] B2: Gradually move along the negative z direction and repeat step B1 to obtain the set of contact collision forces in the positive x and y directions within the z variation range;

[0054] B3: Repeat steps B1 and B2 in the x and y directions respectively to obtain the set of contact collision forces within the x and y variation ranges.

[0055] Among them, in step S4, when the contact collision point C1 of the shaft and hole is on the side of the hole insert, the corresponding blind search shaft and hole alignment strategy is as follows:

[0056] S411: Obtain the current search coordinate system O according to the joint angle, the size of the robot arm and the end tool A x A y A z A The side-end blind search strategy F2 of light-touch climbing is set on the end face.

[0057] S412: Keep the contact force between the shaft and the hole plug unchanged, adjust the yaw angle, and search for the yaw angle. After searching for one week, the T value collected in real time by the force sensor is z Change calculation T z The position P2 corresponding to the maximum value point of , returns the robot arm posture to point P2;

[0058] S413: Keep the contact force between the shaft and the hole plug unchanged, adjust the pitch angle, and search for the pitch angle. After searching for one week, the force sensor collects the Change calculation The position corresponding to the maximum value point of P3 is to return the robot arm posture to P3;

[0059] S414: Record the force direction r2 of the initial collision point, the preset displacement away from the contact surface in the direction of r2, and then move forward along the z-axis toward the end surface by a preset step length;

[0060] S415: Move the preset displacement along the -r2 direction close to the contact surface to determine whether there is contact. If there is contact, the surface has not yet reached the end plane, and the process returns to step S414. Otherwise, it indicates that the end of the shaft has reached the end direction.

[0061] Due to the adoption of the above technical solution, the present invention has the following advantages and positive effects compared with the prior art:

[0062] The present invention realizes an efficient blind search capability for high-precision shaft hole assembly for aerospace, has high search accuracy, especially considering the side collision phenomenon existing in the aerospace shaft hole assembly process, and has good engineering applicability and universality, is easy to implement and has high accuracy. It can be widely used in the field of on-orbit assembly of aerospace shaft holes, and greatly improves the accuracy and efficiency of shaft hole alignment during the on-orbit assembly of shaft holes. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Various other advantages and benefits will become apparent to those skilled in the art by reading the following detailed description of the preferred embodiment.The accompanying drawings are only for the purpose of illustrating the preferred embodiment and are not to be considered as limiting the present invention.

[0064] Figure 1 This is a schematic diagram of the distribution of the axis hole assembly coordinate positions of the present invention;

[0065] Figure 2 This is a flow chart for determining the position of the initial contact collision point of the present invention;

[0066] Figure 3 This is a flow chart of the blind search strategy for the initial contact collision point of the end face of the present invention;

[0067] Figure 4 This is a flow chart of the strategy for determining the boundary point of the end face of the present invention;

[0068] Figure 5 This is a flowchart of the four-way light-touch judgment of the present invention;

[0069] Figure 6 This is a flow chart of the blind search strategy for the initial contact collision point on the side of the present invention. DETAILED DESCRIPTION

[0070] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the specific embodiments of the present invention will be described below with reference to the accompanying drawings. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings and other embodiments can be obtained based on these drawings without inventive work.

[0071] To simplify the drawings, only the parts relevant to the present invention are schematically shown in each figure. They do not represent the actual structure of the product. Furthermore, to simplify the drawings and facilitate understanding, in some figures, only one of the components with the same structure or function is schematically depicted or labeled. As used herein, "one" not only means "only one" but also "more than one."

[0072] The following is a detailed description of a blind search method for the hypotenuse of a right triangle in an aerospace shaft hole assembly according to the present invention, with reference to the accompanying drawings and specific embodiments. The advantages and features of the present invention will become more apparent from the following description and claims.

[0073] Example

[0074] See Figures 1 to 6 This embodiment provides a blind search method for the hypotenuse of a right triangle in an aerospace shaft hole assembly, such as Figure 1 As shown, the implementation of this embodiment involves four elements: a robotic arm, a six-dimensional force sensor, a shaft insert, and a hole insert. The robotic arm is used to move the shaft insert from an initial position to a target position and dock with the hole insert. Specifically, the following steps are included:

[0075] First, in step S1, a coordinate system is established, such as Figure 1 As shown, the rough positioning position A is determined based on the visual information. The position A is located above the end face of the hole plug and is used as the origin of the search coordinate system to establish the search coordinate system O. A x A y A z A , which is also used as the search starting position. At the same time, the robot base coordinate system O is also established. b x b y b z b , and the end tool coordinate system O with the center of the tool end as the origin e x e y e z e Among them, the search coordinate system O A x A y A z A zA Axis direction and z e The axis direction is consistent, the end tool coordinate system O e x e y e z e z e The axis is the direction along which the end tool of the robot arm points to the hole.

[0076] Next, enter step S2, which requires compliant control of the manipulator to move from the initial position to the rough positioning position A, and then move along the blind search trajectory until the shaft and the hole plug-in have initial contact collision, and record the shaft and hole contact collision point C1. The joint space trajectory of the manipulator moving from the initial position to the rough positioning position A is planned in advance, and a quintic polynomial is used for interpolation, that is:

[0077] θ(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5

[0078] θ & (t) = a1 + 2a2t + 3a3t 2 +4a4t 3 +5a4t 4

[0079] θ && (t) = 2a2 + 6a3t + 12a4t 2 +20a5t 3

[0080] Among them, θ is the joint angle, t is the time, a0, a1, a2, a3, a4, a5 are coefficients, and the coefficients are determined according to the specific trajectory planning constraints.

[0081] Enable impedance compliance control, which is based on the impedance control method. The impedance control parameters can be adaptively adjusted according to the different contact states of the axis and hole to ensure the stability of the search process. The input of the compliance control is the six-dimensional force sensor measurement value F (f x , f y , f z ), and the rotation angles of the six joints of the manipulator (θ1, θ2, θ3, θ4, θ5, θ6). The output of the compliant control is the displacement deviation (Δx, Δy, Δz). The corresponding impedance dynamics control equation is:

[0082]

[0083]

[0084]

[0085] Among them, M, s, and K are the equivalent mass, damping, and stiffness of each degree of freedom, respectively.

[0086] Next, the trajectory of the robot arm from the pre-search position A to the contact collision point is planned. The blind search trajectory adopts spiral search, which is implemented based on the forward spiral line. The starting radius of the spiral trajectory can cover the end face range of the hole plug-in, and the spiral radius gradually decreases. The spiral trajectory S0 is:

[0087] r=|r0-λwt / (2*pi)|

[0088] x=r*cos(wt1)-x0

[0089] y=r*sin(wt1)-y0

[0090] z=z0-v s *t1 / (2*pi)

[0091] Among them, r0 is the starting radius of the Archimedean spiral, λ is the pitch of the Archimedean spiral, t1 is the search time, v s is the speed in the z direction.

[0092] The spiral trajectory S0 is based on the search coordinate system O A x A y A z A , the coordinates need to be transformed to the base coordinate system O b x b y b z b , the coordinate transformation relationship is:

[0093]

[0094] in, is the trajectory expression in the base coordinate system, The coordinate transformation matrix from the search coordinate system to the base coordinate system.

[0095] After executing step S2, the initial contact collision point C1 of the shaft and hole may be at the end face, side face, or directly aligned with the hole insert. Therefore, through step S3, the specific position of the contact collision point C1 of the shaft and hole on the hole insert is found, such as Figure 2 As shown, step S3 can be divided into: recording the direction r0 of the resultant force collected by the six-dimensional force sensor, the direction r0 and the end tool coordinate system O e x e y e z e z e The axes form a plane M0. Record z in plane M0e Normal to the axis r e , normal r e Pointing to the base of the robotic arm. Control the end of the robotic arm along the normal r e Make a small movement in the direction to collect force data F e1 And judge whether it is 0. If it is 0, it means that the contact collision point C1 between the shaft and the hole is at r e There is no constraint on the direction, that is, the initial contact point is located at the end face of the hole plug. If it is not 0, the end of the robot arm is controlled to move along -r e Make a small movement in the direction to collect force data F e2 And judge whether it is 0. If it is 0, it means that the contact collision point C1 of the shaft and hole is at -r e There is no constraint on the direction, that is, the initial contact point is located on the side of the hole plug-in. If it is not 0, it means that there are constraints around the shaft plug-in. At this time, the shaft and hole plug-in are in the center alignment state.

[0096] See Figure 3 According to the position of the contact collision point C1 between the shaft and the hole, the corresponding blind search shaft-hole alignment strategy is set to achieve the alignment of the outer contour center of the shaft and the hole plug-in. When the contact collision point C1 between the shaft and the hole is on the end face of the hole plug-in, the blind search strategy of the initial contact point of the end face is adopted, specifically:

[0097] 1) Get the current search coordinate system O according to the joint angle, the size of the robot arm and the end tool A x A y A z A The coordinates in the search coordinate system O A x A y A z A Set the end face search strategy F1 on the end face of the hole insert.

[0098] 2) Keep the contact force between the shaft and the hole plug unchanged, and adjust the yaw angle (y around the end tool coordinate system) e Axis deflection), yaw angle search, after searching for one week, the F z Change calculation F z The position P1 corresponding to the maximum point of is set, and the robot arm posture returns to point P1, and the yaw angle adjustment is completed.

[0099] 3) Keep the contact force between the shaft and the hole plug unchanged, and adjust the pitch angle (around the x of the end tool coordinate system) e Axis deflection), the pitch angle search is performed, and after searching for one week, the F z Change calculation F z The position P1 corresponding to the maximum value point of , makes the robot arm posture return to P1, and the pitch angle adjustment is completed.

[0100] 4) From the initial position, search for the critical point B on the outer edge of the hole plug-in along the end face r1 direction according to the judgment strategy of the end face critical point.

[0101] 5) From the critical point B, search for the critical point C on the outer edge of the hole plug-in along the end face -r1 direction according to the judgment strategy of the end face critical point.

[0102] 6) Search from the critical point C along the normal direction of the end face r1 to the critical point D on the outer edge of the hole plug according to the judgment strategy of the end face critical point;

[0103] 7) Perform a light touch search along the oblique edge BD direction, and move to P1 along the oblique edge BD in the first action. The second action is based on impedance control and performs a light touch movement along the end face normal direction (z direction) to P1+dz. If there is no contact force in the z direction, it indicates that the center of the shaft hole is aligned. At this time, return to P1 in the third action. Otherwise, it indicates that the center of the shaft hole is aligned.

[0104] See Figure 4 The specific strategy for determining the end face boundary point mentioned in the above steps is:

[0105] 1) Get the current search coordinate system O according to the joint angle, the size of the robot arm and the end tool A x A y A z A The coordinates in the search coordinate system O A x A y A z A Set the right triangle hypotenuse search strategy F1 on the end face of the hole plug.

[0106] 2) Move the corresponding step size along the specified xy plane direction.

[0107] 3) Move in the positive z direction to achieve light contact between the shaft and the hole insert, and collect the contact collision force. If there is contact force in the z direction but no contact force in the x and y directions, return to step 2. Otherwise, perform the following steps.

[0108] 4) Perform a light touch judgment around the periphery. If the contact point is located at the critical point of the hole plug-in edge, the search ends. Otherwise, return to the initial contact point and readjust the search direction.

[0109] See Figure 5 The four-way touch judgment mentioned in the above steps is specifically as follows:

[0110] 1) Determine the coordinate position of the initial contact point, keep the z direction unchanged, and gradually move along the positive x and y directions until collision contact occurs, and collect the force data of the contact state.

[0111] 2) Gradually move along the negative z direction and repeat the above steps to obtain the set of contact collision forces in the positive x and y directions within the z variation range.

[0112] 3) Repeat steps 1) and 2) in the x and y directions respectively to obtain the contact collision force set within the x and y variation ranges.

[0113] See Figure 6 In another case of step S4, when the contact collision point C1 between the shaft and the hole is on the side of the hole insert, the blind search strategy for the initial contact point on the side is as follows:

[0114] 1) Get the current search coordinate system O according to the joint angle, the size of the robot arm and the end tool A x A y A z A The side-end blind search strategy F2 of light-touch climbing is set on the end face.

[0115] 2) Keep the contact force between the shaft and the hole plug unchanged, and adjust the yaw angle (y around the end tool coordinate system) e Axis deflection), yaw angle search, after searching for one week, the T z Change calculation T z The position P2 corresponding to the maximum value point of is used to return the robot arm to point P2, and the yaw angle adjustment is completed.

[0116] 3) Keep the contact force between the shaft and the hole plug unchanged, and adjust the pitch angle (around the x of the end tool coordinate system) e Axis deflection), the pitch angle search is performed, and after searching for one week, the real-time data collected by the force sensor is Change calculation The position P3 corresponding to the maximum value point of is used to return the robot arm to point P3, and the pitch angle adjustment is completed.

[0117] 4) Record the force direction r2 of the initial collision point, the preset displacement away from the contact surface in the direction r2, and then move forward along the z-axis toward the end surface for a preset step length;

[0118] 5) Determine whether there is contact by moving the preset displacement close to the contact surface in the -r2 direction. If there is contact, the surface has not yet reached the end plane, and return to step 4). Otherwise, it indicates that the end of the shaft has reached the end direction.

[0119] Finally, the shaft and hole are smoothly docked, and the end point of the electrical connector assembly is determined. This determination is based on the force threshold measured by the six-dimensional torque sensor at the end of the robotic arm. During the alignment process, data on changes in contact force and torque, contact state, and robotic arm joint displacement are recorded. This data is saved as an ".xls" file and written to a specified location. The assembly time is also output.

[0120] The embodiments of the present invention have been described in detail above with reference to the accompanying drawings, but the present invention is not limited to the above embodiments. Even if various changes are made to the present invention, if these changes fall within the scope of the claims of the present invention and their equivalents, they still fall within the scope of protection of the present invention.

Claims

1. A blind search method for the hypotenuse of a right triangle in an aerospace shaft hole assembly, characterized in that: The steps include: S1: Determine the rough positioning position based on visual information , used as the origin of the search coordinate system and the search starting position, at point Establish a search coordinate system ; At the same time, establish the robot arm base coordinate system and the end tool coordinate system with the center of the tool end as the origin ; S2: Softly control the robot arm to move from the initial position to the rough positioning position , and then move along the blind search trajectory until the shaft and hole plug-in have initial contact collision, and record the shaft and hole contact collision point ; S3: Control the robotic arm to perform light touch movements around the perimeter and obtain the contact collision force in real time. Determine the contact collision point of the shaft and hole based on the changes in the collected contact collision force. The specific location on the hole insert; S4: According to the contact collision point of the shaft and hole Set the corresponding blind search axis-hole alignment strategy to achieve the alignment of the outer circle contours of the axis and hole plug-in; S5: Perform smooth docking of the shaft and hole, determine the end point of the electrical connector assembly, record the changes in contact force and torque, contact state, and joint displacement of the robotic arm throughout the entire process, and output the assembly time. Wherein, in said step S4, when the shaft and the hole contact the collision point When on the end face of the hole insert, the corresponding blind search axis and hole alignment strategy is as follows: S401: Get the current search coordinate system based on the joint angle, the size of the robot arm and the end tool The coordinates in the search coordinate system Set the end face search strategy on the end face of the hole insert ; S402: Keep the contact force between the shaft and the hole plug unchanged, adjust the yaw angle, and search for the yaw angle. After searching for one week, the force sensor collects the Change calculation The position corresponding to the maximum point of , so that the robot arm position returns to point; S403: Keep the contact force between the shaft and the hole plug unchanged, adjust the pitch angle, and search for the pitch angle. After searching for one week, the force sensor collects the Change calculation The position corresponding to the maximum point of , so that the robot arm position returns to point; S404: From the initial position along the end surface Direction searches for the critical point B on the outer edge of the hole plug-in according to the judgment strategy of the end face critical point; S405: From critical point B along the end surface Direction searches for the critical point C on the outer edge of the hole plug-in according to the judgment strategy of the end face critical point; S406: From critical point C along the end surface The normal direction searches for the critical point D on the outer edge of the hole plug-in according to the judgment strategy of the end face critical point; S407: Perform a light touch search along the oblique edge BD. In the first action, move along the oblique edge BD to The second action is based on impedance control and is carried out in a light touch manner along the normal direction of the end face. If there is no contact force in the normal direction, it means that the shaft and hole centers are aligned. At this time, the shaft returns to the center in the third action. , otherwise, it indicates that the shaft hole centers are aligned.

2. The blind search method for the hypotenuse of a right triangle in an aerospace shaft hole assembly according to claim 1, characterized in that: In step S1, the rough positioning position Located above the end face of the hole insert, the end tool coordinate system of The axis is the direction of the hole along the end tool of the robot arm, and the search coordinate system of Axis direction and The axis direction is consistent.

3. The blind search method for the hypotenuse of a right triangle in an aerospace shaft hole assembly according to claim 1, characterized in that: In step S2, the flexible control robot arm moves from the initial position to the rough positioning position The trajectory planning is realized by quintic polynomial interpolation, and the formula is as follows in, is the joint angle, For time, is the coefficient.

4. The blind search method for the hypotenuse of a right triangle in an aerospace shaft hole assembly according to claim 1, characterized in that: In step S2, the input of the compliance control is the measurement value of the six-dimensional force sensor at the wrist of the manipulator. , and the rotation angle values ​​of the six joints of the robotic arm , the output of the compliant control is the displacement deviation , the corresponding impedance dynamics governing equation is: in, are the equivalent mass, damping and stiffness of each degree of freedom, respectively.

5. The blind search method for the hypotenuse of a right triangle in aerospace shaft hole assembly according to claim 1, characterized in that: In step S2, the blind search trajectory adopts a spiral search, which is realized based on the advancing spiral line. The starting radius of the spiral line trajectory can cover the end face range of the hole plug-in, and the spiral line radius gradually decreases. for: in, is the starting radius of the Archimedean spiral, is the pitch of the Archimedean spiral, For a week's search duration, is the moving speed in the z direction; spiral trajectory Based on the search coordinate system , the coordinates need to be transformed to the base coordinate system , the coordinate transformation relationship is: in, is the trajectory expression in the base coordinate system, The coordinate transformation matrix from the search coordinate system to the base coordinate system.

6. The blind search method for the hypotenuse of a right triangle in aerospace shaft hole assembly according to claim 1, characterized in that: In step S3, the shaft and the hole contact the collision point The specific steps of obtaining include: S301: Record the direction of the resultant force collected by the six-dimensional force sensor ,direction With the end tool coordinate system of Axis plane ; S302: Recording plane Inside Normal to axis , normal Point to the base of the robotic arm; S303: Make the end of the robot arm move along the normal direction Make small movements in the direction to collect force data And judge whether it is 0. If it is 0, it indicates that the shaft and hole are in contact with the collision point. exist There is no constraint on the direction, that is, the initial contact point is located at the end face of the hole plug; if it is not 0, the end of the robot arm is moved along Make small movements in the direction to collect force data And judge whether it is 0. If it is 0, it indicates that the shaft and hole are in contact with the collision point. exist There is no constraint on the direction, that is, the initial contact point is located on the side of the hole plug-in. If it is not 0, it means that there are constraints around the shaft plug-in. At this time, the shaft and hole plug-in are in the center alignment state.

7. The blind search method for the hypotenuse of a right triangle in aerospace shaft hole assembly according to claim 1, characterized in that: The judgment strategy of the end face critical point is as follows: A1: Get the current search coordinate system based on the joint angle, the size of the robot arm and the end tool The coordinates in the search coordinate system Set up a right triangle hypotenuse search strategy on the end face of the hole plug; A2: Move along the specified xy plane direction by the corresponding step length; A3: Move along the positive z direction to achieve light contact between the shaft and the hole, and collect the contact collision force; if there is contact force in the z direction but no contact force in the x and y directions, return to step A2; otherwise, perform the following steps; A4: Perform a light touch judgment around the perimeter. If the contact point is at the critical point of the hole plug-in edge, the search ends. Otherwise, return to the initial contact point and readjust the search direction.

8. The blind search method for the hypotenuse of a right triangle in a shaft hole assembly for aerospace use according to claim 7, characterized in that: The specific four-way touch judgment is B1: Determine the coordinate position of the initial contact point, keep the z direction unchanged, and gradually move along the positive x and y directions until collision contact occurs, and collect the force data of the contact state; B2: gradually move along the negative z direction and repeat step B1, thereby obtaining the set of contact collision forces moving in the positive x and y directions within the z variation range; B3: Repeat steps B1 and B2 in the x and y directions respectively to obtain a set of contact collision forces within the x and y variation ranges.

9. The blind search method for the hypotenuse of a right triangle in a shaft hole assembly for aerospace use according to claim 8, characterized in that: In step S4, when the shaft and the hole contact the collision point When on the side of the hole insert, the corresponding blind search axis and hole alignment strategy is as follows: S411: Get the current search coordinate system based on the joint angle, the size of the robot arm and the end tool The coordinates in the end face are set to set the side-end face blind search strategy of light-touch climbing. ; S412: Keep the contact force between the shaft and the hole plug unchanged, adjust the yaw angle, and search for the yaw angle. After searching for one week, the force sensor collects the Change calculation The position corresponding to the maximum point of , so that the robot arm position returns to point; S413: Keep the contact force between the shaft and the hole plug unchanged, adjust the pitch angle, and search for the pitch angle. After searching for one week, the force sensor collects the Change calculation The position corresponding to the maximum point of , so that the robot arm position returns to point; S414: Record the force direction of the initial collision point ,along A preset displacement in the direction away from the contact surface, followed by a preset step length along the z-axis toward the end surface; S415: Along The preset displacement of the direction close to the contact surface is used to determine whether there is contact. If there is contact, the surface has not yet reached the end surface plane, and the process returns to step S414; On the contrary, it indicates that the end of the shaft reaches the end face direction.

Images