Box pushing control method for unmanned vehicle

Abstract

The application discloses a box pushing control method for an unmanned vehicle, which comprises the following steps: establishing an unmanned vehicle dynamics model and a box pushing motion model based on the relative action between the unmanned vehicle and the box and the structure of the box; determining the unmanned vehicle motion constraint when the unmanned vehicle and the box keep stable contact based on the box pushing motion model, the relative position relationship and the relative action between the unmanned vehicle and the box during the pushing process; constructing a rolling horizon trajectory optimization problem of the unmanned vehicle based on the unmanned vehicle dynamics model and the unmanned vehicle motion constraint, solving the rolling horizon trajectory optimization problem to obtain the control input of the unmanned vehicle, and controlling the unmanned vehicle according to the control input. The method can realize the stable pushing control of the unmanned vehicle on the box and ensure the stable contact between the unmanned vehicle and the box during the pushing process by analyzing and calculating the motion constraint required for the stable contact between the unmanned vehicle and the box and explicitly introducing the motion constraint into the control of the unmanned vehicle.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned vehicle control, and particularly relates to a box pushing control method for an unmanned vehicle. BACKGROUND

[0002] With the development of automation technology, autonomous mobile robots such as wheeled unmanned vehicles begin to be widely applied to new unmanned factories, intelligent warehouses and other scenarios to perform autonomous cargo handling tasks, such as moving a cargo box on the ground of a warehouse from one location to another. The most commonly used handling operation method at present is a gripping operation method, which installs a mechanical arm on the unmanned vehicle, grips the cargo through the mechanical arm, and then controls the unmanned vehicle to move to the target position and then puts down the cargo through the mechanical arm. However, when the volume of the object to be handled is large or the weight is heavy, the mechanical arm cannot grip, and at this time the handling task cannot be completed. Another handling method is a non-gripping operation method, which directly contacts the unmanned vehicle with the box, and moves the box to the target position through the movement of the vehicle. Among them, the existing box pushing control method of the unmanned vehicle is mainly based on a reactive control, that is, during the process of pushing the box, the unmanned vehicle constantly adjusts its position and orientation in a reactive manner according to the state of the box, so that the unmanned vehicle and the box can maintain contact as much as possible without separation, until the box is pushed to the target position.

[0003] However, the existing reactive box pushing control method cannot guarantee the stable contact between the unmanned vehicle and the box, and the two may separate or slide relative to each other during the pushing process, and the pushing operation process is unstable and low in efficiency. SUMMARY

[0004] To solve the above-mentioned technical problems in the prior art, the present application provides a box pushing control method for an unmanned vehicle.

[0005] The technical scheme of the present application is as follows:

[0006] A box pushing control method for an unmanned vehicle is provided, which comprises:

[0007] Based on the relative action between the unmanned vehicle and the box and the structure of the box, an unmanned vehicle dynamics model and a box pushing motion model are established;

[0008] Based on the box pushing motion model and the relative position relationship and the relative action between the unmanned vehicle and the box during the pushing process, the motion constraint of the unmanned vehicle when the unmanned vehicle and the box maintain stable contact is determined;

[0009] Based on the unmanned vehicle dynamics model and the unmanned vehicle motion constraint, a rolling horizon trajectory optimization problem of the unmanned vehicle is constructed, the rolling horizon trajectory optimization problem is solved to obtain control input of the unmanned vehicle, and the unmanned vehicle is controlled according to the control input.

[0010] In some possible implementation manners, an unmanned vehicle dynamics model is established as follows:

[0011]

[0012] wherein, and respectively represent derivatives of x r , y r , θ r , v r and ω r , x r and y r represent horizontal coordinates and vertical coordinates of the center of the unmanned vehicle in a set world coordinate system O W X W Y W , θ r represents an orientation angle of the unmanned vehicle, v r and ω r respectively represent linear velocity and angular velocity of the unmanned vehicle, a r and ξ r respectively represent linear acceleration and angular acceleration of the unmanned vehicle.

[0013] In some possible implementation manners, it is set that the front end surface of the unmanned vehicle is kept in contact with the box during pushing, the contact force points are two vertices of the front end surface of the unmanned vehicle, and the whole pushing process is quasi-static.

[0014] An box pushing motion model is established as follows:

[0015]

[0016] wherein, w p =[f p,x ,f p,y ,τ p ] T represents a pushing force wrench generated by the pushing force of the unmanned vehicle on the box, f p,x and f p,y respectively represent components of the pushing force on the horizontal coordinate axis and the vertical coordinate axis of the box body coordinate system, τ p represents a torque, represents a transpose of w p , H represents a matrix related to inherent parameters of the box, γ o represents a parameter related to the size of the box, μ g denotes the friction coefficient between the box and the ground, m o denotes the mass of the box, g denotes the gravity coefficient, L o and W o denote the length and the width of the box, v o = [v o,x , v o,y , ω o ] T denotes the motion screw of the box in the box body coordinate system, v o,x and v o,y denote the velocity component of the box in the horizontal coordinate axis and the velocity component in the vertical coordinate axis of the box body coordinate system, ω o denotes the rotation angular velocity of the box.

[0017] In some possible implementation manners, based on the box pushing motion model and the relative position relationship and the relative action between the unmanned vehicle and the box during pushing, the unmanned vehicle motion constraint when the unmanned vehicle and the box maintain stable contact is determined, including:

[0018] determining a friction cone satisfied by the pushing force of the unmanned vehicle on the box according to the relative position relationship and the friction coefficient between the unmanned vehicle and the box during pushing;

[0019] determining a convex cone constraint satisfied by the pushing force screw of the unmanned vehicle on the box according to the friction cone satisfied by the pushing force of the unmanned vehicle on the box;

[0020] determining a cone constraint satisfied by the box motion screw according to the convex cone constraint satisfied by the pushing force screw of the unmanned vehicle on the box and the box pushing motion model;

[0021] determining the box motion constraint according to the cone constraint satisfied by the box motion screw and the constraint condition satisfied by the box velocity during pushing;

[0022] determining the unmanned vehicle motion constraint when the unmanned vehicle and the box maintain stable contact according to the box motion constraint.

[0023] In some possible implementation manners, the friction cone satisfied by the pushing force of the unmanned vehicle on the box is determined in the following manner:

[0024] a friction angle is determined according to the friction coefficient between the unmanned vehicle and the box, the contact force point between the unmanned vehicle and the box is taken as the vertex of the friction cone, and the friction angle is taken as the included angle between the edge of the friction cone and the central normal of the contact surface, so as to determine the friction cone.

[0025] In some possible implementation manners, two pushing forces f1 and f2 generated by the unmanned vehicle on the box during the pushing process are set, and the pushing force moment w generated by the pushing forces f1 and f2 on the box is respectively p,1 and w p,2 ;

[0026] The convex cone constraint satisfied by the pushing force moment of the unmanned vehicle on the box is determined in the following manner:

[0027] A decomposition expression of the pushing force along the direction of the friction cone edge is determined;

[0028] According to the limiting surface of the pushing force moment distribution, a linear transformation relationship formula of the pushing force and the pushing force moment corresponding to the pushing force is determined;

[0029] For each pushing force, a unit vector of the pushing force located in the direction of the friction cone edge is selected, and two force moment components corresponding to the pushing force are calculated according to the decomposition expression and the linear transformation relationship formula;

[0030] Four force moment components corresponding to two pushing forces are used as edges of a four-sided pyramid to form the convex cone constraint.

[0031] In some possible implementation manners, the cone constraint satisfied by the box motion moment is determined in the following manner:

[0032] Four force moment components on the limiting surface of the pushing force moment distribution are calculated;

[0033] According to the force moment on the limiting surface of the pushing force moment distribution and the box pushing motion model, four motion moment components corresponding to the force moment on the limiting surface of the four pushing force moment distributions are calculated;

[0034] Four motion moment components are used as edges of a four-sided pyramid to form the cone constraint.

[0035] In some possible implementation manners, the constraint condition satisfied by the box velocity during the pushing process is:

[0036] ω o =ω r ,v o,x =v r ≥0,

[0037] The box motion constraint is:

[0038] v o,x ≥0,k′v o,x ≤ω o ≤k″v o,x ,

[0039] The unmanned vehicle motion constraint is:

[0040] v r ≥0,k′v r ≤ω r ≤k″v r

[0041] where ω o denotes the angular velocity of the box, ω r denotes the angular velocity of the unmanned vehicle, v r denotes the linear velocity of the unmanned vehicle, v o,x and v o,y denote the velocity components of the box in the lateral and longitudinal axes of the body coordinate system of the box, L o denotes the length of the box, L r denotes the length of the unmanned vehicle.

[0042] k′ and k″ are determined in the following way:

[0043] connecting and two straight lines are obtained, and denote the motion screw components corresponding to the thrust f1, and denote the motion screw components corresponding to the thrust f2.

[0044] The two intersection points of the two straight lines and the semi-plane formed by the constraint condition satisfied by the velocity of the box during the pushing process are calculated;

[0045] The two intersection points are projected onto the V X -ω plane, and the slopes of the lines connecting the origin of the V X -ω plane to the two projected points are determined to obtain k′ and k″, V X denotes the velocity unit vector of the box in the lateral axis of the body coordinate system of the box, and ω denotes the angular velocity unit vector of the box.

[0046] In some possible implementation manners, the rolling horizon trajectory optimization problem of the unmanned vehicle is represented as:

[0047]

[0048] s.t.

[0049]

[0050]

[0051]

[0052]

[0053]

[0054]

[0055] where J denotes the objective function, denotes the target point position of the box, denotes the center point position of the box at the Nth time step, denotes the control input of the unmanned vehicle at the kth time step, q control and q goal denotes the weight coefficient, denotes the state of the unmanned vehicle at the 0th time step, x r (t0) denotes the current state of the unmanned vehicle, denotes the discretized unmanned vehicle dynamics model, denotes the state of the unmanned vehicle at the kth time step, f r denotes the discretized nonlinear model, the state of the unmanned vehicle at the k-1th time step, denotes the control input of the unmanned vehicle at the k-1th time step, a min denotes the minimum linear acceleration of the unmanned vehicle, a max denotes the maximum linear acceleration of the unmanned vehicle, denotes the linear acceleration of the unmanned vehicle at the kth time step, ξ min denotes the minimum angular acceleration of the unmanned vehicle, ξ max denotes the maximum angular acceleration of the unmanned vehicle, denotes the angular acceleration of the unmanned vehicle at the kth time step, denotes the linear velocity of the unmanned vehicle at the kth time step, denotes the angular velocity of the unmanned vehicle at the kth time step, N denotes the number of optimization window steps.

