Method and device for rs vehicle routing planning based on piecewise linear mixed integer programming

Abstract

The application relates to a RS vehicle path planning method and device based on a segmented linear mixed integer programming. The method comprises the following steps: acquiring initial state information of a curvature-limited vehicle. A heading angle corresponding to a middle point position is set as a free variable to be optimized to form a middle configuration. According to a RS vehicle kinematics model allowing forward and reverse movement, a five-letter path template is used to construct a first sub-path from a starting configuration to the middle configuration and a second sub-path from the middle configuration to a target configuration, and the whole path is represented as a combination of the two five-letter path templates. A mixed integer segmented linear programming model with the minimum total path length as the target is constructed. Three mechanism constraints are applied. An optimization solver is called to solve the mixed integer segmented linear programming model, and the optimal middle point heading angle, path segment parameters and the whole optimal path are obtained. The method improves the accuracy, efficiency and global optimality of the three-point path planning of the RS vehicle.

Description

Technical Field

[0001] This application relates to the field of vehicle motion control technology, and in particular to an RS vehicle path planning method and apparatus based on piecewise linear mixed integer programming. Background Technology

[0002] Curvature-constrained path planning is one of the core research directions in the field of autonomous robot navigation, widely used in tasks such as surveillance, inspection, coverage, and transportation. The Dubins vehicle model, which only allows forward motion and has a limited turning radius, laid an important foundation for this field with its shortest path theory. Dubins' classic results show that the shortest path between two known configurations must belong to CSC, CCC, or their degenerate forms, where... This represents the minimum radius left / right turn arc segment. This represents a line segment. The theory has been extended through Pontryagin's maximum principle and differential geometry methods, leading to the development of various efficient computational algorithms.

[0003] The Reeds-Shepp (RS, Shortest Path Analytical Solution) model, building upon the Dubins model, allows vehicles to move backward, better reflecting the actual motion characteristics of unmanned ground vehicles (UGVs) and unmanned surface vessels (USVs). The optimal path between two configurations in this model consists of 48 canonical paths, including forward and reverse circular arcs and special paths. Turning primitives. Although there has been work in recent years to accelerate computing and expand applications, the research focus is still limited to two issues.

[0004] When the target heading is no longer fixed, a two- or three-point free-heading Dubins problem arises, known as the Relaxed Dubins Problem (RDP). In an unobstructed environment, this problem can be solved using geometric optimization methods. However, when Reeds-Shepp bidirectional motion is introduced and the free heading is placed at the intermediate point, the problem becomes significantly more difficult: the periodicity of the heading variable is strongly coupled with the Reeds-Shepp constraints, making exact optimization extremely challenging. Existing methods relying on fixed headings cannot be directly generalized to this scenario, nor can they handle the joint minimization of the two-way path from the starting point to the intermediate point to the destination.

[0005] For Dubins vehicles that are only allowed to move forward, existing research has proposed the three-point Dubins problem, which minimizes the total path length between the starting point, intermediate point, and ending point by optimizing the heading at the intermediate point. These methods typically rely on geometric analysis or necessary optimality conditions and are mainly limited to Dubins kinematic models.

[0006] However, existing modeling and methods for the three-point Dubins problem cannot be directly extended to the Reeds-Shepp (RS) case. The main reason is that the introduction of backward motion leads to an explosive increase in the number of path types. Furthermore, existing Reeds-Shepp methods almost entirely focus on two-point problems or fixed-heading problems, failing to handle the free headway variable at the intermediate point, which is coupled with the travel distances of two paths. Therefore, the three-point Reeds-Shepp problem (3PRSP) with a free intermediate headway has long remained unsolved, lacking both analytical solutions and optimal modeling methods for the system.

[0007] The three-point Reeds-Shepp problem with a free intermediate heading inherits the dual difficulties of Reeds-Shepp path planning and three-point Dubins free heading optimization: path type selection requires integer variables, the constraint set is non-convex and periodic due to congruence relations, traditional gradient-based methods are no longer applicable, and the objective function involves the joint minimization of two-way path lengths. Therefore, no method can currently solve both simultaneously. 1. Two-way Reeds-Shepp vehicle kinematics; 2. Joint optimization problem of two-way paths with free intermediate heading.

[0008] In summary, the key challenge hindering 3PRSP research lies in the difficulty of simultaneously characterizing the strong coupling relationship between path type selection and free heading angle optimization; furthermore, , Introducing trigonometric function constraints directly into the optimization model will significantly increase computational complexity and pose new challenges to the solver. Summary of the Invention

