[Hybrid Navigation System: Part 7] [Implementation-Phase 3] Terminal Phase: YOLO-Based Precision Visual Servoing (IBVS) and Offboard Control Simulation

Bymaponarooo

Hello! To all the graduate students and researchers dedicating their intellect to pushing the boundaries of autonomous unmanned aerial vehicle (UAV) systems—welcome back to the QUAD Drone Lab.

In Part 6 of our series, we navigated through the intermediate correction phase, leveraging pre-loaded satellite reference maps and real-time downward-facing camera feeds to reset accumulated dead-reckoning drift using Absolute Visual Localization (AVL). At this point, our autonomous kamikaze drone (or precision landing UAV) has bypassed GPS jamming and spoofing, arriving safely at the vicinity of the target.

Today, in this final installation of our implementation series (Part 7), we build the “Terminal Phase”. We will explore how to transition our UAV from waypoint navigation to Image-Based Visual Servoing (IBVS) using a front-facing camera integrated with a real-time YOLO object detector. We will implement this system as a native ROS 2 node interacting with PX4 Autopilot’s Offboard Mode via the Micro XRCE-DDS bridge.

1. Understanding Visual Servoing: PBVS vs. IBVS

Visual Servoing utilizes real-time image features captured by onboard cameras to control the motion of a robotic system in a closed loop. In the terminal phase, when a drone must home in on a precise target (such as an aircraft carrier, a vehicle, or a landing pad) without relying on global coordinates, visual servoing becomes indispensable. This control paradigm is divided into two primary categories:

  1. Pose-Based Visual Servoing (PBVS): PBVS estimates the absolute 3D translation and rotation (the 6-DoF relative pose) of the target relative to the camera using geometric models (e.g., solving the Perspective-n-Point / PnP problem). While intuitive, PBVS requires precise camera calibration, a known 3D target model, and is highly susceptible to depth estimation errors and image noise. If the camera calibration drifts slightly, the calculated 3D pose can fluctuate wildly, leading to control instability.
  2. Image-Based Visual Servoing (IBVS): IBVS bypasses 3D pose reconstruction entirely. Instead, it uses the 2D pixel coordinates of target features directly on the image plane as the feedback state. The control objective is to minimize the pixel error vector between the current target position on the screen and a desired reference target position (typically the exact center of the camera frame).

Because IBVS maps pixel errors directly to the workspace velocity vector of the camera, it is highly robust to camera calibration noise, sensor drift, and model uncertainties. This direct mapping makes IBVS the ideal choice for terminal precision guidance on agile, resource-constrained drone platforms.

2. Mathematical Foundation of IBVS Control Law

The goal of IBVS is to drive the pixel error vector e(t) to zero. Let the current target’s center coordinates on the 2D image plane be s(t)=(u,v)T, and the desired coordinate (e.g., the center of the camera feed) be s∗=(uc​,vc​)T. The error vector is defined as: e(t)=s(t)−s∗

To relate the movement of these 2D image points to the 3D linear velocity v=(vx​,vy​,vz​)T and angular velocity ω=(ωx​,ωy​,ωz​)T of the camera, we utilize the Image Jacobian (also called the Interaction Matrix, Ls​): s˙=Ls​vc​

Where vc​=(v,ω)T is the camera’s spatial velocity vector. To ensure an exponential decay of the error (e˙=−λe), we compute the required camera velocity vector using the Moore-Penrose pseudo-inverse of the Interaction Matrix: vc​=−λLs​+e

In our quadrotor setup, we constrain the aircraft to 4-DoF control (translational velocities in body-fixed axes vforward​,vright​,vdown​ and heading control via yaw rate ωz​). We can simplify this complex matrix transformation using decoupled proportional controllers where the horizontal pixel deviation drives yaw rate (ωz​) or lateral speed (vright​), and the vertical pixel deviation drives vertical velocity (vdown​) or forward speed (vforward​).

3. System Architecture: Integrating YOLO and PX4 Offboard Mode

