CN115256376B - Bezier curve-based serpentine robot variable-diameter climbing gait control method - Google Patents

Bezier curve-based serpentine robot variable-diameter climbing gait control method Download PDF

Info

Publication number
CN115256376B
CN115256376B CN202210812975.9A CN202210812975A CN115256376B CN 115256376 B CN115256376 B CN 115256376B CN 202210812975 A CN202210812975 A CN 202210812975A CN 115256376 B CN115256376 B CN 115256376B
Authority
CN
China
Prior art keywords
curve
snake
representing
robot
joint
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202210812975.9A
Other languages
Chinese (zh)
Other versions
CN115256376A (en
Inventor
郜志英
王文豪
臧勇
刘旭鹏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Science and Technology Beijing USTB
Original Assignee
University of Science and Technology Beijing USTB
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by University of Science and Technology Beijing USTB filed Critical University of Science and Technology Beijing USTB
Priority to CN202210812975.9A priority Critical patent/CN115256376B/en
Publication of CN115256376A publication Critical patent/CN115256376A/en
Application granted granted Critical
Publication of CN115256376B publication Critical patent/CN115256376B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/06—Programme-controlled manipulators characterised by multi-articulated arms
    • B25J9/065—Snake robots

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Feedback Control In General (AREA)
  • Manipulator (AREA)

Abstract

The invention relates to a Bezier curve-based serpentine robot variable-diameter climbing gait control method, which comprises the following steps: determining a ridge curve of variable-diameter climbing to obtain a serpentine robot climbing path; step two: discretizing the back curve, and calculating the joint angle corresponding to the back curve by the snake-shaped robot; step three: and the snakelike robot is controlled to realize variable-diameter climbing motion by combining displacement control and rolling control. The invention uses the Bezier curve with high flexibility for the design of climbing gait of the snake-shaped robot, and can match the transition requirements of pipelines with different sizes by changing the positions of the free control points of the Bezier curve, thereby having good adjustment flexibility and improving the throughput of the snake-shaped robot in a complex pipeline structure.

Description

Bezier curve-based serpentine robot variable-diameter climbing gait control method
Technical Field
The application relates to the technical field of robot control, in particular to a variable-diameter climbing gait control method for a snake-shaped robot based on Bezier curves.
Background
Serpentine robots are highly redundant mobile robots with the potential to work in unstructured complex environments such as pipes. At the same time, the complex structure with high degrees of freedom also presents a great challenge for the motion control of the robot.
Gait control methods commonly used at present for serpentine robots can be divided into two categories. The first is a control function method, wherein the joint angle track is directly generated through a parameterized equation, namely a gait equation, and the serpentine robot can be controlled to realize motion forms such as serpentine, spiral rolling and the like by only adjusting the amplitude, frequency and phase shift parameters of the gait equation. The other method is a back curve method, the back curve of the snake-shaped robot is utilized to fit the space curve shape of the motion gesture required to be realized in the actual task, the environmental adaptability of the snake-shaped robot is greatly improved, but the method needs a proper method to carry out joint discretization on the designed curve, and joint angles required by gait control are generated. In recent years, in order to meet the working demands of complex environments such as pipeline detection, exploration and rescue, a gait design method based on a back curve is widely used for gait design of complex three-dimensional motions of a snake-shaped robot.
At present, aiming at the climbing problem of a diameter abrupt change pipeline structure, the gait design method based on a ridge curve mainly comprises two methods of utilizing a conical spiral line and smoothly connecting a simple curve with constant curvature and flexibility rate to design a transition part of the variable diameter pipeline. The second derivative at the connection point cannot be guaranteed to be continuous between the conical spiral line and the spiral line, so that the joint angle at the connection point is suddenly changed in the climbing process of the snake-shaped robot; the method for smoothly connecting the curves with constant curvature and flexibility ratio to design the transition part has poor shape adjustability of the designed curves, and the curves of all sections need to be smoothly connected, so that the design process is very complicated. In view of the above problems, the bezier curve has very high adjustment flexibility, and can be simply and intuitively designed into a desired target form, so that the second derivative at the connection point can be continuous when the bezier curve is used for connecting curve segments.
Disclosure of Invention
In order to overcome the defects in the prior art, the invention uses the Bezier curve with high flexibility for the design of the climbing gait of the snake-shaped robot, and generates an accurate back curve as the climbing path of the snake-shaped robot by changing the position of the free control point of the Bezier curve; the transition requirements of pipelines with different sizes can be matched, and the flexibility is higher; discretizing the generated ridge curve through curvature integration, and combining displacement control and rolling control, so that the throughput of the snake-shaped robot in the complex pipeline structure is improved.
In order to achieve the above purpose, the invention provides a serpentine robot variable diameter climbing gait control method based on Bezier curves, which comprises the following steps:
step 1: determining a ridge curve of variable-diameter climbing to obtain a serpentine robot climbing path;
the back curve alpha (t) comprises at least six-time Bezier curves and two spiral lines with different spiral radiuses, a plurality of control points of the Bezier curves are determined by constructing a control point constraint equation of the Bezier curves, the distance d between the Bezier curves and a central axis of a pipeline, the curvature k (t) and the flexibility rate tau (t) of the back curve are determined, and the optimal positions of free control points of the Bezier curves are obtained by utilizing an optimization method, so that the positions and the shapes of the back curve alpha (t) are determined; finally obtaining a serpentine robot climbing path;
step 2: discretizing the back curve, and calculating the joint angle corresponding to the back curve by the snake-shaped robot;
step 21: modeling the snake-shaped robot by using a corrugated pipe model, and obtaining the curvature k (t) of the ridge curve in the step 1 to calculate the back curvature k of the ridge curve d And side curvature k l The method comprises the steps of carrying out a first treatment on the surface of the The following is shown:
Figure GDA0004212456180000021
wherein: k (k) d (t) represents the back curvature of the back curve; k (k) l (t) represents the lateral curvature of the ridge curve; k (t) represents the curvature of the ridge curve; phi (t) represents the phase angle between the flener coordinate system and the bellows coordinate system; t represents a parameter of a Bezier curve equation;
step 22: and (3) performing integral operation on the back curvature and the side curvature determined in the step (21), and calculating the joint angle of the pitch joint pitch and the joint angle of the yaw joint yaw corresponding to the back curve alpha (t) of the snake-shaped robot, wherein the specific calculation formula is as follows:
Figure GDA0004212456180000022
wherein: θ pitch A joint angle representing a pitch joint; θ yaw A joint angle representing a yaw joint; α' represents the first derivative of the ridge curve α (t) with respect to t; i represents the joint number of the snake-shaped robot; pitch represents the pitch joint of the snake robot; yaw denotes a yaw joint of the snake-shaped robot; t is t i+1 And t i-1 Parameters of the parameter equations at the i+1 and i-1 joints are represented, respectively;
step 23: discretizing the integral expression, and then accumulating and summing; taking the discrete step length as deltat, and calculating the corresponding arc length deltas, as follows:
Δs＝|α'(t j )|Δt；
wherein: Δs represents the arc length corresponding to a unit discrete step; Δt represents the discrete step size; alpha' (t) j ) Representing the ridge curve alpha (t) at the discrete point t j A first derivative at; j represents the serial number of the current discrete point;
calculating the inner back curvature k of the discretized arc length delta s d And side curvature k l Corresponding angle theta of (2) d And theta l The following is shown:
Figure GDA0004212456180000031
wherein: θ d Representing the corresponding angle of back curvature within arc length deltas; θ l Representing the corresponding angle of curvature inside the arc length deltas;
for theta in the joint length l of the snake-shaped robot d 、θ l The accumulated summation is carried out to obtain
Figure GDA0004212456180000035
And->
Figure GDA0004212456180000036
Then +.>
Figure GDA0004212456180000033
And->
Figure GDA0004212456180000034
Respectively adding to obtain the joint angle theta corresponding to the pitching joint and the yawing joint pitch And theta yaw The discretization process of the designed ridge curve is realized;
step 3: the displacement control and the rolling control are combined, and the snake-shaped robot is controlled to realize variable-diameter climbing movement;
the displacement control comprises enabling the snake-shaped robot to change according to the designed shape of the back curve, and the rolling control comprises enabling the joints of the snake-shaped robot to generate rolling motion;
constructing a calculated relationship of the shift control and the scroll control such that the shift length Δs L The following relation is satisfied with the scroll angle Δψ:
Figure GDA0004212456180000032
wherein: Δψ represents the roll angle per step as the snake robot moves along the ridge curve; ΔS L Representing the shift length of each step; d, d robot The joint diameter of the snake-shaped robot; alpha h Representing the inclination angle of a tangent line at the connection point of the spiral line and the Bezier curve;
the rolling angle delta phi of each step is accumulated, and the result of each accumulation is used as the initial value phi of the phase angle of the next step 0 Thereby maintaining the position of the Bezier curve of the serpentine robot relative to the tube diameter change relatively constant.
Preferably, the serpentine robot in step 1 is a serpentine robot with orthogonal joints.
Preferably, the ridge curve α (t) in the step 1 includes a bezier curve and two spiral lines with different spiral radii respectively located at two ends of the bezier curve, which are smoothly connected, and the following steps of the present invention select the bezier curve to describe the gait control process, specifically:
the expression of the ridge curve alpha (t) is as follows:
Figure GDA0004212456180000041
Figure GDA0004212456180000042
Figure GDA0004212456180000043
wherein: m is M 1 Representing a first segment function; m is M 2 Representing a second segment function; m is M 3 Representing a third segment function; x is x 1 、y 1 And z 1 Respectively representing the abscissa, the ordinate and the vertical coordinate of the first section function; x is x 3 、y 3 And z 3 Respectively representing the abscissa, the ordinate and the vertical coordinate of the third section of function; r is (r) 1 And r 3 Respectively representing the spiral radius of the first segment function and the third segment function; p is p 1 And p 3 Pitch representing the unit arc length of the first segment function and the third segment function, respectively; n represents the number of the control point; p (P) N Representing the nth control point of the Bezier curve; t (T) 1 And T 2 The right end point of the first segment function section and the right end point of the third segment function section are respectively represented.
It is preferable that the control point constraint equation in the step 1 is constructed to determine the sextuple bezier curve P 0 、P 1 、P 2 、P 4 、P 5 And P 6 Six control points are specifically:
Figure GDA0004212456180000044
wherein: m is M 1 (T 1 )、M 1 '(T 1 ) And M 1 ”(T 1 ) Respectively represent the first segment function at T 1 First, second and third derivatives of position; m is M 2 (T 1 )、M 2 '(T 1 ) And M 2 ”(T 1 ) Respectively represent the second segment function at T 1 First, second and third derivatives of position; m is M 2 (T 1 +1)、M 2 '(T 1 +1) and M 2 ”(T 1 +1) respectively represent the second segment function at T 1 First, second and third derivatives of the +1 position; m is M 3 (T 1 +1)、M 3 '(T 1 +1) and M 3 ”(T 1 +1) represents the third stage respectivelyThe function is at T 1 First, second and third derivatives of the +1 position.
It is preferable that the distance d from the sixth bezier curve to the central axis of the pipe, the curvature k (t) of the ridge curve, and the flexibility ratio τ (t) in step 1 are specifically:
the method for acquiring the distance d from the six-order Bezier curve to the central axis of the pipeline is as follows:
Figure GDA0004212456180000045
wherein: d represents the distance from the six-degree Bezier curve to the central axis of the pipeline; c (C) 6x (t) and C 6y (t) represents the x, y direction components of the sextuple bezier curve, respectively;
the method for obtaining the curvature k (t) and the bending rate τ (t) of the ridge line is as follows:
Figure GDA0004212456180000051
wherein: k (t) represents the curvature of the ridge curve; τ (t) represents the flexibility of the ridge line; α', α "and α" represent the first, second and third derivatives of the ridge curve α (t) with respect to t, respectively.
Preferably, the step 1 calculates the free control point P of the six-degree bezier curve by using an optimization method 3 The method specifically comprises the following steps:
when the maximum value of the distance d from the six-time Bezier curve to the central axis of the pipeline meets the set safety distance, the curvature at the maximum value point is minimized, and the objective function and constraint conditions of the optimization method are as follows:
Figure GDA0004212456180000052
wherein: d, d max Representing the maximum value of the distance d from the ridge curve transition part to the central axis of the pipeline; t is t max Parameter representing parameter equation of six-order Bezier curve corresponding to maximum pointA number; m represents the safety distance at the set transition; k represents the curvature at the maximum point; a represents a free control point P 3 Is the abscissa of (2); b represents a free control point P 3 And the ordinate.
Preferably, the method for obtaining the phase angle between the flener coordinate system and the bellows coordinate system in step 21 is as follows:
Figure GDA0004212456180000053
wherein: phi (t) represents the phase angle between the flener coordinate system and the bellows coordinate system; phi (phi) 0 Representing an initial phase angle between the flener coordinate system and the bellows coordinate system; t is t i Parameters representing the parameter equation at the ith joint;
parameter t of the parameter equation at the ith joint i The acquisition method of (a) is as follows:
Figure GDA0004212456180000054
wherein: l denotes the joint length of the snake robot.
Compared with the prior art, the invention has the beneficial effects that:
(1) Compared with the variable-diameter climbing gait design method based on the conical spiral line, the variable-diameter climbing gait design method based on the conical spiral line can ensure that the joint angle of the snake-shaped robot cannot be mutated in the climbing process, and has good stability; compared with a gait design method based on simple curve segment connection, the gait design process is simpler;
(2) The invention can match the transition requirements of pipelines with different sizes by adjusting the positions of the free control points of the Bezier curves, has good adjustment flexibility, and improves the throughput of the snake-shaped robot in a complex pipeline structure.
Drawings
FIG. 1 is a flow chart of a method for controlling the variable diameter climbing gait of a serpentine robot based on a Bezier curve in an embodiment of the invention;
FIG. 2 is a graph of the target ridge designed for variable diameter climbing in example S1 of the present invention;
FIG. 3 is a flowchart of a method for discretizing the ridge curve in embodiment S2 of the present invention;
FIG. 4 is a graph of the discrete results of the target ridge curve of embodiment S2 of the present invention;
FIG. 5 shows a shift length ΔS of embodiment S3 of the present invention L A graph of the relation to the roll angle Δψ;
FIG. 6 is a diagram of a simulation process of variable diameter climbing of a snake-shaped robot in an embodiment of the invention;
FIG. 7 is a graph showing the joint angle of a portion of a joint over time when a snake-shaped robot climbs in a variable diameter manner according to an embodiment of the invention.
Detailed Description
Hereinafter, embodiments of the present invention will be described with reference to the drawings.
As shown in the flowchart of fig. 1, the serpentine robot variable diameter climbing gait control method based on the bezier curve specifically comprises the following steps:
s1: designing a variable-diameter climbing back curve and planning a serpentine robot climbing path;
the back curve alpha (t) is formed by smoothly connecting two ends of a Bezier curve with two spiral lines with different spiral radii respectively; in this embodiment, taking the six-degree bezier curve as an example, the expression of the ridge curve α (t) is a three-segment piecewise function, as follows:
Figure GDA0004212456180000061
M 2 ＝C 6 (t-T 1 )；t∈[T 1 ,T 1 +1]；
Figure GDA0004212456180000062
wherein: m is M 1 Representing a first segment function; m is M 2 Representing a second segment function; m is M 3 Representing a third segment function; x is x 1 、y 1 And z 1 Respectively representing the abscissa, the ordinate and the vertical coordinate of the first section function; x is x 3 、y 3 And z 3 Respectively representing the abscissa, the ordinate and the vertical coordinate of the third section of function; r is (r) 1 And r 3 Respectively representing the spiral radius of the first segment function and the third segment function; p is p 1 And p 3 Pitch representing the unit arc length of the first segment function and the third segment function, respectively; t (T) 1 And T 2 Respectively representing the right end point of the first section of function section and the right end point of the third section of function section;
the six-degree Bezier curve acquisition method is shown as follows:
Figure GDA0004212456180000071
wherein: c (C) 6 (t) represents a sextuple bezier curve parameter equation; p (P) N Representing the Nth control point of the sextuple Bezier curve; n represents a bezier curve control point number; t represents a parameter of a parameter equation;
in order to avoid abrupt change of joint angle when the snake-shaped robot runs at the connecting point, the second derivative at the connecting point is ensured to be continuous, and a control point constraint equation is constructed to determine a sextuple Bezier curve P 0 、P 1 、P 2 、P 4 、P 5 And P 6 Six control points are as follows:
Figure GDA0004212456180000072
wherein: m is M 1 (T 1 )、M 1 '(T 1 ) And M 1 ”(T 1 ) Respectively represent the first segment function at T 1 First order of locationSecond and third derivatives; m is M 2 (T 1 )、M 2 '(T 1 ) And M 2 ”(T 1 ) Respectively represent the second segment function at T 1 First, second and third derivatives of position; m is M 2 (T 1 +1)、M 2 '(T 1 +1) and M 2 ”(T 1 +1) respectively represent the second segment function at T 1 First, second and third derivatives of the +1 position; m is M 3 (T 1 +1)、M 3 '(T 1 +1) and M 3 ”(T 1 +1) respectively represents the third segment function at T 1 First, second and third derivatives of the +1 position.
The method for further determining the distance d from the sextuple Bezier curve to the central axis of the pipeline is as follows:
Figure GDA0004212456180000073
wherein: d represents the distance from the six-degree Bezier curve to the central axis of the pipeline; c (C) 6x (t) and C 6y (t) represents the x, y direction components of the sextuple bezier curve, respectively;
the method for obtaining the curvature k (t) and the bending rate τ (t) of the ridge line is as follows:
Figure GDA0004212456180000074
wherein: k (t) represents the curvature of the ridge curve; τ (t) represents the flexibility of the ridge line; α', α "and α" represent the first, second and third derivatives of the ridge curve α (t) with respect to t, respectively.
Then, calculating a free control point P of the sextupe Bezier curve by using an optimization method 3 To determine the position and shape of the ridge curve α (t); the maximum value of the distance d from the six-time Bezier curve to the central axis of the pipeline is enabled to meet the set safety distance, and meanwhile, the curvature at the maximum value point is guaranteed to be minimum, and the objective function and constraint conditions of the optimization method are as follows:
Figure GDA0004212456180000081
wherein: d, d max Representing the maximum value of the distance d from the ridge curve transition part to the central axis of the pipeline; t is t max Representing the parameter of the six-order Bezier curve parameter equation corresponding to the maximum point; m represents the safety distance at the set transition; k represents the curvature at the maximum point; a represents a free control point P 3 Is the abscissa of (2); b represents a free control point P 3 And the ordinate.
In the present embodiment, an initial pipe diameter D is set 1 =160 mm final tube diameter D 2 200mm, diameter d of serpentine robot joint robot Calculation of initial spiral radius r =40 mm 1 =100 mm, final spiral radius r 2 =120 mm, calculating the optimal free control point of the sextuple Bezier curve as P using the optimization method described above 3 (-0.28, -0.07,0.52) the target ridge curve designed for variable diameter climbing is shown in FIG. 2, which is the target ridge curve designed for variable diameter climbing in example S1 of the present invention.
Finally, a snake-shaped robot climbing path is obtained.
S2: discretizing a back curve by adopting a curvature integration method, and calculating the joint angle corresponding to the snake-shaped robot;
s21: modeling the snake-shaped robot by using a corrugated pipe model, obtaining the curvature k (t) of the ridge curve in S1, and calculating the back curvature k of the ridge curve d And side curvature k l The method comprises the steps of carrying out a first treatment on the surface of the The following is shown:
Figure GDA0004212456180000082
wherein: k (k) d 9 t) represents the back curvature of the back curve; k (k) l (t) represents the lateral curvature of the ridge curve; k (t) represents the curvature of the ridge curve; phi (t) represents the phase angle between the flener coordinate system and the bellows coordinate system;
the phase angle acquisition method between the flena coordinate system and the corrugated pipe coordinate system is as follows:
Figure GDA0004212456180000083
wherein: phi (t) represents the phase angle between the flener coordinate system and the bellows coordinate system; phi (phi) 0 Representing an initial phase angle between the flener coordinate system and the bellows coordinate system; t is t i Parameters representing the parameter equation at the ith joint;
parameter t of the parameter equation at the ith joint i The acquisition method of (a) is as follows:
Figure GDA0004212456180000084
wherein: l represents the joint length of the snake-shaped robot; i represents the joint number of the snake-shaped robot;
s22: the back curvature and the side curvature determined in S21 are integrated, and the angles of the pitch joint pitch and the yaw joint yaw of the snake-shaped robot corresponding to the back curve α (t) are calculated, and the specific calculation formula is as follows:
Figure GDA0004212456180000091
wherein: θ pitch A joint angle representing a pitch joint; θ yaw A joint angle representing a yaw joint; α' represents the first derivative of the ridge curve α (t) with respect to t; t is t i+1 And t i-1 Parameters of the parameter equations at the i+1 and i-1 joints are represented, respectively; pitch represents the pitch joint of the snake robot; yaw denotes a yaw joint of the snake-shaped robot;
s23: discretizing the integral expression, and then accumulating and summing; taking the discrete step length as deltat, and calculating the corresponding arc length deltas, as follows:
Δs＝|α'(t j )|Δt；
wherein: Δs represents the discretized arc length; Δt represents the discrete step size;α'(t j ) Representing the ridge curve alpha (t) at the discrete point t j A first derivative at; j represents the number of the current discrete point;
calculating the inner back curvature k of the discretized arc length delta s d And side curvature k l Corresponding angle theta d And theta l The following is shown:
Figure GDA0004212456180000092
wherein: θ d Representing the corresponding angle of back curvature within arc length deltas; θ l Representing the corresponding angle of curvature inside the arc length deltas;
for theta in the joint length l of the snake-shaped robot d 、θ l The accumulated summation is carried out to obtain
Figure GDA0004212456180000093
And->
Figure GDA0004212456180000094
Then +.>
Figure GDA0004212456180000095
And->
Figure GDA0004212456180000096
Respectively adding to obtain the joint angle theta corresponding to the pitching joint and the yawing joint pitch And theta yaw The discretization process of the designed ridge curve is realized;
in this embodiment, a specific discretization flowchart is shown in fig. 3 as a flowchart of a method for discretizing a ridge curve in embodiment S2 of the present invention, taking a discrete step size Δt=0.001, a joint length l=0.0725 m, and a discrete result of a target ridge curve in fig. 2 is shown in fig. 4 as a graph of a discrete result of a target ridge curve in embodiment S2 of the present invention.
S3: the displacement control and the rolling control are combined, and the snake-shaped robot is controlled to realize variable-diameter climbing movement;
the displacement control is to make the snake-shaped robot change continuously according to the designed shape of the back curve; the rolling control is to control the snakelike robot joint to generate rolling motion so as to achieve the purpose of advancing;
constructing the calculated relation between the shift control and the rolling control needs to ensure the shift length delta S of each step L The following relation should be satisfied with the scroll angle Δψ:
Figure GDA0004212456180000101
wherein: Δψ represents the roll angle for each step; ΔS L Representing the shift length of each step; d, d robot The joint diameter of the snake-shaped robot; alpha h Representing a spiral M 1 Inclination angle of tangent line at the junction point with the six-degree Bezier curve.
The rolling angle delta phi of each step is accumulated, and the result of each accumulation is used as the initial value phi of the phase angle of the next step 0 The position of the transition part relative to the position of the pipe diameter change can be kept relatively constant in the climbing process.
In this embodiment, as shown in FIG. 5, the shift length ΔS of embodiment S3 of the present invention L Graph of roll angle Δψ, where R represents the snake robot at M 1 And M is as follows 2 Is connected to the point P of connection 0 The rolling control direction is positioned, M represents the P position of the snake-shaped robot 0 A shift control direction; under the combined action of rolling control and displacement control, the snake-shaped robot is under P 0 The moving speed in the horizontal direction is generated, so that the winding process from the unwinding of the initial pipeline to the final pipeline is completed in the horizontal direction in the variable-diameter climbing process of the snake-shaped robot, and the collision of the snake-shaped robot and the pipeline in the climbing process is avoided.
The three-dimensional simulation software Webots is utilized to verify the gait control method, and a diagram of a variable-diameter climbing simulation process of the snake-shaped robot in the embodiment of the invention is shown in fig. 6. According to the figure, the snake-shaped robot advances along the designed climbing path, and the correctness of the gait control method is verified through the pipeline structure with the abrupt pipe diameter change safely and stably.
Fig. 7 is a graph showing the change of the joint angle of a part of joints with time when the snake-shaped robot climbs in a variable diameter mode, and the graph shows that the joint angle of the snake-shaped robot does not mutate in the climbing process, so that the gait control method can ensure that the joint angle of the snake-shaped robot continuously changes in the climbing process.
In conclusion, the result of the variable-diameter climbing gait control method of the serpentine robot based on the Bezier curve proves that the method has a good application effect.
(1) According to the method for controlling the variable-diameter climbing gait of the snake-shaped robot based on the Bezier curve, provided by the invention, the spiral radius of the front and rear sections of spiral lines of the back curve is determined according to the actual pipeline size, the front and rear sections of spiral lines are smoothly connected by utilizing the six-time Bezier curve to plan the climbing path of the snake-shaped robot, the back curve is reasonable in design process, the joint angle of the snake-shaped robot can be ensured to continuously change in the climbing process, and abrupt change can not occur;
(2) The invention uses curvature integral to discretize the generated ridge curve based on general parameters, and simplifies the calculation process of solving the joint angle of the snake-shaped robot through the discrete ridge curve.
(3) The invention combines displacement control and rolling control, realizes the variable-diameter climbing motion control of the snake-shaped robot, verifies the correctness of the gait control method through simulation, improves the passing capacity of the snake-shaped robot in a pipeline, and has obvious advantages compared with the existing climbing gait design method.
The above examples are only illustrative of the preferred embodiments of the present invention and are not intended to limit the scope of the present invention, and various modifications and improvements made by those skilled in the art to the technical solution of the present invention should fall within the scope of protection defined by the claims of the present invention without departing from the spirit of the present invention.

