CMOR motion planning and accuracy control for heavy-duty robots

3.1 Standard Denavit-Hartenberg (DH) modeling method

The robot modeling needs to describe its attitude and position. In Cartesian coordinate system, any point P in space under coordinate system A can be represented by vector P A , where P x , P y , P z are the components under coordinate system A .

There is another rigid body B in the space and it is connected with the coordinate system F B . The F B coordinate system only has rotation relative to F A , and { X B → , Y B → , Z B → } represents the base of the F B coordinate system, then a 3 × 3 rotation matrix R B A can be used to represent the relationship between the base of the coordinate system F B and the coordinate system F A .

The column vectors of rotation matrix R B A are orthogonal to each other, and there is equation R − 1 B A = R T B A = R B A . The rotation matrix R B A can be transformed by an initial coordinate system around one or more coordinate axes, and the rotation change of rotation θ around the x , y , z axes is shown in the following equations:

A rigid body B fixed in the coordinate system F B as shown in Figure 2 can be described in the coordinate system F A using the rotation matrix R B A and the position vector P B O A , for [ R B A , P B O A ] . For this purpose, the position is described as a 3 × 4 array of non-simultaneous moments. To facilitate matrix operations, the non-simultaneous matrix is expanded to describe the position of rigid body B in coordinate system F A using the sigma matrix, as shown in equation (6).

Figure 2

Schematic diagram of rigid body position and posture in Cartesian space.

A generic sub-transformation matrix can consist of a translation operator and a dot product of rotation operators with a chi-square form, denoted by Trans ( P B O A ) and Rot ( K → , θ ) . For the rotation operator Rot ( K → , θ ) , K → is the source rotation axis and θ is the rotation angle, where the rotation transformation matrix can be described by the following equations:

3.2 CMOR system transport arm

The transport arm is a nine-degree-of-freedom robot with the distribution of degrees of freedom shown in Figure 3. The transport arm contains one moving sub and eight rotating subs. The master arm is responsible for the initial positioning and delivers the front-end actuating robot to a position close to the maintenance point, and then the front-end actuating arm, which is responsible for maintenance, performs further maintenance tasks [25]. This uncoupled master-slave control strategy greatly simplifies the complex linked overall positioning problem, i.e., simultaneous planning of the transport arm and the front-end actuator [26]. In this section, robotic structural parameter modeling is performed for the CMOR robot and is used to analyze the kinematics of the CMOR robot.

1. DH parameters

Figure 3

CMOR schematic diagram.

Using the standard DH method, the transport arm can be built as shown in Figure 4, where coordinate system { X → B , Y → B , Z → B } is the base coordinate system of the transport arm, coordinate system { X → 1 , Y → 1 , Z → 1 } coincides with the origin of the base coordinate system, and coordinate system { X → 10 , Y → 10 , Z → 10 } is the coordinate system of the end mounting platform of the robot. By establishing the coordinate system, it is possible to obtain a table of robot DH parameters as shown in Table 1.

Figure 4

DH model of CMOR.

For ease of writing, the θ i ( i = 1 , 2 , … , 6 ) used in this section refers specifically to the joint angle of the light arm, which differs from the θ i . The coordinate transformation of the end of the robotic arm with respect to the origin can be known from the DH modeling method as shown in equation (10), where [ n → , o → , a → , p → ] is the target constraint. Its main features include the use of homogeneous transformations to describe link relationships, fixed reference frames, and four DH parameters (link length, link twist, link offset, and joint angle) to represent the relative positions and orientations, kinematic equations for calculating end-effector position and orientation, a simple hierarchical structure, and the elimination of redundancy in the model. By bringing in the DH parameters from Table 1, its rotation transformation matrix and translation position vector can be obtained as shown in equations (11) and (12).

For the coordinate change R LWR 6 1 , the following constant equation exists as shown in equation (13). For writing convenience, the subscripts L and R only denote the left and right sides of the constant equation (13), respectively, noted as R LWR , L 6 1 and T LWR , R 6 1 , by bringing in the DH parameter and equation (10), the left and right sides of the equation can be obtained as shown in equations (14)–(16).

In order to better solve the inverse kinematics, it is necessary to assist in solving other coordinate transformations, which are R LWR 5 0 , R LWR 5 1 , R LWR 5 2 . Due to the complexity of the matrix form, they are not listed, but the relevant notation must be made, e.g., t 11 5 0 , LWR denotes the element in row 1, column 1 of R LWR 5 0 .

From equations (10) and (11), the relationship between the joint angle and the end constraint is shown in equation (17) below. The joint angle of each joint can be solved for by categorically discussing whether 03 is zero or not.

1. Light arm satisfies S 345 ≠ 0 .

1. Solving for joint 6:

   From equations (10) and (11) and using knowledge of trigonometric functions, the expression for θ 6 can be obtained as shown in equation (18).

   (18) θ 6 = tan − 1 a z n z ( n z ≠ 0 ) cos − 1 n z ( n z = 0 ) .