[0009] Therefore, it is necessary to provide an RS vehicle path planning method and apparatus based on piecewise linear mixed integer programming that can improve the accuracy, efficiency and global optimality of RS vehicle three-point path planning in order to address the above-mentioned technical problems.

[0010] A method for RS vehicle routing based on piecewise linear mixed integer programming, the method comprising: Obtain the initial configuration, midpoint position, and target configuration of the curvature-constrained vehicle.

[0011] Set the heading angle corresponding to the midpoint position as the free variable to be optimized to form the intermediate configuration.

[0012] Based on the RS vehicle kinematic model that allows forward and backward motion, a first sub-path from the starting configuration to the intermediate configuration and a second sub-path from the intermediate configuration to the target configuration are constructed using five-letter path templates, and the overall path is represented as a combination of the two five-letter path templates.

[0013] Construct a mixed-integer piecewise linear programming model with the objective of minimizing the total path length.

[0014] Apply kinematic constraints, configuration matching constraints, and piecewise linear approximation constraints to the mixed integer piecewise linear programming model.

[0015] The optimization solver is invoked to solve the mixed integer piecewise linear programming model, and the optimal intermediate point heading angle, parameters of each path segment, and overall optimal path are obtained.

[0016] An RS vehicle routing device based on piecewise linear mixed integer programming, the device comprising: The input module is used to obtain the initial configuration, intermediate point position, and target configuration of the curvature-constrained vehicle; The model building module is used to set the heading angle corresponding to the intermediate point position as the free variable to be optimized in order to form an intermediate configuration.

[0017] The path standardization construction module is used to construct the first sub-path from the starting configuration to the intermediate configuration and the second sub-path from the intermediate configuration to the target configuration respectively, based on the RS vehicle kinematic model that allows forward and backward motion, using five-letter path templates, and to represent the overall path as a combination of the two five-letter path templates.

[0018] The model building module is used to construct a mixed-integer piecewise linear programming model with the objective of minimizing the total path length.

[0019] The constraint module is used to apply kinematic constraints, configuration matching constraints, and constraints on piecewise linear approximations of trigonometric functions to mixed-integer piecewise linear programming models.

[0020] The path optimization module is used to call the optimization solver to solve the mixed integer piecewise linear programming model and obtain the optimal intermediate point heading angle, parameters of each path segment, and the overall optimal path.

[0021] The aforementioned RS vehicle path planning method and apparatus based on piecewise linear mixed integer programming firstly acquires the vehicle's initial configuration, intermediate point positions, and target configuration at the perception level, explicitly setting the intermediate point heading angle as a free variable to be optimized. This fully perceives the spatial constraints and degrees of freedom of the task, laying an information foundation for subsequent joint optimization. Secondly, at the measurement level, a five-letter path template (such as CCSCC) is used to construct the first and second sub-paths respectively, and the overall path is represented as a combination of the two templates. This maps discrete RS optimal path types of various kinds into a unified parameterized structure. Simultaneously, piecewise linear approximation constraints are applied to the trigonometric functions, transforming the nonlinear sinθ and cosθ into measurable piecewise linear functions. This quantifies the numerical relationship between path curvature, heading change, and total length, overcoming the deficiency of traditional methods in directly measuring nonlinear constraints. Building upon this foundation, at the decision-making level, a mixed-integer piecewise linear programming model is constructed with the objective of minimizing the total path length. Kinematic constraints and configuration matching constraints are applied simultaneously, allowing real-time feedback of intermediate heading adjustments to the length calculation of each candidate path type. This achieves joint decision-making for "discrete path type selection" and "continuous intermediate heading optimization," avoiding local suboptimal results caused by abrupt heading changes in the discretization cascade method. Finally, at the reconstruction level, an optimization solver is invoked to solve the model, outputting the optimal intermediate heading angle, path segment lengths, gear positions, and steering parameters. Based on this, a continuous RS path from the starting point through the intermediate point to the endpoint, satisfying curvature constraints and having the shortest total length, is reconstructed. This path allows for forward and backward movement, and each segment can automatically degenerate into a shorter structure. In summary, this method fundamentally solves the technical problems of existing three-point Dubins methods being unable to reverse, discretized two-point RS methods having discontinuous intermediate point headings, and nonlinear constraints being difficult to incorporate into the optimization framework. It effectively suppresses path detours and local redundancy, and achieves a leap from static geometric dependence to dynamic mixed-integer programming decision-making, thereby significantly improving the accuracy, efficiency, and global optimality of RS vehicle three-point path planning. Attached Figure Description

[0022] Figure 1 This is a flowchart illustrating an RS vehicle routing method based on piecewise linear mixed integer programming in one embodiment. Figure 2 This is a flowchart illustrating the Reeds-Shepp vehicle three-point problem path planning method based on piecewise linear mixed integer programming in one embodiment. Figure 3 This is a schematic diagram of path segmentation in one embodiment; Figure 4 Analysis for one embodiment Linear approximation diagram; Figure 5This is a schematic diagram of a three-point Reeds-Shepp path planning in one embodiment. Figure 6 This is a structural block diagram of an RS vehicle routing device based on piecewise linear mixed integer programming in one embodiment. Detailed Implementation

[0023] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0024] In one embodiment, such as Figure 1 As shown, an RS vehicle routing method based on piecewise linear mixed integer programming is provided, including the following steps: Step 102: Obtain the initial configuration, intermediate point position, and target configuration of the curvature-constrained vehicle.

[0025] Specifically, the shortest path for a vehicle or robot with a minimum turning radius constraint, starting from the initial configuration, passing through a preset intermediate position, and reaching the target configuration, is collected by sensors configured on the vehicle, including the current initial configuration, intermediate position, and target configuration. The initial configuration is denoted as... The target configuration is denoted as The middle position is denoted as .

[0026] in, These represent the planar coordinates of the starting point, Indicates the heading angle at the starting point; These represent the planar coordinates of the target point, Indicates the heading angle of the target point; Represents the planar coordinates of the midpoint. Midpoint heading angle. Since it is not given in advance, but participates in the solution as a variable to be optimized, the intermediate configuration can be expressed as follows: .

[0027] Step 104: Set the heading angle corresponding to the midpoint position as the free variable to be optimized to form the intermediate configuration.

[0028] Specifically, the Reeds-Shepp vehicle kinematics model is used to describe vehicle motion. To facilitate modeling, the vehicle parameters are normalized without loss of generality, ensuring the minimum turning radius satisfies... The vehicle speed modulus is taken as 1 (ignored in the formula). The vehicle gear variable is denoted as... The value is ,in Indicates moving forward. Indicates reversing. The vehicle steering variable is denoted as... The value is ,in Indicates a right turn. Indicates going straight. This indicates a left turn. The corresponding normalized kinematic equation is:

[0029] Since the normalized radius can be normalized without loss, The last line of the above formula can be further written as:

[0030] Step 106: Based on the RS vehicle kinematic model that allows forward and backward motion, construct the first sub-path from the starting configuration to the intermediate configuration and the second sub-path from the intermediate configuration to the target configuration using five-letter path templates, and represent the overall path as a combination of the two five-letter path templates.

[0031] Specifically, the optimization objective is to minimize the total path length. The total path consists of the first sub-path from the starting configuration to the intermediate configuration and the second sub-path from the intermediate configuration to the target configuration. Therefore, the objective function is:

[0032] in, This represents the length of the first sub-path that satisfies the kinematic equations. This represents the length of the sub-path that satisfies the kinematic equations in the second segment.

[0033] Furthermore, the five-letter path template CCSCC is used to uniformly represent the Reeds-Shepp path between two points. Wherein: The letter C represents the arc segment with the maximum curvature; The letter S represents a straight line segment; The letter L indicates a left-turning arc; The letter R indicates a right-turn arc; The superscript "+" indicates forward movement; The superscript "-" indicates backward movement.

[0034] Therefore, a single two-point (or more precisely, two-dimensional) path using the CCSCC template can be represented as an ordered sequence of 5 paths. For the three-point path planning problem, one CCSCC template is used from the starting configuration to the intermediate configuration, and another CCSCC template is used from the intermediate configuration to the target configuration. Therefore, the overall path is represented as CCSCC-CCSCC, containing a total of 10 paths.

[0035] The variable parameters are defined as follows: The heading angle variable of the intermediate configuration has a range of values. ; : The length variable of the i-th path segment, where ; : The turning variable of the i-th path segment; : The gear position variable of the i-th path segment; : The composite direction variable of the i-th path segment; : The heading angle variable of the vehicle after passing through the k-th segment of the path; : The planar position variable of the vehicle after passing through the k-th path segment; : The periodic integer variable representing the angle corresponding to the i-th arc segment.

[0036] It is worth noting that the steering variable The definition is as follows: when the first When the segment is a circular arc segment, ,in Indicates a right turn. Indicates a left turn; when the first When the segment is a straight line segment, The gear position variable ,in Indicates moving forward. Indicates moving backward. Further definition:

[0037] in, Used to characterize the direction of change in the heading of a circular arc segment.

[0038] Step 108: Construct a mixed-integer piecewise linear programming model with the objective of minimizing the total path length.

[0039] Step 110: Apply kinematic constraints, configuration matching constraints, and piecewise linear approximation constraints to the mixed integer piecewise linear programming model.

[0040] Specifically, based on the different motion patterns of the vehicle on the path segment, the constraints are divided into circular arc segment constraints, straight line segment constraints, intermediate configuration matching constraints, endpoint configuration matching constraints, angular periodic constraints, and piecewise linear approximation constraints.

[0041] Furthermore, the circular arc segment constraints are constructed based on the relationship between the vehicle's initial and final configurations on the circular arc segment. If the vehicle starts from state... Start, along a length of For the circular arc segment motion, the following angular and positional relationships exist:

[0042] in, The heading angle at the end of the arc segment. and The coordinates of the endpoint of the arc segment. This refers to the direction of this gear. This indicates the direction of the turn in this segment. Let be the length of this path segment. This transition equation forms the basis for many constraints.

[0043] Furthermore, the straight segment constraint is constructed based on the relationship between the vehicle's initial and final configurations on the straight segment. If the vehicle starts from state... Departure, travel distance along the current course Then we have:

[0044] in, It indicates the direction of motion of a straight line segment.

[0045] In Example 1, the first sub-path consists of segments 0 to 4, specifically including: segment 0 (circular arc), segment 1 (circular arc), segment 2 (straight line), segment 3 (circular arc), and segment 4 (circular arc). Based on the time sequence of the five path segments, the vehicle state is propagated segment by segment, and constraints are established.

[0046] The constraints for segment 0 (circular arc) include: steering constraints, gear and steering coupling constraints, angle update relationships, angle periodic constraints, horizontal coordinate update relationships, vertical coordinate update relationships, and length boundary constraints.

[0047] in, This represents the heading angle after traversing the 0th arc. It represents the arc length of the 0th arc segment (which is also the radius of the arc). This represents the configuration coordinates after passing through the 0th arc segment. This represents the periodic integer variable corresponding to the 0th arc segment.

[0048] The constraint for the first circular arc segment is similar to that for the zeroth circular arc segment, specifically including:

[0049] Further constraints on the turning relationships between adjacent arc segments can be set to improve search efficiency. For example, the 0th segment and the 1st segment can satisfy an opposite turning relationship, which can be expressed as a corresponding integer constraint.

[0050] The constraints for the second straight line segment include:

[0051] in, This indicates the heading angle after passing through the second straight segment. This represents the configuration coordinates after passing through the second straight line segment. The constraints for the third and fourth arc segments are the same as those for the first and second arc segments, only the indices differ. By applying appropriate angle, position, length, and periodicity constraints to the third and fourth segments, the vehicle reaches the intermediate configuration after completing the fourth segment.

[0052] Furthermore, the intermediate configuration matching constraints include:

[0053] in, Indicates the position of the end of the first sub-path. This represents the heading angle at the end of the first sub-path. It's worth noting that... These are the root-level continuous decision variables in the entire model, used to simultaneously couple the first sub-path and the second sub-path.

[0054] Furthermore, the second sub-path consists of segments 5 through 9, and its constraint construction method is consistent with that of the first sub-path. Specifically, segments 5 and 6 are circular arc segments, segment 7 is a straight line segment, and segments 8 and 9 are circular arc segments. The starting point of the second sub-path satisfies:

[0055] This establishes state propagation constraints from the intermediate configuration to the target configuration.

[0056] Furthermore, the endpoint configuration matching constraints at the end of the second sub-path include:

[0057] in, This is the endpoint of the overall path. This is the heading angle at the end of the overall path.

[0058] Furthermore, the path segment length boundary constraints include: for each circular arc segment, the following conditions are met.

[0059] For each line segment, the following condition is satisfied:

[0060] If necessary, the upper limit of the straight line segment can also be set according to the application scenario.

[0061] The equals sign in the above equation allows any path segment to degenerate to zero length for compatibility with different Reeds-Shepp optimal path types. That is:

[0062] Through this degradation mechanism, the unified CCSCC template can automatically degenerate into shorter path structures such as CCC, CSC, CC, C, and S during actual solution processing, depending on the circumstances. For example... Figure 3 As shown, a path segmentation diagram is provided, illustrating the path constraints and variables of each segment of the CCSCC. The second segment is simplified due to its similarity to the first segment.

[0063] Step 112: Call the optimization solver to solve the mixed integer piecewise linear programming model to obtain the optimal intermediate point heading angle, parameters of each path segment, and the overall optimal path.

