Three-stage flexible robot obstacle avoidance pose control method

Abstract

The application discloses a three-section flexible robot obstacle avoidance pose control method, which comprises the following steps: obtaining a corresponding obstacle map; obtaining an obstacle avoidance path in the obstacle map by using a random exploration obstacle avoidance algorithm, obtaining a control point according to the obstacle avoidance path, and obtaining a proximal section second-order fitting Bezier curve according to a first control point coordinate adjustment strategy; sequentially redetermining control point coordinates to obtain a middle section and a terminal section second-order fitting Bezier curve; verifying and comparing the respective curve lengths of the fitting Bezier curves of the three-section flexible robot, and if the verification and comparison are passed, determining control parameters according to a current virtual pose, otherwise adjusting new coordinates of the control point P1 according to a set control point adjustment strategy. The application realizes the efficiency and accuracy of the continuum robot obstacle avoidance control through the (RRT+Bezier curve) algorithm and the deformation length error control.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot obstacle avoidance control, and particularly relates to a three-section flexible robot obstacle avoidance pose control method. BACKGROUND

[0002] A continuum robot is a kind of bionic robot based on a flexible or segmented elastic structure, and the movement thereof is achieved through continuous deformation. The continuum robot has the characteristics of high degree of freedom, strong flexibility, and adaptability to narrow spaces, and is widely used in scenes such as minimally invasive surgery (such as endoscopic operation), industrial pipeline detection, and nuclear facility maintenance. Unlike traditional rigid-joint robots, the kinematic model of the continuum robot has high nonlinearity and high configuration space dimension, which leads to a significant increase in the complexity of motion planning.

[0003] However, the traditional control method has the following disadvantages: the current obstacle avoidance motion planning method for the continuum robot is very limited, and the core idea is still to solve based on inverse kinematics. For a flexible robot with limited curvature and continuity, the current motion planning method is not completely applicable. The existing common obstacle avoidance algorithms mainly include: 1. "Heuristic algorithm": a feasible solution in an obstacle avoidance environment is given under a certain calculation time, and it is verified whether the robot path following process is safe. If a collision occurs, the path is corrected, and the expected target is reached through repeated debugging. This method is intuitive, but it needs to repeatedly calculate the "spatial intersection" problem. It is difficult to summarize heuristic information during the trial process, and multiple calculations are usually required, which prolongs the planning period; 2. "Artificial potential field method": a repulsive force field is first established around the obstacle, and an attractive force field is established at the target position. Then, the superposition minimum value of the obstacle and the target pose is used to select and determine the robot motion trajectory. This method has heuristic information and belongs to a local optimization method, and has good real-time performance, but it is uncontrollable and difficult to complete global motion planning; 3. "C-space method": the key lies in the construction of the evaluation function, which is a planning method based on global known information. This method has obvious obstacle avoidance effect in planar motion, but it is difficult to find a suitable path in the workspace. In addition, there are path planning algorithms including gradient projection method, mathematical quadratic convex optimization, etc.

[0004] In short, the current control strategy research for flexible body robots is basically based on inverse kinematics solution. Due to the high nonlinearity and redundancy of the research object, the inverse kinematics often has unstable or undesirable results. After the introduction of obstacle avoidance requirements, the obtained pose is often difficult to control or has poor stability, and the success rate of obtaining a feasible solution is greatly reduced. SUMMARY

[0005] The present application aims to provide a three-section flexible robot obstacle avoidance pose control method, which aims to solve the technical problems of the disadvantages of the traditional control method.

[0006] To solve the above problems, the present application provides a kind of, its corresponding technical solutions include:

[0007] Step 1, according to actual environment scene builds virtual obstacle environment to obtain corresponding obstacle map, and marks three flexible robot near end point coordinates and target end point coordinates in the obstacle map;

[0008] Step 2, according to the target end position of three flexible robot, i.e.the target end point coordinates, a random exploration obstacle avoidance algorithm is used to obtain an obstacle avoidance path in the obstacle map, and it is used as the rough obstacle avoidance pose of three flexible robot;

[0009] Step 3, sequentially group each node in the foregoing obstacle avoidance path except the starting point P0, thereby obtaining 6 control points, while the starting point is used as a control point, 7 control points are obtained, marked as P0-P6;And according to the first control point coordinate adjustment strategy, the coordinates of P0-P2 are determined, the three points of the determined P0-P2 coordinates are sequentially connected as control line, to obtain the near end segment two-order fitting Bezier curve;

