Servo motor position control method and system suitable for mobile robot

Abstract

This invention relates to the field of mobile robot control, and discloses a servo motor position control method and system suitable for mobile robots, comprising the following steps: First, constructing a target mathematical model of a permanent magnet synchronous motor with dual-channel disturbances, distinguishing between current and mechanical disturbances; designing two-stage linear cascade disturbance observers and two-stage model-compensated extended state observers to compensate for the combined disturbances of the current and mechanical channels in a layered online manner; introducing the observed disturbance quantity into the model predictive position controller, solving for the control voltage in conjunction with the motor model, and applying multiple linear constraints of current, speed, and voltage; outputting the optimal d / q-axis voltage to drive the servo motor in closed-loop operation, and completing performance verification through multi-condition simulation. This invention overcomes the shortcomings of traditional model predictive control parameters being sensitive and having insufficient disturbance compensation, and is adaptable to various complex working conditions.

Description

Technical Field

[0001] This invention relates to the field of mobile robot control, and in particular to a servo motor position control method and system applicable to mobile robots. Background Technology

[0002] Mobile robots are widely used in intelligent manufacturing, intelligent warehousing, inspection and delivery, medical assistance, and special operations. As core execution equipment, they must perform a series of tasks, including autonomous movement, obstacle avoidance, precise positioning and docking, and actuator control. Unlike fixed servo devices, mobile robots operate under complex conditions. The chassis motors need to maintain stable dynamic performance and positioning accuracy under constantly changing road conditions, loads, robot posture, and power supply voltage. Permanent magnet synchronous motors (PMSMs) are small, fast-responding, and highly precise, making them the mainstream drive components for robot walking, steering, and robotic arm mechanisms. However, motors themselves possess strong multi-variable coupling and nonlinear characteristics, and during operation, they are subject to interference from sudden load changes, ground impacts, slope resistance, and temperature rise leading to parameter drift. Improving the position tracking, disturbance rejection capabilities, and operational safety of servo motors in complex environments is a key research issue in this field.

[0003] Traditional three-loop PI control is simple and easy to implement in engineering, but the controller parameters are fixed. Once the vehicle load changes, motor parameters shift due to heat, or external disturbances are significant, fixed-parameter PI controls struggle to simultaneously achieve fast response, high-precision steady-state control, and strong robustness. Model predictive position control, based on a mathematical model of the motor, predicts current, speed, and position, directly outputting control voltage. It can unify and integrate multi-loop dynamic characteristics while incorporating physical constraints such as voltage and current, making it more suitable for high-dynamic servo scenarios. However, the predictive effectiveness of this method relies entirely on accurate motor parameters. Shifts in parameters such as stator inductance, flux linkage, and moment of inertia can cause discrepancies between the model and the actual motor, leading to positioning errors. Therefore, under conditions such as variable load operation, long-term operation, and low-speed precise parking of logistics vehicles, there is still considerable room for optimization in control performance.

[0004] Therefore, it is necessary to propose a servo motor position control method and system suitable for mobile robots to solve the above problems. Summary of the Invention

[0005] This invention overcomes the shortcomings of the prior art and provides a servo motor position control method and system suitable for mobile robots.

[0006] To achieve the above objectives, the technical solution adopted by the present invention is as follows: The first aspect of this invention provides a servo motor position control method suitable for mobile robots, comprising the following steps: A basic mathematical model of the permanent magnet synchronous motor for mobile robots is established, and the sources of disturbance in the two channels are identified on the basic mathematical model of the permanent magnet synchronous motor. Two-stage linear cascaded disturbance observers were built in the d / q axis current channels of the basic mathematical model of the target permanent magnet synchronous motor, and the comprehensive disturbance term of the current channel was compensated by online observation. In the position / speed mechanical disturbance channel of the basic mathematical model of the target permanent magnet synchronous motor, a two-level model compensation extended state observer is built to observe and compensate the comprehensive disturbance term of the mechanical channel online. Obtain the model-predicted position controller, combine it with the basic mathematical model of the target permanent magnet synchronous motor to solve the control voltage of the permanent magnet synchronous servo motor, and set linear constraints; The optimal d-axis control voltage and q-axis control voltage, after correction by multiple linear constraints, are sent to the driver of the permanent magnet synchronous servo motor for closed-loop position control, and the control performance is verified by operating condition simulation.

[0007] Furthermore, in a preferred embodiment of the present invention, the step of establishing a fundamental mathematical model of the permanent magnet synchronous motor for the mobile robot, and dividing the dual-channel disturbance sources on the fundamental mathematical model of the permanent magnet synchronous motor, specifically involves: The servo motor of the mobile robot is determined, and the servo motor of the mobile robot is a permanent magnet synchronous motor, which is calibrated as a permanent magnet synchronous servo motor. Determine the overall nominal parameters of the permanent magnet synchronous servo motor and compile them into a unified parameter calibration table for the motor; Based on the unified parameter calibration table of the motor, an ideal basic mathematical model of the permanent magnet synchronous servo motor under undisturbed operating conditions is built. The method of building the model is to determine two sets of core equations: the d / q axis voltage and current dynamic equations and the rotor mechanical motion equations, which are used to describe the ideal basic mathematical model of the permanent magnet synchronous servo motor under undisturbed operating conditions. Meanwhile, based on the type of disturbance, two types of disturbance channels are divided into the ideal basic mathematical model of the permanent magnet synchronous servo motor under the disturbance-free working condition: the d / q axis current disturbance channel and the position / speed mechanical disturbance channel. All disturbance terms of the two types of disturbance channels are collected and integrated into the current channel comprehensive disturbance term and the mechanical channel comprehensive disturbance term. The combined disturbance terms of the current channel and the combined disturbance terms of the mechanical channel are embedded into two sets of core equations: the d / q axis voltage and current dynamic equation and the rotor mechanical motion equation. This yields an actual mathematical model of the permanent magnet synchronous servo motor with disturbances that closely matches the actual operating state of the mobile robot. Furthermore, historical data networks are used to retrieve all actual operating scenarios of the mobile robot to define the working condition boundaries. Among them, the operating condition boundary corresponds to two types of disturbance terms, generating the basic mathematical model of the target permanent magnet synchronous motor.

