CN116330302B - A Motion Planning Method Based on Framed Space Curve - Google Patents

Abstract

The present application relates toThe technical field of super-redundancy robot motion planning solves the technical problems that the existing super-redundancy robot has large space motion gait calculated amount and further causes non-real-time motion planning of the super-redundancy robot, and particularly relates to a motion planning method based on a standard frame space curve, which comprises the following steps: s1, constructing a space parameter curve C (t) as a ridge curve of the super-redundancy robot, and calculating a unit tangent vector at the initial position of the ridge curveNormal vector of unitUnit auxiliary normal vectorCoordinate system forming initial part of space parameter curve. The application carries out discretization by calculating the minimum rotation standard frame, avoids the calculation of curvature flexibility rate, has no requirement on the shape and the type of the back curve, and can improve the calculation efficiency by avoiding a large amount of curvature flexibility rate and integral calculation for discretization of the back curve of the super-redundancy robot.

Description

A Motion Planning Method Based on Framed Space Curve

technical field

The invention relates to the technical field of ultra-redundant robot motion planning, in particular to a motion planning method based on framed space curves.

Background technique

Ultra-redundant robots are widely used in narrow space detection and other fields due to their high motion redundancy. At present, most super-redundant robots use the ridge curve discretization method for motion planning. The current curve-based discretization method uses the integral method of curvature and torsion to calculate the joint angle. This method needs to calculate the curvature and torsion of the spine curve equation first. For complex curves, the amount of calculation is very large, which will greatly reduce the discretization efficiency.

Contents of the invention

Aiming at the deficiencies of the prior art, the present invention provides a motion planning method based on framed space curves, which solves the problem that the existing super-redundant robot space motion planning has a large amount of calculation, which leads to the unreal-time motion planning of the super-redundant robot. technical problem.

In order to solve the above-mentioned technical problems, the present invention provides the following technical solutions: a motion planning method based on framed space curves, the method comprising the following steps:

S1. Construct the space parameter curve C(t) as the ridge curve of the super-redundant robot, and calculate the unit tangent vector at the beginning of the ridge curve , unit normal vector /> and the unit vicenormal vector/> The coordinate system constituting the minimum rotating frame at the beginning of the spatial parameter curve /> ;

S2. According to the coordinate system at the initial point of the space parameter curve Before calculation /> connecting rod length/> the unit tangent vector at the corresponding arc length /> , unit normal vector /> and the unit vicenormal vector/> Constitute the smallest rotating frame/> ;

S3, based on the first minimum rotating frame/> and No. /> minimum rotating frame/> Generating motion plans for hyper-redundant robots.

Further, in step S1, the specific process includes the following steps:

S11, discretize the curve parameter t of the spatial parameter curve C(t), wherein the interval is , /> , then the value of the curve parameter t is /> ;

S12, according to the curve parameter t and interval Calculate the arc length of the ridge curve /> ;

S13, make the arc length Equal to hyper-redundant robots before /> connecting rod length/> , and computed with hyper-redundant robot front /> connecting rod length/> Corresponding Curve Parameters/> ;

S14. Calculating curve parameters tangent vector at ;

S15. Calculate the starting point of the ridge curve The unit tangent vector at , unit normal vector /> and the unit vicenormal vector/> .

Further, in step S12, the arc length The calculation formula is:

;

In the above formula, n refers to the number of discrete summation intervals of the arc length of the curve segment, and m represents the serial number of the discrete curve segment, represents the interval, Indicates that the space parameter curve C(t) in /> derivative with respect to t, /> Points to the modulo operation of the quantity.

Further, in step S13, it specifically includes:

by interval cyclically accumulates the arc lengths of discrete curve segments for the integration interval up to arc length /> equal to before /> connecting rod length/> , and then determine the curve parameters that meet the conditions /> .

Further, in step S14, the curve parameters tangent vector at The calculation formula is:

;

In the above formula, is the space parameter curve C(t) versus the curve parameter /> 1st order derivative of .

Further, in step S2, the specific process includes the following steps:

S21. Set the arc length from the starting point on the known curve point /> The minimum rotating frame at is /> and the arc length on the curve /> point /> The unit tangent vector at ;

S22. Calculating the minimum rotating frame The unit tangent vector of /> Based on plane /> is the symmetric vector of the mirror image /> , where the plane /> for point /> and dot /> bisecting surface of

