mobile_robot_motion_recording.mp4
This repository contains a Python simulation for point stabilization of a nonholonomic mobile robot. The goal is to drive the robot from an arbitrary initial configuration to a desired configuration without using a path-planning or trajectory-planning algorithm. The project focuses on configuration stabilization, velocity tracking, disturbance rejection, state estimation, and dynamic control under actuator limits and simulated noise.
The stabilizing control structure is based on the arbitrary configuration stabilization method proposed by Peng et al. [1]. In this project, the original control idea is extended in simulation by adding a dynamic torque-level model, input saturation, wheel slip effects, square-wave disturbances, a nonlinear disturbance observer, and an extended Kalman filter for noisy velocity measurements.
Nonholonomic mobile robots cannot move freely in all directions because of their rolling constraints. For example, a differential-drive robot cannot move sideways directly. Because of this constraint, stabilizing the robot to an arbitrary position and orientation is not as simple as applying a standard linear controller in Cartesian coordinates.
This project studies the point stabilization problem from a practical simulation perspective. Instead of only applying a kinematic controller, the simulation also includes dynamic behavior, torque inputs, process noise, measurement noise, disturbance estimation, and actuator saturation. This makes the project useful for understanding how a theoretical nonholonomic stabilization law can be extended toward a more realistic mobile robot model.
- Point stabilization of a nonholonomic mobile robot from an arbitrary initial pose to a desired pose.
- Configuration error formulation using the relative transformation between the desired pose and the current robot pose.
- Kinematic reference control inspired by the c-nonholonomic trajectory approach.
- Dynamic torque-level control for linear and angular velocity tracking.
- Input saturation for motor torque limits.
- Simulated wheel slip effects between motor, wheel, and ground.
- Square-wave disturbance injection on the left and right wheel torque channels.
- Nonlinear disturbance observer for estimating lumped linear and angular disturbance terms.
- Extended Kalman filter for estimating linear and angular velocity from noisy measurements.
- Euler integration for simulating the nonlinear robot dynamics.
- Plots for robot trajectory, velocity tracking, torque inputs, disturbance estimation, and filtered velocity estimates.
The robot starts from an initial configuration
(x0, y0, theta0)
and must converge to a desired configuration
(xd, yd, thetad)
The current and desired configurations are represented using homogeneous transformation matrices. The relative configuration error is computed as
g_e = g_d^{-1} g
where g is the current robot configuration and g_d is the desired configuration. From this relative error, the simulation computes the position error and orientation error used by the stabilizing controller.
The kinematic controller generates reference linear and angular velocities. These reference velocities are then tracked by a dynamic controller that computes torque commands. The torque commands are saturated before being applied to the robot model. Disturbances are added to the torque channels, and the nonlinear disturbance observer estimates their effect so that the controller can compensate for them.
The measured linear and angular velocities are corrupted with measurement noise. An extended Kalman filter is then used to estimate the robot velocity states before they are used in the feedback loop.
control_mobile_robot_disturbance-main/
├── main.py # Main simulation loop
├── parameters.py # Robot, controller, noise, and simulation parameters
├── dynamics.py # Dynamic model of the mobile robot
├── ekf.py # Extended Kalman filter implementation
├── Ndob.py # Nonlinear disturbance observer
├── draw.py # Robot drawing utility
├── plotall.py # Plotting functions for simulation results
├── disturbance_signal.png # Saved disturbance-related result figure
├── mobile_robot_motion_xy_plane.png # Saved robot trajectory figure
├── mobile_robots_estimated_speeds.png # Saved estimated speed figure
├── motor_linear_angular_speeds.png # Saved velocity tracking figure
├── motor_torques.png # Saved torque input figure
├── mobile_robot_motion_recording.mp4 # Simulation animation or recording
├── LICENSE
└── README.md
The project is written in Python and uses the following main packages:
numpy
scipy
matplotlib
A typical installation command is:
pip install numpy scipy matplotlibClone the repository and enter the project folder:
git clone <repository-url>
cd control_mobile_robot_disturbance-mainRun the main simulation:
python main.pyThe script prints the final robot position and yaw angle, then generates plots showing the main simulation results.
Most parameters are defined in parameters.py. Some important values are:
m = 5 # robot mass
r = 0.5 # wheel radius
l = 1 # distance between wheels
J = 2 # moment of inertia
R = 0.3 # robot radius
vm = 0.5 # maximum reference linear speed
wm = 1.0 # maximum reference angular speed
k1 = 1.0 # kinematic linear control gain
k2 = 4.0 # kinematic angular control gain
k_v = 5 # dynamic linear velocity gain
k_omega = 5 # dynamic angular velocity gain
tau_min = -10 # minimum torque limit
tau_max = 10 # maximum torque limit
dt = 0.01 # simulation time step
t_end = 50 # total simulation timeThe initial and desired configurations are also set in parameters.py:
x0 = 9
y0 = 9
theta0 = np.pi
xd = 0
yd = 0
thetad = np.pi / 2To test a different stabilization scenario, change these initial and desired pose values and rerun main.py.
The project produces several plots, including:
- Robot motion in the XY plane.
- Linear velocity reference and estimated linear velocity.
- Angular velocity reference and estimated angular velocity.
- Motor torque inputs.
- Actual and estimated disturbance signals.
- True and estimated velocity states after filtering.
These outputs are useful for checking whether the robot converges to the desired configuration, whether the dynamic controller tracks the reference velocities, and whether the observer and filter improve robustness under disturbance and noise.
This project does not use a global path planner. The robot is not given a sequence of waypoints. Instead, it directly stabilizes to the desired configuration using feedback control.
The disturbance observer estimates lumped disturbance effects in the linear and angular dynamics. The EKF estimates the linear and angular velocities using noisy measurements. Together, these additions make the simulation more realistic than a purely kinematic point-stabilization example.
The simulation uses Euler integration. For higher numerical accuracy, future versions could replace Euler integration with a higher-order ODE solver such as Runge-Kutta integration.
[1] X. Peng, Z. Sun, M. Chen, and Z. Geng, “Arbitrary Configuration Stabilization Control for Nonholonomic Vehicle With Input Saturation: A c-Nonholonomic Trajectory Approach,” IEEE Transactions on Industrial Electronics, vol. 69, no. 2, pp. 1663–1672, Feb. 2022, doi: 10.1109/TIE.2021.3060674.
@article{peng2022arbitrary,
author={Peng, X. and Sun, Z. and Chen, M. and Geng, Z.},
journal={IEEE Transactions on Industrial Electronics},
title={Arbitrary Configuration Stabilization Control for Nonholonomic Vehicle With Input Saturation: A c-Nonholonomic Trajectory Approach},
year={2022},
volume={69},
number={2},
pages={1663--1672},
doi={10.1109/TIE.2021.3060674}
}This project is released under the MIT License. See the LICENSE file for details.
