A flexible robot path planning method based on navigation path constraints

Abstract

The present invention relates to the field of path planning technology, and in particular to a flexible robot path planning method based on navigation path constraints, including: initializing and setting the parameters of the robot; uniformly discretizing the ideal path curve between the starting point and the target point of the robot; constructing the pose matrix of the actual step point of the robot in the base coordinate system according to each discrete point on the ideal path curve and the principal normal, secondary normal and tangent of the discrete point; solving the joint parameters of the robot's terminal bending unit based on the distance function; solving the joint parameters of other bending units of the robot based on the reverse recursion method. The present invention controls the continuous detection robot to follow the preset path in three-dimensional space, and has high control accuracy and strong adaptability.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a flexible robot path planning method based on navigation path constraints. Background Art

[0002] The continuum robot (referred to as the robot) is a new type of bionic robot. Unlike traditional industrial robots, its system structure does not contain rigid links and identifiable rotational joints. Instead, it relies on a continuous bending structure or elastic skeleton and performs bending movements through some driving method (such as rope drive, nickel-titanium alloy wire or pneumatic drive, etc.) to produce structural deformation of the operating arm. Therefore, it has unique characteristics such as flexibility, flexibility and safety. It is very suitable for complex and narrow working spaces, such as aircraft fuel tank maintenance, post-earthquake rescue, nuclear leak maintenance and minimally invasive surgery.

[0003] Due to the high flexibility and super-redundant freedom of the continuum robot structure, the motion planning method of traditional industrial robots is no longer applicable to continuum robots. Many researchers have proposed new path planning methods for continuum robots. Some scholars have proposed a path planning method based on the inverse kinematics of continuum robots, which can realize the motion planning of robots with fewer joints. However, the overall calculation amount of the path planning method is large, and it is relatively difficult to extend it to multi-joint control; some have proposed a method of using sensors for robot path planning, but the requirements for sensor installation are relatively high and not universal; and some have proposed a path planning method to simplify the forward path search of the robot search space, simplifying three dimensions into two dimensions. Although it can simplify the path difficulty, it is only applicable to planes, which limits the application of continuum robots in complex space environments, such as exploration in space cavities and grasping objects in cluttered environments. Therefore, robot motion planning is an important problem that restricts its in-depth research and rapid development. Summary of the invention

[0004] In view of the shortcomings of the existing algorithms, the present invention controls the continuous detection robot to follow the preset path in three-dimensional space, and has higher control accuracy and stronger adaptability.

[0005] The technical solution adopted by the present invention is: a flexible robot path planning method based on navigation path constraints comprises the following steps:

[0006] Step 1: Initialize the parameters of the robot, including: the number of joint segments, joint length, bending angle of the bending unit, rotation angle, starting point and target point;

[0007] Step 2: evenly discretize the ideal path curve between the starting point and the target point of the robot, and control the robot to step from the initial zero position;

[0008] Step 3: Construct the pose matrix of the actual step point of the robot in the base coordinate system according to the discrete points on the ideal path curve and the principal normal, binormal and tangent of the discrete points;

[0009] Further, it specifically includes:

[0010] Assume that the discrete point closest to the robot on the ideal path curve is M, and s is the arc length from the starting point of the ideal path to point M. The tangent vector of the curve at point M is:

[0011]

[0012] Among them, t is the tangent vector, and the radial vector at point M on the curve is

[0013] if Then the tangent vector and position vector at point M uniquely determine an osculating plane, and the equation of the osculating plane is:

[0014]

[0015] in, is the binormal equation; the standard binormal equation is expressed as:

[0016]

[0017] At a known tangent vector and the binormal vector In the case of , the principal normal at point M is solved by the right-hand rule:

[0018]

[0019] in, is the principal normal vector;

[0020] When the robot moves along the ideal path curve with an arc length of s0, the homogeneous transformation matrix of the robot's actual step point relative to the base coordinate system is:

[0021]

