Engineering Projects | Chang Nie

A Robotic Chemist: Full-Stack Dual-Arm Mobile Manipulation for Biochemical Experiments

Sun, 06 Sep 2026 00:00:00 +0000

In the emerging paradigm of automated biochemistry and AI for Science, computational chemistry pipelines and large language models (LLMs) can autonomously predict retrosynthetic pathways and design high-throughput reaction conditions. However, the definitive bottleneck in scientific discovery remains closed-loop physical validation in wet laboratories. Traditional specialized automated liquid handling stations are bulky, inflexible, and mechanically plumbed to dedicated microplates, rendering them incapable of operating standard multi-purpose human-scale laboratory instruments (such as chemical fume hoods, rotary evaporators, benchtop centrifuges, analytical balances, and magnetic stirrers). Developing mobile dual-arm robotic chemists with human-scale dexterity and spatial mobility represents a pivotal breakthrough toward fully autonomous biochemical experimentation.

Nevertheless, transitioning collaborative dual-arm robots from structured industrial assembly lines into unconstrained wet-lab environments exposes severe physical and perceptual bottlenecks. The robotic platform must interact with completely transparent, specular glassware, execute precision mechanical assembly in narrow centrifuge slots, suppress dynamic liquid sloshing, and sustain multi-step execution over dozens of interconnected phases.

This project developed a full-stack mobile dual-arm robotic chemist system deployed on an omnidirectional mobile base paired with dual collaborative manipulators. Integrating multi-view geometric vision, kinematic demonstration retargeting, parameterized atomic skills, and explicit physical state contracts, the system accomplishes autonomous end-to-end wet-lab operations: handling fragile test tubes and beakers, microliter fluid transfers, and symmetrical dynamic counterbalancing in high-speed centrifuges.

Real Laboratory Challenges & Architectural Layers

Unlike structured factory assembly lines with rigid metal parts and dedicated fixturing, a biological or chemical laboratory imposes extreme demands on spatial perception, compliant bimanual dexterity, and execution fault tolerance:

System Layer	Core Engineering Challenge	Technical Solution
Perception & Proprioception	Transparent borosilicate glass and quartz vessels cause active depth sensors to suffer missing data and severe multipath reflections	Fusing multi-view 2D boundary contours, spatial cone geometry, and CAD priors to recover robust 6D grasping poses
Mobility & Workspace Layout	Mobile base docking drift and tight reachability constraints inside chemical fume hoods	Base navigation provides coarse staging; eye-in-hand cameras perform secondary visual servoing to optimize dual-arm manipulability
Precision Contact Skills	Brittle thin-walled glassware vulnerable to damage; narrow centrifuge slots require strict coaxial entry	Decoupling manipulation into parameterized atomic skill blocks governed by compliant admittance control and adaptive micro-motion search
Fluid Dynamic Stability	Dynamic fluid sloshing, surface tension meniscus clinging, and droplet spillage during rapid arm transit	Parameterizing jerk-bounded, smooth acceleration motion profiles paired with visual feedback during volumetric pouring
Agentic Planning & Recovery	Compounding multi-step execution errors across multi-hour experimental protocols	VLM-driven task hierarchy checking explicit pre/post physical state contracts before and after each action, triggering localized self-healing retries

System & Algorithmic Architecture

Experimental Intent → Skill Learning → Physical Execution → Closed-Loop Feedback

Mobile base traverses workstations; coordinated dual arms execute precision manipulation; hierarchical agents invoke skills with closed-loop physical verification.

Skill-learning route

01Demonstrate and mapTemporal motion estimation · human-to-robot mapping

02Learn across sim and realScene reconstruction · teacher/student policies · transfer

03Reusable skillsGrasp · place · pour · workstation interaction

Task-execution route

04Interpret task and sceneInstructions · object states · prerequisites

05Coordinate the whole bodyMobile base · two arms · reachability and contact

06Assess and recoverCheck completion · retry or replan

Execution feedback → scene updates and skill improvementResearch objective: use physical interaction failures to guide the next learning cycle.

Core Technical Solutions: From Physics Priors to Closed-Loop Self-Healing

To guarantee dependable unattended operation at the physical chemistry bench, we developed an integrated processing pipeline spanning multi-modal perception, ergonomic retargeting, smooth fluid control, and contract-governed self-healing.

1. Robust 6D Pose Estimation on Transparent & Specular Glassware

In wet laboratories, beakers, test tubes, and graduated cylinders are predominantly fabricated from transparent borosilicate glass or fused quartz. Structured-light active infrared patterns pass directly through the glass walls, generating complex internal refractive caustics that destroy active depth triangulation and yield massive depth dropouts.

To bypass active depth degradation, we constructed a multi-view boundary contour intersection and parametric CAD constraint pipeline:

Contour Extraction & Spatial Cone Intersection: Multi-angle high-resolution cameras capture glassware from calibrated vantage points. By back-projecting extracted rim contours into space, the system builds intersecting visual cones whose common intersection resolves the 3D position and surface orientation of the opening;
Parametric Model Alignment: Extending along the central symmetry axis, the system fits a cylindrical geometric model against valid local point clouds, bounding 6D pose errors within reliable tolerances for compliant grasping.

2. Kinematic Retargeting of Human Demonstration & Singularity Avoidance

Expert chemists handle volumetric pipettes, unscrew threaded caps, and pour liquids with innate smoothness and dexterity. To capture this expert intuition, we deployed an optical-inertial motion-capture demonstration rig. However, collaborative manipulators are strictly bounded by joint limits, link lengths, and kinematic singularities. Blind Cartesian waypoint replication frequently triggers joint velocity spikes or self-collisions against the workbench.

We engineered a task-space kinematic retargeting algorithm with manipulability optimization:

Invariant Feature Extraction: Isolating inter-vessel relative poses, approach/retreat normal vectors, and characteristic velocities while filtering non-rigid human tremor;
Manipulability Optimization: Resolving inverse kinematics by projecting manipulability criteria into the null space, ensuring the robot mirrors human bimanual coordination while operating within singularity-free, high-stiffness joint configurations.

3. Precision Alignment and Compliant Centrifuge Assembly

Benchtop centrifuges feature angled cylindrical rotor holes with tight assembly clearances. If the tube axis deviates upon entry, the tube rim binds against the rotor block, risking tube damage or joint overcurrent trips.

We designed a three-stage cascade assembly controller:

Coarse Staging: Following mobile base docking, the arms convey the tube to a staging plane directly above the designated rotor slot;
Eye-in-Hand Coaxial Servoing: High-resolution wrist cameras perform contour fitting on the rotor aperture, extracting relative lateral offsets and axial tilt, dynamically driving coaxial alignment errors toward rapid convergence;
Axial Compliant Insertion & Helical Micro-Search: The manipulator engages axial impedance control, translating vertically downward at low speed while monitoring contact force. If resistance indicates edge binding, the controller executes a localized micro-motion search, allowing the tube to seat cleanly and reliably into the rotor socket.

4. Fluid Motion Stabilization & Meniscus-Monitored Pouring

Transporting open beakers and vials introduces dynamic fluid sloshing, risking chemical spillage and center-of-mass perturbations:

Smooth Trajectory Generation: Filtering acceleration and deceleration commands with jerk-bounded smooth profiles tailored to container dimensions and fluid fill levels, eliminating resonance during high-speed transit and rapid directional changes;
Pouring Meniscus Vision Feedback: When pouring reagents between vessels, dual arms coordinate tilt angle while wrist cameras observe the fluid surface tension meniscus and droplet detachment, providing closed-loop control over volumetric transfer.

Beaker grasp and transfer

Slender-object grasping

Tube alignment and placement

5. Physical State Contracts & Long-Horizon Self-Healing Execution

In long-horizon biochemical workflows (e.g., retrieve vial, unscrew cap, weigh on balance, pipette reagent, vortex mix, symmetric centrifuge load, spin, separate supernatant), open-loop execution inevitably fails when a single micro-slip occurs.

We formulated a hierarchical self-healing execution engine governed by physical state contracts:

Hierarchical Task Orchestration: High-level experimental protocols are compiled into a topological graph of task milestones. A Vision-Language-Action agent handles multi-step sequence reasoning, mid-level coordinators instantiate parameterized skill primitives, and low-level controllers maintain high-frequency feedback loops;
Pre- and Post-Condition Physical Assertions: Every atomic skill is bound to deterministic multi-modal verification contracts (e.g., gripper aperture checks and weight step detection upon grasping, and diametric symmetry verification before centrifuge loading);
Localized Self-Healing Fallback Branches: If a post-condition verification fails, the agent does not abort the experiment. Instead, it activates targeted local recovery branches (open gripper, retract to safe observation pose, re-scan multi-view geometry, and re-attempt grasp with adapted parameters), substantially increasing overall protocol completion.

Real-Robot Experiments & Demos

Precision Tube & Beaker Coordinated ManipulationReal-time recording: the dual-arm robot autonomously identifies labware, executes stable adaptive grasps, and performs precision fluid transfer.

Autonomous Centrifuge Workstation OperationReal-time recording: the robotic arm localizes narrow centrifuge slots with high precision, executing smooth tube insertion and balanced loading.

The video recordings above show the robot in action at a real wet-lab bench:

Transparent Labware Retrieval & Fluid Transfer: Autonomous 6D pose extraction of transparent tubes and open beakers under complex lab lighting, followed by smooth bimanual liquid aspiration, transfer, and dispensing without sloshing or surface spatter;
Precision Centrifuge Workstation Manipulation: Sub-millimeter coaxial insertion of micro-tubes into tight, angled centrifuge rotor sockets, accompanied by symmetric counterbalance loading and smooth retraction out of the fume hood.

Engineering Summary & Technical Takeaways

Mobile dual-arm manipulation in biochemical laboratories represents the definitive proving ground for robotic dexterity bridging deterministic robotics and complex physical reality:

Overcoming Optical Degeneracy via Geometric Priors: Transparent and specular glassware defeats naive depth sensors. Fusing 2D sub-pixel boundary geometry, spatial cone intersections, and implicit 3D scene representations provides the prerequisite foundation for reliable physical contact.
Embedding Physical Dynamics into Trajectory Generation: Handling delicate glassware and fluid reagents requires multi-body dynamic constraints. Explicitly incorporating input shaping, slosh suppression, and force admittance into low-level controllers enables clean, professional-grade laboratory execution.
Explicit State Contracts as the Bedrock of Long Horizons: In open-ended, multi-step scientific workflows, open-loop assumptions are fatal. Arming every atomic manipulation skill with verifiable physical assertions and local recovery branches creates the physical closed loop essential for real-world laboratory automation.
Hardware-Software Co-Design for Unstructured Workspaces: In confined fume hood environments, decoupling mobile base global mobility from dual-arm local manipulability is essential for preventing joint limits and wrist singularities.

CUHK Collaboration: Mobile Dual-Arm Manipulation for Library Shelf Tidying and Book Reshelving

Mon, 21 Sep 2026 00:00:00 +0000

In semi-structured physical environments, returning a book to a shelf appears deceptively simple. For an autonomous mobile dual-arm robot, however, it presents severe multi-physics challenges spanning spatial geometry, contact dynamics, and long-horizon decision-making:

Books on a library shelf are packed tightly side by side, subject to substantial lateral compression and surface friction, leaving clearances often strictly in the range of 1 to 2 millimeters;
Book spines exhibit varied textures and reflective laminated plastic coverings, frequently inducing orientation estimation errors in optical 6D pose estimators;
If the mobile base stops merely a few centimeters off-target, the arm rapidly hits joint singularities or mechanical limits when reaching deep into the shelf slot, or occludes the optical line-of-sight of wrist-mounted cameras;
Extracting or inserting a book without lateral force regulation easily triggers a domino-style collapse of neighboring leaned volumes.

This project is an ongoing research collaboration with . We focus on mobile dual-arm manipulation for autonomous library inventory, shelf reshelving, and tidying. I lead the AI Agent decision-making and physical execution loop, investigating how high-level tidying goals translate into reliable, verifiable physical actions.

Project context

CUHK · Prof. Yunhui Liu's Group

Library book reshelving represents a challenging semi-structured long-horizon manipulation task: reflective laminated spines, tightly compressed leaning books, millimeter clearances, and tight coupling between base parking and arm manipulability.

Projected Goals & Research Focus: End-to-end workflow for dense shelf extraction and millimeter slot insertion; targeting >92% bimanual extraction success and >80% jam recovery success in anticipated hardware deployment.

My contributions

As project lead, established the mobile dual-arm manipulation framework; responsible for coaxial visual servoing and closed-loop agent execution, setting research targets for bimanual extraction (>92% success rate) and jammed slot recovery (>80% recovery rate) in 1.5–3.0 mm tight shelf clearances.

Physical Challenges and the Fundamental Paradigm Shift of Mobile Manipulation

Transitioning dual-arm manipulation from fixed industrial workcells to a mobile wheeled platform represents a fundamental paradigm shift in both theoretical formulation and physical execution. In a fixed workcell, an unobserved object is typically a localized line-of-sight occlusion problem; the arm base is rigidly anchored to the workstation, and inverse kinematics (IK) possesses a static, deterministic geometric reachability envelope. In mobile manipulation, however, the problem expands across three critical physical dimensions:

Environment-Scale Partial Observability: The target book may reside on an entirely different shelf rack, in an adjacent aisle, or placed in a temporary return cart by human visitors. The robot must not only infer localized occlusions within its current view frustum, but must also perform global spatial reasoning across rooms and aisles, planning active exploratory navigation paths while maintaining a persistent topological and object-centric memory graph.
Base Placement as a First-Class Decision: The terminal parking pose of the mobile base directly dictates the kinematic feasibility of all subsequent manipulation motions. Stopping too close to the shelf prevents the arm from unfurling its elbow joints, risking collisions with narrow aisle walls behind the robot; stopping too far leaves deep shelf slots unreachable. Furthermore, the base yaw angle directly constraints the line-of-sight of onboard cameras toward spine boundaries and target slot gaps. Base docking can never be relegated to an independent 2D navigation stack; it must be treated as a first-class decision variable driven by predicted manipulation payoff.
Uncertainty Accumulation across Navigation-Manipulation Cycles: After executing a long transit path and braking to a halt, wheel odometry slippage, mechanical gear backlash, and chassis suspension compliance introduce residual base pose errors on the order of 1 to 3 centimeters and several degrees in heading. Executing pre-planned arm trajectories open-loop from global navigation coordinates inevitably results in hard physical collisions. The system must enforce a deterministic sequence of secondary local perception, pose refinement, and compliant force-closed feedback.

