{"id":166,"date":"2026-04-03T20:36:06","date_gmt":"2026-04-04T01:36:06","guid":{"rendered":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teama\/?page_id=166"},"modified":"2026-11-14T01:28:29","modified_gmt":"2026-11-14T06:28:29","slug":"navigation-sub-system","status":"publish","type":"page","link":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teama\/system-implementation\/navigation-sub-system\/","title":{"rendered":"Navigation Sub-system"},"content":{"rendered":"\n

Fall 2026 Updates<\/h3>\n\n\n\n

Navigation Stack:<\/strong><\/p>\n\n\n\n

The navigation stack takes the Livox MID360 LiDAR and IMU into SLAM\/localization and terrain analysis. A route planner runs Dijkstra routing over a saved terrain graph and outputs carrot waypoints; a local planner produces a collision-free path, a path follower converts it to velocity, and a command mux arbitrates between teleoperation and navigation before commands reach the Unitree interface.<\/p>\n\n\n\n

\"Navigation<\/a>
Fig:<\/strong> Navigation stack: SLAM and localization, route planning, local planning and path following.<\/figcaption><\/figure>\n\n\n\n

Multi-Floor Mapping (Elevator-LIO):<\/strong><\/p>\n\n\n\n

Arise-SLAM could not map across floors because of IMU drift during the elevator ride, so it was replaced with Elevator-LIO for state estimation. Elevator-LIO maps across floors, but its activation condition for IMU correction is still simple and may miss the moment of entering the elevator.<\/p>\n\n\n\n

\"Multi-floor<\/a>
Fig:<\/strong> Multi-floor mapping: Arise-SLAM drifts across floors, while Elevator-LIO maps across floors.<\/figcaption><\/figure>\n\n\n\n

Relocalization:<\/strong><\/p>\n\n\n\n

Relocalization against the saved map was added using GICP (Generalized Iterative Closest Point). In testing, the robot successfully relocalized itself with a structure score of 0.97.<\/p>\n\n\n\n

\"GICP<\/a>
Fig:<\/strong> GICP relocalization against the saved map (structure score 0.97).<\/figcaption><\/figure>\n\n\n\n

Base Servoing Improvements:<\/strong><\/p>\n\n\n\n

Base servoing, a risk identified at the Critical Design Review, was improved by calibrating the IMU, removing lateral velocity, and switching to a non-holonomic planner. The robot now navigates to and from the elevator autonomously.<\/p>\n\n\n\n

Open issues:<\/em> the local planner sometimes produces poor locomotion plans, low locomotion precision can leave the button out of view, and a stiffer left ankle makes waypoints harder to track.<\/p>\n\n\n\n

Spring 2026<\/h3>\n\n\n\n

Base Visual Servoing: <\/strong><\/p>\n\n\n\n

The visual servoing pipeline moves the robot’s base to an approach pose in front of a detected button panel, closing the loop from perception to robot motion. A pose filtering node applies moving-average smoothing and outlier rejection to the raw tracked pose, and the core Position-Based Visual Servoing (PBVS) node computes an approach goal by retracting along the surface normal by a configurable distance. It then sends this goal to the local movement
module, which drives the robot to the target. The robot can detect a button panel, filter the noisy pose estimate, compute a stable approach position,
and drive toward it \u2014 all without manual waypoints.<\/p>\n\n\n\n

\"\"<\/figure>\n\n\n\n

Fig: Robot Navigating to a Shelf<\/strong><\/p>\n\n\n\n

\"\"<\/figure>\n\n\n\n

Fig: Visual servo base pipeline.<\/strong><\/p>\n\n\n\n

<\/p>\n\n\n\n

<\/p>\n\n\n\n

<\/p>\n\n\n\n

<\/p>\n\n\n\n

<\/p>\n","protected":false},"excerpt":{"rendered":"

Fall 2026 Updates Navigation Stack: The navigation stack takes the Livox MID360 LiDAR and IMU into SLAM\/localization and terrain analysis. A route planner runs Dijkstra routing over a saved terrain graph and outputs carrot waypoints; a local planner produces a collision-free path, a path follower converts it to velocity, and a command mux arbitrates between […]<\/p>\n","protected":false},"author":434,"featured_media":0,"parent":93,"menu_order":0,"comment_status":"closed","ping_status":"closed","template":"","meta":{"footnotes":""},"class_list":["post-166","page","type-page","status-publish","hentry"],"_links":{"self":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teama\/wp-json\/wp\/v2\/pages\/166","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teama\/wp-json\/wp\/v2\/pages"}],"about":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teama\/wp-json\/wp\/v2\/types\/page"}],"author":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teama\/wp-json\/wp\/v2\/users\/434"}],"replies":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teama\/wp-json\/wp\/v2\/comments?post=166"}],"version-history":[{"count":5,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teama\/wp-json\/wp\/v2\/pages\/166\/revisions"}],"predecessor-version":[{"id":353,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teama\/wp-json\/wp\/v2\/pages\/166\/revisions\/353"}],"up":[{"embeddable":true,"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teama\/wp-json\/wp\/v2\/pages\/93"}],"wp:attachment":[{"href":"https:\/\/mrsdprojects.ri.cmu.edu\/2026teama\/wp-json\/wp\/v2\/media?parent=166"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}