Control parameter optimization method for second-order balance robot

Abstract

The invention relates to the field of robot balance control, and discloses a control parameter optimization method of a second-order balance robot. According to the method, for a second-order balance system formed by connecting a vehicle body and a swing rod in series, a dynamic model and a linearization discrete state space model are established, and a linear quadratic regulator control framework is constructed; taking diagonal elements of the state weight matrix as to-be-optimized parameters, mapping the to-be-optimized parameters to a logarithmic scale search space after left-right symmetry constraint dimensionality reduction processing, and performing optimization by adopting a particle swarm optimization algorithm; and a composite cost function including a core error term, a control saturation term and a control stability term is constructed based on vehicle body inclination angle response, swing rod inclination angle response, control input and closed-loop poles, and an optimal state weight matrix and a corresponding control gain are output to serve as optimal control parameters of the second-order balance robot. The second-order balance robot control parameter setting method can improve pertinence and stability of second-order balance robot control parameter setting.

Description

Technical Field

[0001] This invention relates to the field of robot balance control, and more specifically to a method for optimizing control parameters of a second-order balancing robot. Background Technology

[0002] This application relates to the field of robot balance control technology, specifically to a method for optimizing control parameters of a second-order balancing robot. A second-order balancing robot typically includes a body, wheels, and a swing arm hinged to the body. The pitch of the body around its axle constitutes the first level of balance, and the oscillation of the swing arm around the body constitutes the second level of balance, belonging to a series balancing system with two-stage attitude adjustment requirements.

[0003] Existing balance control schemes mostly focus on traditional two-wheeled self-balancing vehicles. They typically construct linear quadratic regulators based on linearized models and determine the state weight matrix and input weight matrix through manual parameter testing to balance attitude recovery and control output. These schemes mainly revolve around single-stage pitch balance of the vehicle body, with relatively simple parameter tuning objects, and the evaluation focus is mostly on vehicle attitude error and basic control response.

[0004] However, for a second-order balancing robot, changes in the tilt angle of the pendulum will transmit additional gravitational torque and inertial disturbances to the vehicle body, and the vehicle body's attitude adjustment will in turn affect the pendulum's motion, resulting in significant coupling between the vehicle body and the pendulum. In this case, if the parameter tuning method for single-stage balancing systems is still used, the two-stage balancing cooperation relationship is easily disrupted, leading to problems such as difficulty in effectively compensating for pendulum attitude errors, mismatch between control input and hardware capabilities, and insufficient closed-loop stability. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a method for optimizing control parameters of a second-order balancing robot, thereby solving the technical problems existing in the prior art.

[0006] The above-mentioned technical objective of the present invention is achieved through the following technical solution:

[0007] A method for optimizing control parameters of a second-order balancing robot includes the following steps:

[0008] S1. Establish a dynamic model for a second-order balancing robot. The second-order balancing robot is a second-order balancing system formed by the vehicle body and the pendulum in series. The pitch of the vehicle body around the axle constitutes the first-level balance, and the hinged swing of the pendulum around the vehicle body constitutes the second-level balance.

[0009] S2. Based on the dynamic model, linearization is performed near the upright equilibrium point to establish the state space model of the second-order balancing robot. The state space model is then discretized to construct a linear quadratic regulator control framework. The state vector includes the left wheel angle, right wheel angle, vehicle tilt angle, swing arm tilt angle, left wheel angular velocity, right wheel angular velocity, vehicle tilt angle angular velocity, and swing arm angular velocity. The control input includes the left wheel motor input and the right wheel motor input.

[0010] S3. Construct the state weight matrix and input weight matrix State weight matrix The input weight matrix is ​​a diagonal matrix. It is a diagonal matrix, and the input weight matrix is... The state weight matrix is ​​fixed as a preset input weight diagonal matrix. The diagonal elements are used as parameters to be optimized;

[0011] S4. Based on the symmetry characteristics of the left and right structures and drive configuration of the second-order balancing robot, apply symmetry constraints to the parameters to be optimized, so that the weight of the left wheel position term is equal to the weight of the right wheel position term, and the weight of the left wheel velocity term is equal to the weight of the right wheel velocity term, so as to form a set of parameters to be optimized after dimensionality reduction.

[0012] S5. Map the set of parameters to be optimized to the logarithmic scale search space, initialize the particle swarm, and use the position of each particle to represent a set of candidate state weight parameters.

[0013] S6. Decode the candidate state weight parameters for each particle to recover the actual linear weight values ​​and construct the candidate state weight matrix. and combined with the input weight matrix Solve for the control gain of the corresponding linear quadratic regulator. ;