2. Solving for joints 3, 4, and 5:

   Equation (19) can be obtained from equations (10), (11), and (17).

   (19) S 12 = O y S 345 c 12 = o y S 345 .

   T LWR 5 2 position information for coordinate transformation P LWR 5 2 satisfying the constant equation (20) holds in the x and y directions. Bringing equation (19) to the right-hand side of the system of equations, the right-hand side equation satisfies the following relationship of equation (21). By solving for equation (20), we obtain θ 3 and θ 4 as shown in equations (22) and (23).

   (20) a 4 c 34 + a 3 c 3 = − c 12 o x d 6 − s 12 o y d 6 + s 12 p y + c 12 p x a 4 c 34 + a 3 s 3 = d 6 o z − d 1 + p z ,

   (21) p x , L W R 5 2 = − c 12 o x d 6 − s 12 o y d 6 + s 12 p y + c 12 p x p x , L W R 5 2 = d 6 o z − d 1 + p z ,

   (22) θ 3 = sin − 1 a 3 2 + a 4 2 + P x , LWR 2 5 2 + P y , LWR 2 5 2 2 a 3 P x , LWR 2 5 2 + P y , LWR 2 5 2 Other information Meaningless ( P x ,LWR 2 5 2 = P y , LWR 2 5 2 = 0 ) .

   Among ϕ 3 2 P x , LWR 2 = tan 5 − 1 P x , LWR 5 2 P y , LWR 5 2 .

   (23) ϕ 4 = tan − 1 P x , L W R 5 2 − a 3 s 3 P y , L W R 5 2 − a 3 s 3 − θ 3 .

   For the meaningless situation in equation (22), we need to make the following explanation. When P x , LWR 5 2 = P y , LWR 5 2 = 0 is met, equation (24) of the equation group is valid, and the solution of the equation group is equation (25). For any angle, equation s 3 = c 3 = 0 will never be valid, so this condition is meaningless.

   (24) a 4 c 34 + a 3 c 3 = 0 a 4 c 34 + a 3 s 3 = 0 ,

   (25) s 3 = 0 c 3 = 0 s 34 = 0 c 34 = 0 .

   The solution of θ 5 can be obtained by substituting the solution of θ 3 , θ 4 into equation (26).

   (26) θ 5 = cos − 1 ( − o z ) − θ 3 − θ 4 .

3. Solving for joints 1 and 2:

Equation (27) can be obtained from the identity of the left and right sides of P x , LWR 6 1 in equations (15) and (16). Bring in equation (19) and angle values θ 3 , θ 4 , and θ 5 to obtain θ 1 and θ 2 , as shown in equations (28) and (29).

1. Light arm meets s 345 = 0 .

4.1 Collision detection algorithm based on grayscale values

By studying computer graphics, the collision between the transport arm and the CFETR is transformed into a collision problem for the section to be measured. If a section of the transport arm collides with the CFETR, the gray-level image of the section will be anomalous. The anomalous image is further transformed into the detection of the gray level of the gray level image of the collision cross-section and the collision is detected using the grey level information. Based on the above ideas, a gray-level value-based collision detection algorithm for the transport arm can be designed as shown in Table 2.

4.2 Kinematic algorithm based on gray-scale values and dynamic ACO

By analyzing the structure of the transport arm and the CFETR and modeling it mathematically, a feasible solution for the robot can be obtained using the dynamic ACO algorithm and the grayscale values. However, for multiple degrees of freedom robots, due to the large number of joints, self-interference cannot be avoided simply by using joint angle constraints and needs to be taken into account in the kinematic algorithm. The transport arm robot self-collision algorithm is shown in Table 3, which essentially treats each link as a rod wrapped in a cylinder, and may define the enveloping cylindrical radius of the zth link as C R i , and for adjacent links, determines the number of intersections of the central axis N cross , and for non-adjacent links, such as link i and link j , determines whether the distance S L ij from the enveloping cylindrical axis satisfies the safety requirement. The robot itself is involved in a thousand problems. It represents the robot’s geometry and kinematics in a mathematical model, detects collisions by examining geometric shapes, and ensures compliance with constraints. If no collisions are found and all constraints are met, the solution is considered valid, otherwise, adjustments are made to avoid collisions and achieve a valid solution.

Using the above algorithm, the grayscale and dynamic ACO-based kinematic algorithm solves the robot kinematics as shown in Figure 5 below, with the input value of the algorithm being the target pose of the robot. After solving the inverse kinematics solution using the dynamic ACO algorithm under three different joint constraints, the valid inverse kinematics solution for the transport arm of the CMOR system is solved using the grayscale value-based collision detection algorithm and the self-collision algorithm to determine whether the solution is valid. A grayscale value-based collision detection algorithm is a technique that converts collision information into grayscale values to facilitate visualization and analysis. It involves representing the proximity or distance of objects in a scene as grayscale values, where darker or lighter shades indicate different levels of collision or proximity.