Research architecture

Observe the shelf. Plan the placement.

Parking changes visibility and reachability. Re-observe after moving; verify after placing.

Observe and decide

01Identify books and slotsIdentity · spatial relations · occlusion

02Update the World ModelRegion memory · local state · uncertainty

03Select a pose and skillsAI Agent · visibility · reachability

Act and verify

04Navigate, park, re-observeCorrect parking error · align locally

05Extract and reshelveTwo-arm coordination · support · insertion

06Verify and recover locallyCorrect book · correct slot · bounded retries

Observations and outcomes → revised state and planSimulation and demonstrations support skill development; the control layer constrains physical motion.

Software-Hardware Division and the Deterministic Control Boundary

To deploy complex AI reasoning onto physical hardware safely, our architecture establishes an uncompromising division of responsibilities: the AI Agent and the World Model operate strictly at the task and skill level, governing high-level discrete state transitions, base parking candidates, verification scheduling, and bounded recovery policies. Low-level trajectory interpolation, kinematics, collision checking, torque limits, and emergency stopping are enforced deterministically by the robot control stack.

High-Level Agent Planning System: Implemented within the ROS 2 ecosystem, running a dedicated local language model adapter (agent_planner). It consumes the structured symbolic task state, registered skill definitions, and the object-centric world model, outputting structured plans strictly adhering to predefined JSON schema declarations.
Low-Level Deterministic Control Stack: Built upon ROS 2 and MoveIt 2 for kinematic computation and planning scenes. Free-space arm motions utilize OMPL for collision-free trajectory generation, while slot insertion and retraction strictly invoke Cartesian straight-line interpolation. Mobile base navigation relies on Nav2 maintaining layered costmaps, coordinated with an omnidirectional mobile chassis to provide smooth translational and rotational braking.
Onboard Compute and Real-Time Bus: Computation is hosted on an onboard industrial GPU workstation running perception segmentation, world model maintenance, local trajectory synthesis, and skill status monitoring. Low-level motor controllers communicate with arm joints and base wheel drives over high-speed real-time Ethernet buses, guaranteeing millisecond-level emergency stops whenever an anomalous wrench or deviation is detected.

Joint Optimization of Base Docking and Arm Manipulability

In narrow library aisles where aisle clearance is typically restricted to 0.8 to 1.0 meters, decoupling mobile base parking from manipulator trajectory generation causes severe kinematic deadlocks. We formulate base pose selection as a multi-objective constrained optimization problem, jointly considering arm manipulability (steering clear of singularities and travel limits), wrist camera line-of-sight margins (preventing link self-occlusion), and navigation travel and safety clearances.

Once the base completes navigation and comes to a complete halt, the robot triggers a two-stage local visual refinement routine: onboard stereo and wrist cameras reconstruct local shelf fiducials and target spine edges, computing an incremental correction matrix that absorbs the 1-3 cm base drift before any arm motion commences.

Coordinated Bimanual Manipulation and Compliant Contact Mechanics

The most fragile phase of library shelf manipulation lies in the lateral force interactions during book extraction and slot re-insertion. Under tight packing, pulling a book out with a single arm drags adjacent leaning volumes through surface friction, provoking domino-style collapses. Conversely, during insertion, even sub-degree angular misalignment locks the book against slot edges. We address this through a specialized bimanual strategy.

1. Stabilizing Arm: Lateral Preloading and Anti-Collapse Support

Prior to any contact between the primary manipulator and the target book, the secondary stabilizing arm extends and positions a compliant, high-friction pad against the outer edge of the adjacent leaning book cluster, applying a regulated lateral preload (typically maintained at 5 N to 8 N):

This steady normal force functions as an artificial rigid sidewall, counteracting lateral friction forces induced when the target volume slides out;
A 6-axis force-torque sensor at the stabilizing wrist continuously tracks lateral reaction forces. If anomalous cluster slippage is detected, the controller adaptively adjusts contact depth to preserve the quasi-static equilibrium of the shelf.

2. Primary Manipulating Arm: Tangential Compliant Extraction

The primary arm is equipped with a slim, adaptive parallel-jaw gripper. Perception algorithms locate the upper and lower spine vertices alongside the surface normal, guiding the gripper tips to slide tangentially into the narrow book gaps:

Gripper closing combines motor current sensing with optical displacement feedback, clamping the book spine firmly without crushing fragile paper covers;
Extraction proceeds along the shelf normal. The manipulator operates in an admittance-controlled mode along lateral (Y) and pitch axes, allowing minor passive compliance to accommodate slight alignment errors without tearing delicate book jackets.

3. Tilted Micro-Search and Compliant Slot Insertion

Re-inserting a book into an ultra-narrow slot represents the highest contact-dynamic hurdle. We implement a three-phase insertion protocol:

Tilted Initial Contact: The bottom corner of the book is tilted at a slight angle of 12 to 15 degrees, reducing the initial contact geometry from an area contact to an edge-point contact, substantially diminishing sliding resistance;
Admittance Control and Compliant Search: In the Cartesian task frame, the manipulator tracks a compliant admittance model. The forward insertion axis is parameterized with low virtual stiffness, automatically slowing down upon sudden thrust increases while applying subtle oscillatory dither motions to guide the book along the natural boundaries of the slot;
Full Home Seating: When reaction forces drop to nominal levels and optical distance sensors confirm the spine is flush with the shelf plane, the gripper releases smoothly and retracts.

Object-Centric Shelf World Model

Maintaining the physical state of dozens of individual books across an aisle exceeds the capability of raw dense voxel grids or unstructured point clouds, which incur prohibitive computational overhead and lack semantic handles. We developed an object-centric shelf world model specifically tailored for dense manipulation.

1. Structured Node Representations

The world model represents the environment as an explicit geometric-semantic heterogeneous graph. Each node corresponds to an individual book entity or shelf partition unit:

Spatial Geometry: 3D oriented bounding boxes (3D OBB), 6D spine reference frame, and posterior pose covariance $\Sigma_p$;
Semantic Identity: Optical character recognition (OCR) spine text, barcode/RFID identifiers, and visual feature embeddings;
Topology and Contact: Left neighbor ID, right neighbor ID, supporting shelf plane, tilt angle $\phi_{\text{tilt}}$, and lateral tightness metric.

2. Multi-View Spatial-Temporal Data Association

As the mobile base traverses the aisles, incoming point clouds and RGB streams are continuously associated with existing graph nodes via Hungarian matching over a cost matrix combining 3D Euclidean proximity, visual appearance similarity, and relative topological sequence. When an existing book is re-observed from a different vantage point, a Kalman filtering pipeline updates its 3D pose estimate and reduces uncertainty ellipses. When a previously occupied coordinate is observed to be empty, the model updates the node state to a vacant slot.

3. Counterfactual Simulation for Action Safety

The world model enables the AI Agent to conduct counterfactual forward lookahead before executing physical actions:

Evaluating whether the manipulator’s swept volume will violate collision margins with shelf uprights from a candidate base pose;
Simulating whether extracting a target volume will destabilize adjacent volumes based on gravity vectors and friction coefficients;
If counterfactual analysis indicates collapse risk, the agent proactively generates a coordinated bimanual plan incorporating the stabilizing arm rather than attempting single-arm manipulation.

AI Agent Task Orchestration and Bounded Error Recovery

As lead of the AI Agent workstream, I developed a unified task abstraction layer bridging high-level semantic reasoning with low-level execution via Behavior Trees (BehaviorTree.CPP) and parameterized skill contracts.

1. Structured Task Planning and Schema Validation

When presented with open-ended natural language instructions (such as “Retrieve three misplaced computer science books from the return trolley, place them onto Shelf B2, and tidy the leaning stack”):

The onboard agent_planner digests the symbolic state from the world model (including robot region, shelf vacancies, and trolley inventory) through a local language model, decomposing the directive into a directed acyclic task graph;
Action outputs are restricted to registered skills: NavigateToArea, DockAtShelf, ScanSpines, StabilizeCluster, ExtractBook, VerifyHolding, InsertBook, RetractArm;
A deterministic schema validator verifies that all parameters, coordinate frames, and target IDs are mathematically sound and physically present before generating the reactive behavior tree.

2. Explicit Physical Verification Contracts

To avoid catastrophic cascading failures caused by open-loop assumptions, each atomic skill is bound by strict sensor-driven preconditions and postconditions:

For ExtractBook, preconditions require that the stabilizing arm has verified normal force balance and the gripper is aligned with the spine axis;
Postconditions require multi-modal verification: optical beam interruption on the gripper fingers combined with finger encoder positions confirming that the gripped width corresponds to the expected book thickness. If verification fails, the skill returns an explicit failure state, halting forward progression.

3. Multi-Tier Bounded Error Recovery

Mechanical binding during slot insertion is an inherent physical phenomenon due to paper curvature and localized friction. We embed bounded recovery routines:

Local Micro-Retreat: If insertion force exceeds safety limits (such as thrust exceeding 25 N) prior to target depth, the behavior tree triggers a local recovery branch: retracting 20 mm, applying a 1.5-degree yaw adjustment, and re-attempting compliant insertion;
Bounded Escalation and Safe Abort: If three consecutive recovery attempts encounter persistent jamming, the system terminates insertion, pulls the book back safely, transfers it to the onboard carriage, marks the slot as jammed in the world model, logs the anomaly, and proceeds to the next item, preventing a localized mechanical failure from paralyzing the overarching mission.

Progressive Milestones & Projected Performance Targets

The framework is architected across four difficulty milestones (M1 to M4):

M1: Meeting-Room Clearing. Validating natural language instruction parsing, reachability-driven docking, basic single- and dual-arm manipulation, and onboard carriage payload handling in open environments;
M2: Library Shelf Tidying and Reshelving. Introducing narrow aisle navigation, dense book extraction under compression, millimeter-clearance compliant insertion, bimanual stabilization, and jammed-slot recovery;
M3: Traceable High-Value Transport. Adding sealed container handling, delicate vial grasping, barcode/RFID verification, and audit logging;
M4: Unseen Open-World Generalization. Evaluating long-horizon stability and object-centric topological reasoning across novel room configurations without prior maps.

For M2 dense shelf tidying simulation and experimental benchmarking, the project defines concrete target objectives:

In dense shelf arrays with clearances between 1.5 and 3.0 mm, coordinated bimanual extraction targets an overall success rate exceeding 92%, aiming to prevent neighbor domino collapses;
Base docking optimization targets placing the manipulator in high-dexterity operating envelopes on >88% of initial attempts, minimizing realignment latency;
Under induced binding disturbances, bounded error recovery is designed to resolve over 80% of frictional jams without human intervention.

Engineering Lessons and Technical Takeaways

This project provided deep practical insights into the physical realities of mobile dual-arm manipulation in dense environments:

The Inviolable Role of Deterministic Control: While modern end-to-end vision-language-action models offer broad generalization, their high-frequency output jitter remains hazardous during tight-clearance physical contact. Confining AI models to discrete task planning and counterfactual reasoning while delegating low-level execution to classical admittance control, OMPL, and behavior trees provides the necessary foundation for real-world hardware reliability.
Mobility and Manipulation Must Be Unified: A mobile manipulator is fundamentally more than a robotic arm mounted on a mobile platform. Treating base docking as an independent navigation waypoint is the primary cause of kinematic singularities and visual occlusions. Deriving optimal base docking manifolds directly from manipulation constraints translates mobility into an active extension of the arm’s workspace.
Physical Robustness Arises from Rigorous Error Defense: In long-horizon real-world operations, expecting zero physical failures is unrealistic. True system resilience lies not in preventing all errors, but in verifying physical states against reality and executing bounded local recovery when anomalies occur. The architectural paradigms developed here provide enduring foundations for dense shelf handling, semi-structured mobile manipulation, and embodied robotic systems.

Warwick Collaboration: Decoupled Hierarchical Embodied AI and Agentic Robotics for Long-Horizon Tasks

Mon, 21 Sep 2026 00:00:00 +0000

In embodied robotic manipulation within open, unstructured environments, robots face profound physical uncertainty. Whether performing multi-step industrial assembly, automated laboratory specimen preparation, or parcel sorting in semi-structured warehouses, practical missions frequently encompass dozens of consecutive physical interactions. In such extended horizons, minor unmodelled errors introduced during any individual action compound exponentially, precipitating global task failure if left unmitigated.

This project is an ongoing research collaboration with the robotics and sensor systems group at the University of Warwick. We focus on developing a hierarchical embodied AI and closed-loop AI Agent architecture designed specifically for long-horizon physical manipulation. I am responsible for designing and deploying the full-stack software and hardware robotic system, bridging multimodal perception, high-level task decomposition, metric geometric motion synthesis, and low-level Cartesian impedance control to establish an autonomous operating pipeline with physical self-healing capabilities.

Project context

University of Warwick · Professor Sai Gu

In complex real-world environments, pure end-to-end models struggle to balance multi-minute logical consistency with high-frequency geometric control. We develop a decoupled hierarchical framework where high-level agents orchestrate subgoals while low-level controllers handle geometry and contact dynamics.

Projected Objectives & Research Focus: Closed-loop 8–15 step physical embodied manipulation; targeting >84% end-to-end success, >70% localized recovery, and <3 mm 3D localization error under multimodal fusion.

My contributions

As project lead, formulated the hierarchical agent manipulation architecture; for 8–15 step long-horizon tasks, established target objectives to elevate end-to-end task success rate from 35% to >84%, with >70% autonomous recovery from localized physical anomalies.

Physical Brittleness and the Challenge of Compound Error Propagation