[0014] S7, Control Gain The discrete-time closed-loop system corresponding to the state-space model is substituted into the simulation. Based on the vehicle body tilt angle response and the swing arm tilt angle response obtained from the simulation, a composite cost function value is constructed. The composite cost function includes a core error term, a control saturation term, and a control stability term.

[0015] S8. When the closed-loop system corresponding to the candidate state weight parameter is unstable or the control gain K fails to be solved, the composite cost function value corresponding to the candidate state weight parameter is set to the preset maximum value.

[0016] S9. Based on the composite cost function value of each particle, update the individual optimal position and global optimal position of the particles, and iteratively update each particle according to the speed update rule and position update rule of the particle swarm optimization algorithm.

[0017] S10. When the preset termination condition is met, output the optimal state weight matrix corresponding to the globally optimal particle. and the optimal state weight matrix Corresponding control gain , which serves as the optimized control parameters for the second-order balancing robot.

[0018] Preferably, the state vector of the state space model is an eight-dimensional state vector, which sequentially includes the left wheel angle, right wheel angle, vehicle body tilt angle, rocker arm tilt angle, left wheel angular velocity, right wheel angular velocity, vehicle body tilt angle angular velocity, and rocker arm angular velocity; the control input is a two-dimensional control input, including left wheel motor input and right wheel motor input.

[0019] Preferably, the second-order balancing robot is linearized under the following simplification conditions: the wheels are in pure rolling motion with no relative sliding, the vehicle body and the swing arm are considered rigid bodies and elastic deformation is ignored, and the motor inductance is negligible.

[0020] Preferably, the state weight matrix Q is represented as follows:

[0021] ;

[0022] The input weight matrix R is represented as follows:

[0023] ;

[0024] and the input weight matrix The input weights are fixed as a preset diagonal matrix, and only the state weight matrix is ​​considered. To find the best option.

[0025] Preferably, the state weight matrix The applied symmetry constraints include: weight of the revolver position term. Weight of the right wheel position item Equal weights for the revolver speed term Weight of the right wheel speed term The original eight independent parameters to be optimized are reduced to six parameters to be optimized.

[0026] Preferably, the set of parameters to be optimized is updated with particle positions in a logarithmic search space, and restored to actual linear weight values ​​through exponential mapping before fitness calculation, in order to construct a candidate state weight matrix. .

[0027] Preferably, the composite cost function is a weighted sum of the core error term, the control saturation term, and the control stability term, used to weight the candidate state matrix. The corresponding control accuracy, execution feasibility, and stability margin are jointly evaluated.

[0028] Preferably, the core error term is obtained by accumulating the sum of the absolute values ​​of the vehicle body tilt angle and the absolute values ​​of the swing arm tilt angle at each sampling time during the discrete time domain simulation.

[0029] The control saturation term is obtained based on the cumulative square value of the portion of the control input that exceeds the preset output limit.

[0030] The control stability term is determined based on the relationship between the maximum magnitude of the closed-loop poles and the stability safety threshold. When the maximum magnitude of the closed-loop poles is less than the stability safety threshold, the control stability term is set to zero.

[0031] When the maximum magnitude of the closed-loop pole is greater than or equal to the stability safety threshold, the control stability term is obtained by combining the square of the difference between the maximum magnitude of the closed-loop pole and the stability safety threshold with a preset stability penalty coefficient.

[0032] Preferably, in step S5, a hybrid initialization strategy is adopted when initializing the particle swarm, so that most particles are randomly generated in the search space, and the baseline linear quadratic regulator parameters tuned based on human experience are injected as seeds into the initial population.

[0033] In step S9, the inertia weight adopts an update strategy that linearly decreases with the number of iterations, and the individual cognitive factor and the social cognitive factor are both preset values;

[0034] In step S10, the preset termination condition includes reaching the maximum number of iterations or the composite cost function value being less than a preset reference value.

[0035] Preferably, the input weight matrix Fixed as ;

[0036] The stability safety threshold is 0.98;

[0037] The particle swarm has a population size of 100, a maximum number of iterations of 100, an inertia weight that decreases linearly from 0.9 to 0.4, and an individual cognitive factor and a social cognitive factor of 1.5.

[0038] In summary, the present invention has the following main beneficial effects:

[0039] This application first establishes a control parameter optimization mechanism for a second-order balancing robot formed by the vehicle body and the pendulum in series. By incorporating the first-level balancing of the vehicle body and the second-level balancing of the pendulum into the control object, the parameter optimization process no longer focuses solely on single-level pitch balancing but rather on collaborative design for a two-level coupled balancing state. Simultaneously, by constructing a linear quadratic regulator control framework based on a linearized state-space model and using the state weight matrix as the main optimization object, the obtained control parameters can simultaneously take into account both vehicle body tilt angle recovery and pendulum tilt angle recovery. This improves upon the problem that traditional two-wheeled self-balancing vehicle parameter tuning schemes are difficult to directly adapt to second-order balancing robots and are prone to insufficient pendulum attitude compensation and overall balance instability.