[0008] Furthermore, in a preferred embodiment of the present invention, the step of constructing two-stage linear cascaded disturbance observers in the d / q-axis current channels of the target permanent magnet synchronous motor's basic mathematical model, and compensating for the comprehensive disturbance term of the current channel through online observation, specifically involves: The d-axis current signal and q-axis current signal are acquired in real time through a permanent magnet synchronous servo motor. The d-axis current signal and q-axis current signal are the direct axis and quadrature axis current components output by the stator current in the permanent magnet synchronous servo motor after coordinate transformation. In the permanent magnet synchronous servo motor, two independent linear cascade disturbance observers are configured for the d-axis and q-axis respectively, including a first-level main disturbance observation unit and a second-level residual disturbance observation unit. Combining a class of primary main disturbance observation units, d-axis current signals and q-axis current signals, and the basic mathematical model of the target permanent magnet synchronous motor, the current state variables of the d-axis and q-axis and the comprehensive main disturbance of the current channel are calculated, and the calculated values ​​are used as model compensation terms on a class of secondary residual disturbance observation units. Combining the updated model compensation terms of a class of second-level residual disturbance observation units, a class of first-level main disturbance observation units, and the basic mathematical model of the target permanent magnet synchronous motor, a second-order pole placement method is introduced to configure the gain parameters of the two-stage linear cascade disturbance observers on the d-axis and q-axis. This is used to calculate and derive the iterative operation logic of the two-stage linear cascade disturbance observers on the d-axis and q-axis in both continuous and discrete domains. Combining iterative computation logic, iterative current prediction is performed through two-stage linear cascaded perturbation observers on the d-axis and q-axis, outputting d-axis current estimate, d-axis perturbation estimate, q-axis current estimate, and q-axis perturbation estimate.

[0009] Furthermore, in a preferred embodiment of the present invention, the step of establishing a two-level model compensation expansion state observer in the position / speed mechanical disturbance channel of the target permanent magnet synchronous motor's basic mathematical model to observe and compensate for the comprehensive disturbance term of the mechanical channel online specifically involves: Based on the position / speed mechanical disturbance channel of the target permanent magnet synchronous motor's basic mathematical model, a two-level model compensation extended state observer is configured, and two types of primary main disturbance observation units and two types of secondary residual disturbance observation units are set up. By combining the Class II Level 1 main disturbance observation units, the comprehensive main disturbance of the mechanical channel is calculated, and the calculation results are input into the Class II Level 2 main disturbance observation units as model compensation terms; Combining the second-level residual disturbance observation unit with model compensation terms, the second-level main disturbance observation unit, and the basic mathematical model of the target permanent magnet synchronous motor, the third-order pole placement method is used to configure the gain parameters of the two-level model compensation extended state observer, which is used to derive the iterative operation logic of the two-level model compensation extended state observer in both continuous and discrete domains. Based on iterative computation logic, the two-level observation iterative computation of the two-level model compensation extended state observer is completed, and the comprehensive disturbance calculation value of the mechanical channel is output.

[0010] Furthermore, in a preferred embodiment of the present invention, the step of obtaining the model predictive position controller, combining the basic mathematical model of the target permanent magnet synchronous motor to solve the control voltage of the permanent magnet synchronous servo motor, and setting linear constraints, specifically involves: Obtain a model predictive position controller, wherein the model predictive position controller is a control system for rotor position tracking of a permanent magnet synchronous servo motor; The model predictive position controller introduces d-axis current estimates, d-axis disturbance estimates, q-axis current estimates, q-axis disturbance estimates, and mechanical channel integrated disturbance calculations. Based on the introduced data, the model predictive position controller performs real-time model correction on the basic mathematical model of the target permanent magnet synchronous motor. The target permanent magnet synchronous motor basic mathematical model after real-time correction is subjected to multi-step calculations, including d-axis voltage inverse solution calculation to output d-axis basic control voltage, and q-axis voltage inverse solution calculation to output q-axis basic control voltage. Integrate the d-axis base control voltage and the q-axis base control voltage to output an unconstrained control voltage; Based on the working boundary of the mobile robot, three layers of linear constraint boundary conditions are built in the permanent magnet synchronous servo motor, namely the linear constraint of d / q axis current, the linear constraint of motor speed, and the vector constraint of inverter output voltage. The initial voltage of the permanent magnet synchronous servo motor is obtained and substituted into the three-layer linear constraint boundary conditions for constraint projection processing. If the initial voltage of the permanent magnet synchronous servo motor exceeds the three-layer linear constraint boundary conditions, the voltage is adjusted in real time until the optimal d-axis control voltage and q-axis control voltage after multiple linear constraint corrections are output.

[0011] Furthermore, in a preferred embodiment of the present invention, the step of sending the optimal d-axis control voltage and q-axis control voltage, after correction by multiple linear constraints, to the driver of the permanent magnet synchronous servo motor for closed-loop position control, and verifying the control performance through operating condition simulation, specifically involves: The optimal d-axis control voltage and q-axis control voltage, after being corrected by multiple linear constraints, are transmitted to the driver of the permanent magnet synchronous servo motor. The driver of the permanent magnet synchronous servo motor is the control hardware that performs power conversion, coordinate inverse transformation and three-phase current output of the permanent magnet synchronous servo motor, and drives the permanent magnet synchronous servo motor to operate. The driver of the permanent magnet synchronous servo motor controls the operation of the permanent magnet synchronous servo motor and continuously collects the stator current, rotor position and speed signals of the permanent magnet synchronous servo motor. Based on the stator current, rotor position and speed signals of the permanent magnet synchronous servo motor, the stator current coordinate transformation is completed, and the closed-loop correction of the basic mathematical model of the target permanent magnet synchronous motor is performed synchronously by two-level linear cascaded disturbance observers on the d-axis and q-axis and two-level model compensation extended state observers. The working condition boundaries of the mobile robot are retrieved and classified. Simulation test conditions are built, including the motor position command step switching condition under no load, and the position step switching superimposed with load torque sudden change condition. The control performance of the permanent magnet synchronous servo motor was verified by combining simulation test conditions.

