Sensor Simulation

Learning Objectives

• Understand the principles of sensor simulation in robotics
• Learn to configure and implement various sensor types in simulation
• Configure camera, LiDAR, IMU, and GPS sensors in Gazebo and Unity
• Understand sensor noise models and realistic sensor behavior
• Validate sensor outputs against real-world data

Prerequisites

• Understanding of physics simulation (Chapter 2)
• Basic knowledge of sensor types and their applications
• ROS 2 and simulation environment setup

Introduction to Sensor Simulation

Sensor simulation is crucial for creating realistic digital twins that can effectively support robot development, testing, and training. In simulation, we must model not just the ideal sensor readings, but also the real-world limitations, noise, and imperfections that affect actual sensors.

Why Simulate Sensors?

1. Safe Testing: Test perception algorithms without hardware risk
2. Cost-Effective: Reduce need for expensive hardware during development
3. Controlled Environment: Create specific scenarios for testing
4. Training: Generate large datasets for machine learning
5. Validation: Verify sensor fusion algorithms

Sensor Simulation Challenges

• Noise Modeling: Real sensors have inherent noise and uncertainty
• Computational Cost: Complex sensors require significant processing power
• Realism vs. Performance: Balance accuracy with simulation speed
• Domain Gap: Differences between simulated and real sensor data

Camera Simulation

Cameras are fundamental sensors for robotics, providing rich visual information for navigation, object recognition, and mapping.

Camera Simulation in Gazebo

<!-- In URDF/SDF, define a camera sensor -->
<gazebo reference="camera_link">
  <sensor name="camera" type="camera">
    <update_rate>30.0</update_rate>
    <camera name="head">
      <horizontal_fov>1.3962634</horizontal_fov> <!-- 80 degrees -->
      <image>
        <width>640</width>
        <height>480</height>
        <format>R8G8B8</format>
      </image>
      <clip>
        <near>0.1</near>
        <far>100.0</far>
      </clip>
      <noise>
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.007</stddev>
      </noise>
    </camera>
    <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
      <frame_name>camera_optical_frame</frame_name>
      <min_depth>0.1</min_depth>
      <max_depth>100.0</max_depth>
    </plugin>
  </sensor>
</gazebo>

Camera Configuration Parameters

Parameter	Description	Typical Values
horizontal_fov	Horizontal field of view (radians)	0.7 - 1.57 (40° - 90°)
image.width	Image width in pixels	640, 1280, 1920
image.height	Image height in pixels	480, 720, 1080
update_rate	Sensor update rate (Hz)	15 - 60 Hz
clip.near	Near clipping distance	0.01 - 0.5 m
clip.far	Far clipping distance	10 - 100 m

Camera Simulation in Unity

using UnityEngine;
using Unity.Robotics.Sensors;

public class SimulatedCamera : MonoBehaviour
{
    public Camera camera;
    public float updateRate = 30.0f;
    public float noiseLevel = 0.01f;

    private float nextUpdateTime = 0f;

    void Start()
    {
        camera = GetComponent<Camera>();
        // Configure camera properties
        camera.fieldOfView = 60f; // degrees
    }

    void Update()
    {
        if (Time.time >= nextUpdateTime)
        {
            // Simulate camera capture
            CaptureImage();
            nextUpdateTime = Time.time + 1.0f / updateRate;
        }
    }

    void CaptureImage()
    {
        // In Unity, you might use RenderTexture to capture images
        // and potentially add noise simulation
        RenderTexture renderTexture = new RenderTexture(640, 480, 24);
        camera.targetTexture = renderTexture;
        camera.Render();

        // Add noise simulation here if needed
        ApplyNoise(renderTexture);
    }

    void ApplyNoise(RenderTexture texture)
    {
        // Apply Gaussian noise or other noise models
        // This is a simplified example
    }
}

LiDAR Simulation

LiDAR (Light Detection and Ranging) sensors provide accurate 2D or 3D range measurements, crucial for navigation and mapping.

LiDAR Simulation in Gazebo

