Foot trajectory planning method based on parallel four-legged pipeline wall-climbing robot

Abstract

The application discloses a foot end trajectory planning method based on a parallel four-foot pipeline wall climbing robot, which comprises the following steps: S1, simplifying the structure of the robot and setting the structural parameters of the robot; S2, determining the initial pose of the robot based on the relative relationship between the robot and the pipeline coordinates; S3, constructing a single-leg kinematic model of the robot; S4, planning the single-leg foot end motion trajectory of the robot based on the single-leg kinematic model; and S5, assuming that the robot moves on a plane based on the planning of the single-leg foot end motion trajectory of the robot, and obtaining the specific motion trajectory of the robot. The application reduces the adverse impact caused by the foot end trajectory, improves the continuity, stability and efficiency of the robot motion, and especially reduces the impact caused by the process of the foot end leaving and contacting the ground in the foot end swing phase.

Description

Technical Field

[0001] This invention relates to robotics technology, specifically to a method for planning foot trajectory based on a parallel quadrupedal pipe-climbing robot. Background Technology

[0002] Pipelines are widely used in daily production and life, such as for the long-distance transportation and storage of liquid and gas resources, and as protective structures for electrical equipment. Pipeline equipment can experience various faults during use, requiring regular inspection and maintenance. Detecting and troubleshooting faults inside narrow and complex pipelines is extremely difficult and risky. To replace traditional methods of pipeline internal inspection, a parallel quadrupedal pipeline climbing robot was designed.

[0003] In legged robot movement, to reduce the impact force during movement and decrease the load on the motors, the impact force on the feet should be minimized. Generally, an unreasonable foot trajectory can cause adverse impacts, affecting the continuity, stability, and efficiency of the robot's movement. To avoid such adverse impacts, especially those caused by the foot leaving and contacting the ground during the foot swing phase, it is necessary to plan the movement trajectory of the robot's leg ends. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of the existing technology and provide a simple and reasonable foot trajectory planning method for a parallel quadrupedal pipe climbing robot that does not bite the blade.

[0005] The objective of this invention is achieved through the following technical solution: This method for planning the trajectory of the foot end of a parallel quadrupedal pipe-climbing robot includes the following steps:

[0006] S1. Simplify the robot's structure and set its structural parameters;

[0007] The structural parameters include: the distance 2a between the first joints of the front and rear feet, the distance 2b between the first joints of the left and right feet, the distances L1 and L2 between the first and second joints along the orthogonal axis, the distance L3 between the second and third joints, the length of the active link of the second and third joints L4, the length of the passive link of the second and third joints L5, and the length of the foot end link L6.

[0008] S2. Determine the robot's initial pose based on the relative relationship between the robot and the pipeline coordinates;

[0009] Step S2 includes the following steps:

[0010] S201. Construct the robot's body coordinate system and the pipe's coordinate system: The pipe's radius is R, with the pipe's center as the origin and the pipe's central axis as x. O The axis, vertically upward is z. OEstablish the pipeline's basic coordinate system {O} with the robot's center as the origin, x A0 The direction of the axis is along the direction of the robot's axial movement, z A0 Establish the robot's body coordinate system {A0} with the axis perpendicular to the body and pointing upwards.

[0011] S202. Based on the coordinate system established in step S201, confirm the relative pose parameters between the robot and the pipeline: The robot pose is determined using 6 relative pose parameters, which are as follows: α, β, and γ; among which, α, β, and γ are the position coordinates of the origin of the body coordinate system {A0} in the pipe coordinate system {O}, representing the position of the robot center relative to the origin of the pipe coordinate system; α, β, and γ are the angles of rotation of the body coordinate system {A0} relative to the pipe coordinate system {O} around the x0, y0, and z0 axes, respectively.

[0012] S203. Based on the robot's coordinate system established in step S201 and the relative pose parameters determined in step S202, the homogeneous transformation matrix of the robot's body coordinate system relative to the pipe coordinate system is determined as follows:

[0013]

[0014] in, R(z0,γ) is the homogeneous transformation matrix of the robot's body coordinate system relative to the pipe coordinate system; R(y0,β) is the rotation transformation matrix of the robot's body coordinate system relative to the pipe coordinate system, rotating it about the z0 axis by γ degrees; R(y0,β) is the rotation transformation matrix of the robot's body coordinate system relative to the pipe coordinate system, rotating it about the y0 axis by β degrees; and R(x0,α) is the rotation transformation matrix of the robot's body coordinate system relative to the pipe coordinate system, rotating it about the x0 axis by α degrees. This is the rotation transformation matrix of the robot's body coordinate system relative to the pipe coordinate system;

[0015] S204. Based on the coordinate system established in step S201 and the relative pose parameters determined in step S202, determine the robot's initial pose: the angle of joint 1 is not 0, the foot link is perpendicular to the inner wall, and the origin O of the body coordinate system is... A0 Located at the origin O of the pipeline coordinate system O Directly below, that is The coordinate system deflection angles α = β = γ = 0, and the robot's leg extension length is... in In the initial state The value, For vector O A3 O A4 In y A3 The projected length on the axis; then the geometric relationship of the robot's initial pose can be obtained as follows:

[0016]

[0017]

[0018] S205. Based on the homogeneous transformation matrix of the robot's body coordinate system relative to the pipe coordinate system determined in step S203 and the robot's initial state given in step S204, the homogeneous transformation matrix of the robot's body coordinate system relative to the pipe coordinate system in the robot's initial pose is determined as follows:

[0019]

[0020] S3. Construct a single-leg kinematic model of the robot; Step S3 includes the following steps:

[0021] S301. Establish the three-dimensional spatial coordinate system of each joint of the robot's left front leg based on the position of the rotation joints: a body coordinate system {A0} with the robot's center as the origin, and coordinate systems {A1} and {A2} with the center of the first joint motor of the robot's single leg as the origin, where coordinate system {A2} is relative to the x-axis of coordinate system {A1}. A1 The axis rotation is a joint angle of magnitude θ1, which is the rotation angle of the first joint motor. There are three coordinate systems: {A3} with the center of the second joint motor of the robot's single leg as the origin, {A4} with the center of the end of the parallel structure as the origin, and {A5} with the center of the foot end as the origin. The z-coordinate of coordinate system {A5} relative to coordinate system {A4} is... A4 The axis is translated downward by L6; thus constructing the spatial coordinate system of the robot's left front leg;