[0010] Step 4, according to the new coordinates of the determined P1, the new coordinates of P3-P6 are determined in turn to obtain the intermediate segment two-order fitting Bezier curve and the end segment two-order fitting Bezier curve;

[0011] Step 5, according to the curve length corresponding to the fitting Bezier curve of the near end segment, intermediate segment and end segment of the three flexible robot obtained;The obtained curve length is compared with the actual length value of the near end segment, intermediate segment and end segment of the three flexible robot in turn, to confirm whether they are all less than the set length error threshold d s , then according to the current virtual pose determines control parameter, otherwise according to the set control point adjustment strategy adjusts control point P1' and re-performs step 4 to obtain new control point coordinates until the design requirement is met.

[0012] Further, the foregoing step 1 includes:

[0013] Step 11, real-time acquisition of three flexible robot actual environment scene information;

[0014] Step 12, a virtual obstacle environment is built according to the actual environment scene to obtain a corresponding obstacle map, that is, the maximum reachable working area of the three-segment flexible robot is identified according to the actual environment scene, the maximum inscribed cuboid of the maximum reachable working area is obtained and is taken as an obstacle environment, and the obstacles in the actual environment scene are marked in the obstacle environment after being characterized to form an obstacle map; the foregoing characterization of the obstacles in the actual environment scene refers to identifying the obstacle marking information of each obstacle in the actual environment scene, and arranging each obstacle with its own minimum circumscribed cuboid as a virtual obstacle in the obstacle environment according to the respective obstacle marking information.

[0015] Step 13, the proximal end point coordinates and the target end point coordinates of the three-segment flexible robot are marked in the obstacle map.

[0016] Further, the foregoing step 2 includes:

[0017] Step 21, initialization is performed to create an empty tree: the foregoing proximal end point coordinates are taken as the root node of the RRT algorithm, and the target end point coordinates are taken as the target point;

[0018] Step 22, random sampling is performed and the nearest neighbor node is found, so as to obtain a corresponding extended tree: a point is randomly generated in the configuration space as a sample point, then the nearest node to the sample point in the existing tree is found, which is called the nearest neighbor node, and a fixed step length is grown from the nearest node to the sample point to generate a new node;

[0019] Step 23, collision detection is performed: whether there is a collision between the new node and the obstacles in the obstacle map is checked, if yes, the new node is discarded; otherwise, the new node is added to the existing tree;

[0020] Step 24, a termination condition is set and a path is extracted: whether the new node is close to the end point is judged, if yes, whether the distance between the new node and the end point is less than a set threshold value is further judged, then the algorithm is terminated, and a feasible path from the start point to the end point is found by starting from the end point and backtracking until the start point is reached; otherwise, step 25 is executed;

[0021] Step 25, steps 22-24 are repeated until a maximum iteration number is reached.

[0022] Further, the foregoing step 3 includes:

[0023] Step 31, each node in the foregoing obstacle avoidance path except the start point P0 is sequentially grouped to obtain six node sets, the node closest to the end point in each node set is defined as a control point, and the start point is also taken as a control point to obtain seven control points; the node closest to the end point in each node set is defined as P i(i = 1, 2, 3, 4, 5, 6), then the 7 control points are P0-P6;

[0024] Step 32, define a first control point coordinate adjustment strategy, and determine the coordinates of P0-P2 according to the first control point coordinate adjustment strategy;

[0025] The first control point coordinate adjustment strategy is:

[0026] According to the original coordinates of control points P0 and P2 and the first coordinate relationship formula, the coordinates of control point P1 are reconfirmed; that is, if the original coordinates of control points P0, P1 and P2 satisfy the first coordinate relationship formula, then the original coordinates of control point P1 are taken as the coordinates of reconfirmed control point P1; otherwise, the point of the current control point P1 projected on the perpendicular bisector of the line connecting P0 and P2 is taken as the coordinates of reconfirmed control point P1, that is, new P1';

[0027] The first coordinate relationship formula is

[0028] ||P1-P0||-||P1-P2||<d q

[0029] wherein, d q is a conditional threshold value;

[0030] Step 33, connect the coordinates of P0-P2 determined in the foregoing step as control lines in sequence to obtain a second-order fitting Bezier curve of the proximal segment.

[0031] Further, the foregoing step 4 comprises:

[0032] Step 41, according to the new coordinates P1' of P1 determined in the foregoing step, re-determine the new coordinates P3' of P3, and the corresponding formula is

[0033]

