Peg-in-Hole Assembly Based on Six-Legged Robots with Visual Detecting and Force Sensing

Manipulators with multi degree-of-freedom (DOF) are widely used for the peg-in-hole task. Compared with manipulators, six-legged robots have better mobility performance apart from completing operational tasks. However, there are nearly no previous studies of six-legged robots performing the peg-in-hole task. In this article, a peg-in-hole approach for six-legged robots is studied and experimented with a six-parallel-legged robot. Firstly, we propose a method whereby a vision sensor and a force/torque (F/T) sensor can be used to explore the relative location between the hole and peg. According to the visual information, the robot can approach the hole. Next, based on the force feedback, the robot plans the trajectory in real time to mate the peg and hole. Then, during the insertion, admittance control is implemented to guarantee the smooth insertion. In addition, during the whole assembly process, the peg is held by the gripper and attached to the robot body. Connected to the body, the peg has sufficient workspace and six DOF to perform the assembly task. Finally, experiments were conducted to prove the suitability of the approach.


Introduction
In the past few years, research on legged robots has received much attention [1][2][3][4]. Legged robots require merely isolated footholds for movement on the ground, which provides better adaptability in rough terrains, in contrast to tracked and wheeled robots [5][6][7]. According to current studies, legged robots can be used for detecting and operating tasks in complex environments. The walking robot SILO6 was designed to detect and locate antipersonnel landmines [8]. ATHLETE, studied by Wilcox et al. [9], emphasized handling cargos and manipulating objects on the moon. HUBO [10] and ATLAS [11] are biped robots that perform door opening manipulations. The robot COMAN applied the visual positioning method to turn circular valves [12]. These studies demonstrate that with their natural advantages legged robots are able to replace manpower to perform different kinds of tasks such as assembly processes. For instance, during the manual assembly process, professional techniques and high labor cost are required. Especially in some hazardous environments, manual assembly becomes more difficult and can even cause accidents. With the development of legged robots, the application of the robots in complex and dangerous scenarios shows that there is also possibility to employ them in assembly tasks.
In the assembly task research field, analyses usually highlight the key point of the assembly process, which is the peg-in-hole problem. Zhang et al. developed a fuzzy force control strategy for the dual peg-hole fitting using an ABB IRB1200 robot to accomplish an automatic fitting process [13]. Tang et al. designed a novel display device to adopt the data demonstrated by human beings. A modified learning from demonstration (LfD) algorithm was applied to a FANUC manipulator to carry out the peg-in-hole According to related studies, multi-DOF manipulators are used for the peg-hole insertion assembly. In this article, the peg is held by a gripper connected to the body of a six-legged robot, not connected to a multi-DOF robot arm. The robot body, serving as an end-effector, adopts admittance control (another form of impedance control) for the insertion assembly. Based on its own mobility characteristics, the robot can approach the holes from different locations. Moreover, the peg has enough space to work and sufficient DOFs to accomplish the assembly requirement by linking with a 6-DOF body.
In this article, we propose an approach for six-legged robots to accomplish the peg-in-hole task and verify the approach with a six-parallel-legged robot. A vision sensor and an F/T sensor are used for locating the hole and admittance control is applied to guarantee smooth assembly completion. The robot adopts a tripod gait during its movement process.
The rest of the article are divided as follows: in Section 2, a six-parallel-legged robot is introduced and the coordinate system is defined; in Section 3, the means of peg-in-hole assembly is detailed described; in Section 4, the experiment is conducted to testify the proposed approach; eventually, in Section 5, the article is concluded.

The Six-Legged Robot
A six-parallel-legged robot named Octopus EDU, illustrated in Figure 1, is a 6-DOF mobile platform with integrated walking and manipulation functionalities. The robot can move on different terrains and be used in complex environments for detection and rescue. A gripper is mounted on the front upper area of the body to enable the robot to complete the peg-in-hole assembly task. The robot body is a regular hexagon, on which six legs are arranged in diagonally symmetrical. The six legs have an identical mechanical structure, and each of them is a 3-DOF parallel mechanism (UP-2UPS). Benefitting from the 3-DOF, the robot can adapt to different terrains during its movement. Moreover, the parallel mechanism of the legs improves the load capacity of the robot.
The construction of the robot system is shown in Figure 2. The system includes the sensing, the actuation and the control modules. The sensing system consists of an F/T sensor and a vision sensor. As shown in Figure 1, the F/T sensor is mounted at the end of the gripper and linked to the robot The robot body is a regular hexagon, on which six legs are arranged in diagonally symmetrical. The six legs have an identical mechanical structure, and each of them is a 3-DOF parallel mechanism Sensors 2020, 20, 2861 4 of 22 (UP-2UPS). Benefitting from the 3-DOF, the robot can adapt to different terrains during its movement. Moreover, the parallel mechanism of the legs improves the load capacity of the robot.
The construction of the robot system is shown in Figure 2. The system includes the sensing, the actuation and the control modules. The sensing system consists of an F/T sensor and a vision sensor. As shown in Figure 1, the F/T sensor is mounted at the end of the gripper and linked to the robot body. The information recorded by the F/T sensor is used to sense external forces. The vision sensor is installed at the front of the body so as to detect visual messages in the moving direction. The vision sensor connects with the control system via USB, and in the peg-in-hole task process, the relative orientation and position between the hole and peg are preliminarily detected by the vision sensor. The actuation system is mainly used to drive the robot. Taking the actuation system of one leg as an example, each leg has three parts and the prismatic joint of the limb is regarded as the active joint. The active joint is actuated by using a servo motor via ball screw linkages. Motors are installed at the top of legs, and each of them is controlled by a corresponding servo driver. The motor has a resolver, by which the real position of the motor can be detected.
Sensors 2020, 20, x FOR PEER REVIEW 4 of 23 body. The information recorded by the F/T sensor is used to sense external forces. The vision sensor is installed at the front of the body so as to detect visual messages in the moving direction. The vision sensor connects with the control system via USB, and in the peg-in-hole task process, the relative orientation and position between the hole and peg are preliminarily detected by the vision sensor.
The actuation system is mainly used to drive the robot. Taking the actuation system of one leg as an example, each leg has three parts and the prismatic joint of the limb is regarded as the active joint. The active joint is actuated by using a servo motor via ball screw linkages. Motors are installed at the top of legs, and each of them is controlled by a corresponding servo driver. The motor has a resolver, by which the real position of the motor can be detected.
The control system, working as the central system of the robot, primarily contains the state estimator and the trajectory generator. Operators can send commands to control the robot via a remote terminal, which is connected to the control system via WiFi. To accomplish the instructions of real-time control, an individual PC running Linux system with Xenomai patch is employed as the control system of the legged robot. Moreover, the control system connects with the actuation system via EtherCAT real-time industrial fieldbus. In this way, the state estimator can supervise the situation of the robot and the trajectory generator can plan motion trajectories in real time. Similarly, the F/T sensor sends the information of external forces to the control system via EtherCAT, so that the robot can adjust the trajectory planning strategy immediately according to the force feedback.