[0056] In some possible implementation manners, the solving the receding horizon trajectory optimization problem obtains the control input of the unmanned vehicle, and the unmanned vehicle is controlled according to the control input, including:

[0057] solving the receding horizon trajectory optimization problem, obtaining the optimal state trajectory of the unmanned vehicle at the 1th time step to the Nth time step and the optimal control input of the unmanned vehicle at the 0th time step to the N-1th time step

[0058] controlling the unmanned vehicle by using the first optimal control input ;

[0059] After a preset sampling time, the state of the unmanned vehicle is updated, a recast and solved receding horizon trajectory optimization problem is obtained, the optimal state trajectory and optimal control input are obtained, the first optimal control input is used to control the unmanned vehicle, and the process is repeated until the box reaches the target point position.

[0060] The main advantages of the technical scheme of the present application are as follows:

[0061] The box pushing control method for the unmanned vehicle can realize stable pushing control of the box by the unmanned vehicle by analyzing and calculating the motion constraints required for the stable contact between the unmanned vehicle and the box and explicitly introducing the motion constraints into the control of the unmanned vehicle, and ensures the stable contact between the unmanned vehicle and the box during the pushing process. BRIEF DESCRIPTION OF DRAWINGS

[0062] The accompanying drawings, which are included to provide a further understanding of the embodiments of the present application, constitute a part of the present application and the illustrative embodiments of the present application and their descriptions serve to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0063] Figure 1 A flowchart of the box pushing control method for the unmanned vehicle according to an embodiment of the present application;

[0064] Figure 2 A schematic diagram of the relative position relationship between the unmanned vehicle and the box during the pushing process according to an embodiment of the present application;

[0065] Figure 3 A schematic diagram of the restriction surface of the pushing force screw distribution according to an embodiment of the present application;

[0066] Figure 4 A schematic diagram of the determination method of the friction cone of the unmanned vehicle and the box according to an embodiment of the present application;

[0067] Figure 5 A schematic diagram of the solving process of the pushing force screw cone according to an embodiment of the present application;

[0068] Figure 6 A schematic diagram of the solving process of the box motion screw cone according to an embodiment of the present application;

[0069] Figure 7 A schematic diagram of the solving process of the three-dimensional cone surface where the box motion screw is located according to an embodiment of the present application;

[0070] Figure 8 A schematic diagram of the box motion constraint projection according to an embodiment of the present application. DETAILED DESCRIPTION

[0071] In order to make the objects, technical solutions and advantages of the present application clearer, the technical solutions of the present application will be described below in connection with the specific embodiments of the present application and corresponding drawings. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments of the present application, all other embodiments obtained by a person of ordinary skill in the art without creative work fall within the protection scope of the present application.

[0072] The technical solutions provided by the embodiments of the present application will be described in detail below in connection with the drawings.

[0073] Reference Figure 1 An embodiment of the present application provides a box pushing control method for an unmanned vehicle, which comprises the following steps S1-S3.

[0074] Step S1, based on the relative action between the unmanned vehicle and the box and the structure of the box, an unmanned vehicle dynamics model and a box pushing motion model are established.

[0075] Reference Figure 2 In an embodiment of the present application, it is defined that: r = [x r , y r , θ r , v r , ω r ] T represents the state of the unmanned vehicle, p r = [x r , y r ] T represents the coordinate position of the center O R of the unmanned vehicle in the set world coordinate system O W X W Y W , x r and y r represent the horizontal coordinate and the vertical coordinate of the center of the unmanned vehicle in the set world coordinate system O W X W Y W , θ r represents the orientation angle of the unmanned vehicle, v r and ω r respectively represent the linear velocity and the angular velocity of the unmanned vehicle, a r and ξ r respectively represent the linear acceleration and the angular acceleration of the unmanned vehicle, L r and W r respectively represent the length and the width of the unmanned vehicle.

[0076] Based on the above definitions, in an embodiment of the present application, the following unmanned vehicle dynamics model is established:

[0077]

[0078] wherein, and denote the derivatives of x r , y r , θ r , v r and ω r , respectively.

[0079] Further, it is defined that μ g denotes the friction coefficient between the box and the ground, m o denotes the mass of the box, g denotes the gravity coefficient, L o and W o denote the length and the width of the box, respectively, and μ p denotes the friction coefficient between the unmanned vehicle and the box.

[0080] Referring to Figure 2 , since the unmanned vehicle keeps stable contact with the box during the pushing process, it is set that the width of the box is greater than the width of the unmanned vehicle, and the front end surface of the unmanned vehicle keeps contact with the box during the pushing process. At this time, the contact model can be simplified as a line segment contact, and the contact force points are the two end points of the line segment, i.e. the two vertices C1 and C2 of the front end surface of the unmanned vehicle.

