A method for optimizing path planning of a metamorphic mobile robot

Abstract

The application provides a path planning optimization method for a deformable mobile robot, and specifically comprises the following steps: step S1: simplifying an indoor three-dimensional complex environment into a two-dimensional grid map with gray value information according to a limit state allowing a single-degree-of-freedom deformable wheeled mobile robot to pass, and the color of the grid map corresponds to different obstacle types; step S2: searching nodes by taking the ideal minimum three-dimensional size of the single-degree-of-freedom deformable wheeled mobile robot as a judgment basis, searching a path based on an improved A* algorithm, and obtaining deformable points before and after an obstacle passed by the path; step S3: taking the deformable points as a sub-start point and a sub-end point, searching a path based on the improved A* algorithm, thereby obtaining a sub-path, replacing a corresponding part in the original path with the sub-path, and obtaining a path composed of multiple sub-paths with different z-axis coordinates; step S4: eliminating redundant nodes for each sub-path; and step S5: classifying and optimizing corners in the path by using a B-spline curve.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a path planning optimization method for a deformable mobile robot. Background Art

[0002] Currently, transformable mobile robots are used to implement certain motion functions in specific scenarios due to their excellent maneuverability in complex environments. Path planning is the core of mobile robot control research. The goal of robot path planning is to plan a collision-free path from a starting point to a target point for a transformable mobile robot. The A* algorithm, due to its superior performance and accuracy, is often improved upon to achieve various planning objectives. The traditional A* algorithm inflates obstacles to the robot's safe size before planning, and then plans the robot as a point mass. This method is not suitable for transformable mobile robots with variable sizes. Therefore, it is necessary to design a path planning method for transformable wheeled mobile robots that can leverage the transformable robot's maneuverability in complex environments and effectively reduce the length of the planned path. Furthermore, to ensure smooth operation, an optimization method that considers the transformable robot's motion is designed based on the designed path planning method. Summary of the Invention

[0003] In view of this, the purpose of the present invention is to provide a path planning optimization method for a deformable mobile robot, which simplifies a complex three-dimensional environment into a two-dimensional grid map, taking into account the actual size and steering of the robot during the search process; the overall path planning adopts the idea of ​​planning based on an ideal state and then replanning; for path optimization, the redundant node elimination method is first used to reduce the number of corners and path length, and then the remaining corners are classified and optimized. This optimization scheme is also applicable to robots with turning radius restrictions.

[0004] To achieve the above objectives, the present invention adopts the following technical solution: a path planning optimization method for a deformable mobile robot, specifically comprising the following steps:

[0005] Step S1: Simplify the indoor three-dimensional complex environment into a two-dimensional grid map with grayscale value information according to the limit state that allows the single-degree-of-freedom deformable wheeled mobile robot to pass through, where the colors of the grid map correspond to different obstacle types;

[0006] Step S2: Using the ideal minimum three-dimensional size of the single-degree-of-freedom deformable wheeled mobile robot as a judgment basis, searching for nodes, and performing pathfinding based on the improved A* algorithm to obtain deformable points before and after obstacles along the path;

[0007] Step S3: Using the deformation point as the sub-starting point and the sub-end point, pathfinding is performed based on the improved A* algorithm to obtain a planned sub-path. The sub-path is used to replace the corresponding part of the original path. The obtained path is composed of multiple sub-paths with different z-axis coordinates.

[0008] Step S4: Eliminate redundant nodes on each sub-path;

[0009] Step S5: using B-spline curves to classify and optimize the corners in the path.

[0010] In a preferred embodiment, step S1 specifically includes:

[0011] Based on the fact that a single-degree-of-freedom deformable wheeled mobile robot can change its three-dimensional size to increase its passability in complex obstacle environments, the environment is classified and converted into a two-dimensional grid map with grayscale value information. The black part of the map represents the obstacle area that is completely inaccessible, and the gray part represents the area that the robot can pass through by changing its own three-dimensional size. The gray area is further divided using different grayscale value information according to the maximum passable height of the robot, and the white part represents the passable area; the robot's deformation process is discretized into multiple intermediate states, and multiple obstacle point sets are used to save the horizontal and vertical coordinates of the area points that the robot cannot pass through in each intermediate state, which are used for collision detection when the robot is in this three-dimensional size state.

[0012] In a preferred embodiment, step S2 specifically includes:

[0013] During the planning process, the two-dimensional size and height of the robot are regarded as two independent variables and collision detection is performed separately. The collision detection of height is to detect whether the maximum height allowed to pass corresponding to the grayscale value of the area where the current point is located is greater than the height of the robot's current state. The collision detection of two-dimensional size is to check whether there is an obstacle point corresponding to the robot state located in the two-dimensional size box of the robot. The search for child nodes also needs to determine whether there is a collision during the process of rotating from the current point to the direction of the surrounding eight nodes; the obtained path is planned in the two-dimensional grid map by the ideal three-dimensional size consisting of the minimum two-dimensional size and the minimum height of the robot. At this time, only the black area in the grid map is an obstacle, and other areas are passable. The obtained path is the ideal shortest path;

[0014] Since the robot height corresponding to the actual minimum two-dimensional size is the highest and cannot pass through any gray obstacle, the part of the ideal shortest path that passes through the gray obstacle is not in line with reality and needs to be replanned; since the obtained path is obtained by ideal minimum size planning, its path is the shortest, so only the unrealistic part is replaced and the rest of the path is retained, so a shorter path can be obtained; the deformation point is the point before and after the gray obstacle in the original path that meets the obstacle-free point within the robot's two-dimensional size range corresponding to the gray obstacle height limit.

[0015] In a preferred embodiment, step S3 specifically includes:

[0016] The deformation points before and after a gray obstacle obtained in step S2 are used as the starting and ending points. The size used for planning is the robot size corresponding to the height limit of the gray obstacle. The obstacle point coordinate set is also updated accordingly to improve the A* algorithm to plan sub-paths. The z-axis coordinates are added to the points in the sub-path to represent the operating status of the robot in this section of the path. The sub-path is used to replace the corresponding path segment obtained in step S1. The resulting path is composed of multiple sub-paths with different z-axis coordinates.

[0017] In a preferred embodiment, step S4 specifically includes:

[0018] Each sub-path in the path obtained in step S3 is individually subjected to redundant node elimination so that the size of the robots in each sub-path is the same, which facilitates judgment.

[0019] In a preferred embodiment, in order to reduce the path length and the number of corners, redundant nodes are eliminated to judge the two inflection points in the sub-path segment, and the collision detection size is the robot size corresponding to the z-axis coordinate of the sub-path. If any point in the line connecting inflection point 1 and inflection point 2 meets the no-collision condition, and there is no collision in the process of the robot rotating from the initial posture at inflection point 1 to the direction of the line connecting the two points and from the direction of the line rotating to the initial posture at inflection point 2, then the nodes between inflection point 1 and inflection point 2 are regarded as redundant nodes, and the path from inflection point 1 to inflection point 2 is updated to the line connecting the two, and the z-axis coordinate remains unchanged.

[0020] In a preferred embodiment, step S5 specifically includes:

[0021] Since the path points on the two sides of the corner may have different z-axis changes or remain unchanged, different B-spline optimization schemes are used. The expression of the k+1 order B-spline curve is as follows:

[0022]

[0023] Among them, P i is the coordinate of the control point, N i,k (u) is the basis function of the k-order B-spline, and its recursive formula is as follows:

[0024]

[0025] Where k represents the order of the basis function, i = 0, 1,…, n.

[0026] In a preferred embodiment, if the z-axis coordinates of all points on the two sides of the corner are the same, 4 or 5 control points and a cubic B-spline curve are used to optimize the corner; if there is a change in the z-axis coordinate, 3 control points and a quadratic B-spline curve are used to optimize the corner; the z-axis coordinates of the two sides of the corner are detected separately, and if there is a point with a change in the z-axis coordinate, it is used as one of the endpoints of the curve; otherwise, the midpoint between the inflection point and the adjacent inflection point is used as the endpoint of the curve.

[0027] In a preferred embodiment, for optimizing corners without state changes, in order to make the curve segment as long as possible to facilitate the control of the robot and to make the path shorter, the curve is controlled in a way that the first and last control points are not changed; the initial control point set is

[0028] {p s ,(p s +p c )*Δ,(p c +p e )*Δ,p e} and {p s ,(p s +p c )*Δ,p c ,(p c +p e )*Δ,p e}, where Δ∈(0,1), the distance between the first and last control points and the inflection point is the same, and a set of control points is:

[0029] {(L,0),(ΔL,0),(ΔLcosθ,ΔLsinθ),(Lcosθ,Lsinθ)} or

[0030] {(L,0),(ΔL,0),(0,0),(ΔLcosθ,ΔLsinθ),(Lcosθ,Lsinθ)},

