Unmanned transport equipment and control method, device and medium thereof

Abstract

The present invention discloses an unmanned vehicle and a control method, device and medium thereof. The method comprises: obtaining sensor data of the unmanned vehicle, obtaining a global map and the current position of the unmanned vehicle according to the sensor data, planning the path of the unmanned vehicle according to the current position of the unmanned vehicle and the global map, obtaining the reference position, reference yaw angle and reference steering angle of the unmanned vehicle according to the path of the unmanned vehicle, establishing a kinematic model according to control variables, yaw angle variables of the upper platform and yaw angle variables of the lower chassis, establishing a first error state space equation according to the kinematic model, the reference state of the upper platform and the reference state of the lower chassis, solving the first error state space equation to obtain the control amount of the unmanned vehicle, and controlling the unmanned vehicle according to the control amount. The present invention can improve the intelligence and collaboration of the unmanned vehicle platform and can be applied to the technical field of unmanned vehicle.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned vehicle equipment, and in particular to an unmanned vehicle equipment and a control method, device and medium thereof. Background Art

[0002] The development of safe, reliable, green and intelligent transport equipment has become an inevitable trend. At present, the main types of large-scale transport in China include oil extraction equipment, mining equipment, metal castings, lifting equipment, power equipment, construction machinery, etc. At the same time, the market demand for large-scale transportation is growing rapidly at this stage. Traditional large-scale cargo transportation usually consumes a lot of manpower and material resources, and the types of cargo that can be transported by transport equipment are limited. Unmanned transport equipment is a means of transportation for large-scale cargo transportation.

[0003] Existing unmanned transport equipment has poor collaboration and a low level of intelligence. Summary of the invention

[0004] In order to solve the above problems, the purpose of the present invention is to provide an unmanned transportation equipment and its control method, device and medium, which can improve the intelligence and collaboration of the unmanned transportation platform.

[0005] In one aspect, the present invention provides a method for controlling an unmanned vehicle, comprising the following steps:

[0006] Acquire sensor data of the unmanned vehicle, and obtain a global map and a current position of the unmanned vehicle according to the sensor data; the unmanned vehicle includes an upper platform and a lower chassis;

[0007] Planning a path of the unmanned vehicle according to the current position of the unmanned vehicle and the global map;

[0008] Obtaining a reference state of the upper platform and a reference state of the lower chassis according to the path of the unmanned vehicle; the reference state includes a reference position, a reference yaw angle, and a reference steering angle;

[0009] A kinematic model is established according to the control variables, the yaw angle variables of the upper platform and the yaw angle variables of the lower chassis;

[0010] A first error state space equation is established according to the kinematic model, the reference state of the upper platform and the reference state of the lower chassis, the control amount of the unmanned vehicle is obtained by solving the first error state space equation, and the unmanned vehicle is controlled according to the control amount.

[0011] Optionally, the sensor data includes radar data and inertial sensor data, and obtaining the global map and the current position of the unmanned vehicle according to the sensor data specifically includes:

[0012] Constructing a global map according to the radar data, the inertial sensor data and a preset mapping algorithm;

[0013] The current position of the unmanned vehicle is obtained according to a preset positioning algorithm, the radar data and the inertial sensor data.

[0014] Optionally, the path of the unmanned vehicle includes a current position and a target position of the unmanned vehicle, and obtaining a reference state of the upper platform and a reference state of the lower chassis according to the path of the unmanned vehicle specifically includes:

[0015] Calculating the distance and angle between the current position and the target position, and obtaining a reference steering angle of the lower chassis according to the distance, the angle and a preset wheelbase of the lower chassis;

[0016] Obtaining a reference yaw angle of the upper platform according to the current position, the target position and the arc tangent function, and taking the target position as the reference position of the upper platform;

[0017] The reference position of the lower chassis is obtained according to the reference yaw angle of the upper platform and the reference position of the upper platform, and the reference yaw angle of the next chassis is obtained according to the reference position of the lower chassis.

[0018] Optionally, the control variables include a moving speed variable of the lower chassis and a steering angle variable of the lower chassis, and the kinematic model is established according to the control variables, the yaw angle variable of the upper platform and the yaw angle variable of the lower chassis, specifically including:

[0019] A moving speed equation of the lower chassis is obtained according to a moving speed variable of the lower chassis and a yaw angle variable of the lower chassis;

[0020] Obtaining a yaw rate equation of the lower chassis according to a steering angle variable of the lower chassis, a moving speed variable of the lower chassis and a preset wheelbase;

[0021] The yaw angular velocity equation of the upper platform is obtained according to the moving speed variable of the lower chassis, the yaw angle variable of the upper platform and the yaw angle variable of the lower chassis.

[0022] Optionally, establishing a first error state space equation according to the kinematic model, the reference state of the upper platform and the reference state of the lower chassis specifically includes:

[0023] Obtaining a first state matrix and a second state matrix according to the reference yaw angle of the upper platform, the reference yaw angle of the lower chassis and the reference steering angle of the lower chassis;

[0024] The kinematic model is subjected to Taylor series expansion according to the first state matrix, the second state matrix, the reference state of the upper platform and the reference state of the lower chassis to obtain the first error state space equation.

[0025] Optionally, solving the first error state space equation to obtain the control amount of the unmanned vehicle specifically includes:

[0026] Discretize the first error state space equation for the first time to obtain a discretized second error state space equation;

[0027] Obtaining an augmented matrix according to the second error state space equation, and performing a second discretization on the second error state space equation according to the augmented matrix to obtain a third error state space equation;

[0028] A cost equation is obtained according to the third error state space equation, the minimization of the cost equation is taken as an optimization target, and the third error state space equation is solved according to the optimization target and preset constraints to obtain the control amount of the unmanned vehicle.

[0029] Optionally, the control amount includes a control steering angle of the lower chassis and a control moving speed of the lower chassis, and controlling the unmanned vehicle according to the control amount specifically includes:

[0030] Obtaining a rotation angle of each wheel of the lower chassis according to a control steering angle of the lower chassis;

[0031] A control signal of the unmanned vehicle is obtained according to the rotation angle of each wheel of the lower chassis and the controlled moving speed of the lower chassis, and the unmanned vehicle is controlled according to the control signal.

[0032] In another aspect, the present invention provides a control device for an unmanned vehicle, comprising:

[0033] at least one processor;

[0034] at least one memory for storing at least one program;

[0035] When the at least one program is executed by the at least one processor, the at least one processor implements the method described above.

[0036] In another aspect, the present invention provides a computer-readable storage medium storing a program executable by a processor. When the program executable by the processor is executed by the processor, it is used to perform the method as described above.

[0037] On the other hand, the present invention provides an unmanned transport device, comprising a lower chassis and an upper platform, wherein the lower chassis comprises a steering device, a perception module and a control planning module, and the lower chassis is connected to the upper platform, wherein:

[0038] The perception module is used to collect sensor data and send the sensor data to the control planning module;

[0039] The control planning module is used to obtain the control quantity according to the method described above;

[0040] The steering device is used to move according to the control amount. The implementation of the present invention includes the following beneficial effects: the present invention obtains sensor data of the unmanned vehicle, builds a global map based on the sensor data and obtains the current position of the unmanned vehicle, the unmanned vehicle includes an upper platform and a lower chassis, plans the path of the unmanned vehicle according to the current position of the unmanned vehicle and the global map, obtains the reference state of the upper platform and the reference state of the lower chassis according to the path of the unmanned vehicle; the reference state includes a reference position, a reference yaw angle and a reference steering angle, and establishes a motion according to the control variables, the yaw angle variables of the upper platform and the yaw angle variables of the lower chassis A kinematic model is constructed, and a first error state space equation is established according to the kinematic model, the reference state of the upper platform and the reference state of the lower chassis. The control quantity of the unmanned vehicle is obtained by solving the first error state space equation, and the unmanned vehicle is controlled according to the control quantity. A global map is first constructed, and a path is planned according to the map. The reference state is obtained according to the planned path, and the control quantity is obtained according to the reference state. This can improve the intelligence of the unmanned vehicle platform, and when constructing the reference state, a dual-mode reference path generation strategy for calculating the reference states of the upper platform and the lower chassis respectively is adopted, which can improve the collaboration of the unmanned vehicle platform. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 It is a flow chart of steps of a control method of an unmanned vehicle provided by the present invention;

[0042] Figure 2 It is a flow chart of steps for obtaining a reference state provided by the present invention;

[0043] Figure 3 It is a structural diagram of a control device for unmanned transport equipment provided by the present invention;

[0044] Figure 4 It is a structural schematic diagram of a steering device provided by the present invention;

[0045] Figure 5 It is a schematic diagram of the mechanism of an unmanned transport device provided by the present invention;

[0046] Figure 6It is a module schematic diagram of an unmanned carrier provided by the present invention. DETAILED DESCRIPTION

[0047] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. The step numbers in the following embodiments are only provided for the convenience of explanation and description, and the order between the steps is not limited in any way. The execution order of each step in the embodiment can be adaptively adjusted according to the understanding of those skilled in the art.

[0048] The terms used in the embodiments of the present application are only for the purpose of describing specific embodiments, and are not intended to limit the embodiments of the present application. The singular forms of "a", "said" and "the" used in the embodiments of the present application and the appended claims are also intended to include plural forms, unless the context clearly indicates other meanings.

[0049] When the following description relates to the accompanying drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present application. On the contrary, they are only examples of devices and methods consistent with some aspects of the present application as detailed in the attached claims. In the description of the present application, it is necessary to understand that the terms "first / S100", "second / S200", "third / S300", etc. are only used to distinguish similar objects, and do not have to be used to describe a specific order or sequence, nor can they be understood as indicating or implying relative importance. For those of ordinary skill in the art, the specific meanings of the above terms in the present application can be understood according to the specific circumstances.

[0050] like Figure 1 As shown, Figure 1 The present invention provides a method for controlling an unmanned vehicle, comprising the following steps:

[0051] S100: Acquire sensor data of the unmanned vehicle, and obtain a global map and a current position of the unmanned vehicle according to the sensor data.

[0052] Among them, the unmanned transport equipment includes an upper platform and a lower chassis.

[0053] The sensor data includes radar data measured by radar and inertial sensor data measured by inertial sensors. The radar can be but is not limited to a lidar. The radar data includes the distance from the unmanned vehicle to obstacles and the distance to the vehicle in front. The inertial sensor data includes acceleration, tilt, impact, vibration, rotation and multi-degree-of-freedom (DoF) motion to summarize the inertial navigation data.

[0054] The global map includes, but is not limited to, the location of surrounding obstacles and the location of the vehicle in front.

[0055] The current position includes the horizontal coordinate and the vertical coordinate of the unmanned vehicle in the global map.

[0056] In this step, the current yaw angle and current steering angle of the unmanned equipment can also be obtained according to the current position of the unmanned equipment, wherein the current yaw angle includes the current yaw angle of the front steering device, the current yaw angle of the rear steering device and the yaw angle of the upper platform, and the current steering angle includes the current steering angle of the front steering device and the current steering angle of the rear steering device. The current position, current yaw angle and current steering angle can be used to discretize the state space equation when solving the state space equation.

[0057] Specifically, a global map is constructed by substituting radar data and inertial sensor data into a grid map algorithm (gmapping algorithm). When constructing a global map, real-time GPS, radar data and inertial sensor data can also be combined to obtain the coordinate position and distance information of the object in front, thereby constructing a global map.

[0058] Substitute the radar data and inertial sensor data into the Monte Carlo positioning algorithm to obtain the current position of the unmanned vehicle.

[0059] S200: Plan a path of the unmanned vehicle according to the current position of the unmanned vehicle and the global map.

[0060] Specifically, the Theta* algorithm is used to plan the path of the unmanned vehicle according to the current position of the unmanned vehicle and the global map.

[0061] S300: Obtain a reference state of an upper platform and a reference state of a lower chassis according to the path of the unmanned vehicle.

[0062] The reference state includes a reference position, a reference yaw angle and a reference steering angle.

[0063] The path of the unmanned vehicle includes the current position and target position of the unmanned vehicle

[0064] Specifically, the distance and angle between the current position and the target position are calculated, and the reference steering angle of the lower chassis is obtained according to the distance, the angle and the preset wheelbase of the lower chassis by combining the Rpp algorithm.

[0065] The reference yaw angle of the upper platform is obtained according to the current position, the target position and the inverse tangent function, and the target position is used as the reference position of the upper platform.

[0066] The reference position of the lower chassis is obtained according to the reference yaw angle of the upper platform and the reference position of the upper platform, and the reference yaw angle of the next chassis is obtained according to the reference position of the lower chassis.

[0067] S400: Establish a kinematic model according to the control variables, the unknown yaw angle of the upper platform and the yaw angle variable of the lower chassis.

[0068] The control variables include the moving speed variable of the lower chassis and the steering angle variable of the lower chassis.

[0069] The kinematic model includes, but is not limited to, a moving speed equation, a yaw rate equation of the lower chassis, and a yaw rate equation of the upper platform.

[0070] The moving speed equation of the lower chassis is obtained according to the moving speed variable of the lower chassis and the yaw angle variable of the lower chassis.

[0071] The yaw angular velocity equation of the lower chassis is obtained according to the steering angle variable of the lower chassis, the moving speed variable of the lower chassis and the preset wheelbase.

[0072] The yaw angular velocity equation of the upper platform is obtained according to the moving speed variable of the lower chassis, the yaw angle variable of the upper platform and the yaw angle variable of the lower chassis.

[0073] S500. Establish a first error state space equation according to the kinematic model, the reference state of the upper platform and the reference state of the lower chassis, solve the first error state space equation to obtain a control amount of the unmanned vehicle, and control the unmanned vehicle according to the control amount.

[0074] The difference between the control variable in step S400 and the control amount in this step is that the control variable is an unknown quantity, and the control amount is the parameter value of the control variable obtained by solving the error state space equation. The explanation of the moving speed variable and the control moving speed is similar.

[0075] Specifically, the kinematic model is expanded by Taylor series according to the reference state of the upper platform and the reference state of the lower chassis, and all high-order expansion terms above the second-order expansion terms in the Taylor series expansion are omitted to obtain the first error state space equation.

[0076] An objective function and constraint conditions are established, and the first error state space equation is solved according to the objective function and the constraint conditions to obtain the control quantity of the unmanned vehicle, and the unmanned vehicle is controlled according to the control quantity.

[0077] In some embodiments, in step S100, the sensor data includes radar data and inertial sensor data, and obtaining the global map and the current position of the unmanned vehicle according to the sensor data specifically includes:

[0078] S110, constructing a global map according to radar data, inertial sensor data and a preset mapping algorithm.

[0079] The preset mapping algorithm may be, but is not limited to, a gmapping algorithm.

[0080] Specifically, when the gmapping algorithm is used to build a global map, the platform posture is first updated according to the information, and then the updated posture is used as the initial value for laser scanning matching. After scanning matching, a relatively concentrated distribution given by the laser radar observation model is obtained, including the distribution of obstacles in front and the front vehicle. The mean and variance of the Gaussian distribution are determined in the relatively concentrated distribution by sampling, and the recommended distribution closest to the real distribution is obtained. At the same time, the distance and angle between the unmanned carrier platform and obstacles and the front vehicle in the distribution are assigned corresponding weights according to the radar data and inertial sensor data, and the corresponding global map is obtained.

[0081] S120: Obtain the current position of the unmanned vehicle according to a preset positioning algorithm, radar data, and inertial sensor data.

[0082] The preset positioning algorithm may be, but is not limited to, a Monte Carlo positioning algorithm.

[0083] Specifically, when performing trajectory tracking and positioning according to the Monte Carlo positioning algorithm, the particles (possible positions of the unmanned vehicle) are initialized so that they are evenly distributed in the entire posture space. The unmanned vehicle moves to sense the position of the landmark and obtains the measurement model. At this time, the weight of the particles is updated according to the radar data and the inertial sensor data to obtain the current position of the unmanned vehicle. The unmanned vehicle starts the next movement and resamples, and changes the distribution of the particle set to a new particle set with the same weight but a changed distribution position. New particles are added to the possible position of the unmanned vehicle. The unmanned vehicle measures again to obtain the measurement model. After integration, the particles are redistributed into particles with non-uniform distribution and different weights according to the radar data and the inertial sensor data. The position where the platform is most likely to be located is the part where the particles are densely distributed and the particles have a larger weight. The next step is for the unmanned vehicle to move again and generate a new particle distribution set according to the motion model, and then repeat the process.

[0084] In some embodiments, S200 plans the path of the unmanned vehicle according to the current position of the unmanned vehicle and the global map. The Theta* algorithm allows the father of a vertex to be any vertex, using a consistent function h(s)=c(s, s goal) to calculate the straight-line distance. When selecting a moving path based on the current position and the global map, consider the following two paths to update the g-value of the unexpanded neighboring points of vertex s:

[0085] Path 1: Consider the distance from the starting point to the vertex s [= g(s)] and the straight line distance from s to s' [= c(s, s')], and get a length g(s)+c(s, s').

[0086] Path 2: Consider the distance from the starting point to the parent node of the fixed point s, parent(s) [= g(parent(s))] and the straight-line distance from parent(s) to s' [= c(parent(s), s')]. If s' can "see" s, we get a distance of g(parent(s))+c(parent(s), s'). If s' can "see" parent(s), we know from the triangle inequality that path 2 must be shorter than path 1.

[0087] If any path is shorter than the shortest path, Theta* will update the g-value and the parent node of s', where the fixed point s is the starting point of the unmanned carrier platform and s' is the point passed by the unmanned carrier platform during its movement.

[0088] In some embodiments, Figure 2 As shown, Figure 2 The invention is a flowchart of steps for obtaining a reference state, wherein the lower chassis includes but is not limited to a front steering device and a rear steering device, and step S300 of obtaining a reference state of the upper platform and a reference state of the lower chassis according to the path of the unmanned vehicle specifically includes:

[0089] S310, calculating the distance and angle between the current position and the target position, and obtaining a reference steering angle of the lower chassis according to the distance, the angle and a preset wheelbase of the lower chassis.

[0090] The reference steering angle of the lower chassis includes a reference steering angle of a front steering device and a reference steering angle of a rear steering device.

[0091] Specifically, when calculating the reference steering angle, first calculate the distance l between the current position and the target position (target waypoint) d and the angle α.

[0092] According to the law of cosines, the distance l can be derived d The angle α can be used to obtain the inverse of the radius R of the circle followed by the rear axle at a given steering angle. The derivation process is shown in formulas (1)-(3):

[0093]

[0094]

[0095]

[0096] Among them, sin(.) represents the sine function, cos(.) represents the cosine function, and k represents the reciprocal of the radius R of the circle followed by the rear axle at a given steering angle.

[0097] Referring to the Rpp algorithm, the geometric relationship between the steering angle and the curvature that the rear axle will follow is shown in formula (4):

[0098]

[0099] Among them, δ represents the turning angle of the front wheels, L is the preset wheelbase of the steering device, and R is the radius of the circle followed by the rear axle at a given steering angle.

[0100] According to the inverse tangent function, the steering angle can be obtained as shown in formula (5):

[0101]

[0102] Therefore, the reference steering angle of the front wheel of the front steering device can be obtained as The reference steering angle of the front wheels of the rear steering device is Among them, L1 represents the preset wheelbase of the front steering device and L3 represents the preset wheelbase of the rear steering device.

[0103] The rear wheel steering angle of each steering device can be set with reference to the front wheel steering angle. When the unmanned vehicle is traveling at a low speed, in order to reduce the turning radius, the rear wheel steering direction is usually opposite to that of the front wheel, which is the so-called "reverse steering"; when turning at high speed, in order to improve the stability of the unmanned vehicle and speed up the lateral response speed of the unmanned vehicle, the rear wheel will generate a steering angle in the same direction as the front wheel, which is the so-called "same-direction steering".

[0104] S320, obtaining a reference yaw angle of the upper platform according to the current position, the target position and the arc tangent function, and taking the target position as the reference position of the upper platform.

[0105] Specifically, calculate the horizontal coordinate of the current position and the horizontal coordinate of the target position x r The difference dx r , calculate the ordinate of the current position and the ordinate of the target position y r The difference dy r .

[0106] According to the above difference and the inverse tangent function, the reference yaw angle of the upper platform is obtained, as shown in formula (6):

[0107]

[0108] Among them, θ 2r is the reference yaw angle of the upper platform.

[0109] With coordinates (x r ,y r ) as the reference position of the upper platform.

[0110] The reference posture of the upper platform can be expressed as (x r ,y r ,θ 2r ).

[0111] S330: Obtain a reference position of a lower chassis according to a reference yaw angle of the upper platform and a reference position of the upper platform, and obtain a reference yaw angle of a lower chassis according to the reference position of the lower chassis.

[0112] The reference position of the lower chassis includes a reference position of a front steering device and a reference position of a rear steering device, and the reference yaw angle of the lower chassis includes a reference yaw angle of a front steering device and a reference yaw angle of a rear steering device.

[0113] Specifically, the reference position (x) of the front steering device is obtained according to formula (7): r1 ,y r1 ) and the reference position of the rear steering device (x r2 ,y r2 ):

[0114]

[0115] Wherein, L represents the distance between the front steering device and the rear steering device.

[0116] The reference yaw angle of the next chassis is obtained according to the reference position of the lower chassis, as shown in formula (8):

[0117]

[0118] Among them, θ 1r represents the reference yaw angle of the front steering device, θ 3r Denotes the reference yaw angle of the rear steering device, dy 1r Indicates the difference between the current ordinate of the front steering device and the ordinate of the target position, dx 1r Indicates the difference between the current horizontal coordinate of the front steering device and the horizontal coordinate of the target position, dy 2r Indicates the difference between the current ordinate of the rear steering device and the ordinate of the target position, dx 2r Indicates the difference between the current horizontal coordinate of the rear steering device and the horizontal coordinate of the target position.

[0119] According to the above reference values, the reference posture of the front steering device can be expressed as (x1r ,y 1r ,θ 1r , δ f1r ), the reference posture of the rear steering device can be expressed as (x 2r ,y 2r ,θ 3r , δ f2r ).

[0120] In some embodiments, the control variables in step S400 include the moving speed variable of the lower chassis and the steering angle variable of the lower chassis, and the kinematic model is established according to the control variables, the unknown yaw angle of the upper platform and the yaw angle variable of the lower chassis, specifically including:

[0121] S410, obtaining a moving speed equation of the lower chassis according to the moving speed variable of the lower chassis and the yaw angle variable of the lower chassis.

[0122] The moving speed equation of the lower chassis includes the lateral speed equation and the longitudinal speed equation of the front steering device.

[0123] Specifically, the moving speed equation of the front steering device is obtained according to the moving speed variable of the front steering device and the yaw angle variable of the front steering device, as shown in formula (9):

[0124]

[0125] Among them, v1 is the moving speed variable of the front steering device, θ1 is the yaw angle variable of the front steering device, is the lateral velocity of the front steering device, which is expressed by taking the derivative of the horizontal coordinate, is the longitudinal velocity of the front steering device, expressed as the derivative with respect to the ordinate.

[0126] S420, obtaining a yaw angular velocity equation of the lower chassis according to the steering angle variable of the lower chassis, the moving speed variable of the lower chassis, and a preset wheelbase.

[0127] The steering angle variables of the lower chassis include the front wheel steering angle of the front steering device, the rear wheel steering angle of the front steering device, the front wheel steering angle of the rear steering device and the front wheel steering angle of the rear steering device.

[0128] The yaw rate equation of the lower chassis includes the yaw rate equation of the front steering device and the yaw rate equation of the rear steering device.

[0129] Specifically, the yaw rate equation of the front steering device is shown in formula (10):

[0130]

[0131] in, represents the derivative of the unknown yaw angle of the front steering device, L1 represents the preset wheelbase of the front steering device, δ f1 represents the front wheel steering angle variable of the front steering device, δ r1 represents the rear wheel steering angle variable of the front steering device, as in the aforementioned step S310, δ r1 According to δ f1 get.

[0132] The yaw rate equation of the rear steering device is shown in formula (11):

[0133]

[0134] in, represents the derivative of the unknown yaw angle of the rear steering device, L3 represents the preset wheelbase of the rear steering device, δ f2 represents the front wheel steering angle variable of the rear steering device, δ r2 represents the rear wheel steering angle variable of the rear steering device, as in the aforementioned step S310, δ r2 According to δ f2 It is obtained that v2 represents the moving speed variable of the rear steering device.

[0135] v2 can be calculated based on v1, as shown in formula (12):

[0136]

[0137] Among them, θ1 represents the yaw angle variable of the front steering device, θ2 represents the yaw angle variable of the upper platform, and θ3 represents the yaw angle variable of the rear steering device.

[0138] When the front steering device and the rear steering device operate in a counter-phase steering mode, the front steering device turns in a direction opposite to that of the rear steering device, that is, the front steering angle is equal to the rear steering angle but in opposite directions.

[0139] S430, obtaining a yaw angular velocity equation of the upper platform according to the moving speed variable of the lower chassis, the yaw angle variable of the upper platform and the yaw angle variable of the lower chassis.

[0140] Specifically, the yaw angular velocity equation of the upper platform is obtained according to the moving speed variable of the front steering device, the yaw angle variable of the upper platform and the yaw angle variable of the lower chassis, as shown in formula (13):

[0141]

[0142] Wherein, L2 represents the distance between the front steering device and the rear steering device, θ1 represents the yaw angle variable of the front steering device, θ2 represents the yaw angle variable of the upper platform, and θ3 represents the yaw angle variable of the rear steering device.

[0143] According to steps S410-S430, the kinematic model is obtained as shown in formula (14):

[0144]

[0145] In some embodiments, the aforementioned steps S100-S400 obtain the reference posture of the planned path and the upper platform, and the reference posture of the lower chassis. Subsequently, the dual modular chassis collaborative path tracking controller calculates the control signal by evaluating the difference between the actual state of the truck (the actual state includes the measured actual position, actual yaw angle, and actual steering angle) and the reference posture.

[0146] In step S500, the first error state space equation is established according to the kinematic model, the reference state of the upper platform and the reference state of the lower chassis, specifically including:

[0147] S510. Obtain a first state matrix and a second state matrix according to the reference yaw angle of the upper platform, the reference yaw angle of the lower chassis, and the reference steering angle of the lower chassis.

[0148] Specifically, the first state matrix is ​​obtained according to the reference yaw angle of the upper platform, the reference yaw angle of the lower chassis and the reference steering angle of the lower chassis, as shown in formula (15):

[0149]

[0150] Where A represents the first state matrix, v 1r Indicates the reference displacement speed of the front steering device.

[0151]

[0152]

[0153]

[0154]

[0155] The second state matrix is ​​obtained according to the reference yaw angle of the upper platform, the reference yaw angle of the lower chassis and the reference steering angle of the lower chassis, as shown in formula (16):

[0156]

[0157] S520. Perform Taylor series expansion on the kinematic model according to the first state matrix, the second state matrix, the reference state of the upper platform, and the reference state of the lower chassis to obtain the first error state space equation.

[0158] Specifically, the kinematic model is expanded by Taylor series according to the first state matrix, the second state matrix, the reference state of the upper platform and the reference state of the lower chassis, and the high-order terms of the expanded Taylor series are omitted, that is, the terms above the second-order derivative are omitted, and the first error state space equation is obtained, as shown in formula (17):

[0159]

[0160] Among them, the state vector Control vector Among them, v1 is the moving speed variable of the front steering device, δ f1 is the front wheel steering angle variable of the front steering device, δ f2 is the front wheel steering angle variable of the rear steering device, δ f1r , δ f2r is the corresponding reference steering angle.

[0161] When solving the error state space equation in a discretized manner, the yaw angle variable of the upper platform (the yaw angle of the cooperative path tracking controller of the dual-module chassis) θ2 can be approximately equal to the current yaw angle in each time step, which can be calculated based on the sensor data of the front steering device and the rear steering device, as shown in formula (18):

[0162]

[0163] Among them, arctan() represents the inverse tangent function, x1 and y1 represent the current position of the front steering device, and x2 and y2 represent the current position of the rear steering device.

[0164] In some embodiments, solving the first error state space equation in step S500 to obtain the control amount of the unmanned vehicle specifically includes:

[0165] S530 , discretize the first error state space equation for the first time to obtain a discretized second error state space equation.

[0166] Specifically, the first state matrix, the second state matrix, and the state vector Control vector and the state vector The first derivative of Discretization gives the second error state space equation, as shown in formula (19):

[0167]

[0168] Among them, A k =I+T*A, I is the identity matrix, T is the discrete time, B k =T*B, k represents the current time step of the control signal.

[0169] S540: Obtain an augmented matrix according to the second error state space equation, and discretize the second error state space equation for a second time according to the augmented matrix to obtain a third error state space equation.

[0170] Specifically, in order to constrain the control quantity and its increment, according to the discretized state vector and the discretized control vector Constructing the augmented matrix The third error state space equation is obtained by using the augmented matrix as the new state vector, as shown in formula (20):

[0171]

[0172] in, I is the identity matrix.

[0173] It can be seen that the augmented matrix τ(k) depends on the first discrete A k and B k Matrix A k and B k The accuracy of A determines the control effect of the path tracking controller. k and B k Some parameter values ​​of can be obtained through parameter / state estimation, matrix A k and B k The parameter values ​​in vary with the operating conditions of the autonomous truck at each time step.

[0174] S550. Obtain a cost equation according to the third error state space equation, take minimizing the cost equation as an optimization target, solve the third error state space equation according to the optimization target and preset constraints, and obtain the control amount of the unmanned vehicle.

[0175] Specifically, in order to facilitate path tracking and limit the increase of system input in each time step, constraints are set for solving the third error state space equation. The cost equation of AMPC is constructed according to the third error state space equation, as shown in formula (21):

[0176]

[0177] Where N p is the prediction time step range, N c is the control time step range, Q and R are weighted matrices, ρ is the weight coefficient, and ε is the relaxation factor.

[0178] In addition, in actual operation, the moving speed and steering angle of the lower chassis are constrained by the actuator. At the same time, in order to make the moving speed and steering angle of the lower chassis change smoothly, the control variable and its increment should be constrained. The increment represents the change of the control variable between two time steps. Therefore, the present invention constructs the control variable constraint and the increment constraint as preset constraints. These constraints are intended to limit the maximum input and the maximum change rate of the input between two consecutive time steps. The preset constraints are shown in formula (22):

[0179] u min ≤u(k+1)≤u max

[0180] Δu min ≤Δu(k+1)≤Δu max (twenty two)

[0181] Among them, u(k+1) represents the control amount at time step k+1, u min Indicates the minimum value of the control quantity (parameter value of the control quantity), u max represents the maximum value of the control amount, Δu(k+1) represents the change of the control amount between time step k+1 and time step k, and Δu min Indicates the minimum value of the control quantity change, Δu max Indicates the maximum value of the control variable change.

[0182] Optimization problem of path tracking controller The optimization objective and preset constraints of the dual modular chassis are summarized as shown in formula (23):

[0183]

[0184] Among them, stτ(.)=(..) means that when performing AMPC iteration, (.)=(..).

[0185] In actual applications, the unmanned transport equipment repeatedly executes the above steps S100-S500 and their sub-steps to obtain updated cargo movement trajectory and platform movement trajectory. The dynamic obstacle avoidance unit and the path optimization construction unit timely optimize and adjust the route during the movement of the unmanned transport equipment to achieve intelligent control.

[0186] In some embodiments, the control amount includes a control steering angle of the lower chassis and a control moving speed of the lower chassis, and the step S500 of controlling the unmanned vehicle according to the control amount specifically includes:

[0187] S560: Obtain a rotation angle of each wheel of the lower chassis according to the control steering angle of the lower chassis.

[0188] Specifically, the rotation angle represents the rotation angle of the wheel under a certain control signal. For example, the left front wheel rotation angle of the front steering device is δ FL , right front wheel turning angle δ FR , left rear wheel turning angle δ RL and the right rear wheel turning angle δ RR It can be expressed as shown in formula (24):

[0189]

[0190] Wherein, l is the preset wheelbase of the front steering device; B is the track width of the front steering device.

[0191] The rotation angle of each wheel of the rear steering device can be obtained by rewriting formula (24) according to its steering angle, preset wheelbase and track width.

[0192] S570: Obtain a control signal of the unmanned vehicle according to the rotation angle of each wheel of the lower chassis and the controlled moving speed of the lower chassis, and control the unmanned vehicle according to the control signal.

[0193] Among them, the control moving speed of the lower chassis includes the control moving speed of the front steering angle (which is the parameter value of v1) and the control moving speed of the rear steering angle (which is the parameter value of v2), the control amount includes the parameter value of v1, and the parameter value of v2 is obtained according to the above formula (12).

[0194] Specifically, the controller can calculate control signals according to the rotation angle of each wheel and the control movement speed of the front steering angle and the control movement speed of the rear steering angle obtained in the above step S560, and independently control the front steering device and the rear steering device.

[0195] The unmanned vehicle is controlled to move according to the calculated control signal.

[0196] The implementation of the present invention includes the following beneficial effects:

[0197] The present invention first constructs a global map, plans a path according to the map, obtains a reference state according to the planned path, and obtains a control amount according to the reference state, which can improve the intelligence level of the unmanned transport platform. When constructing the reference state, a dual-mode reference path generation strategy for respectively calculating the reference states of the upper platform and the lower chassis is adopted, which can improve the collaboration of the unmanned transport platform.

[0198] like Figure 3 As shown, Figure 3 The present invention is a structural diagram of a control device for an unmanned vehicle. The present invention also provides a control device for an unmanned vehicle, including:

[0199] at least one processor;

[0200] at least one memory for storing at least one program;

[0201] When the at least one program is executed by the at least one processor, the at least one processor implements the above-mentioned method.

[0202] It can be seen that the contents of the above method embodiments are all applicable to the present system embodiments, the functions specifically implemented by the present system embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0203] like Figure 4-5 As shown, Figure 4 This is a schematic diagram of the structure of a steering device. Figure 5 The present invention further provides an unmanned vehicle, comprising a lower chassis and an upper platform, wherein the lower chassis comprises a steering device, a perception module and a control planning module, and the lower chassis is connected to the upper platform, wherein:

[0204] The perception module is used to collect sensor data and send the sensor data to the control planning module;

[0205] The control planning module is used to obtain the control amount according to the above method and send the control amount to the steering device;

[0206] The steering device is used to move according to the control amount.

[0207] Among them, the perception module includes but is not limited to lidar and inertial sensors. The lidar will emit laser outward. After encountering an object, the laser will be refracted back and received by the sensor, thereby measuring the distance from the body to the obstacle; the inertial sensor detects and measures acceleration, tilt, impact, vibration, rotation and multi-degree-of-freedom (DoF) motion to summarize the inertial navigation data, and combines the real-time GPS and the angle emitted by the lidar to obtain the coordinate position and distance information of the object in front.

[0208] like Figure 6 As shown, Figure 6 It is a module schematic diagram of an unmanned vehicle. The modular variable structure indicates that the steering device of the unmanned vehicle can be connected according to the actual transportation situation. For example, an unmanned vehicle includes two steering devices or more than two steering devices. The control planning module includes but is not limited to a path planning module and a motion control module. The path planning module includes a global path planning unit, a dynamic obstacle avoidance unit and a path optimization construction unit. The motion control module includes an operation model construction unit and a control model construction unit.

[0209] like Figure 4As shown, the steering device is provided with a four-wheel universal structure and a power module, wherein each wheel of the four-wheel universal structure has an independent driving motor, and its speed and direction are independently controlled according to the control amount. As shown in steps S560-S570, a composite design of solid tires and swing arms is adopted on the power module, and a powerful hub motor is added, which eliminates the complex transmission design structure and adapts to the motion state required by different ground conditions. Certain information during the driving of the unmanned vehicle is used to control the steering angle input of the rear wheel to improve the maneuverability and stability of the platform. When the unmanned vehicle is driving at a low speed, in order to reduce the turning radius, the rear wheel steering direction is usually opposite to that of the front wheel, and the so-called "reverse steering"; when turning at high speed, in order to improve the stability of the unmanned vehicle and speed up the lateral response speed of the platform, the rear wheel will generate a steering angle in the same direction as the front wheel, and the so-called "same direction steering".

[0210] In practical applications, multiple steering devices can be set up according to the transported goods to jointly carry the upper platform. Different steering devices are connected by rotating axes to construct the unmanned transport equipment as a whole. The formation is reorganized according to the actual transportation tasks and goals to adapt to the requirements of different cargo forms for unmanned transport equipment and realize the coordinated transportation of large cargo.

[0211] The global path planning unit fuses the data detected by the lidar and inertial sensor to build a map and locate (obtain a local map, which includes the distance and direction between the unmanned vehicle and obstacles and the vehicle in front), and plans the global path (builds a global map and obtains the current position) in combination with the transportation demand of the target object transported by the unmanned vehicle, i.e., the above-mentioned step S100; the dynamic obstacle avoidance unit monitors the obstacle status in real time through the lidar, and the collected information is transmitted to the path optimization construction unit to optimize and adjust the local path in combination with the global path, i.e., the above-mentioned steps S200-S300.

[0212] The kinematic model construction unit processes the mathematical and geometric relationships between the cargo and the unmanned transport equipment to construct a reasonable overall kinematic model, i.e., the above-mentioned step S400. The control model construction unit calculates and plans the path of a single unmanned transport equipment in combination with the constructed kinematic model and controls the formation transport, i.e., the above-mentioned step S500.

[0213] It can be seen that the contents of the above method embodiments are all applicable to the present system embodiments, the functions specifically implemented by the present system embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0214] In addition, the embodiment of the present application also discloses a computer program product or a computer program, and the computer program product or the computer program is stored in a computer-readable storage medium. The processor of the computer device can read the computer program from the computer-readable storage medium, and the processor executes the computer program, so that the computer device performs the above method. Similarly, the contents in the above method embodiment are all applicable to the storage medium embodiment, and the functions specifically implemented by the storage medium embodiment are the same as those in the above method embodiment, and the beneficial effects achieved are also the same as those achieved by the above method embodiment.

[0215] The present invention also provides a computer-readable storage medium, which stores a program executable by a processor. The program executable by the processor is used to implement the above method when executed by the processor.

[0216] It is understood that all or some steps and systems in the disclosed method above can be implemented as software, firmware, hardware and appropriate combinations thereof. Some physical components or all physical components can be implemented as software executed by a processor, such as a central processing unit, a digital signal processor or a microprocessor, or implemented as hardware, or implemented as an integrated circuit, such as an application-specific integrated circuit. Such software can be distributed on a computer-readable medium, and the computer-readable medium can include a computer storage medium (or a non-transitory medium) and a communication medium (or a temporary medium). As known to those of ordinary skill in the art, the term computer storage medium includes volatile and non-volatile, removable and non-removable media implemented in any method or technology for storing information (such as computer-readable instructions, data structures, program modules or other data). Computer storage media include, but are not limited to, RAM, ROM, EEPROM, flash memory or other memory technology, CD-ROM, digital versatile disk (DVD) or other optical disk storage, magnetic cassette, magnetic tape, disk storage or other magnetic storage device, or any other medium that can be used to store desired information and can be accessed by a computer. Furthermore, it is well known to those skilled in the art that communication media typically embodies computer readable instructions, data structures, program modules, or other data in a modulated data signal such as a carrier wave or other transport mechanism, and may include any information delivery media.

[0217] Reference to "embodiments" herein means that a particular feature, structure, or characteristic described in conjunction with the embodiments may be included in at least one embodiment of the present invention. The appearance of the phrase in various places in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment that is mutually exclusive with other embodiments. It is explicitly and implicitly understood by those skilled in the art that the embodiments described herein may be combined with other embodiments.

[0218] The above is a specific description of the preferred implementation of the present invention, but the invention is not limited to the embodiments. Those skilled in the art may make various equivalent modifications or substitutions without violating the spirit of the present invention. These equivalent modifications or substitutions are all included in the scope defined by the claims of this application.

Claims

1. A control method for unmanned transport equipment, characterized in that: The following steps are involved: Acquire sensor data of the unmanned vehicle, and obtain a global map and a current position of the unmanned vehicle according to the sensor data; the unmanned vehicle includes an upper platform and a lower chassis; Planning a path of the unmanned vehicle according to the current position of the unmanned vehicle and the global map; Obtaining a reference state of the upper platform and a reference state of the lower chassis according to the path of the unmanned vehicle; the reference state includes a reference position, a reference yaw angle, and a reference steering angle; A kinematic model is established according to the control variables, the yaw angle variables of the upper platform and the yaw angle variables of the lower chassis; Establishing a first error state space equation according to the kinematic model, the reference state of the upper platform and the reference state of the lower chassis, solving the first error state space equation to obtain a control amount of the unmanned vehicle, and controlling the unmanned vehicle according to the control amount; The control variables include the moving speed variable of the lower chassis and the steering angle variable of the lower chassis. The kinematic model is established according to the control variables, the yaw angle variable of the upper platform and the yaw angle variable of the lower chassis, specifically including: A moving speed equation of the lower chassis is obtained according to a moving speed variable of the lower chassis and a yaw angle variable of the lower chassis; Obtaining a yaw rate equation of the lower chassis according to a steering angle variable of the lower chassis, a moving speed variable of the lower chassis and a preset wheelbase; The yaw angular velocity equation of the upper platform is obtained according to the moving speed variable of the lower chassis, the yaw angle variable of the upper platform and the yaw angle variable of the lower chassis; The first error state space equation is established according to the kinematic model, the reference state of the upper platform and the reference state of the lower chassis, specifically including: Obtaining a first state matrix and a second state matrix according to the reference yaw angle of the upper platform, the reference yaw angle of the lower chassis and the reference steering angle of the lower chassis; Performing Taylor series expansion on the kinematic model according to the first state matrix, the second state matrix, the reference state of the upper platform, and the reference state of the lower chassis to obtain the first error state space equation; The step of solving the first error state space equation to obtain the control quantity of the unmanned vehicle specifically includes: Discretize the first error state space equation for the first time to obtain a discretized second error state space equation; Obtaining an augmented matrix according to the second error state space equation, and performing a second discretization on the second error state space equation according to the augmented matrix to obtain a third error state space equation; A cost equation is obtained according to the third error state space equation, the minimization of the cost equation is taken as an optimization target, and the third error state space equation is solved according to the optimization target and preset constraints to obtain the control amount of the unmanned vehicle.

2. The method according to claim 1, characterized in that The sensor data includes radar data and inertial sensor data, and obtaining a global map and a current position of the unmanned vehicle according to the sensor data specifically includes: Constructing a global map according to the radar data, the inertial sensor data and a preset mapping algorithm; The current position of the unmanned vehicle is obtained according to a preset positioning algorithm, the radar data and the inertial sensor data.

3. The method according to claim 1, characterized in that: The path of the unmanned vehicle includes the current position and the target position of the unmanned vehicle, and obtaining the reference state of the upper platform and the reference state of the lower chassis according to the path of the unmanned vehicle specifically includes: Calculating the distance and angle between the current position and the target position, and obtaining a reference steering angle of the lower chassis according to the distance, the angle and a preset wheelbase of the lower chassis; Obtaining a reference yaw angle of the upper platform according to the current position, the target position and the arc tangent function, and taking the target position as the reference position of the upper platform; The reference position of the lower chassis is obtained according to the reference yaw angle of the upper platform and the reference position of the upper platform, and the reference yaw angle of the next chassis is obtained according to the reference position of the lower chassis.

4. The method according to claim 1, characterized in that: The control amount includes a control steering angle of the lower chassis and a control moving speed of the lower chassis, and controlling the unmanned vehicle according to the control amount specifically includes: Obtaining a rotation angle of each wheel of the lower chassis according to a control steering angle of the lower chassis; A control signal of the unmanned vehicle is obtained according to the rotation angle of each wheel of the lower chassis and the controlled moving speed of the lower chassis, and the unmanned vehicle is controlled according to the control signal.

5. A control device for unmanned transport equipment, characterized in that: include: at least one processor; at least one memory for storing at least one program; When the at least one program is executed by the at least one processor, the at least one processor implements the method according to any one of claims 1 to 4.

6. A computer-readable storage medium storing a program executable by a processor, characterized in that: The processor-executable program is used to perform the method according to any one of claims 1 to 4 when executed by the processor.

7. An unmanned transport device, characterized in that: It includes a lower chassis and an upper platform, wherein the lower chassis includes a steering device, a perception module and a control planning module, and the lower chassis is connected to the upper platform, wherein: The perception module is used to collect sensor data and send the sensor data to the control planning module; The control planning module is used to obtain the control quantity according to the method described in any one of claims 1 to 4; The steering device is used to move according to the control amount.

Images