[0081] Further, it is set that the two pushing forces of the unmanned vehicle on the box generated by the contact are f1 and f2, and the pushing force moment of the pushing forces f1 and f2 on the box is w p =[f p,x ,f p,y ,τ p ] T , f p,x and f p,y denote the components on the horizontal coordinate axis and the components on the vertical coordinate axis of the pushing forces in the body coordinate system O O X O Y O of the box, and τ p denotes the pushing moment, and the motion moment of the box in its body coordinate system O O X O Y O during the pushing process is v o =[v o,x ,v o,y ,ω o ] T , v o,x and v o,y denote the velocity components of the box on the horizontal coordinate axis and the vertical coordinate axis of the body coordinate system of the box, respectively, and ω o denotes the angular velocity of the box.

[0082] Referring toFigure 3 Assuming the entire propulsion process is quasi-static, the force spinor generated by the propulsion is balanced with the force spinor generated by ground friction during the box's motion. According to the theory of confined surfaces, the propulsion force spinor is distributed on a closed convex surface, which can be approximated by an ellipsoidal surface. Meanwhile, under the quasi-static motion model, the propulsion force spinor w... p To constrain points on the curved surface, the box motion spinor v o The direction is the same as the direction of the normal vector at that point. (Appendix) Figure 3 In the diagram, Γ represents the unit thrust vector, and F... X F represents the unit vector of thrust on the horizontal axis of the box-body coordinate system. Y This represents the unit vector of thrust along the vertical axis of the box-body coordinate system.

[0083] Based on the above settings and analysis, in one embodiment of the present invention, the following box-pushing motion model is established:

[0084]

[0085] in, Indicates w p The transpose of , where H represents the matrix related to the inherent parameters of the box. γ o This indicates parameters related to the dimensions of the enclosure.

[0086] Step S2: Based on the box-pushing motion model and the relative positional relationship and relative interaction between the unmanned vehicle and the box during the pushing process, determine the motion constraints of the unmanned vehicle when it maintains stable contact with the box.

[0087] In order to ensure that the unmanned vehicle and the container maintain stable contact during the movement of the unmanned vehicle, and neither separate nor slip relative to each other, the movement of the unmanned vehicle needs to meet certain constraints. The following explains in detail how to determine the movement constraints of the unmanned vehicle when it maintains stable contact with the container.

[0088] In one embodiment of the present invention, based on the box-pushing motion model and the relative positional relationship and relative interaction between the unmanned vehicle and the box during the pushing process, the motion constraints of the unmanned vehicle when maintaining stable contact with the box are determined, further including the following steps S21-S25:

[0089] Step S21: Based on the relative positional relationship and friction coefficient between the unmanned vehicle and the box during the pushing process, determine the friction cone that the thrust of the unmanned vehicle on the box satisfies.

[0090] refer to Figure 4Since the unmanned vehicle generates thrust f1 and f2 on the box through contact, according to Coulomb's law of friction, if there is no relative sliding between the unmanned vehicle and the box, the thrust f1 and f2 need to be in the friction cone with the contact point as the vertex. At the same time, the angle between the edge of the friction cone and the normal of the center of the contact surface is the friction angle.

[0091] In one embodiment of the present invention, the friction angle is calculated using the following formula:

[0092] θ μ =arctanμ p

[0093] Where, θ μ This indicates the angle of friction.

[0094] Step S22: Based on the friction cone satisfied by the thrust of the unmanned vehicle on the box, determine the convex cone constraint satisfied by the rotation of the thrust force of the unmanned vehicle on the box.

[0095] refer to Figure 5 During the pushing process, the thrusts f1 and f2 are set to generate thrust spindle w on the box, respectively. p,1 and w p,2 The cone constraint satisfied by the spinor of the unmanned vehicle's thrust on the box is determined using the following method:

[0096] Determine the decomposition expression of the thrust along the direction of the edge of the friction cone;

[0097] Based on the limiting surface of the thrust spinor distribution, determine the linear transformation relationship between thrust and its corresponding thrust spinor;

[0098] For each thrust, select its unit vector located on the edge of the friction cone, and calculate the two force spinor components corresponding to the thrust based on the decomposition expression and the linear transformation relationship.

[0099] Using the four force spinor components corresponding to the two thrusts as the edges of the tetrahedron, a convex cone constraint is formed.

[0100] Specifically, the decomposition expression of the thrust along the direction of the edge of the friction cone is as follows:

[0101]

[0102] Among them, f1 L f1 R , and Represents the unit vector of the edge of the friction cone. and The corresponding thrust component magnitude,

[0103] Based on the geometric relationship satisfied by the thrust spinor, the linear transformation relationship between thrust and its corresponding thrust spinor is expressed as:

[0104] w p,1 =J1f1,w p,2 =J2f2

[0105] in,

[0106] For the thrusts f1 and f2 represented in the above decomposition expression, we choose their unit vectors located along the edge of the friction cone, i.e. and Then, based on the above decomposition expression and linear transformation relationship, the two force spinor components corresponding to the thrust f1 are calculated. and and the two force spinor components corresponding to the thrust f2 and

[0107] Since both thrusts f1 and f2 satisfy the cone constraint, the force spinor w obtained after linear transformation p,1 w p,2 It also satisfies the cone constraint, and the force spinor w p,1 w p,2 The combined driving force of the spindle w p Also constrained by w p,1 w p,2 In the convex cone formed, that is, the spinor of the driving force w p Located in the four force spinors In a tetrahedron formed by the edges.

