Modeling and LQR control of insect sized flapping wing robot

Abstract

Flying insects perform agile maneuvers like backflips, sharp turns, and collision recovery. Replicating these in sub-gram flying insect robots (FIRs) requires fast and responsive control systems. Current FIRs rely on elaborations of proportional-integral-derivative (PID)-type feedback control, requiring painstaking tuning and task-specific adjustments for complex maneuvers. Optimal control offers a solution but has been limited by approximate models or computational demands unsuitable for onboard implementation. Here, we used a more accurate stroke-averaged model of forces and torques, derived from a sensitive two-axis torque sensor, to implement the first demonstration of optimal control on an FIR that is computationally efficient enough to be performed by a microprocessor carried onboard. Applied to the 150 mg UW RoboFly, this enabled stable hovering (RMS error 2.5 cm) and trajectory tracking at speeds up to 25 cm/s using a linear quadratic regulator (LQR). These results were enabled by a more accurate model and laid the groundwork for integrating low-power receding-horizon control to achieve aggressive maneuvers in FIRs.

Introduction

Research in flapping wing insect-sized robots (FIRs) is motivated by their potential applications. These robots are small in size and are in principle less expensive to manufacture in large numbers, which makes them suitable for applications like detecting gas leaks, looking for survivors in disaster-prone areas, automated farm monitoring, running inspections on manufacturing lines, and weather monitoring. While still tethered and limited to operation inside a lab environment, recent advances in tiny sensors and microcontrollers have brought them one step closer to achieving power¹ and sensor² autonomy.

Controlling FIRs presents significant challenges due to their highly nonlinear dynamics, manufacturing inconsistencies resulting in variability between robots, rapid wear and tear, and a high torque to moment of inertia ratio, approximately 10³ rad/sec², which leads to extremely fast dynamics.

Current state-of-the-art controllers for FIRs primarily utilize adaptive PID flight control systems for hovering³. Despite their widespread use, these controllers require substantial ad-hoc tuning and are task-specific, often failing to consider actuator, state, and environmental constraints. Maneuvering beyond basic linear responses, such as perching⁴ and somersault⁵, requires a sliding mode controller combined with iterative learning of trajectory parameters. However, the parameters derived to perform an aggressive perch in⁴ are specific to that task and cannot be applied to any other task. More recently,⁶ demonstrated a quad-wing design with a PID controller, achieving a 1 cm RMS hovering error. This design, inherently more stable, features four actuators controlling a six-degree-of-freedom (DOF) system, ensuring balanced roll and pitch torque generation. As a result, it offers greater precision, stability, and robustness in flight.

Optimal control strategies like modular Model Predictive Control (MPC)⁷ combine high-level MPC with a low-level controller for torque management, enabling operation beyond hovering. However, these systems have not been demonstrated on actual hardware for maneuvers beyond hovering. Additionally, data-driven MPC approaches like Tube-MPC⁸ show promise for optimizing under actuator constraints and trajectory tracking for complex maneuvers such as ramps and infinity loops but remain too computationally intensive for implementation on sub-150 mg robots. Microprocessors small enough to be carried onboard, such as the 10 mg, 120 MHz STM32F4 used in the first wireless liftoff of an FIR, the UW RoboFly in¹, are capable of floating-point math operations. Nevertheless, their performance will be limited to a fraction of desktop capabilities, just a few hundred MHz, for the foreseeable future.

The PID controller used in⁹ is tuned based on the robot’s dynamics. This enables the robot to hover and follow trajectories, such as an infinity loop. Our approach to controlling FIRs is based on the premise that an accurate model eliminates the need for laborious and unsatisfactory hand-tuning of PID gains. By framing the control problem within an optimal control framework, we can design performance to maximize metrics like power efficiency or completion time. Using this model, we compute optimal gains around a fixed point for control. This was first demonstrated in¹⁰, where the performance of PID and LQR controllers were compared on quadrotors with the target to stabilizing the orientation angles. The LQR controller demonstrated faster response in reaching the reference signal from high initial angles. In this work, we utilize an LQR controller based on the dynamic model of the robot, effectively avoiding the labor-intensive and crash-prone tuning process associated with PID controllers. The recent development of low-cost and accurate torque measurement device¹¹ has improved our ability to map the torque characteristics of these robots accurately. Furthermore, innovations such as pre-stacked actuators¹² and standardized manufacturing processes¹³ have reduced variability in actuator performance and robot construction. In this work, we have made the following contributions:

• 1. For the first time, we developed and validated a stroke-averaged first-principle model by comparing it with high-speed trajectories collected from a sub-150 mg robot.

• 2. To the best of our knowledge, we present the first LQR implementation of controlled flight on an FIR.

This control strategy is computationally efficient, requiring only a small number of multiply-accumulate operations and trigonometric calculations per control step (see the Supplementary Materials). This efficiency makes it suitable for integration on compact microcontrollers, such as the STM32F4, which are ideal for sub-150 mg robots. In our implementation, the LQR gain is computed only once, effectively creating a PD controller with gains derived from the theoretical model of the robot and the specified Q and R matrices. The key innovation lies in obtaining these LQR gains solely from the theoretical model, thereby eliminating the need for labor-intensive, ad-hoc tuning.

We anticipate that our model and LQR controller will serve as key components in fast onboard receding horizon optimal controllers, such as those described in refs. ^14,15, capable of optimizing under actuator limits and state constraints. These controllers could enable more aggressive maneuvers by exploiting the robot’s high torque-to-inertia ratios, adapting control gains for higher translational speeds and larger attitude angles.

Results

RoboFly

RoboFly (shown in Fig. 1) is a flapping-wing robot that weighs 150 mg. The robot features two piezoelectric actuators as muscles to flap its wings. These actuators are linked to a transmission mechanism that amplifies the actuators’ displacement of approximately 200 μm to a wing motion of about 60°. RoboFly has the capability to carry a payload up to 1.5 times its own weight and is powered via a wire-tether, which comprises four wires transmitting signals to operate the actuators.

Fig. 1: RoboFly, an insect-sized flapping robot weighing 146 milligrams, hovers next to a flower using feedback from motion capture cameras.

The robot performs this hovering maneuver using the LQR controller reported in this work.

RoboFly, like other piezo-actuated flapping wing robots^16,17, is operated by low-power 180-volt sinusoidal signals. It can generate roll torque, pitch torque, and thrust almost independently¹⁸. The analog voltage signal is generated using the equation,

$${V}_{signal}=\frac{A+\delta A}{2}\sin \omega t+\frac{{V}_{bias}}{2}+\frac{{V}_{o}}{2}$$

(1)

Here, V_bias is a constant bias signal voltage of 250-volt supplied to the top layer of the actuator. As shown in Fig. 2a, increasing the amplitude (A) of the sinusoidal signal increases the wing flapping amplitude, thereby generating greater thrust. Creating an amplitude differential (δA) between the wings, shown in Fig. 2b, increases thrust on one side while decreasing it on the other, which produces roll torque. Pitch torque (Fig. 2c) is generated by adjusting the wing flapping either forward or backward relative to the robot’s body through a voltage offset (V_o) applied to the sinusoidal signal.

Fig. 2: Torque and thrust generation mechanism in FIRs.

(Top) Inspired by the work in¹¹, this figure shows that changing the signal parameters δA and V_o introduces roll and pitch torques, respectively. Here, V_bias is the bias voltage. (Bottom) Mapping of (a) thrust, (b) roll torque, and (c) pitch torque of the RoboFly used in this work. The thrust mapping is obtained using a high-precision scale, while the torque mappings are obtained using the torque measurement device introduced in ref. ¹¹. Pink dots represent the collected data points, and the green line represents the linear fit of the data. The corresponding equations for these linear fits are provided in Table 1.

Under the “Methods” section (“Theoretical Model”), we present the theoretical model of the robot used in this study, along with the measured and calculated parameters used in the dynamics. Section “Trajectory Data” outlines the trajectory data collection process and provides visualizations of the free flight data. Finally, Section “Model Validation” compares the free flight data to the theoretical model.

Model validation

For simplicity, the model we used in this work excludes damping and drag components, thus setting the force and moment vectors ${[{f}_{{a}_{1}},{f}_{{a}_{2}},{f}_{{a}_{3}}]}^{T}$ and [L, M, N]^T to zero in equations ((2)-(7)). We validated the theoretical model accelerations by comparing them with measured acceleration data from flight tests, as depicted in Fig. 3. This comparison includes seven separate flight trajectories stacked in time.

Fig. 3: Model validation plots.

Measured accelerations (green) from the RoboFly trajectories plotted with the predicted accelerations (pink) calculated using the theoretical model.