Control System
Actuation System  Figure 2. System structure of the robot.

Definition of Coordinate Systems
When moving and operating, the motion state of the robot needs to be expressed in various coordinate systems. To guarantee the smoothness of peg-in-hole task, five coordinate systems are established in Figure 3. The first one is the coordinate system of the robot (RCS), the origin of which is fixed in the middle of the robot body. The Y-axis and Z-axis of the RCS are parallel to the vertical and sagittal axes of the robot, respectively. The location of the RCS changes with the movement of the robot body. The second is the F/T sensor coordinate system (SCS), the origin of which is located at the center of the F/T sensor. The location of the SCS is related to the installation orientation and position of the F/T sensor. For convenience, the directions of the XS-axis and the XR-axis are consistent, and the same with the ZS-axis and the YR-axis. The third is the vision sensor coordinate system (VCS), similar to the SCS, the VCS has been set up after delivery. The VCS location is also related to the mounting position of the vision sensor. The fourth is the peg coordinate system (PCS), which is fixed on the peg. The YP-axis coincides with the center line of the peg and points to the insertion direction during the assembly process. Finally, the ground coordinate system (GCS) is established, which is fixed on the ground and defined by the operator. Since the GCS keeps still when the robot moves and operates, it can refer to the other coordinate systems. For the sake of convenience, the GCS is defined to overlap the RCS if the legged robot is at its original position before the assembly process begins. The control system, working as the central system of the robot, primarily contains the state estimator and the trajectory generator. Operators can send commands to control the robot via a remote terminal, which is connected to the control system via WiFi. To accomplish the instructions of real-time control, an individual PC running Linux system with Xenomai patch is employed as the control system of the legged robot. Moreover, the control system connects with the actuation system via EtherCAT real-time industrial fieldbus. In this way, the state estimator can supervise the situation of the robot and the trajectory generator can plan motion trajectories in real time. Similarly, the F/T sensor sends the information of external forces to the control system via EtherCAT, so that the robot can adjust the trajectory planning strategy immediately according to the force feedback.

Definition of Coordinate Systems
When moving and operating, the motion state of the robot needs to be expressed in various coordinate systems. To guarantee the smoothness of peg-in-hole task, five coordinate systems are established in Figure 3. The first one is the coordinate system of the robot (RCS), the origin of which is fixed in the middle of the robot body. The Y-axis and Z-axis of the RCS are parallel to the vertical and sagittal axes of the robot, respectively. The location of the RCS changes with the movement of the robot body. The second is the F/T sensor coordinate system (SCS), the origin of which is located at the center of the F/T sensor. The location of the SCS is related to the installation orientation and position of the F/T sensor. For convenience, the directions of the X S -axis and the X R -axis are consistent, and the same with the Z S -axis and the Y R -axis. The third is the vision sensor coordinate system (VCS), similar to the SCS, the VCS has been set up after delivery. The VCS location is also related to the mounting Sensors 2020, 20, 2861 5 of 22 position of the vision sensor. The fourth is the peg coordinate system (PCS), which is fixed on the peg. The Y P -axis coincides with the center line of the peg and points to the insertion direction during the assembly process. Finally, the ground coordinate system (GCS) is established, which is fixed on the ground and defined by the operator. Since the GCS keeps still when the robot moves and operates, it can refer to the other coordinate systems. For the sake of convenience, the GCS is defined to overlap the RCS if the legged robot is at its original position before the assembly process begins.
where the matrix R S R indicates the orientation of the SCS in the RCS. O R S is a vector describing the position of the SCS origin expressed in the RCS. For the detailed calculation process readers can refer to [36]. Moreover, T V R and T P R can also be determined by using the same method according to the locations of the vision sensor and the peg.

Peg-in-Hole Method
Compared with previous methods that rely on manipulators, we use the legged robot to execute the peg-in-hole task in this paper. When the legged robot stands on the ground, the body of the robot has six DOFs in space. Thus, the robot body can be regarded as an end effector. Without additionally installing multi-DOF mechanical arms, the assembly task can be completed by the legged robot itself. The method presented in the paper can enrich the study of legged robots. Moreover, the manipulator usually has a fixed base, whereas the legged robot has the mobility to move to different locations. During the peg-in-hole process, the robot can walk to the hole from a long distance. When the robot reaches the location that the vision sensor can detect the hole, the task can be accomplished through the subsequent method. The legs of the robot are not allowed to slip during the entire task.
The sensing system of the robot consists of the vision sensor and the F/T sensor. The relative location between the hole and peg is determined by the combination of visual and force sensing. And then the control system plans the motion of the robot to proceed the peg-in-hole task. The chamfered Considering that parameters need to be demonstrated in various coordinate systems, the pose matrix T is introduced. For instance, G R T indicates the orientation and position of the RCS expressed in the GCS. Similarly, R S T, R V T and R P T describe the orientations and positions of the SCS, the VCS and the PCS in the RCS respectively. As mentioned above, locations of the sensors and the peg determine the pose matrices R S T, R V T and R P T. For instance, the solving process of R S T is stated as below: where the matrix R S R indicates the orientation of the SCS in the RCS. R O S is a vector describing the position of the SCS origin expressed in the RCS. For the detailed calculation process readers can refer to [36]. Moreover, R V T and R P T can also be determined by using the same method according to the locations of the vision sensor and the peg.

Peg-in-Hole Method
Compared with previous methods that rely on manipulators, we use the legged robot to execute the peg-in-hole task in this paper. When the legged robot stands on the ground, the body of the robot has six DOFs in space. Thus, the robot body can be regarded as an end effector. Without additionally installing multi-DOF mechanical arms, the assembly task can be completed by the legged robot itself. The method presented in the paper can enrich the study of legged robots. Moreover, the manipulator usually has a fixed base, whereas the legged robot has the mobility to move to different locations. During the peg-in-hole process, the robot can walk to the hole from a long distance. When the robot Sensors 2020, 20, 2861 6 of 22 reaches the location that the vision sensor can detect the hole, the task can be accomplished through the subsequent method. The legs of the robot are not allowed to slip during the entire task.
The sensing system of the robot consists of the vision sensor and the F/T sensor. The relative location between the hole and peg is determined by the combination of visual and force sensing. And then the control system plans the motion of the robot to proceed the peg-in-hole task. The chamfered peg-hole insertion task is introduced in this article. As exhibited in Figure 4, the whole task includes three subtasks: visual localization, F/T location and the assembly process.  In the process of the visual location subtask, the relative position and orientation between the hole and peg are adjusted preliminarily according to the detection results of the vision sensor. During the F/T location subtask, the peg and hole are located more precisely via the F/T sensor. During locating by the F/T sensor, the peg contacting the chamfer of the hole is defined as Case a, whereas no touching is defined as Case b. Then, the corresponding motions Move a and Move b are applied to adjust the relative position between the hole and peg, respectively. In the end, during the assembly process subtask, the peg is inserted into the hole with the movement of the robot body. During the insertion process, the F/T sensor detect the completion of assembly task. The rest of the paper introduces the three subtasks in details.

