Omnidirectional obstacle avoidance planning control method and device for distributed electric drive modular transport vehicle

Abstract

The invention belongs to the technical field of distributed electric drive modular transport vehicle planning and control, and particularly relates to a distributed electric drive modular transport vehicle obstacle avoidance planning control method and device. The specific process of the method comprises the steps that two front shafts and two rear shafts which are close to each other are equivalent to two equivalent shafts respectively, two e-SPMT units are used for equivalence, and a monorail dynamic model is established; determining a collision boundary and establishing a collision constraint condition design, establishing a cost function including a swept area, a distance to an obstacle and path smoothness, and optimizing the cost function to obtain an expected path meeting the collision constraint condition; on the basis of the expected path and kinematic constraints, an expected speed and a yaw velocity corresponding to each path point in the expected path are generated; taking the expected speed and the yaw velocity as state quantities, and generating a dynamics control quantity according to the monorail dynamics model; the dynamic control quantity is distributed to each wheel longitudinal force and turn angle.

Description

Technical Field

[0001] This invention belongs to the field of planning and control technology for distributed electric drive modular transport vehicles, specifically relating to a method and device for obstacle avoidance planning and control of distributed electric drive modular transport vehicles. Technical Background

[0002] With the development of the transportation industry, self-propelled modular transporters (SPMTs) have been widely used in the transportation field. Due to their modular nature, the modules are coordinated and controlled through hydraulic and electronic systems, and can be combined into longitudinal, lateral, or other complex layouts to adapt to scenarios involving the transportation of large and heavy goods, such as... Figure 1 As shown. However, traditional SPMTs, due to their conventional drive system, are often more suitable for flat, wide work sites during the transportation of large cargo; in addition, their travel speed is generally low, resulting in low transportation efficiency and making them unsuitable for long-distance transportation. With the development of distributed electric drive technology, distributed electric drive self-propelled modular transport vehicles (e-SPMTs) are gradually being used more widely. Compared with traditional SPMTs, the core feature of e-SPMTs is that each independent transport unit is equipped with an independent electric drive system, steering system, and control system. By coordinating the drive / braking and turning angles of multiple omnidirectional wheels, and through the coordination of motion between units, multiple motion modes are provided, enabling long-distance transportation of heavy-duty, ultra-large, indivisible materials. This solves the problems of low transportation efficiency and applicability to flat road sections of traditional SPMTs. However, the multiple degrees of freedom brought by distributed electric drive technology, while improving the flexibility and traffic efficiency of traditional SPMTs, also bring about the problem of drive / braking force and turning angle distribution among omnidirectional wheels, placing higher demands on planning and control schemes.

[0003] Current research on path planning and tracking control in e-SPMT swarm collaborative autonomous transportation is limited. Due to the highly coupled nonlinear dynamics of the e-SPMT transport swarm model and the large number of degrees of freedom in planning, precise synchronization of the cooperative motion between modules is crucial to avoid internal collisions. Furthermore, when facing dynamic and static obstacles, e-SPMTs need to address control allocation issues, generating smooth and safe global and local trajectories that satisfy vehicle kinematics, dynamics, and geometric constraints, and then implementing motion control. Therefore, based on the complex system configuration and kinematic and dynamic constraints of e-SPMTs, efficiently and in real-time solving the obstacle avoidance path optimization problem and achieving precise tracking control is of great significance for the application of e-SPMT swarms in fields such as intelligent logistics. Summary of the Invention

[0004] In view of this, the present invention provides a method and device for omnidirectional obstacle avoidance planning and control of a distributed electric drive modular transport vehicle. This method can improve the planning and control of the passability and flexibility of the e-SPMT group, and realize autonomous obstacle avoidance and posture adjustment when encountering obstacles.

[0005] The technical solution of the present invention is as follows:

[0006] In a first aspect, the present invention provides a method for obstacle avoidance planning and control of a distributed electric-driven modular transport vehicle, the specific process of which is as follows:

[0007] Step 1: Model Establishment: The two front axes and two rear axes that are close to each other are respectively equivalent to two equivalent axes, and two e-SPMT elements are used to establish a single-track dynamic model;

[0008] Step 2: Collision Boundary Determination: Combining road information, obstacle information, and vehicle information obtained from the vehicle perception module, a collision boundary is established, including the road boundary, obstacle extension safety boundary, and vehicle geometric boundary.

[0009] Step 3: Collision constraint design: Ensure that the geometric boundary of the e-SPMT group is within the road boundary and has no intersection with the interior of the obstacle safety boundary;

[0010] Step 4: Path planning: Establish a cost function that includes sweep area, distance to obstacles, and path smoothness, and optimize the cost function to obtain the desired path that satisfies the collision constraints;

[0011] Step 5: Generating kinematic control variables: Based on the desired path and kinematic constraints, generate the desired velocity and yaw rate corresponding to each path point in the desired path;

[0012] Step 6: Generation of dynamic control quantities: Using the desired velocity and yaw rate as state quantities, and the longitudinal force, additional yaw moment and equivalent rotation angle of each axis of the two e-SPMT units as control quantities, a sliding mode controller is designed, and dynamic control quantities are generated according to the single-track dynamic model.

[0013] Step 7: Dynamic control quantity allocation: Allocate the dynamic control quantities to the longitudinal force and rotation angle of each wheel.

[0014] Optionally, the single-track dynamic model described in this invention is:

[0015]

[0016] Among them, F xi and F yi δ represents the longitudinal and lateral forces acting on each axis in the monorail model, respectively. iRepresents the equivalent rotation angle of each axis under the single-track model; where i = 1, 2 represent the equivalent front and rear axes under the single-track dynamic model of the e-SPMT element, respectively, and F hx and F hy These represent the longitudinal and lateral forces exerted by the cargo on the e-SPMT unit, respectively, V. x and V y These represent the longitudinal and lateral velocities in the vehicle coordinate system, respectively; m is the mass of a single e-SPMT unit, and ω... z I represents the yaw rate of the e-SPMT unit. z d represents the moment of inertia of the e-SPMT element about the z-axis of its center of mass. 2i-1 and d 2i These represent the actual distances from the centroid to the four actual axes in the unsimplified e-SPMT model (the existing e-SPMT model). See the detailed annotations below. Figure 4 .

[0017]

[0018] Among them, K i This is the equivalent lateral stiffness of the corresponding axis.

[0019] Optionally, the obstacle boundary described in this invention is an outward extension of the actual obstacle boundary. The extension method is as follows: for each point on the surface of the obstacle, a circle with radius φ is generated with itself as the center. The union of the circles is added to the original obstacle as the extension of the obstacle. The new boundary generated on the outside is the safety boundary.

[0020] Optionally, the cost function described in this invention is as follows:

[0021]

[0022] Where ω1, ω2, ω3 are the set weighting coefficients, A is the sweeping area, and d i Let κ represent the distance to the i-th obstacle, and κ represent a small constant that ensures the denominator is not zero. i Represents the road curvature, ||Δθ i || represents the difference in heading angle between adjacent path points.

[0023] Optionally, the specific process of designing the upper-level kinematic controller described in this invention is as follows:

[0024] First, select a reference point on the desired path, that is, select the point closest to the centroid of each e-SPMT unit;

[0025] Secondly, the tracking error includes the heading angle error and the lateral and longitudinal tracking errors between each path point and the reference point. The tracking error is used as a state quantity, where the six quantities represent the longitudinal tracking error, lateral tracking error, and heading angle tracking error of the two e-SPMT units, respectively.

[0026] The lateral velocity, longitudinal velocity, and yaw rate of the e-SPMT unit are selected as the control variables u = (V x1d V y1d ω 1d V x2d ω 2d ) T ;

[0027] Let the reciprocal of the heading angle reference value be denoted as column vector z, and the state equation is obtained:

[0028]

[0029] Where A represents the state transition matrix, B represents the control matrix, C represents the disturbance transition matrix, and column vector z is considered as the disturbance.

[0030] Next, the state equation is discretized using the MPC algorithm, and new state variables are added. The control problem is transformed into a quadratic programming problem and solved to obtain the current control quantity u(k).

[0031] Optionally, the present invention sets an objective function and constraints, transforming the control problem into a quadratic programming problem for solution. The objective function and constraints are as follows:

[0032]

[0033] Δu(k+i|k)=u(k+i|k)-u(k+i-1|k)

[0034] Where ξ(k+i|k) represents the predicted value of the state quantity at time k+i from time k, ξ ref (k+i|k) represents the reference value for the predicted state quantity at time k+i based on the value at time k. The weight matrix Q represents the transformation of the corresponding elements into a standard quadratic form; Δu(k+i|k) represents the predicted value of the control quantity at time k+i based on the value at time kk. This represents the transformation of the corresponding elements into a standard quadratic form using the weight matrix R; ρ represents the weighting coefficients of the relaxation factor ε.

[0035] Constraints:

[0036] Δu min (k)≤Δu(k+i|k)≤Δu max (k)

[0037] u min(k)≤u(k+i|k)≤u max (k)

[0038]

[0039] Where, Δu min (k), Δu max (k), u min (k), u max (k) represent the minimum and maximum values ​​of the change in the control quantity, and the minimum and maximum values ​​of the control quantity, respectively; and Let them represent n-dimensional and m-dimensional real vector spaces, respectively;

[0040] The optimal control sequence is obtained by solving the objective function and constraints. The first control variable Δu of the optimal control sequence is... * (k) is applied to the current system to obtain the current actual control quantity u(k) = u(k-1) + Δu * (k).

[0041] Optionally, the process for obtaining the dynamic control rate described in this invention is as follows:

[0042] When designing a sliding mode controller, based on the aforementioned single-track dynamics model, the system can be described as follows:

[0043]

[0044] Where, f(x) c ) and g(x c ) is x c The time-varying function, x, is used to represent the relationship between the derivative of the state variable and the state and control variables. c =(V x1 V y1 ,ω1,V x2 V y2 ,ω2) T V xi V yi ω i These represent the longitudinal velocity, lateral velocity, and yaw rate of each e-SPMT unit, respectively; u c =(F cx1 δ cf1 δ cr1 F cx2 δ cf2 δ cr2 ) T F cxi δ represents the longitudinal combined driving braking force of each e-SPMT unit. cfi and δ criThese represent the rotation angles of the equivalent wheel in the e-SPMT unit monorail model.

[0045] The expression for calculating the error vector of each state variable is as follows:

[0046] e = x d -x c

[0047] Where, x d =(V x1d V y1d ω 1d V x2d V y2d ω 2d ) T , representing the expected values ​​of the longitudinal velocity, lateral velocity, and yaw rate of each e-SPMT unit, respectively, and the sliding surface is designed as follows: It is a coefficient matrix;

[0048] Determine the reaching law as These are constant parameters;

[0049] Combining the sliding surface and the approach condition, the governing law is obtained as follows:

[0050]

[0051] Optionally, the specific process of allocating the dynamic control quantity according to the present invention is as follows:

[0052] First, determine the rotation angle of each wheel, and then map the equivalent front and rear axle rotation angles to the rotation angles of each wheel on the front and rear axles, respectively.

[0053] δ1→(δ l1 δ l2 δ r1 δ r2 )

[0054] δ2→(δ l3 δ l4 δ r3 δ r4 )

[0055] The acceleration at the center of mass is The turning radius corresponding to the center of mass is Therefore, the rotation angle distribution values ​​for the first two axes are...

[0056]

[0057] Where d represents the wheelbase between two adjacent shafts of the e-SPMT unit, and c represents the wheel track of the e-SPMT unit;

[0058] The allocation method for the latter two axes is the same as that for the former two axes;

[0059] Secondly, the driving force distribution is determined: the longitudinal force of each e-SPMT unit is evenly distributed among the wheels, that is, each wheel provides 1 / 8 of the total longitudinal force.

[0060] In a second aspect, the present invention provides a distributed electric-driven modular transport vehicle obstacle avoidance planning and control device, comprising: a model building module, a collision boundary determination module, a collision constraint design module, a path planning module, a kinematic controller, a dynamic control quantity generator, and a dynamic control quantity distributor; wherein...

[0061] The model building module is used to convert the two front axes and the two rear axes that are close to each other into two equivalent axes, and to establish a single-track dynamic model using two e-SPMT units.

[0062] The collision boundary determination module is used to determine the collision boundary, including the road boundary, obstacle boundary, and e-SPMT group geometric boundary, based on the road information, obstacle information, and vehicle information obtained by the vehicle perception module.

[0063] The collision constraint design module is used to establish constraint conditions for the e-SPMT group geometric boundary within the road boundary and without intersection with the obstacle safety boundary.

[0064] The path planning module is used to establish a cost function that includes the swept area, the distance to the obstacle, and the path smoothness, and optimize the cost function to obtain the desired path that satisfies the collision constraints.

[0065] The kinematic control instrument generates the desired velocity and yaw rate for each path point in the desired path based on the desired path and kinematic constraints.

[0066] A dynamic controller is used to take the desired speed and yaw rate as state variables, and the longitudinal force, additional yaw moment and equivalent rotation angle of each axis of the two e-SPMT units as control variables. The sliding mode controller is designed and the dynamic control variables are generated according to the single-track dynamic model.

[0067] A dynamic control quantity distributor is used to distribute the dynamic control quantity to the longitudinal force and steering angle of each wheel.

[0068] Beneficial effects:

[0069] First, by combining road information, obstacle information, and vehicle information obtained by the vehicle perception module, this invention establishes a collision boundary that includes the road boundary, the obstacle extension safety boundary, and the vehicle geometric boundary. Furthermore, it designs constraints to prevent collisions through collision point detection.

[0070] Second, an obstacle avoidance path planning method for e-SPMT groups was designed. Specifically, a multi-objective cost function was designed, consisting of three factors: the sweep area during obstacle avoidance, the distance between the e-SPMT group and the obstacle, and the path smoothness. The optimal path was obtained by using an optimization method.

[0071] Third, based on the aforementioned optimal path, an upper-level kinematic controller and a lower-level SMC dynamic controller based on the fundamental principles of the MPC control algorithm are designed. A control quantity allocation method is designed, generating direct drive / braking forces and steering angles allocated to each wheel based on the output of the hierarchical controller.

[0072] Fourth, this invention provides support for vehicle planning constraint design and motion control. Attached Figure Description

[0073] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0074] Figure 1 Map for transporting large and bulky goods;

[0075] Figure 2 A flowchart for the e-SPMT group obstacle avoidance planning and control process;

[0076] Figure 3 A schematic diagram of an e-SPMT unit;

[0077] Figure 4 This is a schematic diagram of the full-wheel model of the e-SPMT unit;

[0078] Figure 5 This is the equivalent model for the e-SPMT monorail.

[0079] Figure 6 This is a schematic diagram of obstacle avoidance for e-SPMT groups.

[0080] Figure 7 These represent the actual boundary and the safety boundary of the obstacle. Detailed Implementation

[0081] The embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0082] It should be noted that, in the absence of conflict, the following embodiments and features can be combined with each other; and, based on the embodiments of this disclosure, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this disclosure.

[0083] It should be noted that various aspects of embodiments within the scope of the appended claims are described below. It will be apparent that the aspects described herein can be embodied in a wide variety of forms, and any particular structure and / or function described herein is merely illustrative. Based on this disclosure, those skilled in the art will understand that one aspect described herein can be implemented independently of any other aspect, and two or more of these aspects can be combined in various ways. For example, any number of aspects set forth herein can be used to implement the device and / or practice the method. Additionally, this device and / or method can be implemented using structures and / or functionalities other than one or more of the aspects set forth herein.

[0084] This application presents an obstacle avoidance planning and control method for a distributed electric-driven modular transport vehicle (e-SPMT). The main contents can be broadly categorized as follows: e-SPMT kinematic / dynamic model establishment; collision boundary determination; path planner design; hierarchical controller design and control quantity allocation method design. Figure 2 The diagram shown is a flowchart of the e-SPMT group obstacle avoidance planning and control process.

[0085] This application provides an obstacle avoidance planning and control method for a distributed electric drive modular transport vehicle, the specific process of which is as follows:

[0086] Step 1: Model Establishment: The two front axes and two rear axes that are close to each other are respectively equivalent to two equivalent axes, and two e-SPMT elements are used to establish a single-track dynamic model;

[0087] Step 2: Collision Boundary Determination: Combining road information, obstacle information, and vehicle information obtained from the vehicle perception module, a collision boundary is established, including the road boundary, obstacle extension safety boundary, and vehicle geometric boundary.

[0088] Step 3: Collision constraint design: Ensure that the geometric boundary of the e-SPMT group is within the road boundary and has no intersection with the interior of the obstacle safety boundary;

[0089] Step 4: Path planning: Establish a cost function that includes sweep area, distance to obstacles, and path smoothness, and optimize the cost function to obtain the desired path that satisfies the collision constraints;

[0090] Step 5: Generating kinematic control variables: Based on the desired path and kinematic constraints, generate the desired velocity and yaw rate corresponding to each path point in the desired path;

[0091] Step 6: Generation of dynamic control quantities: Using the desired velocity and yaw rate as state quantities, and the longitudinal force, additional yaw moment and equivalent rotation angle of each axis of the two e-SPMT units as control quantities, a sliding mode controller is designed, and dynamic control quantities are generated according to the single-track dynamic model.

[0092] Step 7: Dynamic control quantity allocation: Allocate the dynamic control quantities to the longitudinal force and rotation angle of each wheel.

[0093] The above steps will be explained in detail below with reference to the accompanying drawings.

[0094] Step 1: Establish the kinematic and dynamic model of the e-SPMT group

[0095] First, the motion characteristics of e-SPMT bicycles and e-SPMT groups are analyzed, thereby establishing the kinematic and dynamic models of e-SPMT groups.

[0096] The e-SPMT is driven by independent wheel-side motors, and its operating modes mainly include the following two: instantaneous translation and instantaneous rotation around a point. Translation means that all wheels of the e-SPMT have the same steering angle; the e-SPMT only has forward velocity along the direction of travel, and its yaw rate is 0. Because each wheel of the e-SPMT steers independently with a steering angle range of an entire circumference, its rotation can be divided into two forms: the steering center is outside the vehicle body area, and the steering center is within the vehicle body area. When the steering center is within the vehicle body area, the e-SPMT can adjust its heading angle around a point without significant positional changes, giving it a higher passability advantage compared to traditional transport vehicles.

[0097] like Figure 3 As shown, to clarify the kinematic and dynamic characteristics of e-SPMT units, a kinematic / dynamic model of a single e-SPMT needs to be established. Simultaneously, establishing a kinematic model of the cargo can clarify the relative positions and motion relationships between e-SPMT units, thereby enabling formation planning and control. Therefore, the kinematic relationships of the cargo and e-SPMT units are analyzed separately. Based on this, a kinematic model of the cargo is established as follows:

[0098]

[0099] in, This represents the longitudinal velocity, lateral velocity, and angular velocity of the cargo in the world coordinate system. δ1 and δ2 represent the forward velocities of the two e-SPMT units at their center of mass in the world coordinate system, respectively. δ1 and δ2 are the steering angles of the two equivalent wheels (to reduce control variables, the two front and two rear axes that are close to each other are equivalent to two axes respectively. Here, the equivalent wheels are the two equivalent front and rear axes of the monorail model) relative to the cargo. α is the distance between the two hinge points of the cargo.

[0100] Furthermore, based on the geometric constraint that the cargo cannot stretch or contract longitudinally in the vehicle coordinate system, we have:

[0101]

[0102] Meanwhile, the kinematic model of a single e-SPMT unit is

[0103]

[0104] Where i = 1, 2, corresponding to the subscripts f / r, represent the first and second e-SPMT units respectively; V x and V y These represent the longitudinal and lateral velocities in the vehicle coordinate system, respectively. This represents the heading angle of the e-SPMT unit. The geodetic-vehicle coordinate system transformation relationship is as follows:

[0105]

[0106] like Figure 4 As shown, considering the complex and strongly coupled nature of the e-SPMT system, a full-wheel dynamic model of the e-SPMT is established to add dynamic constraints, thereby limiting the generated trajectory and ensuring it conforms to dynamic characteristics. The dynamic model of a single e-SPMT is as follows:

[0107]

[0108] Where m is the mass of a single e-SPMT unit, ω z I represents the yaw rate of the e-SPMT unit. z The z-axis represents the rotational inertia of the e-SPMT element about its center of mass. d1 to d4 represent the actual distances from the center of mass to the four actual axes in the unsimplified e-SPMT model, respectively. See the detailed annotations below. Figure 4 F xi F yi (j=1,2) represents the longitudinal and lateral forces acting on the tire, F hxj F hyj (j = 1, 2) represent the longitudinal and lateral forces exerted by the cargo on each e-SPMT element. The tire uses the Magic Tire model, whose lateral stiffness conforms to the Magic Tire formula, which will not be elaborated here. The above model has high accuracy, and the resulting kinematic and dynamic relationships provide dynamic / kinematic constraints for the planning process.

[0109] like Figure 5As shown, to simplify the control process of the dynamic controller, the original dynamic model is simplified and a single-track dynamic model is established. When establishing the single-track dynamic model, to reduce control variables, the two front axes and two rear axes that are close to each other are respectively equivalent to two axes (i.e., the equivalent front axis and equivalent rear axis of the single-track model). The dynamic controller selects the rotation angle and longitudinal and lateral forces corresponding to the two equivalent axes as control variables. Then, through a certain allocation rule, the equivalent single-track dynamic model is established as follows:

[0110]

[0111] Among them, F xi and F yi (i=1,2) represent the longitudinal and lateral forces acting on each axis in the monorail model, respectively, δ i This represents the equivalent rotation angle of each axis in a single-track model. There is also...

[0112]

[0113] Among them, K i This is the equivalent lateral stiffness of the corresponding axis.

[0114] Step 2: Determining the Collision Boundary

[0115] To ensure that the e-SPMT group does not exceed the road boundary and does not collide with obstacles while moving along the planned trajectory, this invention defines collision boundaries. Specifically, it provides a mathematical description method for defining the road boundary, obstacle collision boundary, and the geometric boundary of the e-SPMT group, such as... Figure 6 As shown.

[0116] Road boundary determination: Assuming the road boundary is continuous, the road boundary can be described as follows:

[0117]

[0118] Among them, f l and f h X0 and X represent the upper and lower boundaries of the road, respectively. e These are the x-coordinates of the starting and ending points of the road in the global coordinate system.

[0119] Obstacle collision boundary determination: Due to errors in vehicle tracking control, and considering the potential for collisions caused by these errors, it is necessary to extend the actual boundary of the obstacle outwards to ensure that collisions do not occur. The extension method is as follows: Assume that the actual boundary curve of the obstacle can be described by discrete points, and its equation is...

[0120] Y i =f ob (X i ),X i ∈(X obstart ,Xobend )

[0121] like Figure 7 As shown, for each point on the surface of the obstacle, a circle with radius φ is generated with itself as the center. The union of the circles is added to the original obstacle as an extension of the obstacle, and the new boundary generated on the outside is the safety boundary.

[0122] Any point on the newly generated security boundary can be described as:

[0123] (X s ,Y s )∈P safe ={(X i ,Y i )|Y i =f obsafe (X i ),X i ∈(X obsafestart ,X obsafeend )}

[0124] e-SPMT Group Geometric Boundary Determination: During the obstacle avoidance process of the e-SPMT group, its geometric boundary can be described by the e-SPMT unit vertices, cargo vertices, and contour lines. Cargo collisions can be detected by determining the location of the equivalent wheel center (i.e., the e-SPMT unit's center of mass), and whether the e-SPMT unit has collided with an obstacle can be determined by determining the location of the e-SPMT unit's center of mass and its heading angle.

[0125] e-SPMT unit centroid O c1 and O c2 Given global coordinates (X1, Y1) and (X2, Y2), the coordinates of any point on the outer contour of the e-SPMT group can be derived from O. c1 and O c2 Global coordinates and heading angle of e-SPMT unit And the various dimensional parameters are expressed. Then the outer contour curve of the e-SPMT group can be expressed as:

[0126] Φ = Φ1∪Φ2∪Φ3

[0127] That

[0128] Step 3: Design of Collision Constraints

[0129] To avoid collisions between the planned path and obstacles, the cost function must be optimized by considering boundary conditions during the planning process. The constraint for preventing collisions can be described as follows: the geometric boundary of the e-SPMT group is within the road boundary and has no intersection with the obstacle safety boundary.

[0130] Based on the established dynamics / kinematic model of a single e-SPMT unit, the collision constraint condition can be mathematically expressed as:

[0131]

[0132] And Φ∩{P safe ≤0}=φ

[0133] After establishing the e-SPMT kinematic / dynamic model and designing collision constraints, a path planner needs to be designed based on the constraints, and a kinematic and dynamic controller needs to be designed based on the planning results.

[0134] Step 4: Path Planner Design

[0135] The design and planning process of this invention is as follows: The path planner plans the desired path for the e-SPMT group to satisfy the kinematic constraints and boundary conditions of each part during transportation. Path planning employs a multi-objective cost optimization method. The cost function consists of three terms: swept area reflects the passability of the e-SPMT group, distance to obstacles reflects safety, and path smoothness reflects driving stability. The swept area-related term is directly composed of the swept area; the obstacle distance-related term is composed of the shortest distance from the e-SPMT group to the obstacle; and the path smoothness-related term is composed of curvature and the rate of change of heading angle. Each of the three terms occupies a certain weight in the cost function. The swept area, distance to obstacles, and path smoothness can be expressed as the relationship between the swept area, obstacle distance, and path smoothness and the discrete path point (X). r ,Y r Related functions.

[0136] The specific cost function is as follows:

[0137]

[0138] Φ∩{P safe ≤0}=φ

[0139] After optimization, the path planner can derive the optimal path that meets the above constraints, which can be represented as a series of discrete path points.

[0140] Step 5: Design of the Upper-Level Kinematic Controller

[0141] The upper-level kinematic controller uses the model predictive control (MPC) algorithm to generate the expected velocity and yaw rate corresponding to each path point of the SMPT based on the expected path output by the planner and the kinematic constraints.

[0142] First, a reference point is selected based on the desired path, choosing the point closest to the centroid of each e-SPMT element, i.e.:

[0143]

[0144] The expected heading angle of each waypoint can be calculated by combining it with the heading angle of the next waypoint. For the last waypoint, its reference heading angle uses the reference heading angle of the previous waypoint.

[0145]

[0146] Secondly, in the controller design, the system state equations are established. First, the state variable is selected as the tracking error, where the actual tracking error values ​​obtained from the e-SPMT position information are: the difference between the heading angle of each e-SPMT unit and the desired heading angle at the reference point, and the heading angle error. and the lateral error e between the vehicle and the reference path point yi (i = 1, 2), the specific expression is as follows:

[0147]

[0148] Furthermore, based on the kinematic model of a single e-SPMT unit, the kinematic differential equations are written:

[0149]

[0150] Considering It is relatively small during operation, making Linearization is performed near the reference path, and the above equations are simplified to the form of system state equations:

[0151]

[0152] Based on this state equation, an upper-level kinematic controller is designed using the Model Predictive Control (MPC) algorithm. The state vector is selected as... Simultaneously, the lateral velocity, longitudinal velocity, and yaw rate of the e-SPMT unit are selected as control variables u = (V x1d V y1d ω 1d V x2d V y2d ω 2d ) T Let the derivative of the heading angle reference value be denoted as column vector z. Then the above system state equation can be simplified as follows:

[0153]

[0154] The MPC algorithm is suitable for discrete systems, employing an approximate discretization method to discretize the system. The discretized system state equations are as follows:

[0155] x s (k+1)=(E+AT s )xs (k)+BT s u(k)+CT s z(k)

[0156] Among them, T s Let E be the sampling interval and E be the identity matrix.

[0157] The quadratic programming problem has the following objective function:

[0158]

[0159] Δu(k+i|k)=u(k+i|k)-u(k+i-1|k)

[0160] Where ξ(k+i|k) represents the predicted value of the state quantity at time k+i from time k, ξ ref (k+i|k) represents the reference value for the predicted state quantity at time k+i based on the value at time k. The weight matrix Q represents the transformation of the corresponding elements into a standard quadratic form; Δu(k+i|k) represents the predicted value of the control quantity at time k+i based on the value at time kk. The weight matrix R represents the transformation of the corresponding elements into a standard quadratic form; ρ is the weighting coefficient of the relaxation factor ε, which is used to ensure that the quadratic programming problem has a solution.

[0161] Constraints:

[0162] Δu min (k)≤Δu(k+i|k)≤Δu max (k)

[0163] u min (k)≤u(k+i|k)≤u max (k)

[0164]

[0165] Where, Δu min (k), Δu max (k), u min (k), u max (k) represents the minimum and maximum values ​​of the change in the control quantity, and the minimum and maximum values ​​of the control quantity, respectively.

[0166] The optimal control sequence is obtained as follows:

[0167] ΔU * (k)=(Δu * (k), Δu * (k+1), …Δu * (k+N) c-1)) T

[0168] Applying the first control variable of the optimal control sequence to the current system, we obtain the current actual control variable as follows:

[0169] u(k)=u(k-1)+Δu * (k)

[0170] The current actual control quantity is the desired velocity and yaw rate output by the upper kinematic controller.

[0171] Step Six: Design of Dynamic Controller

[0172] The lower-level dynamics controller adopts a sliding mode control (SMC) strategy. It accepts the desired velocity and yaw rate input from the upper-level kinematics controller as state variables and relies on the longitudinal force, additional yaw moment and equivalent rotation angle of each axis of the two e-SPMT units as control variables. The output indirect control variables are transformed into direct control variables such as steering angle and driving force of each wheel through steering angle and driving force distribution rules.

[0173] When designing a sliding mode controller, the differential equations of the single-track dynamics model (i.e., the single-track dynamics model) are simplified and written in the form of a state-space expression. Based on the single-track dynamics model, the system can then be described as follows:

[0174]

[0175] Where, x c =(V x1 V y1 ,ω1,V x2 V y2 ,ω2) T u c =(F cx1 δ cf1 δ cr1 F cx2 δ cf2 δ cr2 ) T F cxi δ represents the longitudinal combined driving braking force of each e-SPMT unit. cfi and δ cri These represent the rotation angles of the equivalent wheel in the e-SPMT unit monorail model.

[0176] The error vector expression for each state variable can be written as follows:

[0177] e = x d -x c

[0178] Where, x d =(Vx1d V y1d ω 1d V x2d V y2d ω 2d ) T The sliding surface is designed as λ is the coefficient matrix.

[0179] Determine the reaching law as is a constant parameter. The reaching law ensures that the system satisfies the reaching condition, that is, the system state gradually approaches the vicinity of the phase plane where s=0.

[0180] Combining the sliding surface and the approach condition, the governing law can be obtained as follows:

[0181]

[0182] By adopting the above control law, the dynamic controller can satisfy the condition that the error e tends to 0 with time, and realize that the lower-level dynamic controller can complete the speed tracking of the upper-level desired value through dynamic control variables.

[0183] Step 7: Assignment of dynamic control variables

[0184] After the dynamic controller outputs control quantities including the resultant longitudinal force and the rotation angle of each axis, it is necessary to obtain the longitudinal force and rotation angle of each wheel through a certain distribution rule.

[0185] First, determine the rotation angles of each wheel. Based on the dynamic model of a single e-SPMT unit, map the equivalent front and rear axle rotation angles to the rotation angles of each wheel on the front and rear axles, respectively.

[0186] δ1→(δ l1 ,δ l2 ,δ r1 ,δ r2 )

[0187] δ2→(δ l3 ,δ l4 ,δ r3 ,δ r4 )

[0188] The acceleration at the center of mass is The turning radius corresponding to the center of mass is Therefore, the rotation angle distribution values ​​for the first two axes are...

[0189]

[0190] The allocation method for the latter two axes is the same as that for the former two axes.

[0191] Secondly, determine the driving force distribution. With the steering angle distribution method already determined, the longitudinal force of each e-SPMT unit is evenly distributed among the wheels, that is, each wheel provides 1 / 8 of the total longitudinal force.

[0192] The lateral forces of each wheel can be expressed as Then the driving force of each wheel is

[0193]

[0194] The rotation angle is given by the rotation angle distribution method, and the lateral force can be given by the e-SPMT unit's center of mass velocity, yaw rate, and rotation angle.

[0195] Therefore, the optimal path obtained by the multi-objective planner is output as the desired speed / angular velocity by the kinematic controller, and then output as two sets of indirect control quantities, namely the resultant longitudinal force and the equivalent rotation angle, by the dynamic controller. These are then converted into direct control quantities such as the driving force and rotation angle at each wheel through the control allocation strategy, thereby completing the control process.

[0196] This application provides an obstacle avoidance planning and control device for a distributed electric-driven modular transport vehicle, comprising: a model building module, a collision boundary determination module, a collision constraint design module, a path planning module, a kinematic controller, a dynamic control quantity generator, and a dynamic control quantity distributor; wherein...

[0197] The model building module is used to convert the two front axes and the two rear axes that are close to each other into two equivalent axes, and to establish a single-track dynamic model using two e-SPMT units.

[0198] The collision boundary determination module is used to determine the collision boundary, including the road boundary, obstacle boundary, and e-SPMT group geometric boundary, based on the road information, obstacle information, and vehicle information obtained by the vehicle perception module.

[0199] The collision constraint design module is used to establish constraint conditions for the e-SPMT group geometric boundary within the road boundary and without intersection with the obstacle safety boundary.

[0200] The path planning module is used to establish a cost function that includes the swept area, the distance to the obstacle, and the path smoothness, and optimize the cost function to obtain the desired path that satisfies the collision constraints.

[0201] The kinematic control instrument generates the desired velocity and yaw rate for each path point in the desired path based on the desired path and kinematic constraints.

[0202] A dynamic controller is used to take the desired speed and yaw rate as state variables, and the longitudinal force, additional yaw moment and equivalent rotation angle of each axis of the two e-SPMT units as control variables. The sliding mode controller is designed and the dynamic control variables are generated according to the single-track dynamic model.

[0203] A dynamic control quantity distributor is used to distribute the dynamic control quantity to the longitudinal force and steering angle of each wheel.

[0204] Compared with the prior art, the embodiments of this application have the following advantages:

[0205] 1. Establish an accurate kinematic / dynamic model of e-SPMT, which can quantitatively describe the kinematic and dynamic constraints; establish a single-track dynamic model of e-SPMT, which can quantify the dynamic laws, and thus be used for controller design. The combination of the two models can ensure the accuracy of the constraints in the planning process, while reducing the complexity of the full-wheel model in the controller design process.

[0206] 2. A comprehensive safety boundary design method is adopted, which takes into account road information, obstacle information and e-SPMT geometric position information, to provide comprehensive collision avoidance constraints for obstacle avoidance planning and ensure the safety of e-SPMT.

[0207] 3. Design a multi-objective optimization path planning method that comprehensively considers the safety and motion performance of e-SPMT, providing a solid foundation for the implementation of the control algorithm.

[0208] 4. Design a hierarchical controller. The upper-level kinematic controller uses the MPC algorithm to achieve path tracking while taking into account kinematic and dynamic constraints. It can consider multiple objectives at the same time and achieve overall performance optimization. The lower-level dynamic controller uses the SMC algorithm to generate indirect control quantities for controlling the e-SPMT based on the output value of the upper-level kinematic controller, ensuring system stability and robustness.

[0209] 5. Design a method for distributing steering angle and driving force, which can decouple the indirect control quantity output by the hierarchical controller and apply it to each omnidirectional wheel of the e-SPMT to achieve bottom-level control and ensure the flexibility, safety and reliability of the e-SPMT during omnidirectional movement.

[0210] In summary, the above are merely preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for obstacle avoidance planning and control of a distributed electric-driven modular transport vehicle, characterized in that, The specific process is as follows: Step 1: Model Establishment: The two front axes and two rear axes that are close to each other are respectively equivalent to two equivalent axes, and two e-SPMT elements are used to establish a single-track dynamic model; Step 2: Collision Boundary Determination: Combining road information, obstacle information, and vehicle information obtained from the vehicle perception module, a collision boundary is established, including the road boundary, obstacle extension safety boundary, and vehicle geometric boundary. Step 3: Collision constraint design: Ensure that the geometric boundary of the e-SPMT group is within the road boundary and has no intersection with the interior of the obstacle safety boundary; Step 4: Path planning: Establish a cost function that includes sweep area, distance to obstacles, and path smoothness, and optimize the cost function to obtain the desired path that satisfies the collision constraints; Step 5: Generating kinematic control variables: Based on the desired path and kinematic constraints, generate the desired velocity and yaw rate corresponding to each path point in the desired path; Step 6: Generation of dynamic control quantities: Using the desired velocity and yaw rate as state quantities, and the longitudinal force, additional yaw moment and equivalent rotation angle of each axis of the two e-SPMT units as control quantities, a sliding mode controller is designed, and dynamic control quantities are generated according to the single-track dynamic model. Step 7: Dynamic control quantity allocation: Allocate the dynamic control quantities to the longitudinal force and rotation angle of each wheel.

2. The obstacle avoidance planning and control method for a distributed electric-driven modular transport vehicle according to claim 1, characterized in that, The single-track dynamic model is as follows: Among them, F xi and F yi δ represents the longitudinal and lateral forces acting on each axis in the monorail model, respectively. i Represents the equivalent rotation angle of each axis under the single-track model; where i = 1, 2 represent the equivalent front and rear axes under the single-track dynamic model of the e-SPMT element, respectively, and F hx and F hy These represent the longitudinal and lateral forces exerted by the cargo on the e-SPMT unit, respectively, V. x and V y These represent the longitudinal and lateral velocities in the vehicle coordinate system, respectively; m is the mass of a single e-SPMT unit, and ω... z I represents the yaw rate of the e-SPMT unit. z d represents the moment of inertia of the e-SPMT element about the z-axis of its center of mass. 2i-1 and d 2i These represent the actual distances from the centroid to the four actual axes in the unsimplified e-SPMT model; Among them, K i This is the equivalent lateral stiffness of the corresponding axis.

3. The obstacle avoidance planning and control method for a distributed electric drive modular transport vehicle according to claim 1, characterized in that, The obstacle boundary is an extension of the actual obstacle boundary. The extension method is as follows: for each point on the obstacle surface, a circle with radius φ is generated with itself as the center. The union of the circles is added to the original obstacle as the extension of the obstacle. The new boundary generated on the outside is the safety boundary.

4. The obstacle avoidance planning and control method for a distributed electric drive modular transport vehicle according to claim 1, characterized in that, The cost function is as follows: Where ω1, ω2, ω3 are the set weighting coefficients, A is the sweeping area, and d i This represents the distance to the i-th obstacle. ∈ represents a smaller constant that ensures the denominator is not zero, κ i Represents the road curvature, ||Δθ i || represents the difference in heading angle between adjacent path points.

5. The obstacle avoidance planning and control method for a distributed electric drive modular transport vehicle according to claim 1, characterized in that, The specific process of designing the upper-level kinematic controller is as follows: First, select a reference point on the desired path, that is, select the point closest to the centroid of each e-SPMT unit; Secondly, the tracking error includes the heading angle error and the lateral and longitudinal tracking errors between each path point and the reference point. The tracking error is used as a state quantity, where the six quantities represent the longitudinal tracking error, lateral tracking error, and heading angle tracking error of the two e-SPMT units, respectively. The lateral velocity, longitudinal velocity, and yaw rate of the e-SPMT unit are selected as the control variables u = (V x1d V y1d ω 1d V x2d V y2d ω 2d ) T ; Let the reciprocal of the heading angle reference value be denoted as column vector z, and the state equation is obtained: Where A represents the state transition matrix, B represents the control matrix, C represents the disturbance transition matrix, and column vector z is considered as the disturbance. Next, the state equation is discretized using the MPC algorithm, and new state variables are added. The control problem is transformed into a quadratic programming problem and solved to obtain the current control quantity u(k).

6. The obstacle avoidance planning and control method for a distributed electric drive modular transport vehicle according to claim 1, characterized in that, By setting an objective function and constraints, the control problem is transformed into a quadratic programming problem for solution. The objective function and constraints are as follows: △u(k+i|k)=u(k+i|k)—u(k+i—1|k) Where ξ(k+i|k) represents the predicted value of the state quantity at time k+i from time k, ξ ref (k+i|k) represents the reference value for the predicted state quantity at time k+i based on the value at time k. The weight matrix Q represents the transformation of the corresponding elements into a standard quadratic form; Δu(k+i|k) represents the predicted value of the control quantity at time k+i based on the value at time kk. This represents the transformation of the corresponding elements into a standard quadratic form using the weight matrix R; ρ represents the weighting coefficients of the relaxation factor ε. Constraints: Δu min (k)≤Δu(k+i|k)≤Δu max (k) u min (k)≤u(k+i|k)≤u max (k) Where, Δu min (k), Δu max (k), u min (k), u max (k) represent the minimum and maximum values ​​of the change in the control quantity, and the minimum and maximum values ​​of the control quantity, respectively; and Let them represent n-dimensional and m-dimensional real vector spaces, respectively; The optimal control sequence is obtained by solving the objective function and constraints. The first control variable Δu of the optimal control sequence is... * (k) is applied to the current system to obtain the current actual control quantity u(k) = u(k-1) + Δu * (k).

7. The obstacle avoidance planning and control method for a distributed electric drive modular transport vehicle according to claim 1, characterized in that, The process of obtaining the dynamic control rate is as follows: When designing a sliding mode controller, based on the aforementioned single-track dynamics model, the system can be described as follows: Where, f(x) c ) and g(x c ) is x c The time-varying function, x, is used to represent the relationship between the derivative of the state variable and the state and control variables. c =(V x1 V y1 ,ω1,V x2 V y2 ,ω2) T V xi V yi ω i These represent the longitudinal velocity, lateral velocity, and yaw rate of each e-SPMT unit, respectively; u c =(F cx1 ,δ cy1 ,δ cr1 ,F cx2 ,δ cf2 ,δ cr2 ) T F cxi δ represents the longitudinal combined driving braking force of each e-SPMT unit. cfi and δ cri These represent the rotation angles of the equivalent wheel in the e-SPMT unit monorail model, i = 1, 2; The expression for calculating the error vector of each state variable is as follows: e=x d -x c Where, x d =(V x1d V y1d ω 1d V x2d V y2d ω 2d ) T , representing the expected values ​​of the longitudinal velocity, lateral velocity, and yaw rate of each e-SPMT unit, respectively, and the sliding surface is designed as follows: λ is the coefficient matrix; Determine the reaching law as ε > 0 is a constant parameter; Combining the sliding surface and the approach condition, the governing law is obtained as follows:

8. The obstacle avoidance planning and control method for a distributed electric drive modular transport vehicle according to claim 1, characterized in that, The specific process for allocating the dynamic control quantities is as follows: First, determine the rotation angle of each wheel, and then map the equivalent front and rear axle rotation angles to the rotation angles of each wheel on the front and rear axles, respectively. δ1→(δ l1 ,d l2 ,d r1 ,d r2 ) δ2→(δ l3 ,d l4 ,d r3 ,d r4 ) The acceleration at the center of mass is The turning radius corresponding to the center of mass is Therefore, the rotation angle distribution values ​​for the first two axes are... Where d represents the wheelbase between two adjacent shafts of the e-SPMT unit, and c represents the wheel track of the e-SPMT unit; The allocation method for the latter two axes is the same as that for the former two axes; Secondly, the driving force distribution is determined: the longitudinal force of each e-SPMT unit is evenly distributed among the wheels, that is, each wheel provides 1 / 8 of the total longitudinal force.

9. A distributed electric-driven modular transport vehicle obstacle avoidance planning and control device, characterized in that, include: The system includes a model building module, a collision boundary determination module, a collision constraint design module, a path planning module, a kinematic controller, a dynamic control quantity generator, and a dynamic control quantity distributor. in The model building module is used to convert the two front axes and the two rear axes that are close to each other into two equivalent axes, and to establish a single-track dynamic model using two e-SPMT units. The collision boundary determination module is used to determine the collision boundary, including the road boundary, obstacle boundary, and e-SPMT group geometric boundary, based on the road information, obstacle information, and vehicle information obtained by the vehicle perception module. The collision constraint design module is used to establish constraint conditions for the e-SPMT group geometric boundary within the road boundary and without intersection with the obstacle safety boundary. The path planning module is used to establish a cost function that includes the swept area, the distance to the obstacle, and the path smoothness, and optimize the cost function to obtain the desired path that satisfies the collision constraints. The kinematic control instrument generates the desired velocity and yaw rate for each path point in the desired path based on the desired path and kinematic constraints. A dynamic controller is used to take the desired speed and yaw rate as state variables, and the longitudinal force, additional yaw moment and equivalent rotation angle of each axis of the two e-SPMT units as control variables. The sliding mode controller is designed and the dynamic control variables are generated according to the single-track dynamic model. A dynamic control quantity distributor is used to distribute the dynamic control quantity to the longitudinal force and steering angle of each wheel.

10. The obstacle avoidance planning and control device for a distributed electric drive modular transport vehicle according to claim 9, characterized in that, The single-track dynamic model is as follows: Among them, F xi and F yi δ represents the longitudinal and lateral forces acting on each axis in the monorail model, respectively. i Represents the equivalent rotation angle of each axis under the single-track model; where i = 1, 2 represent the equivalent front and rear axes under the single-track dynamic model of the e-SPMT element, respectively, and F hx and F hy These represent the longitudinal and lateral forces exerted by the cargo on the e-SPMT unit, respectively, V. x and V y These represent the longitudinal and lateral velocities in the vehicle coordinate system, respectively; m is the mass of a single e-SPMT unit, and ω... z I represents the yaw rate of the e-SPMT unit. z d represents the moment of inertia of the e-SPMT element about the z-axis of its center of mass. 2i-1 and d 2i These represent the actual distances from the centroid to the four actual axes in the unsimplified e-SPMT model; Among them, K i This is the equivalent lateral stiffness of the corresponding axis.

Images