Quadruped robot anti-disturbance motion control method based on cost weight adaptive mechanism

Abstract

The invention relates to the technical field of quadruped robot control, and discloses a quadruped robot anti-disturbance motion control method based on a cost weight adaptive mechanism, which comprises the following steps: constructing a quadruped robot state equation based on a single rigid body dynamic model, a model prediction control problem with cost weight, friction cone constraint and gait constraint is formed through discretization; a cost weight self-adaptive mechanism based on disturbance observation is introduced on the basis of a model prediction control framework, the disturbance intensity is estimated by taking the attitude error of the fuselage and the corresponding change rate as disturbance observation values, and the cost weight corresponding to the attitude in the model prediction control problem is dynamically adjusted; by solving a model prediction control problem, an expected ground reaction force and a target state are calculated in real time, and motion control of the quadruped robot is realized in combination with a gait planner and a joint controller. According to the invention, the motion stability and anti-disturbance performance of the quadruped robot under external disturbance are improved based on a cost weight adaptive mechanism of disturbance observation.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of quadruped robot control, and in particular to a quadruped robot anti-disturbance motion control method based on a cost weight adaptive mechanism. BACKGROUND

[0002] With the improvement of mechanical structure design, the improvement of on-board computing power and the breakthrough of robust motion control algorithm, quadruped robots have been applied to intelligent inspection, rescue and fire fighting scenes, and are expected to be applied in more practical scenes to replace humans to do dangerous work. The application scene of the quadruped robot usually has external disturbances, such as unknown external force disturbance, load mutation and dynamic change of terrain. These external disturbances will directly cause the robot attitude to be abnormal, resulting in the robot falling into a posture that is difficult to recover itself, losing the ability to move, and even damaging the hardware. Therefore, developing a quadruped robot anti-disturbance motion control algorithm has a positive significance for improving its stability and safety.

[0003] The conventional quadruped robot motion control algorithm explicitly establishes the dynamics equation and kinematics equation of the quadruped robot, and obtains the optimal motion control input sequence by solving the optimal control problem. In order to improve the anti-disturbance performance of the motion control algorithm, the current method usually starts from improving the expression ability of the dynamics model to the physical properties of the robot, adding a balance control module in addition to the motion control algorithm, and designing a more effective and flexible gait adjustment mechanism. However, the fixed cost weight is used when solving the optimal control problem, and the adaptive weight is not considered to maintain the balance of the robot under disturbance. In addition, an existing method adds a weight adaptive controller based on fuzzy control in the model predictive control framework to reduce the state tracking error under horizontal impact and unstructured scenes. However, the fuzzy operation and fuzzy rules used involve a large number of parameter adjustments, which brings difficulties to the deployment of the algorithm. SUMMARY

[0004] To solve the above technical problems, the present application provides a quadruped robot anti-disturbance motion control method based on a cost weight adaptive mechanism.

[0005] To solve the above technical problems, the present application adopts the following technical solutions:

[0006] A quadruped robot anti-disturbance motion control method based on a cost weight adaptive mechanism, comprising:

[0007] Establish a model predictive control framework: based on a single rigid body dynamics model, a state equation of a quadruped robot is constructed, and after discretization, a model predictive control problem with cost weight, friction cone constraint and gait constraint is formed;

[0008] An adaptive mechanism of cost weight based on disturbance observation is introduced in the framework of model predictive control, which estimates the disturbance intensity according to the attitude error and the corresponding rate of change of the body as disturbance observation values, and dynamically adjusts the cost weight of the attitude in the model predictive control problem;

[0009] By solving the model predictive control problem, the expected ground reaction force and the target state are calculated in real time, and the motion control of the quadruped robot is realized by combining the gait planner and the joint controller.

[0010] In one embodiment, the single rigid body dynamics model is used to construct the state equation of the quadruped robot, and after discretization, a model predictive control problem with cost weight, friction cone constraint and gait constraint is formed, which specifically includes:

[0011] The single rigid body dynamics model of the quadruped robot in the world coordinate system is:

[0012]