[0012] Furthermore, in a preferred embodiment of the present invention, the step of verifying the control performance of the permanent magnet synchronous servo motor in conjunction with simulation test conditions specifically includes: A single-model predictive position control algorithm was built, and two types of simulation test conditions were run into the single-model predictive position control algorithm and the basic mathematical model of the target permanent magnet synchronous motor, respectively. Synchronously collect data on dynamic response convergence time, steady-state maximum position error, steady-state minimum position error, current surge peak, position offset under load disturbance, and disturbance recovery time as quantitative indicators; The two sets of quantitative indicators were compared item by item to verify the control performance of the target permanent magnet synchronous motor's basic mathematical model on the permanent magnet synchronous servo motor.

[0013] A second aspect of the present invention also provides a servo motor position control system suitable for mobile robots. The servo motor position control system includes a memory and a processor. The memory stores a servo motor position control method program. When the program is executed in parallel by an execution unit within the processor, it performs the following steps: A basic mathematical model of the permanent magnet synchronous motor for mobile robots is established, and the sources of disturbance in the two channels are identified on the basic mathematical model of the permanent magnet synchronous motor. Two-stage linear cascaded disturbance observers were built in the d / q axis current channels of the basic mathematical model of the target permanent magnet synchronous motor, and the comprehensive disturbance term of the current channel was compensated by online observation. In the position / speed mechanical disturbance channel of the basic mathematical model of the target permanent magnet synchronous motor, a two-level model compensation extended state observer is built to observe and compensate the comprehensive disturbance term of the mechanical channel online. Obtain the model-predicted position controller, combine it with the basic mathematical model of the target permanent magnet synchronous motor to solve the control voltage of the permanent magnet synchronous servo motor, and set linear constraints; The optimal d-axis control voltage and q-axis control voltage, after correction by multiple linear constraints, are sent to the driver of the permanent magnet synchronous servo motor for closed-loop position control, and the control performance is verified by operating condition simulation.

[0014] This invention addresses the technical deficiencies in the prior art and offers the following advantages: First, a target mathematical model of a permanent magnet synchronous motor with dual-channel disturbances is constructed, distinguishing between current and mechanical disturbances. Two-stage linear cascade disturbance observers and two-stage model compensation extended state observers are designed to provide layered online compensation for combined disturbances in the current and mechanical channels. The observed disturbance quantities are introduced into the model predictive position controller, and the control voltage is solved in conjunction with the motor model, applying multiple linear constraints on current, speed, and voltage. The optimal d / q-axis voltage is output to drive the servo motor in closed-loop operation, and performance verification is completed through multi-condition simulation. This invention overcomes the shortcomings of traditional model predictive control parameters being sensitive and having insufficient disturbance compensation, making it suitable for various complex operating conditions. Attached Figure Description

[0015] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other embodiments can be obtained from these drawings without creative effort.

[0016] Figure 1 A flowchart of a servo motor position control method suitable for mobile robots is shown; Figure 2 A flowchart illustrating the closed-loop position control method for a permanent magnet synchronous servo motor is shown. Figure 3 A program view of a servo motor position control system suitable for a mobile robot is shown. Detailed Implementation

[0017] To better understand the above-mentioned objectives, features, and advantages of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be noted that, unless otherwise specified, the embodiments and features described in these embodiments can be combined with each other.

[0018] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and therefore the scope of protection of the invention is not limited to the specific embodiments disclosed below.

[0019] Figure 1 A flowchart illustrating a servo motor position control method suitable for mobile robots is shown, including the following steps: S102: Establish the basic mathematical model of the permanent magnet synchronous motor of the mobile robot, and identify the dual-channel disturbance sources on the basic mathematical model of the permanent magnet synchronous motor; S104: Two-stage linear cascaded disturbance observers are built in the d / q axis current channels of the basic mathematical model of the target permanent magnet synchronous motor, and the comprehensive disturbance term of the current channel is compensated by online observation. S106: In the position / speed mechanical disturbance channel of the basic mathematical model of the target permanent magnet synchronous motor, a two-level model compensation expansion state observer is built to observe and compensate the comprehensive disturbance term of the mechanical channel online; S108: Obtain the model-predicted position controller, combine the target permanent magnet synchronous motor's basic mathematical model to solve the control voltage of the permanent magnet synchronous servo motor, and set linear constraints; S110: The optimal d-axis control voltage and q-axis control voltage, after correction by multiple linear constraints, are sent to the driver of the permanent magnet synchronous servo motor for closed-loop position control, and the control performance is verified by performing working condition simulation.