[0108] Step S23: Based on the convex cone constraint satisfied by the rotation of the unmanned vehicle's pushing force on the box and the box's pushing motion model, determine the cone constraint satisfied by the rotation of the box's motion.

[0109] In one embodiment of the present invention, the cone constraint satisfied by the rotation of the box motion is determined in the following way:

[0110] Calculate the force spinor components of the four force spinor components on the constrained surface of the driving force spinor distribution;

[0111] Based on the force screw on the limiting surface of the thrust screw distribution and the box-type thrust motion model, calculate the four motion screw components corresponding to the force screw on the limiting surface of the four thrust screw distributions;

[0112] Using the four kinematic spinor components as the edges of the tetrahedron, a cone constraint is formed.

[0113] Specifically, based on the foregoing analysis, the propulsive spindle satisfies the tetrahedral constraint, and simultaneously, according to the box-type propulsion motion model... The driving force spinor must be distributed on the confining surface. This applies to the four edges of the driving force spinor cone obtained in step S22 above. The force spinor on the corresponding restricted surface can be calculated separately.

[0114] Regarding how to calculate The corresponding force spinor on the limiting surface, as follows: Let's take an example to illustrate:

[0115] Assumption for The corresponding force spinor on the confined surface, λ > 0, will Substitution From this, we can obtain an equation about λ. Solving this equation will give us λ, and then we can use... λ>0

[0116] Referring to the above Force spinor on the corresponding confined surface The solution method can be used to obtain... Force spinor on the corresponding constraint surface

[0117] refer to Figure 6 In one embodiment of the present invention, the driving force spindle w p Located at the intersection of the driving force spinor cone and the limiting surface, the four vertices of this intersection are...

[0118] Furthermore, based on the v in the box-driven motion model o ∝Hw p box motion spinor v o The direction of the force spinor on the limiting surface is consistent with the direction of the normal vector of the force spinor. Therefore, the spinor of the box motion v can be calculated. o The cone constraint that is satisfied.

[0119] refer to Figure 6 Specifically, targeting According to v o ∝Hw p Calculate the corresponding kinematic spinor components respectively. Then the spinor of the box motion v o Located in Within the tetrahedron formed. (Attached) Figure 6 In this context, ω represents the unit vector of the box's rotational angular velocity, V X V represents the unit velocity vector of the box on the horizontal axis of the box's body coordinate system. Y This represents the unit velocity vector of the box on the vertical axis of the box's body coordinate system.

[0120] Step S24, according to the cone constraint satisfied by the box motion screw and the constraint condition satisfied by the box velocity in the pushing process, determine the box motion constraint.

[0121] Since the unmanned vehicle and the box need to maintain stable contact without separation or sliding during the pushing process, the angular velocity of the box rotation is the same as that of the unmanned vehicle, the speed component of the box in the x-axis direction of the body coordinate system is the same as the linear speed of the unmanned vehicle and cannot be negative, and the speed component in the y-axis direction satisfies the circular motion constraint, that is, the box velocity in the pushing process satisfies the following constraint condition:

[0122] ω o = ω r , v o,x = v r ≥ 0,

[0123] Referring to Figure 7 , the constraint condition satisfied by the box velocity in the pushing process constitutes a half-plane in the box motion screw space, and since the box motion screw is also located in the four-sided cone obtained in the above step S23, by calculating the intersection of the four-sided cone and the half-plane, a three-dimensional conical surface O-v′ o v″ o .

[0124] In an embodiment of the present application, two straight lines are obtained by connecting and , the equations of the two straight lines are solved together with the constraint equation satisfied by the box velocity in the pushing process to obtain two intersection points v′ o and v″ o of the half-plane constituted by the constraint condition satisfied by the box velocity in the pushing process, and further determine the three-dimensional conical surface O-v′ o v″ o .

[0125] Referring to Figure 8 , set v′ o and v″ o calculated as Project v′ o and v″ o onto the V X -ω plane to obtain the slopes of the two edges of the projected conical surface and

[0126] Based on the obtained k′ and k″, the box motion screw v o = [v o,x , vo,y ,ω o ] T Satisfy the motion constraint:

[0127] v o,x ≥0,k′v o,x ≤ω o ≤k″v o,x ,

[0128] Step S25, according to the box motion constraint, determine the unmanned vehicle motion constraint when the unmanned vehicle and the box keep stable contact.

[0129] According to the constraint equation ω o = ω r ,v o,x = v r ≥0, The box motion constraint can be converted into the unmanned vehicle motion constraint.

[0130] Specifically, the unmanned vehicle motion constraint is:

[0131] v r ≥0,k′v r ≤ ω r ≤ k″v r .

[0132] Step S3, based on the unmanned vehicle dynamics model and the unmanned vehicle motion constraint, construct a receding horizon trajectory optimization problem of the unmanned vehicle, solve the receding horizon trajectory optimization problem to obtain the control input of the unmanned vehicle, and control the unmanned vehicle according to the control input.

[0133] How to construct the receding horizon trajectory optimization problem of the unmanned vehicle based on the unmanned vehicle dynamics model and the unmanned vehicle motion constraint is described below.

[0134] Specifically, the optimization time window of the unmanned vehicle control process is set to NΔt, and Δt represents the sampling time, and N represents the optimization window step number.