[0040] By combining the symmetry characteristics of the left and right structures and drive configuration of a second-order balancing robot, symmetry constraints are applied to the weights of the left and right wheel position terms and the left and right wheel velocity terms in the state weight matrix. Particle swarm optimization is then performed using the dimensionality-reduced set of parameters to be optimized, making the search space more closely match the robot's structural features and avoiding asymmetrical weight allocations that lack physical meaning. At the same time, by mapping the parameters to be optimized to a logarithmic-scale search space and restoring them to actual linear weight values ​​during computation, the state weights that vary across orders of magnitude can achieve a more balanced search accuracy, thereby improving the convergence stability, parameter interpretability, and engineering adaptability of the control parameter optimization process.

[0041] By constructing a composite cost function that includes a core error term, a control saturation term, and a control stability term, the control parameter optimization process not only considers the cumulative error of the vehicle tilt angle and the swing arm tilt angle, but also simultaneously considers whether the control input exceeds the hardware output limit and whether the closed-loop system has sufficient stability margin. Furthermore, by assigning a preset maximum cost value to candidate parameters that are unstable in the closed-loop system or fail to solve for the control gain, the optimization results can automatically eliminate parameter combinations that are theoretically infeasible or engineeringally unfeasible. This improves the accuracy of two-stage attitude control while taking into account the constraints of drive execution capability and the reliability of closed-loop operation, resulting in optimized control parameters that are more suitable for practical second-order balancing robot applications. Attached Figure Description

[0042] Figure 1 This is a comparison diagram of the tilt angle response of the step-balancing robot body and the pendulum of the present invention.

[0043] Figure 2 This is the convergence curve of the optimal cost function of PSO in this invention.

[0044] Figure 3 This is a flowchart of the method of the present invention. Detailed Implementation

[0045] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0046] Example 1

[0047] refer to Figure 1 A method for optimizing control parameters of a second-order balancing robot includes the following steps:

[0048] S1. Establish a dynamic model for a second-order balancing robot. The second-order balancing robot is a second-order balancing system formed by the vehicle body and the pendulum in series. The pitch of the vehicle body around the axle constitutes the first-level balance, and the hinged swing of the pendulum around the vehicle body constitutes the second-level balance.

[0049] S2. Based on the dynamic model, linearization is performed near the upright equilibrium point to establish the state space model of the second-order balancing robot. The state space model is then discretized to construct a linear quadratic regulator control framework. The state vector includes the left wheel angle, right wheel angle, vehicle tilt angle, swing arm tilt angle, left wheel angular velocity, right wheel angular velocity, vehicle tilt angle angular velocity, and swing arm angular velocity. The control input includes the left wheel motor input and the right wheel motor input.

[0050] S3. Construct the state weight matrix and input weight matrix State weight matrix The input weight matrix is ​​a diagonal matrix. It is a diagonal matrix, and the input weight matrix is... The state weight matrix is ​​fixed as a preset input weight diagonal matrix. The diagonal elements are used as parameters to be optimized;

[0051] S4. Based on the symmetry characteristics of the left and right structures and drive configuration of the second-order balancing robot, apply symmetry constraints to the parameters to be optimized, so that the weight of the left wheel position term is equal to the weight of the right wheel position term, and the weight of the left wheel velocity term is equal to the weight of the right wheel velocity term, so as to form a set of parameters to be optimized after dimensionality reduction.

[0052] S5. Map the set of parameters to be optimized to the logarithmic scale search space, initialize the particle swarm, and use the position of each particle to represent a set of candidate state weight parameters.

[0053] S6. Decode the candidate state weight parameters for each particle to recover the actual linear weight values ​​and construct the candidate state weight matrix. and combined with the input weight matrix Solve for the control gain of the corresponding linear quadratic regulator. ;

[0054] S7, Control Gain The discrete-time closed-loop system corresponding to the state-space model is substituted into the simulation. Based on the vehicle body tilt angle response and the swing arm tilt angle response obtained from the simulation, a composite cost function value is constructed. The composite cost function includes a core error term, a control saturation term, and a control stability term.

[0055] S8. When the closed-loop system corresponding to the candidate state weight parameter is unstable or the control gain K fails to be solved, the composite cost function value corresponding to the candidate state weight parameter is set to the preset maximum value.

[0056] S9. Based on the composite cost function value of each particle, update the individual optimal position and global optimal position of the particles, and iteratively update each particle according to the speed update rule and position update rule of the particle swarm optimization algorithm.