[0020] Furthermore, in a preferred embodiment of the present invention, the step of establishing a fundamental mathematical model of the permanent magnet synchronous motor for the mobile robot, and dividing the dual-channel disturbance sources on the fundamental mathematical model of the permanent magnet synchronous motor, specifically involves: The servo motor of the mobile robot is determined, and the servo motor of the mobile robot is a permanent magnet synchronous motor, which is calibrated as a permanent magnet synchronous servo motor. Determine the overall nominal parameters of the permanent magnet synchronous servo motor and compile them into a unified parameter calibration table for the motor; Based on the unified parameter calibration table of the motor, an ideal basic mathematical model of the permanent magnet synchronous servo motor under undisturbed operating conditions is built. The method of building the model is to determine two sets of core equations: the d / q axis voltage and current dynamic equations and the rotor mechanical motion equations, which are used to describe the ideal basic mathematical model of the permanent magnet synchronous servo motor under undisturbed operating conditions. Meanwhile, based on the type of disturbance, two types of disturbance channels are divided into the ideal basic mathematical model of the permanent magnet synchronous servo motor under the disturbance-free working condition: the d / q axis current disturbance channel and the position / speed mechanical disturbance channel. All disturbance terms of the two types of disturbance channels are collected and integrated into the current channel comprehensive disturbance term and the mechanical channel comprehensive disturbance term. The combined disturbance terms of the current channel and the combined disturbance terms of the mechanical channel are embedded into two sets of core equations: the d / q axis voltage and current dynamic equation and the rotor mechanical motion equation. This yields an actual mathematical model of the permanent magnet synchronous servo motor with disturbances that closely matches the actual operating state of the mobile robot. Furthermore, historical data networks are used to retrieve all actual operating scenarios of the mobile robot to define the working condition boundaries. Among them, the operating condition boundary corresponds to two types of disturbance terms, generating the basic mathematical model of the target permanent magnet synchronous motor.

[0021] It should be noted that, firstly, the permanent magnet synchronous servo motor is determined, and the inherent parameters of the motor nameplate and sample (number of pole pairs, resistance, inductance, flux linkage, moment of inertia, etc.) are collected, standardized, tabulated, and stored as the benchmark data for modeling. The two core equations, the d / q axis voltage and current dynamic equations and the rotor mechanical motion equations, describe the electromagnetic coupling relationship and the mechanical motion transmission relationship, respectively. However, they are based on an ideal model and do not consider real disturbances. The disturbances are divided into two categories according to their location. The d / q axis current channel disturbance includes stator resistance, stator inductance, and permanent magnet flux linkage parameter drift caused by motor temperature rise, inverter output voltage error, current sampling circuit sampling deviation and noise, and dynamic errors of d and q axis cross coupling that are not modeled. The position / speed mechanical channel disturbance includes sudden changes in load torque caused by changes in robot load, frictional resistance fluctuations caused by ramps and uneven ground, equivalent rotational inertia shifts caused by long-term operation, and instantaneous external disturbances caused by wheel-to-ground contact and ground impact. Because traditional single extended state observers do not distinguish between current and mechanical disturbances, the disturbance coupling is mixed and the compensation accuracy is low. PI has no disturbance layering compensation capability.

[0022] Two types of disturbance terms are incorporated into the ideal equations to recreate the motor model under real robot operation. By combining a vast amount of historical robot operation scenarios, boundary conditions such as load, ramp, voltage, and low-speed parking are defined, providing a basis for controller linear constraints and observer gain tuning. Finally, different operating conditions are bound to disturbances, resulting in a final target motor model that can distinguish the characteristics of disturbances under different operating conditions. Operating conditions include, but are not limited to, actual operating scenarios such as mobile robot warehousing and transportation, inspection, medical assistance, and special operations. The model can reproduce the real operating characteristics of permanent magnet synchronous servo motors under conditions of variable load, ramp passage, motor temperature rise, and power supply voltage fluctuations.

[0023] Furthermore, in a preferred embodiment of the present invention, the step of constructing two-stage linear cascaded disturbance observers in the d / q-axis current channels of the target permanent magnet synchronous motor's basic mathematical model, and compensating for the comprehensive disturbance term of the current channel through online observation, specifically involves: The d-axis current signal and q-axis current signal are acquired in real time through a permanent magnet synchronous servo motor. The d-axis current signal and q-axis current signal are the direct axis and quadrature axis current components output by the stator current in the permanent magnet synchronous servo motor after coordinate transformation. In the permanent magnet synchronous servo motor, two independent linear cascade disturbance observers are configured for the d-axis and q-axis respectively, including a first-level main disturbance observation unit and a second-level residual disturbance observation unit. Combining a class of primary main disturbance observation units, d-axis current signals and q-axis current signals, and the basic mathematical model of the target permanent magnet synchronous motor, the current state variables of the d-axis and q-axis and the comprehensive main disturbance of the current channel are calculated, and the calculated values ​​are used as model compensation terms on a class of secondary residual disturbance observation units. Combining the updated model compensation terms of a class of second-level residual disturbance observation units, a class of first-level main disturbance observation units, and the basic mathematical model of the target permanent magnet synchronous motor, a second-order pole placement method is introduced to configure the gain parameters of the two-stage linear cascade disturbance observers on the d-axis and q-axis. This is used to calculate and derive the iterative operation logic of the two-stage linear cascade disturbance observers on the d-axis and q-axis in both continuous and discrete domains. Combining iterative computation logic, iterative current prediction is performed through two-stage linear cascaded perturbation observers on the d-axis and q-axis, outputting d-axis current estimate, d-axis perturbation estimate, q-axis current estimate, and q-axis perturbation estimate.

[0024] It should be noted that the driver collects the three-phase stator current of the motor, and after decoupling via Parker coordinate transformation, obtains the direct-axis (d) and quadrature-axis (q) currents, which serve as the real-time input feedback signals for the observer. The purpose is to obtain the decoupled electromagnetic current state variables, providing a raw feedback data source for current disturbance observation. Subsequently, the d-axis and q-axis do not share an observer and operate independently. Each observer is divided into two stages: the first stage estimates the main disturbance, and the second stage compensates for residual disturbances not canceled by the first stage. Because conventional single-order extended observers can only estimate the total disturbance and cannot distinguish between the main disturbance and residual disturbances, they suffer from dynamic lag and insufficient compensation for combined disturbances such as inverter errors and parameter drift; and without a separate axis independent observation design, d- and q-axis disturbances interfere with each other, so they need to be observed separately to eliminate disturbance errors.