Claims (7)

1. A Bezier curve-based serpentine robot variable-diameter climbing gait control method is characterized by comprising the following steps of:
step 1: determining a ridge curve of variable-diameter climbing to obtain a serpentine robot climbing path;
the back curve alpha (t) comprises at least six-time Bezier curves and two spiral lines with different spiral radiuses, a plurality of control points of the Bezier curves are determined by constructing a control point constraint equation of the Bezier curves, the distance d between the Bezier curves and a central axis of a pipeline, the curvature k (t) and the flexibility rate tau (t) of the back curve are determined, and the optimal positions of free control points of the Bezier curves are obtained by utilizing an optimization method, so that the positions and the shapes of the back curve alpha (t) are determined; finally obtaining a serpentine robot climbing path;
step 2: discretizing the back curve, and calculating the joint angle corresponding to the back curve by the snake-shaped robot;
step 21: modeling the snake-shaped robot by using a corrugated pipe model, and obtaining the curvature k (t) of the ridge curve in the step 1 to calculate the back curvature k of the ridge curve d And side curvature k l The method comprises the steps of carrying out a first treatment on the surface of the The following is shown:
Figure FDA0004212456170000011
wherein: k (k) d (t) represents the back curvature of the back curve; k (k) l (t) represents the lateral curvature of the ridge curve; k (t) represents the curvature of the ridge curve; phi (t) represents the phase angle between the flener coordinate system and the bellows coordinate system; t represents a parameter of a Bezier curve equation;
step 22: and (3) performing integral operation on the back curvature and the side curvature determined in the step (21), and calculating the joint angle of the pitch joint pitch and the joint angle of the yaw joint yaw corresponding to the back curve alpha (t) of the snake-shaped robot, wherein the specific calculation formula is as follows:
Figure FDA0004212456170000012
wherein: θ pitch Representing pitch jointsJoint angle; θ yaw A joint angle representing a yaw joint; α' represents the first derivative of the ridge curve α (t) with respect to t; i represents the joint number of the snake-shaped robot; pitch represents the pitch joint of the snake robot; yaw denotes a yaw joint of the snake-shaped robot; t is t i+1 And t i-1 Parameters of the parameter equations at the i+1 and i-1 joints are represented, respectively;
step 23: discretizing the integral expression, and then accumulating and summing; taking the discrete step length as deltat, and calculating the corresponding arc length deltas, as follows:
Δs＝|α'(t j )|Δt；
wherein: Δs represents the arc length corresponding to a unit discrete step; Δt represents the discrete step size; alpha' (t) j ) Representing the ridge curve alpha (t) at the discrete point t j A first derivative at; j represents the serial number of the current discrete point;
calculating the inner back curvature k of the discretized arc length delta s d And side curvature k l Corresponding angle theta of (2) d And theta l The following is shown:
Figure FDA0004212456170000021
wherein: θ d Representing the corresponding angle of back curvature within arc length deltas; θ l Representing the corresponding angle of curvature inside the arc length deltas;
for theta in the joint length l of the snake-shaped robot d 、θ l The accumulated summation is carried out to obtain
Figure FDA0004212456170000022
And->
Figure FDA0004212456170000023
Then +.>
Figure FDA0004212456170000024
And->
Figure FDA0004212456170000025
Respectively adding to obtain the joint angles theta corresponding to the pitching joint and the yawing joint pitch And theta yaw The discretization process of the designed ridge curve is realized;
step 3: the displacement control and the rolling control are combined, and the snake-shaped robot is controlled to realize variable-diameter climbing movement;
the displacement control comprises enabling the snake-shaped robot to change according to the designed shape of the back curve, and the rolling control comprises enabling the joints of the snake-shaped robot to generate rolling motion;
constructing a calculated relationship of the shift control and the scroll control such that the shift length Δs L The following relation is satisfied with the scroll angle Δψ:
Figure FDA0004212456170000026
wherein: Δψ represents the roll angle per step as the snake robot moves along the ridge curve; ΔS L Representing the shift length of each step; d, d robot The joint diameter of the snake-shaped robot; alpha h Representing the inclination angle of a tangent line at the connection point of the spiral line and the Bezier curve;
the rolling angle delta phi of each step is accumulated, and the result of each accumulation is used as the initial value phi of the phase angle of the next step 0 Thereby maintaining the position of the Bezier curve of the serpentine robot relative to the tube diameter change relatively constant.
2. The method for controlling the variable diameter climbing gait of the snake-shaped robot based on the Bezier curve according to claim 1, wherein the snake-shaped robot in the step 1 is a snake-shaped robot with orthogonal joints.
3. The method for controlling the gait of the variable diameter climbing of the serpentine robot based on the bezier curve according to claim 1, wherein the back curve α (t) in the step 1 comprises a six-time bezier curve and two spiral lines with different spiral radii respectively positioned at two ends of the bezier curve, and specifically comprises the following steps:
the expression of the ridge curve alpha (t) is as follows:
Figure FDA0004212456170000031
Figure FDA0004212456170000032
Figure FDA0004212456170000033
wherein: m is M 1 Representing a first segment function; m is M 2 Representing a second segment function; m is M 3 Representing a third segment function; x is x 1 、y 1 And z 1 Respectively representing the abscissa, the ordinate and the vertical coordinate of the first section function; x is x 3 、y 3 And z 3 Respectively representing the abscissa, the ordinate and the vertical coordinate of the third section of function; r is (r) 1 And r 3 Respectively representing the spiral radius of the first segment function and the third segment function; p is p 1 And p 3 Pitch representing the unit arc length of the first segment function and the third segment function, respectively; n represents the number of the control point; p (P) N Representing the nth control point of the Bezier curve; t (T) 1 And T 2 The right end point of the first segment function section and the right end point of the third segment function section are respectively represented.
4. The method as claimed in claim 3, wherein the six-degree Bezier curve P is determined by constructing a control point constraint equation in step 1 0 、P 1 、P 2 、P 4 、P 5 And P 6 Six control points are specifically:
Figure FDA0004212456170000034
wherein: m is M 1 (T 1 )、M 1 '(T 1 ) And M 1 ”(T 1 ) Respectively represent the first segment function at T 1 First, second and third derivatives of position; m is M 2 (T 1 )、M 2 '(T 1 ) And M 2 ”(T 1 ) Respectively represent the second segment function at T 1 First, second and third derivatives of position; m is M 2 (T 1 +1)、M 2 '(T 1 +1) and M 2 ”(T 1 +1) respectively represent the second segment function at T 1 First, second and third derivatives of the +1 position; m is M 3 (T 1 +1)、M 3 '(T 1 +1) and M 3 ”(T 1 +1) respectively represents the third segment function at T 1 First, second and third derivatives of the +1 position.
5. The method for controlling the gait of the variable diameter climbing of the serpentine robot based on the Bezier curve according to claim 3, wherein the six times the Bezier curve-to-pipe center axis distance d, the curvature k (t) of the ridge curve and the flexibility ratio τ (t) in the step 1 are specifically:
the method for acquiring the distance d from the six-order Bezier curve to the central axis of the pipeline is as follows:
Figure FDA0004212456170000035
wherein: d represents the distance from the six-degree Bezier curve to the central axis of the pipeline; c (C) 6x (t) and
Figure FDA0004212456170000041
respectively representing x-direction components and y-direction components of the sextuple Bezier curve;
the method for obtaining the curvature k (t) and the bending rate τ (t) of the ridge line is as follows:
Figure FDA0004212456170000042
wherein: k (t) represents the curvature of the ridge curve; τ (t) represents the flexibility of the ridge line; α ', α "and α'" represent the first, second and third derivatives of the ridge curve α (t) with respect to t, respectively.
6. The method for controlling the gait of a variable diameter climbing of a Bezier-curve-based snake-shaped robot according to claim 3, wherein the step 1 calculates the free control point P of the six-order Bezier curve by using an optimization method 3 The method specifically comprises the following steps:
when the maximum value of the distance d from the six-time Bezier curve to the central axis of the pipeline meets the set safety distance, the curvature at the maximum value point is minimized, and the objective function and constraint conditions of the optimization method are as follows:
Figure FDA0004212456170000043
wherein: d, d max Representing the maximum value of the distance d from the ridge curve transition part to the central axis of the pipeline; t is t max Representing the parameter of the six-order Bezier curve parameter equation corresponding to the maximum point; m represents the safety distance at the set transition; k represents the curvature at the maximum point; a represents a free control point P 3 Is the abscissa of (2); b represents a free control point P 3 And the ordinate.
7. The method for controlling the variable diameter climbing gait of the serpentine robot based on the bezier curve according to claim 3, wherein the method for obtaining the phase angle between the flener coordinate system and the bellows coordinate system in the step 21 is as follows:
Figure FDA0004212456170000044
wherein: phi (t) represents the phase angle between the flener coordinate system and the bellows coordinate system; phi (phi) 0 Representing an initial phase angle between the flener coordinate system and the bellows coordinate system; t is t i Parameters representing the parameter equation at the ith joint;
parameter t of the parameter equation at the ith joint i The acquisition method of (a) is as follows:
Figure FDA0004212456170000045
wherein: l denotes the joint length of the snake robot.
CN202210812975.9A 2022-07-11 2022-07-11 Bezier curve-based serpentine robot variable-diameter climbing gait control method Active CN115256376B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210812975.9A CN115256376B (en) 2022-07-11 2022-07-11 Bezier curve-based serpentine robot variable-diameter climbing gait control method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210812975.9A CN115256376B (en) 2022-07-11 2022-07-11 Bezier curve-based serpentine robot variable-diameter climbing gait control method