Locating the Hole with the Visual System
In the visual location subtask, the relative orientation between the peg and hole is detected by the vision sensor first. Then according to the detection result, the robot rotates to make the peg parallel to the hole. After that, a second visual detection is carried out to modulate the relative location between the peg and hole. The maximum initial error of the approach is mainly limited by the camera view. The initial position for visual detecting allows ±10° rotational error and ±50 cm positional error. Before the visual detection, the robot can move into that visual range from different locations under the user's remote control.

Visual Detecting Design Scheme
As exhibited in Figure 5, the hole coordinate system (HCS) is set up, the origin of which is located at the cross point between the axis of the hole and the contact surface. The directions of the YH-axis and the hole-axis are the same. When detecting using the vision sensor, the location of the hole can be determined according to YH and OH. Since the axis of the hole is perpendicular to the contact cover, In the process of the visual location subtask, the relative position and orientation between the hole and peg are adjusted preliminarily according to the detection results of the vision sensor. During the F/T location subtask, the peg and hole are located more precisely via the F/T sensor. During locating by the F/T sensor, the peg contacting the chamfer of the hole is defined as Case a, whereas no touching is defined as Case b. Then, the corresponding motions Move a and Move b are applied to adjust the relative position between the hole and peg, respectively. In the end, during the assembly process subtask, the peg is inserted into the hole with the movement of the robot body. During the insertion process, the F/T sensor detect the completion of assembly task. The rest of the paper introduces the three subtasks in details.

Locating the Hole with the Visual System
In the visual location subtask, the relative orientation between the peg and hole is detected by the vision sensor first. Then according to the detection result, the robot rotates to make the peg parallel to the hole. After that, a second visual detection is carried out to modulate the relative location between the peg and hole. The maximum initial error of the approach is mainly limited by the camera view. The initial position for visual detecting allows ±10 • rotational error and ±50 cm positional error. Before the visual detection, the robot can move into that visual range from different locations under the user's remote control.

Visual Detecting Design Scheme
As exhibited in Figure 5, the hole coordinate system (HCS) is set up, the origin of which is located at the cross point between the axis of the hole and the contact surface. The directions of the Y H -axis and the hole-axis are the same. When detecting using the vision sensor, the location of the hole can be determined according to Y H and O H . Since the axis of the hole is perpendicular to the contact cover, the direction vector of the hole-axis can be expressed by τ, which is the normal vector of the contact surface. Color images, depth images and point clouds of objects can be obtained by the vision sensor. Identification of the hole can be separated into two steps. Firstly, the point cloud of the contact surface is extracted from the environment, and the normal vector τ described in the VCS is derived by fitting the point cloud information. Thus, the first step of visual detection is fulfilled. The robot rotates according to the detection result, and proceeds with the second positioning. As shown in Figure 6a, the Hough-transform algorithm is used in color images to obtain pixel positions of the edge of the hole. Then, as exhibited in Figure 6b, these pixels are mapped to the depth image, and marked as the candidate points of the hole on the contact surface. After that, the candidate points in the depth image are mapped into the point clouds. Depending on the change of gradient between the candidate points and the surrounding points, the points of the circular hole on the contact surface is selected. Finally, fitting the selected points, we can obtain the vector O H V , that represents the position of the hole in the VCS. By using the following coordinate transformation, the vectors τ and OH can be described in the PCS. Thus, the relative location between the peg and hole is derived: where the orientation matrices R  Color images, depth images and point clouds of objects can be obtained by the vision sensor. Identification of the hole can be separated into two steps. Firstly, the point cloud of the contact surface is extracted from the environment, and the normal vector τ described in the VCS is derived by fitting the point cloud information. Thus, the first step of visual detection is fulfilled. The robot rotates according to the detection result, and proceeds with the second positioning. As shown in Figure 6a, the Hough-transform algorithm is used in color images to obtain pixel positions of the edge of the hole. Then, as exhibited in Figure 6b, these pixels are mapped to the depth image, and marked as the candidate points of the hole on the contact surface. After that, the candidate points in the depth image are mapped into the point clouds. Depending on the change of gradient between the candidate points and the surrounding points, the points of the circular hole on the contact surface is selected. Finally, fitting the selected points, we can obtain the vector V O H , that represents the position of the hole in the VCS. By using the following coordinate transformation, the vectors τ and O H can be described in the PCS. Thus, the relative location between the peg and hole is derived: where the orientation matrices R P R and R V R, denote the orientations of the PCS and the VCS in the RCS, respectively.
where the orientation matrices R

Trajectory Planning for Visual Locating
During the visual location procedure, the trajectory planning of the robot is produced by position control. The trajectory planning can be divided into three parts, as shown in Figure 7. The

Trajectory Planning for Visual Locating
During the visual location procedure, the trajectory planning of the robot is produced by position control. The trajectory planning can be divided into three parts, as shown in Figure 7. The first part is illustrated in Figure 7a: according to the relative angle θ r between the peg and hole detected by the vision sensor, the body and the legs move together which enables the robot to rotate an angle θ r . After the rotation of the robot, the vision sensor is used to detect whether the peg is vertical to the cover of the hole. If not, the above motion is repeated until the angle error is eliminated. The second part is described in Figure 7b: the vision sensor detects again to observe the relative position between the hole and peg, and then the robot moves distances d 1 sideways and d 2 forward to approach the hole. The third part is exhibited in Figure 7c: the robot adjusts the height of the peg to complete visual location. first part is illustrated in Figure 7a: according to the relative angle θ r between the peg and hole detected by the vision sensor, the body and the legs move together which enables the robot to rotate an angle θ r . After the rotation of the robot, the vision sensor is used to detect whether the peg is vertical to the cover of the hole. If not, the above motion is repeated until the angle error is eliminated. The second part is described in Figure 7b: the vision sensor detects again to observe the relative position between the hole and peg, and then the robot moves distances d1 sideways and d2 forward to approach the hole. The third part is exhibited in Figure 7c: the robot adjusts the height of the peg to complete visual location.  As the body and the legs move together in the first and second parts, it is necessary to design the trajectory of the leg. The six-legged robot usually adopts the tripod 3-3 gait, which needs the legs to move alternately for stable walking.
As exhibited in Figure 7a, the robot is at its original position, thus the RCS superposes the GCS. During the rotation process, the robot usually takes one step to rotate to the corresponding position. After the robot rotates θ r along the YG-axis, the relative position between the body and the leg remains unchanged. Therefore in the first part, the trajectory of leg i in the GCS can be expressed as follows: where the position vector P G io represents the initial position of leg i foot-tip, and E G i means the change of the foot-tip position. The trajectory of the leg is generated according to E G i . In the second part, the movement of the robot is a parallel motion. In Figure 7b, the process by which the robot moves a distance d1 along the XR-axis is analyzed as an example. Considering d1 may be larger than the maximum stride length dmax, the number of the step and the length of each step As the body and the legs move together in the first and second parts, it is necessary to design the trajectory of the leg. The six-legged robot usually adopts the tripod 3-3 gait, which needs the legs to move alternately for stable walking.
As exhibited in Figure 7a, the robot is at its original position, thus the RCS superposes the GCS. During the rotation process, the robot usually takes one step to rotate to the corresponding position. After the robot rotates θ r along the Y G -axis, the relative position between the body and the leg remains unchanged. Therefore in the first part, the trajectory of leg i in the GCS can be expressed as follows: Sensors 2020, 20, 2861 where the position vector G P io represents the initial position of leg i foot-tip, and G E i means the change of the foot-tip position. The trajectory of the leg is generated according to G E i . In the second part, the movement of the robot is a parallel motion. In Figure 7b, the process by which the robot moves a distance d 1 along the X R -axis is analyzed as an example. Considering d 1 may be larger than the maximum stride length d max , the number of the step and the length of each step need to be calculated in the walking process. When the robot walks, its six legs move alternately in two groups. The first stride length of the first group legs and the last stride length of the second group legs are half of the other stride lengths. Thus the equation that expressing the relationship between the step number and the stride length can be obtained: (5) where N represents the step number. As N is an integer, we get the value of N. Then we input N into Equation (6) and the stride length d N can be calculated. Thus, the trajectory planning of the second part is also completed.