[0135] In order to construct the receding horizon trajectory optimization problem of the unmanned vehicle, it is necessary to determine the discretized dynamics model of the unmanned vehicle.

[0136] Specifically, for the differential form of the unmanned vehicle dynamics model established in the above step S1, the fourth-order Runge-Kutta method is used for discretization, and the following discretized unmanned vehicle dynamics model is obtained:

[0137]

[0138] Wherein, represents the state of the unmanned vehicle at the k+1 time step, f rdenotes a discretized nonlinear model, a state of the unmanned vehicle at the kth time step, denotes a control input of the unmanned vehicle at the kth time step.

[0139] Further, in order to construct the receding horizon trajectory optimization problem of the unmanned vehicle, a target function also needs to be designed.

[0140] Specifically, in an embodiment of the present application, the target function is designed as:

[0141]

[0142] wherein J k denotes a process target function term, J N denotes a terminal target function term, q control and q goal denote weight coefficients, which are specifically set according to actual conditions.

[0143] The process target function term is used to minimize the control demand of the unmanned vehicle, and is specifically represented as:

[0144]

[0145] Suppose that the unmanned vehicle needs to push the box to a target point position The terminal target function term is represented as:

[0146]

[0147] wherein, denotes a center point position of the box at the Nth time step, ||·|| denotes a 2-norm, is calculated by using the following formula:

[0148]

[0149] denotes a center position of the unmanned vehicle at the Nth time step, denotes a two-dimensional rotation matrix, denotes an orientation angle of the unmanned vehicle at the Nth time step.

[0150] Further, in order to construct the receding horizon trajectory optimization problem of the unmanned vehicle, a control constraint of the unmanned vehicle also needs to be designed.

[0151] Specifically, in an embodiment of the present application, the control constraint of the unmanned vehicle includes a control input constraint and a motion constraint of the unmanned vehicle.

[0152] The control input constraint is determined according to the actual control condition of the unmanned vehicle, and is specifically represented as:

[0153]

[0154] Since the unmanned vehicle needs to maintain stable contact with the box during pushing, the motion constraints are expressed as:

[0155]

[0156] where a min denotes the minimum linear acceleration of the unmanned vehicle, a max denotes the maximum linear acceleration of the unmanned vehicle, denotes the linear acceleration of the unmanned vehicle at the kth time step, ξ min denotes the minimum angular acceleration of the unmanned vehicle, ξ max denotes the maximum angular acceleration of the unmanned vehicle, denotes the angular acceleration of the unmanned vehicle at the kth time step, denotes the linear velocity of the unmanned vehicle at the kth time step, denotes the angular velocity of the unmanned vehicle at the kth time step.

[0157] Based on the discretized dynamics model, the objective function, and the control constraints of the unmanned vehicle set above, the following receding horizon trajectory optimization problem is constructed:

[0158]

[0159] s.t.

[0160]

[0161]

[0162]

[0163]

[0164]

[0165]

[0166] where x denotes the state of the unmanned vehicle at the 0th time step, x r (t0) denotes the current state of the unmanned vehicle, denotes the discretized dynamics model of the unmanned vehicle, denotes the state of the unmanned vehicle at the k-1th time step, denotes the control input of the unmanned vehicle at the k-1th time step.

[0167] Further, in an embodiment of the present application, the control input of the unmanned vehicle is obtained by solving the receding horizon trajectory optimization problem, and the unmanned vehicle is controlled according to the control input, comprising:

[0168] solving the receding horizon trajectory optimization problem to obtain the optimal state trajectory of the unmanned vehicle at the first time step to the Nth time step and the optimal control input of the unmanned vehicle at the 0th time step to the N-1th time step

[0169] using the first optimal control input to control the unmanned vehicle;

[0170] After a preset sampling time, the state of the unmanned vehicle is updated, the receding horizon trajectory optimization problem is reconstructed and solved again to obtain the optimal state trajectory and the optimal control input, the unmanned vehicle is controlled by using the first optimal control input, and the process is repeated until the box reaches the target point.

[0171] Wherein, the preset sampling time is the time interval Δt of the time step.

[0172] In an embodiment of the present application, when solving the receding horizon trajectory optimization problem, the trajectory optimization problem is converted into a nonlinear optimization problem by the multi-point shooting method, and then solved by a general nonlinear optimization solver such as IPOPT.

[0173] The box pushing control method for the unmanned vehicle provided by an embodiment of the present application can realize stable pushing control of the box by the unmanned vehicle by analyzing and calculating the motion constraints required for the unmanned vehicle to maintain stable contact with the box and explicitly introducing the motion constraints into the control of the unmanned vehicle, and can ensure stable contact between the unmanned vehicle and the box during the pushing process.

[0174] The beneficial effects of the box pushing control method for the unmanned vehicle provided by an embodiment of the present application are described below in combination with specific examples:

[0175] In this example, a wheeled unmanned vehicle is used to control the pushing of the box, and the related parameter configurations of the wheeled unmanned vehicle and the box are shown in Table 1.

[0176] Table 1 shows the example parameter configuration