In embodied manipulation, the fundamental barrier to scaling long-horizon tasks lies in compound error propagation. Consider a representative physical task composed of 10 sequential manipulation stages (e.g., identifying a chemical vial on a cluttered bench, uncapping the vessel, aspirating a reagent, re-threading and torquing the cap, transferring the vessel to an assay tray, and stowing it in an incubator). Even if every individual atomic skill possesses an isolated execution reliability of 90%, under open-loop execution the cumulative reliability decays exponentially to roughly 35% across 10 stages, and even further as steps increase. This compounding decay demonstrates that without explicit physical verification contracts and bounded local recovery mechanisms, open-loop robotic systems are virtually guaranteed to fail midway. Contemporary research exhibits two prevalent paradigms, both of which reveal distinct physical vulnerabilities when applied to real-world hardware:

Vulnerabilities of Pure End-to-End Visuomotor Policies (VLA Models): End-to-end policies excel at reactive short-horizon reflex behaviors within bounded training distributions. However, across operational timelines spanning several minutes and dozens of diverse actions, latent vector representations suffer from severe temporal drift. Critically, contemporary vision-language-action models operate at inference rates between 5 Hz and 20 Hz, which is fundamentally inadequate for the hundreds-of-hertz control loops required for stable contact dynamics. Furthermore, when unexpected mechanical binding or slippage occurs, end-to-end models lack explicit mechanisms for backtracking, fault localization, and structured error recovery.
Geometric Hallucinations in Direct LLM Waypoint Generation (LLM-as-Planner): Inquiring large language models to directly generate end-effector SE(3) waypoints or joint trajectories yields semantic commonsense but lacks metric spatial reasoning and inverse kinematics (IK) feasibility awareness. The resulting trajectories routinely violate joint velocity or torque limits, penetrate obstacle envelopes, or command configurations within singular regions. Crucially, direct text generation fosters an open-loop hallucination that issuing an action directive is physically equivalent to successfully completing the interaction.

To overcome these structural limitations, our architectural paradigm enforces hierarchical decoupling between task planning and motion synthesis, combined with strict closed-loop physical state verification. High-level symbolic reasoning is isolated within the AI Agent, high-frequency trajectory tracking and contact compliance are governed by deterministic real-time controllers, and multimodal sensing provides empirical ground truth across every execution transition.

Research architecture

3D perception → hierarchical decisions → robot execution

Task planning and motion control coordinate through observations and execution state.

Sensing and spatial understanding

01Vision and mmWaveComplementary observations · multimodal sensing

023D scene representationObjects · geometry · spatial relations

03Language and temporal groundingInstruction meaning · relevant events

AI Agent-based robot system

04High-level task decisionsInterpret instructions · organize subgoals

05Low-level executionRobot skills · motion control

06Feedback and updatesObserved outcomes · continued planning

My responsibility: build the robot system linking perception, planning, and executionIntegrate the collaboration's research components in a shared robot task loop.

Three-Tier Decoupled Embodied AI Architecture

To achieve semantic adaptability while preserving physical execution determinism, the system is organized into three strictly partitioned hierarchical layers:

1. Level 3: Semantic and Long-Horizon Task Planning Layer (AI Agent Brain, 0.5 Hz - 2 Hz)

Responsible for parsing open-ended natural language directives or complex procedural task objectives into a directed acyclic task graph (DAG) or a reactive behavior tree:

The agent operates via an onboard language model adapter, consuming symbolic state inputs comprising the robot’s current status, a spatial-semantic scene graph, and temporal execution history;
Candidate agent outputs are strictly restricted to a registered library of formally verified parameterized atomic skills (such as ApproachObject, AlignEndEffector, ExecutePinchGrasp, VerifyEngagement, LinearExtract);
Level 3 produces no direct motor current or joint torque commands; its decision manifold resides entirely within the domain of task sequencing, skill invocation, and verification scheduling.

2. Level 2: Metric-Geometric Skill Synthesis Layer (Skill Synthesis & Trajectory Generator, 10 Hz - 50 Hz)

Translates parameterized skill specifications from Level 3 into metric SE(3) Cartesian trajectories and joint space paths:

Ingesting a dynamic Euclidean Signed Distance Field (ESDF) maintained from onboard depth cameras and radar point clouds, the layer invokes OMPL and the Pinocchio rigid-body dynamics library to compute time-parameterized polynomial trajectories compliant with joint velocity and acceleration bounds;
For contact-intensive phases (such as drawer sliding, cap threading, or peg-in-hole insertion), the planner seamlessly switches to Cartesian linear interpolation and helical micro-search primitives, continuously auditing kinematic continuity and self-collision clearance.

3. Level 1: High-Frequency Compliant Execution Layer (Deterministic Real-Time Control, 500 Hz - 1000 Hz)

Serves as the low-level hard real-time foundation, driving joint actuators over deterministic buses while governing physical contact forces:

Executes Cartesian space impedance control: in free-space transit, stiffness is elevated to guarantee tracking accuracy; during physical contact, virtual stiffness is attenuated along contact axes to provide compliant deflection, absorbing positional misalignments;
Actuator currents and 6-axis wrist force-torque sensors are queried at 1 kHz against hardware-level safety thresholds. In the event of an anomalous collision wrench, the controller triggers a controlled stop within 2 milliseconds, preventing hardware damage.

Multimodal 3D Spatial Perception: Visual-mmWave Radar Fusion

In unstructured operational environments, relying exclusively on optical vision exposes the robotic system to catastrophic sensor degradation. In backlit conditions, structured-light and active stereo depth sensors suffer pattern saturation, inducing substantial depth dropouts. On transparent glassware or specular metallic tooling, specular reflections cause false point-cloud shifts of several centimeters. Furthermore, airborne dust or aerosol suspensions completely attenuate optical propagation.

Leveraging Warwick’s extensive expertise in advanced electromagnetic sensing and industrial digitalization, we developed a synchronized RGB-D and 60-77 GHz FMCW Millimeter-Wave (mmWave) Radar spatial perception architecture:

1. Complementary Physical Sensing and Microsecond Synchronization

Optical Sensors: Deliver dense, megapixel-resolution RGB imagery with fine contour boundaries and rich semantic segmentations, capturing micro-scale visual geometry, yet remain highly vulnerable to ambient illumination and optical occlusions;
Millimeter-Wave Radar: Exploits millimeter-range electromagnetic wavelengths that penetrate dust, smoke, and moisture, remaining impervious to illumination extremes; FMCW radar directly returns radial distance and high-precision Doppler velocities, though it exhibits coarse angular resolution and susceptibility to multipath ghost reflections;
Spatial-Temporal Calibration: Utilizing specialized retro-reflective geometric calibration targets, we solve for the rigid spatial extrinsic transformation matrix; hardware synchronization utilizes a Pulse-Per-Second (PPS) reference signal, constraining temporal projection jitter below 1 millisecond.

2. Kinematic Clutter Filtering and Log-Odds Volumetric Occupancy Mapping

To eliminate multipath clutter, the perceptual front-end applies Constant False Alarm Rate (CFAR) adaptive thresholding alongside Doppler velocity distribution analysis, isolating static structural landmarks from moving obstacles.

For volumetric mapping, we maintain a 3D log-odds temporal voxel occupancy grid, fusing optical depth projections with radar backscatter cross-sections via a recursive Bayesian framework. Through this design, when optical cameras experience blinding glare, radar range returns maintain high occupancy confidence, preventing the manipulator from colliding with undetected obstacles; conversely, when radar returns exhibit angular blooming near metallic edges, optical high-resolution masks delineate crisp object surfaces, establishing an all-weather geometric foundation.

Video Temporal Grounding and Physical State Verification

In long-horizon execution, verifying that a preceding manipulation stage has successfully attained its objective is the cornerstone of closed-loop robustness. If the system relies purely on elapsed trajectory time or encoder confirmations to assert success, undetected slippage or jamming will cascade through downstream tasks.

We incorporate a video-semantic grounding mechanism: throughout execution, the system maintains a circular video buffer $\mathcal{V}_{[t-\Delta t, t]}$ spanning window length $\Delta t$. The high-level agent establishes explicit condition descriptors for each sub-action (such as “vessel base lifted completely clear of the tabletop” or “slider rail pulled to terminal mechanical stop”). Utilizing cross-modal cross-attention layers, the model grounds semantic milestones along the temporal video timeline, extracting verifiable timestamped evidence of physical state transitions.

To guard against optical occlusions, every atomic skill is bound to rigorous multi-modal physical assertions:

Visual Assertions: The 3D bounding box centroid of the target object must exhibit a rigid vertical translation exceeding threshold $\Delta z > h_{\text{thresh}}$, accompanied by the visual reappearance of the supporting tabletop texture;
Force-Dynamic Assertions: The wrist 6-axis force-torque sensor must register a vertical gravitational wrench increase $\Delta F_z$ concordant with the object’s calibrated mass ($ \Delta F_z \approx m_{\text{object}} \cdot g \pm \epsilon_f $), while internal gripper finger encoders confirm a grasp aperture matching the item’s metric profile;
Deterministic Gating: Only when both visual geometric evidence and force metrics concurrently satisfy verification criteria does the skill node return a SUCCESS token to the behavior tree. If either assertion fails, execution is immediately intercepted.

3. Hierarchical Bounded Error Recovery

Upon verification failure, the system avoids indiscriminate aborts or blind task restarts. Instead, it triggers localized bounded recovery trees based on the failure classification:

Incipient Slip and Empty Grasps: If force sensing indicates an unweighted gripper, slippage is identified. The behavior tree executes a local re-observation and grasp re-estimation routine, adjusting gripper approach yaw and re-attempting acquisition;
Mechanical Binding and Over-Torque: During insertion or mating, if resistive wrenches surge prior to reaching target depth, alignment binding is diagnosed. The controller executes a structured sequence: retract 15 mm, apply micro-scale dither perturbations to find the slot axis, and resume compliant low-stiffness insertion;
Bounded Escalation and Safe Holding: If repeated recovery attempts reach a pre-defined threshold (e.g., 3 attempts) without resolution, the system gently places the object into a designated buffer dock, decrements object confidence within the scene graph, logs time-series diagnostic data, and switches to an alternate independent subtask, preserving overarching operational uptime.

System Integration & Projected Target Objectives

The system architecture combines a visual-mmWave multimodal perception rig, a hierarchical behavior tree engine, and a 6-DOF compliant manipulator, establishing clear target benchmarks across 8-to-15-step sequential physical manipulation tasks:

Long-Horizon End-to-End Target Completion: By enforcing physical verification contracts and bounded error recovery, the framework targets elevating end-to-end multi-step task success from an open-loop 35% baseline to >84%, with an objective of autonomously resolving >70% of localized execution anomalies without human intervention;
Perception Robustness Objectives under Degraded Conditions: In harsh illumination and airborne particulate environments, the visual-radar fusion pipeline aims to constrain 3D boundary and distance tracking RMSE within 3 millimeters;
Geometric and Kinematic Consistency: The Level 2 motion planning layer is architected to guarantee 100% inverse kinematics solvability and boundary clearance across all agent-synthesized directives, eliminating joint limit collisions.

Engineering Lessons and Technical Takeaways

Developing this hierarchical embodied architecture yielded several fundamental engineering insights into robotic deployment:

Embodied AI Is Fundamentally an Engineering Discipline in Managing Uncertainty: In purely virtual or simulation environments, action outcomes are conveniently abstracted into deterministic transitions. In the physical world, mechanical tolerances, surface friction, thermal drift, and lighting shifts are persistent realities. A robust embodied architecture succeeds not by naively assuming a perfect environment, but by adopting a defensive posture that rigorously audits physical states after every contact and provides deterministic recovery pathways when deviations occur.
Language Models Excel at Symbolic Reasoning; Continuous Mechanics Belongs to Physics-Based Control: Large language models possess unmatched breadth in semantic parsing and long-horizon task structuring. However, forcing them to output high-frequency continuous joint commands is structurally unsound. Positioning the language model as a high-level strategist while assigning low-level trajectory generation, collision checking, and impedance control to deterministic robotics libraries achieves the optimal balance between open-world intelligence and physical safety.
Sensor Diversity Requires Orthogonal Physical Modalities: Redundantly stacking identical optical sensors fails to resolve structural weaknesses such as ambient backlighting, optical glare, or particulate obscuration. Incorporating millimeter-wave radar introduces an orthogonal electromagnetic sensing modality that compensates for fundamental optical blind spots, raising the lower bound of perceptual reliability in harsh, real-world manipulation environments.

Industrial Logistics Robotics: Autonomous Dual-Arm Mobile Manipulation for Heavy Tote Handling

Wed, 01 Jan 2025 00:00:00 +0000

In flexible manufacturing and warehouse environments, fixed manipulators have limited operational reach, while conventional AGVs can transport items across stations but lack manipulative capability. Mobile dual-arm robots combine mobility with dual-arm manipulation, enabling autonomous de-stacking and material transfer.

To address these tasks in industrial logistics, we developed an autonomous material handling system based on a wheeled mobile dual-arm manipulator, realizing 6D tote pose estimation, closed-chain coordinated lifting, inter-station docking, and vibration-damped transport.

Project context

CATL Collaboration · Mobile Dual-Arm Material Handling Robot

In flexible manufacturing, fixed arms have limited reach while conventional AGVs lack manipulation capabilities. We developed an autonomous dual-arm mobile platform to tackle black plastic visual degradation, millimeter gap insertions, closed-chain internal stress suppression, and vibration mitigation during transit.

Key Deliverables & Real-World Validation: Bimanual 10 kg payload handling, high-accuracy 3D tote pose estimation (98%+ unstacking success), closed-kinematic-chain impedance decoupling, and robust inter-station transfer

My contributions

Orchestrated the heavy industrial mobile manipulation system; combined VLM perception and 6D pose estimation for 10 kg bimanual payload handling, achieving 98%+ tote unstacking and docking success for high-frequency station transfer.

Key Engineering Bottlenecks in Production

In modern clean-energy battery production and precision electronic assembly plants, intra-logistics material flow relies heavily on standardized industrial totes (tote boxes) weighing between 15 and 25 kg. Traditional automated facilities commonly deploy heavy-duty six-axis industrial arms or gantry robots at fixed workcells. However, these installations occupy critical floor space, require extensive safety fencing, and cannot flexibly reallocate across disparate workstations. Conversely, conventional automated guided vehicles (AGVs) or autonomous mobile robots (AMRs) only provide planar transit, lacking the dexterity required to unstack, manipulate, and precision-seat nested totes. Mobile dual-arm collaborative robots—integrating an omnidirectional mobile platform with dual collaborative arms—bridge the crucial “last ten meters” of agile intra-logistics material handling.