[0034] In the above formula, m and n are proportional coefficients of the proximal segment and the intermediate segment of the three-segment flexible robot, and

[0035] Step 42, according to the new coordinates P3' of P3 determined in the foregoing step and the original coordinates of P4, re-determine the new coordinates P4' of P4, and the corresponding formula is

[0036]

[0037] Step 43, according to P2, the coordinates of re-determined P3, and the new coordinates P4' of P4, that is, the three points P2, P3' and P4', obtain a fitting Bezier curve of the intermediate segment of the three-segment flexible robot;

[0038] Step 44, according to the foregoing determination of the new coordinates P3' of P3 and the new coordinates P4' of P4, the new coordinates P5' of P5 are re-determined, and the corresponding formula is

[0039]

[0040] m', n' are the redefined scale factors, that is, the continuous scale factors of the middle segment and the end segment of the three-segment flexible robot are redefined:

[0041]

[0042] Step 45, according to the re-determined new coordinates P4' of P4, the new coordinates P5' of P5 and the original P6 coordinates, the new coordinates P6' of P6 are obtained, and the corresponding formula is

[0043]

[0044] Step 46, the new coordinates P4' of P4, the new coordinates P5' of P5 and the new coordinates P6' of P6 are obtained by fitting the Bezier curve of the end segment of the three-segment flexible robot.

[0045] The embodiment of the present application has the following beneficial effects:

[0046] The three-segment flexible robot obstacle avoidance pose control method provided by the present application proposes a pose control algorithm for a continuum robot, which can replace the traditional inverse kinematics operation; specifically, the control algorithm proposed in the present application migrates the RRT algorithm commonly used for mobile robot obstacle avoidance to the continuum robot, and regards the discrete points in the mobile robot algorithm as control points of the continuum robot, thereby providing a reference for the connection between mobile robots and continuum robots in obstacle avoidance control; the control algorithm proposed in the present application adopts the principle of Bezier curve fitting, and can be transplanted to various multi-segment and multi-degree-of-freedom flexible robots as needed. BRIEF DESCRIPTION OF DRAWINGS

[0047] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed in the embodiments or prior art description will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can also be obtained by those skilled in the art without creative labor.

[0048] Among them:

[0049] Figure 1 is the overall step flowchart of the present application;

[0050] Figure 2The design drawing of the control object (three-section flexible robot) of the control strategy proposed in the application;

[0051] Figure 3 The schematic diagram of fitting the RRT obstacle avoidance path by using the Bezier curve in the application;

[0052] Figure 4 The comparison diagram of the circular arc fitting and the Bezier curve fitting when the single-section flexible body is bent in the application;

[0053] Figure 5 The spatial C 1 Continuous schematic diagram;

[0054] Figure 6 The schematic diagram of fitting the three-section flexible robot by using the Bezier curve in the application;

[0055] Figure 7 The schematic diagram of obtaining the final target pose by solving multiple times in the application;

[0056] Figure 8 The schematic diagram of the obstacle avoidance effect in different obstacle environments in the application;

[0057] Figure 9 The schematic diagram of the actual robot working scene and the virtual scene in the application. DETAILED DESCRIPTION

[0058] The technical solutions in the embodiments of the application will be clearly and completely described below with reference to the drawings in the embodiments of the application. Obviously, the described embodiments are only part of the embodiments of the application, rather than all the embodiments of the application. Based on the embodiments in the application, all other embodiments obtained by those skilled in the art without creative labor fall within the protection scope of the application.

[0059] It should be noted that all the directionality indications (such as up, down, left, right, front, back, etc.) in the embodiments of the application are only used to explain the relative position relationship, motion condition, etc. between components in a certain specific posture (as shown in the drawings), and if the specific posture changes, the directionality indications also change accordingly.

[0060] In addition, the description related to "first", "second" and the like in the present application is only for the purpose of description, and cannot be understood as indicating or implying the relative importance of the indicated technical features or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first", "second" can be explicitly or implicitly included at least one of the features. In addition, the technical solutions of various embodiments can be combined with each other, but it must be based on the realization of ordinary skilled in the art, when the combination of technical solutions appears contradictory or cannot be realized, it should be considered that the combination of technical solutions does not exist, also not within the protection scope required by the present application.

[0061] As shown in Figures 1-9 The method provided by the present application specifically comprises:

[0062] Step 1, building a virtual obstacle environment according to the actual environment scene to obtain a corresponding obstacle map, and marking the proximal end point coordinates and the target end point coordinates of the three-segment flexible robot in the obstacle map;

