Motion control method and system of self-moving equipment, self-moving equipment and medium

Abstract

The invention relates to a self-moving device and a motion control method and system thereof, in particular to a motion control method and system suitable for a self-moving device of a double-wheel independent driving structure, the self-moving device and a medium, and aims to solve the problem that when the self-moving device of the double-wheel independent driving structure runs in a complex environment, the motion of the self-moving device cannot be controlled. The technical problem that a finer control strategy is needed in the face of core challenges such as violent load change, strong motor protection requirement and high motion stability requirement is solved. According to the motion control method of the self-moving equipment, the current rotating speeds and current currents of motors on the two sides are monitored in real time, the rotating speeds of the motors on the two sides are dynamically adjusted, the maximum output power is limited, constant acceleration smooth processing is adopted, the expected rotating speed in unit time is calculated, and it is ensured that the speed change is stable; and dynamically coordinating the rotating speeds of the motors on the two sides by utilizing a speed ratio adjusting strategy to keep a motion posture.

Description

Technical Field

[0001] The present invention relates to a self-moving device and a motion control method and system thereof, and in particular to a motion control method and system for a self-moving device suitable for a dual-wheel independent drive structure, a self-moving device and a medium. Background Art

[0002] With the popularization of artificial intelligence and robotics, autonomous devices with automatic navigation and autonomous obstacle avoidance functions are widely used in home gardening, security patrols, industrial logistics and other scenarios. Among them, the dual-wheel independent drive structure has become the mainstream configuration due to its simple structure and low manufacturing cost. However, when operating in complex environments, such devices face core challenges such as drastic load changes, strong motor protection requirements, and high motion stability requirements, and more refined control strategies are urgently needed.

[0003] First, most current two-wheel independent drive control systems lack real-time restrictions and protection mechanisms for the motor's operating status. When a motor on one side continues to output excessive power due to a sudden surge in load, the power limit adjustment is not triggered in time, causing the motor to heat up severely and shorten its life. Secondly, when one motor reaches its power limit, the existing two-wheel independent drive control system is often unable to coordinate the other motor to compensate for the posture, which can cause abnormal equipment movement posture, such as offset, instability, and other problems. Finally, the acceleration and deceleration process generally uses static acceleration curves or linear control, ignoring the influencing factors such as the overall machine mass, wheel surface friction, and slope changes, which are prone to sudden jitter or slippage, reducing the operating stability and safety of the self-propelled equipment. Summary of the invention

[0004] The purpose of the present invention is to solve the core challenges faced by a self-moving device with a dual-wheel independent drive structure when running in a complex environment, such as drastic load changes, strong motor protection requirements, and high motion stability requirements, and the technical problem that a more refined control strategy is urgently needed, and to provide a motion control method, system, self-moving device and medium for a self-moving device.

[0005] In order to solve the deficiencies of the above-mentioned prior art, the present invention provides the following technical solutions:

[0006] A motion control method for a self-moving device, wherein the self-moving device adopts a dual-wheel independent drive structure, and the method is special in that it comprises the following steps:

[0007] Convert the expected linear speed and angular speed of the whole machine into the expected rotation speeds on both sides;

[0008] For each motor on each side, smooth the desired rotational speed through a constant acceleration to obtain the desired rotational speed per unit time, then calculate the desired current based on the error between the desired rotational speed per unit time and the current rotational speed, and then calculate the desired duty cycle based on the error between the desired current and the current current; the unit time is the cycle duration for updating the motor rotational speed.

[0009] When the current duty cycles and current currents of the motors on both sides are greater than or equal to their corresponding rated values, dynamically adjust the desired rotational speed per unit time of the motor on the other side with one side of the motor as a reference to achieve synchronization of the motors on both sides.

[0010] Finally, output the desired rotational speed per unit time and the desired duty cycle of the motors on both sides, which are respectively used to control the corresponding motors to complete the motion control of this control cycle.

[0011] Optionally, in step 2, before starting to calculate the desired rotational speed per unit time by smoothing the desired rotational speed through a constant acceleration, it further includes:

[0012] For each motor on each side, if there is a previous control cycle, update the desired rotational speed to the desired rotational speed per unit time output in the previous control cycle.

[0013] Optionally, smoothing the desired rotational speed through a constant acceleration to obtain the desired rotational speed per unit time includes:

[0014] For each motor on each side, compare the rotational speed difference between the current rotational speed and the desired rotational speed per unit time with the corresponding threshold value to determine the stage of entering the T-shaped acceleration and deceleration curve, and use jerk to update the acceleration; then use the acceleration to calculate the rotational speed at the next moment based on the current rotational speed as the desired rotational speed per unit time.