[0064] In the aforementioned RS vehicle path planning method based on piecewise linear mixed integer programming, firstly, at the perception level, the starting configuration, intermediate point positions, and target configuration of the vehicle are obtained, and the heading angle of the intermediate point is explicitly set as a free variable to be optimized, thereby fully perceiving the spatial constraints and degrees of freedom of the task and laying the information foundation for subsequent joint optimization. Secondly, at the measurement level, a five-letter path template (such as CCSCC) is used to construct the first sub-path and the second sub-path respectively, and the overall path is represented as a combination of the two templates, thereby mapping the discrete multiple types of RS optimal path types into a unified parameterized structure; at the same time, piecewise linear approximation constraints are applied to the trigonometric functions, transforming the nonlinear sinθ and cosθ into measurable piecewise linear functions, thereby quantifying the numerical relationship between path curvature, heading change, and total length, overcoming the deficiency of traditional methods that cannot directly measure nonlinear constraints. Building upon this foundation, at the decision-making level, a mixed-integer piecewise linear programming model is constructed with the objective of minimizing the total path length. Kinematic constraints and configuration matching constraints are applied simultaneously, allowing real-time feedback of intermediate heading adjustments to the length calculation of each candidate path type. This achieves joint decision-making for "discrete path type selection" and "continuous intermediate heading optimization," avoiding local redundancy caused by abrupt heading changes in the discretization cascade method. Finally, at the reconstruction level, an optimization solver is invoked to solve the model, outputting the optimal intermediate heading angle, path segment lengths, gear positions, and steering parameters. Based on this, a continuous RS path from the starting point through the intermediate point to the endpoint, satisfying curvature constraints and having the shortest total length, is reconstructed. This path allows for forward and backward movement, and each segment can automatically degenerate into a shorter structure. In summary, this method fundamentally solves the technical problems of existing three-point Dubins methods being unable to reverse, discretized two-point RS methods having discontinuous intermediate point headings, and nonlinear constraints being difficult to incorporate into the optimization framework. It effectively suppresses path detours and local redundancy, and achieves a leap from static geometric dependence to dynamic mixed-integer programming decision-making, thereby significantly improving the accuracy, efficiency, and global optimality of RS vehicle three-point path planning.

[0065] In one embodiment, a path planning method for the Reeds-Shepp vehicle three-point problem based on piecewise linear mixed integer programming is provided, including the following steps: Step 202: Based on the driving scenario of the three-point vehicle problem, establish a three-point Reeds-Shepp problem optimization model.

[0066] Specifically, based on the driving scenario of the three-point problem of vehicles (starting point, intermediate point, and ending point), it is formalized into a two-way Reeds-Shepp path planning problem with a shared intermediate point. The free heading angle of the intermediate point is explicitly modeled as a decision variable to represent the coupling relationship between the two paths, and an optimization model of the three-point Reeds-Shepp problem oriented towards the free heading of the intermediate point is established.

[0067] Step 204: Based on the motion characteristics of the Reeds-Shepp vehicle, construct a unified parameterized equation and establish a 3PRSP optimization model.

[0068] Specifically, based on the Reeds-Shepp vehicle motion characteristics, a unified parameterized equation is proposed. Through parameterized constraints, 48 ​​Reeds-Shepp path types with a type length of 5 are uniformly characterized. All 48×48 canonical motion primitives are embedded into a single mixed integer optimization framework, and the 3PRSP optimization model is systematically constructed.

[0069] Step 206: Construct a path control model based on the driving scenario of the vehicle's three-point problem.

[0070] Path control models include: minimizing the total time (which is also minimizing the total distance, since the speed is always 1) function and mixed-integer linear programming models.

[0071] Step 208: Construct piecewise linear constraints for the driving path based on the vehicle's pose coordinates at the current moment.

[0072] Step 210: Based on the piecewise linear constraints, input the vehicle's current direction variable and heading angle variable into the minimization total time function for secondary constraints to obtain the total time.

[0073] Step 212: Based on the total time, plan the path for the Reeds-Shepp vehicle three-point problem using a mixed-integer linear programming model.

