Trajectory tracking method and device, electronic equipment and medium

Abstract

The embodiment of the invention provides a trajectory tracking method and device, electronic equipment and a medium, which are used for solving the problem that trajectory tracking cannot be carried out due to insufficient data. The method comprises the following steps: for each moment when the differential robot walks from a current position to a destination, predicting predicted trajectory information in a time window corresponding to the current moment according to an orientation angle detected by a sensor at the current moment; extracting reference sub-information in a time window corresponding to the current moment from the reference trajectory information of the differential robot; according to the difference between the predicted trajectory information and the reference sub-information, determining the predicted speeds of the left and right wheels at each moment in the time window corresponding to the current moment; and controlling the left and right wheels of the differential robot to walk according to the predicted speed of the left and right wheels at the next moment predicted at the current moment.

Description

Technical Field

[0001] The present application relates to the technical field of intelligent path planning, and in particular to a trajectory tracking method, device, electronic device and medium. Background Art

[0002] Differential Drive Robots have been widely used in modern logistics, security inspection, agricultural automation, family companionship and other fields. This type of robot achieves flexible steering and movement by adjusting the speed difference between the left and right wheels, and has the advantages of simple structure and flexible control.

[0003] Traditional differential robot trajectory tracking methods, such as the PID (Proportional-Integral-Derivative Control) controller, adjust the robot's trajectory by calculating the proportion, integral, and differential of the error to make it track the predetermined path. Or the Pure Pursuit Algorithm adjusts the robot's movement direction and speed by calculating the distance and direction difference between the robot's current position and the forward point on the target path.

[0004] Traditional solutions mainly rely on real-time position information and yaw angle data, which are the basis for robot control systems to perform path planning and real-time correction. Although differential robots are relatively mature in design, achieving accurate trajectory tracking in the absence of high-precision sensor information or in the absence of sensor information of poor quality is still a significant technical challenge. In this environment, how to effectively deal with the problem of insufficient data to ensure that the robot can stably and accurately track the predetermined trajectory has become a difficult problem in research and application. Summary of the invention

[0005] The embodiments of the present application provide a trajectory tracking method, device, electronic device and medium. Based on reference trajectory information, the orientation angle at the current moment is used to predict the trajectory information at multiple moments. Based on the gap between the predicted information and the reference information, the speed of the left and right wheels of the differential robot is determined, thereby avoiding the problem of being unable to perform trajectory tracking due to insufficient data and improving the convenience of tracking.

[0006] In a first aspect, an embodiment of the present application provides a trajectory tracking method, comprising:

[0007] Obtain reference trajectory information of the differential robot from its current position to its destination;

[0008] For each moment when the differential robot walks from the current position to the destination, predict the predicted trajectory information within the time window corresponding to the current moment according to the orientation angle at the current moment detected by the sensor; extract the reference sub-information within the time window corresponding to the current moment from the reference trajectory information of the differential robot;

[0009] Determining the predicted speeds of the left and right wheels at each moment in the time window corresponding to the current moment according to the difference between the predicted trajectory information and the reference sub-information;

[0010] The left and right wheels of the differential robot are controlled to move according to the predicted speed of the left and right wheels at the next moment predicted at the current moment.

[0011] The above method can, based on the reference trajectory information, use the heading angle at the current moment to predict the trajectory information at multiple moments, determine the left and right wheel speeds of the differential robot based on the gap between the predicted information and the reference information, and use the determined left and right wheel speeds to move, cycling in sequence until the differential robot reaches the destination, thereby avoiding the problem of being unable to track the trajectory due to insufficient data during walking, and improving the convenience of tracking.

[0012] In a possible implementation, predicting the predicted trajectory information within a time window corresponding to the current moment according to the orientation angle detected by the sensor at the current moment includes:

[0013] Determine the predicted heading angle represented by the left and right wheel speeds in the predicted trajectory information within the time window corresponding to the current moment according to the heading angle detected by the sensor at the current moment and the heading angle prediction relationship; wherein the heading angle prediction relationship is a functional relationship between the two heading angles at two adjacent moments and the left and right wheel speeds;

[0014] According to the arc length at the current moment and the arc length prediction relationship, the predicted arc length expressed by the left and right wheel speeds in the predicted trajectory information within the time window corresponding to the current moment is determined; wherein the arc length prediction relationship is a functional relationship between two arc lengths at two adjacent moments and the left and right wheel speeds.

[0015] In a possible implementation manner, the determining according to a gap between the predicted trajectory information and the reference sub-information includes:

[0016] For each moment in the time window corresponding to the current moment, determining a first functional relationship represented by left and right wheel speeds at the moment according to a displacement residual between the predicted arc length at the moment in the predicted trajectory information and the reference arc length at the moment in the reference sub-information;

[0017] Determine a second functional relationship represented by left and right wheel speeds at the moment according to a heading angle residual between the predicted heading angle at the moment in the predicted trajectory information and the reference heading angle at the moment in the reference sub-information;

[0018] Determine a third functional relationship represented by the left and right wheel speeds at the moment according to a speed residual between the left and right wheel predicted speeds at the moment in the predicted trajectory information and the reference left and right wheel speeds at the moment in the reference sub-information;

[0019] According to the first functional relationship, the second functional relationship, and the third functional relationship, the difference between the information at that moment in the predicted trajectory information and the information at that moment in the reference sub-information, expressed by the left and right wheel speeds, is determined.

[0020] In a possible implementation, determining the predicted speeds of the left and right wheels at each moment in the time window corresponding to the current moment according to the gap between the predicted trajectory information and the reference sub-information includes:

[0021] The left and right wheel speeds used by the minimum value of the sum of the differences at each moment in the time window corresponding to the current moment are used as the left and right wheel predicted speeds at each moment in the time window corresponding to the current moment.

[0022] In a possible implementation manner, the displacement residual between the predicted arc length at the moment in the predicted trajectory information and the reference arc length at the moment in the reference sub-information is determined by:

[0023] Taking the predicted arc length at that moment in the predicted trajectory information as the predicted total displacement at that moment;

[0024] The arc length difference between the reference arc length at that moment in the reference sub-information and the reference arc length at the current moment is used as the reference total displacement at that moment;

[0025] A value obtained by subtracting the reference total displacement from the predicted total displacement is used as a displacement residual between the predicted arc length at that moment in the predicted trajectory information and the reference arc length at that moment in the reference sub-information.

[0026] In a second aspect, an embodiment of the present application provides a trajectory tracking device, the device comprising:

[0027] An acquisition unit, used to acquire reference trajectory information between the differential robot walking from the current position to the destination;

[0028] A prediction unit, for predicting, for each moment when the differential robot walks from the current position to the destination, predicted trajectory information within a time window corresponding to the current moment according to the orientation angle at the current moment detected by the sensor; and extracting reference sub-information within the time window corresponding to the current moment from the reference trajectory information of the differential robot;

[0029] a determination unit, configured to determine the predicted speeds of the left and right wheels at each moment in a time window corresponding to the current moment according to the difference between the predicted trajectory information and the reference sub-information;

[0030] The control unit is used to control the left and right wheels of the differential robot to walk according to the predicted speed of the left and right wheels at the next moment predicted at the current moment.

[0031] In a possible implementation, the prediction unit is specifically configured to determine the predicted heading angle represented by the left and right wheel speeds in the predicted trajectory information within the time window corresponding to the current moment according to the heading angle at the current moment detected by the sensor and the heading angle prediction relationship; wherein the heading angle prediction relationship is a functional relationship between two heading angles at two adjacent moments and the left and right wheel speeds;

[0032] According to the arc length at the current moment and the arc length prediction relationship, the predicted arc length expressed by the left and right wheel speeds in the predicted trajectory information within the time window corresponding to the current moment is determined; wherein the arc length prediction relationship is a functional relationship between two arc lengths at two adjacent moments and the left and right wheel speeds.

[0033] In a possible implementation, the determining unit is further configured to determine, for each moment in the time window corresponding to the current moment, a first functional relationship represented by left and right wheel speeds at the moment according to a displacement residual between the predicted arc length at the moment in the predicted trajectory information and the reference arc length at the moment in the reference sub-information;

[0034] Determine a second functional relationship represented by left and right wheel speeds at the moment according to a heading angle residual between the predicted heading angle at the moment in the predicted trajectory information and the reference heading angle at the moment in the reference sub-information;

[0035] Determine a third functional relationship represented by the left and right wheel speeds at the moment according to a speed residual between the left and right wheel predicted speeds at the moment in the predicted trajectory information and the reference left and right wheel speeds at the moment in the reference sub-information;

[0036] According to the first functional relationship, the second functional relationship, and the third functional relationship, the difference between the information at that moment in the predicted trajectory information and the information at that moment in the reference sub-information, expressed by the left and right wheel speeds, is determined.

[0037] In a third aspect, an embodiment of the present application provides an execution device, including:

[0038] A memory for storing program instructions;

[0039] The processor is used to obtain the program instructions stored in the memory, and implement the method described in the first aspect and different implementation methods of the first aspect according to the obtained program instructions.

[0040] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, wherein the computer-readable storage medium includes computer instructions. When a computer executes the computer instructions, the method described in the first aspect and different implementation methods of the first aspect are implemented.

[0041] Other features and advantages of the present application will be described in the following description, and partly become apparent from the description, or be understood by practicing the present application. The purpose and other advantages of the present application can be realized and obtained by the structures specifically pointed out in the written description, claims, and drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0043] Figure 1 A flow chart of a trajectory tracking method provided in an embodiment of the present application;

[0044] Figure 2 A schematic diagram of a differential robot tracking provided in an embodiment of the present application;

[0045] Figure 3 A flowchart of a method for determining a predicted arc length and a predicted orientation angle provided in an embodiment of the present application;

[0046] Figure 4 A flow chart for determining a gap at a certain moment provided in an embodiment of the present application;

[0047] Figure 5 A flow chart of another trajectory tracking method provided in an embodiment of the present application;

[0048] Figure 6 A structural diagram of a trajectory tracking device provided in an embodiment of the present application;

[0049] Figure 7A structural diagram of an execution device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0050] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. The components of the embodiments of the present application described and shown in the drawings here can be arranged and designed in various different configurations.

[0051] Therefore, the following detailed description of the embodiments of the present application provided in the accompanying drawings is not intended to limit the scope of the present application for which protection is sought, but merely represents selected embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present application.

[0052] It should be noted that relational terms such as "first" and "second" are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article 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, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0053] Differential Drive Robots are widely used in logistics, security inspection, home services, and agricultural automation due to their simple mechanical structure and flexible motion control. These robots achieve flexible steering and precise positioning by independently controlling the speed difference between the left and right wheels, and are adaptable to a variety of application scenarios. However, during the walking process of the differential drive robot, it mainly relies on real-time position information and heading angle (yaw angle) data, which are the basis for the robot control system to perform path planning and real-time correction. In the absence of high-precision sensor information or poor sensor information quality, position information is missing, and it is impossible to ensure that the robot can stably and accurately track the predetermined trajectory.

[0054] In response to the above problems, the embodiments of the present application provide a trajectory tracking method, device and medium, which can, based on the reference trajectory information, predict the trajectory information of multiple moments for each moment using the orientation angle at the current moment, determine the left and right wheel speeds of the differential robot based on the gap between the predicted information and the reference information, and travel using the determined left and right wheel speeds, thereby avoiding the problem of being unable to perform trajectory tracking due to insufficient data during walking, and improving the convenience of tracking.

[0055] The following is a detailed description with reference to the accompanying drawings.

[0056] like Figure 1 As shown, Figure 1 A flow chart of a trajectory tracking method provided in an embodiment of the present application is exemplarily provided, and the method comprises the following steps:

[0057] S100: Obtain reference trajectory information of the differential robot from the current position to the destination;