Locating the Hole by the F/T Sensor
Via the visual location procedure, the relative location between the peg and hole is located preliminarily. Considering the small clearance between them during the insertion procedure, it is more precise to adopt the F/T sensor to locate the relative location between the hole and peg more precisely.

Positioning Scheme Based on the F/T Sensor
The F/T sensor collects the data of force and torque every millisecond, and the data recorded at millisecond n is expressed as follows: In the above equation, S F n and S M n represent the force date and the torque data, respectively, in the SCS. For convenience, the force and torque data should be expressed in the PCS: where P S R describes the orientation of the SCS in the PCS. S O P is a vector describing the origin of the PCS described in the SCS.
During the F/T location process, the change of the robot motion status will lead to errors when the peg contacts the hole. A follow up procedure is applied to decrease the inaccuracy. When detecting the contact, the robot stops moving and remains stationary for a while. Then the peg leaves the hole. In this process, the recorded forces and torques are used to calculate the more accurate contact data S avg : In the above equation, n 0 and n 1 denote the instant the peg contacts the hole and the moment the robot begins to keep still, respectively; n 2 and n 3 respectively indicate the moment the robot restarts to move and the instant the peg is completely separated from the hole.
After the visual location process, the maximum angle positioning error between the hole and peg is 1º. With this error, the peg can also be inserted into the hole to a certain depth. The angle error can be adjusted during the insertion. Therefore, the force location model focuses on determining the relative position error and is designed under the circumstance that the peg is parallel to the hole ahead of the insertion task. In the F/T location process, based on the possibility of contacting the chamfer, two situations of the model are discussed: Case a. Contact the chamfer of the hole: The condition of contacting the chamfer of the hole is illustrated in Figure 8a. As shown in the figure, r p is the radius of the peg; c p denotes the width of the peg chamfer; r h is the radius of the hole; c h denotes the width of the hole chamfer; e represents the deviation between the hole and peg. When the peg contacts the chamfer of the hole: Sensors 2020, 20, x FOR PEER REVIEW 10 of 23 The contact point between the peg and the hole chamfer is defined as A. Since the shapes of the hole and peg are cylindrical, the direction of the contact force coincides with the line OHOP in the XPOPZP plane, as shown in Figure 8a. The force and torque data, S S n , is recorded by the F/T sensor. And calculating S P n via Equations (8) and (9), the data S P n described in the PCS is obtained. Then S P avg can be derived according to Equation (10). Thus, the angle ωA between the vector OPOH and the XP-axis in Case a can be obtained: where F P avgx and F P avgz denote the force data along the XP-axis and the ZP-axis, respectively, in the PCS.  When the value of the deviation e makes Equation (11) invalid, the peg contacts the surface of the hole. As exhibited in Figure 8b, the contact point between the peg and the cover of the hole is defined as B. According to the side view of Figure 8b, ideally the contact force is parallel to the pegaxis. In real tests, friction forces exist. As the friction force is light and the final location is based on Move a, the force along the YP-axis can be extracted for calculation. In this case, F P avgx = F P avgz = M P avgy =0, and torque analyze is the means to solve the position of B in the XPOPZP plane: The contact point between the peg and the hole chamfer is defined as A. Since the shapes of the hole and peg are cylindrical, the direction of the contact force coincides with the line O H O P in the X P O P Z P plane, as shown in Figure 8a. The force and torque data, S S n , is recorded by the F/T sensor. And calculating P S n via Equations (8) and (9), the data P S n described in the PCS is obtained. Then P S avg can be derived according to Equation (10). Thus, the angle ω A between the vector O P O H and the X P -axis in Case a can be obtained: where P F avgx and P F avgz denote the force data along the X P -axis and the Z P -axis, respectively, in the PCS.
Case b. Contact the surface of the hole: When the value of the deviation e makes Equation (11) invalid, the peg contacts the surface of the hole. As exhibited in Figure 8b, the contact point between the peg and the cover of the hole is defined as B. According to the side view of Figure 8b, ideally the contact force is parallel to the peg-axis. In real tests, friction forces exist. As the friction force is light and the final location is based on Move a, the force along the Y P -axis can be extracted for calculation. In this case, P F avgx = P F avgz = P M avgy = 0, and torque analyze is the means to solve the position of B in the X P O P Z P plane: P M avgx = − P z B P F avgy P M avgz = P x B P F avgy (13) where P F avgy is the force data along the Y P -axis in the PCS. P M avgx and P M avgz represent the torque data along the X P -axis and the Z P -axis, respectively, in the PCS. Point B is the centroid of the interface. According to symmetry of the circle, the points B, O P and O H are located in a straight line. Therefore, the angle ω B between the vector O P O H and axis X P in Case b is obtained: The relative position between the hole and peg is expressed by the vector O P O H , which includes the orientation information ω and the size information e. ω A and ω B , the orientation information of O P O H under different contact conditions, are obtained according to the above method. Then, the deviation e can be derived in the process of trajectory planning as follows.