Nevertheless, picking boxes in laboratory mockups differs fundamentally from continuous 24/7 industrial production. Deploying such systems into battery manufacturing plants exposes severe physical and perceptual constraints:

Optical Dropouts on Black Light-Absorbing Plastics & Heavy Surface Scuffing: Factory totes are fabricated from black electrostatic discharge polypropylene (ESD-PP), which strongly absorbs near-infrared (NIR) structured light. Furthermore, continuous friction against automated roller conveyors coats tote surfaces with specular wear tracks and molding blemishes. Commodity industrial 3D cameras projecting active patterns experience severe optical attenuation, resulting in over 40% missing or wildly floating depth returns around critical rim contours and handle pockets.
Dense Stacking with Tight Lateral Clearances (Tight Packing Clearance): Totes are arranged in dense 3-to-5-layer palletized stacks or logistic carts, where lateral gaps between adjacent totes often measure less than 20 mm. End-effector parallel grippers clearing into handle recesses operate under bilateral tolerances of only 3 to 5 mm. A positioning error of mere millimeters causes rigid impact with neighboring containers, inducing pallet tipping and stack collapse.
Closed-Chain Kinematic Internal Stresses & Overcurrent Protection: While single-arm manipulation represents an open kinematic chain, two robotic arms grasping a single rigid 10 kg container instantaneously form a closed kinematic loop. Slight tracking discrepancies between the two arms (even sub-millimeter trajectory drift or millisecond communication jitter) generate destructive internal forces exceeding 300 N across the rigid container. These parasitic forces instantly trip robot joint torque limiters, aborting the process or shearing container handles.
Transit Dynamic Rocking & Chassis Overturning Moments: During long-distance transit across factory epoxy floors at 1.0–1.5 m/s, floor expansion joints, ramp transitions, and emergency deceleration subject high-mounted suspended payloads to severe inertial shocks. If the dual arms maintain an extended cantilevered posture, dynamic rocking produces massive tipping moments that destabilize the mobile base.

System & Algorithmic Architecture

Target Recognition → Station Alignment → Dual-Arm Handling → Closed Loop

High-precision navigation aligns with workstations; 3D vision resolves 6D tote poses; coordinated dual arms execute adaptive grasp and stable transport.

Perception and task preparation

01Observe the sceneImages and depth · tote segmentation · neighbors

02Select the targetUnstacking order · handling mode · target pose

03Navigate and alignApproach · side view · fine pickup position

Tote transfer cycle

04Lift with both armsPose update · bilateral grasp · coordinated lift

05Transport the toteStable hold · travel to the delivery area

06Place and releasePosition adjustment · lower · release and retreat

Action complete → reset state → refresh target → next transferNavigation arrival and manipulation status connect the stages of the task.

Full-Stack Architecture & Algorithmic Design

To surmount these industrial hurdles, we engineered a full-stack robotics architecture uniting multi-modal geometric vision, closed-chain admittance decoupling, and hierarchical docking servoing.

1. Robust 6D Pose Estimation on Weak-Texture Totes & Stacking DAG Reasoning

Rather than relying purely on noisy surface point clouds, we developed a hierarchical pipeline combining 2D edge contour back-projection with surface normal-constrained CAD model alignment:

Multi-Modal Edge Extraction & Spatial Ray Back-Projection: Exploiting high-contrast luminance gradients in 2D RGB streams, sub-pixel edge detection isolates the continuous rectangular boundary of the tote rim and bilateral handle pockets. Ray back-projection lifts these 2D line primitives into 3D camera frustums, anchoring them against sparse non-degraded point clusters to construct an initial geometric wireframe;
CAD-Constrained Coarse-to-Fine Geometric Alignment: Point Pair Features paired with aspect ratio priors generate bounded pose candidates. Joint non-linear optimization over surface normal distances and 3D silhouette consistency bounds 6D pose estimation within millimeter-level precision;
Stacking Contact Graph & Safe Top-Down De-Stacking: From recovered multi-tote spatial poses, the system constructs a Directed Acyclic Graph encoding vertical physical support relationships. The planner strictly enforces top-down topological extraction, executing virtual load-bearing checks before gripper closure to eliminate structural collapse risks.

2. Virtual-Mechanism Leader-Follower Admittance Control & Internal Stress Decoupling

To eliminate internal mechanical stresses during dual-arm rigid payload manipulation, we formulated a virtual linkage leader-follower admittance control framework, upgrading dual-arm coordination from open-loop trajectory tracking to closed-loop force-position compliance:

Contact Wrench Space Decoupling: Wrist-mounted 6-axis force/torque sensors monitor interaction forces. In the virtual linkage frame established at the payload centroid, measured forces are orthogonally decomposed into net accelerating forces driving motion and parasitic internal stresses that pinch or twist the tote;
Asymmetric Leader-Follower Dynamic Admittance Regulation: The primary arm operates as the kinematic leader tracking Cartesian trajectories. The secondary arm executes dynamic admittance control in task space: maintaining pre-tension along the clamping axis to prevent slip while minimizing stiffness along shear and rotational axes, compliantly yielding to cancel destructive internal torques.

3. Hierarchical Base-to-Workstation Alignment & Hand-Eye Visual Servoing

Industrial floor conditions and wheel slip limit mobile base parking repeatability, exceeding tolerance margins for narrow-gap de-stacking.

We implemented a two-stage docking architecture:

Coarse AMR Ingress: The mobile base navigates to nominal docking waypoints via 2D LiDAR SLAM, orienting its heading to maximize dual-arm manipulability relative to the target stack, before locking chassis brakes;
Eye-in-Hand Fine Servoing: Wrist-mounted cameras observe tote handle features and workstation fiducials from close range, solving closed-loop relative transformation errors directly into the manipulator kinematics solver to cancel residual base offsets.

4. Transit Anti-Sway Dynamics & Jerk-Limited Trajectory Profiles

To ensure payload stability during inter-station transit across extended plant floors:

Low-Center-of-Gravity Transport Posture: After unstacking, the arms immediately retract the tote inward against the central chassis axis and descend to a low elevation, doubling chassis overturning stability margins during emergency braking;
S-Curve Jerk-Bounded Velocity Profiling: The mobile base limits jerk throughout motion execution, suppressing start-stop inertial jolts that could dislodge sensitive parts inside the totes;
Whole-Body Deformable Convex Hull Protection: The chassis, deployed arms, and grasped tote are continuously wrapped in dynamic bounding polyhedra, adapting LiDAR safety fields between navigation and manipulation modes.

5. Whole-Body Kinematic Redundancy & 1 kHz Synchronous Bus

The combined mobile base and dual collaborative manipulators form a redundant robotic system. In the control layer, task-priority projection enforces end-effector grasping poses and closed-chain distance constraints, while secondary null-space projection maximizes manipulability and avoids joint limits.

Running a real-time Linux kernel, the system synchronizes chassis drives, joint servomotors, and 6-axis force sensors over industrial bus communication at 1 kHz with microsecond-level cycle jitter, ensuring high-frequency dynamic response.

6. Fault Detection & Self-Healing Recovery State Machine

To mitigate unpredicted factory disturbances:

Grasp Slip Monitoring: During initial vertical lift, the vertical force profile is evaluated against nominal mass. A sudden drop triggers an immediate abort, gently reseating the crate before executing secondary visual re-alignment;
Compliant Contact Back-Off: If lateral contact forces indicate unexpected contact during narrow-gap insertion, the follower arm compliantly backs off along the contact normal, re-estimates orientation via eye-in-hand vision, and re-engages.

Factory Demonstration & Field Verification

Autonomous Dual-Arm Tote Transfer PipelineReal-world demonstration: robot autonomously identifies stacked totes, executes synchronized dual-arm lifting, navigates across workstations, and achieves millimeter-accurate placement.

The footage above captures the autonomous robot executing continuous closed-loop handling inside battery manufacturing facilities:

De-Stacking from Multi-Tier Stacks: Reliably separating dark-colored totes from 4-tier stacks with 15 mm lateral clearances under dynamic shop-floor illumination;
Synchronized Dual-Arm Heavy Transport: Transporting 10 kg payloads while wrist F/T sensors maintain parasitic torques below 2 N·m;
Precision Docking to Conveyor Lines: Traversing 50-meter plant corridors, circumnavigating parked forklifts, and seating totes onto automated roller lines within 2 mm repeatability.

Engineering Summary & Technical Takeaways

Deploying dual-arm mobile manipulators in heavy industrial logistics highlights key architectural principles:

Closed-Chain Admittance as a Fundamental Enabler: In contact-rich manipulation of rigid heavy objects, open-loop position control inevitably induces damaging stresses. Closed-loop force admittance provides the necessary compliance for reliable operation.
Multi-Modal Geometry Beyond Sensory Dropouts: Fusing 2D photometric contours with 3D structural normals overcomes optical failures on dark, specular materials where pure point-cloud registration breaks down.
Layered Decoupling Absorbs Global Tolerances: Macroscopic navigation drift must be isolated and compensated by local visual and force servoing at the end-effector, ensuring operational repeatability across disparate workstations.
Deterministic Bus Synchronization Prevents Limit-Cycle Instability: In high-gain admittance control ($K_d > 500\text{ N/m}$), communication jitter or phase delays exceeding 2 ms inevitably trigger contact chattering and system limit cycles. Enforcing 1 kHz deterministic bus timing with PREEMPT_RT kernel scheduling is as vital to stability as the algorithmic formulation itself.

Production-Grade 4D Auto-Annotation: Multi-Camera Rig SfM & High-Precision Geometry

Thu, 20 Jun 2024 00:00:00 +0000

Project context

NETA Auto Collaboration · Automotive 3D Reconstruction & 4D Annotation

Meeting the demand for massive high-precision training ground truth in autonomous driving, this project established an industrial data pipeline spanning sensor sync, semantic perception, fused localization, rig SfM, and multi-run map aggregation, substantially improving offline labeling throughput.

Production Deliverables & Automotive Validation: Automotive 4D auto-annotation production pipeline; processed 1,000+ km fleet data (sub-5 cm cross-run registration), online extrinsic self-calibration, and 80%+ labeling efficiency gain

My contributions

Led a 5-person algorithm team engineering the multi-camera 3D reconstruction and 4D annotation pipeline; processed 1,000+ km multi-run fleet data with sub-5 cm cross-run spatial alignment, boosting annotation efficiency by over 80%.

As autonomous driving perception models shift from single-frame monocular detection toward bird’s-eye-view (BEV) architectures, 3D occupancy networks, and end-to-end foundation models, the availability of massive, temporally consistent 4D ground truth—encompassing metric road surfaces, 3D lane topologies, and static infrastructure—has become the definitive catalyst for algorithmic advancement.

Conventional workflows relying on mobile LiDAR scanning paired with manual outsourcing suffer from long turnaround times, high labeling costs, and limited generalization across heterogeneous sensor layouts. However, naively transplanting classical Structure-from-Motion (SfM) pipelines directly onto vehicle surround-view video streams immediately encounters severe physical and mathematical degeneracies: collinear motion corridors, textureless road surfaces, moving self-vehicle shadows, and unconstrained camera extrinsics drift.

In partnership with leading electric vehicle manufacturer NETA Auto, we developed and deployed an automotive-grade 4D vision auto-annotation data factory. As Project Tech Lead, I directed the overall algorithmic architecture and led the development of the core rigid multi-camera rig SfM reconstruction engine, ground-feature decoupling, spatiotemporal-guided dense multi-view stereo, and multi-session cross-epoch aggregation. The core geometric optimization principles and engineering systems developed during this industrial deployment were subsequently published at .

Why Driving Scenarios Break Classic SfM

In standard photogrammetry textbooks, classic SfM frameworks (such as COLMAP or VisualSFM) are built on the foundational assumption of an operator walking around an isolated, static monument with a single camera, capturing omnidirectional views with wide convergent baselines. Onboard vehicular data collected on highways and urban thoroughfares violates nearly every one of these assumptions:

Straight Corridor Motion & Collinear Triangulation Degeneracy: On highways and arterial avenues, vehicles travel along straight corridors for kilometers at a time. The optical centers of front and rear cameras align along a 1D straight line. In multi-view epipolar geometry, pure forward translation with collinear centers is ill-conditioned. When moving directly forward along the optical axis, the epipole coincides with the image principal center. Rays originating near the optical axis intersect at near-zero parallax angles. Sub-pixel feature matching noise is amplified along the longitudinal axis, causing reconstructed 3D points to stretch and drift along the direction of travel.
Extrinsics Drift in Unconstrained Multi-Camera Optimization: Production autonomous vehicles feature 6 to 8 surround cameras rigidly bolted to the vehicle body (front wide, front telephoto, front-left, front-right, rear-left, rear-right). Standard SfM treats each camera at each timestamp as an unconstrained 6-DoF pose variable. For an $N$-frame sequence, this introduces $6N$ independent pose variables. As pairwise feature matching accumulates noise, unconstrained optimization causes the physically rigid sensor extrinsics to artificially drift over time, introducing severe ghosting and discontinuous stitching artifacts in downstream BEV projections.
Textureless Asphalt & Moving Self-Vehicle Shadows: The lower half of onboard camera images is dominated by asphalt surfaces covering $>40\%$ of the image area. Asphalt is visually homogeneous, lacking sharp corner features under direct sunlight. Even more destructive is the vehicle’s dynamic shadow cast onto the road surface: the shadow boundary presents high-contrast, sharp edges that classical feature detectors (such as SIFT or Harris) aggressively extract as salient landmarks. However, the shadow translates synchronously with the vehicle, violating the static scene assumption. Triangulating shadow features forces the optimizer to fit non-existent points beneath the chassis, pulling vehicle trajectory estimates off course.
Multi-Session Cross-Epoch Aggregation Without Shared Image Matches: Autonomous driving maps require merging multiple independent drives through the same intersection collected across different days (e.g., overcast Monday morning vs. sunset Friday evening). Variable solar angles, changing shadows, and transient traffic mean disparate driving runs share zero visual feature correspondences, leaving independent reconstructions geometrically isolated.