The model can predict translational accelerations in the body coordinate system, represented by ${[\dot{u},\dot{v},\dot{w}]}^{T}$, with the root mean squared (L²) error of 53.4 m/s², 56.9 m/s², and 36.7 m/s², respectively. The errors in rotational accelerations, ${[\dot{p},\dot{q}]}^{T}$, are 9.2e³ rad/s² and 7.6e³ rad/s², respectively. The more substantial errors in the rotational domain can largely be attributed to the aforementioned aspect of small flight vehicles that their angular accelerations are large.

We believe and our results show that the model is still adequate for controller design purposes. This is based on two key considerations: Firstly, the actuation delay for the rotational system is minimal, as rotational acceleration occurs almost instantaneously once torque is applied. Secondly, the robot will eventually have a gyroscope onboard which is capable of providing rapid rotational velocity feedback at 1 to 16 kHz, which will significantly enhance the ability to perform rapid feedback corrections. Thus, even with its limitations, this model provides a sufficient foundation for developing an effective controller.

Controller implementation

In our experiments, we did not control the yaw rotation of the robot. Hence the LQR was designed to optimize the dynamics in the body coordinate system, which are independent of the yaw rotation. The state vector of the robot is defined by $\sigma ={[{d}_{x},{d}_{y},{d}_{z},u,v,w,\phi ,\theta ,p,q]}^{T}$. Here, $d={[{d}_{x},{d}_{y},{d}_{z}]}^{T}$ is the position in body coordinates and ${V}_{b}=\dot{d}=[u,v,w]$. The controller assumes that the angular velocity about z_b, denoted as r, which arises from manufacturing uncertainties, remains constant. Its value affects fictitious forces and torques in body coordinates that appear in equations (2)–(7). The controller calculates the inputs u* = [A, δA, V_o], which directly controls the acceleration in z_b, torque about x_b, and torque about y_b axis. The Q matrix used in our experiments is a diagonal matrix, Q = diag([0.02, 0.02, 0.01, 0.1, 0.1, 0.1, 1, 1, 4, 4]); R is also a diagonal matrix, R = diag([2, 1, 1]). The ratio of Q and R matrix used here was obtained with the knowledge of our model and only one experimental flight. The feedback loop includes a motion capture system that provides state feedback in terms of the robot’s position and orientation (expressed as quaternions). This data feeds into a Simulink real-time system in which the body angular velocities, Euler angles, and velocity in world frame are calculated. The controller receives the desired position in world coordinates. Finally, the resulting error in the world coordinates is converted to the body coordinates and is multiplied by the precalculated LQR gain matrix k to determine the control inputs, u = −k(σ_des − σ), for the robot. The control loop used for our experiments is shown in Fig. 4a.

Fig. 4: LQR control loop and hovering trajectories.

a The LQR control loop used to perform hovering and trajectory tracking maneuvers. Here, R represents the 3-2-1 rotation matrix. b The plot displays five different hovering trajectories for 2 s flight time and four different hovering trajectories for 7 s flight time, showing the robot maintaining a stable attitude and remaining close to the starting position.

Hovering

The initial task was to hover around a desired position. We did nine such flights, five flights lasting 2 s, and four flights lasting 7 s, and the robot managed to hover using the precalculated LQR gain, with RMS errors of 2.5 cm, 2.4 cm, 2.6 cm, 2.8 cm, and 2.5 cm for 2 s flights, and RMS errors of 2.7 cm, 2.3 cm, 2.52 cm, and 3.04 cm for the 7 s flights. For hovering tasks, a PID controller, as referred to in refs. ^16,19, outperforms this with an average RMS error of 2 cm. We think this is due to the fact that PID controllers are manually tuned for specific tasks. The LQR controller used here does not include any model for the flapping wing aerodynamics, which makes it less precise. However, since the LQR controller used here was only tuned once to determine the Q and R matrices, it takes less flight time to tune and implement this controller. Five 2 s hover trajectories and four 7 s hover trajectories are shown in Fig. 4b. Position and attitude plots over time are provided in the Supplementary materials.

Response to external disturbance

To avoid crashes during our experiments, we suspend the robot using a lightweight Kevlar thread. There is slack in the Kevlar thread in all our experiment videos, which demonstrates that the thread does not exert any force on the robot. However, in this particular experiment, we intentionally applied force by pulling the robot with the Kevlar thread to test the controller’s response to external disturbances. This causes the accelerations >1g. As shown in Fig. 5a, the robot was able to stabilize itself and fly toward the desired position. The controller response to external disturbance is a roll torque of approximately -4 μNm and pitch torque of 1 μNm, as can be seen in the data attached in the Supplementary materials.