<!-- 2D LiDAR example -->
<gazebo reference="laser_link">
  <sensor name="laser" type="ray">
    <update_rate>10</update_rate>
    <ray>
      <scan>
        <horizontal>
          <samples>720</samples>
          <resolution>1</resolution>
          <min_angle>-1.570796</min_angle> <!-- -90 degrees -->
          <max_angle>1.570796</max_angle>   <!-- 90 degrees -->
        </horizontal>
      </scan>
      <range>
        <min>0.1</min>
        <max>30.0</max>
        <resolution>0.01</resolution>
      </range>
    </ray>
    <plugin name="laser_controller" filename="libgazebo_ros_ray_sensor.so">
      <topic_name>scan</topic_name>
      <frame_name>laser_link</frame_name>
    </plugin>
  </sensor>
</gazebo>

<!-- 3D LiDAR example (Velodyne-like) -->
<gazebo reference="velodyne_link">
  <sensor name="velodyne" type="ray">
    <update_rate>10</update_rate>
    <ray>
      <scan>
        <horizontal>
          <samples>1800</samples>
          <resolution>1</resolution>
          <min_angle>-3.14159</min_angle> <!-- -180 degrees -->
          <max_angle>3.14159</max_angle>   <!-- 180 degrees -->
        </horizontal>
        <vertical>
          <samples>16</samples>
          <resolution>1</resolution>
          <min_angle>-0.2618</min_angle> <!-- -15 degrees -->
          <max_angle>0.2618</max_angle>   <!-- 15 degrees -->
        </vertical>
      </scan>
      <range>
        <min>0.1</min>
        <max>100.0</max>
        <resolution>0.01</resolution>
      </range>
    </ray>
    <plugin name="velodyne_controller" filename="libgazebo_ros_velodyne_gpu.so">
      <topic_name>velodyne_points</topic_name>
      <frame_name>velodyne_link</frame_name>
      <min_range>0.9</min_range>
      <max_range>100.0</max_range>
      <gaussian_noise>0.008</gaussian_noise>
    </plugin>
  </sensor>
</gazebo>

LiDAR Configuration Parameters

Parameter	Description	Typical Values
samples	Number of rays per scan	360 - 1800
min_angle	Minimum horizontal angle	-π to π
max_angle	Maximum horizontal angle	-π to π
vertical.samples	Number of vertical layers (3D)	16, 32, 64, 128
range.min	Minimum detectable range	0.05 - 0.5 m
range.max	Maximum detectable range	10 - 100 m
update_rate	Sensor update rate	5 - 20 Hz

IMU Simulation

Inertial Measurement Units (IMUs) provide acceleration and angular velocity measurements, essential for state estimation and control.

IMU Simulation in Gazebo

<gazebo reference="imu_link">
  <sensor name="imu_sensor" type="imu">
    <always_on>true</always_on>
    <update_rate>100</update_rate>
    <imu>
      <angular_velocity>
        <x>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>0.0017</stddev> <!-- ~0.1 deg/s -->
          </noise>
        </x>
        <y>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>0.0017</stddev>
          </noise>
        </y>
        <z>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>0.0017</stddev>
          </noise>
        </z>
      </angular_velocity>
      <linear_acceleration>
        <x>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev> <!-- 17 mg -->
          </noise>
        </x>
        <y>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
          </noise>
        </y>
        <z>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
          </noise>
        </z>
      </linear_acceleration>
    </imu>
    <plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
      <topicName>imu</topicName>
      <bodyName>imu_link</bodyName>
      <frameName>imu_link</frameName>
      <serviceName>imu_service</serviceName>
      <gaussianNoise>0.0017</gaussianNoise>
      <updateRateHZ>100.0</updateRateHZ>
    </plugin>
  </sensor>
</gazebo>

IMU Configuration Parameters

Parameter	Description	Typical Values
update_rate	Sensor update rate	100 - 1000 Hz
angular_velocity.stddev	Angular velocity noise (rad/s)	0.001 - 0.01
linear_acceleration.stddev	Linear acceleration noise (m/s²)	0.01 - 0.1