[0177] Length of unmanned vehicle L r / m]] 1.00 Unmanned vehicle lane acceleration range [a min , max ] / ms -2 ]]> [-1.0,1.0] Width of the vehicle W r / m]]> 0.48 Unmanned vehicle angular acceleration range [ξ min ,ξ max ] / s -2 ]]> [-0.4,0.4] Box length L o / m]] 0.32 The unmanned vehicle and the box friction coefficient μ p ]]> 0.27 Box width W o / m]] 0.50 Coefficient of friction of the box with the ground μ g ]]> 0.30 Box mass m o / kg 2.80 Box size parameter γ o ]] 39.47

[0178] Based on the above parameters, the friction angle of the contact surface friction cone is calculated as θ μ = 15.11°.

[0179] The unmanned vehicle pushing force moment cone is calculated, and based on the above parameters, the force moment on the four edges of the pushing force moment cone is calculated

[0180] The box motion screw cone is calculated based on the above parameters. Firstly, the force screw on the limiting surface in the direction of the four edges of the force screw cone is calculated Then the corresponding box motion screw cone is calculated

[0181] The unmanned vehicle motion constraint is calculated based on the above parameters. Firstly, v′ representing the surface constraint of the box motion screw cone is calculated o = [0.90, 0.24, -0.37] T and v″ o = [0.90, 0.24, 0.37] T are calculated; then the projected cone edge slopes k′ = -0.41 and k″ = 0.41 are calculated; finally, the motion constraint required for the unmanned vehicle to maintain stable contact with the box is calculated:

[0182] v r ≥ 0, -0.41v r ≤ ω r ≤ 0.41v r

[0183] The rolling time domain trajectory optimization problem is constructed. The rolling optimization window step number is designed as N = 20, the sampling time is designed as Δt = 0.1 seconds, the target function term weight coefficient is designed as q control = 0.1, q goal = 2.0, and the rolling time domain trajectory optimization problem is constructed as follows:

[0184]

[0185] s.t.

[0186]

[0187]

[0188]

[0189]

[0190]

[0191]

[0192] The above optimization problem is solved in real time using the IPOPT optimizer to obtain the control quantity of the unmanned vehicle, and the unmanned vehicle is controlled based on the control quantity, which can stably push the box to the target position.

[0193] It should be noted that the relational terms herein such as first and second and the like are used solely to distinguish one from another entity or action without necessarily requiring or implying any actual relationship or order between such entities or actions. Moreover, the terms "comprises", "comprising", or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but can include other elements not expressly listed or inherent to such process, method, article, or apparatus.

[0194] Finally, it should be noted that the above embodiments are merely used for describing the technical solutions of the present application, rather than limiting them; even though the present application has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still make modifications to the technical solutions recorded in the foregoing embodiments, or make equivalent replacements to some technical features therein; and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A box pushing control method for an unmanned vehicle, characterized by, The method comprises the following steps: Based on the relative action between the unmanned vehicle and the box, and the structure of the box, the unmanned vehicle dynamics model and the box pushing motion model are established; Based on the box pushing motion model, and the relative position relationship and the relative action between the unmanned vehicle and the box during the pushing process, the unmanned vehicle motion constraint when the unmanned vehicle and the box maintain stable contact is determined; Based on the unmanned vehicle dynamics model and the unmanned vehicle motion constraint, the rolling horizon trajectory optimization problem of the unmanned vehicle is constructed, the rolling horizon trajectory optimization problem is solved to obtain the control input of the unmanned vehicle, and the unmanned vehicle is controlled according to the control input; The rolling horizon trajectory optimization problem of the unmanned vehicle is represented as: ； wherein, represents an objective function, , , , represents a target point position of the box, represents a center point position of the box at the th time step, represents a control input of the unmanned vehicle at the th time step, and represents a weight coefficient, represents a state of the unmanned vehicle at the 0th time step, represents a current state of the unmanned vehicle, represents a discretized unmanned vehicle dynamics model, represents a state of the unmanned vehicle at the th time step, represents a discretized nonlinear model, a state of the unmanned vehicle at the th time step, represents a control input of the unmanned vehicle at the th time step, represents a minimum linear acceleration of the unmanned vehicle, represents a maximum linear acceleration of the unmanned vehicle, represents a linear acceleration of the unmanned vehicle at the th time step, represents a minimum angular acceleration of the unmanned vehicle, represents a maximum angular acceleration of the unmanned vehicle, represents an angular acceleration of the unmanned vehicle at the th time step, represents a linear velocity of the unmanned vehicle at the th time step, represents an angular velocity of the unmanned vehicle at the th time step, represents an optimization window step number; and was determined using the following: connection and resulting in two straight lines, and denote the thrust corresponding components of the motion spinor, and denote the thrust corresponding components of the motion spinor; Calculate the intersection of two straight lines and the semi-plane formed by the constraint condition satisfied by the box velocity during the pushing process; Project the two intersection points onto the plane, respectively determine the slopes of the lines from the plane origin to the two projected points, and obtain and , and , denotes the velocity unit vector of the box in the horizontal coordinate axis of the box body coordinate system, denotes the rotation angular velocity unit vector of the box.

2. The box pushing control method for an unmanned vehicle according to claim 1, wherein The unmanned vehicle dynamics model is established as follows: ； wherein, , , , and denote the derivatives of , , , and , and denote the horizontal and vertical coordinates of the unmanned vehicle center in the set world coordinate system , denotes the orientation angle of the unmanned vehicle, and denote the linear and angular velocities of the unmanned vehicle, and denote the linear and angular accelerations of the unmanned vehicle.