Full Pipeline Architecture: From Raw Logs to 4D Vector Maps

To process tens of thousands of kilometers of production fleet video logs with high throughput, we established a decoupled, fail-safe processing pipeline:

Wider program

4D vision auto-annotation system

From synchronized vehicle data to a structured local map, with multi-camera SfM supplying geometry for annotation and mapping.

Upstream foundations

01Data preparationCameras · IMU · GNSS · chassis · calibration

02Semantic perceptionScene and lane masks

03Sensor-fusion localizationMetric vehicle trajectory

04 · My geometry workMulti-camera SfM reconstruction and system interfacesProgram coordination · poses and extrinsics · sparse/dense geometry · downstream delivery

Downstream applications

05Ground reconstructionDense road surface

06Auto-annotationLanes · signs · static elements

07Multi-run local mapAggregation and topology

Multi-Sensor Spatiotemporal Synchronization: Aligning six surround cameras, high-rate IMUs, GNSS receivers, and wheel odometry into a common timestamp reference and vehicle rear-axle coordinate frame;
Dynamic Semantic Masking: Deploying lightweight segmentation models to isolate moving vehicles, cyclists, pedestrians, and sky regions, shielding static 3D geometry from dynamic clutter;
Fused Inertial-GNSS Initialization: Fusing satellite positioning and dead-reckoning to initialize metric-scale trajectory guesses;
Rig-Centric Multi-Camera SfM (Core Engine): Treating surround cameras as a unified rigid rig to jointly solve vehicle poses and calibrate sensor extrinsics, generating a centimeter-level sparse road skeleton;
Spatiotemporal-Guided Dense MVS: Leveraging motion geometry priors to prune unshared image pairs, accelerating PatchMatch multi-view stereo to produce dense ground meshes;
Multi-Session Cross-Epoch Aggregation: Aligning independent driving passes coarse-to-fine into a globally seamless local high-definition map.

01Inputs & calibration Six-view imagery · multi-sensor pose · semantic masks · initial calibration

02Constrained association SuperPoint · temporal/overlap priors · two-view geometry

03Rig-aware sparse SfM Initialization · rig registration · triangulation · RIG BA · outlier filtering

04Densification & aggregation ACMP depth · COLMAP fusion · ground filtering · multi-run alignment

05Engineering outputs Refined poses/extrinsics · sparse/dense ground cloud · downstream priors

Core Algorithmic Design & Engineering Implementation

1. Rig Bundle Adjustment (Rig BA) with Rigid-Rig Parameterization

To eliminate extrinsics drift and reduce optimization dimensionality, we reformulated the multi-view optimization variables: rather than treating every frame as an independent pose, all surround cameras captured at timestamp $t$ are grouped into a rigid camera set. A camera’s absolute world pose is factored into a time-varying vehicle chassis pose and a static chassis-to-camera mounting extrinsic.

Under this parameterization, the number of pose variables collapses dramatically. We formulate the Rig Bundle Adjustment (Rig BA) framework to jointly optimize vehicle chassis trajectories, mounting extrinsics, and 3D static landmark coordinates.

This rigid formulation delivers two decisive advantages:

Complementary Viewpoint Geometry: When the vehicle drives into direct sun glare or passes alongside large trucks, forward cameras may suffer severe overexposure or visual occlusion. However, lateral and rear cameras continue tracking static architecture, trees, and street furniture. Through fixed internal rig extrinsics, unobstructed cameras constrain the chassis pose, eliminating longitudinal depth stretching;
Online Extrinsic Self-Calibration: By treating mounting extrinsics as optimization parameters, the solver refines mounting extrinsics over multi-kilometer runs to high precision, absorbing mechanical chassis flexing.

Exploiting multi-camera overlap priors removes numerous visually similar but physically impossible cross-camera false matches.

2. Spatial Overlap Constraints & Epipolar Pruning

Exhaustive pairwise matching across $N$ timestamps and $M$ cameras incurs an intractable $O(M \cdot N^2)$ computational explosion.

We introduced structured camera rig overlap priors:

A front-wide camera and front-left camera share only $\sim 30^\circ$ of horizontal FOV overlap. The right half of the front camera image cannot geometrically intersect the front-left camera’s viewing frustum;
Restricting feature searches strictly to overlapping spatial frustums ($u < W/2$ on the front camera) eliminates false associations between visually repetitive facades;
Enforcing Essential matrix $E$ epipolar distance checks and Homography $H$ planar constraints removes $>95\%$ of outlier correspondences while reducing matching runtime by $80\%$.

3. Decoupled Ground-Plane Filtering Prior to Triangulation

To prevent dynamic vehicle shadows and weak-texture asphalt from corrupting 3D point clouds:

Within the coarse sparse point cloud, we partition the road corridor into 5-meter sliding longitudinal cells;
Locally Optimal RANSAC (LO-RANSAC) fits local tangent ground planes $n_{\text{road}}^\top X + d = 0$;
Feature points residing within a 10 cm slice of the fitted plane are tagged and excluded from sparse bundle adjustment. The bundle adjustment solver converges purely on high-elevation rigid structures (building facades, light poles, overhead sign gantries), eliminating shadow-induced trajectory bias.

Neighborhood filtering strips isolated outliers while grid-based RANSAC suppresses thickness artifacts along the road surface.

4. Spatiotemporal-Guided Dense PatchMatch Stereo (MVS)

Generating continuous 3D ground meshes and voxel grids for occupancy networks requires high-throughput dense multi-view stereo:

Using the estimated vehicle trajectory, image pairs are screened based on optical parallax angles $\theta_{\text{parallax}} \in [5^\circ, 30^\circ]$ within a sliding temporal window of $\pm 15$ frames, avoiding global point-cloud searches;
GPU-accelerated multi-view PatchMatch (ACMP) iteratively optimizes depth maps and surface normal fields, employing slanted plane support priors to interpolate dense, continuous depth across textureless asphalt;
A multi-view geometric consistency filter fuses depth estimates across all six surround cameras into unified metric point clouds.

Standard COLMAP Dense

ACMP + Optimized Fusion

The hybrid strategy drastically accelerates fusion while improving reconstruction over weak-texture asphalt.

5. Coarse-to-Fine Multi-Session Aggregation Network

To reconcile asynchronous passes through identical intersections collected on different dates:

Coarse Georeferencing: Differential RTK-GNSS coordinates anchor each pass into a common metric coordinate system;
Fine Geometric Registration: The system isolates invariant vertical landmarks (traffic light poles, bridge pillars, structural facades). PnP and RANSAC solve the inter-session rigid transformation $T_{\text{run1} \to \text{run2}}$;
Global Pose Graph Relaxation: Fixing the primary reference pass, all subsequent passes are formulated as nodes connected by relative geometric constraints in a global pose graph. Non-linear least-squares optimization absorbs inter-session drift, stitching disparate runs into a seamless high-definition map.

Direct Overlay Before Aggregation

After Multi-Session Aggregation

Cross-session visual association and transform optimization reconcile distinct driving passes into one consistent spatial map.

Production Quality Assurance & Field Validation

The full-stack pipeline was validated in production data centers against ground-truth 128-beam mobile LiDAR SLAM and RTK reference trajectories:

Dense Surface Accuracy: Reprojecting dense visual point clouds onto forward-facing camera images and overlaying synchronized LiDAR sweeps demonstrated absolute road elevation errors under $\pm 3\text{ cm}$ within a 50-meter range, with sharp curb profiles;
Trajectory Consistency: Over 5-kilometer continuous city and highway loops, pure visual Rig BA suppressed cumulative yaw drift to 0.05°/100m, keeping closed-loop position errors below 0.1% of total travel distance;
Extrinsic Alignment Continuity: Applying optimized extrinsics to 360° BEV stitching eliminated lane marking shear across camera seams, restoring smooth physical continuity.

BEV projection comparison: lane markings across camera boundaries exhibit smooth physical continuity following optimization.

Pipeline Video Recordings

The four video sequences below present the end-to-end journey from raw onboard video and high-throughput dense depth generation to static 3D point cloud extraction and pixel-level reprojection checks:

Surround Camera StreamSynchronized surround-view video captured from production autonomous test fleets under dynamic driving conditions.

Static 3D Point Cloud ReconstructionHigh-precision static road geometry and architectural structures recovered by multi-camera rig SfM.

High-Throughput Dense Depth EstimationDense depth estimation guided by temporal and spatial overlap priors, achieving dramatic pipeline speedups.

Geometry & Appearance ReprojectionPixel-level reprojection verification demonstrating spatial consistency between recovered poses, dense geometry, and raw imagery.

Onboard Surround Video Stream: Synchronized six-camera footage recorded by production vehicles navigating dense city traffic;
Static Scene 3D Reconstruction: Dense, crisp geometric point-cloud models of overpasses, street furniture, and road structures;
High-Accuracy Dense Depth Estimation: Real-time dense disparity and depth maps generated across all views via spatiotemporal PatchMatch;
Appearance & Geometric Reprojection: Pixel-level reprojection checks confirming geometric alignment against original camera frames.

Engineering Summary & Technical Takeaways

Scaling Structure-from-Motion and multi-view stereo to automotive-grade automated annotation demonstrates key engineering principles:

Mechanical Rigidity as an Algorithmic Anchor: Multi-camera physical rigidity must be embedded directly into optimization formulations. Grouping surround cameras into a rigid rig halts extrinsic drift and overcomes collinear triangulation degeneracies through complementary multi-directional views.
Semantic Decoupling Guards Against Environmental Noise: In the presence of textureless asphalt and moving shadows, accepting all features indiscriminately destabilizes optimization. Decoupling ground features from vertical structures ensures robust convergence.
Bridging Isolated Trajectories into Spatial Networks: Autonomous driving requires scalable, repeatable mapping. Linking independent driving sessions through topological pose graphs establishes the foundation for large-scale digital twins and end-to-end driving models.

Autonomous Valet Parking: Production-Grade Full-Stack System for Underground Garages

Mon, 01 Jan 2024 00:00:00 +0000

Project context

Voyager Tech Mass-Production Partnership

Overcoming challenges in multi-floor underground garages—including featureless corridors, dynamic obstacles, tight clearance, and total GNSS denial—this system established an onboard-compute-powered L4 autonomous valet parking stack validated across extensive real-vehicle fleet trials.

Core Deliverables & Real-World Validation: Automotive pure-onboard AVP system; 500+ in-vehicle road tests (95%+ parking success), GNSS-denied multi-sensor localization, narrow-slot maneuvering, and remote summoning

My contributions

As project lead, architected the full-stack AVP system framework; directed planning and control under GNSS denial, completing 500+ in-vehicle road tests with 95%+ parking success.

Automated Valet Parking (AVP) encompasses autonomous stall search, tight-space bay parking, and designated-point summoning. In contrast to infrastructure-heavy vehicle-to-infrastructure (V2I) approaches—which require deploying dense ultra-wideband (UWB) anchors, roadside LiDARs, or dedicated camera networks throughout parking structures—vehicle-centric AVP requires zero structural modifications to parking facilities. By relying entirely on onboard sensors and compute, it delivers superior cross-facility generalization and commercial scalability.

In partnership with Tier-1 smart driving supplier Voyager Tech, we developed and deployed an onboard AVP system utilizing automotive-grade mass-production sensors: 4 surround-view ultra-wide fisheye cameras and 12 ultrasonic sensors. As Project Tech Lead, I directed the top-level system architecture and led algorithm development across the integrated driving-parking planning and control stack, validating GNSS-denied multi-sensor SLAM, corridor dynamic obstacle avoidance, Hybrid A* tight-space maneuvering, and low-speed chassis control on real production test vehicles.

Key Engineering Bottlenecks in Underground Garages

Underground garages represent semi-enclosed, highly constrained operational design domains (ODDs) that present formidable physical and geometric challenges to autonomous systems:

Total Satellite Denial & Severe Perceptual Degeneracy (GNSS-Denied & Perceptual Aliasing): Upon entering a basement structure, satellite positioning is immediately lost. Simultaneously, multi-story garages present uniform, highly repetitive architectural features: mirrored structural pillars, polished epoxy resin flooring with severe specular reflections, and featureless concrete walls. Under dim, flickering fluorescent illumination, standard point-feature extractors (such as ORB or SIFT) suffer from catastrophic false associations, precipitating visual odometry drift and scale collapse.
Severe Geometric Confinement & Non-Holonomic Kinematic Limits: Primary basement aisle widths typically measure between 2.8 and 3.2 meters, with clearance between concrete pillars occasionally narrowing below 2.5 meters. Standard passenger vehicles feature wheelbases approaching 3 meters and overall lengths near 5 meters. Constrained by non-holonomic Ackermann steering kinematics, the vehicle cannot translate laterally. When executing reverse maneuvers into a standard 2.4m $\times$ 5.2m perpendicular bay, the vehicle must execute multi-point forward-reverse (D/R gear shift) maneuvers under safety margins of less than 30 cm, where minor tracking deviations cause structural collisions.
Dynamical & Control Dichotomy Between Cruising and Parking: During corridor cruising, the system requires continuous, smooth motion at 10–15 km/h, with lateral acceleration strictly constrained for passenger comfort. Conversely, during stall insertion, vehicle velocity drops below 3 km/h, while steering actuators must rapidly sweep toward maximum lock within minimal linear displacement. This induces massive static tire scrub torque against the floor. Conventional trajectory-tracking controllers fail under this dual-regime operation, triggering actuator oscillation, severe deadband lag, or tracking overshoot.
Multi-Story Spiral Ramps & Dynamic Grade Distortions: Underground garages incorporate steep connecting spiral ramps with grades ranging between 10% and 15% (5.7°–8.5°). During ramp traversal, dynamic suspension compression induces pronounced chassis pitch and roll angles, introducing geometric skew into inverse perspective mapping (IPM). Furthermore, the longitudinal gravitational component severely perturbs velocity feedback loops, risking roll-back during stop-and-go maneuvers on inclines.

To systematically address diverse user requirements, the system supports four operational workflows:

Prior Map Condition	Mission Mode	System Execution	Target Outcome
Mapped Garage	Memorized Slot	Load local semantic graph and localize	Direct cruise to user’s assigned stall
Mapped Garage	Random Search	Cruise along aisles and detect open spaces	Immediate trajectory planning into first valid slot
Unmapped Facility	Exploratory Cruise	Build topological slot map on the fly	Explore garage, identify valid spot, and park
Mapped Garage	Remote Summon	Receive app command and execute park-out	Navigate through aisles to pickup foyer

System architecture

Campus-scale automated valet parking

From user intent and onboard sensing to mapping, hierarchical planning, vehicle control, and safety replanning.

Task, sensing, and compute

01 · HMIMaps and parking tasksMap download/update · park-in/out · remote summon

02 · SensorsSurround and near-field sensingFour fisheye cameras · ultrasonic sensing · IMU · wheel/GPS

03 · VehicleOnboard compute and chassisEmbedded GPU/ECU · CAN · steering, braking, and drive

Autonomy core

04 · PerceptionBEV multi-task perceptionSlots, markings, and text · drivable space · obstacle detection and tracking

05 · LocalizationLocalization and semantic mapVisual/inertial/wheel fusion · loop closure · semantic ICP · exploration mapping

06 · PlanningHierarchical driving and parking planningGlobal guidance · local avoidance · Hybrid A* · optimal-control smoothing

Program leadership and integrationSystem design, module interfaces, and vehicle integrationArchitecture and milestones · cross-module interfaces · planning/control focus · scenario testing and fault correction

Execution, safety, and application

07 · ControlLateral/longitudinal controlTrack the reference trajectory and command the vehicle

08 · SafetyObstacle response and replanningNear-field coverage · dynamic updates · stop and detour

09 · ServiceExplore, park, retrieve, and summonKnown/unknown maps · random, remembered, or assigned slots · return to pickup

Full-Stack Architecture & Algorithmic Design

To overcome these constraints under onboard embedded compute limits, we developed a tightly coupled, modular processing pipeline encompassing bird’s-eye-view multi-task perception, factor-graph fusion localization, hierarchical planning, and precision chassis actuation.

1. Surround-View Fisheye BEV Multi-Task Perception & Near-Field Shielding

The sensing suite consists of 4 automotive-grade ultra-wide fisheye cameras (each offering $>190^\circ$ horizontal FOV) positioned at the front, rear, and wing mirrors, paired with 12 ultrasonic transceivers.

Dynamic Pitch-Compensated Inverse Perspective Mapping: To eliminate image warping caused by vehicle acceleration, braking, and ramp slope variations, high-frequency IMU pitch and roll estimates are fed directly into the online homography transformation. This dynamic transformation stabilizes the bird’s-eye view, ensuring projected ground markings and stall boundaries maintain rigorous metric scale.
Lightweight Shared Backbone with Multi-Task Branched Heads: Operating within embedded thermal envelopes, the perception network adopts a single feature-extractor backbone paired with four decoupled task heads: parking stall head, ground marking head, obstacle detection head, and free space boundary head.
Ultrasonic Near-Field Time-of-Flight Protection: When approaching obstacles within close range, vehicle blind zones fall outside camera sightlines. Short-range and long-range ultrasonic sensors establish a continuous proximity barrier, using multi-transceiver time-of-flight triangulation to filter multi-path false echoes and provide deterministic hardware-level collision prevention.

2. Factor-Graph Multi-Sensor Fusion Odometry & Hierarchical Mapping

To guarantee drift-free localization across extended GNSS-denied trajectories, we constructed a tightly coupled factor-graph optimization backend:

Slip-Adaptive Wheel Odometry & IMU Preintegration: On polished epoxy basement surfaces, tire micro-slip frequently occurs during cornering and braking. The fusion filter continuously evaluates consistency between non-driven wheel speeds and IMU yaw rates, adaptively tuning observation covariances to bridge disturbances without pose divergence;
Visual-Semantic Landmark Constraints & Global Loop Closure: In addition to IMU and odometry factors, the factor graph integrates geometric constraints derived from detected parking slot corners and lane centerlines, triggering full pose-graph relaxation upon loop detection to eliminate accumulated drift;
Hierarchical Lightweight Map Representation: A local rolling occupancy grid caches dynamic obstacles within a sliding window, while a global topological semantic graph stores corridor centerlines and stall attributes with minimal memory footprints.

3. Hierarchical Cruising & Parking Motion Planning

Recognizing the distinct kinematic scales of aisle navigation versus stall maneuvering, the planning pipeline is decoupled into two specialized stages:

01Task and map state Known/unknown map · target slot or pickup point · current pose

02Global driving guide Centerline graph · improved A* · turn-radius arcs · B-spline

03Local avoidance trajectories State sampling · centerline/transition/obstacle/curvature costs · online replanning

04Parking search Orientation-aware Hybrid A* · nonholonomic vehicle constraints · collision checks

05Trajectory optimization and execution Optimal control · IPOPT · lateral/longitudinal tracking · feedback loop

Corridor Cruising: Reference Spline Smoothing & Dynamic Evasion

In aisle transit, the global planner fits continuous-curvature reference corridors using spline curves. The local planner samples polynomial trajectory candidates in the Frenet frame at high frequency, evaluating safety distance, comfort, and lateral acceleration to smoothly evade obstacles or yield to traffic.

Bay Parking: Modified Hybrid A* & Non-Linear Optimal Control

Upon reaching the designated stall entry position, the system transitions to precision parking mode using a two-stage coarse-to-fine framework:

Coarse Geometric Search (Modified Hybrid A)*: Search progresses in continuous configuration space under non-holonomic turning radius constraints and distance field heuristics, generating kinematically feasible initial paths within milliseconds;
Continuous Trajectory Optimization: Taking the Hybrid A* path as an initial guess, constrained non-linear optimal control enforces kinematic differential equations, steering rate limits, and multi-circle collision avoidance, producing continuous, curvature-smooth trajectories that prevent driveline shudder.

4. Precision Chassis Tracking & Delay Compensation

At low crawl speeds with aggressive steering angles, drive-by-wire latency constitutes the primary source of path deviation:

Kinematic Preview with Curvature Feedforward: Lateral control employs delay-adaptive preview lookahead, combining curvature feedforward with tracking error feedback to compensate for steering actuator delay;
Grade-Adaptive Gravitational Feedforward: Longitudinal tracking couples speed feedback with real-time IMU grade estimation, directly compensating for gravity components on steep ramps to prevent roll-back;
Multi-Tier Hardware Safety Watchdog: High-frequency watchdogs monitor system communications, issuing direct emergency brake requests if ultrasonic sensors detect unexpected close-proximity incursions.

Real-Vehicle Field Verification

Surround-View BEV Multi-Task PerceptionDynamic in-vehicle playback: real-time bird-eye view semantics, 4-way unwrapped fisheye streams, and high-precision parking space and lane marking tracking.

Autonomous Garage Exploration & Full-Stack System IntegrationAutonomous vehicle operation in multi-floor garages: synchronized surround perception, visual odometry, vehicle kinematics, and incremental semantic parking map construction.

Field trials were conducted across diverse commercial underground parking facilities, spanning modern multi-story garages, unlit legacy basements, and complex spiral structures:

Perpendicular, Parallel, and Slanted Bay Ingress: In tight stalls flanked by parked vehicles with less than 30 cm of lateral margin, the vehicle consistently parked within two to three forward-reverse maneuvers, achieving alignment within 1° of stall markings and symmetric wheel clearance;
Multi-Floor Spiral Navigation & Dynamic Clearance: Across hundreds of meters of subterranean transit, the vehicle smoothly negotiated consecutive blind corners, executing automatic lane yields when encountering oncoming delivery vans or wandering pedestrians;
End-to-End Remote Summoning: Production vehicles operated completely unmanned in response to mobile app summon commands, autonomously exiting bays and arriving at ground-level passenger staging areas.

Engineering Takeaways & Architectural Lessons

Automated Valet Parking represents one of the most demanding low-speed autonomous driving domains due to tight geometric tolerances and extreme perceptual degradation. Key takeaways include:

Multi-Modal Synergy in Featureless Environments: When individual sensing modalities degrade, combining vehicle kinematics, IMU preintegration, and topological semantic landmarks provides the only viable defense against long-term localization drift.
Explicit Physics Constraints in Trajectory Formulation: Kinematic paths must not be treated as purely geometric splines. Embedding actuator rate limits, acceleration limits, and gear-shift costs directly into the optimization formulation is essential for smooth, production-grade tracking.
Decoupled Architecture with Deterministic Safety Shields: Decoupling open corridor cruising from tight stall optimization preserves high real-time control frequencies, while dedicated hardware-level ultrasonic emergency braking guarantees operational safety under real-world uncertainties.

Commercial Autonomous Mowing Robot: Multi-Modal Fusion & Terrain-Aware 3D Perception

Sun, 01 Jan 2023 00:00:00 +0000

Project context

Positec Group (WORX) Mass-Production Partnership

Overcoming the severe challenges of unstructured, deformable outdoor turf—including thick grass returns, non-planar slopes, hidden trenches, and dynamic pedestrian/pet traffic—this system brings automotive-grade autonomous driving technologies to outdoor mobile robotics, pioneering truly scene-aware, wire-free lawn robotics.

Key Deliverables & Field Validation: Commercial-grade integrated robotic hardware-software system; >98% obstacle avoidance success rate, terrain-adaptive 3D obstacle perception engine, positive/negative obstacle extraction, and field-proven autonomous mowing

My contributions

As project lead, developed slope-adaptive LiDAR point-cloud filtering and visual obstacle avoidance for Positec robotic lawn mowers, achieving >98% obstacle avoidance success rate and resolving point-cloud clutter filtering, positive/negative obstacle separation, and multimodal sensor fusion on rolling terrain.

Commercial and residential robotic mowers have traditionally relied on low-frequency inductive perimeter wires buried beneath turf edges, coupled with random-bounce navigation. Over expansive undulating estates, municipal parks, or irregular municipal greens, physical perimeter wiring incurs prohibitive civil trenching costs, difficult wire fault troubleshooting, and inherent random-walk inefficiencies characterized by high re-mow overlap and missed cutting patches. Boundary-wire-free autonomous perception, all-terrain topological navigation, and systematic parallel pattern mowing represent the defining technological paradigm for next-generation outdoor groundskeeping robotics.

This project partnered with global outdoor power equipment leader Positec Group (WORX) to engineer a production-ready, boundary-wire-free commercial mowing robot equipped with full-scene autonomous perception, self-adaptive mapping, and global coverage motion planning. As Project Tech Lead, I directed the top-level full-stack hardware-software architecture, cross-module subsystem integration, and field deployment; simultaneously, as principal perception lead, I directed algorithm development for multi-beam LiDAR terrain-adaptive point-cloud modeling, bifurcated positive/negative obstacle extraction, and spatial cross-modal vision-LiDAR fusion.

Key Perception Challenges in Undulating Turf

On structured urban asphalt, the ground surface is predominantly rigid, planar, and continuous; point cloud reflections protruding 10 cm above the surface unambiguously signify physical obstacles. However, when transitioning into unstructured, undulating outdoor turf, these foundational geometric assumptions break down completely:

Vegetation Clutter & Multiple-Scattering Ambiguity (“Grass vs. Obstacle”): Turfgrass blades are flexible, semi-translucent, non-rigid, and continuously wind-perturbed. Infrared laser pulses penetrating dense turf experience multiple diffuse scattering across grass tips, mid-stems, and underlying soil. In raw 3D point clouds, this manifests as a loose, porous noise blanket 20 to 35 cm thick. If points protruding above bare soil are treated as obstacles, the mower becomes paralyzed by false phantom obstacles; conversely, applying naive 30 cm geometric height filtering causes low garden hoses, tree roots, perimeter curbs, and recumbent domestic pets to be completely overlooked, risking blade damage and safety hazards.
Non-Planar Undulating Terrain & Local Tangent Variations: Real outdoor lawns feature rolling hills, swales, drainage mounds, and subtle undulating contours. Global planar ground extraction (such as standard RANSAC ground fitting) fails within 3 to 5 meters of the vehicle, mistaking normal uphill grass surfaces for solid vertical walls and triggering emergency halts.
The “Negative Obstacle” Crisis (Depressions, Drainage Ditches, and Step Drops): Commercial mowers feature compact chassis with drive wheel diameters of only 15 to 20 cm. On contiguous grass, 7 cm subsidence ruts, open drainage trenches, or lawn edges dropping off into patios will instantly trap front casters, strike cutting discs, or induce vehicle rollovers. Negative obstacles do not generate positive point-cloud reflections; rather, they present as voids of missing laser returns and ray-shadow gaps, rendering classical spatial clustering algorithms ineffective.
Severe Optical Variations in Outdoor Natural Environments: Lawn maintenance subjects camera sensors to harsh dynamic lighting transitions: unshielded midday sun, dappled tree shadows, and acute morning/evening glare. Active structured-light infrared sensors suffer complete pattern washout under direct solar irradiance, while passive stereo algorithms experience extensive disparity dropouts across repetitive, textureless green turf.
Ultra-Low Embedded Power & Thermal Budgets: Unlike passenger autonomous testbeds carrying water-cooled industrial PCs in vehicle trunks, autonomous mowers operate strictly on compact onboard lithium battery packs. The complete SLAM, perception, and coverage planning pipeline must execute deterministically at 20 Hz within a compute budget limited to ~15 Watts on embedded edge SoCs (such as NVIDIA Jetson modules).

System architecture

An autonomous mobile robot for unstructured lawns

Localization, perception, planning, and control share one onboard sensor and compute platform.

Onboard sensing and compute

01 · VisionMulti-camera and RGB-DLawn, objects, semantics, and appearance

02 · GeometryLiDARRange, terrain, and obstacle point clouds

03 · MotionRTK/GNSS · IMU · wheel odometryGlobal constraints and continuous local motion

Autonomy core

04Multi-sensor localizationVision, LiDAR, inertial, wheel, and satellite cues

05Visual objects and semanticsMowable area, object categories, and scene semantics

06 · Program leadership and technical focusSystem integration + LiDAR obstacle detection + multi-sensor obstacle fusionLead module integration and field commissioning; separate obstacles from uneven grass and unify 3D geometry, semantic segmentation, and visual detections for planning

Robot behavior