[0022] S302. Based on the spatial coordinate system of the left foreleg determined in step S301, obtain the planar coordinate system of the left foreleg.

[0023] S303. Based on the spatial coordinate system of the left front leg in step S301 and the planar coordinate system of the left front leg in step S302, the homogeneous transformation matrix of coordinate system {A0} relative to coordinate system {A5} is as follows:

[0024]

[0025] in, Let {A0} be the homogeneous transformation matrix relative to coordinate system {A5}. Let {A0} be the homogeneous transformation matrix relative to coordinate system {A1}. Let {A1} be the homogeneous transformation matrix relative to coordinate system {A2}. Let {A2} be the homogeneous transformation matrix relative to coordinate system {A3}. Let {A3} be the homogeneous transformation matrix relative to coordinate system {A4}. Let {A4} be the homogeneous transformation matrix relative to coordinate system {A5}.

[0026] S304. Based on the homogeneous transformation matrix of step S303, assume that the coordinates of the foot of the left foreleg in coordinate system {A5} are... A5 P = [x A5 y A5 z A5 1] T =[0 0 0 1] T We can obtain the relationship between the coordinates of the foot end of the left foreleg in coordinate system {A0} and the joint rotation angle, that is, the forward kinematics solution formula of the robot's left foreleg is:

[0027]

[0028] in, A0 P = [x A0 y A0 z A0 1] T Let A0 be the coordinates of the foot of the left foreleg in the coordinate system {A0}. Let {A0} be the homogeneous transformation matrix of coordinate system {A5} relative to coordinate system {A0}. For vector O A3 O A4 In x A3 Projected length on the axis For vector O A3 O A4 In y A3 Projected length on the axis;

[0029] S305. Based on the robot's left front leg spatial coordinate system established in step S301, the robot's left front leg planar coordinate system established in step S302, and the robot's left front leg forward kinematics solution obtained in step S304, the robot's left front leg inverse kinematics solution can be obtained as follows:

[0030]

[0031] Wherein, θ1, θ2 and θ3 are the rotation angles of joint motor No. 1, joint motor No. 2 and joint motor No. 3, respectively;

[0032] Due to structural limitations of the robot, the values ​​of θ2 and θ3 are constrained, resulting in the final inverse kinematics formula:

[0033]

[0034] in,

[0035]

[0036] S4. Based on the single-leg kinematic model, plan the single-leg foot motion trajectory of the robot;

[0037] Step S4 includes the following specific steps:

[0038] S401. Based on the homogeneous transformation matrix determined in step S203 and the initial state of the robot determined in step S204, the coordinates of the robot's left foreleg foot in the initial state of the body coordinate system {A0} are determined as follows:

[0039] S402. Determine that the robot's foot movement is divided into a support phase and a swinging phase, and the combination of the two phases constitutes a complete cycle of foot movement trajectory. Let the complete motion cycle be T. m The supporting phase period is T s The oscillation phase period is T f Then T m =T s +T f ;

[0040] S403, Determine h x h y and h z These represent the positions of the robot's foot tip along the body coordinate system x A0 y A0 and z A0 The maximum step size of the axis, and has

[0041] S404, Based on the parameter h determined in step S403 x h y and h z Establish an auxiliary coordinate system {T} on the plane containing the complete trajectory. The plane containing the auxiliary coordinate system {T} is parallel to the x-axis. A0 O A0 y A0 The included angle between the planes is

[0042] S405. The support phase linear trajectory of the robot's left foreleg foot in the body coordinate system {A0} is:

[0043]

[0044] Where l is the x-coordinate of the foot in the auxiliary coordinate system {T} T The position on the axis is related to the gait sampling time t, i.e. and 0≤t≤T s ;

[0045] S406. The cycloidal trajectory of the robot's left foreleg foot in the auxiliary coordinate system {T} is as follows:

[0046]

[0047] Where t is the gait sampling time, T f Let the oscillation phase period be 0 ≤ t ≤ T. f ;

[0048] S407. The cycloidal trajectory of the robot's left foreleg foot moving in the body coordinate system {A0} is as follows:

[0049]

[0050] In the formula, (x T ,y T ) represents the position of the foot in the auxiliary coordinate system {T};

[0051] S5. Based on the planning of the robot's single-leg foot trajectory, assuming the robot moves on a plane, obtain the robot's specific motion trajectory.

[0052] Preferably, based on the inverse kinematics formula of the robot's left foreleg obtained in step S305, the inverse kinematics formulas of the other three leg structures can be obtained:

[0053] S306. Based on the robot body coordinate system established in step S201, assume that the coordinate position of the four-legged foot end in the body coordinate system {A0} is: left front leg (x A0-1 ,y A0-1 ,z A0-1 ), right foreleg (x) A0-2 ,y A0-2 ,z A0-2 ), left hind leg and right hind leg (x A0-4 ,y A0-4 ,z A0-4 The angles of the three joints corresponding to each foot are θ. 1-k θ 2-k and θ 3-k , where k = 1, 2, 3, “;

[0054] S307. Based on the inverse kinematics formula of the robot's left front leg obtained in step S305 and the assumed coordinate position of the four-legged foot tip in the body coordinate system {A0} in step S306, the inverse kinematics solution of the four-legged foot tip in the body coordinate system {A0} can be obtained as follows:

[0055]

[0056] in,

[0057]

[0058] Both p and q are constant values, determined according to the robot's leg number.

[0059] Preferably, step S5 includes the following steps:

[0060] S501. Assign values ​​to the robot's structural parameters;

[0061] S502. Based on the coordinates of the initial state determined in step 401, when the robot is moving in a plane, set the size of the joint angles of each joint of the robot's single leg in the initial state to determine the position of the foot in the body coordinate system {A0}.

[0062] S503, Based on the parameter h determined in step S403 x h y and h z Step S501 determines the robot's structural parameters a, b, L1, L2, L3, L4, L5, L6, θ0, θ1, θ2 and θ3, and determines the robot's motion trajectory parameters when moving in a plane.

[0063] S504. Based on the support phase straight-line trajectory of the robot's left foreleg foot in the body coordinate system {A0} determined in step S405, the specific equation of the support phase straight-line trajectory of the robot (left foreleg foot in the body coordinate system {A0}) during planar motion is as follows:

[0064]

[0065] The support phase straight line trajectory of the robot's left front leg foot in the body coordinate system {A0} can be obtained when the robot moves in a plane.

[0066] S505. Based on the swing phase composite cycloidal trajectory of the robot's left foreleg foot in the auxiliary coordinate system {T} determined in step S406 and the swing phase composite cycloidal trajectory of the robot's (0) left foreleg foot in the body coordinate system {A0} determined in step S407, the specific equation of the swing phase composite cycloidal trajectory of the robot's left foreleg foot in the body coordinate system {A0} when the robot is moving in a plane is as follows:

[0067]

[0068] The composite cycloidal trajectory of the robot's left front leg foot in the auxiliary coordinate system {T} and the composite cycloidal trajectory of the robot's left front leg foot in the body coordinate system {A0} when the robot is moving in a plane can be obtained.

[0069] S506. Based on the straight trajectory of the support phase of the robot's left front leg foot in the body coordinate system {A0} when the robot moves in a plane as obtained in step S504 and the composite cycloidal trajectory of the swing phase of the robot's (0) left front leg foot in the body coordinate system {A0} when the robot moves in a plane as obtained in step S505, the complete trajectory of the robot's (0) left front leg foot in the body coordinate system {A0} can be obtained.

[0070] S507. Based on the inverse kinematics formula of the robot's left front leg obtained in step S305 and the support phase straight line trajectory of the robot's left front leg foot in the body coordinate system {A0} during the robot's planar motion obtained in step S504, the change curves of the three joint angles of the robot's left front leg foot during the straight line motion can be obtained.

[0071] S508. Based on the inverse kinematics formula of the robot's left front leg obtained in step S305 and the oscillating composite cycloidal trajectory of the robot's left front leg foot in the body coordinate system {A0} during planar motion obtained in step S505, the change curves of the three joint angles of the robot's left front leg foot along the composite cycloidal motion can be obtained.

[0072] Compared with the prior art, the present invention has the following advantages: Based on the requirements of motion continuity and zero impact when lifting and landing, the present invention plans the trajectory of the robot's foot swing phase based on a composite cycloidal trajectory, thereby reducing the adverse impact caused by the foot trajectory and improving the continuity, stability and efficiency of robot motion; in particular, it reduces the impact caused by the foot leaving and contacting the ground during the foot swing phase. Attached Figure Description

[0073] Figure 1 This is a simplified structural diagram of a parallel quadrupedal wall-climbing robot according to an embodiment of the present invention.

[0074] Figure 2 This is a schematic diagram of the body coordinate system and the pipeline coordinate system of a parallel quadrupedal wall-climbing robot according to an embodiment of the present invention.

[0075] Figure 3 This is a schematic diagram of the initial pose of a parallel quadrupedal wall-climbing robot according to an embodiment of the present invention.

[0076] Figure 4 This is a schematic diagram of the spatial coordinate system of the left front leg of the quadrupedal motion module of the parallel quadrupedal wall-climbing robot according to an embodiment of the present invention.

[0077] Figure 5 This is a schematic diagram of the left front leg planar coordinate system of the quadrupedal motion module of the parallel quadrupedal wall-climbing robot according to an embodiment of the present invention.

[0078] Figure 6This is a simplified diagram of the left front leg planar coordinate system of the quadrupedal motion module of the parallel quadrupedal wall-climbing robot according to an embodiment of the present invention.

[0079] Figure 7 This is a schematic diagram of the left front leg end of the quadrupedal motion module of the parallel quadrupedal wall-climbing robot in an embodiment of the present invention moving along a straight trajectory in the robot's body coordinate system {A0} during the support phase.

[0080] Figure 8 This is a schematic diagram of the left front leg of the parallel quadrupedal wall-climbing robot in an embodiment of the present invention moving along a compound cycloidal trajectory in the auxiliary coordinate system {T} during the swing phase.

[0081] Figure 9 This is a schematic diagram of the left front leg of the parallel quadruped wall-climbing robot in the embodiment of the present invention moving along a compound cycloidal trajectory in the robot's body coordinate system {A0} during the swing phase.

[0082] Figure 10 This is a schematic diagram of the complete motion trajectory of the left foreleg end of the quadrupedal motion module of the parallel quadrupedal wall-climbing robot according to an embodiment of the present invention in the robot's body coordinate system {A0}.

[0083] Figure 11 This invention relates to an embodiment of a parallel quadrupedal wall-climbing robot. The diagram shows the changes in the angles of the three joint motors of the left front leg of the quadrupedal motion module during the support phase movement along a straight trajectory.

[0084] Figure 12 This invention relates to an embodiment of a parallel quadrupedal wall-climbing robot, showing the changes in the angles of the three joint motors of the left front leg of the quadrupedal motion module during the swing phase as the quadruped moves along a compound cycloidal trajectory.

[0085] Figure 13 This is a schematic diagram of the structure of a parallel quadrupedal pipe-climbing robot according to an embodiment of the present invention.

[0086] Figure 14 This is a schematic diagram of the parallel leg mechanism in the parallel quadrupedal pipe climbing robot of this invention.

[0087] Among them, 100 is the main body, 200 is the quadrupedal movement device, 1 is the parallel leg mechanism, 2 is the vacuum adsorption mechanism, 3 is the hip joint support, 4 is the first joint motor, 5 is the second joint motor, 6 is the third joint motor, 7 is the first link assembly, 8 is the second link assembly, 9 is the hinge frame, 10 is the thigh link, 11 is the lower leg link, 12 is the hinge piece, 13 is the hinge plate, 14 is the first hinge shaft, 15 is the connecting part, 16 is the mounting part, 17 is the bolt, 18 is the second hinge shaft, 19 is the clamping plate, FL is the left front leg, FR is the right front leg, BL is the left hind leg, and BR is the right hind leg. Detailed Implementation

[0088] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0089] like Figure 13 and 14 As shown, the parallel quadrupedal pipe-climbing robot used in this embodiment is characterized by: a main body and a quadrupedal motion device. The quadrupedal motion device includes four sets of parallel leg mechanisms and a vacuum adsorption mechanism corresponding to each set of parallel leg mechanisms. The four sets of parallel leg mechanisms are respectively installed around the main body, and every two sets of parallel leg mechanisms are symmetrically arranged. The parallel leg mechanism includes a hip joint support, a first joint motor, a second joint motor, a third joint motor, a first link assembly, and a second link assembly. The first joint motor, the second joint motor, and the third joint motor are all installed on the hip joint support. The first link assembly and the second link assembly are hinged in parallel to the hip joint support, and one end of the first link assembly and the second link assembly are respectively connected to the first joint motor and the second joint motor. The other end of the first link assembly and the other end of the second link assembly are connected through a hinge frame. The vacuum suction cup of the corresponding vacuum adsorption mechanism is installed on the hinge frame. The rotating shaft of the third joint motor is connected to the main body. Both the first and second linkage assemblies include a thigh linkage, a calf linkage, and a hinge. One end of the thigh linkage is hinged to the hip joint support. The thigh linkage is hinged to the calf linkage via the hinge. The calf linkage in the first linkage assembly and the calf linkage in the second linkage assembly are hinged together via a hinge frame. The thigh linkage in the first linkage assembly is connected to the first joint motor, and the thigh linkage in the second linkage assembly is connected to the second joint motor. The thigh linkage, calf linkage, and hip joint support in the first and second linkage assemblies constitute a planar five-bar parallel structure.