S23, the minimum rotating frame The unit binormal vector in /> Based on plane /> Mirror as a symmetric vector /> , and then the symmetric vector /> Based on plane /> Mirror symmetry and calculate the unit binormal vector /> , where the plane for point /> Symmetric vector at and the unit tangent vector /> bisecting surface of

S24, using the right-hand spiral rule based on the unit tangent vector and the unit binormal vector/> Calculate the unit normal vector /> ;

S25. According to the unit tangent vector , unit vice-normal vector/> and the unit normal vector /> Constitute the smallest rotating frame/> .

Further, in step S22, the symmetric vector The calculation formula is:

;

In the above formula, point to /> as the starting point /> is the position vector of the end point, /> is the unit tangent vector /> point on the curve /> about plane/> Symmetric vector of .

Further, in step S23, the unit secondary normal vector The calculation formula is:

;

In the above formula, for /> About Plane/> mirrored symmetric vector, /> is the unit tangent vector /> and symmetric vectors /> vector difference, /> To form the minimum rotating frame /> The unit binormal of the pose matrix.

Further, in step S3, the specific process includes the following steps:

S31. Calculate the first minimum rotating frame/> and No. /> minimum rotating frame/> Attitude difference /> ;

S32. Based on attitude difference Calculation of joint angles for orthogonal joints of hyper-redundant robots /> ;

S33. According to the joint angle Generating motion plans for hyper-redundant robots.

With the above technical solution, the present invention provides a motion planning method based on framed space curves, which at least has the following beneficial effects:

The present invention performs discretization by calculating the minimum rotating frame, avoids the calculation of curvature torsion, has no requirements on the shape and type of the ridge curve, and avoids a large amount of curvature torsion and integral calculation for the discretization of the ridge curve of the super-redundant robot. Computational efficiency can be improved.

Description of drawings

The drawings described here are used to provide a further understanding of the application and constitute a part of the application. The schematic embodiments and descriptions of the application are used to explain the application and do not constitute an improper limitation to the application. In the attached picture:

Fig. 1 is the schematic diagram of discretization based on minimum rotating frame of ridge curve of the present invention;

Fig. 2 is the schematic diagram that the present invention calculates the minimum rotating frame based on the double reflection method of spatial geometry;

Fig. 3 is a schematic diagram of generating a motion plan of a hyper-redundant robot according to the present invention.

In the figure: 1. Ultra-redundant robot connecting rod; 2. Minimum rotating frame; 3. Spine curve.

Detailed ways

In order to make the above objects, features and advantages of the present invention more obvious and understandable, the present invention will be described in further detail below in conjunction with the accompanying drawings and specific embodiments. In this way, the realization process of how the application applies technical means to solve technical problems and achieve technical effects can be fully understood and implemented accordingly.

Those of ordinary skill in the art can understand that all or part of the steps in the method of the embodiment can be completed by instructing the relevant hardware through a program. Therefore, the present application can adopt a complete hardware embodiment, a complete software embodiment, or a combination of software and hardware Forms of Embodiments of Aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) having computer-usable program code embodied therein.

Please refer to Figures 1-3, which show a specific implementation of this embodiment. In this embodiment, discretization is performed by calculating the minimum rotating frame, which avoids the calculation of curvature and torsion, and has no effect on the shape and type of the ridge curve. Requirements, for the discretization of the ridge curve of the super-redundant robot, a large number of curvature torsion and integral calculations can be avoided, and the calculation efficiency can be improved.

Please refer to Fig. 1, the present embodiment proposes a kind of motion planning method based on framed space curve, and this method comprises the following steps:

S1. Construct the space parameter curve C(t) as the ridge curve of the super-redundant robot, and calculate the unit tangent vector at the beginning of the ridge curve , unit normal vector /> and the unit vicenormal vector/> The coordinate system that forms the initial point of the spatial parameter curve /> ;

;

In this embodiment, the specific process of step S1 includes the following steps:

S11, discretize the curve parameter t of the spatial parameter curve C(t), wherein the interval is , /> , then the value of the curve parameter t is /> ;

S12, according to the curve parameter t and interval Calculate the arc length of the ridge curve /> ;

arc length The calculation formula is:

;

In the above formula, n refers to the number of discrete summation intervals of the arc length of the curve segment, and m represents the serial number of the discrete curve segment, represents the interval, Indicates that the space parameter curve C(t) in /> derivative with respect to t, /> Points to the modulo operation of the quantity.

