Robot force perception assembly method and system for spatial electrical connector in any attitude

Abstract

The application belongs to the technical field of automatic control, and particularly discloses a robot force perception assembly method for a space electrical connector under an arbitrary posture, which comprises the following steps: after single-point contact of a plug and a socket, a reverse conical diffusion motion is adopted to identify a deflection angle range of the plug and the socket in combination with a contact torque mutation condition; if the deflection angle range is small deflection angle contact, then corresponding plug and socket posture information is recorded according to a contact torque mutation direction, that is, the corresponding plug and socket are in a double-point contact state; if the deflection angle range is large deflection angle contact, then single-point contact is judged to be bottom surface contact or side surface contact according to a spiral motion track tracking error; in the double-point contact state, expected contact force along the tail end is set to guide the plug to move along the surface of the socket, so as to realize Z-axis alignment of the plug and the socket; and expected contact torque along the Z-axis of the plug is set to drive protection grooves of the plug and the socket to be aligned. The application guarantees that the space assembly task can still be completed under extreme conditions, and improves assembly efficiency.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of automatic control, and more particularly relates to a robot force perception assembly method and system for a space electrical connector in any attitude. BACKGROUND

[0002] At present, the on-orbit operation of space robots mainly focuses on the handling of large objects and the grasping of simple geometric shapes, and it is difficult to carry out fine operations on non-cooperative targets, especially non-cooperative targets with complex contact characteristics. From the perspectives of safety, efficiency and cost, it is far from enough for astronauts to complete complex and heavy space operations through extravehicular activities. In recent years, the on-orbit operating robots, which have been developing rapidly, play a vital role in human space activities, and their figures appear in various space equipment, greatly expanding the activity ability and exploration range of human beings in the universe. It is becoming a consistent approach for space powers around the world to use on-orbit operating robots to assist or replace astronauts to complete a large number of dangerous tasks. The robot screw assembly of the electrical connector is a non-identified space target with multiple point and multiple domain contact, which is the most complex space operation integrating the action sequences of guiding, positioning, pressing, locking, tightening and unlocking. Therefore, it is of great significance to realize the robot automatic assembly of space electrical connectors.

[0003] In order to solve the above problems, Chinese patent CN114571456A discloses an electrical connector assembly method and system based on robot skill learning, which adjusts the initial pose of the electrical connector to be assembled based on image information, controls the robot to contact the assembly hole with the electrical connector to be assembled and performs trajectory search; the pose of the electrical connector to be assembled carried by the robot end and the contact force and the joint angle of the next action are used as the input and output values of the skill learning network; the reward function in the network is learned based on the depth change in the robot assembly process, the network is iteratively converged, and the assembly depth and contact force are judged to complete the assembly process.

[0004] However, the electrical connector assembly method and system based on robot skill learning disclosed in Chinese patent CN114571456A need to use vision and force sensation as input at the same time. Although this method realizes the assembly task of the robot clamping the electrical connector, it does not consider the influence of the single-point alternating light source (sunlight) caused by the self-rotation of the near-earth satellite on the camera recognition in the actual assembly environment, the line-of-sight occlusion problem of the assembly contact point in the narrow space, and the search method when the spatial attitude of the assembly object is uncertain. The electrical connector assembly problem in the case of lacking visual assistance and uncertain assembly object attitude has not been solved. SUMMARY