Trajectory Planning for F/T Locating
For trajectory planning, the control system sends the robot's instantaneous position to the actuation system every millisecond. During the F/T location process, an instantaneous stop is required when the peg contacts the hole. In order to fulfill the above requirement, the gait of the body is planned via a discrete force control approach in this situation: In Equation (15), the 6D vectors A n " V n and D n , describe the acceleration, the velocity and the trajectory of the robot body with respect to time n, respectively. t represents the space of time between two commands that the controller sends. F mov is a virtual 6D force vector decided by the moving direction. M mov = diag (m 1 , m 2 , . . . , m 6 ) and C mov = diag (c 1 , c 2 , . . . , c 6 ) are the mass and damp matrices, respectively.
When the peg contacts the hole, the robot plans the body trajectory by applying different F mov . F mov is a 6D vector, its first three elements describe the change of the position and its second three elements represent the change of orientation. In the process of the actual moving, the contact between the hole and peg shall be judged by S n , which is the force and torque data recorded by the F/T sensor. If the value of the force feedback exceeds the specific range, it means that the peg touched the hole. At this moment, the elements of F mov are all set to zero to achieve the instantaneous stop of the robot. According to Equation (15), the contact force sensed by the F/T sensor is not used for calculation. The contact force has no impact on trajectory generation in Equation (15). Thus, M mov and C mov are merely determined by the required accelerations and velocities of the body. The acceleration can be changed according to M mov . And the velocity can be derived from F mov and C mov , when the robot moves at a constant speed. The third term in Equation (15) is used to generate the target positions during the F/T locating process. The trajectory planning method is introduced in detail as follows.
Before the peg moves, the peg position is defined as O. The peg moves along the Y P -axis to approach the hole at first, as shown in Figures 9a and 10a. Then after contacting the hole, the trajectory of the peg is generated according to the contact condition. If the contact condition is Case a, the robot adopts the motion Move a to adjust the position of the peg. As exhibited in Figure 9b, the peg contacts the chamfer of the hole at point A, and the peg locates at the position C. According to ω A calculated in Equation (12), the movement direction of the peg is obtained. In Figure 9b,c, the peg moves along the direction of the angle ω A until it contacts the other side of the chamfer at point A 1 . At this moment, the end of the peg is located at point D, and the movement that the peg contacting the other side of the chamfer is the motion C→D of the peg. The distance from C to D is defined as d A . Since d A = 2e, the deviation e is derived. After that, the peg moves the distance e along the direction from D to C. Finally, the peg moves to the position E to mate the peg and hole. In the above movement, the peg will contact the hole during the motion O→C→D, and the trajectory is planned via Equation (15). The peg moves with the robot body, and the movement is planned in the GCS. Therefore, during the motion O→C→D, F mov is generated as follows: Sensors 2020, 20, x FOR PEER REVIEW 12 of 23 At this moment, the end of the peg is located at point D, and the movement that the peg contacting the other side of the chamfer is the motion C→D of the peg. The distance from C to D is defined as dA. Since dA = 2e, the deviation e is derived. After that, the peg moves the distance e along the direction from D to C. Finally, the peg moves to the position E to mate the peg and hole. In the above movement, the peg will contact the hole during the motion O→C→D, and the trajectory is planned via Equation (15). The peg moves with the robot body, and the movement is planned in the GCS. Therefore, during the motion O→C→D, Fmov is generated as follows:      In the above equation, G P R = G R R R P R. During the motion O→C, the matrix [010] T means the virtual force in the PCS and we can mark it as 1 N along the Y P -axis. Meanwhile, C mov is set to calculate the velocity of the robot. For instance, the virtual damp factor along the Y-axis in the PCS can be set to 50 N·s/m. When the robot moves at a constant speed, the velocity along the Y P -axis is 0.02 m/s. When the peg locates at the position C, it will stay static for a while. In this case, all elements of F mov are set to zero. During the motion C→D, F mov is determined by ω A via Equation (16). M mov and C mov are deduced from the required acceleration and velocity. We want to obtain a relatively large acceleration, so in the experiment we set M mov = diag(1, 1, 1, 1, 1, 1) during the F/T locating process. If a larger acceleration is needed, the values of M mov can be reduced. When the robot moves at a constant speed during the F/T location process, we want the total velocity to be 0.02 m/s. Thus, the values of C mov can be set to C mov = diag(50, 50, 50, 50, 50, 50) in the experiment. According to the values of C mov , when the robot moves at a constant speed during the motion C→D, the velocities along the X P -axis and the Z P -axis in the PCS are 0.02cosω A m/s and 0.02sinω A m/s, respectively.
If the contact condition is Case b, the robot applies the motion Move b to adjust the position of the peg. As exhibited in Figure 10a,b, the peg moves from O to F. The peg contacts the surface of the hole and B is the force point of the interface. ω B is calculated according to Equation (13) and Equation (14). Since the peg is pressed on the surface of the hole at the position F, the other motions should be proceeded after the peg moving away from the hole. Thus in Figure 10b,c, the peg leaves the hole along the Y P -axis. Then, after moving to the position G, the peg moves the distance d B along the direction of the angle ω B . According to Equation (11), if the contact condition is Case b, we can get that e ≥ r h − r p + c h + c p . Therefore, in order to decrease the position error between the hole and peg, we can make the distance d B = r h − r p + c h + c p . After that, as denoted in Figure 10d, after the motion from G to H, the peg moves along the Y P -axis to contact the hole again. And the contact condition is analyzed via the feedback of the F/T sensor. If the contact condition is Case b, repeat the motion Move b. Otherwise, the motion Move a is applied to mate the peg and hole.
The F/T locating process is illustrated in the F/T locate block diagram of Figure 4. The peg and the hole are mated by applying the two motion patterns, Move a and Move b. In the process of Move b, the peg will contact the hole when moving from O to F. During the motion from O to F, the 6D vector can be generated via Equation (17): In addition, during the movement F→G→H, the peg will not contact the hole. Thus in this case, position control can be applied to plan the trajectory of the body. Finally, when the peg moves from H to the hole position, the trajectory planning is the same as the motion from O to F.