[0025] Based on the mathematical model of the target permanent magnet synchronous motor, the first-stage unit first estimates the basic current state and the main disturbances in the current path, including parameter drift and inverter voltage error. The estimation results of the main disturbances are used as model compensation quantities to correct the internal model of the second-stage observer, reducing the deviation between the model and the actual motor. The advantage is that the cascaded progressive compensation approach, with the first stage providing main compensation and the subsequent stage refining residual errors, results in faster dynamic response and smaller steady-state estimation error compared to a single-stage observer. Subsequently, the two-stage cascade observer is equivalent to a second-order system, and the observer gain is standardized and tuned using the pole placement method. Simultaneously, two sets of iterative formulas are derived for continuous-domain simulation and discrete-domain physical controllers, used to perform observation iteration calculations in each control cycle. The corrected current state and the comprehensive disturbance values ​​of the current path are output as correction quantities and fed into the subsequent model predictive position controller. The significance of this scheme is that it eliminates prediction biases caused by various disturbances in the current path at the source.

[0026] Furthermore, in a preferred embodiment of the present invention, the step of establishing a two-level model compensation expansion state observer in the position / speed mechanical disturbance channel of the target permanent magnet synchronous motor's basic mathematical model to observe and compensate for the comprehensive disturbance term of the mechanical channel online specifically involves: Based on the position / speed mechanical disturbance channel of the target permanent magnet synchronous motor's basic mathematical model, a two-level model compensation extended state observer is configured, and two types of primary main disturbance observation units and two types of secondary residual disturbance observation units are set up. By combining the Class II Level 1 main disturbance observation units, the comprehensive main disturbance of the mechanical channel is calculated, and the calculation results are input into the Class II Level 2 main disturbance observation units as model compensation terms; Combining the second-level residual disturbance observation unit with model compensation terms, the second-level main disturbance observation unit, and the basic mathematical model of the target permanent magnet synchronous motor, the third-order pole placement method is used to configure the gain parameters of the two-level model compensation extended state observer, which is used to derive the iterative operation logic of the two-level model compensation extended state observer in both continuous and discrete domains. Based on iterative computation logic, the two-level observation iterative computation of the two-level model compensation extended state observer is completed, and the comprehensive disturbance calculation value of the mechanical channel is output.

[0027] It should be noted that this step specifically establishes an independent two-level observation structure for the rotational speed and position mechanical loop; it consists of two cascaded levels: a primary main disturbance observation unit and a secondary residual disturbance observation unit, handling all mechanical disturbances separately and not sharing the architecture with the current channel observer. This is because traditional single-stage extended state observers, with only a single-level structure, cannot distinguish between the primary disturbances caused by load, friction, and inertia and the residual disturbances caused by observation lag; electromagnetic and mechanical disturbances share the same observation module, leading to disturbance coupling and mutual interference, resulting in low estimation accuracy and poor noise suppression. Specifically, the primary unit, based on the target motor motion model, estimates the dominant mechanical disturbances such as load torque, friction, and inertia changes; this estimated primary disturbance is then fed into the secondary observation unit as a correction compensation, pre-correcting the motion model within the secondary observer and reducing the static deviation between the model and the actual robot operating conditions. Because ordinary extended state observers directly use the nominal motion model without prior primary disturbance compensation, model mismatch will continuously lead to steady-state observation lag, resulting in slow disturbance recovery speed when encountering sudden load changes or ramp impacts.

[0028] Subsequently, the standardized third-order pole placement theory is used to uniformly tune all observation gains, eliminating the need for repeated manual adjustments. Simultaneously, the continuous-domain simulation iteration formula and the discrete-domain servo controller physical operation iteration formula are derived. These are used to perform two levels of observation iteration calculations in each control cycle, integrating the estimation results of the main disturbance and residual disturbance, outputting the comprehensive disturbance quantization values ​​of the entire mechanical channel, and sending them to the model predictive position controller to correct the motion prediction model.

[0029] Furthermore, in a preferred embodiment of the present invention, the step of obtaining the model predictive position controller, combining the basic mathematical model of the target permanent magnet synchronous motor to solve the control voltage of the permanent magnet synchronous servo motor, and setting linear constraints, specifically involves: Obtain a model predictive position controller, wherein the model predictive position controller is a control system for rotor position tracking of a permanent magnet synchronous servo motor; The model predictive position controller introduces d-axis current estimates, d-axis disturbance estimates, q-axis current estimates, q-axis disturbance estimates, and mechanical channel integrated disturbance calculations. Based on the introduced data, the model predictive position controller performs real-time model correction on the basic mathematical model of the target permanent magnet synchronous motor. The target permanent magnet synchronous motor basic mathematical model after real-time correction is subjected to multi-step calculations, including d-axis voltage inverse solution calculation to output d-axis basic control voltage, and q-axis voltage inverse solution calculation to output q-axis basic control voltage. Integrate the d-axis base control voltage and the q-axis base control voltage to output an unconstrained control voltage; Based on the working boundary of the mobile robot, three layers of linear constraint boundary conditions are built in the permanent magnet synchronous servo motor, namely the linear constraint of d / q axis current, the linear constraint of motor speed, and the vector constraint of inverter output voltage. The initial voltage of the permanent magnet synchronous servo motor is obtained and substituted into the three-layer linear constraint boundary conditions for constraint projection processing. If the initial voltage of the permanent magnet synchronous servo motor exceeds the three-layer linear constraint boundary conditions, the voltage is adjusted in real time until the optimal d-axis control voltage and q-axis control voltage after multiple linear constraint corrections are output.