[0005] In view of the above defects or improvement needs of the prior art, the application provides a robot force sensing assembly method and system under any posture of a space electrical connector, wherein a robot force sensing assembly method under any posture of a space electrical connector is designed in correspondence with the features of six-axis robot force control and the connection process characteristics of the plug and the socket of the electrical connector, after single-point contact of the plug and the socket, a reverse conical diffusion motion is adopted in combination with a contact torque mutation condition to identify the deflection angle range of the plug and the socket, so as to determine whether the torque in the XY direction is abrupt; if the deflection angle range is small deflection angle contact, that is, the torque in the XY direction is abrupt, the mutation corresponding plug and socket posture information is recorded according to the contact torque mutation direction, that is, the corresponding plug and socket are in a double-point contact state; if the deflection angle range is large deflection angle contact, that is, the torque in the XY direction is not abrupt, the single-point contact is judged to be bottom surface contact or side surface contact according to the spiral motion trajectory tracking error; if it is bottom surface contact, the end plane is moved to the side surface contact state; if it is side surface contact, the expected contact force of the unknown surface tangent is set according to the current contact force direction, the robot is guided to move along the position surface, and the trajectory gradient discontinuity point is monitored until the plug and socket in the hole are in a double-point contact state; in the double-point contact state, the expected contact force along the end is set to guide the plug to move along the socket surface, and the edge alignment and plane alignment motion constraint planes are identified, so as to realize Z-axis alignment of the plug and the socket; the expected contact torque along the Z axis of the plug is set to drive the alignment of the protection groove of the plug and the socket, and the force sensing assembly of the plug and the socket is realized after alignment. That is, the application proposes a search and assembly strategy under any posture (large initial deflection angle and small initial deflection angle) of the plug and the socket in the absence of visual assistance, improves the operation efficiency of the robot assembly task of the space electrical connector, and ensures that the space assembly task can still be completed under extreme conditions.

[0006] To achieve the above object, according to one aspect of the application, a robot force sensing assembly method under any posture of a space electrical connector is provided, comprising the following steps:

[0007] Step one, the plug and the socket are in any relative posture in space, the robot drives the plug to move, after single-point contact of the plug and the socket, a reverse conical diffusion motion is adopted in combination with a contact torque mutation condition to identify the deflection angle range of the plug and the socket, so as to determine whether the torque in the XY direction is abrupt;

[0008] Step two, if the deflection angle range is small deflection angle contact, that is, the torque in the XY direction is abrupt, the mutation corresponding plug and socket posture information is recorded according to the contact torque mutation direction, that is, the corresponding plug and socket are in a double-point contact state;

[0009] Step three, if the deflection angle range is a large deflection angle contact, that is, the torque in the XY direction does not change suddenly, then the single-point contact is judged to be bottom contact or side contact according to the spiral motion trajectory tracking error. If it is bottom contact, move along the end plane to the side contact state. If it is side contact, set the expected contact force tangential to the unknown surface according to the current contact force direction, guide the robot to move along the position surface, and monitor the trajectory gradient discontinuity point at the same time until the plug and socket in the hole are in a double-point contact state;

[0010] Step 4: In the double-point contact state, set the desired contact force along the end to guide the plug to move along the socket surface, and identify the edge alignment and plane alignment motion constraint planes to achieve Z-axis alignment of the plug and socket;

[0011] Step 5: Set the desired contact torque along the Z axis of the plug to drive the alignment of the protective grooves of the plug and the socket, and realize the force-sensing assembly of the plug and socket after alignment.

[0012] As a further preferred embodiment, in step 1, whether single-point contact is achieved is determined by detecting the magnitude of the current contact force;

[0013] When the single point is in contact, keep the position of the plug unchanged and make the center point of the robot's sixth axis P axis-6 =[x a6 ,y a6 ,z a6 ] changes according to the following rules:

[0014]

[0015] Where k0 is the proportional coefficient, w is the angular velocity, is the time step, x0, y0, z0 are P axis-6 The starting point.

[0016] As a further preferred embodiment, in step 1, when the initial relative posture deviation between the plug and the socket is small, the plug and the socket will have a double-point contact moment t * , at t * The reading of the force sensor at the moment, that is, the end contact force F c ∈R 6 The moment in the end plane (τ cx ,τ cy ) will mutate; and for the case of a large posture deflection angle, due to the limited inverted cone search step T, the plug and socket do not have a double-point contact state within a small contact range, so the contact torque (τ cx ,τ cy ) will not cause a sudden change, based on which it can be preliminarily identified whether the initial contact between the plug and the socket is a small deflection angle or a large deflection angle.

[0017] As a further preferred, in step two, for the case of small angle, the contact torque (τ cx ,τ cy ) in time series T is filtered and gradient calculated to obtain the plug pose record at time t * , which can obtain the state of double-point contact of plug and socket and further search.