Assembly Strategy
After the location procedures, the peg is inserted into the hole to perform the assembly task. However during the insertion, the movement error of the robot affects the assembly result. When the peg and the hole are mated, a fixed coordinate system (QCS) is set to superpose the current PCS before the insertion process. In the QCS the insertion direction is along the Y Q -axis, and the motion along Y Q is planned via Equation (15). Thus, the insertion process can be stopped immediately when the external forces on the peg exceed a certain range. In addition, the peg contacts the wall of the hole along the X Q OZ Q plane. In order to guarantee the smooth assembly, the authors present the admittance control. Therefore, the robot body performs as if its target and desired positions were linked via a virtual mass-spring-damper system, as shown in Figure 11.
In Figure 11, (xd, zd) and (xt, zt) denote the desired and the target positions in workspace separately. Based on the difference between these two positions, the robot can actively generate the virtual damping and spring forces. In addition, m, c and k represent parameters of the mass, the damping and the stiffness successively. Thus, the trajectory along the XQOZQ plane can be planned. Figure 12 shows the robot admittance control system. Figure 12. The robot admittance control system. As exhibited in Figure 12, Fdx represents the desired force along the XQ-axis. Fsx is the force information recorded by the F/T sensor, and the contact force Fcx is extracted through the filter. Then, depending on Fdx and Fcx, the compensated position xc along the XQ-axis is obtained via the admittance filter. After that, xt can be derived according to xd and xc and zt can also be obtained by applying the Figure 11. The virtual mass-spring-damper model.
In Figure 11, (x d , z d ) and (x t , z t ) denote the desired and the target positions in workspace separately. Based on the difference between these two positions, the robot can actively generate the virtual damping and spring forces. In addition, m, c and k represent parameters of the mass, the damping and the stiffness successively. Thus, the trajectory along the X Q OZ Q plane can be planned. Figure 12 shows the robot admittance control system. In Figure 11, (xd, zd) and (xt, zt) denote the desired and the target positions in workspace separately. Based on the difference between these two positions, the robot can actively generate the virtual damping and spring forces. In addition, m, c and k represent parameters of the mass, the damping and the stiffness successively. Thus, the trajectory along the XQOZQ plane can be planned. Figure 12 shows the robot admittance control system. Figure 12. The robot admittance control system. As exhibited in Figure 12, Fdx represents the desired force along the XQ-axis. Fsx is the force information recorded by the F/T sensor, and the contact force Fcx is extracted through the filter. Then, depending on Fdx and Fcx, the compensated position xc along the XQ-axis is obtained via the admittance filter. After that, xt can be derived according to xd and xc and zt can also be obtained by applying the Figure 12. The robot admittance control system. As exhibited in Figure 12, F dx represents the desired force along the X Q -axis. F sx is the force information recorded by the F/T sensor, and the contact force F cx is extracted through the filter. Then, depending on F dx and F cx , the compensated position x c along the X Q -axis is obtained via the admittance filter. After that, x t can be derived according to x d and x c and z t can also be obtained by applying the same method. Finally, the target position u t in the robot joint space is calculated by using the inverse kinematic model, and thus trajectory planning of the motion is generated. The kinematic model was introduced in [37].
In the above control process, the parameters of the admittance controller and the low-pass filter have a significant influence on the reliability of the system. Analyzing the transfer function of the system, we can obtain: G(s) = s 2 + k 1 s + k 2 ms 4 + (c + mk 1 )s 3 + (k + ck 1 + mk 2 )s 2 + (kk 1 + ck 2 )s + (kk 2 + k r k 2 ) (18) According to the Routh-Hurwitz stability criterion, the Routh-Hurwitz Table is established and the first column turns out to be: 1 +ck+m 2 k 1 k 2 c+mk 1 c 3 k 1 k 2 +c 2 kk 2 1 +c 2 mk 2 1 k 2 +ck 2 k 1 +cmkk 3 1 +cm 2 k 1 k 2 2 −2cmkk 1 k 2 −(c 2 k 2 +2cmk 1 k 2 +m 2 k 2 1 k 2 )kr c 2 k 1 +cmk 2 1 +ck+m 2 k 1 k 2 To guarantee the stability of the system, all elements of R c must be positive. Considering that m, c, k, k 1 , k 2 , k r are all positive, only the fourth element (denoted by E R4 ) may become negative. The parameters k 1 and k 2 are determined by the filter and not used to regulate the stability. Therefore, two parameters should be critically analyzed. The first one is k r , which is unknown and determined by the materials. It may be very large, causing E R4 to negative. The second one is c. Observing E R4 , it can be found that it is cubic to c and quadratic to m and k, which means c has greater weight on the influence on E R4 . Moreover, the smaller m is, the higher bandwidth of the system will be. And also, k decides the offset of the peg, which should not be too large. As a result, c becomes a key parameter to regulate the system reliability.
In the experiments, by adjusting the filtering effect, k 1 , k 2 were set to be 11.25 and 63.33. And m, c, k were tuned to be 12, 2254 and 196. In such case, E R4 is positive when k r < 24324. That means the system would be unstable if the contact rigidity between the peg and the hole are larger than 24,324 N/m. If the system becomes unstable because of the large k r , the parameter c can be enlarged to enable the system stability again.
In the above procedures (illustrated in Figure 12), the orientations of the QCS and the PCS remain the as the force described in the QCS and the PCS. The model of the mass-spring-damper system can be expressed by: The target position (x t , z t ) is obtained according to Equation (20). Moreover, y t along the Y Q -axis is derived via Equation (15). Thus, the trajectory planning in the QCS is obtained, and converted to the GCS as follows: As mentioned above, the relative location between the peg and hole is compensated by employing the admittance control method. Due to the inaccurate location and the movement during the assembly, the relative angle error between the peg and hole also exists. However, there is a coupling relationship between the position and the orientation during the insertion assembly. Changing both at the same time may affect the smooth completion of the assembly process. In this article, a tactic on the basis of the position admittance control and the orientation compensation is applied to design the trajectory of the peg as exhibited in Figure 13. Regardless of the angle error, the peg can still be inserted some distance into the hole along the YQ-axis, as denoted in Figure 13a. During the above phase, admittance control is applied to compensate the position error. Then, as shown in Figure 13b, the peg is unable to continue the insertion when contacting two sides of the hole-wall. In this case, the motion generated by admittance control is stopped. During this two-contact phase, the torque applying on the peg is caused by the angular misalignment between the two mating parts. Rotating the peg according to the torque to compensate for the angular error is the process called orientation compensation strategy. And as shown in Figure 13c, after decreasing the angular misalignment, admittance control is used again to continue the assembly process. In addition, according to the new orientation and position of the PCS, we can reset the QCS to coincide with the current PCS.
Therefore, the whole insertion assembly can be regarded as a cyclic process: admittance control → orientation compensation → admittance control. Finally, when the assembly task is finished, the peg is released by the gripper and the robot moves the body away from the peg.