[0057] S10. When the preset termination condition is met, output the optimal state weight matrix corresponding to the globally optimal particle. and the optimal state weight matrix Corresponding control gain , which serves as the optimized control parameters for the second-order balancing robot.

[0058] The second-order balancing robot in this embodiment includes a vehicle body, left wheel, right wheel, left drive motor, right drive motor, and a pendulum. The vehicle body rolls in contact with the ground via the left and right wheels, and the pendulum is hinged to the vehicle body. During operation, the pitch motion of the vehicle body around its axle constitutes the first level of balance, and the oscillation of the pendulum around the hinge point of the vehicle body constitutes the second level of balance. These two are not independent but are connected in series through mechanical linkage and inertia. When the pendulum's attitude changes, it transmits additional gravitational torque and inertial disturbance to the vehicle body, affecting the vehicle body's balance maintenance. The attitude adjustment and wheel drive generated by the vehicle body to restore balance, in turn, act on the pendulum, affecting the pendulum's attitude recovery process. Therefore, this invention does not address the problem of parameter tuning for a single-stage pitch balance of the vehicle body, as in traditional two-wheeled self-balancing vehicles, but rather the optimization problem of second-order coupled control parameters that simultaneously consider the stability of the two-stage balance points of the vehicle body and the pendulum. This characteristic is the fundamental difference between this invention and conventional single-stage balancing robot parameter tuning schemes.

[0059] To achieve the eight-dimensional state quantity acquisition described in the claims, in this embodiment, angle encoders are configured on the left and right wheels respectively to output the left and right wheel angles, and the angular velocities of the left and right wheels are calculated based on the position changes at adjacent sampling times. An attitude sensor is installed on the vehicle body to output the vehicle body tilt angle and tilt angle angular velocity. A pendulum angle sensor is installed at the pendulum hinge to output the pendulum tilt angle; the pendulum angular velocity can be obtained in two ways: first, by performing differential calculation using the output values ​​of the pendulum angle sensor at adjacent sampling times; second, by directly measuring it using an angular velocity sensor installed on the pendulum. To ensure a clear implementation path and ease of engineering application, this embodiment adopts the first method, i.e., obtaining the pendulum angular velocity from the discrete differential value of the pendulum tilt angle. The controller performs synchronous acquisition and processing of the above state quantities under a unified time reference, thereby constructing the state vector of the second-order balancing robot. To avoid symbol confusion, the state vector is defined in this embodiment as:

[0060]

[0061] in, Indicates the first The angle of the left wheel at each sampling moment, Indicates the first The angle of the right wheel at each sampling time. Indicates the vehicle body tilt angle. Indicates the tilt angle of the pendulum. and These represent the angular velocities of the left and right wheels, respectively. Indicates the angular velocity of the vehicle body tilt angle. This represents the angular velocity of the pendulum. Correspondingly, the control input is defined as:

[0062]

[0063] in, Indicates the first The control input applied to the left drive motor at each sampling time Indicates the first The control input applied to the right drive motor at each sampling time.

[0064] To enable those skilled in the art to implement this method, the structural and physical parameters involved in this embodiment are obtained in the following ways: the mass of the vehicle body and the mass of the pendulum arm can be obtained by weighing after the entire machine is assembled; the radius of the left wheel, the radius of the right wheel, the wheelbase, the geometric dimensions of the vehicle body, and the geometric dimensions of the pendulum arm can be determined based on structural design drawings and physical measurement results; the positions of the center of mass of the vehicle body and the center of mass of the pendulum arm can be determined based on the results of three-dimensional modeling combined with weighing and balancing tests; the moment of inertia can be determined based on the calculation results of the three-dimensional model, or measured using the swing method commonly used in the art. All of the above parameters are obtained through conventional parameter acquisition methods that those skilled in the art can directly use in robot design and control, without relying on unverifiable data sources.

[0065] In this embodiment, a dynamic model is first established for a second-order balancing robot. To facilitate controller design, the following simplification conditions are adopted without disrupting the basic motion mechanism of the system: the wheels roll purely on the ground without relative slippage; the vehicle body and pendulum are considered rigid bodies and elastic deformation is ignored; the influence of motor inductance on the control process is negligible; and only the mechanical characteristics closely related to balance control are retained. These simplification conditions match the control parameter optimization objective of this invention. The aim is to ensure that the model can represent the two-stage balance coupling relationship while avoiding the introduction of secondary factors unrelated to the optimization of control parameters into the state equations, thus preventing excessively high solution complexity.

[0066] Under these simplified conditions, a continuous-time state-space model of the system can be established based on the Lagrange equations. For this invention, the important point is not to list all the intermediate derivations, but to clarify the final model form used for parameter optimization and controller design. Therefore, this embodiment expresses the continuous state-space model as follows:

[0067]

[0068] in, It is a continuous-time state vector. For continuous-time control input vectors, The system state matrix, The input matrix. and The parameters are determined by the structural, mass, geometric, and inertial parameters of the second-order balancing robot, and reflect the coupling relationship between the vehicle body and the pendulum.

[0069] Since the controller described in this invention operates on a digital controller, it is necessary to convert the continuous model into a discrete model. Therefore, the controller sampling period is set to... Furthermore, using zero-order preserved discretization, the continuous state-space model is discretized as follows:

[0070]

[0071] in, The discrete state matrix, The discrete input matrix is ​​calculated as follows:

[0072]

[0073]

[0074] in, Represents matrix exponentiation. The variable is the integral variable. This discretization process allows subsequent linear quadratic regulator design, closed-loop pole analysis, and particle swarm optimization iterations to be uniformly based on a discrete-time domain model consistent with the actual digital control cycle, thereby avoiding deviations when parameters obtained only in the continuous domain are applied in the actual controller.

[0075] In this embodiment, a linear quadratic regulator control framework is constructed for the discrete state-space model. The control law adopts the standard state feedback form:

[0076]

[0077] in, Let be the state feedback gain matrix to be determined. To obtain... Construct the following performance metric function:

[0078]

[0079] in, This represents the performance index of a linear quadratic regulator. This represents the total number of simulation sampling steps. Represents the state weight matrix. This represents the input weight matrix. The matrix is ​​used to characterize the penalty intensity corresponding to each state variable deviating from the target equilibrium state. This is used to characterize the penalty intensity corresponding to the control input amplitude. It can be derived by solving the discrete algebraic Riccati equation. and Obtain the corresponding optimal state feedback gain matrix To ensure that the matrix definition in the claims has a clear implementation basis, in this embodiment, the state weight matrix is... and input weight matrix Set them as diagonal matrices respectively, i.e.:

[0080]

[0081]

[0082] in, to The weights corresponding to the left rudder angle, right wheel angle, vehicle tilt angle, sway bar tilt angle, left wheel angular velocity, right wheel angular velocity, vehicle tilt angle angular velocity, and sway bar angular velocity are respectively. and The weights correspond to the control inputs of the left and right drive motors, respectively. This embodiment does not employ the manual trial-and-error method commonly used in traditional two-wheeled self-balancing vehicles to determine the weights. and , and will not and Instead of performing an unconstrained search as a completely free variable, the search object is subject to targeted constraints based on its structural and execution characteristics, taking into account the second-order balancing robot. Specifically, in this embodiment, the input weight matrix... The input weights are fixed to a preset diagonal matrix, and only the state weight matrix is ​​considered. Optimization is performed. In a specific implementation, the following is chosen:

[0083]

[0084] The reason is that, for the second-order balancing robot targeted by this invention, once the hardware platform, motor drive capability, and control cycle are determined, the input penalty baseline does not need to repeatedly change within an excessively large search space; what truly determines the two-stage balancing coordination capability is the weight distribution relationship between the various state variables. Therefore, this invention focuses its optimization efforts on... Instead of simply extending the problem to a higher-dimensional unconstrained joint search, the weight allocation of the matrix is ​​handled differently. This approach reduces the search dimensionality and strengthens the technical difference between this invention and ordinary PSO parameter tuning or ordinary LQR manual parameter trial schemes.

[0085] Because the second-order balancing robot in this embodiment is symmetrical in its left and right structure and drive configuration, the left and right wheels have the same control meaning in terms of position and velocity. Therefore, if... Treating all eight diagonal elements as independent variables not only increases the search degrees of freedom but also easily leads to asymmetrical weight allocation results lacking physical meaning. Therefore, this embodiment modifies the state weight matrix... Apply symmetry constraints such that: ; ;

[0086] Therefore, the original eight independent parameters to be optimized were reduced to six. To facilitate subsequent processing by the particle swarm optimization algorithm, this embodiment defines the six-dimensional weight vector to be optimized as follows:

[0087]

[0088] in, The common weight of the corresponding left and right wheel position items, Corresponding to the weight of the vehicle body tilt angle, Corresponding weight of the tilt angle of the pendulum. The common weight of the speed terms of the left and right wheels, The corresponding weights for vehicle tilt angle and angular velocity. The weights correspond to the angular velocities of the pendulum. At this point, the state weight matrix... It can be refactored as:

[0089]