[0015] The jerk is the change rate of the acceleration, and the jerk adopts a fixed value.

[0016] Or the jerk is dynamically adjusted according to the current current of the motor on that side, including:

[0017] j inc = max(j min , j base_inc (1 - β·I curr / I max ))

[0018] j dec = max(j min , j base_dec (1 - β·I curr / I max ))

[0019] Among them, j inc , j dec are the jerks in the acceleration stage and the deceleration stage respectively; jmin is the minimum jerk, not less than the resolution of jerk; j the_inc and j the_dec are the theoretical jerks during the acceleration phase and deceleration phase respectively; I curr is the current current of the motor on this side, I max is the maximum current; β is the suppression proportionality factor.

[0020] Optionally, calculating the rotational speed at the next moment based on the current rotational speed using the acceleration as the expected rotational speed per unit time includes:

[0021] For each motor on each side, using the current rotational speed, acceleration, jerk, and the cycle duration of speed update, weighted sum the theoretical rotational speed at the next moment and the current rotational speed to obtain the rotational speed at the next moment as the expected rotational speed per unit time.

[0022] Optionally, calculating the expected current based on the error between the expected rotational speed per unit time and the current rotational speed includes:

[0023] For each motor on each side, collect the current rotational speed;

[0024] Introduce an integral limit value in the speed loop PID (Proportional-Integral-Derivative Controller) to limit the integral term, and dynamically adjust the proportional gain and derivative gain according to the magnitude and change rate of the error between the expected rotational speed per unit time and the current rotational speed, and further adjust the derivative gain according to the acceleration direction, and then substitute the error between the expected rotational speed per unit time and the current rotational speed into the speed loop PID to output the expected current;

[0025] Use a preset expected power to limit the expected current, and update the expected current to the limited expected current.

[0026] Optionally, calculating the expected duty cycle based on the error between the expected current and the current current includes:

[0027] For each motor on each side, collect the current current;

[0028] Introduce an integral limit value in the current loop PID to limit the integral term, and dynamically adjust the proportional gain, integral gain, and derivative gain according to the magnitude and change rate of the error between the expected current and the current current, and further adjust the derivative gain according to the current mutation direction, and then substitute the error between the expected current and the current current into the current loop PID to obtain the output of the current loop PID;

[0029] After using the maximum duty cycle to limit the output of the current loop PID, output the expected duty cycle.

[0030] Optionally, when the current duty cycle and current of the motors on both sides are greater than or equal to the corresponding rated values, dynamically adjust the expected rotational speed per unit time of the motor on the other side based on the motor on one side to achieve synchronization of the motors on both sides, including:

[0031] Step A1: For the motors on both sides, determine whether the current duty cycles are both ≥ the rated values;

[0032] If so, execute Step A2; otherwise, determine whether the current duty cycles are both < the rated values;

[0033] If so, execute Step A5; otherwise, determine whether the current duty cycle of the left motor is ≥ the rated value,

[0034] If so, execute Step A3; otherwise, execute Step A4;

[0035] Step A2: Determine whether r L <r R , r L =n curr_L / ExN L , r R =n curr_R / ExN R , where n curr_L , ExN L are the current rotational speed and the expected rotational speed per unit time of the left motor respectively, and n curr_R , ExN R are the current rotational speed and the expected rotational speed per unit time of the right motor respectively;

[0036] If so, execute Step A3; otherwise, execute Step A4;

[0037] Step A3: Let ExN R =ExN R ·r L , and then execute the output of the expected rotational speed per unit time and the expected duty cycle of the motors on both sides, which are used to control the corresponding motors respectively, and complete the motion control of this control cycle;

[0038] Step A4: Let ExN L =ExN L ·r R , and then execute the output of the expected rotational speed per unit time and the expected duty cycle of the motors on both sides, which are used to control the corresponding motors respectively, and complete the motion control of this control cycle;

[0039] Step A5: For the motors on both sides, determine whether the current currents are both ≥ the rated values;

[0040] If so, execute Step A2; otherwise, determine whether the current currents are both < the rated values;

[0041] If so, execute the expected rotational speed and expected duty cycle of the motors on both sides per unit time, which are respectively used to control the corresponding motors to complete the motion control of the current control cycle; otherwise, determine whether the current current of the left motor is ≥ the rated value;

[0042] If so, execute step A3; otherwise, execute step A4.

[0043] Meanwhile, the invention also provides a motion control system for a self-mobile device, including two control groups, and each control group includes a motor and a motor controller. The special feature is that:

[0044] The motor controller of each said control group controls the motor to implement the steps of the motion control method of the self-mobile device as described above.