[0022] Among them, M m is the position coordinate of point M in the base coordinate system;

[0023] Taking the tangent, principal normal and binormal of each point as the x, y and z axes of the coordinate system of the point, the homogeneous transformation matrix of the point relative to the base coordinate system is solved, and then the position matrix of the robot at all ideal path points is obtained.

[0024] Step 4: Calculate the bending angle and rotation angle using the distance function, the homogeneous transformation matrix of the terminal bending unit and the actual stepping point position coordinates of the robot;

[0025] Furthermore, the bending angle and the rotation angle are calculated using the distance function, the homogeneous transformation matrix of the terminal bending unit and the actual stepping point position coordinates of the robot;

[0026] Use the distance function to solve the bending angle and rotation angle when the distance between the actual step point of the robot and the ideal path point is the smallest;

[0027] Furthermore, the formula of the distance function is:

[0028] d=min{(x s -x l ) 2 +(y s -y l ) 2 +(z s -z l ) 2} (7)

[0029] In the formula, (x s ,y s ,z s ) is the actual step point position of the robot, obtained through the forward kinematics model, (x l ,y l ,z l ) is the position of the ideal path point on the path;

[0030] Furthermore, the formula of the homogeneous transformation matrix of the terminal bending unit is:

[0031]

[0032] in, is the homogeneous transformation matrix of the i-th bending unit relative to the initial coordinate system of the first segment;

[0033] Furthermore, the formulas for the bending angle and rotation angle are:

[0034]

[0035] Among them, (x i ,y i ,z i ) is the actual stepping point position coordinate of the robot;

[0036] Step 5: Solve the joint parameters of other bending units of the robot based on the reverse recursion method;

[0037] Further, it specifically includes:

[0038] The robot's non-intervention bending unit and pushing unit are equivalent to the robot's translation joint, and the joint parameters of the non-intervention bending unit are always 0; using the reverse recursive method, the robot's actual step point matrix is ​​multiplied by the inverse matrix of the homogeneous transformation matrix of the n-th bending unit relative to the first n-1 bending units to obtain the transformation matrix of the other n-1 bending units and the actual step point position;

[0039] Step 6: According to the joint parameters of each bending unit of the robot obtained in steps 3-5, control the robot to move forward one step;

[0040] Step 7: Determine whether the robot has reached the target position. If it has not reached the target point, re-solve the two joint variable values ​​of each bending unit when it moves forward one step. If it has reached the target point, end the robot's path planning.

[0041] Beneficial effects of the present invention:

[0042] 1. The motion planning method of the multi-module flexible detection robot is a numerical method. The control of the robot is simple and easy to implement, and the movement characteristics of the robot are fully considered.

[0043] 2. The motion planning method proposed in the present invention improves the accuracy of the robot in following the ideal path, has high adaptability, and is of great significance for improving the practicality of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 is a flow chart of the continuum robot motion planning method of the present invention;

[0045] Figure 2 is a schematic diagram of a single-joint kinematic model of a continuum robot of the present invention;

[0046] Figure 3 It is a schematic diagram of the multi-joint coordinate system of the continuum robot of the present invention;

[0047] Figure 4 This is a simulation experiment of the continuum robot of the present invention following different ideal paths. DETAILED DESCRIPTION

[0048] The present invention is further described below in conjunction with the accompanying drawings and embodiments. This figure is a simplified schematic diagram, which only illustrates the basic structure of the present invention in a schematic manner, and therefore it only shows the components related to the present invention.

[0049] like Figure 1 As shown, a flexible robot path planning method based on navigation path constraints includes the following steps:

[0050] Step 1: Initialize the robot parameters, including the number of robot joint segments n, joint length l, and the bending angle θ of each bending unit.i ∈(0,π / 2) , rotation angle α i ∈(0,2π), including the push unit, the starting point is the origin of the coordinate system (x0,y0,z0), and the target point is A(x A ,y A ,z A ), and place the actual step point of the robot at the origin of the base coordinate system;