GPS Simulation

GPS sensors provide global position information, important for outdoor navigation.

GPS Simulation in Gazebo

<gazebo reference="gps_link">
  <sensor name="gps_sensor" type="gps">
    <always_on>true</always_on>
    <update_rate>1</update_rate>
    <gps>
      <position_sensing>
        <horizontal>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>0.1</stddev> <!-- 10cm accuracy -->
          </noise>
        </horizontal>
        <vertical>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>0.15</stddev> <!-- 15cm accuracy -->
          </noise>
        </vertical>
      </position_sensing>
    </gps>
    <plugin name="gps_plugin" filename="libgazebo_ros_gps.so">
      <topicName>gps/fix</topicName>
      <frameName>gps_link</frameName>
      <updateRate>1.0</updateRate>
    </plugin>
  </sensor>
</gazebo>

Sensor Noise Models

Real sensors have various types of noise that must be modeled for realistic simulation:

Gaussian Noise

Most common noise model, characterized by mean (μ) and standard deviation (σ):

import numpy as np

def add_gaussian_noise(measurement, mean=0.0, std_dev=0.01):
    """Add Gaussian noise to a sensor measurement"""
    noise = np.random.normal(mean, std_dev)
    return measurement + noise

Bias and Drift

Sensors often have systematic errors that change over time:

import numpy as np

class SensorWithBias:
    def __init__(self, initial_bias=0.0, drift_rate=0.001):
        self.bias = initial_bias
        self.drift_rate = drift_rate
        self.time_since_start = 0.0

    def measure(self, true_value, dt):
        """Simulate sensor measurement with bias and drift"""
        self.time_since_start += dt
        current_bias = self.bias + self.drift_rate * self.time_since_start
        noise = np.random.normal(0.0, 0.01)  # Gaussian noise
        return true_value + current_bias + noise

Multi-Sensor Fusion Simulation

In real robots, multiple sensors are often combined for better state estimation:

import numpy as np

class SensorFusionSimulator:
    def __init__(self):
        self.imu = SensorWithBias(initial_bias=0.001, drift_rate=0.0001)
        self.gps = SensorWithBias(initial_bias=0.1, drift_rate=0.001)
        self.odom = SensorWithBias(initial_bias=0.01, drift_rate=0.0005)

    def get_fused_state(self, true_state, dt):
        """Simulate fused sensor readings"""
        imu_reading = self.imu.measure(true_state['imu'], dt)
        gps_reading = self.gps.measure(true_state['gps'], dt)
        odom_reading = self.odom.measure(true_state['odom'], dt)

        # Simple weighted average fusion
        fused_state = {
            'position': 0.7 * gps_reading['position'] + 0.3 * odom_reading['position'],
            'velocity': 0.6 * imu_reading['velocity'] + 0.4 * odom_reading['velocity'],
            'orientation': 0.8 * imu_reading['orientation'] + 0.2 * gps_reading['orientation']
        }

        return fused_state

Unity Sensor Simulation

Unity provides various approaches for sensor simulation:

using UnityEngine;
using Unity.Robotics;
using System.Collections;

public class UnitySensorSimulator : MonoBehaviour
{
    public Camera cameraSensor;
    public float lidarRange = 30.0f;
    public int lidarRays = 720;

    // IMU simulation
    private Vector3 lastPosition;
    private Quaternion lastRotation;
    private float lastTime;

    void Start()
    {
        lastPosition = transform.position;
        lastRotation = transform.rotation;
        lastTime = Time.time;
    }

    void Update()
    {
        SimulateIMU();
        SimulateLidar();
    }