[0018] As a further preferred, in step three, if the range of the angle is large angle, the spiral expected contact force of plug and socket is set over time, and the error of the current position and the expected position of the plug is obtained in real time, if the error is less than the threshold, it is the contact of the bottom surface of the plug and the top edge of the socket, otherwise, it is the contact of the edge of the plug and the side of the socket.

[0019] As a further preferred, the spiral expected contact force F d =[f dx ,f dy ,f dz ,0,0,0] T , wherein,

[0020]

[0021] By monitoring the error d(P c ,P d ) of the current position P c and the expected position P d of the plug:

[0022]

[0023] In the formula, f0,C is a constant, is the time step, and w is the angular velocity.

[0024] As a further preferred, in the case of side contact, the robot moves the direction of contact sensing of the unknown surface of the side, and in the process of contacting the unknown surface, the current end contact force F c The component (f cx ,f cy ) of the end plane is perpendicular to the unknown surface, therefore, by constructing the expected force F d along the tangent of the socket profile, and guiding the robot to move along the unknown surface until the trajectory gradient discontinuity point is obtained, the single-point contact of the plug and the socket at large angle is realized to double-point contact, and the pose of the plug at this time is obtained.

[0025] Wherein, the calculation formula of the expected force F d along the tangent of the socket profile includes:

[0026] F d =fdx +f dy = (f cx +f cy ) x R.UnitZ

[0027] wherein R.UnitZ is the unit component of the plug rotation matrix R ∈ SO(3) along the z direction.

[0028] As a further preferred, in step four, the expected contact force f dz = k1 * R.UnitZ in the direction of R.UnitZ is set as the expected contact force to drive the plug movement to guide the plug to move along the surface of the socket, in the process, the plane swept by the line connecting the center point of the sixth axis of the robot and the current position of the socket is the movement constraint plane of the plug and the socket, based on which the Z axis alignment of the plug and the socket, i.e. the plane alignment, is achieved;

[0029] wherein k1 is a gain parameter, and R.UnitZ is the unit component of the plug rotation matrix R ∈ SO(3) along the z direction.

[0030] As a further preferred, after the edge alignment and the plane alignment of the plug and the socket are achieved, the expected torque τ dz = k2 * R.UnitZ along R.UnizZ is set to drive the robot to find the protection slot, and the expected contact force f dz = k3 * R.UnitZ along R.UnizZ is set to ensure the close fit of the plug and the socket, wherein k2 and k3 are gain coefficients, when the protection slot alignment is identified, due to the existence of the expected contact force f dz , the plug will move along the protection slot, which in turn causes the current contact force f cz to suddenly decrease, therefore, by monitoring the height change and the force change of the plug, it can be determined whether the plug and the socket complete the protection slot alignment, and the force sensing assembly of the plug and the socket is achieved after the alignment.

[0031] According to another aspect of the present application, a robot force sensing assembly system for spatial electrical connectors in any attitude is also provided, comprising:

[0032] The first master control module is used for the case that the plug and the socket are in any relative attitude in space, the robot drives the plug movement, and after the plug and the socket contact at a single point, the plug and the socket are identified by using the reverse cone diffusion movement combined with the contact torque mutation to determine whether the torque in the XY direction suddenly changes.

[0033] The second master control module is used for the case that the plug and the socket are in a small angle range, i.e. the torque in the XY direction suddenly changes, and the plug and the socket pose information corresponding to the mutation is recorded according to the contact torque mutation direction, i.e. the plug and the socket are in a double-point contact state.

[0034] The third master module is used for judging single-point contact as bottom surface contact or side surface contact according to the helical motion trajectory tracking error in the case that the range of the deflection angle is large deflection angle contact, i.e. the moment of force in the XY direction does not change suddenly, if it is bottom surface contact, moving to the side surface contact state along the end plane, if it is side surface contact, setting the expected contact force of the unknown surface tangent according to the current contact force direction, guiding the robot to move along the position surface, and monitoring the trajectory gradient discontinuous point until the plug-in socket in the hole is in the double-point contact state;

[0035] The fourth master module is used for setting the expected contact force along the end in the double-point contact state, guiding the plug to move along the socket surface, and identifying the edge alignment and plane alignment motion constraint planes to realize the Z-axis alignment of the plug and the socket.