[0051] Step 2: Evenly discretize the ideal path, and evenly discretize the ideal path curve between the robot's starting point and the target point into m points, that is, point (x0, y0, z0) to point (x m ,y m ,z m ), each ideal path point is expressed in the base coordinate system as:

[0052] L i =[(x0,y0,z0),...,(x i ,y i ,z i )]i=0,...,m (1)

[0053] Control the robot to move forward step by step from the starting point of the navigation path, manipulate the robot linear guide platform, and control the robot to move forward one step from the initial zero position, where the step length is the distance between two discrete points;

[0054] Step 3: Solve the pose of the actual step point of the robot in the base coordinate system during each step movement; specifically, construct the pose matrix of the actual step point of the robot in the base coordinate system according to the discrete points on the given ideal path curve and the principal normal, binormal and tangent of the discrete points;

[0055] Assume that the ideal path is C 2 Class curve, set the discrete point closest to the robot on the ideal path curve as M (for example, the actual step point at the current moment is discrete point 3, then the nearest discrete point is 4), s is the arc length from the starting point of the ideal path to point M, then the tangent vector of the curve at point M is:

[0056]

[0057] Among them, t is the tangent vector, and the radial vector at point M on the curve is

[0058] if in, and Radial vector Relative to the first and second derivatives of s, the tangent vector and position vector at point M can uniquely determine an osculating plane, and the equation of the osculating plane is:

[0059]

[0060] in, is the binormal equation, and the standard binormal equation is expressed as:

[0061]

[0062] At a known tangent vector and the binormal vector In the case of , the principal normal at point M is solved by the right-hand rule:

[0063]

[0064] Therefore, when the robot moves along the ideal path curve with an arc length s0, the homogeneous transformation matrix of the robot's actual step point relative to the base coordinate system is:

[0065]

[0066] Among them, M m is the position coordinate of point M in the base coordinate system. The ideal path curve is divided into m points at equal distances, and the tangent, principal normal and binormal of each point are used as the x, y and z axes of its coordinate system. The homogeneous transformation matrix of the point relative to the base coordinate system is solved, and then the position and posture matrix of the robot at all ideal path points is obtained.

[0067] Step 4: Solve the joint parameters of the robot's terminal bending unit based on the distance function. Specifically, use the distance function of fmincon to solve the bending angle θ when the distance between the robot's actual step point and the ideal path point is the smallest. i and the rotation angle α i ;

[0068] The distance function is:

[0069] d=min{(x s -x l ) 2 +(y s -y l ) 2 +(z s -z l ) 2} (7)

[0070] In the formula, (x s ,y s ,z s ) is the actual step point position of the robot, obtained through the forward kinematics model, (x l ,y l ,z l ) is the position of the ideal path point on the path;

[0071] Figure 2 This is a diagram of the robot single joint kinematics model provided by the present invention, in which l in the driving space i1 , l i2 , l i3 are the lengths of the ropes passing through the 1st, 2nd, and 3rd rope holes of the i-th segment, respectively. The rope hole numbers are defined in a counterclockwise direction and are consistent with the rope hole numbers at the corresponding positions on each disk. α in the joint space i ,θ i are the rotation angle and bending angle of the i-th bending unit respectively; the robot is driven and controlled by ropes, and the bending deformation of the robot is achieved by changing the length of the ropes; the center O of the base end connection disk of the i-th bending unit i-1 As the origin, establish the initial coordinate system ∑ i-1 (O i-1 -x i-1 y i-1 z i-1 ), the direction of the straight line passing through the coordinate origin and guide rope hole 1 is taken as x i-1 Axis, the tangent direction of the center skeleton through the origin is z i-1 axis, and finally determine the y axis of the coordinate system by the right-hand rule i-1 Axis; Similarly, connect the top of the i-th segment to the disk midpoint O i Establish a coordinate system for the origin∑ i( O i -x i y i z i ).