[0045] The invention also provides a self-mobile device, and the special feature is that: it includes a whole-machine controller and the motion control system of the self-mobile device as described above, and the whole-machine controller is communicatively connected to the motor controllers of the two control groups in the motion control system of the self-mobile device.

[0046] The invention also provides a computer-readable storage medium, and the special feature of the computer-readable storage medium storing a computer program is that:

[0047] When the computer program is executed by a processor, it implements the steps of the motion control method of the self-mobile device as described above.

[0048] Compared with the prior art, the beneficial effects of the present invention are:

[0049] (1) For the motion control method of a self-mobile device of the present invention, by real-time monitoring the current rotational speeds and current currents of the motors on both sides, dynamically adjusting the rotational speeds of the motors on both sides, limiting the maximum output power, and using constant acceleration smoothing processing to calculate the expected rotational speed per unit time to ensure smooth speed change. In addition, when the power of a single motor is limited, the speed ratio adjustment strategy is used to dynamically coordinate the rotational speeds of the motors on both sides to maintain the motion posture; the present invention achieves three control objectives of protecting the motor, maintaining the operation posture of the whole machine when a single motor reaches the rated power, and smooth acceleration and deceleration of the whole machine.

[0050] (2) In the present invention, by making the expected rotational speed updated to the expected rotational speed per unit time output in the previous control cycle, the continuity and stability of the control instruction are ensured, effectively preventing speed jumps caused by control cycle switching, thereby further improving the smoothness and tracking performance of the self-mobile device.

[0051] (3) By introducing dynamic Jerk adjustment into the T-shaped acceleration and deceleration curve, the present invention enables the acceleration to change gradually, thereby greatly reducing the mutation phenomenon in motion control and making the acceleration and deceleration processes smoother and more natural. Moreover, Jerk is dynamically adjusted according to the current current of the motor on that side, enabling the system to adaptively adjust the acceleration and deceleration intensity according to the actual load condition of the motor, improving the flexibility and safety of the control system, and protecting the motor from overload damage.

[0052] (4) The present invention introduces an integral limit value into the speed-loop PID to prevent the integral term from overshooting after long-term error accumulation, resulting in overshoot or oscillation in the control system. A proportional gain adjustment factor and an integral gain adjustment factor are introduced. When the error is large, the proportional gain is dynamically increased to shorten the adjustment time. When the speed changes violently, the derivative gain is reduced to prevent instability caused by noise amplification. In addition, the control jitter and mechanical shock during sudden speed change or acceleration and deceleration switching are alleviated by an anti-shock coefficient, and a preset power limit is used to expect the current to prevent the motor power from exceeding the limit, thereby achieving efficient and stable motion control in a complex motion environment.

[0053] (5) The present invention introduces a proportional gain adjustment factor and an integral gain adjustment factor into the current-loop PID to accelerate error convergence and improve the response speed. A derivative gain adjustment factor and an anti-shock coefficient are introduced to suppress noise and current shock and maintain stable control. An integral limit value is used to prevent excessive accumulation of the integral term and reduce the integral saturation phenomenon. After calculating the expected current and the expected duty ratio, power limiting is performed to avoid the motor power exceeding the rated value, thereby achieving a good balance in dynamic response, stability, noise suppression, and motor protection and being applicable to various complex motion control tasks.

[0054] (6) When the current duty ratio and the current of both motors reach or exceed the rated value, the present invention dynamically adjusts the other motor based on the motor on the side with a smaller ratio of the expected rotational speed to the current rotational speed per unit time as a reference to protect the motor and maintain the motion posture of the self-mobile device. Even if the power of one side motor is limited, the self-mobile device can still maintain normal straight-line driving or turning motion. In addition, the present invention adopts a multi-layer control logic, first judging the current duty ratio and then judging the current current to achieve fast response and early protection, avoiding control lag caused by current detection delay and taking into account both real-time performance and protection. Description of the Drawings

[0055] Figure 1 is a flowchart of step 2 in an embodiment of a motion control method for a self-mobile device of the present invention;

[0056] Figure 2 is a flowchart of step 2.3.2 in an embodiment of the present invention;

[0057] Figure 3This is the flowchart of step 3 in an embodiment of the present invention. Detailed implementation manners

[0058] The present invention will be further described below in conjunction with the accompanying drawings and exemplary embodiments.

[0059] A motion control method for a self - moving device. The self - moving device adopts a two - wheel independent drive structure and includes the following steps:

[0060] Step 1: Convert the expected linear velocity and expected angular velocity of the whole machine into expected rotational speeds on both sides;