Figure 5

Flow chart of inverse kinematics solution of transport arm considering collision.

5.1 Walking with two, three, and six gait switching on flat ground

In accordance with the planning of gait switching, in order to cover all cases of two, three, and six gait switching with each other, the starting gait was chosen to be six gait 6-VI beats in the following motion sequence: six gait → three gait → two gait → three gait → six gait → two gait → six gait, and when the robot switched to the target gait it walked the complete three cycles of linear motion in accordance with the target gait. In order to reduce the simulation time taken, the motion frequency of the robot is ω = π / 2 , the simulation time is T = 198 s, and the detailed motion process is as follows:

1. Six-step → three-step

The starting stance of the six-step is selected for the 6-VI beat as shown in Figure 6. First, the robot remains stationary during T = 0 – 2 s , and at T = 0 – 2 s , the robot walks three cycles of motion to T = 38 s in a flat six-step stance, and then its target stance is three-step stance 3-III according to the switching rule. According to the switching rules, first, during T = 38 – 40 s , leg1 and leg5 swing to Position1 and the rest of the support phase remains in place differently. Second, during T = 40 – 42 s , leg3 and leg4 swing to Position1 and the rest of the foot end walks a three-step support phase. Finally, during T = 42 – 44 s , leg2 and leg6 swing to Position1 and the rest of the foot end walks a three-step support phase, which at T = 44 s is the standard three-step 3-III beat, completing the switching process, as shown in Figure 7.

Figure 6

Initial position of robot’s flat gait switch.

Figure 7

Moving process of robot’s flat gait switching T = 0–44 s.

The rest of the gait switching (three gait → two gait → three gait → six gait → two gait → six gait) is similar to this process. See Appendix 2 for details. According to the stability analysis of gait switching, the radius of the inscribed circle of the intersection of the stable support regions of the robot is r ssa = 407.01 mm during the gait switching process. Through the simulation process of robot gait switching, the motion law of each joint of its single leg can be obtained. According to the calculation method of robot single leg integrated centroid, the robot single leg integrated centroid can be obtained, as shown in Figure 8.

Figure 8

Change in integrated centroid coordinates of robot leg during gait switching.

From the relationship between the integrated center of mass of a single leg and the center of mass of the whole robot, the action area of the center of mass of the whole robot can be obtained. According to this, the relationship between the circle radius and the time of the projection area of the center of mass of the whole robot in the horizontal plane can be obtained, as shown in Figure 9. Its maximum circumscribed radius is r com = 267.977 mm , and r ssa = r com can be obtained according to the stability criterion, so it can be judged that the stability analysis of the gait switching process is correct and effective, and it also verifies that the robot gait switching method is feasible and effective.

Figure 9

Circumcircle radius of centroid action area of robot gait switching.

5.2 Rotational walking on flat ground with a fixed point

The simulation experiment was carried out with the robot rotating 180° to the left, with each rotation of 18° being completed in ten passes. The initial parameters of the robot were set based on the results of the robot’s fixed-point rotation gait planning analysis, i.e., the height of the body above the ground H = 1,000 mm and the initial position of leg1 a 1 = 5.6653 ° , R 1 = 1800.0911 mm ; initial position of leg2 a 2 = − 20 ° , R 2 = 1 , 800 mm ; initial position of leg3 a 3 = 12.6779 ° , R 3 = 1937.1296 mm ; initial position of leg4 a 4 = 20 ° , R 4 = 1 , 300 mm ; initial position a 5 = − 9.93 ° , R 5 = 1767.1646 mm for leg5; initial position a 6 = − 20 ° , R 6 = 2 , 500 mm for leg6.

(1) At T = 0 s , the robot receives a motion command and then starts to move according to the two-step left rotation rule of 18°. During T = 0 – 2 s leg1, leg3, and leg5 are in the oscillation phase and oscillate towards the target position a 1 = − 20 ° , R 6 = 2 , 500 mm , a 3 = − 20 ° , R 3 = 1 , 600 mm , a 5 = 20 ° , R 5 = 1 , 800 mm , respectively, according to the planned motion rule; leg2, leg4, and leg6 are in the support phase and support the body towards the target position a 2 = 9.95 ∘ , R 2 = 1767.16 mm , a 4 = − 12.68 ∘ , R 4 = 1937.13 mm , a 6 = − 5.67 ∘ , R 6 = 1800.09 mm , respectively, according to the planned motion rule. This is shown in Figure 10.

Figure 10

The process of the robot rotating at a fixed point on the flat ground.

The motion law of each joint of the robot’s leg during this motion process can be obtained through simulation. The motion law of the robot’s leg integrated centroid can be obtained through the calculation method of the leg integrated centroid, as shown in Figure 11.

Figure 11

Integrated centroid coordinates of robot’s flat ground fixed point rotation leg.