[0031] L is the distance between the first and last control points and the inflection point, θ is the inflection angle, and the curvature of the curve calculated is:

[0032] or

[0033]

[0034] Among them, A, B, C, and D are equations about u, k1 and k2 are the curvatures of the curves generated by 4 and 5 control points respectively, k1 decreases as Δ increases, k2 decreases as Δ increases when Δ∈(0,0.57), and increases as Δ increases when Δ∈[0.57,1). When the curvature increases, the curve approaches the original corner, and when the curvature decreases, the curve moves away from the original corner. It is found that k1(Δ=0)=k2(Δ=0.5), when k1(Δ=1), the curve is the line connecting the first and last control points, and when k2(Δ=0), the curve coincides with the original corner path, so we use a 4-control point solution to control Δ from 1 to 0, and then use a 5-control point solution to control Δ from 0.5 to 0, and we can The control curve gradually approaches the original corner to achieve the obstacle avoidance effect; at the same time, when targeting a robot with a turning radius limit, we can pre-calculate the Δ value, and substitute the obtained curve as the extreme position close to the original corner as boundary 2, set the optimal turning radius, and calculate the curve obtained by Δ as the optimal curve as boundary 1. The control curve is from boundary 1 to boundary 2 until the curve meets the collision-free condition; if the curve changes to coincide with boundary 2 and still cannot meet the collision-free condition, then update the head and tail control points, that is, change the head and tail control points in the original corner closer to the turning point, and change the value of Δ to control the curve so that the maximum curvature in the curve does not exceed the curvature corresponding to the minimum turning radius of the robot, until a collision-free curve is generated.

[0035] In a preferred embodiment, for optimizing the corners with state changes, in order to facilitate the updating of control points, a three-control point solution is adopted for optimization, and the initial control point set is {p s ,p c ,p e}, where p c is the inflection point, p s and p e are the points with smaller and larger z-axis coordinates in the two nodes, respectively. Because for a single-degree-of-freedom deformable wheeled mobile robot, the lower the height, the larger its two-dimensional size. Therefore, the two-dimensional size of the robot gradually decreases in the optimization curve, which is convenient for collision detection. In the optimization process, a deformable rectangular box is used to represent the change of the two-dimensional size of the robot. Every time a curve point is generated, it is judged whether there is an obstacle point in the rectangular box of the point. The area corresponding to the actual three-dimensional size of the robot that cannot pass through the rectangular box is an obstacle. The obstacle point set is updated while the rectangular box changes. The curve is generated with 3 control points. The initial rectangular box is point p s The two-dimensional size of the robot corresponding to the z-axis coordinate is generated. Each time a curve point is generated, a collision check is performed on it. When there is an obstacle point in the rectangular box where the curve point is located, the rectangular box is reduced, and the corresponding obstacle point set will also change. Update p s For the last curve point that meets the obstacle-free condition, update p c The tangent direction of the obtained curve and the previous p c、p e The intersection of the lines to ensure the continuity of the generated curve, p e remains unchanged until the generated curve point is equal to p e If the rectangular box is reduced to the robot's two-dimensional size corresponding to the z-axis coordinate at pe and still cannot generate a valid curve, delete all curve points and change the p in the initial control point to s or p e Update to a point along the corner edge that approaches the inflection point, and repeat the above steps until a valid curve is generated. At this point, the deformed rectangular box forms an outline. In the actual process, as long as the two-dimensional dimensions of the robot are within the outline and the height meets the requirements, the robot can run smoothly along this curve.

[0036] Compared with the prior art, the present invention has the following beneficial effects:

[0037] The present invention simplifies a complex three-dimensional environment into a two-dimensional grid map. During the entire process, the three-dimensional size of the robot is regarded as two independent variables, namely the height and the two-dimensional size, for collision detection, thereby improving search efficiency.

[0038] The present invention takes advantage of the shorter path planned by the A* algorithm. On the basis of planning the ideal shortest path with the ideal optimal three-dimensional size, the ideal shortest path is retained as much as possible, and the unreasonable parts are replanned to make the total path shorter.

[0039] Corners are classified and optimized through B-spline curves. When optimizing corners without state changes, the robot's optimal turning radius is given priority, and the robot's minimum turning radius is used as a constraint for optimization. When optimizing corners with state changes, the boundary is composed of a deformed rectangular frame. The robot's range of activity is larger, which facilitates the robot's control. The entire optimization scheme is reliable and easy to implement. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 A schematic flow chart of a path planning and path optimization method according to a preferred embodiment of the present invention;