To execute this control loop, we implement an onboard pipeline consisting of a camera sensor, a companion computer (e.g., NVIDIA Jetson Orin), and the PX4 Flight Controller.

The communication sequence is designed as follows:

  1. Image Processing: A ROS 2 node subscribes to the camera stream (e.g., /camera) and converts it to an OpenCV format.
  2. YOLO Target Detection: A lightweight deep learning model, such as YOLOv8 Nano, runs real-time inference on the image, returning the bounding box coordinates (u,v) of the target.
  3. Coordinate Frame Transformation: Because the image plane represents a 2D coordinate system, we transform the computed velocity vectors to match PX4’s local NED (North-East-Down) frame convention.
  4. Offboard Control Publishing: The node publishes TrajectorySetpoint messages (defining target velocities) and OffboardControlMode messages.

⚠️ Critical Developer Warning (Offboard Failsafe): PX4’s internal safety manager monitors offboard command streams. If Offboard messages are delayed or drop below a frequency of 2Hz (longer than 0.5s interval), PX4 will trigger a safety Failsafe (typically executing an emergency hover or return-to-land). Therefore, our ROS 2 timer callback must continually publish heartbeat signals at a high frequency (e.g., 20Hz), even if a target is temporarily lost.

4. ROS 2 Python Node Example: YOLO-Based IBVS Terminal Controller

Below is a complete, deployable ROS 2 Python Node (terminal_servo_node.py). This script subscribes to a /camera image topic, performs real-time YOLOv8 object detection, maps the pixel errors to the vehicle’s body-frame velocities using an IBVS proportional controller, transforms those velocities into the local NED frame using the vehicle’s current yaw angle, and commands PX4 via TrajectorySetpoint uORB messages.

Python
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy

# Import CV Bridge and computer vision libraries
import cv2
from cv_bridge import CvBridge
import numpy as np
from ultralytics import YOLO

# Import ROS 2 message types for camera and PX4 uORB bridge
from sensor_msgs.msg import Image
from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand, VehicleAttitude, VehicleStatus