S13, make the arc length Equal to hyper-redundant robots before /> connecting rod length/> , and computed with hyper-redundant robot front /> connecting rod length/> Corresponding Curve Parameters/> ;

by interval cyclically accumulates the arc lengths of discrete curve segments for the integration interval up to arc length /> equal to before /> connecting rod length/> , and then determine the curve parameters that meet the conditions /> ;

;

In the above formula, is the first derivative of the space parameter curve C(t) to the curve parameter t, /> Indicates the number of connecting rods whose lengths are accumulated and summed, /> Indicates the length of the i-th connecting rod, /> Indicates the derivative derivative symbol.

S14. Calculating curve parameters tangent vector at ;

curve parameters tangent vector at The calculation formula is:

;

In the above formula, is the space parameter curve C(t) versus the curve parameter /> 1st order derivative of .

S15. Calculate the starting point of the ridge curve The unit tangent vector at , unit normal vector /> and the unit vicenormal vector/> , thus constituting the coordinate system at the initial point of the space parameter curve /> .

unit tangent vector The calculation formula is:

;

In the above formula, for /> The first derivative of the spatial parameter curve C(t) at the curve parameter t.

unit subnormal vector The calculation formula is:

;

In the above formula, for /> The second derivative of the space parameter curve C(t) at the curve parameter t, /> Represents a vector cross product operation.

unit normal vector The calculation formula is:

;

In the above formula, is the unit vicenormal vector at the beginning of the ridge curve, /> is the unit tangent vector at the beginning of the ridge curve.

S2. According to the coordinate system at the initial point of the space parameter curve Before calculation /> connecting rod length/> The point corresponding to the arc length, that is, the point on the curve /> The unit tangent vector at , unit normal vector /> and the unit vicenormal vector/> form a minimal rotating frame .

In this embodiment, the specific process of step S2 includes the following steps:

S21. Set the arc length from the starting point on the known curve point /> The minimum rotating frame at is /> and the arc length on the curve /> point /> The unit tangent vector at ;

S22. Calculating the minimum rotating frame The unit tangent vector of /> Based on plane /> is the symmetric vector of the mirror image /> , where the plane /> for point /> and dot /> bisecting surface of

S23, the minimum rotating frame The unit binormal vector in /> Based on plane /> Mirror as a symmetric vector /> , and then the symmetric vector /> Based on plane /> Mirror symmetry and calculate the unit binormal vector /> , where the plane for point /> Symmetric vector at and the unit tangent vector /> bisecting surface of

S24, using the right-hand spiral rule based on the unit tangent vector and the unit binormal vector/> Calculate the unit normal vector /> ;

S25. According to the unit tangent vector , unit vice-normal vector/> and the unit normal vector /> Constitute the smallest rotating frame/> .

Please refer to Figure 2, the calculation of the minimum rotating frame based on the double reflection method of space geometry The process is as follows:

First assume that the point on the curve is known The minimum rotating frame at is /> ;

;

In the above formula, is the point on the curve /> The unit tangent vector at , /> is the point on the curve /> The unit normal vector at , /> is the point on the curve /> The unit binormal vector at .

to determine the starting point Minimum rotating frame/> , then the minimum rotating frame can be calculated /> The unit tangent vector of /> Based on plane /> is the symmetric vector of the mirror image /> , where the plane /> for point /> and dot /> The bisecting surface of is then:

;

In the above formula, point to /> as the starting point /> is the position vector of the end point, /> is the unit tangent vector /> point on the curve /> about plane/> Symmetric vector of .

Similarly, the minimum rotating frame The unit binormal vector in /> Based on plane /> Mirror as a symmetric vector , and then the symmetric vector /> Based on plane /> Mirror symmetry and calculate the unit binormal vector /> .

Which plane for point /> Symmetric vector at and the unit tangent vector /> the bisecting surface of , /> for the curve at /> The unit tangent vector at can be calculated by formula (5), about the unit subnormal vector /> Calculated as follows:

;

In the above formula, for /> About Plane/> mirrored symmetric vector, /> is the unit tangent vector /> and symmetric vectors /> vector difference, /> To form the minimum rotating frame /> The unit binormal of the pose matrix.

Finally, the right-hand spiral rule is used based on the unit tangent vector and the unit binormal vector/> Calculate the unit normal vector /> , the unit normal vector /> The calculation formula is:

;

in To form the minimum rotating frame /> attitude matrix /> The unit normal vector of , and further constitute the minimum rotating frame /> .