[0063] Step 2, according to the target end position of the three-segment flexible robot, i.e. the target end point coordinates, a feasible obstacle avoidance path is obtained in the obstacle map by using a random exploration obstacle avoidance algorithm, and the path is used as the rough obstacle avoidance pose of the three-segment flexible robot;

[0064] Step 3, sequentially grouping each node in the foregoing obstacle avoidance path except the starting point P0, thereby obtaining 6 control points, and simultaneously taking the starting point as a control point, obtaining 7 control points, denoted as P0-P6; and determining the coordinates of P0-P2 according to the foregoing first control point coordinate adjustment strategy, sequentially connecting the coordinates of P0-P2 determined as the control line to obtain the proximal segment second-order fitting Bezier curve;

[0065] Step 4, according to the coordinates of P1 determined, sequentially redetermining the coordinates of P3-P6 to obtain the intermediate segment second-order Bezier curve and the end segment second-order fitting Bezier curve;

[0066] Step 5, according to the curve length corresponding to each of the fitting Bezier curves of the proximal segment, the intermediate segment and the end segment of the three-segment flexible robot obtained; sequentially comparing the obtained curve length with the actual length value of each segment of the proximal segment, the intermediate segment and the end segment of the three-segment flexible robot to confirm whether they are all less than the set length error threshold d s , if yes, determining the control parameters according to the current virtual pose, otherwise adjusting the coordinates of the control point P1' according to the set control point adjustment strategy and reperforming step 4 to obtain new control points until the design requirements are met.

[0067] In conclusion, the application realizes the efficiency, accuracy and universality of the obstacle avoidance control of the continuum robot by fusing the (RRT+ Bezier curve) optimization algorithm with physical constraints (deformation length error control), and provides reliable technical support for the complex environment operation of the flexible robot.

[0068] Further embodiments, the aforementioned step 1 comprises:

[0069] Step 11, real-time acquisition of actual environment scene information of the three-section flexible robot by using sensors; the aforementioned sensors include vision, laser radar, ultrasonic or tactile sensors, etc., selected according to the actual design scene.

[0070] Step 12, according to the actual environment scene, a virtual obstacle environment is built to obtain the corresponding obstacle map, as shown in Figure 1 ; that is, according to the actual environment scene, the maximum reachable working area of the three-section flexible robot is identified, the maximum inscribed cuboid of the maximum reachable working area is obtained and used as the obstacle environment, and the obstacles in the actual environment scene are characterized and marked in the obstacle environment to form the obstacle map; the aforementioned characterization of the obstacles in the actual environment scene refers to identifying the obstacle marker information (position and volume information) of each obstacle in the actual environment scene, and arranging each virtual obstacle in the obstacle environment according to the respective obstacle marker information.

[0071] Step 13, marking the proximal end point coordinates and the target end point coordinates of the three-section flexible robot in the obstacle map.

[0072] The design idea of the above steps, this step maps the maximum reachable area of the three-section flexible robot as the obstacle environment and identifies the corresponding markers by using the kinematic constraints of the robot itself to define "virtual obstacles", so as to limit the motion range in the subsequent path planning and avoid structural damage or task failure caused by over-limit action.

[0073] Further embodiments, the aforementioned step 2 comprises:

[0074] Step 21, initialization is performed to create an empty tree: the aforementioned proximal end point coordinates are set as the root node of the RRT algorithm, and the target end point coordinates are set as the target point;

[0075] Step 22, random sampling and finding the nearest neighbor node are performed to obtain the corresponding extended tree: a point is randomly generated in the configuration space (free space) as a sample point, then the nearest node to the aforementioned sample point is found in the existing tree, called the nearest neighbor node, and a fixed step length is grown from the nearest node to the sample point to generate a new node;

[0076] Step 23, collision detection: check whether there is a collision between the new node and the obstacle in the obstacle map, if yes, discard the new node; otherwise, add the new node to the existing tree;

[0077] Step 24, set the termination condition and extract the path: determine whether the new node is close to the end point, if yes, further determine whether the distance between the new node and the end point is less than the set threshold, then the algorithm terminates, and the path is extracted by backtracking from the end point to the start point to find a feasible path from the start point to the end point; otherwise, execute step 25;

[0078] Step 25, repeat steps 22-24 until the maximum number of iterations is reached, and the maximum number of iterations in this example is 100.