[0041] Figure 2 A schematic diagram of converting a three-dimensional map into a two-dimensional grid map according to a preferred embodiment of the present invention;

[0042] Figure 3 Collision detection and node search in a preferred embodiment of the present invention, wherein (a) is a schematic diagram of robot collision detection, and (b) is a schematic diagram of robot subnode search;

[0043] Figure 4 A schematic diagram of a path generated by the improved A* algorithm according to a preferred embodiment of the present invention;

[0044] Figure 5A schematic diagram of eliminating redundant nodes according to a preferred embodiment of the present invention;

[0045] Figure 6 Schematic diagram of stateless corner optimization according to a preferred embodiment of the present invention; wherein, (a) is a 4-control-point solution, where the first and last control points remain unchanged, and the Δ value is changed to generate a comparison graph of the curves; (b) is a 5-control-point solution, where the first and last control points remain unchanged, and the Δ value is changed to generate a comparison graph of the curves; and (c) is a solution with moving the first and last control points.

[0046] Figure 7 A schematic diagram of stateful corner optimization according to a preferred embodiment of the present invention;

[0047] Figure 8 This is a diagram of algorithm simulation results for a preferred embodiment of the present invention. DETAILED DESCRIPTION

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

[0049] It should be noted that the following detailed descriptions are illustrative and intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the art to which the present application belongs.

[0050] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application; as used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form, and it should be understood that when the terms "comprise" and / or "include" are used in this specification, they indicate the presence of features, steps, operations, devices, components and / or their combinations.

[0051] refer to Figure 1-8 The path planning optimization method for a deformable mobile robot described in this embodiment specifically includes: step S1, simplifying a three-dimensional complex indoor environment into a two-dimensional grid map with grayscale value information according to the limit state allowing a single-degree-of-freedom deformable wheeled mobile robot to pass through, where the colors of the grid map correspond to different obstacle types; step S2, searching for nodes based on the ideal minimum three-dimensional size of the single-degree-of-freedom deformable wheeled mobile robot as a judgment basis, performing pathfinding based on an improved A* algorithm, and obtaining deformable points before and after the obstacles passed by the path; step S3, using the deformed points as sub-starting points and sub-end points, performing pathfinding based on the improved A* algorithm, thereby obtaining a planned sub-path, replacing the corresponding part of the original path with the sub-path, and the obtained path is composed of multiple sub-paths with different z-axis coordinates; step S4, eliminating redundant nodes for each sub-path; and step S5, classifying and optimizing the corners in the path using a B-spline curve.

[0052] Step S1: simplify the indoor three-dimensional complex environment into a two-dimensional grid map with grayscale value information according to the limit state that allows the single-degree-of-freedom deformable wheeled mobile robot to pass through, and the colors of the grid map correspond to different obstacle types.

[0053] like Figure 2 As shown in the figure, the three-dimensional complex environment is converted into a two-dimensional grid map with grayscale value information. The black part of the map is the area that the robot cannot pass through in any state, with a grayscale value of 0, the white part is the passable area, with a grayscale value of 255, and the gray part is the area where the robot is allowed to pass in a fully expanded state, with a grayscale value of 150; when there are many types of obstacles in the environment, we can subdivide the gray part according to the maximum height allowed for the robot to pass through, and the grayscale value is represented by 1-254. The higher the height allowed to pass, the smaller the grayscale value.

[0054] Step S2: Using the ideal minimum three-dimensional size of a single-degree-of-freedom deformable wheeled mobile robot as a basis for searching nodes, performing pathfinding based on the improved A* algorithm, and obtaining deformable points before and after obstacles along the path.

[0055] Figure 3 (a) is a schematic diagram of the robot's collision detection. o1 and o2 represent two different types of obstacles. o1 allows the robot to pass through in its fully deployed state, while o2 does not allow the robot to pass through in any state. The rectangular box in the middle is the two-dimensional size of the robot. If the robot is in its fully deployed state, the area o1 is a passable area, and only the coordinate points in obstacle o2 are added to the corresponding obstacle point set.

[0056] Figure 3 (b) is a schematic diagram of the robot subnode search. In addition to detecting that the subnode has no collision in its posture, it is also necessary to detect that the robot has no collision in the process of turning from the current point θ1 to θ2;