[0036] The fifth master module is used for setting the expected contact moment along the Z-axis of the plug to drive the alignment of the protection groove of the plug and the socket, and realizing the force sensing assembly of the plug and the socket after the alignment.

[0037] Overall, compared with the prior art, the above technical scheme conceived by the present application mainly has the following technical advantages:

[0038] 1. The present application proposes a search and assembly strategy for the plug and the socket in the case of lacking visual assistance under arbitrary pose (large initial deflection angle and small initial deflection angle), which improves the operation efficiency of the spatial electrical connector robot assembly task and ensures that the spatial assembly task can still be completed under extreme conditions.

[0039] 2. The present application gradually constrains the search space of the plug by constructing the search trajectory and the expected force, and in this process, the diameter size of the electrical connector is not constrained, which has good adaptability to electrical connectors of different sizes.

[0040] 3. The present application is carried out under the framework of admittance operation, so it has good environmental adaptability and can maintain the stability of the expected contact force when facing complex contact surfaces, avoiding damage to the electrical connector. BRIEF DESCRIPTION OF DRAWINGS

[0041] Figure 1 It is a spatial robot force-guided electrical connector geometric constraint identification and assembly workflow involved in the preferred embodiment of the present application;

[0042] Figure 2 It is a plug and socket plane contact schematic diagram of an aviation electrical connector in the embodiment of the present application;

[0043] Figure 3 It is a rounded cone search trajectory schematic diagram of the plug of the aviation electrical connector in the embodiment of the present application;

[0044] Figure 4 is a schematic diagram of the expected contact force of the plug of the aviation electrical connector in an embodiment of the present invention;

[0045] Figure 5 Schematic diagram of an unknown surface of a plug tracking socket of an aviation electrical connector in an embodiment of the present invention. DETAILED DESCRIPTION

[0046] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely for the purpose of explaining the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below may be combined with each other as long as they do not conflict with each other.

[0047] like Figure 1 As shown, an embodiment of the present invention provides a robot force sensing assembly method for a spatial electrical connector in an arbitrary posture. The assembly process is tested on a UR5 robot equipped with a six-dimensional force sensor. The force sensor has completed gravity compensation and is controlled by the admittance controller in Cartesian space by default. Of course, other driving devices for driving the movement of the plug and capable of implementing the method of the present invention are also applicable to the present invention. Here, only the UR5 robot is used as an example for specific description. Among them, the schematic diagram of the plug and socket is as follows Figure 2 The specific assembly steps include:

[0048] Step 1: The plug and socket are in any relative position in space. The robot drives the plug to move. After the plug and socket make single-point contact, the robot uses inverted cone diffusion motion combined with the sudden change of contact torque to identify the deflection range of the plug and socket to determine whether there is a sudden change in the torque in the XY direction.

[0049] In the initial state, the electrical connector is fixed to the end of the robot by a clamp, and the center point P of the plug end can be obtained through the robot's forward kinematics. The global coordinates and the robot base coordinates coincide, the position of the socket relative to the global coordinates is arbitrary, and the relative position of the plug and socket is also arbitrary. In this invention, it is assumed that the plug and socket have been roughly positioned by visual positioning to achieve the process of approaching the plug and socket. By monitoring the current relay sensor reading F c ∈R 6 Size||F c || Determine whether single point contact is achieved. When single point contact is achieved, maintain the current position P of the electrical connector socket plug c The robot's end position remains unchanged, so that the robot's sixth axis center point P axis-6 =[x a6 ,y a6 ,z a6 ]Changes according to the following rules

[0050]

[0051] where k0 is a proportional coefficient, w is angular velocity, is time step, x0, y0, z0 are the initial point of P axis-6 , the plug of the electrical connector will move along the trajectory shown in the specified time step. Figure 3

[0052] At the same time, when the initial relative pose deviation of the plug and the socket is small, the plug and the socket will have a double-point contact instant t * . At the moment t * , the reading of the force sensor, i.e., the end contact force F c ∈R 6 , the moment (τ cx , τ cy ) in the end plane will change abruptly. For the case of a large attitude deviation angle, since the inverted cone search step T is limited, the plug and the socket do not have a double-point contact state in a small contact range, so the contact moment (τ cx , τ cy ) will not change abruptly. Based on this feature, it can be preliminarily identified whether the initial contact of the plug and the socket is a small deviation angle or a large deviation angle.