[0079] The above step design idea is that the RRT algorithm is used to start from the start point, randomly expand to the surrounding space like tree growth, and gradually approach the target area, so as to explore the unknown environment and generate a rough obstacle avoidance pose of the three-section flexible robot by means of random sampling and tree expansion.

[0080] Further embodiments, the foregoing step 3 comprises:

[0081] Step 31, sequentially group each node in the foregoing obstacle avoidance path except the start point P0, thereby obtaining 6 node sets, define the node closest to the end point in each node set as a control point, and define the start point as a control point, thereby obtaining 7 control points; the node closest to the end point in each node set is defined as P i (i=1, 2, 3, 4, 5, 6), and the 7 control points are P0-P6; the sequential grouping rule is that in the obtained obstacle avoidance path, all nodes are sequentially divided into 6 groups according to the order from the start point to the end point and the number of nodes is as evenly divided as possible, and the excess nodes are collected into the last node set if they cannot be evenly divided;

[0082] Step 32, define a first control point coordinate adjustment strategy, and determine the coordinates of P0-P2 according to the first control point coordinate adjustment strategy;

[0083] The foregoing first control point coordinate adjustment strategy is:

[0084] According to the original coordinates of the control points P0 and P2 and the first coordinate relationship formula, the coordinates of the control point P1 are reconfirmed; that is, if the original coordinates of the control points P0, P1 and P2 satisfy the first coordinate relationship formula, the original coordinates of the control point P1 are taken as the coordinates of the reconfirmed control point P1; otherwise, the point of the current control point P1 projected on the perpendicular bisector of the line connecting P0 and P2 is taken as the coordinates of the reconfirmed control point P1, that is, the new P1';

[0085] The first coordinate relationship formula is

[0086] ||P1-P0||-||P1-P2||<d q

[0087] wherein d q is a conditional threshold, generally 0.2; that is, the horizontal and vertical coordinates of the original coordinates of the control points P0, P1 and P2 all need to satisfy the condition;

[0088] Step 33, sequentially connecting the coordinates of P0-P2 determined in the foregoing as control lines to form a second-order Bezier curve of an approximate circular arc; thereby obtaining a second-order fitting Bezier curve of the proximal segment, that is, drawing a second-order fitting Bezier curve of the proximal segment containing the three control points, and the expression is

[0089] B(t) = (1-t) 2 P0+2t(t-1)P1+t 2 P2

[0090] More specifically, in the present embodiment, the value range of t is set to 0-1, and the step is 0.01.

[0091] The design idea of the above step is that for a multi-segment continuum manipulator, the existing research generally adopts the segmented constant curvature method for kinematic modeling. However, the method of fitting continuum posture with a circular arc is relatively limited. Bezier curve, as a commonly used parametric curve in computer graphics and related fields, is another expression form that can represent the shape of a continuum robot. As shown in Figure 4 , a continuum robot in different postures is fitted with a circular arc (red dashed line) and a second-order Bezier curve (blue solid line) respectively. Both curves can achieve good fitting effect, but the distance L O of the control point (center) O of the circular arc from the fitted curve is much larger than the distance L P of the control point P of the Bezier curve from the fitted curve. Moreover, the curvature information of the circular arc in practical application is difficult to estimate. In comparison, the Bezier curve has more flexible adaptive performance, that is, after the start point and the end point of the curve are determined, the shape of the curve can be adjusted by adjusting the position of the intermediate control point only. Therefore, this strategy does not need to use the kinematic method, nor does it need to know the curvature information in advance.

[0092] Further embodiments, the foregoing step 4 comprises:

[0093] Step 41, according to the new coordinates P1' of P1 determined in the foregoing, the new coordinates P3' of P3 are re-determined, and the corresponding formula is

[0094]

[0095] In the above formula, m, n are the proportional coefficients of the proximal segment and the intermediate segment of the three-segment flexible robot, and

[0096] Step 42, according to the above-mentioned determination of the new coordinates P3' of P3 and the original coordinates of P4, the new coordinates P4' of P4 are re-determined, and the corresponding formula is

[0097]

[0098] Step 43, according to P2, the new coordinates of P3 and the new coordinates of P4, i.e. P2, P3', P4', three points, the intermediate segment fitting Bezier curve of the three-segment flexible robot is obtained;

[0099] Step 44, according to the above-mentioned determination of the new coordinates P3' of P3 and the new coordinates P4' of P4, the new coordinates P5' of P5 are re-determined, and the corresponding formula is

[0100]

[0101] m', n' are the redefined proportional coefficients, i.e. the continuous proportional coefficients of the intermediate segment and the terminal segment of the three-segment flexible robot are redefined:

[0102]

[0103] Step 45, according to the above-mentioned re-determination of the new coordinates P4' of P4, the new coordinates P5' of P5 and the original P6 coordinates, the new coordinates P6' of P6 are obtained, and the corresponding formula is

[0104]

[0105] Step 46, the new coordinates P3' of P3, the new coordinates P5' of P5 and the new coordinates P6' of P6 are re-determined, and the terminal segment fitting Bezier curve of the three-segment flexible robot is obtained; the finally obtained three-segment flexible robot fitting schematic diagram using Bezier curve is as shown in Figure 6 .

[0106] The design idea of the above steps is that, according to the C 1 continuity requirement and the Bezier curve limitation requirement, the auxiliary points P3', P4' and P5', P6' are sequentially determined, and finally the three-segment continuous second-order Bezier curve is generated by using P0, P1', P2, P3', P4', P5' and P6'; that is, according to the C 1 continuity requirement and the Bezier curve limitation requirement, it is ensured that the tangent vector B1'(1) of the robot proximal segment curve B1(t) at t=1 is the same as the tangent vector B2'(0) of the next curve B2(t) at t=0. Space C 1 The specific meaning of continuity can be seen inFigure 5 . The end segment and the intermediate segment C 1 For example, the following formula is used:

[0107]

[0108] In the above formula, m and n are proportional coefficients, which can be determined by the following formula:

[0109]

[0110] The end segment and the intermediate segment C 1 Similarly, the horizontal and vertical coordinates of each control point are calculated according to this rule.

[0111] In a further embodiment, the aforementioned step 5 comprises:

[0112] Step 51, according to the obtained fitting Bezier curve of the proximal segment, the intermediate segment and the end segment of the three-segment flexible robot, the corresponding curve length of each segment is obtained;

[0113] Step 52, the curve length obtained in step 51 is compared with the actual length value of each segment of the proximal segment, the intermediate segment and the end segment of the three-segment flexible robot in turn, and it is confirmed whether they are all less than the set length error threshold d s If yes, the control parameters are determined according to the current virtual pose, otherwise step 53 is executed;

[0114] The curve length obtained in step 51, i.e. the fitting length s j is compared with the actual length value s r of each segment of the proximal segment, the intermediate segment and the end segment of the flexible robot, and the difference value corresponding to each segment is obtained; and the length error threshold d s (generally set to 0.1) is compared one by one, if each segment ||s j -s r ||<d s , the control parameters can be determined according to the current virtual pose;

[0115] Step 53, according to the set control point adjustment strategy, the coordinates of the control point P1' are adjusted and the new P0, P1', P2, P3', P4', P5' and P6' are obtained by re-performing step 4, and then the fitting Bezier curve of the proximal segment, the intermediate segment and the end segment of the new three-segment flexible robot is obtained and step 51 is executed;

[0116] The aforementioned control point adjustment strategy is: if ||s j -s r ||<d s , s j and s r are compared, when sj >s r When s j <s r , the control point coordinate P1' is adjusted in the direction away from the midpoint of the line connecting P0 and P2, and the fine adjustment step size is also set to 0.05.

[0117] The design idea of the above steps is that the lengths of the generated three-segment curves are compared with the real length values of the respective segments of the flexible robot, an error threshold is set, and it is determined whether the length error value meets the requirements. If it meets the requirements, the driving parameters are calculated and output, and if it does not meet the requirements, the auxiliary point P1 is fine-adjusted to meet the requirements.

[0118] More specifically, in the present embodiment, the existing infinitesimal method is used to calculate the virtual length, i.e., the fitting length s j : the formula B j (t)←(1-t) 2 P 2j +2t(t-1)P 2j+1 +t 2 P 2j+2 is used to obtain the discrete points on the jth segment of the three-segment flexible robot, the distances between all adjacent discrete points on a single segment are calculated, and the lengths of the fitted individual segments are calculated.

[0119]

[0120] In further embodiments, the method further includes step 6: if the design requirements are still not met after reaching the given number of iterations in step 5, then step 2 is performed. After steps 1-5 are completed, if none of the target points meet step 5, then step 2 of selecting the RRT path is performed again. When the final target pose is obtained after multiple RRT path selections, the process of each selection is retained, as shown in Figure 7 . In addition, according to the above steps, when the obstacle environment changes, the control strategy proposed by the present application can still achieve good obstacle avoidance effect and pose control effect, as shown in Figure 8 .