class TerminalServoNode(Node):
    def __init__(self):
        super().__init__('terminal_servo_node')
        
        # Initialize the CvBridge for ROS 2 <-> OpenCV image conversion
        self.bridge = CvBridge()
        
        # Load lightweight YOLOv8 nano model for real-time inference
        self.model = YOLO('yolov8n.pt')
        
        # Define target camera dimensions (Default Gazebo camera resolution)
        self.CAM_W = 640
        self.CAM_H = 480
        self.CENTER_X = self.CAM_W // 2
        self.CENTER_Y = self.CAM_H // 2
        
        # IBVS Proportional Controller Gains (lambda constants)
        self.K_YAW = 0.005    # Maps horizontal pixel error to Yaw rate (rad/s)
        self.K_Z = 0.003      # Maps vertical pixel error to Z-axis descend rate (m/s)
        self.V_FORWARD = 3.0  # Constant forward cruising speed toward the target (m/s)
        
        # Flight state trackers
        self.current_yaw = 0.0
        self.is_armed = False
        self.offboard_setpoint_counter = 0
        
        # Establish High-Reliability QoS Profiles matching PX4's DDS middleware
        qos_profile = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
            history=HistoryPolicy.KEEP_LAST,
            depth=1
        )
        
        # 1. Declare Publishers (Writing commands to PX4)
        self.offboard_control_mode_pub = self.create_publisher(
            OffboardControlMode, '/fmu/in/offboard_control_mode', 10)
        self.trajectory_setpoint_pub = self.create_publisher(
            TrajectorySetpoint, '/fmu/in/trajectory_setpoint', 10)
        self.vehicle_command_pub = self.create_publisher(
            VehicleCommand, '/fmu/in/vehicle_command', 10)
            
        # 2. Declare Subscribers (Reading telemetry and sensor data from PX4)
        self.image_sub = self.create_subscription(
            Image, '/camera', self.image_callback, 10)
        self.attitude_sub = self.create_subscription(
            VehicleAttitude, '/fmu/out/vehicle_attitude', self.attitude_callback, qos_profile)
        self.status_sub = self.create_subscription(
            VehicleStatus, '/fmu/out/vehicle_status', self.status_callback, qos_profile)
            
        # Initialize a 20Hz (50ms) timer loop for offboard heartbeat maintenance
        self.timer = self.create_wall_timer(0.05, self.timer_callback)
        self.get_logger().info("YOLO IBVS Terminal Servo Node Initialized.")

    def attitude_callback(self, msg):
        """ Extracts current vehicle Yaw (rad) from PX4 VehicleAttitude quaternion """
        q = msg.q  # Quaternion format: [w, x, y, z]
        w, x, y, z = q, q[14], q[15], q[16]
        siny_cosp = 2.0 * (w * z + x * y)
        cosy_cosp = 1.0 - 2.0 * (y * y + z * z)
        self.current_yaw = np.arctan2(siny_cosp, cosy_cosp)

    def status_callback(self, msg):
        """ Updates local arming state based on PX4 status """
        self.is_armed = (msg.arming_state == VehicleStatus.ARMING_STATE_ARMED)

    def timer_callback(self):
        """ Maintains offboard control heartbeats at 20Hz to prevent failsafes """
        self.publish_offboard_control_mode()
        
        # Command vehicle to enter Offboard Mode and Arm after initial heartbeats are verified
        if self.offboard_setpoint_counter == 10:
            # Transition flight mode to OFFBOARD (Custom Mode: 1, Sub Mode: 6)
            self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_DO_SET_MODE, 1.0, 6.0)
            # Send ARM command to motors
            self.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0)
            
        if self.offboard_setpoint_counter < 11:
            self.offboard_setpoint_counter += 1

    def image_callback(self, data):
        """ Process front-facing camera frames, run YOLO tracking, and apply IBVS control """
        try:
            # Convert ROS 2 Image msg to OpenCV BGR image
            frame = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except Exception as e:
            self.get_logger().error(f"Image conversion error: {e}")
            return

        # Run real-time YOLO inference (disable verbose output to conserve CPU)
        results = self.model.predict(frame, conf=0.5, verbose=False)
        target_found = False

        for result in results:
            boxes = result.boxes
            if len(boxes) > 0:
                # Target detected! Retrieve bounding box of first detected object
                x1, y1, x2, y2 = map(int, boxes.xyxy)
                u = (x1 + x2) // 2
                v = (y1 + y2) // 2

                # 1. Compute pixel error vector: e(t) = s(t) - s*
                err_u = u - self.CENTER_X
                err_v = v - self.CENTER_Y

                # 2. Compute IBVS workspace velocity commands
                yaw_speed = float(err_u * self.K_YAW)   # Horizontal pixel error drives Yaw velocity
                down_speed = float(err_v * self.K_Z)    # Vertical pixel error drives descend velocity

                # Clamp velocities for structural safety limits
                yaw_speed = np.clip(yaw_speed, -0.5, 0.5)
                down_speed = np.clip(down_speed, -1.0, 1.0)

                # 3. Transform body-frame velocity (FRD) to local georeferenced frame (NED) expected by PX4
                v_north = self.V_FORWARD * np.cos(self.current_yaw)
                v_east = self.V_FORWARD * np.sin(self.current_yaw)
                v_down = down_speed

                # 4. Publish target velocities to PX4 EKF pipeline
                self.publish_trajectory_setpoint(v_north, v_east, v_down, yaw_speed)

                # Draw bounding box and target locking reticle for operator visualization
                cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0), 2)
                cv2.line(frame, (self.CENTER_X, self.CENTER_Y), (u, v), (0, 0, 255), 2)
                target_found = True
                break

        # If target is lost, send a zero-velocity hover command to prevent runaway drift
        if not target_found:
            self.publish_trajectory_setpoint(0.0, 0.0, 0.0, 0.0)

        # Output the live video feedback
        cv2.imshow("ROS 2 IBVS Terminal Guidance Locking...", frame)
        cv2.waitKey(1)

    def publish_offboard_control_mode(self):
        """ Publishes the active control interfaces to the PX4 flight stack """
        msg = OffboardControlMode()
        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        msg.position = False
        msg.velocity = True  # Enable velocity vector control loop
        msg.acceleration = False
        msg.attitude = False
        msg.body_rate = True  # Enable direct Yaw Speed control interface
        self.offboard_control_mode_pub.publish(msg)

    def publish_trajectory_setpoint(self, v_north, v_east, v_down, yaw_speed):
        """ Sends localized NED velocity vectors and Yaw rate to PX4 /fmu/in/trajectory_setpoint """
        msg = TrajectorySetpoint()
        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        msg.position = [float('nan'), float('nan'), float('nan')] # NaN out position to enforce velocity control
        msg.velocity = [v_north, v_east, v_down]
        msg.yawspeed = yaw_speed
        self.trajectory_setpoint_pub.publish(msg)

    def publish_vehicle_command(self, command, param1=0.0, param2=0.0):
        """ Publishes VehicleCommand commands to actuate system states like arming or flight modes """
        msg = VehicleCommand()
        msg.timestamp = int(self.get_clock().now().nanoseconds / 1000)
        msg.param1 = param1
        msg.param2 = param2
        msg.command = command
        msg.target_system = 1
        msg.target_component = 1
        msg.source_system = 1
        msg.source_component = 1
        msg.from_external = True
        self.vehicle_command_pub.publish(msg)