[0053] That is, during the inverted cone expected trajectory movement, the plug and the socket are mostly in a single-point contact state. When the initial relative pose deviation of the plug and the socket is small, the plug and the socket will have a double-point contact instant t * . At the moment t * , the reading of the force sensor, i.e., the end contact force F c ∈R 6 , the moment (τ cx , τ cy ) in the end plane will change abruptly. For the case of a large attitude deviation angle, since the inverted cone search step T is limited, the plug and the socket do not have a double-point contact state in a small contact range, so the contact moment (τ cx , τ cy ) will not change abruptly. Based on this feature, it can be preliminarily identified whether the initial contact of the plug and the socket is a small deviation angle or a large deviation angle. In addition, by adjusting the parameters k o , T, the search range and the search speed can be controlled, and then the identification of the large and small inclination angles can be controlled.

[0054] Step two, if the deviation angle range is a small deviation angle contact, i.e., the moment of the XY direction changes abruptly, then according to the contact moment abrupt change direction, the plug and socket pose information corresponding to the abrupt change is recorded, i.e., the plug and socket are in a double-point contact state. Specifically:

[0055] ​For the case of small inclination, the contact force (τ cx ,τ cy ) in time sequence T is filtered and gradient is calculated to obtain the pose record of the plug at time t * , the state of double-point contact of the plug and the socket is obtained, and further search is performed.

[0056] Step three, if the range of the inclination is large inclination contact, i.e., the moment of force in the XY direction does not change abruptly, the single-point contact is judged to be bottom surface contact or side surface contact according to the tracking error of the spiral motion trajectory, if it is bottom surface contact, the end plane is moved to the side surface contact state, if it is side surface contact, the expected contact force of the unknown surface tangent is set according to the current contact force direction, the robot is guided to move along the position surface, and the trajectory gradient discontinuous point is monitored until the plug and socket in the hole are in a double-point contact state. Specifically,

[0057] For the case of large inclination, the spiral expected contact force F d of the electrical connector is set as a function of time dx , f dy , f dz , 0, 0, 0] T

[0058]

[0059] Where f0,C is a constant. is the time step. By monitoring the error d(P c ,P 3 ) of the current position P d ∈R 3 and the expected position P c ∈R d ,

[0060]

[0061] to monitor whether it is a plane contact, i.e., the plug bottom surface and the socket top edge contact, or an edge contact, i.e., the plug edge and the socket side surface contact.

[0062] That is, under the condition of ensuring the pose of the plug unchanged, when the bottom surface of the plug and the socket are in contact, due to the support of the socket to the plug plane, the tracking of the expected contact force of the plug in the end plane is weakly constrained by geometry, d(P c ,P d ) is small. When the edge of the plug and the side surface of the socket are in contact, due to the insufficient constraint of the side surface of the socket to the bottom edge of the plug (which may be out of contact or collide with the side surface of the socket during spiral motion), d(P c ,Pd ) is larger. By recognizing the difference of this parameter, it can be identified whether the plug and the socket are in edge contact or plane contact at any initial contact.

[0063] When it is detected that it is plane contact, the plug only needs to move in any direction under the end coordinate system to contact the side surface of the socket and convert to edge contact. When it is detected that it is edge contact, the next step is directly performed.

[0064] Step four, in the double-point contact state, the desired contact force along the end is set, the plug is guided to move along the surface of the socket, and the edge alignment and plane alignment motion constraint planes are recognized to realize the Z-axis alignment of the plug and the socket. Specifically:

[0065] In the case of edge contact, the robot moves in the direction of contact sensing of the unknown surface of the side surface. In the process of contacting the unknown surface, the current end contact force F c ∈R 6 The component (f cx ,f cy ) in the end plane is perpendicular to the unknown surface, therefore, by constructing the tangential desired force F d along the socket profile, as shown in Figure 4