[0090] Through the above processing, on the one hand, the key state weights related to the actual dynamics of the second-order balancing robot are preserved, and on the other hand, redundant search degrees of freedom that have no practical control significance under the left-right symmetry structure are explicitly eliminated, so that the optimization problem has obvious object constraint characteristics from the beginning. This is of great significance for distinguishing it from general particle swarm optimization schemes, because this invention does not perform abstract optimization of conventional LQR controllers, but establishes a search space around the specific control object of the second-order balancing robot with bi-level coupling and left-right symmetry.

[0091] Since the reasonable weight range corresponding to different state variables in the state weight matrix usually spans multiple orders of magnitude, directly searching in the linear space can easily lead to problems such as some weights changing too coarsely and others changing too sensitively, thus affecting global search capability and local convergence accuracy. To solve this problem, this embodiment does not directly use linear weight values ​​as particle positions, but defines particle positions in a logarithmic scale search space.

[0092] Let the first The particle in the first The six-dimensional position vector at the next iteration is:

[0093]

[0094] Before calculating fitness, the particle positions are exponentially mapped to restore the actual linear weight values:

[0095]

[0096] in, Indicates the first The particle in the first The decoded result obtained during the iteration is the first... Each actual linear weight value, This represents the natural exponential function. Further, substituting the six decoded linear weight values ​​into the aforementioned reconstruction relation yields the candidate state weight matrix corresponding to the particle:

[0097]

[0098] In this way, the particle swarm optimization algorithm deals with particle positions on a logarithmic scale during the search phase, while using the recovered linear weight matrix during the solution phase of the linear quadratic regulator. This approach ensures the relative balance of the search step size when the weights change across orders of magnitude, which is beneficial for quickly locating state-weight combinations with better control effects over a wide range.

[0099] In this embodiment, the particle swarm optimization algorithm does not simply take minimizing the mathematical cost of the linear quadratic regulator as its sole objective. Instead, it constructs a composite cost function value to address the bi-stage coupling balance, actuator constraints, and stability margin requirements of the second-order balancing robot. The composite cost function value is defined as follows:

[0100] ;

[0101] in, This represents the total composite cost function value. , and These are the weights for the core error term, the control saturation term, and the control stability top term, respectively. The definitions of these three types of sub-items are as follows: First, the core error term... The degree of cumulative attitude deviation used to characterize bi-level equilibrium is defined as follows:

[0102]

[0103] in, Indicates the first The absolute value of the vehicle tilt angle at each sampling time. Indicates the first The absolute value of the pendulum tilt angle at each sampling time. This represents the total number of sampling steps in the simulation. This formula incorporates both the first-level balance error of the vehicle body and the second-level balance error of the pendulum in the evaluation, rather than evaluating only the vehicle body attitude or only the pendulum attitude. Therefore, it can truly reflect the control requirements of a second-order balancing robot that need to consider both levels of attitude recovery. Second, control saturation term. It is used to characterize the degree to which the control input exceeds the execution capability boundary, and is defined as:

[0104]

[0105] in, Indicates the first Each sampling moment corresponds to the motor's control input. This represents the preset output limit determined by the rated output capability of the motor driver. This indicates that a penalty is only incurred when the absolute value of the control input exceeds the preset output limit; otherwise, the penalty is zero. The purpose of this clause is to prevent the optimization process from excessively increasing the feedback gain to simply reduce attitude error, causing the output control quantity to exceed the physical capabilities of the driver, thus making theoretically feasible parameters impractical in engineering. Third, control stability term. The stability margin used to characterize a closed-loop system is defined as follows:

[0106]

[0107] in, Represents the closed-loop state matrix The largest of all eigenvalues, Indicates the stability safety threshold. This represents the stability penalty coefficient. In one specific implementation, it is taken as: In other words, even if the closed-loop poles have not yet crossed the unit circle boundary, as long as their maximum magnitude has approached the unit circle, this embodiment will still apply a penalty, thereby avoiding a situation where the system is theoretically stable but lacks stability margin and is overly sensitive to disturbances and parameter fluctuations. Unlike simply using closed-loop stability as a binary condition for judgment, this invention further incorporates stability margin into a continuous penalty mechanism, making the parameter optimization results more in line with the robustness requirements of actual robot control.

[0108] It should be noted that the core error term, control saturation term, and control stability term are combined to form a composite cost function because second-order balancing robots face the dual constraints of two-stage coupling and limited actuator capabilities. Optimizing only the attitude error can easily result in parameter combinations with excessively large feedback gain and significant control input spikes; considering only the input amplitude constraint can lead to excessively slow attitude recovery speeds for the vehicle body and the lever; and neglecting stability margin can result in closed-loop poles approaching the unit circle boundary excessively. Therefore, the composite cost function in this embodiment is not a simple superposition of common control indices, but rather an evaluation chain established for the specific object of a second-order balancing robot. Its evaluation objectives cover three interrelated aspects: two-stage balancing error, actuator realizability, and closed-loop stability margin. This point is of great significance in supporting the inventiveness of this invention compared to conventional single-stage balancing robot parameter tuning schemes.