[0061] It should be noted that the above - mentioned self - moving device conforms to the "robot" defined in GB / T 39405 - 2020 and can be a personal / home service robot according to the application field; the two - wheel independent drive structure means that the two drive wheels on both sides are driven and controlled by independent motors respectively; the above - mentioned expected linear velocity and expected angular velocity of the whole machine refer to the expected moving speed of the self - moving device in the linear direction and the expected rotational speed of the self - moving device respectively.

[0062] Step 2: For each motor, smooth the above - mentioned expected rotational speed through a constant acceleration to calculate the expected rotational speed per unit time, then calculate the expected current according to the error between the expected rotational speed per unit time and the current rotational speed, and then calculate the expected duty cycle according to the error between the expected current and the current current;

[0063] It should be noted that the above - mentioned smoothing the above - mentioned expected rotational speed through a constant acceleration means that through a preset acceleration limit value, the expected rotational speed is discretized in time to generate a speed trajectory that conforms to the constant acceleration constraint. In this embodiment, a T - type acceleration - deceleration curve is used to smooth the rotational speed change, and an S - type curve can also be used in other embodiments:

[0064] The above - mentioned unit time is the cycle duration of the motor rotational speed update; the current rotational speed can be measured in real time through a sensor (such as an encoder); the expected current is the current required to reach the expected rotational speed per unit time;

[0065] Calculating the expected current based on the error between the expected rotational speed per unit time and the current rotational speed means adjusting the error through a PID or other control algorithm, and then adjusting the current rotational speed to reach the expected rotational speed per unit time; calculating the expected duty cycle based on the error between the expected current and the current current means adjusting the error through a PID or other control algorithm, and then adjusting the current current to reach the expected current; in order to control each motor in the dual-wheel independent drive structure, this embodiment adopts a dual-loop control based on PID, that is, first using the speed-loop PID to calculate the expected current according to the error between the expected rotational speed per unit time and the current rotational speed, and then using the current-loop PID to calculate the expected duty cycle according to the error between the expected current and the current current; in other embodiments, sliding mode variable structure control or fuzzy PID control can also be adopted;

[0066] Step 3: When the current duty cycle and the current current of the motors on both sides are greater than or equal to the corresponding rated values, taking one of the motors on one side as a reference, dynamically adjust the expected rotational speed per unit time of the motor on the other side to achieve synchronization of the motors on both sides;

[0067] The above-mentioned taking one of the motors on one side as a reference and dynamically adjusting the expected rotational speed per unit time of the motor on the other side to achieve synchronization of the motors on both sides means adjusting the expected rotational speed per unit time of the motor on the other side based on the rotational speed ratio of one of the motors on one side, and the rotational speed ratio is the ratio of the current rotational speed to the expected rotational speed per unit time; specifically, it can be adjusted in stages according to the difference in the rotational speed ratios on both sides, with a fixed increment adjusted each time, or updating the expected rotational speed per unit time of the motor on the other side to be the expected rotational speed per unit time of the motor on the other side multiplied by the rotational speed ratio of the motor on one of the sides;

[0068] Step 4: Output the expected rotational speed per unit time and the expected duty cycle of the motors on both sides, which are respectively used to control the corresponding motors to complete the motion control of this control cycle.

[0069] In some embodiments, the above-mentioned step 1 includes: obtaining the expected linear velocity and the expected angular velocity of the whole machine, and converting them into the expected rotational speeds on both sides according to the wheelbase and the wheel radius;

[0070] The expected rotational speeds on both sides can both be expressed as:

[0071]

[0072] where n exp is the expected rotational speed on one side, v exp is the expected linear velocity of the whole machine, ω exp is the expected angular velocity of the whole machine, L is the wheelbase, and r is the wheel radius.

[0073] In some embodiments, referring to Figure 1 , the above-mentioned step 2 includes:

[0074] Step 2.1: For each motor, determine whether there is a previous control cycle. If so, update the desired speed to the desired speed per unit time output in the previous control cycle, and then execute Step 2.2; otherwise, directly execute Step 2.2;

[0075] Step 2.2: For each motor, smooth the desired speed through a T-shaped acceleration-deceleration curve to obtain the desired speed per unit time;

[0076] Step 2.3: For each motor, collect the current speed, and use the speed loop PID to calculate the desired current according to the error between the desired speed per unit time and the current speed;

[0077] Step 2.4: For each motor, collect the current current, and use the current loop PID to calculate the desired duty cycle according to the error between the desired current and the current current.

[0078] In some embodiments, the above Step 2.2 includes:

[0079] Step 2.2.1: For each motor, compare the speed difference between the current speed and the desired speed per unit time with the corresponding threshold value to determine the stage of entering the T-shaped acceleration-deceleration curve, and then update the acceleration using jerk (the change rate of acceleration, i.e., the jerk). The above jerk is a fixed value or dynamically adjusted according to the current current of this side of the motor to protect the motor in real time and achieve smooth speed changes; specifically:

[0080] If the absolute value of the speed difference does not exceed the corresponding threshold value, enter the constant speed state and set the acceleration to 0;

[0081] If the speed difference is greater than the corresponding threshold value, enter the acceleration state, and update the acceleration to the current acceleration plus the jerk of this control cycle, and compare it with the maximum acceleration, and take the smaller value as the acceleration;

[0082] If the speed difference is less than the negative of the corresponding threshold value, enter the deceleration state, and update the acceleration to the current acceleration minus the jerk of this control cycle, and compare it with the negative of the maximum acceleration, and take the larger value as the acceleration;

[0083] It can be expressed as:

[0084] If |Δn| ≤ ε, enter the constant speed state and set a = 0;

[0085] If Δn > ε, enter the acceleration state and set a = min(a curr + j·Δt, a max );

[0086] If Δn < ―ε, enter the deceleration state and set a = amx(a curr ―j·Δt, ―amax );

[0087] where Δn is the rotational speed difference, and ε L is the threshold value of the rotational speed difference, which can be dynamically set according to the system control precision (such as encoder resolution); a and a curr are the acceleration and the current acceleration respectively, and a max is the maximum linear acceleration; j is the jerk of this control period, and Δt is the period duration of speed update;

[0088] Step 2.2.2: For each motor, use the current rotational speed, acceleration, jerk, and the period duration of speed update to calculate the theoretical rotational speed at the next moment as Weighted sum the theoretical rotational speed at the next moment and the current rotational speed to obtain the rotational speed at the next moment, as follows:

[0089]

[0090] where ξ is the smoothing factor, with a value range of 0 to 1. When the speed error is large, ξ tends to 1, indicating that the system responds quickly. When the speed error is small, ξ tends to 0, indicating that the system adjusts smoothly to reduce drastic changes; γ is the adjustment coefficient, which is used to control the response sensitivity to the change of speed error;

[0091] Take the rotational speed at the next moment as the expected rotational speed per unit time.

[0092] This embodiment integrates the theoretical rotational speed at the next moment and the current rotational speed, and realizes smooth transition through the weight coefficient, avoiding the overshoot problem that may be caused by the traditional error integration.

[0093] In other embodiments, Step 2.2.2 can also adopt:

[0094] Use the acceleration, the current rotational speed, and the period duration of speed update to calculate the rotational speed at the next moment, as follows:

[0095] n next = n curr + a·Δt

[0096] where n next , n curr are the rotational speed at the next moment and the current rotational speed respectively;

[0097] Take the rotational speed at the next moment as the expected rotational speed per unit time.

[0098] In some embodiments, in Step 2.2.1, the above jerk is dynamically adjusted according to the current current of this motor, including:

[0099] j inc = max(j min , j base_inc(1 - β·I curr / I max ))

[0100] j dec = max(j min , j base_dec (1 - β·I curr / I max ))

[0101] where j inc , j dec are the jerks in the acceleration phase and deceleration phase respectively; j min is the minimum jerk, not less than the jerk resolution, to prevent adjustment dead zone or ineffective acceleration; j the_inc , j the_dec are the theoretical jerks in the acceleration phase and deceleration phase respectively, which can be given according to the error or planning; I curr is the current current, I max is the maximum current, which is actually measured according to different motors or drive limits; β is the suppression proportionality factor, with a value range of 0 to 1, and the larger the value, the more sensitive the response.

[0102] The present invention directly correlates the current parameter with the jerk adjustment, realizes proportional suppression through the suppression proportionality factor, avoids overshoot or response delay caused by traditional fixed thresholds, and sets the minimum jerk to prevent jerk oscillation caused by current fluctuation, thereby enhancing the system stability.

[0103] In some embodiments, step 2.3 includes:

[0104] Step 2.3.1, collect the current speed for each side of the motor;

[0105] Step 2.3.2, for each side of the motor, calculate the desired current using a speed loop PID based on the error between the desired speed per unit time and the current speed;

[0106] Specifically, referring to Figure 2 , introduce an integral limit value to limit the integral term in the speed loop PID, and dynamically adjust the proportional gain and derivative gain according to the magnitude and change rate of the error between the desired speed per unit time and the current speed, and further adjust the derivative gain according to the acceleration direction, and finally substitute the error between the desired speed per unit time and the current speed into the speed loop PID to output the desired current;

[0107] Specifically, the above-mentioned desired current can be expressed as:

[0108]