[0074] It is worth noting that the Reeds-Shepp vehicle three-point problem path planning method based on piecewise linear mixed-integer programming first formalizes the vehicle three-point problem into a two-way three-point path planning problem. By explicitly modeling the free heading angle of the intermediate point as a decision variable, the coupling relationship between the two path segments is represented, thereby establishing an optimization model for this scenario and improving the accuracy of path modeling. Second, by constructing a unified Reeds-Shepp vehicle motion parameterization equation, a unified characterization of 48 path types is achieved, and 48×48 canonical motion primitives are integrated into a single mixed-integer optimization framework. This solves the problems of scattered path types and difficulty in systematic optimization in traditional methods, improving the completeness and systematicity of the 3PRSP optimization model. Simultaneously, to address the nonlinear constraint problem brought about by Reeds-Shepp kinematics, a mixed-integer piecewise linear approximation method is used, effectively reducing the solution complexity of nonlinear programming, achieving high efficiency and stability in numerical solutions, and avoiding the problems of traditional solution methods easily converging to local optima and excessively long solution times. Finally, the performance of the method was fully verified through a large number of numerical experiments. The results show that the method has superior performance in terms of solution quality and solution efficiency, and can meet the dual requirements of path planning accuracy and real-time performance, providing reliable technical support for vehicles to perform tasks efficiently.

[0075] In one embodiment, the model contains and Nonlinear terms are approximated using piecewise linear constraints. Specifically, the angle interval is... The interval is divided into multiple segment points, and the relationship between the sine and cosine functions on that interval is approximated by multiple small line segments. Piecewise linear constraints stipulate that the angle variable and its corresponding sine and cosine values ​​must satisfy a piecewise linear functional relationship defined by the segment points. (Explanation follows) Figure 4 As shown, it illustrates The function is in The concept of piecewise linear approximation over an interval. This figure illustrates... A linear approximation within a certain interval using multiple line segments (here, the number of line segments is set to 7 for visual clarity, but other numbers can also be used).

[0076] It is worth noting that piecewise linear approximation replaces the original curve with small line segments, and its approximation error is related to the segment length. The smaller the segment length, the smaller the error between the analytical value and the piecewise linear approximation value, and the higher the model solution accuracy; the larger the segment length, the smaller the model size and the faster the solution, but the larger the approximation error. Therefore, an appropriate segment length can be selected based on the required computational accuracy and available computational resources.

[0077] In one embodiment, after piecewise linearization, the three-point Reeds-Shepp path planning model is converted into a mixed-integer piecewise linear programming model, and solved using the Gurobi solver to obtain the optimal intermediate heading angle, path length of each segment, gear variable of each segment, steering variable of each segment, and the final optimal path type.

[0078] In one embodiment, the solution output includes: Optimal intermediate heading angle ; Optimal total path length ; Length of each path segment ; Variables of turning at each path segment ; Variable gear position for each path segment ; Pose state at each stage With heading angle ; Final path type string.

[0079] In one embodiment, based on the solution obtained and It can recover the physical meaning of each path segment. Among them, when... When, it corresponds to a left-turning arc; when When, it corresponds to a right-turning arc; when When, it corresponds to a straight line segment; when When, it corresponds to the forward segment; when When the time is right, the corresponding step is left. Therefore, the corresponding path segment can be represented as L+, L-, R+, R-, S+, or S-.

[0080] In one embodiment, when all gear variables are restricted to the forward state, i.e., constrained:

[0081] The model can degenerate into a three-point Dubins path planning model; when retaining When the value range is defined, the model is used to solve the complete three-point Reeds-Shepp path planning problem.

[0082] It is worth noting that, based on the above modeling method, the intermediate configuration heading angle and the two sub-paths can be directly optimized together. Compared with the method of fixing the intermediate heading angle, solving the two two-point Reeds-Shepp paths separately, and then cascading them, this method can reduce the heading abrupt changes and local redundancy at the intermediate connection position, thereby improving the smoothness, continuity, and global optimality of the overall path.

[0083] In one embodiment, this method is applicable to mobile systems constrained by minimum turning radius, such as inspection robots, unmanned vehicles, mobile work platforms, autonomous vehicles, and unmanned boats, and is particularly suitable for path planning tasks that require passing through a specified intermediate target point and whose heading angle is unknown.

[0084] In one embodiment, the Gurobi solver, combined with piecewise linearization and generated constraints, provides an efficient and relatively accurate three-point Reeds-Shepp path planning method. The results are as follows... Figure 5 As shown in the figure. The initial pose can be seen in the figure as follows: The midpoint position is The target pose is The optimal route with the shortest time is shown in the figure below: the path type is R+L. R L+R+L The intermediate heading angle is 5.5000, and the total length is 4.2193.

[0085] In one embodiment, a CCSCC template is used as a unified five-letter path template, where C represents the arc segment with maximum curvature and S represents the straight line segment. Each letter segment in the CCSCC template is assigned a continuous path length variable, a discrete steering variable, and a discrete gear position variable. By allowing the path length variable to take a value of zero, the five-letter path template degenerates into a shorter path structure during the solution process.