[0090] The articulated frame includes a hinge plate and a first hinge shaft. The hinge plate includes a connecting portion and a mounting portion integrally formed with the connecting portion. The lower leg connecting rod in the first connecting rod assembly is hinged to the connecting portion via the first hinge shaft. The lower leg connecting rod in the second connecting rod assembly is fixedly connected to the connecting portion via bolts. The vacuum suction cup is mounted on the mounting portion. This articulated frame has a simple structure and is easy to install, further improving the reliability of the parallel leg mechanism.

[0091] The hinge includes a second hinge shaft and two clamping plates. One end of each clamping plate is fixed to the thigh link with bolts, and one end of each clamping plate is located on both sides of the thigh link. The other ends of each clamping plate are hinged to the lower leg link via the second hinge shaft, and the other ends of each clamping plate are located on both sides of the lower leg link. As the joint between the thigh link and the lower leg link, the hinge in this embodiment ensures both the rigidity of the leg and the stability of leg movement.

[0092] Specifically, such as Figure 13 and 14 As shown, the four parallel leg mechanisms are the left front leg, right front leg, left rear leg, and right rear leg. The left and right front legs are mounted on the front sides of the main body and are symmetrically arranged; while the left and right rear legs are mounted on the rear sides of the main body and are symmetrically arranged. The left, right, left, and right front legs have the same structure, only their installation positions on the main body differ. That is, the left, right, left, and right front legs all have a parallel leg structure, including a hip joint bracket, a first joint motor, a second joint motor, a third joint motor, a first linkage assembly, and a second linkage assembly. In this embodiment, the first, second, and third joint motors are all Feite STS3032TTL serial port servos. The first and second linkage assemblies are respectively hinged to two adjacent sides of the hip joint bracket, and the first and second linkage assemblies are controlled independently by the second and third joint motors, respectively. This parallel structure avoids the problems of increased torque and insufficient linkage stiffness caused by a series structure.

[0093] For the aforementioned parallel quadrupedal pipe-climbing robot, a method for foot trajectory planning based on the parallel quadrupedal pipe-climbing robot is proposed, including the following steps:

[0094] S1. Simplify the robot's structure and set its structural parameters; specifically, simplify the structure of the aforementioned parallel quadrupedal pipe-climbing robot, as follows: Figure 1 As shown. The structural parameters of the robot are as follows: the distance between the first joints of the front and rear feet is 2a, the distance between the first joints of the left and right feet is 2b, the distances between the first and second joints along the orthogonal axis are L1 and L2, the distance between the second and third joints is L3, the length of the active link of the second and third joints is L4, the length of the passive link of the second and third joints is L5, and the length of the foot end link is L6.

[0095] S2. Determine the robot's initial pose based on the relative relationship between the robot and the pipeline coordinates; specifically, step S2 includes the following steps:

[0096] S201. Construct the robot's body coordinate system and the pipe's coordinate system: (e.g.) Figure 2 As shown, let the radius of the pipe be R, with the center of the pipe as the origin and the central axis of the pipe as x. O The axis, vertically upward is z. O Establish the pipeline's basic coordinate system {O} with the robot's center as the origin, x A0 The direction of the axis is along the direction of the robot's axial movement, z A0 Establish the robot's body coordinate system {A0} with the axis perpendicular to the body and pointing upwards.

[0097] S202. Based on the robot's body coordinate system and the pipe's coordinate system established in step S201, confirm the relative pose parameters of the robot and the pipe: The robot pose is determined using 6 relative pose parameters, which are as follows: α, β, and γ; among which, α, β, and γ are the position coordinates of the origin of the body coordinate system {A0} in the pipe coordinate system {O}, representing the position of the robot center relative to the origin of the pipe coordinate system; α, β, and γ are the angles of rotation of the body coordinate system {A0} relative to the pipe coordinate system {O} around the x0, y0, and z0 axes, respectively.

[0098] S203. Based on the coordinate system established in step S201 and the relative pose parameters determined in step S202, the homogeneous transformation matrix of the robot body coordinate system relative to the pipe coordinate system is determined as follows:

[0099]

[0100] in, R(z0,γ) is the homogeneous transformation matrix of the robot's body coordinate system relative to the pipe coordinate system; R(y0,β) is the rotation transformation matrix of the robot's body coordinate system relative to the pipe coordinate system, rotating it about the z0 axis by γ degrees; R(y0,β) is the rotation transformation matrix of the robot's body coordinate system relative to the pipe coordinate system, rotating it about the y0 axis by β degrees; and R(x0,α) is the rotation transformation matrix of the robot's body coordinate system relative to the pipe coordinate system, rotating it about the x0 axis by α degrees. This is the rotation transformation matrix of the robot's body coordinate system relative to the pipe coordinate system;

[0101] S204. Based on the coordinate system established in step S201 and the relative pose parameters determined in step S202, determine the robot's initial pose, such as... Figure 3 As shown: the angle of joint number one (i.e., the hip joint) is not 0, the foot-end connecting rod is perpendicular to the inner wall, and the origin O of the fuselage coordinate system is... A0 Located at the origin O of the pipeline coordinate system O Directly below, that is The coordinate system deflection angles α = β = γ = 0, and the robot's leg extension length is... in In the initial state The value, For vector O A3 O A4 In y A3 The projected length on the axis; then the geometric relationship of the robot's initial pose can be obtained as follows:

[0102]

[0103] S205. Based on the homogeneous transformation matrix of the robot's body coordinate system relative to the pipe coordinate system determined in step S203 and the initial state of the robot given in step S204, the homogeneous transformation matrix of the robot's body coordinate system relative to the pipe coordinate system in the initial state is determined as follows:

[0104]

[0105] S3. Construct a single-leg kinematic model of the robot; specifically, taking the robot's left foreleg as an example, the single-leg kinematic model of the robot is constructed. Step S3 includes the following steps:

[0106] S301. Establish the three-dimensional spatial coordinate system of each joint of the robot's left front leg based on the position of the rotational joints: such as Figure 4 As shown, there is a body coordinate system {A0} with the robot's center as the origin, and coordinate systems {A1} and {A2} with the center of the first joint motor of the robot's single leg as the origin. In coordinate system {A1}, x... A1 The axial direction is collinear with the rotation axis of joint 1 and with the x-axis in coordinate system {A0}. A0 The axes are parallel; coordinate system {A1} and coordinate system {A0} are parallel at x. A0 axis and y A0 The directional distances between the axes are a and b, respectively; coordinate system {A2} and coordinate system {A1} are located at the same origin, and coordinate system {A2} is relative to the x-axis of coordinate system {A1}. A1 The joint angle with a rotational magnitude of θ1 is the rotation angle of the first joint motor; the coordinate system {A3} is with the center of the second joint motor of the robot's single leg as the origin, where x in coordinate system {A3}... A3 axis and y A3 The axes are respectively with x A2 axis and y A2 The axis is parallel, and the origin is O. A3 Relative to the origin O of {A2} A2 Along x A2 axis and y A2 The axes are translated by L1 and L2 respectively; a coordinate system {A4} with the center of the end of the parallel structure as the origin is established. A4 axis and y A4 The axes are respectively with x A3 axis and y A3 The axis is parallel, and the origin is O. A4 Relative to the origin O of {A3} A3 Along x A3 axis and y A3 Translation of axes respectively and in, and The origin O of coordinate system {A4} is respectively A4The origin O of coordinate system {A3} A3 The line vector in x A3 and y A3 The projected length on the axis; and a coordinate system {A5} with the center of the foot as the origin, where the z-axis of coordinate system {A5} is relative to coordinate system {A4}. A4 The axis is translated downward by L6; thus constructing the spatial coordinate system of the robot's left front leg;

[0107] S302. Based on the spatial coordinate system of the left front leg determined in step S301, obtain the planar coordinate system of the left front leg. Specifically, in the robot's single-leg coordinate system, the origins of coordinate systems {A2}, {A3}, and {A4}, as well as the x-axis and y-axis, are located on a unified plane, and the z-axis is parallel, forming a planar two-degree-of-freedom parallel structure controlled by joint motors 2 and 3. Project the parallel structure onto the two-dimensional plane x... A2 O A2 y A2 Obtain a single-leg plane coordinate system, such as Figure 5 As shown. The single-leg plane coordinate system is then simplified to obtain the following: Figure 6 As shown, θ2 and θ3 are the rotation angles of the second and third joint motors, respectively, and δ and φ are the angles between the second joint motor and the third passive joint link relative to the horizontal direction, i.e., CO. A4 with BO A4 The rotation angles relative to the horizontal direction are ψ, where ψ is the angle between CB and the horizontal direction.

[0108] S303. Based on the spatial coordinate system of the left front leg in step S301 and the planar coordinate system of the left front leg in step S302, the homogeneous transformation matrix of coordinate system {A0} relative to coordinate system {A5} is as follows:

[0109]

[0110] in, Let {A0} be the homogeneous transformation matrix relative to coordinate system {A5}. Let {A0} be the homogeneous transformation matrix relative to coordinate system {A1}. Let {A1} be the homogeneous transformation matrix relative to coordinate system {A2}. Let {A2} be the homogeneous transformation matrix relative to coordinate system {A3}. Let {A3} be the homogeneous transformation matrix relative to coordinate system {A4}. Let {A4} be the homogeneous transformation matrix relative to coordinate system {A5}.

[0111] S304. Based on the homogeneous transformation matrix of step S303, assume that the coordinates of the foot of the left foreleg in coordinate system {A5} are...A5 P = [x A5 y A5 z A5 1] T =[0 0 0 1] T We can obtain the relationship between the coordinates of the foot end of the left foreleg in coordinate system {A0} and the joint rotation angle, that is, the forward kinematics solution formula of the robot's left foreleg is:

[0112]

[0113] in, A0 P = [x A0 y A0 z A0 1] T Let A0 be the coordinates of the foot of the left foreleg in the coordinate system {A0}. Let {A0} be the homogeneous transformation matrix of coordinate system {A5} relative to coordinate system {A0}. For vector O A3 O A4 In x A3 Projected length on the axis For vector O A3 O A4 In y A3 Projected length on the axis;

[0114] in, and The solution steps are as follows:

[0115] S3041. Based on the robot's single-leg plane coordinate system established in step S302, we can obtain:

[0116]

[0117] O A3 B=O A3 A+AB=(L3+L4cosθ3 L4sinθ3)=(x B y B );

[0118] S3042. Based on the vector relationships in step S3041, we can obtain:

[0119]

[0120] S3043. Based on the robot's single-leg plane coordinate system established in step S302, we can obtain:

[0121]

[0122] in,

[0123] S3044. Based on the angle relationship obtained in step S3043, we can obtain:

[0124] 2L5 cosδ(x B -x C )+2L5 sinδ(y B -y C )=|CB| 2 ;

[0125] Let c = 2L5(x) B -x C ), d=2L5(y B -y C From this, we can obtain:

[0126] c cosδ+d sinδ=|CB| 2 ;

[0127] S3045. Based on the angle relationship obtained in step S3044, we can obtain:

[0128]

[0129] Solving for the given information yields:

[0130] S3046. Based on the robot's single-leg planar coordinate system established in step S302 and the angle relationship obtained in step S3045, we can obtain:

[0131] O A3 O A4 =O A3 C+CO A4

[0132] =(x C +L5 cosδ y C +L5 sinδ)

[0133] =(L4 cosθ2+L5 cosδ L4 sinθ2+L5 sinδ)

[0134] Furthermore, we can obtain:

[0135]

[0136] in,

[0137]

[0138] c = 2L5(x) B -x C )=2L5(L3+L4cosθ3-L4cosθ2),