[0013] where m is the mass of the robot, represents the acceleration of the robot's center of mass, f i is the ground reaction force at the foot end of the i-th leg of the quadruped robot, is the gravity acceleration vector, I is the moment of inertia of the robot's center of mass, and ω is the body angular velocity, represents the body angular acceleration, r i is the position of the foot end of the i-th leg of the quadruped robot relative to the robot's center of mass, and Θ is the Euler angle representing the rigid body attitude; the Z-Y-X Euler angle is used to represent the rigid body attitude, then ψ, θ, φ are the yaw angle, pitch angle and roll angle, respectively, represents the transpose; T(Θ) is the conversion matrix of the angular velocity ω to the Euler angle change rate ;

[0014] The simultaneous dynamics equations give the continuous-time state equation:

[0015]

[0016] x(t) represents the robot state at time t, represents the time derivative of x(t); A(t) represents the system matrix at time t, and B(t) represents the input matrix at time t, represents the velocity of the robot's center of mass;

[0017] A(t) and B(t) are discretized into A k and B k using zero-order hold and second-order Taylor expansion, and the discrete-time state equation is obtained:

[0018] x k+1 = A k x k + B k u k ;

[0019] x k and u k are the system state and input at the kth discrete time, respectively, and the input is the ground reaction force at the foot, A k represents the system matrix at the kth discrete time, B k represents the input matrix at the kth discrete time;

[0020] According to the discrete-time state equation of the quadruped robot, the following linear model predictive control problem with a prediction time step of N is obtained:

[0021]

[0022] is the system state at the kth time in the reference trajectory, Q and P are both diagonal positive definite weight matrices, and each element on the diagonal is a cost weight, each of which corresponds to a system state or input; k ≤ C k u k ≤ ub k is the friction cone constraint; D k u k = 0 12×1 is the gait constraint; C k represents the friction cone constraint matrix at the kth discrete time, lb k represents the lower bound of the friction cone constraint at the kth discrete time, ub k represents the upper bound of the friction cone constraint at the kth discrete time, D k represents the gait constraint matrix at the kth discrete time.

[0023] In one embodiment, in the continuous-time state equation:

[0024]

[0025]

[0026] represents the transpose, p is the position of the robot's center of mass, g is the acceleration of gravity, and the intermediate variable

[0027] In one embodiment, the attitude error of the fuselage and the corresponding rate of change are used as disturbance observation values to estimate the disturbance strength, and the cost weight corresponding to the attitude in the model predictive control problem is dynamically adjusted, specifically including:

[0028] According to the target body attitude, the target body angular velocity, the actual body attitude and the actual body angular velocity, the roll angle error, the pitch angle error and the corresponding change rates are calculated as disturbance observation values;

[0029] A double-error weight increment mapping function is used to calculate the attitude corresponding cost weight increment in real time according to the roll angle error and the change rate thereof, and the pitch angle error and the change rate thereof; the attitude cost weight increment is added to the initial attitude cost weight to dynamically adjust the cost weight corresponding to the attitude state in the model predictive control problem; the double-error weight increment mapping function can output weight increments of different intensities according to whether the attitude error and the change rate thereof are of the same sign, and make the adjustment amount tend to saturation when the error or the change rate thereof exceeds a preset scaling factor.

[0030] In one embodiment, the calculation of the roll angle error, the pitch angle error and the corresponding change rates as disturbance observation values according to the target body attitude, the target body angular velocity, the actual body attitude and the actual body angular velocity specifically includes:

[0031] e φ = φ d - φ;

[0032] e θ = θ d - θ;

[0033]

[0034] e φ and e θ are the roll angle error and the pitch angle error respectively, and are the roll angle error change rate and the pitch angle error change rate respectively, represents the yaw angle error change rate; v d and θ d are the target body roll angle and the target body pitch angle, v and θ are the actual body roll angle and the actual body pitch angle, ω d is the target body angular velocity, ω is the actual body angular velocity, represents transposition, T(Θ) is the conversion matrix of ω converted into the Euler angle change rate .

[0035] In one embodiment, the double-error weight increment mapping function is used to calculate the attitude corresponding cost weight increment in real time according to the roll angle error and the change rate thereof, and the pitch angle error and the change rate thereof, specifically including:

[0036]

[0037] wherein, ΔQ φ and ΔQQ θ and Q max are the initial cost weight of roll angle φ and pitch angle θ respectively, and Q and Q are the upper bound of cost weight increment. max ,

[0038] In one embodiment, the attitude cost weight increment is added to the initial attitude cost weight to dynamically adjust the cost weight corresponding to the attitude state in the model predictive control problem, specifically comprising:

[0039] The fixed initial cost weight is added to the weight increment to obtain the cost weight corresponding to the attitude for solving the model predictive control problem:

[0040] Q φ = Q φ,0 + ΔQ φ ;

[0041] Q θ = Q θ,0 + ΔQ θ ;

[0042] Q φ,0 , Q θ,0 are the initial cost weight of roll angle φ and pitch angle θ respectively, Q φ , Q θ are the adjusted cost weight of roll angle φ and pitch angle θ respectively.

[0043] Compared with the prior art, the beneficial technical effects of the present application are:

[0044] The present application designs a cost weight adaptive mechanism based on disturbance observation to improve the motion stability and anti-disturbance performance of quadruped robots under external disturbance. Before the linear model predictive controller solves the expected ground reaction force, the cost weight adaptive mechanism calculates the attitude error and its rate of change according to the target attitude and angular velocity and the actual attitude and angular velocity, estimates the disturbance strength and provides the weight increment. Then the linear model predictive controller adds the fixed initial cost weight Q φ,0 , Q θ,0 to the weight increment to obtain the attitude cost weight for planning and solving.

[0045] To improve the anti-disturbance performance, most of the quadruped robot motion control methods solve the optimal control problem (OCP) in the fixed weight problem framework, and do not consider the role of dynamic weight. The existing weight self-adaptive controller based on fuzzy control uses the fuzzy operation and fuzzy rule involving a large number of parameter adjustments, and is difficult to deploy. Compared with the above, the method proposed in the application takes a double-error-weight increment mapping function as the core, involves fewer parameters and the parameters have physical meaning, is prone to deployment, and considers the joint influence of the attitude error and its rate of change on the cost weight, thereby improving the motion stability and anti-disturbance performance of the robot under external disturbance. BRIEF DESCRIPTION OF DRAWINGS

[0046] Figure 1 The system block diagram in the embodiment of the application is shown. DETAILED DESCRIPTION

[0047] A preferred embodiment of the application will be described in detail below with reference to the accompanying drawings.

[0048] The application proposes a quadruped robot motion control method based on cost weight self-adaptive model predictive control to improve the motion stability and anti-disturbance performance of the quadruped robot under external disturbance. The cost weight self-adaptive mechanism based on disturbance observation used in the method takes a double-error-weight increment mapping function as the core and does not involve a large number of parameter adjustments. The cost weight corresponding to the attitude in the model predictive control problem can be dynamically adjusted in real time according to the attitude error and its rate of change, thereby enhancing the attitude balance ability of the robot under abnormal attitude.

[0049] The application designs a quadruped robot anti-disturbance motion control method for external disturbance scenes. The method adds a cost weight self-adaptive mechanism based on disturbance observation on the basis of model predictive control (MPC). The specific technical solutions are as follows:

[0050] 1. Model predictive control

[0051] In the MPC, the quadruped robot is assumed to be a single rigid body regardless of the joint configuration of the leg, and the robot is regarded as a single rigid body. The single rigid body dynamics model of the quadruped robot in the world coordinate system is as follows:

[0052]

[0053] wherein m is the mass of the robot, p is the position of the mass center of the robot, f i is the ground reaction force at the foot end of the ith leg, is the gravity acceleration vector, I is the moment of inertia of the mass center of the robot, ω is the body angular velocity, r i is the position of the foot end of the ith leg relative to the mass center of the robot, Θ is the Euler angle representing the attitude of the rigid body. The attitude of the rigid body is represented by the Z-Y-X Euler angle, and then ψ, θ, φ are yaw, pitch, roll, respectively. T(Θ) is the transformation matrix from angular velocity ω to the rate of change of Euler angles

[0054] The simultaneous dynamics equations give the continuous-time state equation:

[0055]

[0056] where,

[0057]

[0058]

[0059] where, g is the acceleration of gravity.

[0060] A(t) and B(t) are discretized into A k and B k using zero-order hold (ZOH) and second-order Taylor expansion, respectively. The discrete-time state equation is given as:

[0061] x k+1 = A k x k + B k u k ;

[0062] According to the discrete-time state equation of the quadruped robot, the following linear MPC problem with prediction time step N is obtained:

[0063]