[0058] Among them, combined Figure 2 As shown, the differential robot 200 needs to walk from the current position to the destination S. The differential robot 200 needs to perform route planning first. The planned route is the reference trajectory information, wherein the reference trajectory information includes the reference arc length, reference heading angle, reference left wheel speed, and reference right wheel speed at each moment.

[0059] Exemplarily, a spline curve is used to generate the motion trajectory of the robot. Based on the characteristics of the spline curve and the kinematic model of the differential robot, the motion trajectory not only contains position reference information, but also can obtain speed, acceleration information and reference information of the orientation angle through calculation; based on the motion trajectory and the kinematic model of the differential robot, the reference speeds of the left and right wheels are calculated.

[0060] S101: for each moment when the differential robot walks from the current position to the destination, predict the predicted trajectory information within the time window corresponding to the current moment according to the orientation angle at the current moment detected by the sensor; extract the reference sub-information within the time window corresponding to the current moment from the reference trajectory information of the differential robot;

[0061] For example, the time window corresponding to the current moment may be 3. Figure 4 As shown, the current moment is moment t, and the time windows corresponding to the current moment are moment t+1, moment t+2, and moment t+3. According to the orientation angle at moment t detected by the sensor, the predicted trajectory information at moment t+1, the predicted trajectory information at moment t+2, and the predicted trajectory information at moment t+3 are predicted; the reference information at moment t+1, the reference information at moment t+2, and the reference information at moment t+3 are extracted from the reference trajectory information of the differential robot.

[0062] Since an ideal model under an ideal environment is used to determine the predicted trajectory information and the reference trajectory information, there are some interference items in the actual application process, such as ground friction, robot assembly errors, etc., which lead to errors between the actual running trajectory and the predicted trajectory and the reference trajectory. Therefore, in the embodiment of the present invention, the moment can be determined by referring to the number of times. For example, since the trajectory information is predicted at each moment, the current position is the 0 moment, so the first prediction is the first moment, and the second prediction is the second moment. When obtaining the reference sub-information in the time window corresponding to the current moment from the reference trajectory information, it can be obtained according to the number of predictions. For example, if 10 predictions have been made, the current moment is the eleventh moment. From the reference trajectory information, the information in the time window starting from the eleventh moment is the reference sub-trajectory.

[0063] S102: Determine the predicted speed of the left and right wheels at each moment in the time window corresponding to the current moment according to the difference between the predicted trajectory information and the reference sub-information;

[0064] Taking the above as an example, the predicted speeds of the left and right wheels at time t+1, the predicted speeds of the left and right wheels at time t+2, and the predicted speeds of the left and right wheels at time t+3 can be determined based on the gap between the predicted trajectory information at time t+1 and the reference information, the gap between the predicted trajectory information at time t+2 and the reference information, and the gap between the predicted trajectory information at time t+3.

[0065] S103: Control the left and right wheels of the differential robot to move according to the predicted speed of the left and right wheels at the next moment predicted at the current moment.

[0066] Taking the above as an example, from time t to the current time t+1, the left and right wheels of the differential robot are controlled to walk according to the predicted speed of the left and right wheels at time t+1.

[0067] Similarly, when the differential robot walks to time t+1, then time t+1 is taken as the current time, and the time window corresponding to the current time is time t+2, time t+3, and time t+4. Referring to the above process, the predicted speeds of the left and right wheels at time t+2 are determined, and from time t+1 to time t+2, the left and right wheels of the differential robot are controlled to walk according to the predicted speeds of the left and right wheels at time t+2 predicted at time t+1. And so on, until the destination is reached.

[0068] Recombination Figure 2 As shown, the solid line is the reference trajectory, and the dotted line is the trajectory of the predicted speed of the left and right wheels at the next moment predicted for each moment; the dotted line moves close to the solid line and eventually reaches the destination.

[0069] In some embodiments, in combination Figure 3As shown, according to the orientation angle detected by the sensor at the current moment, the predicted trajectory information within the time window corresponding to the current moment is predicted, including:

[0070] S300: determining a predicted heading angle represented by left and right wheel speeds in predicted trajectory information within a time window corresponding to the current moment according to the heading angle detected by the sensor at the current moment and the heading angle prediction relationship; wherein the heading angle prediction relationship is a functional relationship between two heading angles at two adjacent moments and the left and right wheel speeds;

[0071] S301: Determine the predicted arc length expressed by the left and right wheel speeds in the predicted trajectory information within the time window corresponding to the current moment according to the arc length at the current moment and the arc length prediction relationship; wherein the arc length prediction relationship is a functional relationship between two arc lengths at two adjacent moments and the left and right wheel speeds.

[0072] The arc length prediction relationship and the orientation angle prediction relationship are obtained by converting the motion model of the differential robot, where the motion model of the differential robot is:

[0073] x(t+Δt)=x(t)+v·Δt·cos(yaw(t))

[0074] y(t+Δt)=y(t)+v·Δt·sin(yaw(t))

[0075] yaw(t+Δt)=yaw(t)+ω·Δt

[0076] Among them, t represents the moment, Δt is the difference between moment t and the next moment t+1; x(t) represents the position of the differential robot in the x-axis direction in the rectangular coordinate system at moment t; x(t+Δt) represents the displacement of the differential robot in the x-axis direction in the rectangular coordinate system between moment t and the next moment after moment t; v represents the speed of the differential robot, yaw(t) is the orientation angle of the differential robot at moment t; y(t) represents the position of the differential robot in the y-axis direction in the rectangular coordinate system at moment t; y(t+Δt) represents the displacement of the differential robot in the y-axis direction in the rectangular coordinate system between moment t and the next moment after moment t; yaw(t+Δt) represents the orientation angle of the differential robot at the next moment after moment t; ω is the angular velocity of the differential robot.

[0077] Since the embodiment of the present invention focuses on the arc length of the differential robot, the x and y in the rectangular coordinate system are converted into a calculation formula for the arc length. Then the formula converted from the motion model of the differential robot is specifically:

[0078] s(t+Δt)=s(t)+v·Δt (1)

[0079] yaw(t+Δt)=yaw(t)+ω·Δt (2)

[0080] in:

[0081] v=(v r +v l ) / twenty three)

[0082] ω=(v r -v l ) / L (4)

[0083] Where s(t) represents the arc length of the differential robot at time t; s(t+Δt) represents the arc length of the differential robot at the next moment after time t; v r is the speed of the right wheel of the differential robot; v l is the speed of the left wheel of the differential robot, and L is the distance between the wheels of the differential robot. Formulas (1) and (3) are the arc length prediction relationships; formulas (2) and (4) are the heading angle prediction relationships.

[0084] Taking the time window corresponding to the current time t as 3 as an example, the specific process of determining the predicted trajectory information of t+1, t+2, and t+3 is as follows.

[0085] The process of determining the predicted trajectory information at t+1 is:

[0086] When the orientation angle at the current time t is known When the left and right wheel speeds at time t are Substitute into formula (4) to calculate ω0 at time t; where:

[0087]

[0088] The ω0 at time t and the heading angle Substitute into formula (2) and calculate the heading angle at the next moment t+1 The calculation formula is:

[0089] ; The left and right wheel speeds at time t are Substitute into formula (3) to calculate v0 at time t, where:

[0090]

[0091] Set the arc length at the current time t is 0, then v0 at time t and the arc length at the current time t Substituting into formula (1), we can calculate the arc length at time t+1: in, The calculation formula is:

[0092]

[0093] The process of determining the predicted trajectory information at t+2 is:

[0094] The left and right wheel speeds at time t+1 Substitute into formula (4) to calculate ω1 at time t+1, that is:

[0095]

[0096] The ω1 at time t+1 and the orientation angle at time t+1 Substitute into formula (2) and calculate the heading angle at time t+2 The calculation formula is:

[0097]

[0098] The left and right wheel speeds at time t+1 Substitute into formula (3) to calculate v1 at time t+1, that is:

[0099]

[0100] v1 at time t+1, arc length at time t+1 Substituting into formula (1), we can calculate the arc length at the next moment t+2: The calculation formula is:

[0101]

[0102] The process of determining the predicted trajectory information at t+3 is:

[0103] The left and right wheel speeds at time t+2 Substitute into formula (4) to calculate ω2 at time t+2, that is:

[0104]

[0105] The ω2 at time t+2 and the orientation angle at time t+2 Substitute into formula (2) and calculate the heading angle at time t+3 The calculation formula is:

[0106]

[0107] The left and right wheel speeds at time t+2 Substitute into formula (3) to calculate v2 at time t+2, that is:

[0108]

[0109] v2 at time t+2, arc length at time t+2 Substituting into formula (1), we can calculate the arc length at time t+3: The calculation formula is:

[0110]

[0111] In summary, according to the t+1 moment and Determine the predicted trajectory information at time t+1 as the heading angle and arc length represented by the left and right wheel speeds at time t; and At time t+2 and Determine the predicted trajectory information at t+2 as the heading angle and arc length represented by the left and right wheel speeds at time t and time t+1; and pred1 , yaw at time t+2 pred2 and pred2 , yaw at time t+3 pred3 and pred3 , determine the predicted trajectory information at time t+3 as the heading angle and arc length represented by the left and right wheel speeds at time t, time t+1, and time t+2.

[0112] Therefore, the embodiment of the present invention uses the arc length prediction relationship and the heading angle prediction relationship to represent the predicted trajectory information in the time window corresponding to the current moment using the left and right wheel speeds.

[0113] In the embodiment of the present invention, by simplifying the trajectory tracking method, the separate processing of the x-axis and the y-axis in the traditional method is avoided, and the error is measured by calculating the total displacement. This processing method significantly reduces the calculation complexity while maintaining the trajectory tracking accuracy, and is particularly suitable for application in an environment lacking high-precision position information.

[0114] In some embodiments, the difference between the predicted trajectory information in the time window corresponding to the current moment and the reference sub-information in the time window corresponding to the current moment is determined by combining Figure 4 As shown, including:

[0115] S400: for each moment in the time window corresponding to the current moment, determine a first functional relationship represented by left and right wheel speeds at the moment according to a displacement residual between a predicted arc length at the moment in the predicted trajectory information and a reference arc length at the moment in the reference sub-information;

[0116] S401: determining a second functional relationship represented by left and right wheel speeds at the moment according to a heading angle residual between a predicted heading angle at the moment in the predicted trajectory information and a reference heading angle at the moment in the reference sub-information;

[0117] S402: determining a third functional relationship represented by the left and right wheel speeds at the moment according to the speed residual between the left and right wheel predicted speeds at the moment in the predicted trajectory information and the reference left and right wheel speeds at the moment in the reference sub-information;

[0118] S403: Determine the difference between the information at the moment in the predicted trajectory information and the information at the moment in the reference sub-information, expressed in terms of left and right wheel speeds, according to the first functional relationship, the second functional relationship, and the third functional relationship.

[0119] The displacement residual between the predicted arc length at that moment in the predicted trajectory information and the reference arc length at that moment in the reference sub-information is determined by:

[0120] The predicted arc length at that moment in the predicted trajectory information is used as the predicted total displacement at that moment;

[0121] The arc length difference between the reference arc length at that moment in the reference sub-information and the reference arc length at the current moment is used as the reference total displacement at that moment;

[0122] The value obtained by subtracting the reference total displacement from the predicted total displacement is used as the displacement residual between the predicted arc length at that moment in the predicted trajectory information and the reference arc length at that moment in the reference sub-information.

[0123] In detail, when predicting the predicted trajectory information in the time window corresponding to the current moment based on the arc length prediction relationship and the heading angle prediction relationship, the predicted arc length at that moment in the predicted trajectory information is the predicted arc length between the current moment and that moment. The reference arc length at that moment in the reference sub-information is the reference arc length between that moment and the initial moment of the differential robot. In order to compare the reference value and the predicted value of the arc length between the current moment and that moment, it is necessary to determine the reference value of the arc length between the current moment and that moment, and the reference value of the arc length between the current moment and that moment is the arc length difference between the reference arc length at that moment in the reference sub-information and the reference arc length at the current moment.