[0139] d = 2L5(y B -y C ) = 2L5(L4sinθ3 - L4sinθ2);

[0140] Furthermore, due to the structural limitations of the robot, it is required that...

[0141] S305. Based on the robot's left front leg spatial coordinate system established in step S301, the robot's left front leg planar coordinate system established in step S302, and the robot's left front leg forward kinematics solution obtained in step S304, the robot's left front leg inverse kinematics solution can be obtained as follows:

[0142] In the formula, θ1, θ2 and θ3 are the rotation angles of joint motor No. 1, joint motor No. 2 and joint motor No. 3, respectively.

[0143] Due to structural limitations of the robot, the values ​​of θ2 and θ3 are constrained, resulting in the final inverse kinematics formula:

[0144]

[0145] in,

[0146]

[0147]

[0148] Based on the inverse kinematics formula of the robot's left foreleg obtained in step S305, the inverse kinematics formulas of the other three leg structures can be obtained:

[0149] S306. Based on the robot body coordinate system established in step S201, assume that the coordinate position of the four-legged foot end in the body coordinate system {A0} is: left front leg (x A0-1 ,y A0-1 ,z A0-1 ), right foreleg (x) A0-2 ,y A0-2 ,z A0-2 ), left hind leg and right hind leg (x A0-4 ,y A0-4 ,z A0-4 The angles of the three joints corresponding to each foot are θ. 1-k θ 2-k and θ 3-k , where k = 1, 2, 3, 4”;

[0150] S307. Based on the inverse kinematics formula of the robot's left front leg obtained in step S2305 and the assumed coordinate position of the four-legged foot tip in the body coordinate system in step S306, the inverse kinematics solution of the four-legged foot tip in the body coordinate system {A0} is obtained as follows:

[0151]

[0152] in,

[0153]

[0154]

[0155] Both p and q are constants, determined by the robot's leg numbers. k is the robot's leg number, where k = 1, 2, 3, 4 represent the left front leg, right front leg, left hind leg, and right hind leg, respectively. The values ​​of p and q depend on the leg numbers.

[0156]

[0157] S4. Based on the single-leg kinematic model, plan the motion trajectory of the robot's single-leg foot; specifically, step S4 includes the following steps:

[0158] S401. Based on the homogeneous transformation matrix determined in step S203 and the initial state of the robot determined in step S204, the coordinates of the robot's left foreleg foot in the initial state of the body coordinate system {A0} are determined as follows:

[0159] S402. The robot's foot movement is divided into a support phase and a swinging phase, which combine to form a complete cycle of foot movement trajectory. The support phase primarily supports the robot's movement on the ground; therefore, a smooth straight line is chosen as its trajectory. During the swinging phase, the foot's velocity and acceleration must remain continuous without abrupt changes. Upon leaving the ground and landing, both velocity and acceleration are zero. A composite cycloidal trajectory is selected for related trajectory planning. Let the complete motion cycle be T. m The supporting phase period is T s The oscillation phase period is T f Then T m =T s +T f ;

[0160] S403, Determine h x h y and h z These represent the positions of the robot's foot tip along the body coordinate system x A0 y A0 and z A0The maximum step size of the axis, and has

[0161] S404, Based on the parameter h determined in step S403 x h y and h z Establish an auxiliary coordinate system {T} on the plane containing the complete trajectory. The plane containing the auxiliary coordinate system {T} is parallel to the x-axis. A0 O A0 y A0 The included angle between the planes is

[0162] S405. The support phase linear trajectory of the robot's left foreleg foot in the body coordinate system {A0} is:

[0163]

[0164] Where l is the x-coordinate of the foot in the auxiliary coordinate system {T} T The position on the axis is related to the gait sampling time t, i.e. and 0≤t≤T s ;

[0165] S406. The cycloidal trajectory of the robot's left foreleg foot in the auxiliary coordinate system {T} is as follows:

[0166]

[0167] Where t is the gait sampling time, T f Let the oscillation phase period be 0 ≤ t ≤ T. f ;

[0168] S407. The cycloidal trajectory of the robot's left foreleg foot moving in the body coordinate system {A0} is as follows:

[0169]

[0170] In the formula, (x T ,y T ) represents the position of the foot in the auxiliary coordinate system {T}.

[0171] S5. Based on the planning of the robot's single-leg foot trajectory, assuming the robot moves on a plane, obtain the robot's specific motion trajectory. Specifically, step S5 includes the following steps:

[0172] S501. Assign values ​​to the robot's structural parameters; in this embodiment, the specific values ​​of each structural parameter are as follows:

[0173]

[0174] S502. Based on the initial coordinates determined in step 401, when the robot is moving in a plane, set the angles of each joint of the robot's single leg in the initial state, i.e., θ1 = 0°, θ2 = 124.08°, θ3 = 55.92°, to determine the position of the foot in the body coordinate system {A0}. This position is:

[0175]

[0176] S503, Based on the parameter h determined in step S403 x h y and h z Step S501 determines the robot's structural parameters a, b, L1, L2, L3, L4, L5, L6, θ0, θ1, θ2, and θ3, and determines the robot's motion trajectory parameters during planar motion. The specific values ​​of these motion trajectory parameters are as follows:

[0177]

[0178] S504. Based on the support phase straight-line trajectory of the robot's left foreleg foot in the body coordinate system {A0} determined in step S405, the specific equation of the support phase straight-line trajectory of the robot (left foreleg foot in the body coordinate system {A0}) during planar motion is as follows:

[0179]

[0180] Among them, the supporting phase period T s =10s, then The unit is mm. The support phase linear trajectory of the robot's left foreleg foot in the body coordinate system {A0} during planar motion can be obtained, as shown in the following figure. Figure 7 ;

[0181] S505. Based on the swing phase composite cycloidal trajectory of the robot's left foreleg foot in the auxiliary coordinate system {T} determined in step S406 and the swing phase composite cycloidal trajectory of the robot's (0) left foreleg foot in the body coordinate system {A0} determined in step S407, the specific equation of the swing phase composite cycloidal trajectory of the robot's left foreleg foot in the body coordinate system {A0} when the robot is moving in a plane is as follows:

[0182]

[0183] Among them, the oscillation phase period T f =10s, the cycloidal trajectory of the robot's left foreleg foot in the auxiliary coordinate system {T} can be obtained, such as Figure 8As shown, the cycloidal trajectory of the robot's left foreleg foot moving in the body coordinate system {A0} during planar motion can be obtained, as follows: Figure 9 As shown;

[0184] S506. Based on the straight-line trajectory of the support phase of the robot's left front leg foot in the body coordinate system {A0} during planar motion obtained in step S504, and the composite cycloidal trajectory of the swing phase of the robot's (0) left front leg foot in the body coordinate system {A0} during planar motion obtained in step S505, the complete trajectory of the robot's (0) left front leg foot in the body coordinate system {A0} can be obtained, as follows: Figure 10 As shown;

[0185] S507. Based on the inverse kinematics formula of the robot's left front leg obtained in step S305 and the support phase straight-line trajectory of the robot's left front leg foot in the body coordinate system {A0} during planar motion obtained in step S504, the curves of the change of the three joint angles of the robot's left front leg foot during straight-line motion can be obtained, such as... Figure 11 As shown, joint number one remains unchanged, while the angles of joints number two and three change in a certain symmetrical relationship and increase over time, meaning the foot moves backward along the trajectory.

[0186] S508. Based on the inverse kinematics formula of the robot's left front leg obtained in step S305 and the cycloidal trajectory of the robot's left front leg foot in the body coordinate system {A0} during planar motion obtained in step S505, the curves of the changes in the angles of the three joints of the robot's left front leg foot along the cycloidal trajectory can be obtained, such as... Figure 12 As shown, the first joint increases and then decreases with time, and the derivative of the curve at the beginning and end positions is 0, which meets the requirements of the zero-impact motion trajectory; the angle changes of the second and third joints also show a certain symmetrical relationship and decrease with time, that is, the foot moves forward along the trajectory.

[0187] The above-described specific embodiments are preferred embodiments of the present invention and are not intended to limit the present invention. Any other changes or equivalent substitutions made without departing from the technical solution of the present invention are included within the protection scope of the present invention.

Claims

1. A method for foot trajectory planning based on a parallel quadrupedal pipe-climbing robot, characterized in that, Includes the following steps: S1. Simplify the robot's structure and set its structural parameters; The structural parameters include: the distance between the first and second joints of the forefoot and hindfoot. Distance between the first and second joints of the left and right feet The distance between joint 1 and joint 2 along the orthogonal axis and The distance between joint number two and joint number three The length of the active connecting rod of joints No. 2 and No. 3 Length of passive connecting rods of joints 2 and 3 Foot-end connecting rod length ; S2. Determine the robot's initial pose based on the relative relationship between the robot and the pipeline coordinates; Step S2 includes the following steps: S201. Construct the robot's body coordinate system and the pipe's coordinate system: The pipe's radius is R, with the pipe's center as the origin, and the pipe's central axis is... The axis, vertically upward is Establish the pipeline foundation coordinate system With the robot's center as the origin, The direction of the axis is along the direction of the robot's axial movement. Establish the robot's body coordinate system with the axis perpendicular to the body and pointing upwards. ; S202. Based on the coordinate system established in step S201, confirm the relative pose parameters between the robot and the pipeline: The robot pose is determined using 6 relative pose parameters, which are as follows: , , , , and ;in, For the fuselage coordinate system The origin is in the pipe coordinate system The position coordinates in the diagram represent the position of the robot's center relative to the origin of the pipe coordinate system. , , For the fuselage coordinate system Relative to the pipe coordinate system Around axis, axis, The angle of rotation of the axis; S203. Based on the robot's coordinate system established in step S201 and the relative pose parameters determined in step S202, determine the homogeneous transformation matrix of the robot's body coordinate system relative to the pipe coordinate system. S204. Based on the coordinate system established in step S201 and the relative pose parameters determined in step S202, determine the robot's initial pose: the angle of joint number one is not... The foot-end connecting rod is perpendicular to the inner wall, and the origin of the fuselage coordinate system is... Located at the origin of the pipeline coordinate system Directly below, that is coordinate system deflection The robot's leg extension length is ,in In the initial state The value, For vectors exist The projected length on the axis; then the geometric relationship of the robot's initial pose can be obtained as follows: ， ； S205. Based on the homogeneous transformation matrix of the robot's body coordinate system relative to the pipe coordinate system determined in step S203 and the robot's initial pose given in step S204, determine the homogeneous transformation matrix of the robot's body coordinate system relative to the pipe coordinate system under the robot's initial pose. S3. Construct a single-leg kinematic model of the robot; S4. Based on the single-leg kinematic model, plan the single-leg foot motion trajectory of the robot; Step S4 includes the following specific steps: S401. Based on the homogeneous transformation matrix of coordinates determined in step S203 and the robot's initial pose determined in step S204, determine the position of the robot's left foreleg foot in the body coordinate system. The initial state coordinates are ; S402. Determine that the robot's foot movement consists of a support phase and a swinging phase, which combine to form a complete cycle of foot movement trajectory. Let the complete movement cycle be... The supporting phase period is The oscillation phase period is ,but ; S403, Confirm , and The robot's foot end is located in the body coordinate system. , and The maximum step size of the axis, and has ; S404, Based on the parameters determined in step S403 , and Establish an auxiliary coordinate system on the plane containing the complete motion trajectory. Auxiliary coordinate system The plane in which it is located and The included angle between the planes is ; S405, The left foreleg foot of the robot in the body coordinate system The straight-line trajectory of the support phase in the middle motion is: ； in, For the foot in the auxiliary coordinate system of Position on the axis, relative to gait sampling time Related, that is ,and , ; S406, The left foreleg foot of the robot is in the auxiliary coordinate system The trajectory of the compound cycloid in the oscillating phase of the motion is: ； in, For gait sampling time, Assuming the oscillation phase period is... ; ; S407, The left foreleg foot of the robot in the body coordinate system The trajectory of the compound cycloid in the oscillating phase of the motion is: ； In the formula, For the foot in the auxiliary coordinate system The position in the middle; S5. Based on the planning of the robot's single-leg foot trajectory, assuming the robot moves on a plane, obtain the robot's specific motion trajectory.

2. The foot trajectory planning method based on a parallel quadrupedal pipe-climbing robot according to claim 1, characterized in that: Step S3 includes the following steps: S301. Establish the three-dimensional spatial coordinate system of each joint of the robot's left front leg based on the position of the rotation joints: the body coordinate system with the robot's center as the origin. The coordinate system with the center of the first joint motor of the robot's single leg as the origin. and coordinate system The coordinate system Relative coordinate system In The axis rotation size is The joint angle, which is the rotation angle of the first joint motor, is defined in a coordinate system with the center of the second joint motor of the robot's single leg as the origin. A coordinate system with the center of the end of the parallel structure as the origin. and a coordinate system with the center of the foot as the origin. The coordinate system Relative to coordinate system of Axial downward translation Thus, the spatial coordinate system of the robot's left front leg is constructed; S302. Based on the spatial coordinate system of the left foreleg determined in step S301, obtain the planar coordinate system of the left foreleg. S303, Based on the spatial coordinate system of the left front leg in step S301 and the planar coordinate system of the left front leg in step S302, coordinate system Relative to coordinate system The coordinate homogeneous transformation matrix: ； in, coordinate system Relative to coordinate system The homogeneous transformation matrix, coordinate system Relative to coordinate system The homogeneous transformation matrix, coordinate system Relative to coordinate system The homogeneous transformation matrix, coordinate system Relative to coordinate system The homogeneous transformation matrix, coordinate system Relative to coordinate system The homogeneous transformation matrix, coordinate system Relative to coordinate system The homogeneous transformation matrix; S304. Based on the homogeneous coordinate transformation matrix of step S303, assume that the foot of the left foreleg is in the coordinate system. The coordinates in are We can obtain the position of the foot of the left foreleg in the coordinate system. The relationship between the coordinates and joint rotation angles, i.e., the forward kinematics formula for the robot's left front leg, is as follows: ； ； in, The foot of the left foreleg in the coordinate system coordinates in coordinate system Relative to coordinate system The coordinate homogeneous transformation matrix, For vectors exist Projected length on the axis For vectors exist Projected length on the axis; S305. Based on the robot's left front leg spatial coordinate system established in step S301, the robot's left front leg planar coordinate system established in step S302, and the robot's left front leg forward kinematics solution obtained in step S304, the robot's left front leg inverse kinematics solution can be obtained as follows: ； in, , and These are the rotation angles of joint motors No. 1, No. 2, and No. 3, respectively. Due to the structural limitations of robots, and By constraining the values ​​of , the final inverse kinematics formula is obtained as follows: ； in, ， ， ， ， ， ， ， 。 3. The foot trajectory planning method based on a parallel quadrupedal pipe-climbing robot according to claim 2, characterized in that: Based on the inverse kinematics formula of the robot's left foreleg obtained in step S305, the inverse kinematics formulas of the other three leg structures can be obtained: S306. Based on the robot body coordinate system established in step S201, assume that the four legs represent the robot body coordinate system. The coordinates are: left foreleg Right foreleg Left hind leg (x) A0-3 ,y A0-3 ,z A0-3 ) and right hind leg The angles of the three joints corresponding to each foot are as follows: , and ,in, ; S307. Based on the inverse kinematics formula of the robot's left front leg obtained in step S305 and the assumption in step S306 that the four-legged foot ends are in the body coordinate system. The coordinates of the four legs in the fuselage coordinate system can be obtained. The inverse kinematic solution is: ； in, ， ， ， ， ， ， ， ； Both p and q are constant values, determined according to the robot's leg number.

4. The foot trajectory planning method based on a parallel quadrupedal pipe-climbing robot according to claim 3, characterized in that: Step S5 includes the following steps: S501. Assign values ​​to the robot's structural parameters; S502. Based on the coordinates of the initial state determined in step 401, when the robot is moving in a plane, the size of each joint angle of the robot's single leg in the initial state is set to determine the position of the foot in the body coordinate system. The position in the middle; S503, Based on the parameters determined in step S403 , and Step S501 determines the structural parameters of the robot and the motion trajectory parameters of the robot during planar motion; S504. Based on the coordinate system of the robot's left foreleg foot determined in step S405... From the straight-line trajectory of the support phase during the motion, we can obtain the position of the robot's left foreleg foot in the body coordinate system during planar motion. The specific equation for the straight-line trajectory of the support phase in the middle motion is: ; It can be obtained that when the robot moves in a plane, the tip of the robot's left foreleg is in the body coordinate system. The straight-line trajectory of the support phase during the motion; S505. Based on the left foreleg foot of the robot determined in step S406, in the auxiliary coordinate system... The cycloidal trajectory of the swing phase during the motion and the left foreleg foot end of the robot in the body coordinate system determined by step S407. By analyzing the cycloidal trajectory of the oscillating phase during the robot's motion, we can obtain the position of the robot's left foreleg foot in the body coordinate system during planar motion. The specific equation for the composite cycloidal trajectory of the oscillating phase in the middle motion is: ； It can be obtained that when the robot moves in a plane, the tip of the robot's left foreleg is in the body coordinate system. The oscillating phase of the cycloid trajectory in motion; S506. Based on the information obtained in step S504 regarding the robot's left foreleg foot position in the body coordinate system during planar motion, the robot's coordinate system... The linear trajectory of the support phase during the motion and the result of step S505 show the position of the robot's left foreleg foot in the body coordinate system during planar motion. The cycloidal trajectory of the robot's left foreleg foot in the body coordinate system can be obtained from the oscillating phase of the mid-motion. The complete trajectory of the movement; S507. Based on the inverse kinematics formula of the robot's left front leg obtained in step S305 and the coordinate system of the robot's left front leg foot in the body coordinate system during planar motion obtained in step S504. The straight trajectory of the support phase during the middle motion can be used to obtain the curves of the changes in the angles of the three joints of the robot's left foreleg foot during the straight motion. S508. Based on the inverse kinematics formula of the robot's left front leg obtained in step S305 and the coordinate system of the robot's left front leg foot in the body coordinate system during planar motion obtained in step S505. By analyzing the cycloidal trajectory of the swing phase in the middle motion, we can obtain the curves showing the changes in the angles of the three joints of the robot's left foreleg foot during the cycloidal motion.

Images