[0109] where I out is the desired current, K n_p, K n_i , K n_d are respectively the proportional gain, integral gain, and derivative gain of the speed loop PID. e n is the difference between the desired rotational speed and the current rotational speed per unit time; γ n is the proportional gain adjustment factor of the speed loop PID, which is used to increase the proportional gain and improve the response speed when the error is large; η n is the derivative gain adjustment factor of the speed loop PID, which is used to reduce the derivative gain and suppress noise interference when the rate of change of the error is large; δ n is the impact resistance coefficient of the speed loop PID, with a value range of 0.2 to 0.5; sign is the sign of the change trend, where positive and negative respectively represent acceleration or deceleration; E n is the integral term of the speed loop PID, and E n_max is the integral limit value of the speed loop PID;

[0110] Step 2.3.3: For each motor on each side, use the preset desired power to limit the desired current, and update the desired current to the limited desired current; it should be noted that the above-mentioned preset desired power refers to a power upper limit value preset in the motion control of the self-mobile device to protect the motor and the drive circuit;

[0111] Specifically, for each motor on each side, calculate the limited current using the preset desired power, the actual voltage, and the redundancy protection coefficient as follows:

[0112]

[0113] where, I limit is the limited current, α is the redundancy protection coefficient, η ∈ [0.85, 0.95], P exp is the preset desired power, and U actual is the actual voltage;

[0114] Use the limited current to limit the desired current, and update the desired current to the limited desired current.

[0115] In some embodiments, the above-mentioned step 2.4 includes:

[0116] Step 2.4.1: For each motor on each side, collect the current current;

[0117] Step 2.4.2: For each motor on each side, introduce the integral limit value in the current loop PID to limit the integral term, and dynamically adjust the proportional gain, integral gain, and derivative gain according to the magnitude and rate of change of the error between the desired current and the current current. Then, further adjust the derivative gain according to the current mutation direction, and then substitute the error between the desired current and the current current into the current loop PID to obtain the output of the current loop PID;

[0118] After using the maximum duty cycle to limit the output of the current loop PID, the desired duty cycle is output;

[0119] The above-mentioned desired duty cycle can be expressed as:

[0120]

[0121]

[0122] where D out is the desired duty cycle; clamp is a limiting function in the format of clamp(value, min, max), and if value exceeds [min, max], the boundary value is returned; D′ out is the output of the current loop PID, D max is the maximum duty cycle, K I_p 、K I_i 、K I_d are the proportional gain, integral gain, and derivative gain of the current loop PID respectively, e I is the difference between the desired current and the current; γ I 、β I are the proportional gain adjustment factor and integral gain adjustment factor of the current loop PID respectively, which are used to increase the proportional gain and integral gain when the error is large; η I is the derivative gain adjustment factor of the current loop PID, which is used to decrease the derivative gain when the rate of change of the error is large; δ I is the impact resistance coefficient of the current loop PID, with a value between 0.2 and 0.5; sign is the change trend symbol, which is used to determine the direction of current mutation; E I is the integral term of the current loop PID, E I_max is the integral limit value of the current loop PID; Δt I is the period duration of current update.

[0123] In some embodiments, referring to Figure 3 , the above step 3 includes:

[0124] Step 3.1: For the motors on both sides, determine whether the current duty cycle is ≥ the rated value;

[0125] If so, execute step 3.2; otherwise, determine whether the current duty cycle is < the rated value;

[0126] If so, execute step 3.5; otherwise, determine whether the current duty cycle of the left motor is ≥ the rated value,

[0127] If so, execute step 3.3; otherwise, execute step 3.4;

[0128] Step 3.2: Determine whether r L <rR , r L = n curr_L / ExN L , r R = n curr_R / ExN R , where n curr_L , ExN L are respectively the current rotational speed of the left motor and the expected rotational speed per unit time, and n curr_R , ExN R are respectively the current rotational speed of the right motor and the expected rotational speed per unit time;

[0129] If so, execute Step 3.3; otherwise, execute Step 3.4;

[0130] Step 3.3: Let ExN R = ExN R · r L , and then execute Step 4;

[0131] Step 3.4: Let ExN L = ExN L · r R , and then execute Step 4;

[0132] Step 3.5: For both motors, determine whether the current is ≥ the rated value;

[0133] If so, execute Step 3.2; otherwise, determine whether the current is < the rated value;

[0134] If so, execute Step 4; otherwise, determine whether the current of the left motor is ≥ the rated value;

[0135] If so, execute Step 3.3; otherwise, execute Step 3.4.

[0136] Step 3 finds the side with "less deviation" by comparing the speed ratios of both motors and uses it to correct the other side to ensure motion symmetry and balance;