S3, based on the first minimum rotating frame/> and No. /> minimum rotating frame/> Generate motion plans for hyper-redundant robots;

Specifically, No. minimum rotating frame/> The expression is:

;

No. minimum rotating frame/> The expression is:

;

In this embodiment, the specific process of step S3 includes the following steps:

S31. Calculate the first minimum rotating frame/> and No. /> minimum rotating frame/> Attitude difference /> ;

Specifically, the attitude difference The calculation formula is:

;

In the above formula, .

S32. Based on attitude difference Calculation of joint angles for orthogonal joints of hyper-redundant robots /> ;

Specifically, the joint angle The calculation formula is:

;

Among them, since the joints of the super-redundant robot are orthogonal joints, the odd-numbered joints perform side swing motion, and the even-numbered joints perform pitch motion, so the calculation methods of odd-numbered joint angles and even-numbered joint angles are different. Through this step, the joint angle , and then generate a motion plan for a hyper-redundant robot.

S33. According to the joint angle Generating motion plans for hyper-redundant robots.

Please refer to Figure 3, curve 1 slowly changes to curve 2 and curve 3, based on the minimum rotating frame The corresponding joint angles over time are calculated by formula (18) , to complete the motion planning of the super-redundant robot. In Figure 3, X, Y, and Z are the three-dimensional coordinate directions of the coordinate system describing the spine curve.

As an example, curve 1, curve 2 and curve 3 in Figure 3 are all spatial quartic Bezier curves, and their parameter equations are respectively:

The parametric equation of curve 1 is:

;

The parametric equation of curve 2 is:

;

The parametric equation of curve 3 is:

;

in, , then their unit tangent vectors at the beginning of the ridge curve /> ,

unit normal vector ,

unit subnormal vector ,

Set the link length for a hyper-redundant robot 0.1m;

Therefore, the ridge curve of the example is based on the link length Based on the minimum rotating frame /> After discretization, the joint angles of the orthogonal joints of the hyper-redundant robot corresponding to curves 1, 2, and 3 can be calculated. for:

The joint angles of the orthogonal joints of the super-redundant robot corresponding to curve 1 for:

[0.067, -0.470, 0.052, -0.594, 0.060, -0.389, 0.0983, -0.187, 0.182, -0.083, 0.343, -0.0241, 0.471, 0.022, 0.342, 0.079, 0.181, 0.177, 0.097 ,0.364,0.059,0.577 ,0.048,0.490,0.059,0.266,0.086,0.138,0.127,0.077,0.185,0.046] radians.

The joint angles of the orthogonal joints of the super-redundant robot corresponding to curve 2 for:

[0.064,-0.643,0.046,-0.730,0.061,-0.348,0.108,-0.146,0.190,-0.063,0.304,-0.019,0.363,0.012,0.286,0.046,0.182,0.094,0.114,0 .173, 0.076, 0.294 ,0.056,0.413,0.051,0.399,0.062,0.273,0.093,0.163,0.159,0.095,0.296,0.056] radians.

Curve 3 corresponds to the joint angle of the hyper-redundant robot orthogonal joint for:

[0.064, -0.646, 0.045, -0.746, 0.058, -0.360, 0.096, -0.155, 0.158, -0.073, 0.236, -0.033, 0.290, -0.008, 0.269, 0.014, 0.200, 0.0440, 0.138 ,0.089,0.096, 0.168,0.070,0.303,0.057,0.451,0.060,0.430,0.096,0.270,0.240,0.142] radians.

In order to obtain the corresponding joint angles that change with time Finally, the motion planning of the super-redundant robot is completed in this way.

This embodiment avoids a large number of curvature torsion and integral calculations for the discretization of the spine curve of the super-redundant robot, and can improve calculation efficiency.

Each embodiment in this specification is described in a progressive manner, each embodiment focuses on the differences from other embodiments, and the same or similar parts of each embodiment can be referred to each other. For each of the above embodiments, since they are basically similar to the method embodiments, the description is relatively simple, and for related parts, please refer to the part of the description of the method embodiments.

The above embodiments have described the present invention in detail. The principles and implementation methods of the present invention have been described by using specific examples herein. The descriptions of the above embodiments are only used to help understand the method of the present invention and its core idea; meanwhile, for Those skilled in the art will have changes in the specific implementation and scope of application according to the idea of the present invention. In summary, the contents of this specification should not be construed as limiting the present invention.