[0066] f dx +f dy =(f cx +f cy )×R.UnitZ(0.4)

[0067] R.UnitZ is the unit component of the plug rotation matrix R∈SO(3) along the z direction

[0068] Since the plug is in a single-point contact state most of the time in the process of tracking the unknown surface of the socket, and there is only one double-point contact instant in the hole, when the single-point contact is converted to the double-point contact, since the constraint state changes suddenly, the tracking trajectory generated at this time is discontinuous, as shown in Figure 5 .

[0069] After realizing the double-point contact, the plug and the socket need to realize edge alignment and plane alignment. Therefore, the motion constraint space of the plug and the socket needs to be further constructed.

[0070] In the case of keeping the posture of the plug unchanged, by learning the visual auxiliary assembly operation process of the human hand, in the present application, the desired contact force f dz =k1*R.UnitZ is added along the direction of R.UnitZ, wherein k1 is a gain parameter. Further, the robot will move a distance along the upper surface of the socket by tracking f dz , P axis-6 -P c ​The plane swept by the connection is the movement constraint plane of the plug and the socket. The translational movement and the rotational movement of the edge alignment and the plane alignment of the plug and the socket are completed in the plane. As shown in Figure 5

[0071] Step five, set the desired contact torque along the Z axis of the plug to drive the alignment of the protection slot of the plug and the socket, and achieve the force sensing assembly of the plug and the socket after the alignment.

[0072] After the edge alignment and the plane alignment of the plug and the socket are achieved, the desired torque τ dz =k2R.UnitZ along the R.UnizZ is set to drive the robot six-axis rotation to find the protection slot. And the desired contact force f dz =k3R.UnitZ along the R.UnizZ is set to ensure the close contact of the plug and the socket, and k2, k3 are gain coefficients.

[0073] When the alignment of the protection slot is identified, since the desired contact force f dz exists, the plug will move along the protection slot, which will cause the current contact force f cz to change suddenly and decrease instantaneously. Therefore, by monitoring the height change and the force change of the plug, whether the plug and the socket complete the alignment of the protection slot can be determined. Further, whether the plug and the socket complete the search and assembly tasks other than the screwing can be determined.

[0074] According to another aspect of the present application, a robot force sensing assembly system for spatial electrical connectors in any attitude is also provided, which is used to execute the method of any embodiment or combination of multiple embodiments, comprising:

[0075] The first master control module is used to drive the plug to move by the robot when the plug and the socket are in any relative attitude in space. After the single-point contact of the plug and the socket, the conical diffusion movement is combined with the contact torque mutation to identify the range of the deflection angle of the plug and the socket, so as to determine whether the torque in the XY direction changes suddenly.

[0076] The second master control module is used to record the plug and socket attitude information corresponding to the double-point contact state of the plug and the socket according to the contact torque mutation direction in the case that the deflection angle range is small and the torque in the XY direction changes suddenly.

[0077] ​The third main control module is used for judging single-point contact as bottom surface contact or side surface contact according to the helical motion trajectory tracking error in the case of large deflection angle contact, i.e. no sudden change of the moment of force in XY direction, if it is bottom surface contact, moving to side surface contact along the end plane, if it is side surface contact, setting the expected contact force of unknown surface tangent according to the current contact force direction, guiding the robot to move along the position surface, and monitoring the trajectory gradient discontinuous point until the plug and socket in the hole are in double-point contact state;

[0078] The fourth main control module is used for setting the expected contact force along the end in the double-point contact state, guiding the plug to move along the socket surface, and identifying the edge alignment and plane alignment motion constraint planes to realize the Z-axis alignment of the plug and socket.

[0079] The fifth main control module is used for setting the expected contact moment along the Z-axis of the plug to drive the alignment of the protection groove of the plug and socket, and realizing the force sensing assembly of the plug and socket after the alignment.

[0080] Those skilled in the art can easily understand that the above description is only the preferred embodiment of the present application, and is not used to limit the present application, and any modification, equivalent replacement and improvement made within the spirit and principle of the present application should be included in the protection scope of the present application.

Claims