[0086] In one embodiment, the trigonometric function domains of the circular and linear motion constraints of the path are discretized into multiple segment points. Small line segments are used to approximate the variation relationship of the sine and cosine functions within this interval, thus constructing a piecewise linear function. The sine and cosine nonlinear constraints in the original kinematic model are replaced with a piecewise linear function relationship that must be satisfied between the heading angle variable and auxiliary variables representing its sine and cosine values. By adjusting the number of segment points and according to a preset balance coefficient, the mixed-integer piecewise linear programming model is converted into a mixed-integer linear programming form that can be directly processed by commercial solvers. The preset balance coefficients are determined by the approximation accuracy and solution efficiency.

[0087] In one embodiment, state propagation constraints are established sequentially in chronological order for the first circular arc segment, the second circular arc segment, the straight line segment, the third circular arc segment, and the fourth circular arc segment; The state propagation constraint is specifically as follows: based on the starting heading angle θ0, gear variable g, steering variable s, and path length γ of the current path segment, update according to θ1=θ0+ s·(2g-1)·γ+2π·K to obtain the heading angle θ1 of the next path segment, where K is a periodic integer variable; Based on the updated heading angle θ1, starting point coordinates (x0, y0), and steering variable s, according to: x1 = x0 + s·(sinθ1 - sinθ0) y1 = y0 + s·(-cosθ1 + cosθ0) The planar coordinates (x1, y1) of the next segment are updated. Then, θ1, θ0 and the coordinates of the starting point are input into the planar coordinate update formula, and the complete state (x1, y1, θ1) of the next path segment is output, thus obtaining the first sub-path.

[0088] In one embodiment, a turning relationship constraint is applied to adjacent arc segments, which forces the turning variable s0 of the first arc segment and the turning variable s1 of the second arc segment to satisfy (s0-s1)²>0, meaning that a left turn must be followed by a right turn or a right turn must be followed by a left turn, in order to generate a candidate path that conforms to the RS optimal path type.

[0089] It should be understood that, although Figures 1-2 The steps in the flowchart are shown sequentially as indicated by the arrows; however, these constraint steps are not necessarily executed in the order indicated by the arrows. Unless explicitly stated herein, there is no strict order requirement for the execution of these steps, and they can be executed in other orders. Furthermore, Figures 1-2 At least some of the steps in the process may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least some of the sub-steps or stages of other steps.

[0090] In one embodiment, such as Figure 6 As shown, an RS vehicle path planning device based on piecewise linear mixed integer programming is provided, including: an input module 602, a model building module 604, a path standardization building module 606, a model building module 608, a constraint module 610, and a path optimization module 612, wherein: Input module 602 is used to acquire the initial configuration, intermediate point position and target configuration of the curvature-constrained vehicle; Model building module 604 is used to set the heading angle corresponding to the intermediate point position as the free variable to be optimized in order to form an intermediate configuration.

[0091] The path standardization construction module 606 is used to construct the first sub-path from the starting configuration to the intermediate configuration and the second sub-path from the intermediate configuration to the target configuration respectively, based on the RS vehicle kinematic model that allows forward and backward motion, using five-letter path templates, and to represent the overall path as a combination of the two five-letter path templates.

[0092] Model building module 608 is used to build a mixed integer piecewise linear programming model with the goal of minimizing the total path length.

[0093] Constraint module 610 is used to apply kinematic constraints, configuration matching constraints, and constraints for piecewise linear approximation of trigonometric functions to a mixed integer piecewise linear programming model.

[0094] The path optimization module 612 is used to call the optimization solver to solve the mixed integer piecewise linear programming model and obtain the optimal intermediate point heading angle, the parameters of each path segment, and the overall optimal path.

[0095] Specific limitations regarding the RS vehicle routing device based on piecewise linear mixed-integer programming can be found in the limitations of the RS vehicle routing method based on piecewise linear mixed-integer programming described above, and will not be repeated here. Each module in the aforementioned RS vehicle routing device based on piecewise linear mixed-integer programming can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in or independent of the processor in a computer device, or stored in the memory of a computer device in software form, so that the processor can call and execute the operations corresponding to each module.

[0096] Those skilled in the art will understand that Figure 6 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0097] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), Synchlink, DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and RAMbus dynamic RAM (RDRAM), etc.

[0098] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0099] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this application should be determined by the appended claims.

Claims