[0030] It should be noted that traditional servo motors generally employ a layered PI three-loop closed-loop design, which involves multi-loop parameter coupling and cumbersome tuning, making it impossible to simultaneously consider dynamic response and multivariable constraints. Ordinary model predictive controllers only use current as the control target and cannot directly achieve position closed-loop control. Therefore, an integrated model predictive position controller is used to replace the layered PI structure, eliminating the multi-loop parameter tuning process and simultaneously optimizing current, speed, and position dynamics, adapting to the frequent start-stop and low-speed precise docking requirements of mobile robots. Subsequently, the estimated values ​​of d / q-axis current, estimated values ​​of current disturbance, and calculated values ​​of comprehensive disturbances in the mechanical channel are all imported into the controller. The internal equations of the predictive model are updated in real time to offset model deviations caused by parameter drift, load abrupt changes, inverter errors, and frictional impacts. The aim is to achieve dynamic self-correction of the model, allowing the model to closely match the actual operating state of the motor. Based on the corrected accurate motor model, inverse kinematics calculations of the d-axis and q-axis voltages are performed respectively to obtain the theoretical control voltage without considering hardware limitations. The d-axis focuses on excitation regulation, while the q-axis directly determines the electromagnetic torque and position dynamic response. The subsequent voltage integration yields an unconstrained voltage. Because this unconstrained voltage is highly susceptible to exceeding the inverter's output limit, the motor's rated current, and maximum speed, potentially causing hardware overload and current surges, it's necessary to establish three layers of linear constraints based on the operating condition boundaries. These constraints respectively limit the maximum winding current, the maximum allowable motor speed, and the inverter's output voltage amplitude. Finally, a constraint projection algorithm is used to correct the unconstrained voltage vector. If the theoretical voltage exceeds any constraint boundary, the voltage amplitude and phase are automatically optimized and adjusted. While meeting all hardware limits, the optimal position tracking effect is preserved, and the final control voltage sent to the driver is output.

[0031] Figure 2 A flowchart illustrating a method for closed-loop position control of a permanent magnet synchronous servo motor is shown, including the following steps: S202: The optimal d-axis control voltage and q-axis control voltage, after correction by multiple linear constraints, are sent to the driver of the permanent magnet synchronous servo motor for closed-loop position control, and the control performance is verified by performing working condition simulation. S204: Verify the control performance of the permanent magnet synchronous servo motor by combining simulation test conditions.

[0032] Furthermore, in a preferred embodiment of the present invention, the step of sending the optimal d-axis control voltage and q-axis control voltage, after correction by multiple linear constraints, to the driver of the permanent magnet synchronous servo motor for closed-loop position control, and verifying the control performance through operating condition simulation, specifically involves: The optimal d-axis control voltage and q-axis control voltage, after being corrected by multiple linear constraints, are transmitted to the driver of the permanent magnet synchronous servo motor. The driver of the permanent magnet synchronous servo motor is the control hardware that performs power conversion, coordinate inverse transformation and three-phase current output of the permanent magnet synchronous servo motor, and drives the permanent magnet synchronous servo motor to operate. The driver of the permanent magnet synchronous servo motor controls the operation of the permanent magnet synchronous servo motor and continuously collects the stator current, rotor position and speed signals of the permanent magnet synchronous servo motor. Based on the stator current, rotor position and speed signals of the permanent magnet synchronous servo motor, the stator current coordinate transformation is completed, and the closed-loop correction of the basic mathematical model of the target permanent magnet synchronous motor is performed synchronously by two-level linear cascaded disturbance observers on the d-axis and q-axis and two-level model compensation extended state observers. The working condition boundaries of the mobile robot are retrieved and classified. Simulation test conditions are built, including the motor position command step switching condition under no load, and the position step switching superimposed with load torque sudden change condition. The control performance of the permanent magnet synchronous servo motor was verified by combining simulation test conditions.

[0033] It should be noted that the model predictive position controller output and the d / q axis optimal voltage command optimized by constraint projection are sent to the servo driver to complete the data interaction between the control algorithm and the execution hardware. Throughout the motor operation, the driver collects three-phase stator current, rotor real-time position, and speed information in real time through current sampling circuits and encoders, serving as the original feedback data source for closed-loop control. Traditional algorithms only mention the collection of current and position without explaining the continuous real-time collection mechanism, which cannot meet the requirements of high-frequency periodic closed-loop iteration. The three-phase current collected by the driver is decoupled into d / q axis current components through Park transform and sent to a two-stage linear cascade disturbance observer and a two-stage model compensation extended state observer. The output disturbance estimate continuously corrects the basic mathematical model of the target permanent magnet synchronous motor, forming an uninterrupted closed-loop cycle of "collection—observation—model correction—voltage solution".

[0034] In the subsequent phase, based on the working condition boundaries defined by scenarios such as robot warehousing, inspection, and ramps, two sets of representative simulation test scenarios were divided to simulate two core working scenarios: robot start-stop and reversal under no-load conditions and impact disturbances on loaded / ramp vehicles.

[0035] Furthermore, in a preferred embodiment of the present invention, the step of verifying the control performance of the permanent magnet synchronous servo motor in conjunction with simulation test conditions specifically includes: A single-model predictive position control algorithm was built, and two types of simulation test conditions were run into the single-model predictive position control algorithm and the basic mathematical model of the target permanent magnet synchronous motor, respectively. Synchronously collect data on dynamic response convergence time, steady-state maximum position error, steady-state minimum position error, current surge peak, position offset under load disturbance, and disturbance recovery time as quantitative indicators; The two sets of quantitative indicators were compared item by item to verify the control performance of the target permanent magnet synchronous motor's basic mathematical model on the permanent magnet synchronous servo motor.