[0064] where, x k and u k are the system state and input at the kth discrete time, respectively. x0 is determined by the current state of the robot, and the rest of x k and u k are the predicted system state and input (ground reaction force). x k is the system state at the kth discrete time in the reference trajectory, which is calculated from the velocity command. Q and P are both diagonal positive definite weight matrices, and each element on the diagonal is a cost weight, which corresponds to a state or output. The numerical value of the cost weight represents the weight of the corresponding state or output in the optimization objective. The constraints lb k ≤ C k u k ≤ ub k are the friction cone constraints to prevent foot slipping. The constraints Du k ux = 0 12×1 As a gait constraint, the ground reaction force of the swing leg at the kth discrete time is restricted to be zero. The linear model predictive controller is then solved to obtain the desired ground reaction force u0and the target state x1= x d .

[0065] In addition to the linear model predictive controller, a gait planner and a joint controller are needed to realize the motion control of the quadruped robot. The gait planner is responsible for planning the gait timing, i.e., the relative relationship in time between the support motion and swing motion of the quadruped structure. The joint controller executes the support control or swing control according to the phase of the leg. The support control converts the ground reaction force into the required joint torque; the swing control calculates the joint torque required to track the swing trajectory of the foot.

[0066] 2. Disturbance-observed-based adaptive mechanism for cost weight

[0067] The disturbance-observed-based adaptive mechanism for cost weight estimates the disturbance strength according to the attitude error and its rate of change of the body as disturbance observation values, and adjusts the cost weight corresponding to the attitude in the weight matrix Q. The core of this mechanism is the following double-error-weight increment mapping function:

[0068]

[0069] where ΔQ φ and ΔQ θ are the cost weight increments corresponding to the roll angle φ and the pitch angle θ, respectively, and are the upper bounds of the increments. Δφ max , Δθ max and are the scaling factors. e φ and e θ are the roll angle error and the pitch angle error, respectively, and are the roll angle error rate and the pitch angle error rate, respectively, which are calculated as follows:

[0070] e φ = φ d - φ;

[0071] e θ = θ d - θ;

[0072]

[0073] where the superscript d represents the component in the target state x d output by the above linear model predictive controller. φd and θ d Let ω be the target fuselage roll and pitch angles, and φ and θ be the actual fuselage roll and pitch angles. d Let ω be the target fuselage angular velocity, and ω be the actual fuselage angular velocity.

[0074] Cost-weighted adaptive model predictive control system, such as Figure 1 As shown, the system consists of five main parts: user commands, a linear model predictive controller, a joint controller and gait planner, a cost-weight adaptive mechanism, and a robot that executes joint commands. Before the linear model predictive controller solves for the optimal input sequence, the cost-weight adaptive module determines the target body posture. Target fuselage angular velocity ω d Actual fuselage attitude The attitude error e is calculated from the actual fuselage angular velocity ω. φ e θ and attitude error change rate This is used to estimate the disturbance strength and provide the cost weight increment ΔQ. φ and ΔQ θ Subsequently, the linear model predicts the controller to use a fixed initial cost weight Q. φ,0 Q θ,0 Adding this to the weight increment yields the attitude cost weights used for planning and solving:

[0075] Q φ =Q φ,0 +ΔQ φ ;

[0076] Q θ =Q θ,0 +ΔQ θ ;

[0077] Except Q φ and Q θ Other cost weights are not adjusted and remain fixed at the initial cost weights.

[0078] The parameters that need to be adjusted in the cost weight adaptive mechanism based on perturbation observation designed in this invention are: Δφ max , Δθ max and To facilitate parameter adjustments before method deployment, the specific physical meaning of each parameter will be explained below.

[0079] The range of the hyperbolic tangent function tanh(·) is (-1, 1), and therefore, according to the above double error-weighted increment function, we can obtain: therefore, The cost weight increment ΔQφ , ΔQ θ .

[0080] The range of the function |tanh(·)| is [0, 1). When |e φ | ≥ Δφ max , ΔQ φ is calculated as

[0081]

[0082] That is, e φ corresponds to the saturated adjustment amount . Therefore, the parameter Δφ max can be regarded as the minimum roll angle error that makes e φ correspond to the saturated adjustment amount. Similarly, Δθ max and also have the same physical meaning.