[0072] In order to realize the initial coordinate system ∑ i-1 (O i-1 -x i-1 y i-1 z i-1 ) to the coordinate system∑ i (O i -x i y i z i ) is transformed, firstly the coordinate system ∑ i-1 (O i-1 -x i-1 y i-1 z i-1 ) i-1 Axis around z i-1 Axis rotation α i Angle to the bending plane, and then the origin O i-1 Translate along the arc to the origin O i Then revolve the coordinate system around y i-1 Axis rotation θ i Angle z i-1 Axis and top zi The axes coincide; finally, around z i Axis rotation - α i The angle between the newly obtained coordinate system and the coordinate system ∑ i (O i -x i y i z i ) completely overlap, so the homogeneous transformation matrix from the i-1th segment to the i-th segment bending unit is:

[0073]

[0074] where s and c are the abbreviations for sin and cos, and α i ∈(0,π / 2),θ i ∈(0,2π);

[0075] After obtaining the position of the i-th bending unit in space, the transformation matrices of every two adjacent bending units are multiplied by the chain rule, as follows: Figure 3 As shown, it is a schematic diagram of the coordinate system of the multi-joint robot provided by the present invention, and the homogeneous transformation matrix of the terminal bending unit is obtained:

[0076]

[0077] in, is the homogeneous transformation matrix of the i-th bending unit relative to the initial coordinate system of the first segment;

[0078] From the forward kinematics model, we can know that the actual stepping point position of the robot can be obtained according to the homogeneous transformation matrix from the i-1th segment to the i-th segment bending unit, that is, x i ,y i ,z i for:

[0079]

[0080] Solving the above equation, we can get α i ,θ i for:

[0081]

[0082] Step 5: Solve the joint parameters of other bending units of the robot based on the reverse recursion method; the robot's non-intervention bending unit and the push unit are equivalent to the robot's translation joints. Figure 3 As shown in the figure, it is a bending unit that has not passed the starting point, and its joint parameters are always 0. Using the reverse recursive method, the actual step point matrix of the robot is multiplied by the homogeneous transformation matrix of the nth bending unit relative to the n-1th bending unit (i.e. ) is used to obtain the transformation matrix of the n-1 bending unit and its actual step point position. The bending angle and rotation angle of the n-1 other bending units are solved by formulas (10) and (11). Similarly, the bending angle and rotation angle of other bending units can be solved. Other bending units are the parts between the unintervening bending units and the end bending units, such as Figure 3 As shown;

[0083] The homogeneous transformation matrix of other bending units is:

[0084]

[0085] When j is 1, we get is the homogeneous transformation matrix of the robot's n-1 bending unit relative to the base coordinate system, and so on;

[0086] Step 6: According to the joint parameters of each bending unit of the robot obtained in steps 3-5, control the robot to move forward one step;

[0087] Step 7: Determine whether the robot has reached the target position. If it has not reached the target point, re-solve the two joint variable values ​​of each bending unit when it moves forward one step. If it has reached the target point, end the robot's path planning.

[0088] In order to verify the multi-module flexible detection robot motion planning method proposed in the present invention, a robot path following simulation experiment was carried out:

[0089] Design a complex spatial path and use the motion planning method provided by the present invention to drive the robot to move along the ideal path; Figure 4 As shown, it is a simulation of a robot (10 bending units, each bending unit is 50 mm long) gradually following the ideal path, which are the specific situations of the robot with 5, 7, 9 and 10 bending units following the ideal path through the starting point, wherein the ideal path consists of a straight line and a curve, the starting point is the coordinate origin, and the total length is 500 mm; the thinner line in the figure is the ideal path, and the thicker line is the robot's forward curve. It can be seen from the figure that the robot always maintains a small deviation in the process of following the ideal path, indicating that the method proposed in the present invention can effectively control the robot to move along the preset path and has high control accuracy.