[0124] Exemplarily, the initial starting time of the differential robot is t0, the current time is t, and this time is t+1, then the reference arc length at this time in the reference trajectory information is the reference arc length between t0 and t+1, the reference arc length at time t in the reference trajectory information is the reference arc length between t0 and t, and the predicted arc length at this time in the predicted trajectory information is the predicted arc length between t and t+1. In order to compare the reference value and predicted value of the arc length between t and t+1, the reference value of the arc length between t and t+1 is equal to the reference arc length between t0 and t+1 minus the reference arc length between t0 and t.

[0125] It can be understood as: all errors on the current trajectory are considered to have been perfectly corrected. At each time step, the total displacement error at the current moment is set to 0, which means that during the optimization process, it is assumed that the robot has accurately walked through all previous trajectory parts. This assumption allows us to focus only on the trajectory deviation at the current moment at each time step, rather than the accumulated trajectory error. In this way, the optimization problem only needs to consider the trajectory deviation at the current moment, and only requires the orientation angle information, without any position information.

[0126] Exemplarily, the time windows corresponding to the current moment are t+1, t+2, and t+3, and the moment is t+1, taking the determination of the gap between the reference information and the predicted information at the moment t+1 as an example.

[0127] Determine a first functional relationship represented by left and right wheel speeds at time t+1 according to a displacement residual between a predicted arc length at time t+1 in the predicted trajectory information and a reference arc length at time t+1 in the reference sub-information;

[0128] The first functional relationship expressed by the left and right wheel speeds at time t+1 is determined according to the following formula:

[0129]

[0130] Wherein, Δs(t+1) is the displacement residual between the predicted arc length at time t+1 in the predicted trajectory information and the reference arc length at time t+1 in the reference sub-information, is the reference arc length between time t0 and time t+1; is the reference arc length from time t0 to time t.

[0131] Will Substitute the calculation formula of into the formula of Δs(t+1), refer to the above content It can be expressed by the left wheel speed and the right wheel speed at time t, so the first functional relationship expressed by the left and right wheel speeds at time t+1 is the functional relationship expressed by the left wheel speed and the right wheel speed at time t.

[0132] The second functional relationship expressed by the left and right wheel speeds at time t+1 is determined according to the following formula:

[0133]

[0134] Wherein, Δyaw(t+1) is the residual between the predicted orientation angle at time t+1 in the predicted trajectory information and the reference orientation angle at time t+1 in the reference sub-information. is the predicted heading angle at time t+1; is the reference orientation angle between time t+1.

[0135] Will Substitute the calculation formula into the formula of Δyaw(t+1), refer to the above content It can be expressed by the left wheel speed and the right wheel speed at time t, so the second functional relationship expressed by the left and right wheel speeds at time t+1 is the functional relationship expressed by the left wheel speed and the right wheel speed at time t.

[0136] The third functional relationship of the left wheel speed at time t+1 is determined according to the following formula:

[0137]

[0138] Where Δv l (t+1) is the residual between the predicted speed of the left wheel at time t+1 in the predicted trajectory information and the reference speed of the left wheel at time t+1 in the reference sub-information, is the predicted speed of the left wheel at time t+1; is the left wheel reference speed between time t+1.

[0139] The third functional relationship of the right wheel speed at time t+1 is determined according to the following formula:

[0140]

[0141] Where Δv r (t+1) is the residual between the predicted speed of the right wheel at time t+1 in the predicted trajectory information and the reference speed of the right wheel at time t+1 in the reference sub-information, is the predicted speed of the right wheel at time t+1; is the reference speed of the right wheel between time t+1.

[0142] Therefore, the third functional relationship expressed by the left and right wheel speeds at time t+1 is a functional relationship expressed by the left wheel speed and the right wheel speed at time t+1.

[0143] The difference between the information at time t+1 in the predicted trajectory information and the information at time t+1 in the reference sub-information, expressed in terms of left and right wheel speeds, is determined by the following formula:

[0144]

[0145] Among them, G i is the gap at the i-th moment, w yaw is the weight of the residual towards the angle, w s is the weight of the arc length residual, w v is the weight of the residual speed of the left and right wheels; L is the distance between the wheels of the differential robot, yaw i is the predicted heading angle at the i-th moment; is the reference orientation angle at the i-th moment; si is the predicted total displacement at the i-th moment; is the reference total displacement at the i-th moment; v r,i is the predicted speed of the right wheel at the i-th moment; is the reference speed of the right wheel at the i-th moment; v l,i is the predicted speed of the left wheel at the i-th moment; is the left wheel reference speed at the i-th moment.

[0146] in, is a first functional relationship expressed by left and right wheel speeds; is a second functional relationship expressed by left and right wheel speeds; is the third function relationship of the right wheel speed; It is the third function relationship of the left wheel speed.

[0147] The left and right wheel speeds used as the minimum value of the sum of the differences at each moment in the time window corresponding to the current moment are used as the predicted left and right wheel speeds at each moment in the time window corresponding to the current moment.

[0148] The minimum value of the sum of the differences at each moment in the time window corresponding to the current moment is determined by the weighted least squares method. The formula is:

[0149]

[0150] Taking the time windows corresponding to the current time as time t+1, time t+2, and time t+3 as an example, calculate the difference at time t+1, the difference at time t+2, and the difference at time t+3;

[0151] In detail, the difference at time t+1 is:

[0152]

[0153] Among them, the reference orientation angle at time t+1 is Reference arc length at time t+1 Reference arc length at time t Reference speed v of left and right wheels at time t+1 r1 _ref, v l1 _ref are all known, predict the heading angle and predicted arc length It is expressed by the left and right wheel speeds at time t+1, so the difference G1 at time t+1 is expressed by the left and right wheel speeds at time t+1.