[0109] In this embodiment, a particle swarm optimization algorithm is used to optimize the aforementioned six-dimensional weight vector. Let the particle swarm population size be... , No. The particle in the first The position vector at the next iteration is The velocity vector is The particle velocity update formula and position update formula are as follows:

[0110]

[0111]

[0112] in, Indicates the first The particle in the first During the nth iteration 1D velocity components, Indicates the first The particle in the first During the nth iteration Positional components of a dimension, This indicates that the particle has reached the [number]th [number]. The position of the individual's optimal point in the next iteration is at the [number]th [position]. The components of the dimension, This indicates that the entire particle swarm has reached the [number]th [number]. The global optimum position in the next iteration is at the [number]th [location]. The components of the dimension, Represents individual cognitive factors, Represents social cognitive factors, and For interval Random numbers within.

[0113] To balance the global exploration capability in the early stages of the search with the local convergence capability in the later stages, the inertia weight in this embodiment adopts a linearly decreasing strategy:

[0114]

[0115] in, Indicates the first Inertia weights in the next iteration Indicates the initial inertia weight. Indicates the termination of inertia weight. This represents the maximum number of iterations. In one specific implementation, it is taken as:

[0116]

[0117]

[0118]

[0119]

[0120] The specific parameter settings are summarized in Table 1:

[0121]

[0122] During particle swarm initialization, not all particles are generated completely randomly; instead, a hybrid initialization strategy is employed. Specifically, most particles are randomly generated within a pre-defined search space to maintain search breadth. Simultaneously, a set of baseline linear quadratic regulator parameters, tuned empirically, are injected as seed particles into the initial swarm. These baseline linear quadratic regulator parameters refer to a set of initial state weights that enable the robot to maintain a near-upright posture, based on existing experimental experience with second-order balancing robots. Injecting these parameters as seed particles avoids starting the search from a completely ineffective initial region, thereby improving convergence speed and reducing the probability of encountering a large number of meaningless candidate solutions.

[0123] Once a candidate state weight matrix is ​​obtained by decoding a particle, the controller solution module solves for the corresponding state feedback gain matrix based on the candidate state weight matrix and the fixed input weight matrix. If the discrete algebraic Riccati equation has no feasible solution, or if the obtained closed-loop system exhibits divergence in simulation, then the composite cost function value of the particle is directly set to a preset maximum value. In engineering implementation, A fixed constant can be chosen that is much larger than the cost range of a normal feasible solution, for example, tens of times or more the average cost of a feasible solution, to ensure that such invalid parameter combinations are not misjudged as optimal solutions. In this embodiment, it does not rely on a single fixed value, but only requires that it be sufficient to exclude invalid solutions from the optimization candidate set.

[0124] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. A method for optimizing control parameters of a second-order balancing robot, characterized in that, Includes the following steps: S1. Establish a dynamic model for a second-order balancing robot. The second-order balancing robot is a second-order balancing system formed by the vehicle body and the pendulum in series. The pitch of the vehicle body around the axle constitutes the first-level balance, and the hinged swing of the pendulum around the vehicle body constitutes the second-level balance. S2. Based on the dynamic model, linearization is performed near the upright equilibrium point to establish the state space model of the second-order balancing robot. The state space model is then discretized to construct a linear quadratic regulator control framework. The state vector includes the left wheel angle, right wheel angle, vehicle tilt angle, swing arm tilt angle, left wheel angular velocity, right wheel angular velocity, vehicle tilt angle angular velocity, and swing arm angular velocity. The control input includes the left wheel motor input and the right wheel motor input. S3. Construct the state weight matrix and input weight matrix State weight matrix The input weight matrix is ​​a diagonal matrix. It is a diagonal matrix, and the input weight matrix is... The state weight matrix is ​​fixed as a preset input weight diagonal matrix. The diagonal elements are used as parameters to be optimized; S4. Based on the symmetry characteristics of the left and right structures and drive configuration of the second-order balancing robot, apply symmetry constraints to the parameters to be optimized, so that the weight of the left wheel position term is equal to the weight of the right wheel position term, and the weight of the left wheel velocity term is equal to the weight of the right wheel velocity term, so as to form a set of parameters to be optimized after dimensionality reduction. S5. Map the set of parameters to be optimized to the logarithmic scale search space, initialize the particle swarm, and use the position of each particle to represent a set of candidate state weight parameters. S6. Decode the candidate state weight parameters for each particle to recover the actual linear weight values ​​and construct the candidate state weight matrix. and combined with the input weight matrix Solve for the control gain of the corresponding linear quadratic regulator. ; S7, Control Gain The discrete-time closed-loop system corresponding to the state-space model is substituted into the simulation. Based on the vehicle body tilt angle response and the swing arm tilt angle response obtained from the simulation, a composite cost function value is constructed. The composite cost function includes a core error term, a control saturation term, and a control stability term. S8. When the closed-loop system corresponding to the candidate state weight parameter is unstable or the control gain K fails to be solved, the composite cost function value corresponding to the candidate state weight parameter is set to the preset maximum value. S9. Based on the composite cost function value of each particle, update the individual optimal position and global optimal position of the particles, and iteratively update each particle according to the speed update rule and position update rule of the particle swarm optimization algorithm. S10. When the preset termination condition is met, output the optimal state weight matrix corresponding to the globally optimal particle. and the optimal state weight matrix Corresponding control gain , which serves as the optimized control parameters for the second-order balancing robot.