[0036] It should be noted that a traditional single-model predictive position control algorithm without two-stage linear cascaded disturbance observers and two-stage model compensation extended state observers was constructed as a control benchmark. Two test scenarios—no-load step condition and step-plus-abrupt-load condition—were run independently in the control group algorithm and the complete control scheme of this invention (equipped with the target permanent magnet synchronous motor's basic mathematical model), ensuring complete consistency in simulation environment, motor parameters, and operating conditions. Six core quantitative indicators were synchronously collected throughout the entire process of both algorithms, covering four core performance dimensions: dynamic following speed, steady-state positioning accuracy, current surge suppression, and resistance to mechanical disturbances. Each quantitative indicator was compared, and the improvement in control performance brought about by the target motor model based on dual-channel disturbance correction was comprehensively determined by the reduction in convergence time, the decrease in steady-state error, the reduction in peak current, and the improvement in disturbance offset and recovery time.

[0037] Based on a comprehensive assessment of various indicators, the control method of this application is confirmed to have advantages such as faster response speed, higher steady-state positioning accuracy, smaller current surge and stronger robustness under complex working conditions such as motor parameter drift, frequent start-stop, variable load and ramp crossing. It is suitable for the servo position control needs of various mobile robots in intelligent manufacturing, intelligent warehousing, inspection and delivery, medical assistance and special operation scenarios.

[0038] like Figure 3 As shown, a second aspect of the present invention also provides a servo motor position control system suitable for mobile robots. The servo motor position control system includes a memory and a processor. The memory contains a servo motor position control method program. When the program is executed in parallel by the execution unit within the processor, it performs the following steps: A basic mathematical model of the permanent magnet synchronous motor for mobile robots is established, and the sources of disturbance in the two channels are identified on the basic mathematical model of the permanent magnet synchronous motor. Two-stage linear cascaded disturbance observers were built in the d / q axis current channels of the basic mathematical model of the target permanent magnet synchronous motor, and the comprehensive disturbance term of the current channel was compensated by online observation. In the position / speed mechanical disturbance channel of the basic mathematical model of the target permanent magnet synchronous motor, a two-level model compensation extended state observer is built to observe and compensate the comprehensive disturbance term of the mechanical channel online. Obtain the model-predicted position controller, combine it with the basic mathematical model of the target permanent magnet synchronous motor to solve the control voltage of the permanent magnet synchronous servo motor, and set linear constraints; The optimal d-axis control voltage and q-axis control voltage, after correction by multiple linear constraints, are sent to the driver of the permanent magnet synchronous servo motor for closed-loop position control, and the control performance is verified by operating condition simulation.

[0039] The above are merely specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A servo motor position control method suitable for mobile robots, characterized in that, Includes the following steps: A basic mathematical model of the permanent magnet synchronous motor for mobile robots is established, and the sources of disturbance in the two channels are identified on the basic mathematical model of the permanent magnet synchronous motor. Two-stage linear cascaded disturbance observers were built in the d / q axis current channels of the basic mathematical model of the target permanent magnet synchronous motor, and the comprehensive disturbance term of the current channel was compensated by online observation. In the position / speed mechanical disturbance channel of the basic mathematical model of the target permanent magnet synchronous motor, a two-level model compensation extended state observer is built to observe and compensate the comprehensive disturbance term of the mechanical channel online. Obtain the model-predicted position controller, combine it with the basic mathematical model of the target permanent magnet synchronous motor to solve the control voltage of the permanent magnet synchronous servo motor, and set linear constraints; The optimal d-axis control voltage and q-axis control voltage, after correction by multiple linear constraints, are sent to the driver of the permanent magnet synchronous servo motor for closed-loop position control, and the control performance is verified by operating condition simulation.

2. The servo motor position control method for mobile robots according to claim 1, characterized in that, The establishment of the fundamental mathematical model of the permanent magnet synchronous motor for the mobile robot, and the identification of dual-channel disturbance sources on the fundamental mathematical model of the permanent magnet synchronous motor, are as follows: The servo motor of the mobile robot is determined, and the servo motor of the mobile robot is a permanent magnet synchronous motor, which is calibrated as a permanent magnet synchronous servo motor. Determine the overall nominal parameters of the permanent magnet synchronous servo motor and compile them into a unified parameter calibration table for the motor; Based on the unified parameter calibration table of the motor, an ideal basic mathematical model of the permanent magnet synchronous servo motor under undisturbed operating conditions is built. The method of building the model is to determine two sets of core equations: the d / q axis voltage and current dynamic equations and the rotor mechanical motion equations, which are used to describe the ideal basic mathematical model of the permanent magnet synchronous servo motor under undisturbed operating conditions. Meanwhile, based on the type of disturbance, two types of disturbance channels are divided into the ideal basic mathematical model of the permanent magnet synchronous servo motor under the disturbance-free working condition: the d / q axis current disturbance channel and the position / speed mechanical disturbance channel. All disturbance terms of the two types of disturbance channels are collected and integrated into the current channel comprehensive disturbance term and the mechanical channel comprehensive disturbance term. The combined disturbance terms of the current channel and the combined disturbance terms of the mechanical channel are embedded into two sets of core equations: the d / q axis voltage and current dynamic equation and the rotor mechanical motion equation. This yields an actual mathematical model of the permanent magnet synchronous servo motor with disturbances that closely matches the actual operating state of the mobile robot. Furthermore, historical data networks are used to retrieve all actual operating scenarios of the mobile robot to define the working condition boundaries. Among them, the operating condition boundary corresponds to two types of disturbance terms, generating the basic mathematical model of the target permanent magnet synchronous motor.

3. The servo motor position control method for mobile robots according to claim 1, characterized in that, The method involves constructing two-stage linear cascaded disturbance observers in the d / q-axis current channels of the target permanent magnet synchronous motor's fundamental mathematical model, and compensating for the comprehensive disturbance term of the current channel through online observation. Specifically: The d-axis current signal and q-axis current signal are acquired in real time through a permanent magnet synchronous servo motor. The d-axis current signal and q-axis current signal are the direct axis and quadrature axis current components output by the stator current in the permanent magnet synchronous servo motor after coordinate transformation. In the permanent magnet synchronous servo motor, two independent linear cascade disturbance observers are configured for the d-axis and q-axis respectively, including a first-level main disturbance observation unit and a second-level residual disturbance observation unit. Combining a class of primary main disturbance observation units, d-axis current signals and q-axis current signals, and the basic mathematical model of the target permanent magnet synchronous motor, the current state variables of the d-axis and q-axis and the comprehensive main disturbance of the current channel are calculated, and the calculated values ​​are used as model compensation terms on a class of secondary residual disturbance observation units. Combining the updated model compensation terms of a class of second-level residual disturbance observation units, a class of first-level main disturbance observation units, and the basic mathematical model of the target permanent magnet synchronous motor, a second-order pole placement method is introduced to configure the gain parameters of the two-stage linear cascade disturbance observers on the d-axis and q-axis. This is used to calculate and derive the iterative operation logic of the two-stage linear cascade disturbance observers on the d-axis and q-axis in both continuous and discrete domains. Combining iterative computation logic, iterative current prediction is performed through two-stage linear cascaded perturbation observers on the d-axis and q-axis, outputting d-axis current estimate, d-axis perturbation estimate, q-axis current estimate, and q-axis perturbation estimate.

4. The servo motor position control method for mobile robots according to claim 1, characterized in that, In the position / speed mechanical disturbance channel of the target permanent magnet synchronous motor's basic mathematical model, a two-level model compensation extended state observer is built to observe and compensate for the comprehensive disturbance term of the mechanical channel online. Specifically: Based on the position / speed mechanical disturbance channel of the target permanent magnet synchronous motor's basic mathematical model, a two-level model compensation extended state observer is configured, and two types of primary main disturbance observation units and two types of secondary residual disturbance observation units are set up. By combining the Class II Level 1 main disturbance observation units, the comprehensive main disturbance of the mechanical channel is calculated, and the calculation results are input into the Class II Level 2 main disturbance observation units as model compensation terms; Combining the second-level residual disturbance observation unit with model compensation terms, the second-level main disturbance observation unit, and the basic mathematical model of the target permanent magnet synchronous motor, the third-order pole placement method is used to configure the gain parameters of the two-level model compensation extended state observer, which is used to derive the iterative operation logic of the two-level model compensation extended state observer in both continuous and discrete domains. Based on iterative computation logic, the two-level observation iterative computation of the two-level model compensation extended state observer is completed, and the comprehensive disturbance calculation value of the mechanical channel is output.

5. The servo motor position control method for mobile robots according to claim 1, characterized in that, The acquisition model predicts the position controller, and combines the basic mathematical model of the target permanent magnet synchronous motor to solve for the control voltage of the permanent magnet synchronous servo motor, and sets linear constraints, specifically: Obtain a model predictive position controller, wherein the model predictive position controller is a control system for rotor position tracking of a permanent magnet synchronous servo motor; The model predictive position controller introduces d-axis current estimates, d-axis disturbance estimates, q-axis current estimates, q-axis disturbance estimates, and mechanical channel integrated disturbance calculations. Based on the introduced data, the model predictive position controller performs real-time model correction on the basic mathematical model of the target permanent magnet synchronous motor. The target permanent magnet synchronous motor basic mathematical model after real-time correction is subjected to multi-step calculations, including d-axis voltage inverse solution calculation to output d-axis basic control voltage, and q-axis voltage inverse solution calculation to output q-axis basic control voltage. Integrate the d-axis base control voltage and the q-axis base control voltage to output an unconstrained control voltage; Based on the working boundary of the mobile robot, three layers of linear constraint boundary conditions are built in the permanent magnet synchronous servo motor, namely the linear constraint of d / q axis current, the linear constraint of motor speed, and the vector constraint of inverter output voltage. The initial voltage of the permanent magnet synchronous servo motor is obtained and substituted into the three-layer linear constraint boundary conditions for constraint projection processing. If the initial voltage of the permanent magnet synchronous servo motor exceeds the three-layer linear constraint boundary conditions, the voltage is adjusted in real time until the optimal d-axis control voltage and q-axis control voltage after multiple linear constraint corrections are output.

6. The servo motor position control method for mobile robots according to claim 1, characterized in that, The driver that sends the optimal d-axis control voltage and q-axis control voltage, after correction by multiple linear constraints, to the permanent magnet synchronous servo motor is used for closed-loop position control, and the control performance is verified by operating condition simulation. Specifically: The optimal d-axis control voltage and q-axis control voltage, after being corrected by multiple linear constraints, are transmitted to the driver of the permanent magnet synchronous servo motor. The driver of the permanent magnet synchronous servo motor is the control hardware that performs power conversion, coordinate inverse transformation and three-phase current output of the permanent magnet synchronous servo motor, and drives the permanent magnet synchronous servo motor to operate. The driver of the permanent magnet synchronous servo motor controls the operation of the permanent magnet synchronous servo motor and continuously collects the stator current, rotor position and speed signals of the permanent magnet synchronous servo motor. Based on the stator current, rotor position and speed signals of the permanent magnet synchronous servo motor, the stator current coordinate transformation is completed, and the closed-loop correction of the basic mathematical model of the target permanent magnet synchronous motor is performed synchronously by two-level linear cascaded disturbance observers on the d-axis and q-axis and two-level model compensation extended state observers. The working condition boundaries of the mobile robot are retrieved and classified. Simulation test conditions are built, including the motor position command step switching condition under no load, and the position step switching superimposed with load torque sudden change condition. The control performance of the permanent magnet synchronous servo motor was verified by combining simulation test conditions.

7. The servo motor position control method for mobile robots according to claim 6, characterized in that, The control performance of the permanent magnet synchronous servo motor is verified by combining simulation test conditions, specifically as follows: A single-model predictive position control algorithm was built, and two types of simulation test conditions were run into the single-model predictive position control algorithm and the basic mathematical model of the target permanent magnet synchronous motor, respectively. Synchronously collect data on dynamic response convergence time, steady-state maximum position error, steady-state minimum position error, current surge peak, position offset under load disturbance, and disturbance recovery time as quantitative indicators; The two sets of quantitative indicators were compared item by item to verify the control performance of the target permanent magnet synchronous motor's basic mathematical model on the permanent magnet synchronous servo motor.

8. A servo motor position control system suitable for mobile robots, characterized in that, The servo motor position control system includes a memory and a processor. The memory contains a servo motor position control method program. When the program is executed in parallel by the execution unit in the processor, it implements the servo motor position control steps for mobile robots as described in any one of claims 1-7.

Images