1. A method for RS vehicle path planning based on piecewise linear mixed integer programming, characterized in that, The method includes: Obtain the initial configuration, midpoint position, and target configuration of the curvature-constrained vehicle; Set the heading angle corresponding to the intermediate point position as the free variable to be optimized to form an intermediate configuration; Based on the RS vehicle kinematics model that allows forward and backward motion, a first sub-path from the starting configuration to the intermediate configuration and a second sub-path from the intermediate configuration to the target configuration are constructed using five-letter path templates, and the overall path is represented as a combination of the two five-letter path templates. Construct a mixed-integer piecewise linear programming model with the objective of minimizing the total path length; Kinematic constraints, configuration matching constraints, and constraints for piecewise linear approximation of trigonometric functions are applied to the mixed integer piecewise linear programming model. The optimization solver is invoked to solve the mixed integer piecewise linear programming model to obtain the optimal intermediate point heading angle, parameters of each path segment, and the overall optimal path.

2. The method according to claim 1, characterized in that, Construct a mixed-integer piecewise linear programming model with the objective of minimizing the total path length, including: The CCSCC template is used as the unified five-letter path template, where C represents the arc segment with the maximum curvature and S represents the straight line segment; Assign a continuous path length variable, a discrete steering variable, and a discrete gear position variable to each letter segment in the CCSCC template. By allowing the path length variable to take the value of zero, the five-letter path template degenerates into a shorter path structure during the solution process.

3. The method according to claim 1, characterized in that, Applying kinematic constraints, configuration matching constraints, and constraints for piecewise linear approximation of trigonometric functions to the mixed integer piecewise linear programming model includes: The trigonometric function domain intervals of the circular and linear motion constraints of the path are discretized into multiple segment points. The changes of the sine and cosine functions on this interval are approximated by small line segments to construct a piecewise linear function. The sine and cosine nonlinear constraints in the original kinematic model are replaced with the piecewise linear function relationship that must be satisfied between the heading angle variable and the auxiliary variables representing its sine and cosine values. By adjusting the number of segmentation points and according to a preset balance coefficient, the mixed-integer piecewise linear programming model is converted into a mixed-integer linear programming form that can be directly processed by commercial solvers. The preset balance coefficient is determined by the approximation accuracy and solution efficiency.

4. The method according to any one of claims 1 to 3, characterized in that, Construct the first sub-path from the initial configuration to the intermediate configuration, including: Establish state propagation constraints for the first circular arc segment, the second circular arc segment, the straight line segment, the third circular arc segment, and the fourth circular arc segment in chronological order. The state propagation constraint is specifically as follows: based on the starting heading angle θ0, gear variable g, steering variable s, and path length γ of the current path segment, update according to θ1=θ0+s·(2g-1)·γ+2π·K to obtain the heading angle θ1 of the next path segment, where K is a periodic integer variable; Based on the updated heading angle θ1, starting point coordinates (x0, y0), and steering variable s, according to: x1 = x0 + s·(sinθ1 - sinθ0) y1 = y0 + s·(-cosθ1 + cosθ0) The updated planar coordinates (x1, y1) are obtained for the next segment; Then input θ1, θ0 and the coordinates of the starting point into the plane coordinate update formula, output the complete state (x1, y1, θ1) of the next path segment, and obtain the first sub-path.

5. The method according to claim 4, characterized in that, Establish state propagation constraints between the first and second circular arc segments, including: Apply a turning relationship constraint to adjacent arc segments. This constraint forces the turning variable s0 of the first arc segment and the turning variable s1 of the second arc segment to satisfy (s0-s1)²>0, meaning that a left turn must be followed by a right turn or a right turn must be followed by a left turn, in order to generate a candidate path that conforms to the RS optimal path type.

6. An RS vehicle path planning device based on piecewise linear mixed integer programming, characterized in that, The device includes: The input module is used to obtain the initial configuration, intermediate point position, and target configuration of the curvature-constrained vehicle; The model building module is used to set the heading angle corresponding to the intermediate point position as a free variable to be optimized in order to form an intermediate configuration; The path standardization construction module is used to construct, based on the RS vehicle kinematics model that allows forward and backward motion, a first sub-path from the starting configuration to the intermediate configuration and a second sub-path from the intermediate configuration to the target configuration using five-letter path templates, and to represent the overall path as a combination of the two five-letter path templates. The model building module is used to construct a mixed-integer piecewise linear programming model with the objective of minimizing the total path length. The constraint module is used to apply kinematic constraints, configuration matching constraints, and constraints on the piecewise linear approximation of trigonometric functions to the mixed integer piecewise linear programming model. The path optimization module is used to call the optimization solver to solve the mixed integer piecewise linear programming model and obtain the optimal intermediate point heading angle, parameters of each path segment, and the overall optimal path.

Images