1. A robot force sensing assembly method for a spatial electrical connector in arbitrary postures, characterized in that: The following steps are involved: In step 1, the plug and socket are in any relative position in space. The robot drives the plug to move. After the plug and socket make single-point contact, the robot uses inverted cone diffusion motion combined with the contact torque mutation to identify the deflection angle range of the plug and socket, and thus determines whether the torque in the XY direction has a mutation. Step 2: If the deflection angle range is a small deflection angle contact, that is, the torque in the XY direction suddenly changes, then the position information of the plug and socket corresponding to the sudden change is recorded according to the direction of the contact torque mutation, that is, the corresponding plug and socket is in a double-point contact state; Step three, if the deflection angle range is a large deflection angle contact, that is, the torque in the XY direction does not change suddenly, then the single-point contact is judged to be bottom contact or side contact according to the spiral motion trajectory tracking error. If it is bottom contact, move along the end plane to the side contact state. If it is side contact, set the expected contact force tangential to the unknown surface according to the current contact force direction, guide the robot to move along the position surface, and monitor the trajectory gradient discontinuity point at the same time until the plug and socket in the hole are in a double-point contact state; Step 4: In the double-point contact state, set the desired contact force along the end to guide the plug to move along the socket surface, and identify the edge alignment and plane alignment motion constraint planes to achieve Z-axis alignment of the plug and socket; Step 5: Set the desired contact torque along the Z axis of the plug to drive the protective grooves of the plug and the socket to align, and after alignment, realize the force-sensing assembly of the plug and socket; In step 1, whether single-point contact is achieved is determined by detecting the magnitude of the current contact force; When the single point is in contact, keep the plug in the same position and make the center point of the robot's sixth axis P axis-6 =[x a6 ,y a6 ,z a6 ] changes according to the following rules: Where k0 is the proportional coefficient, w is the angular velocity, is the time step, x0, y0, z0 are P axis-6 The starting point; In step 3, if the deflection angle range indicates contact at a large deflection angle, a desired spiral contact force of the plug and socket is set to change over time, and the error between the current position of the plug and the desired position is obtained in real time. If the error is less than a threshold, contact is determined between the bottom surface of the plug and the top edge of the socket; otherwise, contact is determined between the edge of the plug and the side of the socket. The desired contact force F of the spiral d =[f dx ,f dy ,f dz ,0,0,0] T ,in, By monitoring the current position P of the plug c and the desired position P d The error d(P c ,P d ): Where f0,C are constants, is the time step, and w is the angular velocity.

2. The robot force sensing assembly method of a spatial electrical connector in an arbitrary posture according to claim 1 is characterized in that: In step 1, when the initial relative posture deviation between the plug and the socket is small, there will be a double-point contact moment t between the plug and the socket. * , at t * The reading of the force sensor at the moment, that is, the end contact force F c ∈R 6 The moment in the end plane (τ cx ,τ cy ) will mutate; and for the case of a large posture deflection angle, due to the limited inverted cone search step T, the plug and socket do not have a double-point contact state within a small contact range, so the contact torque (τ cx ,τ cy ) will not cause a sudden change, based on which it can be preliminarily identified whether the initial contact between the plug and the socket is a small deflection angle or a large deflection angle.

3. The robot force sensing assembly method of a spatial electrical connector in an arbitrary posture according to claim 1 is characterized in that: In step 2, when the initial contact state is a small inclination angle, the contact torque (τ cx ,τ cy ) performs filtering and gradient calculation to obtain the time t * By recording the plug posture under the plug, the double-point contact status of the plug and socket can be obtained and further search can be carried out.

4. The robot force sensing assembly method of a spatial electrical connector in an arbitrary posture according to claim 1 is characterized in that: In the case of side contact, the robot senses the direction of movement by contacting the unknown surface on the side. During the contact with the unknown surface, the current end contact force F c The component of force in the end plane (f cx ,f cy ) is perpendicular to the unknown surface, so by constructing the desired force F along the tangent direction of the socket contour d , and guide the robot to move along the unknown surface until the trajectory gradient discontinuity point is obtained, thereby realizing the transition from single-point contact to double-point contact between the plug and the socket at a large deflection angle, and obtaining the position of the plug at this time; Wherein, the expected force F of the socket profile tangent is d The calculation formula includes: F d =f dx +f dy =(f cx +f cy )×R.UnitZ Where R.UnitZ is the unit component of the plug rotation matrix R∈SO(3) along the z direction.