Fig. 5: Results of LQR implementation on the RoboFly.

a Response to the external disturbance applied via Kevlar thread is shown in the photo composite. The robot does a recovery maneuver to get back to the stable attitude and ultimately flies to the desired position. b The photo composite shows the RoboFly tracking a 10 cm radius circular trajectory over a 4.5 s flight using a constant pre-calculated LQR gain. c The desired trajectory, depicted in green, was provided to the controller in the form of position and velocity set points while magenta and purple represent the two actual trajectories followed by the robot. The RMS errors for these two trajectories are 2.02 cm and 2.1 cm. The video link of the experiments is shown in the abstract page.

Trajectory tracking

Here, the robot was tasked with following a pre-computed circular trajectory of 10 cm radius in 4.5 s flight time. The robot was able to do two such trajectories with RMS errors of 2.02 cm and 2.1 cm in x-y position tracking. The photo composite of the maneuver is shown in Fig. 5b. The desired waypoints on the trajectory were given in the form of set points of position and velocity. The controller used the same Q and R matrices as the hovering maneuver. For comparison, in ref. ⁸ authors tracked a 5 cm radius circular trajectory with the reported x-y position error of 1.8 cm. Our higher position tracking error is likely due to the much higher flight velocity (25 cm/s vs. a maximum speed of 5.2 cm/s in ref. ⁸) and the larger circular trajectory, which would result in larger disturbances from the wire tether. Position and attitude plots over time are provided in the Supplementary materials.

Discussion

This study developed and validated a theoretical model for the UW RoboFly and implemented an infinite-horizon Linear Quadratic Regulator (LQR) control strategy. This enabled stable hovering, recovery maneuvers, and trajectory tracking using pre-calculated LQR gains. A single constant gain was used in all experiments. Unlike PD controllers, which require time-intensive manual tuning, the LQR controller is more suitable for insect-sized flying robots with limited flight time. LQR uses the full-state dynamics and a cost function to compute the optimal control gain. The cost function uses Q and R matrices, which define the relative precision required for each state. Thus, LQR is heuristically easier to use when a relatively accurate model is given.

Although the LQR controller showed higher RMS position errors than recent studies, this was likely due to the absence of a detailed flapping-wing aerodynamic model, which impacted precision. However, the simplicity of the LQR approach significantly reduced implementation and testing time.

This work provides a foundation for some important next steps. First, the single LQR gain could be replaced with one that is “gain-scheduled” for linearizations about different states, such as forward flight or flight into the wind. This would also impose a minimal computational load on any conceivable microcontroller. Second, collecting data over a broader range of flight conditions could improve the accuracy of the model and the performance of the controller. Third, it is straightforward to augment the LQR controller with a Kalman filter and form an LQG controller, and provide an architecture to support sensor autonomy². Fourth, our current controller does not perform any sort of adaptivity. Equations (2)–(4) assume that the thrust vector is nearly vertical. However, that is not always the case. If the thrust vector is not vertical, it results in horizontal forces, which cause the robot to overshoot the desired set point. This is dependent on the robot and can change by the definition of rigid body in a MoCap arena. Future models could account for the offset between the thrust vector and the body-z axis and estimate the drag model using the discrepancy between the expected and actual translational velocity as input^16,19. Such refinements are expected to yield even lower RMS errors for specific tasks and potentially enable more agile maneuvers.

While gain scheduling could enhance controller performance across a wider range of conditions, it does not consider the actuator output limits inherent to flying insect robots. Advanced control techniques, known as Receding Horizon Control (RHC) or Model Predictive Control (MPC), can accurately factor those in the optimization process. Although ref. ⁸ applied this technique, their controllers were too demanding for a microcontroller. We expect that the advances used in Tiny-MPC¹⁴ to perform RHC on the 120 MHz microcontroller onboard the 30 g Crazyflie helicopter could be adapted to the different dynamics and constraints of the RoboFly. Taking into account actuator constraints and any improved system characterization, more precise and aggressive agile maneuvers should be possible without hand-tuning.

Methods

Theoretical model

The dynamics of a flapping wing robot of the size of RoboFly can be defined by the same first principle model as a quadrotor⁹. Using the convention of (X, Y, Z) as coordinates in an inertial frame and (x_b, y_b, z_b) as coordinates in the body frame, first-order differential equations of the body velocity V_b = [u, v, w]^T of the robot is defined by