[0154] Similarly, the difference at time t+2 is:

[0155]

[0156] Among them, the reference orientation angle at time t+2 is Reference arc length at time t+2 Reference speed v of left and right wheels at time t+2 r2 _ref, v l2 _ref are all known, predict the heading angle and predicted arc length It is expressed by the left and right wheel speeds at time t+1 and time t+2; then the difference G2 at time t+2 is expressed by the left and right wheel speeds from time t+1 to time t+2.

[0157] Similarly, the difference at time t+3 is:

[0158]

[0159] Among them, the reference heading angle yaw at time t+3 is ref3 , the reference arc length s at time t+3 ref3 , left and right wheel reference speed v at time t+3 r3 _ref, v l3 _ref are all known, predict the heading angle and predicted arc length It is expressed by the left and right wheel speeds from time t+1 to time t+3; then the difference G3 at time t+3 is expressed by the left and right wheel speeds from time t+1 to time t+3.

[0160] When solving for the minimum value of the sum of the differences between time t+1, time t+2, and time t+3, the function actually being solved becomes expressed by the left and right wheel speeds from time t+1 to time t+3. This means solving for the left and right wheel speeds from time t+1 to time t+3 so that after their values ​​are substituted into the function, the function value is minimized.

[0161] The mechanical structure and application scenarios of differential robots are diverse. Traditional trajectory tracking algorithms usually need to be adjusted and optimized according to different application scenarios, resulting in poor versatility of the algorithm. The embodiments of the present invention simplify the trajectory tracking problem and reduce the dependence on specific location information, so that the algorithm has higher versatility and adaptability. Whether it is a warehousing robot used for logistics transportation or a mobile robot used for family companionship, it can be adapted to different application scenarios and needs by adjusting the weight parameters. This flexibility enables this solution to be widely used in various differential robot systems, providing an efficient and stable trajectory tracking solution.

[0162] Traditional differential robot trajectory tracking algorithms rely on high-precision position information, and usually require the use of expensive sensors such as GPS or lidar to obtain accurate positions on the x and y axes in real time. However, these sensors are expensive and may not work properly in certain environments. This solution introduces a simplified total displacement error calculation method, relying only on a low-cost IMU (inertial measurement unit) to provide real-time orientation angle information, rather than relying on high-precision position information sensors. This not only reduces hardware costs, but also improves the robustness and applicability of the system, allowing the differential robot to reliably perform trajectory tracking tasks in complex environments.

[0163] Traditional trajectory tracking algorithms may experience large error accumulation when processing low-precision or noisy sensor data, causing the robot to deviate from the predetermined trajectory. This is because these algorithms usually rely on accurate position information to track the path and have a low tolerance for errors. The embodiment of the present invention simplifies the trajectory optimization process for each time frame by assuming that the total displacement error on the current trajectory is 0 and that the part of the trajectory that has been traveled is accurate. The system is more robust in processing trajectory errors and can maintain high tracking accuracy even in the case of noisy sensor data. In addition, the weighted least squares method is used to solve the optimization problem, which makes the trajectory tracking smoother and more stable, improving the overall performance of the differential robot in practical applications.

[0164] In some embodiments, during the walking process of the differential robot, the position information may be interfered with, resulting in failure to receive the position information. Therefore, during the walking process of the differential robot, sometimes there is a position and sometimes there is no position. Based on this scenario, an embodiment of the present invention provides a differential robot walking method, combined with Figure 5 As shown, including:

[0165] S500: receiving information sent by the sensor;

[0166] S501: If the information sent by the sensor includes position and orientation angle, a PID control algorithm is used to perform trajectory tracking;

[0167] Among them, the PID control algorithm can be implemented by referring to the above introduction.

[0168] S502: If the information sent by the sensor includes the orientation angle, predict the predicted trajectory information within the time window corresponding to the current moment according to the orientation angle detected by the sensor at the current moment; extract the reference sub-information within the time window corresponding to the current moment from the reference trajectory information of the differential robot;

[0169] Among them, the current moment can also be determined according to the number. For example, the differential robot first tracks the trajectory according to the PID control algorithm, and then tracks according to the method provided by the embodiment of the present invention. Then, the moment passed by the PID control algorithm is determined. If there are 5 moments, then the sixth moment is the current moment.

[0170] S503: Determine the predicted speed of the left and right wheels at each moment in the time window corresponding to the current moment according to the difference between the predicted trajectory information and the reference sub-information;

[0171] S504: Control the left and right wheels of the differential robot to move according to the predicted speed of the left and right wheels at the next moment predicted at the current moment.

[0172] Steps 502 to 504 can refer to Figure 1 to Figure 4 content implementation.

[0173] Based on the same technical concept, see Figure 6 As shown, Figure 6 An exemplary trajectory tracking device 600 provided in an embodiment of the present application is shown. The device 600 can perform any step in the above trajectory tracking method, and in order to avoid repetition, it is not described here. The device 600 includes an acquisition unit 601, a prediction unit 602, a determination unit 603, and a control unit 604.

[0174] An acquisition unit 601 is used to acquire reference trajectory information of the differential robot from a current position to a destination;

[0175] The prediction unit 602 is used to predict the predicted trajectory information in the time window corresponding to the current moment according to the orientation angle at the current moment detected by the sensor for each moment when the differential robot walks from the current position to the destination; and extract the reference sub-information in the time window corresponding to the current moment from the reference trajectory information of the differential robot;

[0176] A determination unit 603, configured to determine the predicted speeds of the left and right wheels at each moment in a time window corresponding to the current moment according to the difference between the predicted trajectory information and the reference sub-information;

[0177] The control unit 604 is used to control the left and right wheels of the differential robot to move according to the predicted speed of the left and right wheels at the next moment predicted at the current moment.