[0057] The curve generated based on the ideal minimum three-dimensional size is as follows Figure 4 As shown in the black path, a shorter path can be obtained by preserving the black path as much as possible during replanning. Because the actual robot's minimum 2D height cannot pass through the gray obstacle area, the path segments near the gray obstacle are replanned. Points s1, s2, s3, and s4 in the figure are the deformation points before and after the gray obstacle that the black path passes through.

[0058] S3, using the deformation point as the sub-starting point and sub-end point, and performing path finding based on the improved A* algorithm to obtain the planned sub-path, and replacing the corresponding part of the original path with the sub-path. The obtained path is composed of multiple sub-paths with different z-axis coordinates;

[0059] by Figure 4Replanning is performed using s1, s2, s3, and s4 as starting or ending points. For example, for the first gray obstacle, s1 is the sub-starting point and s2 is the sub-endpoint. Planning is performed based on the robot's fully unfolded dimensions, and the resulting path is represented by the dotted line. Since the area between the two gray obstacles does not restrict the robot's height, planning based on the robot's minimum two-dimensional dimensions yields the optimal path. Find the first deformation point after the gray obstacle in the dotted line, such as point c1 in the figure. Using c1 as the sub-starting point and s3, the deformation point before the next gray obstacle, as the sub-endpoint, planning is performed based on the robot's fully folded state to obtain the resulting path, represented by the dotted line. Repeat this process for each path segment near each gray obstacle. The final path segment uses the last deformation point as the sub-starting point and the path endpoint as the sub-endpoint. The path is planned based on the robot's fully folded state, represented by a dashed line. The portion of the original path before s1 is combined with the dashed line to form a more optimal path from the start point to the end point. The height information of the dashed line segments s1→c1 and s3→c2 corresponds to the height information corresponding to the gray obstacle's grayscale value at that location. The height information of the remaining path segments corresponds to the robot's fully folded state. The deformation points in the path are changed to s1, s3, c1, and c2. There is a special case where the area within the gray obstacle is relatively open, which is still passable even with the robot's fully unfolded dimensions. Therefore, s1 is used as the sub-starting point and s4 as the sub-endpoint. The path is planned based on the robot's fully folded state, represented by a dotted line. The deformation point c3 after the last gray obstacle is used as the sub-starting point and the path endpoint as the sub-endpoint. The path is planned based on the robot's fully folded state, represented by a dotted line. The resulting path is the portion of the original path before s1 plus the dotted line segment. Compare the lengths of the two paths and take the shortest one. If the two paths are the same length, the latter option is chosen because the robot's state changes less in the dotted-dash segment of the original path.

[0060] S4, eliminate redundant nodes for each sub-path;

[0061] Figure 5 This is a schematic diagram of eliminating redundant nodes on a subpath. In the figure, s1 and s2 are two inflection points in the path segment, and the dashed line connects the two points. The solid boxes represent the robot's posture at points s1 and s2 in the original path, with heading angles θ1 and θ3. The dashed boxes represent the robot's posture to be tested at points s1 and s2, with heading angle θ2. Eliminating redundant nodes between two points requires not only determining whether the robot can avoid collisions along the dashed line from s1 to s2 in the posture indicated by the dashed boxes, but also determining whether the robot can avoid collisions when its heading angle changes from θ1 to θ2 at s1 and from θ2 to θ3 at s2, similar to the subnode search strategy.

[0062] S5, using B-spline curve to classify and optimize the corners in the path;

[0063] Figure 6 This is a schematic diagram of some curves in the stateless corner optimization solution. Figure 6 (a) is a 4-control point solution. The first and last control points remain unchanged, and the Δ value is changed. The comparison of the generated curves shows that k1 decreases with the increase of Δ. The smaller Δ is, the closer the curve is to the original corner path. However, even if Δ is reduced to 0, the curve is still far from the original corner path. Figure 6 (b) shows the 5-control-point solution. The first and last control points remain unchanged, but the Δ value is changed. A comparison of the generated curves shows that the curve gradually changes to coincide with the original corner path. Therefore, by combining the 4-control-point and 5-control-point solutions, the curve can be controlled to change from the line connecting the first and last control points to coincide with the original corner path, achieving obstacle avoidance. Figure 6 (c) is the solution of moving the head and tail control points. It can be seen that the curve can also be controlled to approach the original corner. When the robot has a turning radius limit, this solution is used when the above solution cannot be implemented. When the head and tail control points are moved, the Δ value is calculated at the same time to ensure that the maximum curvature of the curve does not exceed the turning curvature limit of the robot;