$$\dot{u}=g\sin \theta +{f}_{{a}_{1}}-(qw-rv)$$

(2)

$$\dot{v}=-g\cos \theta \sin \phi +{f}_{{a}_{2}}-(rv-pw)$$

(3)

$$\dot{w}=-g\cos \theta \cos \phi +{f}_{{a}_{3}}-(pv-qu)+\frac{\Gamma }{m+{m}_{M}}$$

(4)

Here, ω_b = [p, q, r]^T is the body angular velocity, g is the acceleration due to gravity, m is the mass of the robot and, m_M is the mass of the MoCap markers. Rotation is defined by the widely used 321 rotation matrix. The 321 rotation follows the yaw (ψ) → pitch (θ) → roll (ϕ) sequence. Γ is the thrust applied to the robot in the positive z_b direction. ${F}_{a}={[\,{f}_{{a}_{1}},{f}_{{a}_{2}},{f}_{{a}_{3}}]}^{T}$ is the unmodeled dynamic force, which includes stroke averaged aerodynamic force in body frame caused by the drag due to flapping wings. Similarly, rotational dynamics can be defined by the first-order differential equation in body angular velocity (ω_b = [p, q, r]^T).

$$\dot{p}=L+\frac{{\tau }_{r}}{{J}_{xx}}-\frac{{J}_{zz}-{J}_{yy}}{{J}_{xx}}qr$$

(5)

$$\dot{q}=M+\frac{{\tau }_{p}}{{J}_{yy}}-\frac{{J}_{xx}-{J}_{zz}}{{J}_{yy}}rp$$

(6)

$$\dot{r}=N-\frac{{J}_{yy}-{J}_{xx}}{{J}_{zz}}pq$$

(7)

Here, J = diag([J_xx, J_yy, J_zz]) is the diagonal moment of inertia matrix of the robot. ${\tau }_{c}={[{\tau }_{r},{\tau }_{p},0]}^{T}$ is defined as the input torque vector comprising of roll torque (τ_r) about x_b and pitch torque (τ_p) about y_b. τ_a = [L, M, N]^T is the unmodeled dynamic moment, which also includes averaged aerodynamic torque caused by the drag due to flapping wings.

Model parameters

We precisely measured the robot’s parameters using a sensitive, custom-build force torque sensor. Results are tabulated in Table 1. The mass of the robot was determined using a high-precision balance with a resolution of 0. 1 mg. To estimate the robot’s moment of inertia, we measured the mass of its various components, including the piezoelectric actuators, wings, airframes, and motion capture markers. These measurements were then input into a CAD model to calculate the robot’s moment of inertia matrix. The high-precision scale also facilitated in the calculation of thrust mapping, where the thrust generated by the robot at specific flapping amplitudes (A) was recorded. During the experiment, the wings were flapped for a period of 1 s at the constant frequency of 180 Hz. The scale can take accurate stroke-averaged measurements of thrust. The test setup also makes sure that the robot is away from any surrounding objects to avoid ground effects. We employed a least squares fit method to model the thrust based on this data (Fig. 2-left). The learned linear fit of the thrust mapping is shown in Table 1. Given that the lifetime of these robots is about 10 min²⁰, we aimed to minimize the total operating time to prevent mechanical fatigue. Therefore, for the mapping of thrust and torques, we took only two or three measurements to establish a trendline that can be used in the model.

Table 1 Measured parameters of RoboFly

To measure the torque response to different voltage inputs, that is, torque mapping, we used a device similar to the one in ref. ¹⁸. By applying inputs V_o and δV, we generate roll and pitch torques, respectively. These torques induced angular deflections on the device, which are linearly correlated with the applied torques, which was observed in practice in ref. ¹⁸. These deflections were accurately measured using the motion capture system, allowing us to map torques effectively (Fig. 2-middle and right). This comprehensive approach to parameter measurement ensures a robust foundation for our model.

Trajectory data

We collected 8 s of trajectory data from 7 separate flights with wings flapping at a frequency of 180 Hz, controlled by a PID flight controller ¹³. To capture flight perturbations, we set the desired points away from the robot’s initial position, focusing on collecting more data with perturbations in lateral, longitudinal, and vertical dynamics. To avoid capturing too much stable hovering data, most flight trajectories were shortened to the duration required for the robot to reach the set positions. The robot was equipped with four retro-reflective markers to track its position and orientation through a motion capture system comprised of four Prime 13 cameras by OptiTrak, Inc., Salem, OR. Position and quaternions from the motion capture system running at 240 Hz were used to calculate V_b, ω_b, and Euler angles of the robot offline.