[0178] Optionally, the prediction unit 602 is specifically configured to determine, based on the heading angle at the current moment detected by the sensor and the heading angle prediction relationship, a predicted heading angle represented by left and right wheel speeds in the predicted trajectory information within a time window corresponding to the current moment; wherein the heading angle prediction relationship is a functional relationship between two heading angles at two adjacent moments and left and right wheel speeds;

[0179] According to the arc length at the current moment and the arc length prediction relationship, the predicted arc length expressed by the left and right wheel speeds in the predicted trajectory information within the time window corresponding to the current moment is determined; wherein the arc length prediction relationship is a functional relationship between two arc lengths at two adjacent moments and the left and right wheel speeds.

[0180] Optionally, the determining unit 603 is further configured to determine, for each moment in the time window corresponding to the current moment, a first functional relationship represented by left and right wheel speeds at the moment according to a displacement residual between the predicted arc length at the moment in the predicted trajectory information and the reference arc length at the moment in the reference sub-information;

[0181] Determine a second functional relationship represented by left and right wheel speeds at the moment according to a heading angle residual between the predicted heading angle at the moment in the predicted trajectory information and the reference heading angle at the moment in the reference sub-information;

[0182] Determine a third functional relationship represented by the left and right wheel speeds at the moment according to a speed residual between the left and right wheel predicted speeds at the moment in the predicted trajectory information and the reference left and right wheel speeds at the moment in the reference sub-information;

[0183] According to the first functional relationship, the second functional relationship, and the third functional relationship, a difference expressed by left and right wheel speeds between the information at that moment in the predicted trajectory information and the information at that moment in the reference sub-information is determined.

[0184] Optionally, the determining unit 603 is specifically configured to:

[0185] The left and right wheel speeds used by the minimum value of the sum of the differences at each moment in the time window corresponding to the current moment are used as the left and right wheel predicted speeds at each moment in the time window corresponding to the current moment.

[0186] Optionally, the determining unit 603 is further configured to:

[0187] Taking the predicted arc length at that moment in the predicted trajectory information as the predicted total displacement at that moment;

[0188] The arc length difference between the reference arc length at that moment in the reference sub-information and the reference arc length at the current moment is used as the reference total displacement at that moment;

[0189] A value obtained by subtracting the reference total displacement from the predicted total displacement is used as a displacement residual between the predicted arc length at that moment in the predicted trajectory information and the reference arc length at that moment in the reference sub-information.

[0190] In addition, combined Figure 1-Figure 6The trajectory tracking method and system according to the described embodiments of the present invention may be implemented by an electronic device.

[0191] Electronic devices, including: processors;

[0192] a memory for storing instructions executable by the processor;

[0193] The processor is configured to execute the instructions to implement the malicious encrypted traffic classification method as described in any one of the above descriptions.

[0194] Based on the above introduction, an exemplary Figure 7 electronic equipment structure.

[0195] The electronic device may include a processor 710 and a memory 720 storing computer program instructions.

[0196] Specifically, the processor 710 may include a central processing unit (CPU), or an application specific integrated circuit (ASIC), or may be configured to implement one or more integrated circuits of the embodiments of the present invention.

[0197] The memory 720 may include a large capacity memory for data or instructions. For example, but not limitation, the memory 720 may include a hard disk drive (HDD), a floppy disk drive, a flash memory, an optical disk, a magneto-optical disk, a magnetic tape, or a universal serial bus (USB) drive or a combination of two or more of these. In appropriate cases, the memory 720 may include a removable or non-removable (or fixed) medium. In appropriate cases, the memory 720 may be inside or outside the data processing device. In a specific embodiment, the memory 720 is a non-volatile solid-state memory. In a specific embodiment, the memory 720 includes a read-only memory (ROM). In appropriate cases, the ROM may be a mask-programmed ROM, a programmable ROM (PROM), an erasable PROM (EPROM), an electrically erasable PROM (EEPROM), an electrically rewritable ROM (EAROM) or a flash memory or a combination of two or more of these.

[0198] The processor 710 implements any one of the methods for performing tasks in the above embodiments by reading and executing computer program instructions stored in the memory 720 .

[0199] In one example, the electronic device may further include a communication interface 730 and a bus 740. Figure 7As shown, the processor 710, the memory 720, and the communication interface 730 are connected via a bus 740 and communicate with each other.

[0200] The communication interface 730 is mainly used to implement communication between various modules, devices, units and / or equipment in the embodiment of the present invention.

[0201] Bus 740 includes hardware, software or both, and the parts of electronic equipment are coupled to each other. For example, but not limitation, bus may include accelerated graphics port (AGP) or other graphics bus, enhanced industrial standard architecture (EISA) bus, front side bus (FSB), hypertransport (HT) interconnection, industrial standard architecture (ISA) bus, infinite bandwidth interconnection, low pin count (LPC) bus, memory bus, micro channel architecture (MCA) bus, peripheral component interconnection (PCI) bus, PCI-Express (PCI-X) bus, serial advanced technology attachment (SATA) bus, video electronics standard association local (VLB) bus or other suitable bus or two or more of these combinations. In appropriate cases, bus 740 may include one or more buses. Although the embodiment of the present invention describes and shows a specific bus, the present invention considers any suitable bus or interconnection.

[0202] The electronic device can execute the malicious encrypted traffic classification method in the embodiment of the present invention based on the received task, thereby realizing the combination Figure 1-Figure 6 Described trajectory tracking method and system.

[0203] In addition, in combination with the electronic device in the above embodiments, an embodiment of the present invention may provide a storage medium, and when the instructions in the storage medium are executed by the processor of the electronic device, the electronic device can execute the trajectory tracking method as described in any one of the above items.

[0204] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks 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 produce 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 1 A device that provides the functions specified in a block or multiple blocks.

[0205] 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 including 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.

[0206] 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 The steps for the functions specified in one or more boxes.

[0207] Although the preferred embodiments of the present invention have been described, those skilled in the art may make other changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.

[0208] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

Claims