2. The method for optimizing control parameters of a second-order balancing robot according to claim 1, characterized in that, The state vector of the state space model is an eight-dimensional state vector, which sequentially includes the left wheel angle, right wheel angle, vehicle body tilt angle, rocker arm tilt angle, left wheel angular velocity, right wheel angular velocity, vehicle body tilt angle angular velocity, and rocker arm angular velocity; the control input is a two-dimensional control input, including left wheel motor input and right wheel motor input.

3. The method for optimizing control parameters of a second-order balancing robot according to claim 2, characterized in that, The second-order balancing robot is linearized under the following simplification conditions: the wheels are in pure rolling motion with no relative sliding, the vehicle body and the swing arm are considered rigid bodies and elastic deformation is ignored, and the motor inductance is negligible.

4. The method for optimizing control parameters of a second-order balancing robot according to claim 3, characterized in that, The state weight matrix Q is represented as follows: ； The input weight matrix R is represented as follows: ； and the input weight matrix The input weights are fixed as a preset diagonal matrix, and only the state weight matrix is ​​considered. To find the best option.

5. The method for optimizing control parameters of a second-order balancing robot according to claim 4, characterized in that, The state weight matrix The applied symmetry constraints include: the weight of the revolver position term. Weight of the right wheel position item Equal weights for the revolver speed term Weight of the right wheel speed term The original eight independent parameters to be optimized are reduced to six parameters to be optimized.

6. The method for optimizing control parameters of a second-order balancing robot according to claim 5, characterized in that, The set of parameters to be optimized is updated with particle positions in a logarithmic search space, and restored to actual linear weight values ​​through exponential mapping before fitness calculation, in order to construct the candidate state weight matrix. .

7. The method for optimizing control parameters of a second-order balancing robot according to claim 6, characterized in that, The composite cost function is a weighted sum of the core error term, the control saturation term, and the control stability term, used to weight the candidate state matrix. The corresponding control accuracy, execution feasibility, and stability margin are jointly evaluated.

8. The method for optimizing control parameters of a second-order balancing robot according to claim 7, characterized in that, The core error term is obtained by accumulating the sum of the absolute values ​​of the vehicle body tilt angle and the absolute values ​​of the swing arm tilt angle at each sampling time during the discrete time domain simulation. The control saturation term is obtained based on the cumulative square value of the portion of the control input that exceeds the preset output limit. The control stability term is determined based on the relationship between the maximum magnitude of the closed-loop poles and the stability safety threshold. When the maximum magnitude of the closed-loop poles is less than the stability safety threshold, the control stability term is set to zero. When the maximum magnitude of the closed-loop pole is greater than or equal to the stability safety threshold, the control stability term is obtained by combining the square of the difference between the maximum magnitude of the closed-loop pole and the stability safety threshold with a preset stability penalty coefficient.

9. The method for optimizing control parameters of a second-order balancing robot according to claim 8, characterized in that, In step S5, a hybrid initialization strategy is adopted when initializing the particle swarm, so that most particles are randomly generated in the search space, and the baseline linear quadratic regulator parameters tuned based on human experience are injected as seeds into the initial population. In step S9, the inertia weight adopts an update strategy that linearly decreases with the number of iterations, and the individual cognitive factor and the social cognitive factor are both preset values; In step S10, the preset termination condition includes reaching the maximum number of iterations or the composite cost function value being less than a preset reference value.

10. The method for optimizing control parameters of a second-order balancing robot according to claim 9, characterized in that, The input weight matrix Fixed as ; The stability safety threshold is 0.98; The particle swarm has a population size of 100, a maximum number of iterations of 100, an inertia weight that decreases linearly from 0.9 to 0.4, and an individual cognitive factor and a social cognitive factor of 1.5.

Images