[0064] Figure 7 This is a state-variable corner optimization solution. The black straight line in the figure is the original corner before optimization. In order to increase the curved path to facilitate robot movement and reduce the total path length, A, B, and C are used as the three initial control points to generate a B-spline curve. Assuming that point A has the smallest Z-axis coordinate among the three control points, that is, the robot's two-dimensional size is the largest at point A, this state is used as the basis for collision detection to gradually judge the generated curve points;

[0065] like Figure 7 As shown in (a), A' is the last point in the curve generated by the control point based on the current two-dimensional size that meets the collision detection condition. The control points are updated to A', B', and C, reducing the two-dimensional size of the judgment. At the same time, the obstacle point set is updated to generate a new curve point. B' is the intersection of the tangent of the curve segment AA' at A' and the straight line segment BC to maintain the continuity of the two adjacent curve segments. Finally, a curve composed of multiple curve segments can be generated as shown in Figure 7 As shown in (b), the curve segments of different colors represent different and gradually decreasing two-dimensional sizes of collision detection. Figure 7 As shown in (c), the gray outline consists of the area occupied by the two-dimensional dimensions of each curve point that meets the collision detection requirement. In practice, as long as the two-dimensional dimensions corresponding to each curve point are within the gray outline, the robot will not collide with the black obstacle. In particular, when the two-dimensional dimensions used as the judgment basis are reduced to the two-dimensional dimensions corresponding to the robot's fully folded state and still no complete curve is generated, all curve points are deleted and point A is moved to point A1. Point A1 is the point obtained by moving point A one unit length on line segment AB. The initial control points are then A1, B, and C. Repeat the above steps.

[0066] If the complete curve cannot be generated when point A1 moves to the point where it is about to leave line segment AB, then Figure 7 As shown in (d), the black obstacles are considered separately from other obstacles. Since the gray obstacles in the original path segment have a greater impact on the optimization, they are excluded from the curve control. o1 and o2 are the gray obstacles closest to point B among the straight line segments AB and BC. The control points A2 and C2 are the points on the straight line segment with distances d1 and d2 from o1 and o2, where d1 and d2 are the safety distances of obstacles o1 and o2. The optimization scheme is selected at the path segment A2-B-C2 based on the grayscale values ​​corresponding to the areas where o1 and o2 are located.

[0067] Figure 8 The simulation results of the traditional A* algorithm for generating paths and the proposed method for optimizing the path planning of a deformable mobile robot are compared. Figure 8 (ab) is the path planned by the traditional A* algorithm in two scenarios with the robot in a fully deployed state. Figure 8 (cd) is the path planned by the traditional A* algorithm in two scenarios with the robot in a fully folded state. Figure 8 (ef) is the path planned by the improved A* algorithm in two scenarios. Figure 8 (gh) is the path after redundant nodes are eliminated, Figure 8 (ij) is the path after path optimization, Figure 8 (k, l) is the three-dimensional perspective diagram of the optimized path. Table 1 shows the length comparison. It can be found that the path planning optimization method for a deformable mobile robot proposed in this invention is relatively easy to implement, and the path length is shorter than that of the traditional A* algorithm. It successfully utilizes the good passability of the single-degree-of-freedom deformable mobile robot in complex environments.

[0068]

[0069] The above description is a preferred embodiment of the present invention. Any improvements and adjustments made by ordinary technicians in this technical field without departing from the principles of the present invention are also within the scope of protection of the present invention.

Claims