    void SimulateIMU()
    {
        float deltaTime = Time.time - lastTime;

        // Calculate linear acceleration
        Vector3 velocity = (transform.position - lastPosition) / deltaTime;
        Vector3 acceleration = (velocity - (lastPosition - lastPosition) / deltaTime) / deltaTime; // Simplified

        // Calculate angular velocity
        Quaternion deltaRotation = transform.rotation * Quaternion.Inverse(lastRotation);
        Vector3 angularVelocity = new Vector3(
            Mathf.Atan2(2 * (deltaRotation.x * deltaRotation.w + deltaRotation.y * deltaRotation.z),
                        1 - 2 * (deltaRotation.z * deltaRotation.z + deltaRotation.w * deltaRotation.w)),
            Mathf.Atan2(2 * (deltaRotation.y * deltaRotation.w - deltaRotation.z * deltaRotation.x),
                        Mathf.Sqrt(1 - Mathf.Pow(2 * deltaRotation.x * deltaRotation.w + 2 * deltaRotation.y * deltaRotation.z, 2))),
            Mathf.Atan2(2 * (deltaRotation.z * deltaRotation.w + deltaRotation.x * deltaRotation.y),
                        1 - 2 * (deltaRotation.x * deltaRotation.x + deltaRotation.y * deltaRotation.y))
        ) / deltaTime;

        // Add noise to simulated measurements
        acceleration += Random.insideUnitSphere * 0.01f; // Add noise
        angularVelocity += Random.insideUnitSphere * 0.001f; // Add noise

        lastPosition = transform.position;
        lastRotation = transform.rotation;
        lastTime = Time.time;
    }

    void SimulateLidar()
    {
        // Perform raycasts to simulate LiDAR
        for (int i = 0; i < lidarRays; i++)
        {
            float angle = (float)i / lidarRays * 2 * Mathf.PI;
            Vector3 direction = new Vector3(Mathf.Cos(angle), 0, Mathf.Sin(angle));

            RaycastHit hit;
            if (Physics.Raycast(transform.position, direction, out hit, lidarRange))
            {
                // Process hit distance with noise
                float noisyDistance = hit.distance + Random.Range(-0.05f, 0.05f);
                // Publish to ROS or process as needed
            }
            else
            {
                // No hit within range
            }
        }
    }
}

Sensor Validation

To ensure realistic sensor simulation:

1. Compare with real sensors: Validate simulation outputs against real sensor data
2. Statistical analysis: Verify noise characteristics match expected distributions
3. Edge case testing: Test with extreme conditions and verify realistic behavior
4. Cross-validation: Compare multiple simulation approaches

Best Practices for Sensor Simulation

Accuracy Considerations

1. Model realistic noise: Include appropriate noise models for each sensor type
2. Consider sensor limitations: Account for field of view, range, and resolution limits
3. Update rates: Match simulation update rates to real sensor rates
4. Environmental factors: Consider lighting, weather, and other environmental effects

Performance Optimization

1. Sensor scheduling: Not all sensors need to update simultaneously
2. Level of detail: Adjust sensor complexity based on requirements
3. Selective rendering: Only render what sensors actually "see"
4. Caching: Cache expensive sensor computations when possible

Integration with ROS 2

Sensors in simulation should publish standard ROS 2 message types:

• Camera: sensor_msgs/Image and sensor_msgs/CameraInfo
• LiDAR: sensor_msgs/LaserScan or sensor_msgs/PointCloud2
• IMU: sensor_msgs/Imu
• GPS: sensor_msgs/NavSatFix

Exercises

1. Camera Calibration: Simulate a camera with known intrinsic parameters and verify output
2. LiDAR Mapping: Create a LiDAR sensor and use it to map a simulated environment
3. IMU Integration: Simulate IMU data and integrate to estimate position
4. Sensor Fusion: Combine multiple simulated sensors to improve state estimation

Summary

Sensor simulation is a critical component of realistic digital twins for robotics. By properly modeling various sensor types including cameras, LiDAR, IMU, and GPS with appropriate noise characteristics and update rates, we can create simulation environments that effectively support robot development, testing, and training. The key is to balance realism with computational efficiency while ensuring that simulation results are representative of real-world performance.

References

• Gazebo Sensor Documentation
• ROS 2 Sensor Message Types
• Unity Robotics Sensors
• Sensor Simulation in Robotics