Claims (8)

1. The motion planning method based on the standard space curve is characterized by comprising the following steps of:

s1, constructing a space parameter curveC(t) As a ridge curve of the super redundant robot, and calculate a unit tangent vector at the start of the ridge curveUnit normal vector->Unit auxiliary normal vector->Coordinate system forming initial part of space parameter curve；

S2, according to a coordinate system of the initial position of the space parameter curveBefore calculation->Length of individual connecting rod->Unit tangent vector at the corresponding arc>Unit normal vector->Unit auxiliary normal vector->Form the minimum rotary frame->；

In step S2, the specific process includes the following steps:

s21, setting the arc length of the distance starting position on the known curveIs->The minimum rotary frame at the position is +.>And arc length +.>Is->Unit tangent vector at->；

S22, calculating minimum rotation standard frameUnit tangent vector +.>Based on plane->Symmetry vector being mirror imageWherein plane->For->And (4) point->Is divided into two halves;

s23, rotating the minimum mark frameUnit auxiliary normal vector->Based on plane->Mirror image as symmetrical vectorThe symmetry vector is further->Based on plane->Mirror symmetry and calculating the unit auxiliary normal vector +.>In a plane ofFor->Symmetry vector ∈>And unit tangent vector->Is divided into two halves;

s24, adopting a right-handed spiral rule to base on unit tangent vectorAnd unit auxiliary normal vector->Calculating unit normal vector；

S25, according to the unit tangent vectorUnit auxiliary normal vector->And unit normal vector->Forming the minimum rotary frame；

S3 based on the firstMinimum rotation frame->First->Minimum rotation frame->And generating a motion plan of the super-redundant robot.

2. A method of motion planning according to claim 1, characterized in that: in step S1, the specific process includes the following steps:

s11, the space parameter curveDiscretizing the curve parameter t of (2), wherein the interval is +.>，/>The curve parameter t takes the value +.>；

S12, according to the curve parameter t and the intervalCalculating the arc length of the dorsal curve +.>；

S13, making the arc lengthEqual to the front +.>Length of individual connecting rod->And calculates the front +/for super redundant robot>Length of individual connecting rod->Corresponding curve parameters>；

S14, calculating curve parametersTangential vector of the place->；

S15, calculating the starting of the ridge curveUnit tangent vector at->Unit normal vector->Unit auxiliary normal vector。

3. A method of motion planning according to claim 2, characterized in that: in step S12, arc lengthThe calculation formula of (2) is as follows:

；

in the above, n refers to the number of discrete summation intervals of the arc length of the curve segment, m refers to the number of discrete curve segment sequences,the interval is indicated as such,representing that the spatial parameter curve C (t) is +.>Derivative of t>And (5) pointing to perform modular arithmetic.

4. A method of motion planning according to claim 2, characterized in that: in step S13, specifically, the method includes:

by spacing at intervalsAccumulating the arc length of the discrete curve segments for the integration interval cycles until the arc length +.>Equal to front->Length of individual connecting rod->Further determining the curve parameters satisfying the condition +.>。

5. A method of motion planning according to claim 2, characterized in that: in step S14, curve parametersTangential vector of the place->The calculation formula of (2) is as follows:

；

in the method, in the process of the application,for the spatial parameter curve C (t) versus curve parameter +.>1 st derivative of (c).

6. A method of motion planning according to claim 1, characterized in that: in step S22, the symmetry vectorThe calculation formula of (2) is as follows:

；

in the above-mentioned method, the step of,indicate->Is the starting point->Is the position vector of the end point, +.>Is a unit tangent vector +.>Dot +.>About plane->Is a symmetric vector of (a).

7. A method of motion planning according to claim 1, characterized in that: in step S23, a unit auxiliary normal vectorThe calculation formula of (2) is as follows:

；

in the above-mentioned method, the step of,is->About plane->Mirror symmetry vector, ">Is a unit tangent vector +.>And symmetry vector->Vector difference of>To form a minimum rotary frame->And a unit sub-normal vector of the gesture matrix.

8. A method of motion planning according to claim 1, characterized in that: in step S3, the specific process includes the following steps:

s31, calculate the firstMinimum rotation frame->First->Minimum rotation frame->Is the attitude difference of (2)；

S32, based on attitude difference valueCalculating joint angle of orthogonal joint of super-redundant robot>；

S33, according to joint anglesAnd generating a motion plan of the super-redundant robot.