Publications (2)

Publication Number Publication Date
CN115256376A CN115256376A (en) 2022-11-01
CN115256376B true CN115256376B (en) 2023-06-16

Family

ID=83764692

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210812975.9A Active CN115256376B (en) 2022-07-11 2022-07-11 Bezier curve-based serpentine robot variable-diameter climbing gait control method

Country Status (1)

Country Link
CN (1) CN115256376B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116476078B (en) * 2023-06-19 2023-09-05 安徽大学 Laser radar-based stable target tracking method for snake-shaped robot

Family Cites Families (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2009009673A2 (en) * 2007-07-10 2009-01-15 Raytheon Sarcos, Llc Modular robotic crawler
CN105773594B (en) * 2016-03-11 2018-01-23 清华大学 Multi-modal combination of rigidity and flexibility snake-shaped robot device
JP6733046B2 (en) * 2016-08-22 2020-07-29 キヤノン株式会社 Continuum robot, kinematic model correction method for continuum robot, and continuum robot control method
CN106903680B (en) * 2017-03-01 2019-06-25 华南理工大学 Snake-shaped robot based on elastic envelope climbs safely control method
CN108568817B (en) * 2018-03-13 2021-05-11 同济大学 Delta robot track connection control method based on Bezier curve
CN108994836A (en) * 2018-08-20 2018-12-14 上海交通大学 A kind of snake-shaped robot path follows planing method
JP7441047B2 (en) * 2020-01-10 2024-02-29 三菱重工業株式会社 Route generation device, control device, inspection system, route generation method and program
CN111347401A (en) * 2020-03-29 2020-06-30 南京电博机器人技术有限公司 Snakelike line climbing robot and line patrol method
GB2598926B (en) * 2020-09-18 2022-09-21 Eelume As Underwater snake robot with extreme length
CN112549010B (en) * 2020-12-22 2022-11-08 南昌大学 Design method of multi-joint snake-shaped robot self-adaptive trajectory tracking controller based on improved Serpenoid curve
CN112666947B (en) * 2020-12-22 2022-12-06 南昌大学 Design method of snake-shaped robot self-adaptive trajectory tracking controller based on improved LOS method
CN112731802B (en) * 2020-12-25 2022-03-29 华南理工大学 Self-adaptive climbing control method, system, device and medium for snake-shaped robot
CN112743544B (en) * 2020-12-25 2022-10-25 华南理工大学 Target positioning method, system and medium based on serpentine gait of snake-shaped robot
CN113733095A (en) * 2021-09-18 2021-12-03 南开大学 Three-dimensional motion gait generation method for wheel-free snake-shaped robot

Also Published As

Publication number Publication date
CN115256376A (en) 2022-11-01

Similar Documents

Publication Publication Date Title
CN105785921B (en) A kind of speed planning method during industrial robot nurbs curve interpolation
CN107390691B (en) AGV path tracking method
CN106393106B (en) The robot nurbs curve of parameter adaptive densification moves interpolating method
CN109571466B (en) Seven-degree-of-freedom redundant mechanical arm dynamic obstacle avoidance path planning method based on rapid random search tree
CN111806467B (en) Variable speed dynamic track changing planning method based on vehicle driving rule
CN115256376B (en) Bezier curve-based serpentine robot variable-diameter climbing gait control method
CN101612734B (en) Pipeline spraying robot and operation track planning method thereof
CN109782779B (en) AUV path planning method in ocean current environment based on population hyperheuristic algorithm
JP4929506B2 (en) Legged mobile robot
KR101196374B1 (en) Path planning system for mobile robot
WO2021165745A1 (en) Collision avoidance motion planning method for industrial robot
CN110398969A (en) Automatic driving vehicle adaptive prediction time domain rotating direction control method and device
JP4807583B2 (en) Projective transformation convergence calculation processing method
CN105183009B (en) A kind of redundant mechanical arm method for controlling trajectory
CN113146610B (en) Mechanical arm tail end track tracking algorithm based on zero space obstacle avoidance
CN110653137B (en) Spraying method for keeping spray head vertical to spraying surface
CN111290272B (en) Attitude stationarity adjusting method based on multi-legged robot
CN106346129A (en) Robot welding motion control method based on laser seam tracking sensor
CN106844951B (en) Method and system for solving inverse kinematics of super-redundant robot based on segmented geometric method
CN103692440A (en) Spatial path tracking method of continuous robot
CN112882485B (en) Geometric calculation method for inverse kinematics of biped robot
CN112975992A (en) Error-controllable robot track synchronous optimization method
CN114779785A (en) Mobile robot smooth track planning method based on PSO parameter setting
Yamada et al. Steering of pedal wave of a snake-like robot by superposition of curvatures
CN110125942B (en) Plane trajectory tracking method for mobile snakelike mechanical arm

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant