Trajectory tracking control method and device and operation machine

Abstract

The invention discloses a trajectory tracking control method and device and an operation machine, and relates to the technical field of operation machines. The trajectory tracking control method comprises the steps that the actual running position of a walking mechanism and the actual speed of a driving wheel are obtained in real time, and the walking mechanism moves based on the two-wheel differential driving principle; based on the reference operation position, the driving wheel reference speed, the actual operation position and the driving wheel actual speed of the walking mechanism, a preset state space model is adopted to calculate the error between the actual motion state and the expected motion state of the walking mechanism, and the current state quantity error variation is obtained; and controlling the speed of a driving wheel of the walking mechanism based on the current state quantity error variation. The state quantity deviation between the current actual track and the expected track can be accurately calculated, the speed of the driving wheel is adjusted according to the deviation, control is easy and convenient, accurate tracking of the expected track can be achieved, and driving according to the expected path is facilitated.

Description

Technical Field

[0001] The present application relates to the technical field of working machinery, and specifically to a trajectory tracking control method, a trajectory tracking device, a working machinery, a machine-readable storage medium, and an electronic device. Background Art

[0002] Trajectory tracking is an important part of realizing intelligent driving. It enables some working machinery such as excavators and skid loaders to travel along preset paths, improving driving safety and convenience.

[0003] However, the existing trajectory tracking control research objects are mainly based on vehicles based on the Ackermann angle, such as bicycles and cars. For tracked and wheeled vehicles, such as excavators and skid loaders, the existing trajectory tracking control cannot be implemented, and thus autonomous driving cannot be achieved. Summary of the invention

[0004] The purpose of the embodiments of the present application is to provide a trajectory tracking control method, a trajectory tracking device, an operating machine, a machine-readable storage medium and an electronic device, so as to solve the problem that trajectory tracking control cannot be achieved for tracked or wheeled vehicles in the prior art.

[0005] In order to achieve the above-mentioned object, the present application provides a trajectory tracking control method in a first aspect, comprising:

[0006] Acquire the actual running position of the walking mechanism and the actual speed of the driving wheel in real time, wherein the walking mechanism moves based on the principle of two-wheel differential drive;

[0007] Based on the reference running position of the walking mechanism, the reference speed of the driving wheel, the actual running position and the actual speed of the driving wheel, a preset state space model is used to calculate the error between the actual motion state of the walking mechanism and the expected motion state, and the error change of the current state quantity is obtained;

[0008] Based on the current state quantity error variation, the driving wheel speed of the walking mechanism is controlled.

[0009] In the embodiment of the present application, the process of constructing the preset state space model includes:

[0010] Construct differential kinematics model;

[0011] Taking the running position of the walking mechanism as the initial state variable and the driving wheel speed of the walking mechanism as the initial control variable, transforming the differential kinematic model to obtain a kinematic model expression;

[0012] Performing Taylor series expansion on the kinematic model expression at a reference point and discarding high-order terms to obtain a new kinematic model expression;

[0013] Based on the new kinematic model expression, an expression for the variation of the state variable error is obtained;

[0014] The expression of the variation of the state variable error is discretized to obtain a state space model.

[0015] In the embodiment of the present application, the differential kinematics model is:

[0016]

[0017] Among them, v l , v r are the actual speeds of the driving wheels on both sides, l is the distance between the driving wheels on both sides, is the overall speed of the walking mechanism, It is expressed as the angular velocity of the walking mechanism, θ is the actual angle of the walking mechanism, It is expressed as the speed of the walking mechanism in the x direction, Expressed as the speed of the walking mechanism in the y direction.

[0018] In the embodiment of the present application, the new kinematic model expression is:

[0019]

[0020] in, is the differential kinematics model, f(X r ,U r ) is the differential kinematics model of the reference point, x, y are the actual positions of the walking mechanism, θ is the actual angle of the walking mechanism, v l , v r are the actual speeds of the driving wheels on both sides, are the speeds of the driving wheels on both sides at the reference point, θ r is the angle of the walking mechanism at the reference point, l is the distance between the driving wheels on both sides, X is the initial state variable, X=[x,y,θ] T , U is the initial control quantity, U=[v l ,v r ] T , X r is the initial state variable of the reference point, X r =[x r ,y r ,θ r ] T , U r is the initial control quantity of the reference point, and are all Jacobian matrices,

[0021]

[0022] In the embodiment of the present application, the expression for the variation of the state variable error is:

[0023]

[0024] in, is the change in state variable error, A is B is is the new state variable, expressed as is the new control quantity, expressed as It is expressed as the angular velocity of the walking mechanism at the reference point, It is expressed as the speed of the walking mechanism in the x direction at the reference point, Expressed as the speed of the walking mechanism in the y direction at the reference point.

[0025] In the embodiment of the present application, the preset state space model is:

[0026]

[0027] in, for T is the control period, k is the time, is the new state variable at time k, is the new control quantity at time k, is the new state variable at time k+1.

[0028] In the embodiment of the present application, the control of the driving wheel speed of the walking mechanism based on the current state quantity error variation includes:

[0029] Based on the current state quantity error variation, an optimization function is established with the realization of a preset optimal control state as the optimization goal;

[0030] Solving the optimization function to obtain an optimal control quantity;

[0031] Based on the optimal control amount, the driving wheel speed of the traveling mechanism is controlled.

[0032] A second aspect of the present application provides a trajectory tracking control device, comprising:

[0033] An acquisition module is used to acquire the actual running position of the walking mechanism and the actual speed of the driving wheel in real time, wherein the walking mechanism moves based on the principle of two-wheel differential drive;

[0034] A determination module is used to calculate the error between the actual motion state and the expected motion state of the walking mechanism using a preset state space model based on the reference running position of the walking mechanism, the reference speed of the driving wheel, the actual running position and the actual speed of the driving wheel, and obtain the error change of the current state quantity;

[0035] A control module is used to control the driving wheel speed of the walking mechanism based on the current state quantity error change.

[0036] A third aspect of the present application provides a working machine, including a walking mechanism, wherein the walking mechanism adopts the above-mentioned trajectory tracking control method to perform trajectory tracking.

[0037] A fourth aspect of the present application provides an electronic device, the electronic device comprising:

[0038] at least one processor;

[0039] a memory connected to the at least one processor;

[0040] The memory stores instructions that can be executed by the at least one processor, and the at least one processor implements the above-mentioned trajectory tracking control method by executing the instructions stored in the memory.

[0041] A fifth aspect of the present application provides a machine-readable storage medium, on which instructions are stored. When the instructions are executed by a processor, the processor is configured to execute the above-mentioned trajectory tracking control method.

[0042] Through the above technical solution, by obtaining the actual running position of the walking mechanism and the actual speed of the driving wheel in real time, the walking mechanism moves based on the two-wheel differential drive principle, based on the reference running position of the walking mechanism, the reference speed of the driving wheel, the actual running position and the actual speed of the driving wheel, the preset state space model is used to calculate the error between the actual motion state of the walking mechanism and the expected motion state, and the current state quantity error change is obtained. Based on the current state quantity error change, the speed of the driving wheel of the walking mechanism is controlled. The state quantity deviation between the current actual trajectory and the expected trajectory can be accurately calculated through the state space model, and then the driving wheel speed is adjusted according to the state quantity deviation. The control is simple and convenient, and the accurate tracking of the expected trajectory can be achieved, so as to facilitate driving along the expected path.

[0043] Other features and advantages of the embodiments of the present application will be described in detail in the subsequent specific implementation section. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] The accompanying drawings are used to provide a further understanding of the embodiments of the present application and constitute a part of the specification. Together with the following specific implementations, they are used to explain the embodiments of the present application, but do not constitute a limitation on the embodiments of the present application. In the accompanying drawings:

[0045] Figure 1 A schematic diagram of a flow chart of a trajectory tracking control method according to an embodiment of the present application is shown;

[0046] Figure 2 A schematic diagram of a differential kinematics model according to an embodiment of the present application is schematically shown;

[0047] Figure 3 The schematic diagram of the excavator trajectory tracking control step flow chart according to the embodiment of the present application is shown schematically;

[0048] Figure 4 The schematic diagram shows the effect of LQR trajectory tracking control according to an embodiment of the present application;

[0049] Figure 5 A schematic diagram of the structure of a trajectory tracking control device according to an embodiment of the present application is shown;

[0050] Figure 6 The internal structure of a computer device according to an embodiment of the present application is schematically shown.

[0051] Description of Reference Numerals

[0052] 410 - acquisition module; 420 - determination module; 430 - control module; A01 - processor; A02 - network interface; A03 - internal memory; A04 - display screen; A05 - input device; A06 - non-volatile storage medium; B01 - operating system; B02 - computer program. DETAILED DESCRIPTION

[0053] In order to make the purpose, technical scheme and advantages of the embodiments of the present application clearer, the technical scheme in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. It should be understood that the specific implementation methods described herein are only used to illustrate and explain the embodiments of the present application, and are not used to limit the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of this application.

[0054] It should be noted that if the embodiments of the present application involve directional indications (such as up, down, left, right, front, back...), the directional indications are only used to explain the relative position relationship, movement status, etc. between the components under a certain specific posture (as shown in the accompanying drawings). If the specific posture changes, the directional indication will also change accordingly.

[0055] In addition, if there are descriptions involving "first", "second", etc. in the embodiments of the present application, the descriptions of "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or suggesting their relative importance or implicitly indicating the number of technical features indicated. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In addition, the technical solutions between the various embodiments can be combined with each other, but they must be based on the ability of ordinary technicians in the field to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such combination of technical solutions does not exist and is not within the scope of protection required by this application.

[0056] Figure 1 The following schematically shows a flow chart of a trajectory tracking control method according to an embodiment of the present application. Figure 1 As shown, the embodiment of the present application provides a trajectory tracking control method, which can be applied to a working machine that moves based on the two-wheel differential drive principle, such as an excavator, a skid loader, etc., and can also be applied to a robot. In this embodiment, in order to facilitate the description of the scheme, the working machine is mainly described as an excavator. The method may include the following steps:

[0057] Step 210: Acquire the actual running position of the walking mechanism and the actual speed of the driving wheel in real time, wherein the walking mechanism moves based on the two-wheel differential drive principle;

[0058] In this embodiment, the walking mechanism can be a wheeled walking mechanism, a crawler walking mechanism, etc., which is moved by two-wheel differential drive. For example, the crawler walking mechanism of the excavator is moved by two-wheel differential drive. The actual operating position of the above-mentioned walking mechanism can be obtained by obtaining centimeter-level positioning in real time through a real-time dynamic (Real-Time Kinematic, RTK) positioning sensor installed on the operating machinery; for indoor two-wheel differential robots, RTK cannot work indoors, and the actual operating position of the above-mentioned walking mechanism can also be obtained by using UWB (ultra-wideband) positioning, RFID positioning, WIFI positioning, Bluetooth positioning, Zigbee positioning and other methods, among which UWB (ultra-wideband) positioning can achieve centimeter-level accuracy, and other methods can achieve meter-level accuracy. Different methods can be selected according to actual needs. The above-mentioned actual operating position includes the actual position and angle of the operating mechanism. The actual speed of the above-mentioned driving wheel can be obtained by real-time detection by the encoder on the driving wheel. Taking the excavator as an example, an RTK positioning sensor is installed on the excavator to detect the actual operating position of the excavator; encoders are installed on the crawler rotation hubs on both sides of the excavator chassis to detect the speed of the crawlers on both sides to obtain the actual speed of the drive wheels.

[0059] Step 220: Based on the reference running position of the walking mechanism, the reference speed of the driving wheel, the actual running position and the actual speed of the driving wheel, a preset state space model is used to calculate the error between the actual motion state and the expected motion state of the walking mechanism to obtain the current state error change;

[0060] In this embodiment, the reference operating position and the driving wheel reference speed refer to the corresponding operating position and driving wheel speed when operating according to the desired trajectory. The reference operating position and the driving wheel reference speed can be obtained after the desired trajectory is determined. The preset state space model can be pre-constructed and used to calculate the error between the actual motion state and the expected motion state of the walking mechanism. The actual motion state refers to the current motion state during actual operation, and the expected motion state refers to the current motion state when operating according to the expected trajectory. The above motion state can be described by a differential kinematic model. By substituting the reference operating position, the driving wheel reference speed, the actual operating position and the actual speed of the driving wheel of the walking mechanism into the preset state space model, the current state quantity error change can be calculated.

[0061] In some embodiments, in order to accurately calculate the state error between the actual trajectory and the expected trajectory, a differential kinematics model may be constructed. Specifically, the construction process of the preset state space model includes:

[0062] First, a differential kinematics model is constructed;

[0063] In this embodiment, the differential kinematics model is a mathematical model used to describe a motion system with a differential device. The differential kinematics model is usually based on the following key parameters: 1. Wheel radius: affects the linear speed of the wheel. 2. The rotation speed of the left and right wheels. 3. The steering angle of the vehicle or robot. By analyzing and calculating these parameters, important information such as the motion trajectory, speed and acceleration of the system can be obtained, providing a theoretical basis for the design and optimization of the control system.

[0064] For details, please see Figure 2 , Figure 2 The schematic diagram of the differential kinematics model according to the embodiment of the present application is shown schematically. Assume that the distance between the driving wheels on both sides is l, and the turning radius is C to O c The length of the connecting line, when turning w c =w l =w r ,so So we can find the angular velocity Turning radius is Therefore, the differential kinematics model can be organized as:

[0065]

[0066] Among them, v l , v r are the actual speeds of the driving wheels on both sides, l is the distance between the driving wheels on both sides, is the overall speed of the walking mechanism, f3 is the angular velocity of the walking mechanism, θ is the actual angle of the walking mechanism, f1 represents the speed of the walking mechanism in the x direction, f2 represents the speed of the walking mechanism in the y direction.

[0067] Then, the running position of the walking mechanism is used as the initial state variable, and the driving wheel speed of the walking mechanism is used as the initial control variable, and the differential kinematic model is transformed to obtain a kinematic model expression;

[0068] In this embodiment, the state variable can be selected as X = [x, y, θ] T , that is, the initial state variable, the control quantity U=[v l ,v r ] T , that is, the initial control quantity. Accordingly, the above differential kinematics model can be rewritten as: That is, the kinematic model expression is obtained.

[0069] Then, the kinematic model expression is subjected to Taylor series expansion at a reference point and high-order terms are discarded to obtain a new kinematic model expression;

[0070] In this embodiment, the state variables and control quantities corresponding to the reference point are: The derivatives of the kinematic model expression at the reference point and its neighboring points are first calculated using Taylor series expansion, and then the higher-order terms are ignored to obtain a new kinematic model expression. Taylor series is a method of expressing a function using an infinite series of terms, which are obtained by the derivative of the function at a certain point. The above-mentioned prior art of Taylor series expansion input is not described in detail here.

[0071] Wherein, the new kinematic model expression is:

[0072]

[0073] in, is the differential kinematics model, f(X r ,U r ) is the differential kinematics model of the reference point, x, y are the actual positions of the walking mechanism, θ is the actual angle of the walking mechanism, v l , v r are the actual speeds of the driving wheels on both sides, are the speeds of the driving wheels on both sides at the reference point, θ r is the angle of the walking mechanism at the reference point, l is the distance between the driving wheels on both sides, X is the initial state variable, X=[x,y,θ] T , U is the initial control quantity, U=[v l ,v r ] T , X r is the initial state variable of the reference point, X r =[x r ,y r ,θ r ] T , U r is the initial control quantity of the reference point, and are all Jacobian matrices,

[0074]

[0075] Then, based on the new kinematic model expression, an expression for the variation of the state variable error is obtained;

[0076] In this embodiment, after obtaining the new kinematic model expression, the kinematic model of the reference point is subtracted from the actual kinematic model to obtain the expression for the change in the state variable error.

[0077] Among them, the expression of the change amount of the state variable error is:

[0078]

[0079]

[0080] in, is the change in state variable error, A is B is is the new state variable, expressed as is the new control quantity, expressed as It is expressed as the angular velocity of the walking mechanism at the reference point, It is expressed as the speed of the walking mechanism in the x direction at the reference point, Expressed as the speed of the walking mechanism in the y direction at the reference point.

[0081] Finally, the expression of the variation of the state variable error is discretized to obtain a state space model.

[0082] In this embodiment, the above discretization process can be forward Euler discretization, which uses the information of known points to estimate the value at the next moment. Then after sorting, the preset state space model can be obtained as:

[0083]

[0084] in, for for T is the control period, k is the time, is the new state variable at time k, is the new control quantity at time k, is the new state variable at time k+1.

[0085] Based on the above state-space model, the optimal control law can be solved by using methods such as linear quadratic regulator (LQR), proportional-integral-derivative control (PID), model predictive control (MPC), etc. to obtain the optimal control quantity.

[0086] Step 230: Based on the current state quantity error variation, the driving wheel speed of the walking mechanism is controlled.

[0087] In this embodiment, the above-mentioned current state quantity error change reflects the error between the current actual tracking trajectory and the expected tracking trajectory. The driving wheel speed of the walking mechanism can be adjusted according to the current state quantity error change, so that the running trajectory coincides with the expected trajectory to achieve trajectory tracking.

[0088] In some embodiments, LQR can be used to solve the above optimization problem, and the driving wheel speed of the walking mechanism is controlled based on the current state quantity error change, including:

[0089] Firstly, based on the current state quantity error variation, an optimization function is established with the realization of a preset optimal control condition as the optimization goal;

[0090] In this embodiment, the above-preset optimal control state is that the state variable quickly and steadily tends to zero and maintains a stable state, and the control amount is minimal. For the above-mentioned discrete system, that is, the current state variable error change, the optimal control is to choose the appropriate control ..., so that the state quantity …can quickly stabilize towards zero and remain in a stable state, while …as small as possible. These two quantities usually constrain each other. If a larger input is taken, the state quantity can reach the zero state faster. Therefore, the optimization function can be established with the state variable quickly and steadily tending to zero and maintaining a stable state and the control quantity being minimized as the optimization goal. This is a typical multi-objective optimization optimal control problem, and the following quadratic cost function can be defined as:

[0091]

[0092] Among them, Q is a semi-positive definite state weighting matrix, R is a positive definite control weighting matrix, and both are usually diagonal matrices; the larger the element in Q, the faster we hope the state quantity will approach zero; the larger the element in R, the smaller the control quantity will be, N is the total number of discrete time moments, and k represents the kth moment of discrete time.

[0093] Then, the optimization function is solved to obtain the optimal control quantity;

[0094] In this embodiment, in trajectory tracking, the first optimization objective in the above optimization function represents the cumulative size of the path deviation of the tracking process, and the second optimization objective represents the loss of control energy in the tracking process, so that the trajectory tracking problem is transformed into an optimal control problem.

[0095] Specifically, LQR has strong robustness, can handle multi-state situations, and can also reduce energy consumption. When LQR is used to solve the optimization function, the linear quadratic regulator is a linear function of the state variable, which can be expressed as:

[0096]

[0097] where P is the solution of the following Riccati equation:

[0098]

[0099] The steps of the LQR solution control law are as follows:

[0100] 1. Determine the number of iteration steps S and set the precision range EPS;

[0101] 2. Set the initial iteration value P = Q;

[0102] 3. Perform iterative loop, t = 1,..., S;

[0103] If ||P new - P|| < EPS, jump out of the loop; otherwise, let P = P new .

[0104] 4. Calculate the feedback coefficient

[0105] 5. Obtain the optimal control quantity

[0106] It should be noted that the above solution processes all belong to the prior art and will not be elaborated here.

[0107] Finally, based on the optimal control quantity, control the driving wheel speed of the walking mechanism.

[0108] In this embodiment, the optimal driving wheel speed can be obtained according to the optimal control quantity, so as to control the driving wheel speed of the walking mechanism to achieve trajectory tracking.

[0109] In the above implementation process, by obtaining the actual running position and the actual driving wheel speed of the walking mechanism in real time, the walking mechanism moves based on the two-wheel differential drive principle. Based on the reference running position, the reference driving wheel speed, the actual running position and the actual driving wheel speed of the walking mechanism, use the preset state space model to calculate the error between the actual motion state and the desired motion state of the walking mechanism, and obtain the current state quantity error change amount. Based on the current state quantity error change amount, control the driving wheel speed of the walking mechanism. Through the state space model, the state quantity deviation between the current actual trajectory and the desired trajectory can be accurately calculated, and then the driving wheel speed is adjusted according to the state quantity deviation. The control is simple and convenient, and the accurate tracking of the desired trajectory can be realized, so as to drive according to the desired path.

[0110] Next, taking the control of the excavator trajectory tracking as an example, the solution will be described in detail. Please refer to Figure 3 , Figure 3 which schematically shows the flow chart of the excavator trajectory tracking control steps according to the embodiment of the present application.

[0111] The crawler walking mechanism of the excavator moves through two-wheel differential drive. An RTK positioning sensor is installed on the excavator to detect the position of the excavator in real time; encoders are installed on the crawler rotation centers on both sides of the chassis to detect the speed of the crawlers on both sides in real time. By modeling the kinematics of the differential chassis, the discretized state equation is obtained, that is, the state space model is obtained. According to the position of the excavator, the position of the excavator and the expected tracking trajectory, the current state quantity error change is calculated according to the state space model. Then, based on the current state quantity error change, a quadratic cost function is defined, and the LQR control law is solved to obtain the optimal control quantity, thereby controlling the speed of the crawlers on both sides of the excavator.

[0112] like Figure 4 As shown, Figure 4 The schematic diagram of the LQR trajectory tracking control effect according to the embodiment of the present application is shown. Assume that the initial position of the vehicle is at the coordinate (0, 3), the horizontal axis represents the displacement in the x-axis direction, and the vertical axis represents the displacement in the y-axis direction. The distance between the driving wheels on both sides is 2m, the initial left wheel speed is 1m / s, the initial right wheel speed is 1m / s, the initial heading angle is horizontal, let Q and R be unit diagonal matrices, the expected tracking trajectory is the blue dotted curve, the actual tracking trajectory is the red solid curve, the green dot represents the current position of the vehicle, Figure 4 It can be seen from the curve in that LQR control can track the desired trajectory well.

[0113] Figure 1 FIG. 1 is a flow chart of the trajectory tracking control method in the embodiment. It should be understood that although Figure 1 The steps in the flowchart are shown in sequence as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless otherwise specified in this document, there is no strict order restriction for the execution of these steps, and these steps can be executed in other orders. Moreover, Figure 1 At least part of the steps may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily executed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least part of the sub-steps or stages of other steps.

[0114] This embodiment provides a working machine, including a traveling mechanism, wherein the traveling mechanism uses the above-mentioned trajectory tracking control method to perform trajectory tracking.

[0115] In this embodiment, the working machine may be a crawler-type excavator, a wheeled-type skid loader, etc. Correspondingly, the traveling mechanism may be a wheeled traveling mechanism, a crawler-type traveling mechanism, etc., located on the chassis of the working machine.

[0116] In the above implementation process, the walking mechanism can accurately calculate the deviation between the current actual trajectory and the expected trajectory through the state space model, and then adjust the driving wheel speed according to the deviation. The control is simple and convenient, and accurate tracking of the expected trajectory can be achieved, so that the operating machinery can travel according to the expected path, thereby realizing automatic driving of the operating machinery.

[0117] Please see Figure 5 , Figure 5 The structure diagram of a trajectory tracking control device according to an embodiment of the present application is schematically shown. This embodiment provides a trajectory tracking control device, including an acquisition module 410, a determination module 420 and a control module 430, wherein:

[0118] An acquisition module 410 is used to acquire in real time the actual running position of the walking mechanism and the actual speed of the driving wheel, wherein the walking mechanism moves based on the two-wheel differential drive principle;

[0119] A determination module 420 is used to calculate the error between the actual motion state and the expected motion state of the walking mechanism using a preset state space model based on the reference running position of the walking mechanism, the reference speed of the driving wheel, the actual running position and the actual speed of the driving wheel, and obtain the current state error change;

[0120] The control module 430 is used to control the driving wheel speed of the walking mechanism based on the current state quantity error change.

[0121] The trajectory tracking control method device includes a processor and a memory. The acquisition module 410, the determination module 420 and the control module 430 are all stored in the memory as program units, and the processor executes the program units stored in the memory to implement corresponding functions.

[0122] The processor includes a kernel, which calls the corresponding program unit from the memory. One or more kernels can be set, and trajectory tracking control can be achieved by adjusting kernel parameters.

[0123] The memory may include non-permanent memory in a computer-readable medium, random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM, and the memory includes at least one memory chip.

[0124] An embodiment of the present invention provides a machine-readable storage medium on which a program is stored. When the program is executed by a processor, the trajectory tracking control method is implemented.

[0125] An embodiment of the present invention provides a processor, which is used to run a program, wherein the trajectory tracking control method is executed when the program is running.

[0126] In one embodiment, a computer device is provided. The computer device may be a terminal, and its internal structure diagram may be as follows: Figure 6 As shown. The computer device includes a processor A01, a network interface A02, a display screen A04, an input device A05 and a memory (not shown in the figure) connected via a system bus. Among them, the processor A01 of the computer device is used to provide computing and control capabilities. The memory of the computer device includes an internal memory A03 and a non-volatile storage medium A06. The non-volatile storage medium A06 stores an operating system B01 and a computer program B02. The internal memory A03 provides an environment for the operation of the operating system B01 and the computer program B02 in the non-volatile storage medium A06. The network interface A02 of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor A01, a trajectory tracking control method is implemented. The display screen A04 of the computer device can be a liquid crystal display or an electronic ink display, and the input device A05 of the computer device can be a touch layer covered on the display screen, or a button, trackball or touchpad set on the computer device housing, or an external keyboard, touchpad or mouse, etc.

[0127] Those skilled in the art will understand that Figure 6 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0128] In one embodiment, the trajectory tracking control method and device provided in the present application can be implemented in the form of a computer program. The computer program can be Figure 6 The computer device shown in the figure is run. The memory of the computer device can store various program modules constituting the trajectory tracking control method device, such as: Figure 5 The acquisition module 410, the determination module 420 and the control module 430 are shown. The computer program composed of various program modules enables the processor to execute the steps of the trajectory tracking control method of each embodiment of the present application described in this specification.

[0129] Figure 6 The computer device shown can be Figure 5The acquisition module 410 in the trajectory tracking control method device shown executes step 210. The computer device may execute step 220 through the determination module 420. The computer device may execute step 230 through the control module 430.

[0130] An embodiment of the present application provides an electronic device, the electronic device comprising: at least one processor; a memory connected to the at least one processor; wherein the memory stores instructions that can be executed by the at least one processor, the at least one processor implements the above-mentioned trajectory tracking control method by executing the instructions stored in the memory, and the processor implements the following steps when executing the instructions:

[0131] Acquire the actual running position of the walking mechanism and the actual speed of the driving wheel in real time, wherein the walking mechanism moves based on the principle of two-wheel differential drive;

[0132] Based on the reference running position of the walking mechanism, the reference speed of the driving wheel, the actual running position and the actual speed of the driving wheel, a preset state space model is used to calculate the error between the actual motion state of the walking mechanism and the expected motion state, and the current state error change is obtained;

[0133] Based on the current state quantity error variation, the driving wheel speed of the walking mechanism is controlled.

[0134] In one embodiment, the process of constructing the preset state space model includes:

[0135] Construct differential kinematics model;

[0136] Taking the running position of the walking mechanism as the initial state variable and the driving wheel speed of the walking mechanism as the initial control variable, transforming the differential kinematic model to obtain a kinematic model expression;

[0137] Performing Taylor series expansion on the kinematic model expression at a reference point and discarding high-order terms to obtain a new kinematic model expression;

[0138] Based on the new kinematic model expression, an expression for the variation of the state variable error is obtained;

[0139] The expression of the variation of the state variable error is discretized to obtain a state space model.

[0140] In one embodiment, the differential kinematics model is:

[0141]

[0142] Among them, v l , v rare the actual speeds of the driving wheels on both sides, l is the distance between the driving wheels on both sides, is the overall speed of the walking mechanism, It is expressed as the angular velocity of the walking mechanism, θ is the actual angle of the walking mechanism, It is expressed as the speed of the walking mechanism in the x direction, Expressed as the speed of the walking mechanism in the y direction.

[0143] In one embodiment, the new kinematic model expression is:

[0144]

[0145] in, is the differential kinematics model, f(X r ,U r ) is the differential kinematics model of the reference point, x, y are the actual positions of the walking mechanism, θ is the actual angle of the walking mechanism, v l , v r are the actual speeds of the driving wheels on both sides, are the speeds of the driving wheels on both sides at the reference point, θ r is the angle of the walking mechanism at the reference point, l is the distance between the driving wheels on both sides, X is the initial state variable, X=[x,y,θ] T , U is the initial control quantity, U=[v l ,v r ] T , X r is the initial state variable of the reference point, X r =[x r ,y r ,θ r ] T , U r is the initial control quantity of the reference point, and are all Jacobian matrices,

[0146]

[0147] In one embodiment, the variation expression of the state variable error is:

[0148]

[0149] in, is the change in state variable error, A is B is is the new state variable, expressed as is the new control quantity, expressed as It is expressed as the angular velocity of the walking mechanism at the reference point, It is expressed as the speed of the walking mechanism in the x direction at the reference point, Expressed as the speed of the walking mechanism in the y direction at the reference point.

[0150] In one embodiment, the preset state space model is:

[0151]

[0152] in, for for T is the control period, k is the time, is the new state variable at time k, is the new control quantity at time k, is the new state variable at time k+1.

[0153] In one embodiment, the controlling of the driving wheel speed of the walking mechanism based on the current state quantity error variation includes:

[0154] Based on the current state quantity error variation, an optimization function is established with the realization of a preset optimal control state as the optimization goal;

[0155] Solving the optimization function to obtain an optimal control quantity;

[0156] Based on the optimal control amount, the driving wheel speed of the traveling mechanism is controlled.

[0157] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.

[0158] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1A device that provides the functions specified in a block or multiple blocks.

[0159] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0160] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0161] In a typical configuration, a computing device includes one or more processors (CPU), input / output interfaces, network interfaces, and memory.

[0162] The memory may include non-permanent memory in a computer-readable medium, random access memory (RAM) and / or non-volatile memory in the form of read-only memory (ROM) or flash RAM. The memory is an example of a computer-readable medium.

[0163] Computer readable media include permanent and non-permanent, removable and non-removable media that can be implemented by any method or technology to store information. Information can be computer readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disk read-only memory (CD-ROM), digital versatile disk (DVD) or other optical storage, magnetic cassettes, magnetic disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device. As defined herein, computer readable media does not include temporary computer readable media (transitory media), such as modulated data signals and carrier waves.

[0164] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0165] The above are only embodiments of the present application and are not intended to limit the present application. For those skilled in the art, the present application may have various changes and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included within the scope of the claims of the present application.

Claims

1. A trajectory tracking control method, characterized in that: include: Acquire the actual running position of the walking mechanism and the actual speed of the driving wheel in real time, wherein the walking mechanism moves based on the principle of two-wheel differential drive; Based on the reference running position of the walking mechanism, the reference speed of the driving wheel, the actual running position and the actual speed of the driving wheel, a preset state space model is used to calculate the error between the actual motion state of the walking mechanism and the expected motion state, and the error change of the current state quantity is obtained; Based on the current state quantity error variation, the driving wheel speed of the walking mechanism is controlled.

2. The method according to claim 1, characterized in that The construction process of the preset state space model includes: Construct differential kinematics model; Taking the running position of the walking mechanism as the initial state variable and the driving wheel speed of the walking mechanism as the initial control variable, transforming the differential kinematic model to obtain a kinematic model expression; Performing Taylor series expansion on the kinematic model expression at a reference point and discarding high-order terms to obtain a new kinematic model expression; Based on the new kinematic model expression, an expression for the variation of the state variable error is obtained; The expression of the variation of the state variable error is discretized to obtain a state space model.

3. The method according to claim 2, characterized in that The differential kinematic model is: Among them, v l , v r are the actual speeds of the driving wheels on both sides, l is the distance between the driving wheels on both sides, is the overall speed of the walking mechanism, It is expressed as the angular velocity of the walking mechanism, θ is the actual angle of the walking mechanism, It is expressed as the speed of the walking mechanism in the x direction, Expressed as the speed of the walking mechanism in the y direction.

4. The method according to claim 2, characterized in that: The new kinematic model expression is: in, is the differential kinematics model, f(X r , U r ) is the differential kinematics model of the reference point, x, y are the actual positions of the walking mechanism, θ is the actual angle of the walking mechanism, v l , v r are the actual speeds of the driving wheels on both sides, are the speeds of the driving wheels on both sides at the reference point, θ r is the angle of the walking mechanism at the reference point, l is the distance between the driving wheels on both sides, X is the initial state variable, X=[x,y,θ] T , U is the initial control quantity, U=[v l , v r ] T , X r is the initial state variable of the reference point, X r =[x r ,y r ,θ r ] T , U r is the initial control quantity of the reference point, and are all Jacobian matrices, 5. The method according to claim 2, characterized in that: The expression of the variation of the state variable error is: in, is the change in state variable error, A is B is is the new state variable, expressed as is the new control quantity, expressed as It is expressed as the angular velocity of the walking mechanism at the reference point, It is expressed as the speed of the walking mechanism in the x direction at the reference point, Expressed as the speed of the walking mechanism in the y direction at the reference point.

6. The method according to claim 1, characterized in that The preset state space model is: in, for for T is the control period, k is the time, is the new state variable at time k, is the new control quantity at time k, is the new state variable at time k+1.

7. The method according to claim 1, characterized in that The controlling of the driving wheel speed of the walking mechanism based on the current state quantity error variation includes: Based on the current state quantity error variation, an optimization function is established with the realization of a preset optimal control state as the optimization goal; Solving the optimization function to obtain an optimal control quantity; Based on the optimal control amount, the driving wheel speed of the traveling mechanism is controlled.

8. A trajectory tracking control device, characterized in that: include: An acquisition module is used to acquire the actual running position of the walking mechanism and the actual speed of the driving wheel in real time, wherein the walking mechanism moves based on the principle of two-wheel differential drive; A determination module is used to calculate the error between the actual motion state and the expected motion state of the walking mechanism using a preset state space model based on the reference running position of the walking mechanism, the reference speed of the driving wheel, the actual running position and the actual speed of the driving wheel, and obtain the error change of the current state quantity; A control module is used to control the driving wheel speed of the walking mechanism based on the current state quantity error change.

9. A working machine, characterized in that: It comprises a walking mechanism, and the walking mechanism adopts the trajectory tracking control method according to any one of claims 1 to 7 to perform trajectory tracking.

10. An electronic device, characterized in that: The electronic device includes: at least one processor; a memory connected to the at least one processor; The memory stores instructions that can be executed by the at least one processor, and the at least one processor implements the trajectory tracking control method according to any one of claims 1 to 7 by executing the instructions stored in the memory.

11. A machine-readable storage medium having instructions stored thereon, characterized in that: When the instruction is executed by a processor, the processor is configured to execute the trajectory tracking control method according to any one of claims 1 to 7.

Images