1. A trajectory tracking method, characterized in that: The method comprises: Obtain reference trajectory information of the differential robot from its current position to its destination; For each moment when the differential robot walks from the current position to the destination, predict the predicted trajectory information within the time window corresponding to the current moment according to the orientation angle at the current moment detected by the sensor; extract the reference sub-information within the time window corresponding to the current moment from the reference trajectory information of the differential robot; Determining the predicted speeds of the left and right wheels at each moment in the time window corresponding to the current moment according to the difference between the predicted trajectory information and the reference sub-information; The left and right wheels of the differential robot are controlled to move according to the predicted speed of the left and right wheels at the next moment predicted at the current moment.

2. The method according to claim 1, characterized in that The step of predicting the predicted trajectory information within the time window corresponding to the current moment according to the orientation angle at the current moment detected by the sensor includes: Determine the predicted heading angle represented by the left and right wheel speeds in the predicted trajectory information within the time window corresponding to the current moment according to the heading angle detected by the sensor at the current moment and the heading angle prediction relationship; wherein the heading angle prediction relationship is a functional relationship between the two heading angles at two adjacent moments and the left and right wheel speeds; According to the arc length at the current moment and the arc length prediction relationship, the predicted arc length expressed by the left and right wheel speeds in the predicted trajectory information within the time window corresponding to the current moment is determined; wherein the arc length prediction relationship is a functional relationship between two arc lengths at two adjacent moments and the left and right wheel speeds.

3. The method according to claim 2, characterized in that The step of determining the difference between the predicted trajectory information and the reference sub-information includes: For each moment in the time window corresponding to the current moment, determining a first functional relationship represented by left and right wheel speeds at the moment according to a displacement residual between the predicted arc length at the moment in the predicted trajectory information and the reference arc length at the moment in the reference sub-information; Determine a second functional relationship represented by left and right wheel speeds at the moment according to a heading angle residual between the predicted heading angle at the moment in the predicted trajectory information and the reference heading angle at the moment in the reference sub-information; Determine a third functional relationship represented by the left and right wheel speeds at the moment according to a speed residual between the left and right wheel predicted speeds at the moment in the predicted trajectory information and the reference left and right wheel speeds at the moment in the reference sub-information; According to the first functional relationship, the second functional relationship, and the third functional relationship, the difference between the information at that moment in the predicted trajectory information and the information at that moment in the reference sub-information, expressed by the left and right wheel speeds, is determined.

4. The method according to claim 3, characterized in that The step of determining the predicted speeds of the left and right wheels at each moment in the time window corresponding to the current moment according to the gap between the predicted trajectory information and the reference sub-information includes: The left and right wheel speeds used by the minimum value of the sum of the differences at each moment in the time window corresponding to the current moment are used as the left and right wheel predicted speeds at each moment in the time window corresponding to the current moment.

5. The method according to claim 3, characterized in that The displacement residual between the predicted arc length at the moment in the predicted trajectory information and the reference arc length at the moment in the reference sub-information is determined by: Taking the predicted arc length at that moment in the predicted trajectory information as the predicted total displacement at that moment; The arc length difference between the reference arc length at that moment in the reference sub-information and the reference arc length at the current moment is used as the reference total displacement at that moment; A value obtained by subtracting the reference total displacement from the predicted total displacement is used as a displacement residual between the predicted arc length at that moment in the predicted trajectory information and the reference arc length at that moment in the reference sub-information.

6. A trajectory tracking device, characterized in that: The device comprises: An acquisition unit, used to acquire reference trajectory information between the differential robot walking from the current position to the destination; A prediction unit, for predicting, for each moment when the differential robot walks from the current position to the destination, predicted trajectory information within a time window corresponding to the current moment according to the orientation angle at the current moment detected by the sensor; and extracting reference sub-information within the time window corresponding to the current moment from the reference trajectory information of the differential robot; a determination unit, configured to determine the predicted speeds of the left and right wheels at each moment in a time window corresponding to the current moment according to the difference between the predicted trajectory information and the reference sub-information; The control unit is used to control the left and right wheels of the differential robot to walk according to the predicted speed of the left and right wheels at the next moment predicted at the current moment.

7. The device according to claim 6, characterized in that The prediction unit is specifically configured to determine the predicted heading angle represented by the left and right wheel speeds in the predicted trajectory information within the time window corresponding to the current moment according to the heading angle detected by the sensor at the current moment and the heading angle prediction relationship; wherein the heading angle prediction relationship is a functional relationship between the two heading angles at two adjacent moments and the left and right wheel speeds; According to the arc length at the current moment and the arc length prediction relationship, the predicted arc length expressed by the left and right wheel speeds in the predicted trajectory information within the time window corresponding to the current moment is determined; wherein the arc length prediction relationship is a functional relationship between two arc lengths at two adjacent moments and the left and right wheel speeds.

8. The device according to claim 7, characterized in that The determining unit is further configured to determine, for each moment in the time window corresponding to the current moment, a first functional relationship represented by left and right wheel speeds at the moment according to a displacement residual between the predicted arc length at the moment in the predicted trajectory information and the reference arc length at the moment in the reference sub-information; Determine a second functional relationship represented by left and right wheel speeds at the moment according to a heading angle residual between the predicted heading angle at the moment in the predicted trajectory information and the reference heading angle at the moment in the reference sub-information; Determine a third functional relationship represented by the left and right wheel speeds at the moment according to a speed residual between the left and right wheel predicted speeds at the moment in the predicted trajectory information and the reference left and right wheel speeds at the moment in the reference sub-information; According to the first functional relationship, the second functional relationship, and the third functional relationship, the difference between the information at that moment in the predicted trajectory information and the information at that moment in the reference sub-information, expressed by the left and right wheel speeds, is determined.

9. An electronic device, characterized in that: include: Memory, used to store computer programs or instructions; A processor is used to execute the computer program or instructions in the memory so that the trajectory tracking method as described in any one of claims 1 to 5 is executed.

10. A computer-readable storage medium, characterized in that: When the instructions in the storage medium are executed by a processor, the processor is enabled to execute the trajectory tracking method as described in any one of claims 1 to 5.

Images