[0121] The above disclosure is only the preferred embodiments of the present application, and of course cannot limit the scope of the rights of the present application, so equivalent changes made according to the claims of the present application are still within the scope covered by the present application.

Claims

1. A three-stage flexible robot obstacle avoidance pose control method, characterized in that, The application relates to a three-segment flexible robot obstacle avoidance method, which comprises the following steps: step 1, a virtual obstacle environment is built according to an actual environment scene to obtain a corresponding obstacle map, and a near-end point coordinate of the three-segment flexible robot and a target end point coordinate are marked in the obstacle map; step 2, a random exploration obstacle avoidance algorithm is used to obtain an obstacle avoidance path in the obstacle map according to the target end position of the three-segment flexible robot, that is, the target end point coordinate, and the obstacle avoidance path is used as a rough obstacle avoidance pose of the three-segment flexible robot; step 3, each node in the obstacle avoidance path except a starting point P0 is sequentially grouped into six grouping sets, so that six control points are obtained, and the starting point is used as a control point, so that seven control points are obtained and recorded as P0-P6; a first control point coordinate adjustment strategy is used to determine the coordinates of P0-P2, the three coordinates of P0-P2 are sequentially connected as a control line, and a near-end segment second-order fitting Bezier curve is obtained; step 4, new coordinates of P3-P6 are sequentially determined according to the determined new coordinate of P1, and a middle segment second-order Bezier curve and an end segment second-order fitting Bezier curve are obtained; step 1 comprises the following steps: step 11, actual environment scene information of the three-segment flexible robot is obtained in real time; step 12, a virtual obstacle environment is built according to the actual environment scene to obtain a corresponding obstacle map, that is, a maximum reachable working area of the three-segment flexible robot is recognized according to the actual environment scene, a maximum inscribed cuboid of the maximum reachable working area is obtained and used as an obstacle environment, and obstacles in the actual environment scene are characterized and marked in the obstacle environment, so that an obstacle map is formed; the characterization of the obstacles in the actual environment scene refers to recognizing obstacle marking information of each obstacle in the actual environment scene, and arranging each virtual obstacle in the obstacle environment according to the respective obstacle marking information and using the respective minimum circumscribed cuboid as the virtual obstacle; and step 13, the near-end point coordinate of the three-segment flexible robot and the target end point coordinate are marked in the obstacle map. Step 2 comprises the following steps: step 21, initialization is performed to create an empty tree: the near-end point coordinate is used as a root node of an RRT algorithm, and the target end point coordinate is used as a target point; step 22, random sampling is performed and a nearest neighbor node is found, so that a corresponding extended tree is obtained: a point is randomly generated in a configuration space as a sample point, then a nearest neighbor node is found in an existing tree and is closest to the sample point, and a fixed step is grown from the nearest neighbor node to the sample point to generate a new node; step 23, collision detection is performed: whether the new node collides with obstacles in the obstacle map is checked, if yes, the new node is discarded; otherwise, the new node is added to the existing tree; step 24, a termination condition is set and a path is extracted: whether the new node is close to the end point is judged, if yes, whether the distance between the new node and the end point is less than a set threshold value is further judged, then the algorithm is terminated, and a feasible path from the starting point to the end point is found by backtracking from the end point to the starting point; otherwise, step 25 is executed; and step 25, steps 22-24 are repeated until a maximum iteration number is reached. ​ ​ ​ ​ Step 5, according to the obtained curve length corresponding to the fitting Bezier curve of the proximal segment, the middle segment and the terminal segment of the preceding three-segment flexible robot; compare the obtained curve length with the actual length value of each segment of the proximal segment, the middle segment and the terminal segment of the preceding three-segment flexible robot in sequence, and confirm whether they are all less than the set length error threshold d s If yes, the control parameters are determined according to the current virtual pose, otherwise the new coordinates P1' of the control point P1 are adjusted according to the set control point adjustment strategy and step 4 is re-performed, so as to obtain new control points until the design requirements are met.