07Local environment modelTraversable lawn and dynamic/static obstacles

08Path and coverage planningSafe avoidance, coverage mowing, and return-to-charge

09Chassis and mowing controlTurn perception into real-time motion decisions

Core Algorithmic Architecture & Engineering Design

To achieve reliable autonomy under low-power compute and natural outdoor environmental degradation, we developed a full-stack autonomy pipeline spanning factor-graph SLAM, polar terrain manifold modeling, positive/negative obstacle decoupling, and cross-modal sensor fusion.

1. Factor-Graph Multi-Sensor Fusion Localization & Slip Adaptation

Maintaining tightly spaced, millimeter-parallel “boustrophedon” (lawn-striping) patterns over hours of continuous operation demands high-rate, drift-free 6-DOF pose estimation:

Heterogeneous Sensor Topology: The platform integrates dual-antenna RTK-GNSS, an automotive-grade 6-axis IMU, precision wheel encoders, and a multi-beam 3D LiDAR. In open sky, dual-antenna differential GNSS delivers centimeter-level position fixes and true orientation heading;
Slip-Adaptive Wheel Odometry Factor: On dew-soaked morning grass or damp sloping soil, mower wheels frequently encounter micro-slip and wheelspin. The system continuously cross-examines wheel acceleration against axial IMU accelerations, establishing a slip-residual monitoring mechanism that dynamically downweights wheel kinematic constraints to prevent spurious odometry drift;
Unified Smoothing Backend via GTSAM: Implemented in GTSAM, the sliding-window optimization backend simultaneously resolves RTK position factors, IMU pre-integration factors, slip-adaptive kinematic odometry factors, and LiDAR planar surface constraints over the trajectory. When transitioning under dense tree canopies or adjacent to tall buildings where GNSS signals degrade, inertial and LiDAR odometry sustain continuous localization without trajectory jumps.

2. Terrain-Adaptive LiDAR Point-Cloud Modeling & Decoupled Hazard Extraction

Extracting physical hazards out of hundreds of thousands of diffuse grass returns forms the central algorithmic breakthrough of the perception pipeline:

Terrain-adaptive point-cloud pipeline: from polar grid partitioning and slope continuity estimation to clustering, temporal tracking, and obstacle dispatch.Open full size

We built a local polar manifold terrain analysis and ray-gap detection pipeline:

Structured Polar Elevation Grid: Raw 3D point clouds within a 10-meter operational radius are projected into an egocentric polar coordinate grid. Dividing space into fine radial range cells and azimuth sectors, the system computes lowest elevation, highest elevation, point count density, and elevation variance per cell;
Radial Tangent Ground Manifold Propagation: Bypassing global ground plane assumptions, local tangent planes are propagated outward along each azimuth ray starting from the tire-ground contact footprint. Enforcing smoothness and bounded gradient step constraints, the algorithm conforms to undulating terrain contours while dynamically isolating soil levels from overlying turfgrass;
Dual-Mode Positive Hazard Classification (Protruding Obstacles): Operating across manicured lawns and wild overgrown grass:
1. Manicured Turf Mode: Point clusters exceeding 16 cm in elevation with surface normals diverging from local ground normals are classified as obstacles (tree trunks, stones, fences);
2. Overgrown Meadow Mode: Elevation thresholds adaptively elevate to 35 cm, combined with vertical point density entropy. Because grass stems produce uniformly dispersed vertical returns whereas rigid solid objects concentrate dense returns, entropy testing reliably separates dense obstacles from tall grass;
Negative Hazard Extraction (Pits, Trenches, and Cliffs): Depressions lack direct reflective returns. We model the expected line-of-sight laser propagation geometry. When encountering a ditch or step drop ($\ge 7\text{ cm}$ depth), the laser beam either passes over the depression or strikes the distant lip at an anomalous radial distance discontinuity, creating a ray-shadow gap. Detecting this discontinuity paired with abrupt elevation drops marks the negative hazard boundary, projecting a repulsive field into the local costmap;
Two-Pass Connected Component Clustering & Kalman Tracking: Surviving obstacle cells undergo fast raster clustering into 3D bounding boxes. Bounding boxes are tracked across frames via Hungarian matching and Kalman filtering, eliminating transient wind-blown grass blades that persist for only 1–2 cycles.

LiDAR delivers precise metric geometry and 3D bounding dimensions, but cannot infer object semantics (distinguishing an innocent toy from a sleeping pet). Cameras capture rich color semantics but lack reliable depth on textureless lawns.

Multi-modal obstacle fusion pipeline: aligning 3D point clusters with 2D semantic masks to output verified, category-aware navigation obstacles.Open full size

We deployed TensorRT-accelerated lightweight object detection networks on the embedded GPU, processing at 20 Hz to detect pedestrians, domestic animals (dogs, cats), hoses, garden tools, and flowerbed edges. Reprojecting 3D LiDAR clusters onto 2D camera pixels via the camera-LiDAR calibration matrix yields three complementary outputs:

Category-Aware High-Confidence Fused Targets: 3D LiDAR bounding projections match 2D visual bounding detections with high Intersection-over-Union (IoU). The target receives unambiguous category labeling (e.g., sleeping pet) and millimeter 3D coordinates. The motion planner assigns highest avoidance priority, generating a generous $>1.0\text{ m}$ standoff safety corridor;
Point-Cloud-Only Geometric Obstacles: Unclassified visual targets (e.g., untextured grey boulders, tree stumps, ground trenches) confirmed by strong LiDAR geometry. The planner handles them as rigid obstacles, adopting standard 0.3 m edge clearances;
Vision-Only Distant Precautionary Candidates: Visual detections of humans or pets at 10–15 meters where LiDAR point returns remain sparse (1–2 points). The system preemptively curtails speed, maintaining tracking focus until approaching within dense LiDAR verification range.

Field association example: visual object semantics and 3D point-cloud supports lock together in real-time navigation frames.

4. Autonomous Coverage Planning & Anti-Sideslip Control

Boustrophedon Complete Coverage Path Planning (CCPP): Decomposing complex yards into convex polygonal cells, the planner computes parallel sweeping tracks with continuous turn loops along boundaries, minimizing deadhead transit;
Gravity-Compensated Slope Anti-Sideslip Control: On lateral grass slopes reaching 20° (36% grade), gravity pulls the mower downhill, producing severe trajectory lateral slip. The wheel controller actively monitors chassis roll and pitch from the fused IMU and injects lateral velocity feedforward compensation into differential drive outputs, cancelling downhill sliding drift and preserving razor-straight cutting tracks on steep embankments.

Outdoor Field Playback

Field Playback: Terrain-Adaptive Point-Cloud PerceptionReal-world field operation: red 3D point clusters mark detected low-profile obstacles and terrain boundaries, while front/rear cameras provide real-time visual confirmation.

The field recordings above demonstrate real-world autonomous closed-loop operation on undulating villa lawns: synchronized dual-camera video feeds align with 32-beam 3D LiDAR point clouds. Red highlighted clusters cleanly reject tall grass clutter while locking onto low-profile rocks, flowerbed curbs, and terrain drop-offs under dappled sunlight and changing outdoor conditions.

Engineering Summary & Technical Takeaways

Deploying autonomous mowers into real-world unstructured greenery highlights primary engineering insights:

Leveraging Physics Priors Against Natural Ambiguity: When dealing with diffuse foliage returns and non-planar slopes, generic data-driven models or flat-ground assumptions inevitably fail. Understanding laser scattering mechanics within vegetation and parameterizing radial slope continuity allows clean clutter rejection at minimal compute cost.
Negative Obstacle Detection as an Essential Safety Prerequisite: For low-clearance mobile bases, ground trenches and step drops are as hazardous as rigid vertical walls. Elevating negative hazard detection to equal architectural priority ensures safe off-road operations.
Balancing Algorithmic Sophistication with Edge Thermal Limits: In battery-powered commercial robotics, algorithms detached from hardware constraints cannot reach production. Orchestrating lightweight neural networks with compact geometric point-cloud processing enables robust 20 Hz real-time loops on compact low-power SoCs.
Active Gravity Feedforward Beats Passive Integral Trimming: On steep, slippery outdoor grass, passive integral controllers take seconds to wind up, resulting in significant downhill lane departure. Directly feeding physical gravity projections into dynamic steering compensation keeps tracking tight across slope transitions.

Full-Stack Autonomous Driving Research Platform: Hardware-Software Co-Design & Real-Vehicle Verification

Sat, 01 Jan 2022 00:00:00 +0000

Autonomous driving research requires validation beyond idealized simulator environments to directly address real-world ground friction, physical sensor noise, bus communication latency, and actuator non-linearities. In 2019, commercial autonomous testbeds were prohibitively expensive and strictly closed-source, precluding in-depth research into chassis dynamics and custom low-level sensor drivers. To establish an accessible, controllable physical research platform, we undertook the ground-up hardware and software construction of an autonomous driving research testbed.

Between 2019 and 2022, we designed and built two complementary research vehicles: a low-speed pure-electric passenger vehicle configured for campus road cruising, and an omnidirectional research AGV featuring 4-wheel independent driving and 4-wheel independent steering (4WID-4WIS). The engineering effort spanned mechanical mounting fabrication, high/low-voltage isolated power distribution, reverse engineering of proprietary CAN bus protocols, development of Linux SocketCAN drivers, microsecond-level hardware time synchronization across heterogeneous sensors, and a full-stack ROS-based software autonomy suite encompassing perception, SLAM, planning, and control. The platform successfully completed closed-loop verification across diverse representative operational scenarios under realistic campus road traffic.

Project context

Autonomous Driving Research Core · Ground-Up Hardware/Software R&D

To evaluate autonomous driving algorithms on physical hardware rather than purely in simulation, we built full-stack research vehicle platforms from the ground up. Covering chassis adaptation, power distribution, SocketCAN drivers, and ROS autonomy modules, the platform provided an experimental testbed for ongoing laboratory research.

Platform Deliverables & Multi-Scenario Real-Vehicle Demonstrations: Two operational drive-by-wire autonomous research platforms; 300+ hours of road testing, 8 real-vehicle closed-loop demonstrations (<5 cm lateral tracking error, <60 ms AEB braking response), and a complete custom ROS autonomy stack.

My contributions

As project lead, built two generations of drive-by-wire autonomous vehicle research platforms (passenger cart and 4WIS AGV) from scratch, accumulating 300+ hours of physical road testing; responsible for drive-by-wire modification, power isolation, CAN bus reverse engineering, and multi-scenario closed-loop validation.

Two Complementary Research Chassis

To comprehensively ground our research across disparate operational domains, we developed two distinct physical platforms:

Low-Speed Campus Research Cart: Configured with passenger-carrying seating, a standard automotive steering column, and hydraulic braking. Its open frame and high payload capacity accommodate heavy sensor payloads: a 32-beam spinning 3D LiDAR, forward stereo and surround wide-angle cameras, dual-antenna RTK-GNSS, a high-rate IMU, and a ruggedized industrial PC equipped with high-performance discrete GPUs. This vehicle serves as the primary testbed for large-scale metric-topological SLAM, multi-modal perception, traffic interaction, and cooperative V2X platooning.
Compact 4-Wheel Independent Drive & Steer (4WID-4WIS) AGV: Equipped with eight independent brushless DC servomotors (four driving wheels, four steering hubs). Bypassing the non-holonomic kinematic limits of traditional Ackermann steering, the vehicle dynamically reconfigures its steering mode in software: standard front-wheel Ackermann, diagonal crab-steering, pure lateral translation, and zero-radius pivot spinning around its chassis center. This platform provides an agile testbed for high-speed obstacle avoidance maneuvers and extreme tight-space parking.

Both vehicles enforce a foundational design paradigm: exposing algorithms directly to real-world tire-ground compliance, CAN transmission jitter, and actuator deadbands, thereby establishing a physical benchmark for subsequent algorithmic robustness.

Full-Stack Autonomous Driving Architecture

Multi-Sensor Fusion → State Estimation → Planning → Drive-by-Wire Actuation

Full-stack onboard ROS autonomy connects perception, SLAM, and planning; the drive-by-wire chassis executes closed-loop control over CAN.

Sensing and computation

01Observe surroundingsCamera · LiDAR · ultrasonics

02Positioning inputsRTK GNSS · IMU · LiDAR map

03Onboard computationIndustrial PC · ROS · HMI

Autonomy and actuation

04Perception and localizationObjects · lanes · obstacles · vehicle pose

05Decision and planningReference route · driving objective

06Control and drive-by-wireMotion commands · CAN · chassis actuation

Vehicle motion → new observations and feedback → next control updateMap-based and RTK route following use their respective localization inputs.

Ground-Up Electrical & Sensor Topology

Without commercial turnkey kits, integrating disparate sensors and actuators into a fail-safe vehicular platform required overcoming fundamental electrical isolation, extrinsic calibration, and temporal synchronization challenges.

Subsystem	Configuration & Role	Core Engineering Challenges Overcome
Drive-by-Wire & CAN Bus	Electronic steering motor, throttle potentiometer, and hydraulic brake booster	Reverse-engineered proprietary cyclic DBC protocols; built 100 Hz deterministic Linux SocketCAN drivers with heartbeat fail-safe routines
3D LiDAR	32-beam / 16-beam mechanical spinning LiDAR for 3D mapping and obstacle ranging	Custom CNC aluminum vibration dampers isolating chassis road shock; calibrated beam reflectivity intensity decay curves
HDR Industrial Cameras	Narrow-FOV (60°) and wide-angle (120°) cameras for lane segmentation and object vision	Mitigated severe lens flare transitioning into shaded tree canopies; designed rigid camera-LiDAR multi-modal calibration targets
RTK-GNSS Differential GPS	Dual-antenna receiver delivering centimeter-level 3D coordinates and true heading	Optimized 1.5 m baseline antenna placement; mitigated carrier-phase slip and multipath jumps beside dense concrete campus structures
Automotive IMU	6-axis tactical-grade inertial unit providing 200 Hz linear acceleration and yaw rates	Mechanically aligned IMU sensing center with chassis rear-axle centerline; performed online thermal bias drift calibration
Ultrasonic Transceiver Array	8 bumper-mounted ultrasonic sensors for near-field blind-spot guarding (0.2–2.5 m)	Optimized acoustic beam-cone overlap geometry, enforcing sequential time-multiplexed firing to eliminate cross-talk
Power Distribution & Isolation	48V traction battery pack converted to isolated 12V/24V clean bus	Custom electrical distribution enclosure with TVS diodes and optocouplers, suppressing motor back-EMF voltage surges and ground loops