[0137] And a multi-layer control logic is adopted. First, the current duty cycle is judged, and then the current is judged for fast response and early protection to avoid control lag caused by current detection delay, taking into account both real-time performance and protection.

[0138] It should be noted that the rated value refers to the maximum allowable value of the equipment under normal and safe operating conditions.

[0139] A motion control system for a self-mobile device provided by the present invention includes two control groups, and each control group includes a motor controller, a motor drive circuit, a motor, a current sampling unit, and a rotational speed sampling unit;

[0140] Among them, the motor controller uses an MCU (Microcontroller Unit), and the MCU is connected to the motor drive circuit, and the motor drive circuit is connected to the motor; the speed sampling unit uses an encoder, which is used to detect the position, speed and direction of the motor rotor in real time through mechanical coupling, and the output end is connected to the interface of the motor controller; the current sampling unit uses a current sampling circuit, and its sampling method can be a shunt resistance method, a Hall sensor method, a current transformer method, or a combination of a Hall sensor and an amplifier, etc.

[0141] It should be noted that the motor controller can also use a DSP (Digital Signal Processor) or an FPGA (Field-Programmable Gate Array), etc.; the motor drive circuit uses an H-bridge circuit or a full-bridge drive chip, etc.

[0142] A self-mobile device provided by the present invention includes the motion control system and the whole machine controller of the above self-mobile device. The motor controllers of the above two control groups interact with the whole machine controller through the CAN bus to obtain the desired linear speed and the desired angular speed of the whole machine.

[0143] A computer-readable storage medium provided by the present invention, the computer-readable storage medium stores a computer program. The readable storage medium provided in this embodiment includes a non-volatile readable storage medium and a volatile readable storage medium; computer-readable instructions are stored on the readable storage medium, and when the computer-readable instructions are executed by one or more processors, one or more processors are caused to implement the steps of the motion control method of the self-mobile device in the above embodiment.

[0144] Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing relevant hardware through computer-readable instructions. The above computer-readable instructions can be stored in a non-volatile readable storage medium or a volatile readable storage medium. When the computer-readable instructions are executed, they can include the processes of the embodiments of the above various methods. Among them, any reference to a memory, storage, database, or other medium used in the various embodiments provided in the present application can include non-volatile and / or volatile memories. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or an external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), Rambus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and Rambus dynamic RAM (RDRAM), etc.

[0145] Those skilled in the art can clearly understand that in practical applications, the above functions can be allocated to different functional units or modules as needed, that is, the internal structure of the above self-mobile device can be divided into different functional units or modules to complete all or part of the functions described above.

[0146] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the various embodiments of the present invention, and should all be included in the protection scope of the present invention.

Claims

1. A motion control method for a self - moving device, the self - moving device adopting a two - wheel independent drive structure, characterized in that, The steps are as follows: Convert the expected linear velocity and expected angular velocity of the whole machine into the expected rotational speeds on both sides; For each motor on each side, smooth the expected rotational speed through a constant acceleration to obtain the expected rotational speed per unit time, then calculate the expected current based on the error between the expected rotational speed per unit time and the current rotational speed, and then calculate the expected duty cycle based on the error between the expected current and the current current; The per unit time is the cycle duration for updating the motor rotational speed; When the current duty cycles and current currents of the motors on both sides are greater than or equal to the corresponding rated values, dynamically adjust the expected rotational speed per unit time of the motor on the other side with one side of the motor as a reference to achieve synchronization of the motors on both sides; Finally, output the expected rotational speed per unit time and the expected duty cycle of the motors on both sides, which are respectively used to control the corresponding motors to complete the motion control of this control cycle.

2. The motion control method of a self - moving device according to claim 1, characterized in that, In step 2, before calculating the expected rotational speed per unit time by smoothing the expected rotational speed through a constant acceleration, it further includes: For each motor on each side, if there is a previous control cycle, update the expected rotational speed to the expected rotational speed per unit time output in the previous control cycle.

3. The motion control method of a self-moving device according to claim 1 or 2, characterized in that, Smoothing the expected rotational speed through a constant acceleration to obtain the expected rotational speed per unit time includes: For each motor on each side, compare the rotational speed difference between the current rotational speed and the expected rotational speed per unit time with the corresponding threshold value to determine the stage of entering the T-shaped acceleration and deceleration curve, and use jerk to update the acceleration; then use the acceleration to calculate the rotational speed at the next moment based on the current rotational speed as the expected rotational speed per unit time; The jerk is the change rate of the acceleration, and the jerk adopts a fixed value, or the jerk is dynamically adjusted according to the current current of the motor on that side, including: j inc = max(j min , j base_inc (1 - β·I curr / I max )) j dec = max(j min , j base_dec (1 - β·I curr / I max )) where j inc and j dec are the jerks during the acceleration phase and deceleration phase respectively; j min is the minimum jerk, not less than the jerk resolution; j the_inc and j the_dec are the theoretical jerks during the acceleration phase and deceleration phase respectively; I curr is the current of this side motor, and I max is the maximum current; β is the suppression ratio factor.