2. The three-stage flexible robot obstacle avoidance pose control method of claim 1, wherein, ​ ​ ​ ​ 3. The three-stage flexible robot obstacle avoidance pose control method of claim 1, wherein, ​ ​ ​ ​ ​ ​ 4. The three-stage flexible robot obstacle avoidance pose control method of claim 3, wherein, The aforementioned step 3 comprises: Step 31, sequentially grouping each node in the foregoing obstacle avoidance path except the starting point P0, thereby obtaining 6 node sets, defining the node closest to the end point in each node set as a control point, and defining the starting point as a control point, thereby obtaining 7 control points; the node closest to the end point in each node set is defined as P i , i = (1, 2, 3, 4, 5, 6), then the 7 control points are P0-P6; Step 32, defining a first control point coordinate adjustment strategy, and determining the coordinates of P0-P2 according to the aforementioned first control point coordinate adjustment strategy; The aforementioned first control point coordinate adjustment strategy is: According to the original coordinates of control points P0 and P2 and the first coordinate relationship formula, the new coordinates of control point P1 are reconfirmed; that is, if the original coordinates of control points P0, P1 and P2 satisfy the first coordinate relationship formula, the original coordinates of control point P1 are taken as the new coordinates of control point P1 after reconfirmation; otherwise, the point of the current control point P1 projected on the perpendicular bisector of the line connecting P0 and P2 is taken as the coordinates of control point P1 after reconfirmation, that is, the new P1'; The aforementioned first coordinate relationship formula is ， wherein, d q is a conditional threshold value; Step 33, connecting the aforementioned determined new coordinates of P0-P2 in order as a control line to obtain a second-order fitting Bezier curve of the proximal segment of the three-segment flexible robot.

5. The three-stage flexible robot obstacle avoidance pose control method of claim 4, wherein, The aforementioned step 4 comprises: Step 41, re-determining the new coordinates of P3 according to the aforementioned new coordinates P1' of P1, and the corresponding formula is ， In the above formula, is a proportionality factor for the proximal segment and the intermediate segment of the three-segment flexible robot and ; Step 42, re-determining the new coordinates P4' of P4 according to the aforementioned new coordinates P3' of P3 and the original coordinates of P4, and the corresponding formula is ； Step 43, obtaining a fitting Bezier curve of the middle segment of the three-segment flexible robot according to P2, the coordinates of the re-determined P3, and the new coordinates of P4, that is, P2, P3', P4'; Step 44, re-determining the new coordinates P5' of P5 according to the aforementioned new coordinates P3' of P3 and the new coordinates P4' of P4, and the corresponding formula is ， to redefine the proportionality coefficient, i.e. to redefine the continuous proportionality coefficient of the intermediate segment and the end segment of the three-segment flexible robot: ； Step 45, obtaining the new coordinates P6' of P6 after reconfirmation according to the aforementioned new coordinates P4' of P4, the new coordinates P5' of P5, and the original coordinates of P6, and the corresponding formula is ， Step 46, obtaining a fitting Bezier curve of the terminal segment of the three-segment flexible robot according to the new coordinates P4' of P4, the new coordinates P5' of P5, and the new coordinates P6' of P6.

6. The three-stage flexible robot obstacle avoidance pose control method of claim 5, wherein, The aforementioned step 5 comprises: Step 51, according to the curve lengths corresponding to the fitting Bezier curves of the proximal segment, the middle segment and the terminal segment of the three-segment flexible robot respectively obtained in the foregoing steps; Step 52, sequentially compare the curve length obtained in step 51 with the real length values of the proximal segment, the intermediate segment and the distal segment of the aforementioned three-segment flexible robot, and confirm whether they are all less than the set length error threshold d s If yes, determine the control parameters according to the current virtual pose, otherwise execute step 53; Step 53, adjusting the coordinates of control point P1' according to the set control point adjustment strategy and re-performing step 4 to obtain new P0, P1', P2, P3', P4', P5' and P6', and then obtain the fitting Bezier curves of the proximal segment, the middle segment and the terminal segment of the new three-segment flexible robot and perform step 51; The foregoing control point adjustment strategy is: if , compare s j with s r , when s , adjust the control point coordinate P1' in the direction of approaching the midpoint of the line connecting P0 and P2, and set the fine adjustment step size as 0.05 each time; when s , adjust the control point coordinate P1' in the direction of moving away from the midpoint of the line connecting P0 and P2, and also set the fine adjustment step size as 0.05 each time; wherein s j represents the curve length, i.e. the fitting length, of the fitting Bezier curve corresponding to each of the proximal segment, the intermediate segment and the terminal segment of the flexible robot obtained in step 51, s r represents the actual length value of each segment of the proximal segment, the intermediate segment and the terminal segment of the flexible robot.

7. The three-stage flexible robot obstacle avoidance pose control method of claim 1, wherein, The method further comprises step 6: if the design requirements are still not met after reaching the given number of iterations in step 2, performing step 2.

Images