5. The robot force sensing assembly method of a spatial electrical connector in an arbitrary posture according to claim 1 is characterized in that: In step 4, set the desired contact force f in the R.UnitZ direction dz =k1*R.UnitZ is the desired contact force to drive the plug, guiding it along the surface of the socket. During this process, the plane swept by the line connecting the center point of the robot's sixth axis and the current position of the socket is the motion constraint plane of the plug and socket. Based on this constraint plane, the Z-axis alignment of the plug and socket, i.e., plane alignment, is achieved. Where k1 is the gain parameter and R.UnitZ is the unit component of the plug rotation matrix R∈SO(3) along the z direction.

6. The robot force sensing assembly method of a spatial electrical connector in an arbitrary posture according to claim 1 is characterized in that: After achieving the desired edge and plane alignment of the plug and socket, set the desired moment τ along R.UnizZ dz = k2R.UnitZ, to drive the robot to find the protection slot and set the desired contact force f along R.UnizZ dz =k3R.UnitZ, ensuring that the plug and the socket fit tightly together, where k2 and k3 are gain coefficients. When the protection groove is aligned, the expected contact force f dz , the plug will move along the protective groove, resulting in the current contact force f cz A sudden change occurs and the plug drops instantly. Therefore, by monitoring the height change and force change of the plug, it can be determined whether the plug and socket have completed the alignment of the protective grooves, and after alignment, the force-sensing assembly of the plug and socket can be realized.

7. A robot force sensing assembly system for a spatial electrical connector in any posture, characterized in that: include: The first main control module is used to drive the plug to move when the plug and socket are in any relative position in space. After the plug and socket make single-point contact, the robot uses inverted cone diffusion motion combined with the sudden change of contact torque to identify the deflection range of the plug and socket, and thus determines whether there is a sudden change in the torque in the XY direction; The second main control module is used to record the plug and socket posture information corresponding to the sudden change according to the direction of the contact torque mutation when the deflection angle range is small, that is, when the torque in the XY direction suddenly changes, that is, the corresponding plug and socket are in a double-point contact state; The third main control module is used to determine whether the single-point contact is bottom contact or side contact based on the spiral motion trajectory tracking error when the deflection angle range is large, that is, the torque in the XY direction does not change suddenly. If it is bottom contact, it moves along the end plane to the side contact state. If it is side contact, it sets the expected contact force in the tangential direction of the unknown surface according to the current contact force direction, guides the robot to move along the unknown surface, and monitors the discontinuity points of the trajectory gradient at the same time until the plug and socket in the hole are in a double-point contact state; The fourth main control module is used to set the desired contact force along the end in the double-point contact state, guide the plug to move along the socket surface, and identify the edge alignment and plane alignment motion constraint planes to achieve Z-axis alignment of the plug and socket; The fifth main control module is used to set the desired contact torque along the Z axis of the plug to drive the protection grooves of the plug and the socket to align, and realize the force-sensing assembly of the plug and socket after alignment; In the first main control module, whether single-point contact is achieved is determined by detecting the magnitude of the current contact force; When the single point is in contact, keep the plug in the same position and make the center point of the robot's sixth axis P axis-6 =[x a6 ,y a6 ,z a6 ] changes according to the following rules: Where k0 is the proportional coefficient, w is the angular velocity, is the time step, x0, y0, z0 are P axis-6 The starting point; In the third main control module, if the deflection angle range is large deflection angle contact, the expected spiral contact force of the plug and socket is set to change over time, and the error between the current position of the plug and the expected position is obtained in real time. If the error is less than the threshold, it means that the bottom surface of the plug and the top edge of the socket are in contact; otherwise, it means that the edge of the plug and the side of the socket are in contact; The desired contact force F of the spiral d =[f dx ,f dy ,f dz ,0,0,0] T ,in, By monitoring the current position P of the plug c and the desired position P d The error d(P c ,P d ): Where f0,C are constants, is the time step, and w is the angular velocity.

Images