[0090] Based on the above ideal embodiments of the present invention, the relevant staff can make various changes and modifications without departing from the technical concept of the present invention through the above description. The technical scope of the present invention is not limited to the contents of the specification, and its technical scope must be determined according to the scope of the claims.

Claims

1. A flexible robot path planning method based on navigation path constraints, characterized in that: The following steps are involved: Step 1: Initialize the parameters of the robot, including: the number of joint segments, joint length, bending angle of the bending unit, rotation angle, starting point and target point; Step 2: evenly discretize the ideal path curve between the starting point and the target point of the robot, and control the robot to step from the initial zero position; Step 3: Construct the pose matrix of the actual step point of the robot in the base coordinate system according to the discrete points on the ideal path curve and the principal normal, binormal and tangent of the discrete points; Step 4: Solve the joint parameters of the robot's terminal bending unit based on the distance function; Step 5: Solve the joint parameters of other bending units of the robot based on the reverse recursion method; Step 6: According to the joint parameters of each bending unit of the robot obtained in steps 3-5, control the robot to move forward one step; Step 7: Determine whether the robot has reached the target position. If it has not reached the target point, re-solve the two joint variable values ​​of each bending unit when it moves forward one step. If it has reached the target point, end the robot's path planning.

2. The flexible robot path planning method based on navigation path constraints according to claim 1 is characterized in that: Step three specifically includes: Assume that the discrete point closest to the robot on the ideal path curve is M, and s is the arc length from the starting point of the ideal path to point M. The tangent vector of the curve at point M is: Among them, t is the tangent vector, and the radial vector at point M on the curve is if Then the tangent vector and position vector at point M uniquely determine an osculating plane, and the equation of the osculating plane is: in, is the binormal equation; the standard binormal equation is expressed as: At a known tangent vector and the binormal vector In the case of , the principal normal at point M is solved by the right-hand rule: in, is the principal normal vector; When the robot moves along the ideal path curve with an arc length of s0, the homogeneous transformation matrix of the robot's actual step point relative to the base coordinate system is: Among them, M m is the position coordinate of point M in the base coordinate system; Taking the tangent, principal normal and binormal of each point as the x, y and z axes of the coordinate system of the point, the homogeneous transformation matrix of the point relative to the base coordinate system is solved, and then the position matrix of the robot at all ideal path points is obtained.

3. The flexible robot path planning method based on navigation path constraints according to claim 1 is characterized in that: Step 4 specifically includes: using the distance function, the homogeneous transformation matrix of the terminal bending unit and the actual step point position coordinates of the robot to calculate the bending angle and the rotation angle.

4. The flexible robot path planning method based on navigation path constraints according to claim 3 is characterized in that: The formula for the distance function is: d=min{(x s -x l ) 2 +(y s -y l ) 2 +(z s -z l ) 2 } (7) In the formula, (x s ,y s ,z s ) is the actual step point position of the robot, obtained through the forward kinematics model, (x l ,y l ,z l ) is the position of the ideal path point on the path.

5. The flexible robot path planning method based on navigation path constraints according to claim 3 is characterized in that: The formula for the homogeneous transformation matrix of the terminal bending unit is: in, is the homogeneous transformation matrix of the i-th bending unit relative to the initial coordinate system of the first segment.

6. The flexible robot path planning method based on navigation path constraints according to claim 3 is characterized in that: The formulas for the bend angle and rotation angle are: Among them, (x i ,y i ,z i ) is the actual step point position coordinate of the robot.

7. The flexible robot path planning method based on navigation path constraints according to claim 1 is characterized in that: Step 5 specifically includes: The robot's non-intervention bending unit and pushing unit are equivalent to the robot's translation joint, and the joint parameters of the non-intervention bending unit are always 0. The reverse recursive method is used to multiply the robot's actual step point matrix by the inverse matrix of the homogeneous transformation matrix of the n-th bending unit relative to the first n-1 bending units to obtain the transformation matrix of the other n-1 bending units and the actual step point position.

Images