3. The box pushing control method for an unmanned vehicle according to claim 1, wherein It is assumed that the front end surface of the unmanned vehicle maintains contact with the box during the pushing process, the contact force points are the two vertices of the front end surface of the unmanned vehicle, and the entire pushing process is quasi-static; The box pushing motion model is established as follows: ； wherein, represents a pushing force moment generated by the pushing force of the unmanned vehicle to the box, and respectively represent a component of the pushing force on the transverse coordinate axis and a component of the pushing force on the longitudinal coordinate axis of the box body coordinate system, represents a pushing force moment, represents a transpose of, represents a matrix related to the intrinsic parameters of the box, , represents a parameter related to the size of the box, , represents a friction coefficient between the box and the ground, represents a mass of the box, represents a gravity coefficient, and respectively represent a length and a width of the box, represents a motion moment of the box in the box body coordinate system, and respectively represent a velocity component of the box on the transverse coordinate axis and a velocity component of the box on the longitudinal coordinate axis of the box body coordinate system, represents an angular velocity of the box.

4. The box pushing control method for an unmanned vehicle according to any one of claims 1-3, wherein, Based on the box pushing motion model, and the relative position relationship and the relative action between the unmanned vehicle and the box during the pushing process, the unmanned vehicle motion constraint when the unmanned vehicle and the box maintain stable contact is determined, which comprises: According to the relative position relationship between the unmanned vehicle and the box during the pushing process and the friction coefficient, the friction cone satisfied by the pushing force of the unmanned vehicle on the box is determined; According to the friction cone satisfied by the pushing force of the unmanned vehicle on the box, the convex cone constraint satisfied by the pushing force moment of the unmanned vehicle on the box is determined; According to the convex cone constraint satisfied by the pushing force moment of the unmanned vehicle on the box and the box pushing motion model, the cone constraint satisfied by the box motion moment is determined; According to the cone constraint satisfied by the box motion moment and the constraint condition satisfied by the box velocity during the pushing process, the box motion constraint is determined; According to the box motion constraint, the unmanned vehicle motion constraint when the unmanned vehicle and the box maintain stable contact is determined.

5. The box pushing control method for an unmanned vehicle according to claim 4, wherein The friction cone satisfied by the pushing force of the unmanned vehicle on the box is determined by the following method: The friction angle is determined according to the friction coefficient between the unmanned vehicle and the box, the contact force points between the unmanned vehicle and the box are taken as the vertices of the friction cone, and the friction angle is taken as the included angle between the edge of the friction cone and the central normal of the contact surface, so as to determine the friction cone.

6. The box pushing control method for an unmanned vehicle according to claim 5, wherein Setting two pushing forces generated by the unmanned vehicle to the box during pushing process and , the pushing forces and generate pushing force moments and to the box respectively; The convex cone constraint satisfied by the pushing force moment of the unmanned vehicle on the box is determined by the following method: The decomposition expression of the pushing force along the edge direction of the friction cone is determined; According to the limiting surface of the pushing force moment distribution, the linear transformation relationship formula of the pushing force and its corresponding pushing force moment is determined; For each pushing force, its unit vector located in the edge direction of the friction cone is selected respectively, and the two force moment components corresponding to the pushing force are calculated according to the decomposition expression and the linear transformation relationship formula; The four force moment components corresponding to the two pushing forces are taken as the edges of the four-sided cone to form the convex cone constraint.

7. The box pushing control method for an unmanned vehicle according to claim 6, wherein The cone constraint satisfied by the box motion moment is determined by the following method: Calculate four force screw components on the limit surface of the pushing force screw distribution; According to the force screw on the limit surface of the pushing force screw distribution and the box pushing motion model, calculate four motion screw components corresponding to the four force screws on the limit surface of the pushing force screw distribution; Take the four motion screw components as edges of a four-sided pyramid to form a cone constraint. 8.The box pushing control method for the unmanned vehicle according to claim 7, wherein, The constraint condition satisfied by the box velocity in the pushing process is: ； The box motion constraint is: ； The unmanned vehicle motion constraint is: ； wherein, denotes the angular velocity of the box, denotes the angular velocity of the unmanned vehicle, denotes the linear velocity of the unmanned vehicle, and denote the velocity components of the box in the lateral and longitudinal coordinate axes of the body coordinate system of the box, respectively, denotes the length of the box, denotes the length of the unmanned vehicle.

9. The box pushing control method for an unmanned vehicle according to claim 8, wherein, Solving the receding horizon trajectory optimization problem to obtain the control input of the unmanned vehicle, and controlling the unmanned vehicle according to the control input, comprising: solving a rolling horizon trajectory optimization problem to obtain an optimal state trajectory of the unmanned vehicle from a first time step to a last time step and an optimal control input of the unmanned vehicle from a zeroth time step to the last time step ​​​ Utilizing the first optimal control input controlling the unmanned vehicle; After a preset sampling time, update the state of the unmanned vehicle, re-construct and solve the receding horizon trajectory optimization problem to obtain the optimal state trajectory and the optimal control input, and control the unmanned vehicle by using the first optimal control input, and repeat the process until the box reaches the target point position.

Images