[0083] Meanwhile, the designed double-error-weight increment mapping function has the following key characteristics: taking ΔQ φ as an example, when e φ and are of the same sign, the system is in a state of continuous increase of roll angle error, and the function outputs a larger ΔQ φ , realizing stronger intervention; when e φ and are of different signs, the system is in a state of continuous decrease of roll angle error, and the function outputs a smaller ΔQ φ , realizing weaker intervention. The above two different response modes show that the double-error-weight increment mapping function can provide appropriate weight increments for different roll angle errors e φ and their change rates , realizing the joint influence of the attitude error e φ and its change rate on the cost weight Q φ .

[0084] The terms used herein are merely used to describe specific embodiments, and are not intended to limit the present application. The terms "include", "contain" and the like used herein indicate the existence of the described features, steps, operations and / or components, but do not exclude the existence or addition of one or more other features, steps, operations or components.

[0085] The technical features of the above embodiments can be combined in any manner. To make the description concise, not all possible combinations of the technical features in the above embodiments are described, but as long as the combinations of the technical features do not contradict, they should be considered as within the scope of the present application.

[0086] It will be obvious to a person skilled in the art that the application is not limited to the details of the foregoing exemplary embodiments and can be implemented in other concrete forms without departing from the spirit or essential characteristics of the application. The embodiments are therefore to be considered in all respects as illustrative and not restrictive, the scope of the application being indicated by the appended claims rather than by the foregoing description, and all changes which come within the meaning and range of equivalency of the claims are therefore intended to be embraced therein and no

[0087] Furthermore, it should be understood that although the description is made on the basis of embodiments, not every embodiment contains only one independent technical solution, and the description is made in this way only for the sake of clarity, and a person skilled in the art should consider the description as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by a person skilled in the art.

Claims

1. A disturbance-resistant motion control method for a quadruped robot based on a cost-weight adaptive mechanism, characterized in that, include: Establish a model predictive control framework: Construct the state equations of a quadruped robot based on a single rigid body dynamics model, and then discretize them to form a model predictive control problem with cost weights, friction cone constraints and gait constraints; Based on the model predictive control framework, a cost weight adaptive mechanism based on disturbance observation is introduced. The attitude error of the fuselage and the corresponding rate of change are used as disturbance observation values ​​to estimate the disturbance intensity and dynamically adjust the cost weight corresponding to the attitude in the model predictive control problem. By solving the model predictive control problem, the desired ground reaction force and target state are calculated in real time, and the motion control of the quadruped robot is realized by combining a gait planner and a joint controller.

2. The method for disturbance-resistant motion control of a quadruped robot based on a cost-weight adaptive mechanism according to claim 1, characterized in that, The process involves constructing the state equations of a quadruped robot based on a single rigid body dynamics model, and then discretizing them to form a model predictive control problem with cost weights, friction cone constraints, and gait constraints. Specifically, this includes: Treating the robot as a single rigid body, the single rigid body dynamics model of the quadruped robot in the world coordinate system is as follows: Where m is the robot's mass. f represents the acceleration of the robot's center of mass. i Let be the ground reaction force acting on the tip of the i-th leg of the quadruped robot. Let I be the gravitational acceleration vector, I be the moment of inertia of the robot's center of mass, and ω be the angular velocity of the robot body. The angular acceleration of the fuselage, r i Let θ be the position of the tip of the i-th leg of the quadruped robot relative to the robot's center of mass, and Θ be the Euler angle representing the rigid body's attitude. Using ZYX Euler angles to represent the rigid body attitude, then... ψ, θ, and φ represent the yaw angle, pitch angle, and roll angle, respectively. Indicates transpose; T(Θ) is the rate of change of angular velocity ω converted to Euler angles. The transformation matrix; Combining the dynamic equations, we obtain the continuous-time state equations: x(t) represents the robot's state at time t. Let x(t) represent the time derivative of x(t); A(t) represent the system matrix at time t; and B(t) represent the input matrix at time t. The velocity of the robot's center of mass; Discretize A(t) and B(t) using zero-order preservation and second-order Taylor expansion. k and B k This leads to the discrete-time state equation: x k+1 =A k x k +B k u k ; x k and u k These represent the system state and input at the k-th discrete time, respectively, with the input being the ground reaction force at the foot, A. k B represents the system matrix at the k-th discrete time point. k This represents the input matrix at the k-th discrete time step; Based on the discrete-time state equations of the quadruped robot, the following linear model predictive control problem with N prediction time steps is obtained: For the system state at the k-th discrete time in the reference trajectory, Q and P are both diagonal positive definite weight matrices, and each element on the diagonal is a cost weight, with each cost weight corresponding to a system state or input; lb k ≤C k u k ≤ub k For friction cone constraint; D k u k =0 12×1 For gait constraints; C k Let lb represent the friction cone constraint matrix at the k-th discrete time. k Let ub represent the lower bound of the friction cone constraint at the Kth discrete time. k D represents the upper bound of the friction cone constraint at the Kth discrete time. k Let represent the gait constraint matrix at the k-th discrete time.