Experiment
Experiments were conducted on the six-parallel-legged robot to perform the peg-in-hole assembly and validate the proposed means. During different peg-in-hole manipulations, the robot started to perform the task at different initial positions. The experiments were executed 10 times at each initial position. The success rate was over 80 percent under the summary of the experiments at different initial locations. In the experiment, the diameter of the peg was 50 mm; the clearance between the hole and peg was less than 0.1 mm. Both the peg and the hole were chamfered.
Before the peg-hole insertion, the robot was unaware of the hole location. Once the operation started, the visual sensor was applied to detect the orientation and position of the hole at first. And the visual detection results were applied to guide the robot to walk close to the hole. After that, the robot automatically designed the movement in real time on the basis of the feedback of the force to locate the hole more precisely. Finally, after the alignment procedure, the six-legged robot inserted the peg into the hole. Figure 14 shows snapshots of the experiment, and locations of the robot at diverse instants are stated in subfigures of Figure 14.
Subfigures ① to ⑥ in Figure 14 show the visual locating process. In subfigure ①, the robot was at its initial position and detected the orientation of the hole by the vision sensor. And as shown in subfigure ②, the robot rotated to make the peg parallel to the hole according to the measured orientation information. Then, the robot proceeded the second time visual detection to locate the Regardless of the angle error, the peg can still be inserted some distance into the hole along the Y Q -axis, as denoted in Figure 13a. During the above phase, admittance control is applied to compensate the position error. Then, as shown in Figure 13b, the peg is unable to continue the insertion when contacting two sides of the hole-wall. In this case, the motion generated by admittance control is stopped. During this two-contact phase, the torque applying on the peg is caused by the angular misalignment between the two mating parts. Rotating the peg according to the torque to compensate for the angular error is the process called orientation compensation strategy. And as shown in Figure 13c, after decreasing the angular misalignment, admittance control is used again to continue the assembly process. In addition, according to the new orientation and position of the PCS, we can reset the QCS to coincide with the current PCS.
Therefore, the whole insertion assembly can be regarded as a cyclic process: admittance control → orientation compensation → admittance control. Finally, when the assembly task is finished, the peg is released by the gripper and the robot moves the body away from the peg.

Experiment
Experiments were conducted on the six-parallel-legged robot to perform the peg-in-hole assembly and validate the proposed means. During different peg-in-hole manipulations, the robot started to perform the task at different initial positions. The experiments were executed 10 times at each initial position. The success rate was over 80 percent under the summary of the experiments at different initial locations. In the experiment, the diameter of the peg was 50 mm; the clearance between the hole and peg was less than 0.1 mm. Both the peg and the hole were chamfered.
Before the peg-hole insertion, the robot was unaware of the hole location. Once the operation started, the visual sensor was applied to detect the orientation and position of the hole at first. And the visual detection results were applied to guide the robot to walk close to the hole. After that, the robot automatically designed the movement in real time on the basis of the feedback of the force to locate the hole more precisely. Finally, after the alignment procedure, the six-legged robot inserted the peg into the hole. Figure 14 shows snapshots of the experiment, and locations of the robot at diverse instants are stated in subfigures of Figure 14.   Figure 16 denotes the body and feet positions during the F/T location and insertion assembly processes. Since the peg was held by the gripper and fixed to the body, the trajectory of the peg can be described via the trajectory of the robot body. Besides, as the 2nd and the 5th legs moved alternately in 3-3 gait, here the motions of these two feet were applied to exhibit motions of all feet. Subfigures ⑦ to ⑫ in Figure 14 show the F/T location and the insertion assembly processes. In addition, Figure 17 and Figure 18 exhibit the data of force and torque recorded in the F/T sensor. As illustrated in subfigure ⑦, the robot body moved forward until the peg contacted the hole. Meanwhile, we can find that force and torque traces illustrated in Figure 17 and Figure 18 change obviously. After ⑦, the traces in Figure 16 illustrate the temporary stationary position of the robot. During the temporary stationary period, the data of F/T sensor was recorded. Using the recorded data, the average contact force/torque can be calculated via Equation (10). And the average contact force/torque expressed in the PCS is derived via Equations (8) and (9). Then, using the method introduced in Section 3.2.1, the robot identified the contact condition of Case b. In this case, the motion, Move b, was applied for another contact. As stated in Section 3.2.2, the motion O→F in the process of Move b is generated via Equation (15), and Fmov for the motion O→F is calculated via Equation (17). Since the robot rotated in the visual location process before the F/T location process, the matrix R P G used in Equation (16) and Equation (17) should be deduced first. In the experiment, R P G was:  Figure 14 show the visual locating process. In subfigure 1 , the robot was at its initial position and detected the orientation of the hole by the vision sensor. And as shown in subfigure 2 , the robot rotated to make the peg parallel to the hole according to the measured orientation information. Then, the robot proceeded the second time visual detection to locate the position of the hole, as denoted in subfigure 3 . After that, in subfigures 4 , 5 and 6 , the robot approached the hole via lateral (parallel to the X P -axis) and forward (parallel to the Y P -axis) movements and adjusted the height of the body to mate the peg and hole. Figure 15 denotes the body and feet positions during the visual location process. Figure 16 denotes the body and feet positions during the F/T location and insertion assembly processes. Since the peg was held by the gripper and fixed to the body, the trajectory of the peg can be described via the trajectory of the robot body. Besides, as the 2nd and the 5th legs moved alternately in 3-3 gait, here the motions of these two feet were applied to exhibit motions of all feet.   Figure 16 denotes the body and feet positions during the F/T location and insertion assembly processes. Since the peg was held by the gripper and fixed to the body, the trajectory of the peg can be described via the trajectory of the robot body. Besides, as the 2nd and the 5th legs moved alternately in 3-3 gait, here the motions of these two feet were applied to exhibit motions of all feet.  Subfigures ⑦ to ⑫ in Figure 14 show the F/T location and the insertion assembly processes. In addition, Figure 17 and Figure 18 exhibit the data of force and torque recorded in the F/T sensor. As illustrated in subfigure ⑦, the robot body moved forward until the peg contacted the hole. Meanwhile, we can find that force and torque traces illustrated in Figure 17 and Figure 18 change obviously. After ⑦, the traces in Figure 16 illustrate the temporary stationary position of the robot. During the temporary stationary period, the data of F/T sensor was recorded. Using the recorded data, the average contact force/torque can be calculated via Equation (10). And the average contact force/torque expressed in the PCS is derived via Equations (8) and (9). Then, using the method introduced in Section 3.2.1, the robot identified the contact condition of Case b. In this case, the motion, Move b, was applied for another contact. As stated in Section 3.2.2, the motion O→F in the process of Move b is generated via Equation (15), and Fmov for the motion O→F is calculated via Equation (17). Since the robot rotated in the visual location process before the F/T location process, G Figure 15. Motions of the body and feet in the process of visual locating. Subfigures 7 to 17 in Figure 14 show the F/T location and the insertion assembly processes. In addition, Figures 17 and 18 exhibit the data of force and torque recorded in the F/T sensor. As illustrated in subfigure 7 , the robot body moved forward until the peg contacted the hole. Meanwhile, we can find that force and torque traces illustrated in Figures 17 and 18 change obviously. After 7 , the traces in Figure 16 illustrate the temporary stationary position of the robot. During the temporary stationary period, the data of F/T sensor was recorded. Using the recorded data, the average contact force/torque can be calculated via Equation (10). And the average contact force/torque expressed in the PCS is derived via Equations (8) and (9). Then, using the method introduced in Section 3.2.1, the robot identified the contact condition of Case b. In this case, the motion, Move b, was applied for another contact. As stated in Section 3.2.2, the motion O→F in the process of Move b is generated via Equation (15), and F mov for the motion O→F is calculated via Equation (17). Since the robot rotated in the visual location process before the F/T location process, the matrix G P R used in Equation (16) and Equation (17) should be deduced first. In the experiment, G P R was: According to R P G and Equation (17), we can obtain that  Then, as shown in subfigure ⑧, the peg contacted the hole again. The traces exhibited in Figures 17 and 18 also change due to the contact. Similar to the previous explanation, the robot judged the contact condition of Case a according to the force feedback. Thus, the motion, Move a, was adopted to align the hole and peg. During the process of Move a, the motion O→C→D is planned via Equation (15).
During the motion O→C, Fmov is calculated via the first equation in Equation (16). According to R P G in Equation (22), we obtained that in Equation (22). And the F/T locating process was accomplished after Move a. Then, the robot began to insert the peg into the hole, as shown in subfigures ⑨ and ⑩. Finally, in subfigures ⑪ and ⑫, the peg-in-hole task was finished, and the robot moved away from the hole. Figure 16 denotes the locations of the body and the feet in the processes of F/T locating and the insertion assembly.   Then, as shown in subfigure ⑧, the peg contacted the hole again. The traces exhibited in Figures 17 and 18 also change due to the contact. Similar to the previous explanation, the robot judged the contact condition of Case a according to the force feedback. Thus, the motion, Move a, was adopted to align the hole and peg. During the process of Move a, the motion O→C→D is planned via Equation (15).
During the motion O→C, Fmov is calculated via the first equation in Equation (16). According to R P G in Equation (22), we obtained that in Equation (22). And the F/T locating process was accomplished after Move a. Then, the robot began to insert the peg into the hole, as shown in subfigures ⑨ and ⑩. Finally, in subfigures ⑪ and ⑫, the peg-in-hole task was finished, and the robot moved away from the hole. Figure 16 denotes the locations of the body and the feet in the processes of F/T locating and the insertion assembly.