1. A path planning optimization method for a deformable mobile robot, characterized in that: The specific steps include: Step S1: Simplify the indoor three-dimensional complex environment into a two-dimensional grid map with grayscale value information according to the limit state that allows the single-degree-of-freedom deformable wheeled mobile robot to pass through, where the colors of the grid map correspond to different obstacle types; Step S2: Using the ideal minimum three-dimensional size of the single-degree-of-freedom deformable wheeled mobile robot as a judgment basis, searching for nodes, and performing pathfinding based on the improved A* algorithm to obtain deformable points before and after obstacles along the path; Step S3: Using the deformation point as the sub-starting point and the sub-end point, pathfinding is performed based on the improved A* algorithm to obtain a planned sub-path. The sub-path is used to replace the corresponding part of the original path. The obtained path is composed of multiple sub-paths with different z-axis coordinates. Step S4: Eliminate redundant nodes on each sub-path; Step S5: using B-spline curves to classify and optimize the corners in the path; Step S1 specifically includes: Based on the fact that a single-degree-of-freedom deformable wheeled mobile robot can change its three-dimensional size to increase its passability in a complex obstacle environment, the environment is classified and converted into a two-dimensional grid map with grayscale value information. The black part of the map represents the obstacle area that is completely impassable, and the gray part represents the area that the robot can pass through by changing its own three-dimensional size. The gray part is further divided into different grayscale value information according to the maximum passable height of the robot, and the white part represents the passable area. The robot's deformation process is discretized into multiple intermediate states, and multiple obstacle point sets are used to store the horizontal and vertical coordinates of the points in the area that the robot cannot pass through in each intermediate state, which are used for collision detection of the robot in this three-dimensional state. Step S2 specifically includes: During the planning process, the two-dimensional size and height of the robot are regarded as two independent variables and collision detection is performed separately. The collision detection of height is to detect whether the maximum height allowed to pass corresponding to the grayscale value of the area where the current point is located is greater than the height of the robot's current state. The collision detection of two-dimensional size is to check whether there is an obstacle point corresponding to the robot state located in the two-dimensional size box of the robot. The search for child nodes also needs to determine whether there is a collision during the process of rotating from the current point to the direction of the surrounding eight nodes; the obtained path is planned in the two-dimensional grid map by the ideal three-dimensional size consisting of the minimum two-dimensional size and the minimum height of the robot. At this time, only the black area in the grid map is an obstacle, and other areas are passable. The obtained path is the ideal shortest path; Since the robot height corresponding to the actual minimum two-dimensional size is the highest and cannot pass through any gray obstacle, the part of the ideal shortest path that passes through the gray obstacle is not in line with reality and needs to be replanned; since the obtained path is obtained by ideal minimum size planning, its path is the shortest, so only the unrealistic part is replaced and the rest of the path is retained, so a shorter path can be obtained; the deformation point is the point before and after the gray obstacle in the original path that meets the obstacle-free point within the robot's two-dimensional size range corresponding to the gray obstacle height limit.

2. A path planning optimization method for a deformable mobile robot according to claim 1, characterized in that: Step S3 specifically includes: The deformation points before and after a gray obstacle obtained in step S2 are used as the starting and ending points. The size used for planning is the robot size corresponding to the height limit of the gray obstacle. The obstacle point coordinate set is also updated accordingly to improve the A* algorithm to plan sub-paths. The z-axis coordinates are added to the points in the sub-path to represent the operating status of the robot in this section of the path. The sub-path is used to replace the corresponding path segment obtained in step S1. The resulting path is composed of multiple sub-paths with different z-axis coordinates.

3. The path planning optimization method for a deformable mobile robot according to claim 1, characterized in that: Step S4 specifically includes: Each sub-path in the path obtained in step S3 is individually subjected to redundant node elimination so that the size of the robots in each sub-path is the same, which facilitates judgment.

4. A path planning optimization method for a deformable mobile robot according to claim 3, characterized in that: In order to reduce the path length and the number of corners, redundant nodes are eliminated to judge the two inflection points in the sub-path segment. The collision detection size is the robot size corresponding to the z-axis coordinate of the sub-path. If any point in the line connecting inflection point 1 and inflection point 2 meets the collision-free condition, and there is no collision in the process of the robot rotating from the initial posture at inflection point 1 to the direction of the line connecting the two points and from the direction of the line rotating to the initial posture at inflection point 2, then the nodes between inflection point 1 and inflection point 2 are considered redundant nodes, and the path from inflection point 1 to inflection point 2 is updated to the line connecting the two points, and the z-axis coordinate remains unchanged.

5. The path planning optimization method for a deformable mobile robot according to claim 1, characterized in that: Step S5 specifically includes: Since the path points on the two sides of the corner may have different z-axis changes or remain unchanged, different B-spline optimization schemes are used. The expression of the k+1 order B-spline curve is as follows: Among them, P i is the coordinate of the control point, N i,k (u) is the basis function of the k-order B-spline, and its recursive formula is as follows: Where k represents the order of the basis function, i = 0, 1,…, n.

6. A path planning optimization method for a deformable mobile robot according to claim 5, characterized in that: If the z-axis coordinates of all points on the two sides of the corner are the same, 4 or 5 control points and a cubic B-spline curve are used to optimize the corner; if there is a change in the z-axis coordinate, 3 control points and a quadratic B-spline curve are used to optimize the corner; the z-axis coordinates of the two sides of the corner are detected separately. If there is a point with a change in the z-axis coordinate, it is used as one of the endpoints of the curve. Otherwise, the midpoint between the inflection point and the adjacent inflection point is used as the endpoint of the curve.