def main(argc=None):
    rclpy.init(args=argc)
    node = TerminalServoNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("Node terminated by operator.")
    finally:
        # Disarm safety protocols on shutdown
        node.publish_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 0.0)
        cv2.destroyAllWindows()
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

5. Implementation Debugging Guide & Coordination

If you are implementing this in your own lab, take note of these critical integration details:

  • Axis Conventions: ROS 2 conventionally represents coordinates in ENU (East-North-Up) and body frames in FLU (Forward-Left-Up). PX4 Autopilot exclusively operates in local NED (North-East-Down) and body FRD (Forward-Right-Down). The mathematical rotation code inside image_callback handles this projection cleanly, translating your body-relative command vectors into georeferenced velocity targets before publishing.
  • QoS (Quality of Service) Requirements: Attempting to subscribe to PX4’s telemetry topics using standard ROS 2 subscribers will fail silently due to QoS mismatch. PX4 requires BEST_EFFORT reliability and TRANSIENT_LOCAL durability for fast sensor streams—ensure your subscriber QoS configurations strictly mirror our Python setup.

🚀 Summary of our Hybrid Navigation Journey

With this terminal phase implemented, our grand “3-Stage Hybrid Navigation System” is complete!

  1. Cruising Phase: GPS is lost; the UAV maintains orientation and estimates position using IMU, Magnetometers, and Barometric sensors via EKF2 in Dead-Reckoning mode.
  2. Mid-Course Phase: To cancel sensor drift, the UAV periodically uses a downward-facing camera to perform Terrain Referenced Navigation (OpenCV / YOLO AVL) against geo-tagged reference maps.
  3. Terminal Phase: Upon nearing the coordinates, the UAV switches to Image-Based Visual Servoing (YOLO + ROS 2 + Offboard), locking onto the visual signature of the target and completing its mission with sub-meter precision.

We hope this series provides a robust, academically grounded framework for your research. If you encounter any camera latency issues, EKF2 tuning roadblocks, or have questions about exporting YOLO models with TensorRT for edge hardware optimization, please leave a comment below!

Good luck with your flight tests, and fly safe!

YouTube Tutorial

Author: maponarooo, CEO of QUAD Drone Lab

Date: July 22, 2026

Similar Posts

답글 남기기