Discussion
A six-parallel-legged robot was used in the experiment to demonstrate the methodology presented in the paper. The proposed method can also be used by other six-legged robots to perform the peg-in-hole task considering that six-legged robots with other leg mechanisms can adopt the tripod gait for moving as well. Thus, the legged robot is able to fulfill the peg-hole assembly task by According to G P R and Equation (17), we can obtain that F mov = [ −0.0991 0 −0.9951 0 0 0 ] T during the motion O→F in the experiment. Then, as shown in subfigure 8 , the peg contacted the hole again. The traces exhibited in Figures 17  and 18 also change due to the contact. Similar to the previous explanation, the robot judged the contact condition of Case a according to the force feedback. Thus, the motion, Move a, was adopted to align the hole and peg. During the process of Move a, the motion O→C→D is planned via Equation (15).
During the motion O→C, F mov is calculated via the first equation in Equation (16). According to G P R in Equation (22), we obtained that F mov = [ −0.0991 0 −0.9951 0 0 0 ] T in the experiment.
In addition, during the motion C→D, F mov is calculated via the second equation in Equation (16). ω A used in Equation (16) can be calculated via Equation (12). In the experiment, we obtained that cosω A = 0.9723 and sinω A =−0.2337 according to the force feedback. Thus in the experiment, F mov was [ 0.9675 −0.2337 −0.0964 0 0 0 ] T during the motion C→D via G P R in Equation (22). And the F/T locating process was accomplished after Move a. Then, the robot began to insert the peg into the hole, as shown in subfigures 9 and 10 . Finally, in subfigures 11 and 12 , the peg-in-hole task was finished, and the robot moved away from the hole. Figure 16 denotes the locations of the body and the feet in the processes of F/T locating and the insertion assembly.

Discussion
A six-parallel-legged robot was used in the experiment to demonstrate the methodology presented in the paper. The proposed method can also be used by other six-legged robots to perform the peg-in-hole task considering that six-legged robots with other leg mechanisms can adopt the tripod gait for moving as well. Thus, the legged robot is able to fulfill the peg-hole assembly task by taking advantage of its own mobility without installing additional mechanical arms.
In addition, the focus of this paper is to perform the chamfered peg-in-hole task. There is no change in the visual location process if the method is extended to a chamfer-less situation. The vision sensor is used to detect the hole first, and the robot is adjusted based on the visual feedback. Then, in the following F/T location process, if there is no chamfer, the contact condition is always Case b (defined in Section 3.2.1) when the deviation between the hole and peg is e > e 0 = r h − r p . According to the analysis of Case b introduced in Section 3.2.1, the orientation information of O P O H is derived and the peg moves the distance ne 0 (n is used to adjust the distance) along O P O H to reduce the deviation. After that, the peg contacts the hole again. If the peg still contacts the same side of the hole according to the force feedback, the peg continues to move along the distance ne 0 along O P O H . If the peg contacts the other side, the new deviation between the hole and peg is ne 0 -e. In this case, the peg moves the distance 0.5ne 0 along the opposite direction for another contact. The procedure above is repeated until the deviation is smaller than e 0 . Thus, the F/T location process is completed. Finally, the insertion assembly is proceeded. When the clearance between the hole and peg is larger, the insertion process becomes easier due to the larger permissible error. Otherwise, when the clearance is smaller, the insertion speed is reduced to provide more time for adjusting the relative position between the hole and peg.
In the experiments, the F/T location process relied on the results of the visual detection. Occasionally, the orientation information of the vision sensor is not accurate so the experimental results may be affected. To reduce the angle error, the hole is detected 20 times during each visual sensing process and the detection results are averaged to increase the accuracy. Moreover, we give priority to angular precision during the vision location procedure. After the rotation movements, if the angle error is eliminated according to the visual feedback, the robot begins to approach the hole to reduce the position error. Otherwise, the robot continues to rotate to decrease the angle error.
In future work, we plan to improve the presented method in the paper by reducing the reliance on visual detection and various peg-hole forms, such as different sizes and the chamfer-less situation will be studied. In addition, the forces exerted by the legged-robot during the insertion process are related to the frictional interaction between the feet and the ground and the robot needs to maintain its stability during the entire task. As these two challenges are easily satisfied in our case, we did not analyze them concretely. Therefore, we also plan to analyze the two challenges in the case of a low-friction surface with a lighter robot in future.

Conclusions
The paper proposes a means for a six-legged robot to accomplish a peg-hole insertion task. The conclusions can be summarized as follows: (1) A vision sensor and an F/T sensor are used to detect the orientation and position of the hole.
(2) On the base of the feedback of the force sensing, the trajectory of the robot is planned in real time.
(3) The peg is held by the gripper and connected to the robot body directly. The body adopts admittance control for the insertion process. (4) The proposed method is conducted by a six-parallel-legged robot. Based on the mobile performance of the prototype, it can approach holes located at different positions to perform the assembly task. (5) Verification experiments were conducted, and the experimental results proved the effectiveness of the method.