This directory contains the nonlinear Simulink and Simscape Multibody simulation of SEBA-ROBOT together with its robust servomechanism LQR motion controller.
The model is organized as a closed-loop simulation consisting of:
- a nonlinear Simscape Multibody two-wheeled inverted-pendulum robot plant
- an RSLQR motion controller
- wheel-speed-based forward-velocity and yaw-rate feedback
- a motion-command generator for forward velocity and yaw rate
- centralized robot-parameter definition
- simulation signal logging and visualization
The controller stabilizes the body pitch while tracking commanded forward velocity and yaw rate through independent left- and right-wheel torque commands.
Four simulation test cases are provided to evaluate balance recovery, forward-velocity tracking, yaw-rate tracking, and combined motion tracking. Each test case includes a result plot and an animation of the corresponding multibody simulation.
The complete nonlinear dynamics, reduced velocity-control model, linearization, augmented-state formulation, and RSLQR controller derivation are documented in the Dynamics and Velocity-Tracking Control Model.
The model was developed and tested using MATLAB R2025b with:
- MATLAB
- Simulink
- Simscape
- Simscape Multibody
The controller gain is hardcoded for normal simulation, so Control System Toolbox is not required to run the model. It is required only when using the lqr function to recalculate the controller gain.
The following rigid-body and physical parameters are used by the simulation and controller model:
| Symbol | Meaning | Value |
|---|---|---|
| body mass | ||
| mass of each wheel | ||
| distance from the wheel axle to the body center of mass | ||
| wheel radius | ||
| wheel center-to-center separation | ||
| body moment of inertia about the pitch axis | ||
| body moment of inertia about the yaw axis | ||
| wheel moment of inertia about the rolling axis | ||
| wheel moment of inertia about the vertical axis | ||
| wheel rotational damping coefficient | ||
| gravitational acceleration |
The robot body is modeled as a rectangular rigid body with dimensions:
0.10 m × 0.10 m × 0.25 m
Each wheel is modeled as a solid cylinder with a width of:
0.030 m
The left and right wheel revolute joints use the following internal-mechanics settings:
Equilibrium position: 0 deg
Spring stiffness: 0
Damping coefficient: 0.0005 N·m·s/rad
The ground is modeled as a fixed rigid plane. Each wheel interacts with the ground through an independent Spatial Contact Force block using the same contact parameters.
Method: Smooth Spring-Damper
Stiffness: 1e4 N/m
Damping: 1e4 N/(m/s)
Transition-region width: 2 mm
Method: Smooth Stick-Slip
Static-friction coefficient: 100
Dynamic-friction coefficient: 1
Critical velocity: 1e-3 m/s
These values are numerical parameters of the current contact model and have not been obtained through experimental wheel-ground identification.
The simulation uses:
Solver type: Variable-step
Solver: daessc
The remaining solver settings use their configured automatic or default values.
The simulation uses the robust servomechanism LQR controller derived in the Dynamics and Velocity-Tracking Control Model.
The controller receives the robot state and motion commands, calculates the left- and right-wheel torque-rate commands, and integrates them to produce the wheel torque commands applied to the Robot Plant.
The RSLQR Gain Solver block inside the RSLQR Controller subsystem contains the LQR weighting matrices and controller-gain calculation.
The weighting matrices are:
Q = diag([
70; % Forward-velocity tracking error
40; % Yaw-rate tracking error
70; % Forward acceleration
1; % Pitch rate
1; % Pitch acceleration
1 % Yaw acceleration
]);
R = 20*eye(2);Normal simulation uses the following hardcoded gain:
K_c = [
-1.32287565553227, -1.00000000000000, -1.89671635710169, -4.14420766193970, -0.575208158135163, -0.162330467920892;
-1.32287565553227, 1.00000000000000, -1.89671635710170, -4.14420766193971, -0.575208158135163, 0.162330467920892
];The first row produces the left-wheel torque-rate command, and the second row produces the right-wheel torque-rate command.
The lqr calculation in the RSLQR Gain Solver block is disabled during normal simulation. It is enabled only when recalculating the gain after changing the robot parameters or LQR weighting matrices. Control System Toolbox is required only for this recalculation.
The following named vector signals are logged:
command = [v_cmd; psi_dot_cmd]
state = [v; theta; theta_dot; psi_dot]
torques = [T_L; T_R]
The model uses:
Signal logging variable: logsout
Single simulation output: out
Save format: Dataset
Dataset signal format: timeseries
No separate initialization script is required. All parameters and controller settings needed to run the simulation are defined within seba_control.slx.
Open seba_control.slx in MATLAB and run the model from Simulink, or use:
out = sim(bdroot);Retrieve the logged dataset using:
logs = out.logsout;Retrieve the named signals using:
command = logs.getElement('command').Values;
state = logs.getElement('state').Values;
torques = logs.getElement('torques').Values;Four test cases are provided:
- balance recovery
- forward-velocity tracking
- yaw-rate tracking
- combined motion tracking
For each test, replace the code in the Motion Command Profile MATLAB Function block with the corresponding command-generation code shown below.
The result plots compare the commanded and measured forward velocity and yaw rate, and also show the body pitch angle and left- and right-wheel torque commands. The animations show the corresponding motion of the Simscape Multibody robot.
This test evaluates recovery from a nonzero initial body pitch while both motion commands remain zero.
Initial pitch angle: 25 deg
Simulation stop time: 7 s
Inside the Robot Plant, set the initial condition of the 1/s pitch-angle integrator connected to Transform Sensor1 to:
25*pi/180Set the rotation angle of both Rigid Transform7 and Rigid Transform8 to:
Method: Standard Axis
Angle: 25 deg
Axis: +X
These two rigid transforms belong to the left- and right-wheel branches. Using the same initial angle in the pitch integrator and both rigid transforms keeps the measured pitch state consistent with the initial Simscape Multibody configuration.
Use the following command profile:
function r = fcn(t)
%#codegen
v_cmd = 0;
psi_dot_cmd = 0;
r = [v_cmd; psi_dot_cmd];
end
balance_recovery.mp4
This test evaluates positive and negative forward-velocity tracking while the yaw-rate command remains zero.
Initial pitch angle: 0 deg
Simulation stop time: 22 s
Use the following command profile:
function r = fcn(t)
%#codegen
if t < 2
v_cmd = 0;
elseif t < 7
v_cmd = 0.15;
elseif t < 12
v_cmd = 0;
elseif t < 17
v_cmd = -0.15;
else
v_cmd = 0;
end
psi_dot_cmd = 0;
r = [v_cmd; psi_dot_cmd];
end
velocity_tracking.mp4
This test evaluates positive and negative yaw-rate tracking while the forward-velocity command remains zero.
Initial pitch angle: 0 deg
Simulation stop time: 22 s
Use the following command profile:
function r = fcn(t)
%#codegen
v_cmd = 0;
if t < 2
psi_dot_cmd = 0;
elseif t < 7
psi_dot_cmd = 2*pi/5;
elseif t < 12
psi_dot_cmd = 0;
elseif t < 17
psi_dot_cmd = -2*pi/5;
else
psi_dot_cmd = 0;
end
r = [v_cmd; psi_dot_cmd];
end
yaw_rate_tracking.mp4
This test evaluates simultaneous forward-velocity and yaw-rate tracking.
Initial pitch angle: 0 deg
Simulation stop time: 22 s
Use the following command profile:
function r = fcn(t)
%#codegen
if t < 2
v_cmd = 0;
psi_dot_cmd = 0;
elseif t < 7
v_cmd = 0.15;
psi_dot_cmd = pi/5;
elseif t < 12
v_cmd = 0;
psi_dot_cmd = 0;
elseif t < 17
v_cmd = -0.15;
psi_dot_cmd = -pi/5;
else
v_cmd = 0;
psi_dot_cmd = 0;
end
r = [v_cmd; psi_dot_cmd];
end
combined_motion_tracking.mp4
The simulation focuses on the robot rigid-body dynamics, wheel-ground interaction, motion control, and command tracking.
The presented results correspond to the documented robot parameters, controller settings, contact model, solver configuration, command profiles, simulation durations, and initial conditions.
The wheel-ground contact and friction values are simulation parameters rather than experimentally identified material properties.
Hardware-specific effects such as sensor noise, actuator saturation, motor electrical dynamics, backlash, communication delay, and battery-voltage variation are outside the current simulation scope.