7. A path planning optimization method for a deformable mobile robot according to claim 6, characterized in that: For optimizing corners without state changes, in order to make the curve segment as long as possible to facilitate the control of the robot and to make the path shorter, the curve is controlled in a way that the first and last control points are not changed; the initial control point set is {p s ,(p s +p c )*Δ,(p c +p e )*Δ,p e } and {p s ,(p s +p c )*Δ,p c ,(p c +p e )*Δ,p e }, where Δ∈(0,1), the distance between the first and last control points and the inflection point is the same, and a set of control points is: {(L,0),(ΔL,0),(ΔLcosθ,ΔLsinθ),(Lcosθ,Lsinθ)} or {(L,0),(ΔL,0),(0,0),(ΔLcosθ,ΔLsinθ),(Lcosθ,Lsinθ)}, L is the distance between the first and last control points and the inflection point, θ is the inflection angle, and the curvature of the curve calculated is: or Among them, A, B, C, and D are equations about u, k1 and k2 are the curvatures of the curves generated by 4 and 5 control points respectively, k1 decreases as Δ increases, k2 decreases as Δ increases when Δ∈(0,0.57), and increases as Δ increases when Δ∈[0.57,1). When the curvature increases, the curve approaches the original corner, and when the curvature decreases, the curve moves away from the original corner. It is found that k1(Δ=0)=k2(Δ=0.5), when k1(Δ=1), the curve is the line connecting the first and last control points, and when k2(Δ=0), the curve coincides with the original corner path, so we use a 4-control point solution to control Δ from 1 to 0, and then use a 5-control point solution to control Δ from 0.5 to 0, and we can The control curve gradually approaches the original corner to achieve the obstacle avoidance effect; at the same time, when targeting a robot with a turning radius limit, we can pre-calculate the Δ value, and substitute the obtained curve as the extreme position close to the original corner as boundary 2, set the optimal turning radius, and calculate the curve obtained by Δ as the optimal curve as boundary 1. The control curve is from boundary 1 to boundary 2 until the curve meets the collision-free condition; if the curve changes to coincide with boundary 2 and still cannot meet the collision-free condition, then update the head and tail control points, that is, change the head and tail control points in the original corner closer to the turning point, and change the value of Δ to control the curve so that the maximum curvature in the curve does not exceed the curvature corresponding to the minimum turning radius of the robot, until a collision-free curve is generated.

8. The path planning optimization method for a deformable mobile robot according to claim 6, characterized in that: For the optimization of corners with state changes, in order to facilitate the update of control points, a three-control point solution is used for optimization, and the initial control point set is {p s ,p c ,p e }, where p c is the inflection point, p s and p e are the points with smaller and larger z-axis coordinates in the two nodes, respectively. Because for a single-degree-of-freedom deformable wheeled mobile robot, the lower the height, the larger its two-dimensional size. Therefore, the two-dimensional size of the robot gradually decreases in the optimization curve, which is convenient for collision detection. In the optimization process, a deformable rectangular box is used to represent the change of the two-dimensional size of the robot. Every time a curve point is generated, it is judged whether there is an obstacle point in the rectangular box of the point. The area corresponding to the actual three-dimensional size of the robot that cannot pass through the rectangular box is an obstacle. The obstacle point set is updated while the rectangular box changes. The curve is generated with 3 control points. The initial rectangular box is point p s The two-dimensional size of the robot corresponding to the z-axis coordinate is generated. Each time a curve point is generated, a collision check is performed on it. When there is an obstacle point in the rectangular box where the curve point is located, the rectangular box is reduced, and the corresponding obstacle point set will also change. Update p s For the last curve point that meets the obstacle-free condition, update p c The tangent direction of the obtained curve and the previous p c 、p e The intersection of the lines to ensure the continuity of the generated curve, p e remains unchanged until the generated curve point is equal to p e coincide; If a valid curve cannot be generated after the rectangular box is reduced to the robot's two-dimensional size corresponding to the z-axis coordinate at pe, all curve points are deleted and the p in the initial control point is changed to s or p e Update to a point along the corner edge that approaches the inflection point, and repeat the above steps until a valid curve is generated. At this point, the deformed rectangular box forms an outline. In the actual process, as long as the two-dimensional dimensions of the robot are within the outline and the height meets the requirements, the robot can run smoothly along this curve.

Images