Body offset

A critical difference between the robot’s trajectory data and the modeled dynamics from equations (2)–(7) can stem from the misalignment between the body z-axis, as defined in the motion capture software, and the robot’s thrust vector. The dynamics, detailed in equations (2)–(4), assume that the thrust vector is perfectly aligned with the body z-axis, an assumption that may not hold in practice. This misalignment issue occurs because the thrust vector’s direction is not known at the time that the robot’s body coordinates are defined in the motion capture software. A tilted thrust vector introduces lateral and longitudinal forces. To reduce this error, we redefined the body coordinate system after performing a short 0.3 s uncontrolled trimmed flight. Since during a trimmed flight, the robot takes off approximately vertically, so its trajectory can be used to estimate the direction of the thrust vector and therefore align the z-axis of the body coordinate system to that direction. This, however, is based on the assumption that the body coordinate system is not redefined in between the experiments and remains the same throughout the data collection and control process.

Visualization of collected data

Unlike traditional aircraft, where the flight envelope is defined by changes in velocity against variations in angle of attack, flapping wing robots exhibit continuous angle of attack variations throughout the flapping cycle. Therefore, we determined that representing the flight state in terms of angular orientation and velocity would be a more pertinent characterization. Characterizing in this way elucidates the robot’s ability to maintain controlled flight across different tilt angles and the associated longitudinal/lateral speeds at these angles, which are critical for maneuverability. A robot that can sustain higher tilt angles and translational speeds in controlled flight is indicative of superior maneuverability, enabling it to execute tighter turns. As shown in Fig. 6, based on data collected from the flight trajectories of the RoboFly, the robot is able to get to attitude angles of up to approximately 30° and body velocity of around 0. 4 m/s.

Fig. 6: Visualization of the collected data.

The graph shows the robot achieving high attitude angles up to 30° and corresponding lateral/longitudinal speeds exceeding 0.4 m/s in the collected data. This highlights significant perturbations, which will be used to validate the stroke-averaged dynamics developed in this work. The color intensity on the graph represents the density of data points.

Infinite horizon LQR

Infinite horizon LQR controller²¹ optimizes the quadratic cost function subject to linearized dynamics constraints. In this work, we use the continuous time formulation of LQR.

$$J=\mathop{\int}\nolimits_{0}^{\infty }\sigma {(t)}^{T}Q\sigma (t)+u{(t)}^{T}Ru(t)$$

(8)

$$\,{\text{subject}}\,{\text{to}}\,\dot{\sigma }(t)={A}_{d}\sigma (t)+{B}_{d}u(t)$$

(9)

Here, σ(t) is the state vector of the robot at time t, A_d and B_d are the linearized dynamics matrices. If the system is completely stabilizable, we can write this optimal control problem in terms of the Hamiltonian function, which incorporates both the system dynamics and the cost function. The Hamiltonian for the LQR problem is given by H(x, u, λ) = σ^TQσ + u^TRu + λ^T(A_dσ + B_du). λ is the costate vector, Q and R are positive semi-definite and positive definite matrices respectively. By taking the derivative of Hamiltonian with respect to σ, u, and λ and setting it to zero we get the closed loop dynamics as,

$$\left[\begin{array}{c}\dot{\sigma }\\ \dot{\lambda }\end{array}\right]=\left[\begin{array}{cc}A&-B{R}^{-1}{B}^{T}\\ -Q&-{A}^{T}\\ \end{array}\right]\left[\begin{array}{c}\sigma \\ \lambda \end{array}\right]$$

(10)

The steady-state solution of the Riccati equation can be described in terms of the eigenvectors of the Hamiltonian matrix in the closed loop dynamic equation (10) associated with the negative real part eigenvalues. These negative real part eigenvalues of the matrix are also the eigenvalues of the closed-loop matrix A − BR⁻¹BP. The feedback gain $k=-{R}^{-1}{B}^{{\prime} }P$ can be obtained using P as the Schur form of the closed-loop dynamics matrix such that:

$$u(t)=-k({\sigma }_{des}(t)-\sigma (t))$$

(11)

Here σ_des(t) is the desired state of the robot at time t.