4. The motion control method of a self - moving device according to claim 3, characterized in that, Using the acceleration to calculate the rotational speed at the next moment based on the current rotational speed as the expected rotational speed per unit time includes: For each motor on each side, use the current rotational speed, acceleration, jerk, and the cycle duration of speed update to weighted sum the theoretical rotational speed at the next moment and the current rotational speed to obtain the rotational speed at the next moment as the expected rotational speed per unit time.

5. A motion control method for a self - moving device according to claim 4, characterized in that, Calculating the expected current based on the error between the expected rotational speed per unit time and the current rotational speed includes: For each motor on each side, collect the current rotational speed; Introduce an integral limit value in the speed loop PID to limit the integral term, and dynamically adjust the proportional gain and differential gain according to the magnitude and change rate of the error between the expected rotational speed per unit time and the current rotational speed, and further adjust the differential gain according to the acceleration direction, and then substitute the error between the expected rotational speed per unit time and the current rotational speed into the speed loop PID to output the expected current; Use a preset expected power to limit the expected current and update the expected current to the limited expected current.

6. The motion control method of a self - moving device according to claim 5, characterized in that, Calculating the expected duty cycle based on the error between the expected current and the current current includes: For each motor on each side, collect the current current; Introduce an integral limit value in the current loop PID to limit the integral term, and dynamically adjust the proportional gain, integral gain, and differential gain according to the magnitude and change rate of the error between the expected current and the current current, and further adjust the differential gain according to the current mutation direction, and then substitute the error between the expected current and the current current into the current loop PID to obtain the output of the current loop PID; After using the maximum duty cycle to limit the output of the current loop PID, the expected duty cycle is output.

7. A motion control method for a self-mobile device according to claim 3, wherein when the current duty cycles and the current currents of the motors on both sides are greater than or equal to the corresponding rated values, the expected rotational speed per unit time of the motor on the other side is dynamically adjusted based on one of the motors on one side to achieve synchronization of the motors on both sides, including: Step A1: For the motors on both sides, determine whether the current duty cycles are both ≥ the rated value; If so, execute Step A2; otherwise, determine whether the current duty cycles are both < the rated value; If so, execute Step A5; otherwise, determine whether the current duty cycle of the left motor is ≥ the rated value, If so, execute Step A3; otherwise, execute Step A4; Step A2. Determine whether r L <r R ,r L =n curr_L / ExN L ,r R =n curr_R / ExN R ,where n curr_L 、ExN L are the current rotational speed of the left motor and the expected rotational speed per unit time respectively, and n curr_R 、ExN R are the current rotational speed of the right motor and the expected rotational speed per unit time respectively; If so, execute Step A3; otherwise, execute Step A4; Step A3. Let ExN R = ExN R ·r L , and then execute the expected rotational speed and expected duty cycle of the motors on both sides of the output per unit time to control the corresponding motors respectively, completing the motion control of this control cycle; Step A4. Let ExN L = ExN L ·R r , and then execute the expected rotational speed per unit time and the expected duty cycle of the motors on both sides of the output, which are used to control the corresponding motors respectively, and complete the motion control of this control cycle; Step A5: For the motors on both sides, determine whether the current currents are both ≥ the rated value; If so, execute Step A2; otherwise, determine whether the current currents are both < the rated value; If so, execute the output of the expected rotational speed per unit time and the expected duty cycle of the motors on both sides, which are respectively used to control the corresponding motors to complete the motion control of the current control cycle; otherwise, determine whether the current current of the left motor is ≥ the rated value; If so, execute Step A3; otherwise, execute Step A4.

8. A motion control system for a self-mobile device, comprising two control groups, each control group including a motor and a motor controller, wherein: The motor controller of each said control group controls the motor to implement the steps of the motion control method for the self-mobile device according to any one of claims 1 to 7.

9. A self-moving device, characterized in that: It includes an overall machine controller and the motion control system of the self-mobile device according to claim 8, and the overall machine controller is communicatively connected to the motor controllers of the two control groups in the motion control system of the self-mobile device.

10. A computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and wherein: When the computer program is executed by a processor, it implements the steps of the motion control method for the self-mobile device according to any one of claims 1 to 7.

Images