3. The method for disturbance-resistant motion control of a quadruped robot based on a cost-weight adaptive mechanism according to claim 2, characterized in that, In the continuous-time state equation: This represents the transpose, where p is the position of the robot's center of mass, g is the acceleration due to gravity, and the intermediate variable is...

4. The method for disturbance-resistant motion control of a quadruped robot based on a cost-weight adaptive mechanism according to claim 1, characterized in that, The method of using the attitude error of the fuselage and its corresponding rate of change as disturbance observations to estimate the disturbance intensity and dynamically adjust the cost weights corresponding to the attitude in the model predictive control problem specifically includes: Based on the target fuselage attitude, target fuselage angular velocity, actual fuselage attitude, and actual fuselage angular velocity, calculate the roll angle error, pitch angle error, and corresponding rate of change as disturbance observation values; Using a dual-error weight increment mapping function, the cost weight increment corresponding to the attitude is calculated in real time based on the roll angle error and its rate of change, and the pitch angle error and its rate of change. The attitude cost weight increment is added to the initial attitude cost weight to dynamically adjust the cost weight corresponding to the attitude state in the model predictive control problem. The dual-error weight increment mapping function can output weight increments of different intensities based on whether the attitude error and its rate of change have the same sign, and the adjustment amount tends to saturate when the error or its rate of change exceeds a preset scaling factor.

5. The method for disturbance-resistant motion control of a quadruped robot based on a cost-weight adaptive mechanism according to claim 4, characterized in that, The calculation of roll angle error, pitch angle error, and corresponding rates of change as disturbance observations based on the target fuselage attitude, target fuselage angular velocity, actual fuselage attitude, and actual fuselage angular velocity specifically includes: e φ =φ d -f; e θ =θ d -θ; e φ and e θ These are roll angle error and pitch angle error, respectively. and These are the rates of change of roll angle error and the rates of change of pitch angle error, respectively. φ represents the rate of change of yaw angle error. d and θ d φ and θ represent the target fuselage roll and pitch angles, respectively, while ω represents the actual fuselage roll and pitch angles. d Let ω be the target fuselage angular velocity, and ω be the actual fuselage angular velocity. This indicates the transpose, and T(Θ) is the rate of change of ω in Euler angles. The transformation matrix.

6. The method for disturbance-resistant motion control of a quadruped robot based on a cost-weight adaptive mechanism according to claim 5, characterized in that, The method of using a dual-error weight increment mapping function to calculate the cost weight increment corresponding to the attitude in real time based on the roll angle error and its rate of change, and the pitch angle error and its rate of change, specifically includes: Where, ΔQ φ and ΔQ θ These represent the cost weight increments corresponding to the roll angle φ and pitch angle θ, respectively. and As the upper bound of the cost weight increment, Δφ max , Δθ max and All are scaling factors.

7. The method for disturbance-resistant motion control of a quadruped robot based on a cost-weight adaptive mechanism according to claim 6, characterized in that, The step of adding the incremental attitude cost weight to the initial attitude cost weight to dynamically adjust the cost weight corresponding to the attitude state in the model predictive control problem specifically includes: Adding the fixed initial cost weights to the weight increments yields the cost weights corresponding to the attitude when solving the model predictive control problem: Q φ =Q φ,0 +ΔQ φ ; Q θ =Q θ,0 +ΔQ θ ; Q φ,0 Q θ,0 These are the initial cost weights corresponding to the roll angle φ and pitch angle θ, respectively, Q φ Q θ These are the adjusted cost weights corresponding to the roll angle φ and pitch angle θ, respectively.

Images