1. Hardware Hard-Triggering & Microsecond Temporal Synchronization

At vehicle speeds of 25 km/h, a 100 ms timing desynchronization between sensors induces an spatial projection error approaching 0.7 meters. Relying on software system timestamps (ROS Time) introduces non-deterministic OS scheduling jitter incompatible with feature-level sensor fusion.

We engineered a multi-tier hardware synchronization architecture:

PTP and PPS Nanosecond Clock Locking: The RTK-GNSS receiver, upon locking satellite constellations, generates a precision Pulse Per Second (PPS) square wave and GPRMC NMEA timing packets. The 32-beam LiDAR ingests the PPS signal, synchronizing its internal clock to absolute UTC time within nanoseconds;
FPGA/Microcontroller External Optical Exposure Triggering: A dedicated microcontroller captures the PPS pulse and synthesizes a phase-locked 20 Hz square wave sent to the external trigger pins of industrial cameras. This forces camera sensor exposure to coincide exactly with the moment the LiDAR spinning beam sweeps through the forward field of view;
Driver nodes read hardware frame timestamps directly from peripheral registers, compressing inter-sensor temporal alignment error below 1 millisecond.

2. Rigid Extrinsic Joint Calibration (LiDAR-Camera-IMU)

To eliminate spatial misalignments:

The rigid transformation between the camera optical center and LiDAR coordinate frame is optimized using a custom calibration board featuring hollow circular cutouts, simultaneously minimizing 3D laser point-to-plane fitting errors and 2D visual circular re-projection errors;
IMU-to-LiDAR spatial extrinsics are identified through continuous sinusoidal steering maneuvers and rapid acceleration profiles in open areas, parameterizing trajectories via continuous B-splines to minimize angular velocity and linear acceleration residuals, reaching millimeter and 0.1-degree alignment accuracy.

3. Proprietary CAN Bus Reverse Engineering & Deterministic SocketCAN Drivers

With native chassis controller (VCU) documentation withheld, we captured raw bus traffic using CAN analyzers during manual driving. By correlating driver throttle inputs, pedal pressure, steering wheel sweeps, and wheel encoder pulses, we systematically isolated and mapped the cyclic DBC communication matrix.

We subsequently built an industrial-grade Linux SocketCAN driver:

Implemented a 100 Hz deterministic periodic thread transmitting steering angle, throttle voltage, and brake pressure setpoints;
Integrated a dual-directional watchdog supervisor: if the VCU misses valid heartbeat packets for $>100\text{ ms}$, onboard firmware immediately disengages autonomous drive and engages gentle mechanical braking, preventing runaway faults caused by upstream software crashes.

Full-Stack ROS Autonomy Architecture

The software stack operates on Ubuntu Linux and ROS with optimized shared-memory IPC, partitioned into three core decoupled pipelines:

1. Localization & High-Precision Mapping Pipeline

Tightly Coupled LiDAR-Inertial SLAM: Extracting geometric edge and planar features from raw 32-beam point clouds, the frontend fuses high-rate IMU pre-integration priors within a factor graph optimization backend. Between keyframes, relative motion increments and bias updates are factored alongside LiDAR point-to-plane geometric residuals into an incremental smoothing and mapping (iSAM2) factor graph, rendering globally consistent 3D point-cloud maps of campus roadways;
50 Hz Online NDT Localization: Within the pre-built metric voxel map, incoming point clouds are registered using the Normal Distributions Transform (NDT) at 50 Hz. Each spatial voxel approximates local point distributions via 3D Gaussians. Even when traversing covered pedestrian corridors and dense underpasses where GNSS signals vanish, NDT scan registration maintains continuous state estimation without trajectory drift or discrete position jumps;
Dual-Antenna Differential Kinematic Heading: Fixed dual antennas establish an instantaneous physical baseline across the vehicle roof, determining true vehicular heading even at zero velocity without dead-reckoning integration drift.

2. Environmental Perception & Scene Understanding Pipeline

Bird’s-Eye-View Visual Lane Tracking: Monocular camera images undergo distortion rectification and Inverse Perspective Mapping (IPM). A lightweight segmentation network extracts lane boundary pixels, fitting polynomial curves to output continuous road curvature and lateral offset for steering regulation;
Multimodal 3D Object Detection & Tracking: 2D deep learning detectors classify pedestrians, bicycles, and vehicles in image space. Detected bounding frustums are back-projected into spatial point clouds to segment candidate point clusters and fit 3D oriented bounding boxes. An Interacting Multiple Model Unscented Kalman Filter (IMM-UKF) bridges constant-velocity and constant-turn-rate motion hypotheses to publish stable obstacle velocity vectors.

3. Hierarchical Planning & High-Rate Actuation

Hierarchical Decision State Machine: Finite state machines (FSM) govern macroscopic mission transitions across cruise, dynamic yield, intersection negotiation, bay parking, and emergency stop. Local trajectory generators sample candidate polynomial trajectories in the Frenet frame to dodge static obstacles and slow pedestrians;
Dual-Loop Dynamic Tracking Control: Lateral control combines Pure Pursuit lookahead with Linear Time-Varying Model Predictive Control (LTV-MPC) over a dynamic bicycle model, enforcing steering angle limits and tire lateral slip angle saturation bounds to eliminate overshoot during high-curvature turns. Longitudinal control employs closed-loop PID with speed feedforward, transmitting steering and braking commands at 100 Hz over CAN.

Seven Real-Vehicle Closed-Loop Scenarios

Real-Time 3D LiDAR SLAM MappingOnboard 3D LiDAR incrementally constructing metric point-cloud maps of complex campus terrain with crisp structural fidelity.

High-Precision SLAM Waypoint TrackingAutonomous waypoint navigation localized entirely within self-built 3D LiDAR point-cloud maps.

Vision-Based Lane Centering on 4WIS AGVMonocular front-facing camera segmenting lane boundaries to drive smooth centering control at speed.

Dynamic Multi-Object Roadside TrackingSynchronized camera-LiDAR fusion tracking pedestrians, vehicles, and dynamic campus traffic.

Autonomous Emergency Braking (AEB)Sub-millisecond emergency brake actuation upon sudden obstacle intrusion across the vehicle path.

Autonomous Precision Bay ParkingCoordinated ultrasonic-vision trajectory optimization backing the vehicle into narrow parking stalls.

V2X Multi-Vehicle PlatooningDirect inter-vehicle ad-hoc wireless communications coordinating two autonomous cars in synchronized following along curves.

The seven video recordings document extensive field testing across campus thoroughfares:

Real-Time 3D LiDAR Mapping: The passenger vehicle cruises smoothly under dense tree canopies, incrementally rendering metric 3D point-cloud geometry of building facades and road curbstones;
High-Precision SLAM Waypoint Following: Operating with GNSS antennas disconnected, the vehicle navigates autonomously through long, winding campus avenues guided solely by real-time NDT point-cloud matching;
Vision Lane Centering on 4WIS AGV: The forward camera parses faded white road lines, while the 4WIS chassis dynamically modulates steer angles to remain centered in the travel lane;
Dynamic Roadside Object Tracking: Onboard detectors identify pedestrians crossing intersections and oncoming bicycles, maintaining 3D bounding boxes and trajectory predictions;
Autonomous Emergency Braking (AEB) Verification: When an obstacle mannequin is rapidly thrust into the vehicle path, perception computes Time-to-Collision ($\text{TTC} < 1.2\text{s}$) within 60 ms, triggering full hydraulic clamp lockup to halt the vehicle 0.8 meters before the target;
Precision Bay Parking: Ultrasonic sensors measure bay clearance, allowing the planner to execute a two-stage reverse trajectory that parks the 4WIS AGV neatly aligned with painted stall markers;
V2X Connected Platooning: Over 5.8 GHz ad-hoc wireless mesh links, lead-vehicle acceleration and braking intentions are shared at 100 Hz, enabling two autonomous vehicles to maintain tight, zero-lag platoon spacing through multi-turn courses.

Engineering Summary & Technical Takeaways

Building two complete physical autonomous platforms from bare metal established foundational systems engineering insights:

Physical Ground Truth Governs Algorithmic Success: Simulators hide bus dropouts, high-frequency frame vibration, electrical ground loops, and motor deadbands. Only by directly debugging power distribution surges and actuator dead zones on real iron can one design resilient control architectures with adequate feedforward and fail-safe watchdogs.
Deterministic Timing Underpins Sensor Fusion: Sophisticated geometric algorithms fail if multi-modal sensor streams suffer millisecond-scale timestamp jitter. Enforcing hardware-level PPS clock discipline and external exposure triggering is a prerequisite for reliable real-world perception.
Reusable Modular System Foundation: The full-stack hardware-software architecture established in this project provided directly transferable foundational modules for subsequent laboratory research and industry partnerships in mobile robotics and autonomous valet parking.
Hardware-Level Heartbeat Watchdogs as Safety Primitives: Upstream Linux software stacks inevitably experience occasional thread freezes, driver crashes, or kernel scheduling delays. Guaranteeing that the low-level chassis microcontroller automatically cuts throttle and engages gradual hydraulic braking when cyclic CAN heartbeats drop for 100 ms is what keeps physical autonomous prototypes safe during high-speed live road trials.

Engineering Projects | Chang Nie

A Robotic Chemist: Full-Stack Dual-Arm Mobile Manipulation for Biochemical Experiments

Real Laboratory Challenges & Architectural Layers

Core Technical Solutions: From Physics Priors to Closed-Loop Self-Healing

1. Robust 6D Pose Estimation on Transparent & Specular Glassware

2. Kinematic Retargeting of Human Demonstration & Singularity Avoidance

3. Precision Alignment and Compliant Centrifuge Assembly

4. Fluid Motion Stabilization & Meniscus-Monitored Pouring

5. Physical State Contracts & Long-Horizon Self-Healing Execution

Real-Robot Experiments & Demos

Engineering Summary & Technical Takeaways

CUHK Collaboration: Mobile Dual-Arm Manipulation for Library Shelf Tidying and Book Reshelving

Project context

My contributions

Physical Challenges and the Fundamental Paradigm Shift of Mobile Manipulation

Software-Hardware Division and the Deterministic Control Boundary

Joint Optimization of Base Docking and Arm Manipulability

Coordinated Bimanual Manipulation and Compliant Contact Mechanics

1. Stabilizing Arm: Lateral Preloading and Anti-Collapse Support

2. Primary Manipulating Arm: Tangential Compliant Extraction

3. Tilted Micro-Search and Compliant Slot Insertion

Object-Centric Shelf World Model

1. Structured Node Representations

2. Multi-View Spatial-Temporal Data Association

3. Counterfactual Simulation for Action Safety

AI Agent Task Orchestration and Bounded Error Recovery

1. Structured Task Planning and Schema Validation

2. Explicit Physical Verification Contracts

3. Multi-Tier Bounded Error Recovery

Progressive Milestones & Projected Performance Targets

Engineering Lessons and Technical Takeaways

Warwick Collaboration: Decoupled Hierarchical Embodied AI and Agentic Robotics for Long-Horizon Tasks

Project context

My contributions

Physical Brittleness and the Challenge of Compound Error Propagation

Three-Tier Decoupled Embodied AI Architecture

1. Level 3: Semantic and Long-Horizon Task Planning Layer (AI Agent Brain, 0.5 Hz - 2 Hz)

2. Level 2: Metric-Geometric Skill Synthesis Layer (Skill Synthesis & Trajectory Generator, 10 Hz - 50 Hz)

3. Level 1: High-Frequency Compliant Execution Layer (Deterministic Real-Time Control, 500 Hz - 1000 Hz)

Multimodal 3D Spatial Perception: Visual-mmWave Radar Fusion

1. Complementary Physical Sensing and Microsecond Synchronization

2. Kinematic Clutter Filtering and Log-Odds Volumetric Occupancy Mapping

Video Temporal Grounding and Physical State Verification

1. Cross-Modal Video Temporal Grounding

2. Multi-Modal Physical Verification Contracts

3. Hierarchical Bounded Error Recovery

System Integration & Projected Target Objectives

Engineering Lessons and Technical Takeaways

Industrial Logistics Robotics: Autonomous Dual-Arm Mobile Manipulation for Heavy Tote Handling

Project context

My contributions

Key Engineering Bottlenecks in Production

Full-Stack Architecture & Algorithmic Design

1. Robust 6D Pose Estimation on Weak-Texture Totes & Stacking DAG Reasoning

2. Virtual-Mechanism Leader-Follower Admittance Control & Internal Stress Decoupling

3. Hierarchical Base-to-Workstation Alignment & Hand-Eye Visual Servoing

4. Transit Anti-Sway Dynamics & Jerk-Limited Trajectory Profiles

5. Whole-Body Kinematic Redundancy & 1 kHz Synchronous Bus

6. Fault Detection & Self-Healing Recovery State Machine

Factory Demonstration & Field Verification

Engineering Summary & Technical Takeaways

Production-Grade 4D Auto-Annotation: Multi-Camera Rig SfM & High-Precision Geometry

Project context

My contributions

Why Driving Scenarios Break Classic SfM

Full Pipeline Architecture: From Raw Logs to 4D Vector Maps

Core Algorithmic Design & Engineering Implementation

1. Rig Bundle Adjustment (Rig BA) with Rigid-Rig Parameterization

2. Spatial Overlap Constraints & Epipolar Pruning

3. Decoupled Ground-Plane Filtering Prior to Triangulation

4. Spatiotemporal-Guided Dense PatchMatch Stereo (MVS)

5. Coarse-to-Fine Multi-Session Aggregation Network

Production Quality Assurance & Field Validation

Pipeline Video Recordings

Engineering Summary & Technical Takeaways

Autonomous Valet Parking: Production-Grade Full-Stack System for Underground Garages

Project context

My contributions

Key Engineering Bottlenecks in Underground Garages

Full-Stack Architecture & Algorithmic Design