Integrated system and method for autonomous vehicle navigation andb3d environment mapping using multi-modal sensing

Abstract

The disclosed invention provides robust multi-sensor fusion, real-time autonomous navigation, adaptive environmental response, simplified deployment process, and reduced operational costs. Its components form an interconnected pipeline for autonomous navigation and environmental perception. Sensors provide environmental data; processing components such as but not limited to the sensor data processor interpret sensor data; vision components such as those in the core processor handle visual perception; data management via the data manager ensures proper data flow; communication between the various processors enables component interaction; and output components such as those in the output layer processor deliver usable results.

Description

INTEGRATED SYSTEM AND METHOD FOR AUTONOMOUS VEHICLE NAVIGATION AND3D ENVIRONMENT MAPPING USING MULTI-MODAL SENSING INVENTORS: Sudheendra KrishnamurthyCROSS-REFERENCE TO RELATED APPLICATIONS

[0001] This application claims priority through India Provisional Application 202511036258 filed on April 15, 2025, incorporated herein by reference.BACKGROUND OF THE INVENTION

[0002] Current autonomous navigation systems for remotely operated vehicles (ROVs), drones, and industrial vehicles face several critical challenges, including reliability issues such as poor performance in low-visibility conditions, navigation failures in GPS-denied environments, and / or inaccurate depth perception, leading to collisions; operational limitations such as manual docking / landing requirements, slow inspection speeds due to safety concerns, and / or limited autonomous capabilities; integration challenges such as complex setup requirements, poor interoperability between sensors, and / or high latency in real-time processing; and / or cost impact arising from extended operation times, higher personnel requirements, equipment damage from collisions, and / or increased downtime.

[0003] The current market solutions for autonomous vehicle navigation and 3D environment mapping has several limitations, including requiring manual piloting with basic automation or only possessing basic obstacle avoidance such as in commercial ROV Systems; being dependent on GPS navigation, requiring visual navigation that is typically limited to well-lit conditions, and / or having limited range perception such as in drone navigation systems; requiring fixed path navigation, use of 2-D based navigation, and / or having limited obstacle handling, such as with industrial automated guided vehicle (AGV) solutions.{NATAER0000066575.0} Page 1 of 19

[0004] In most of these solutions, key limitations include single-sensor dependency, poor performance in challenging environments, limited real-time processing, high latency, manual intervention requirements, and / or requiring separate systems for different platforms.BRIEF DESCRIPTION OF DRAWINGS

[0005] Various figures are included herein which illustrate aspects of embodiments of the disclosed inventions.

[0006] Fig. 1 is a block diagram of an exemplary modular, scalable integrated system for autonomous vehicle navigation and three-dimensional (3D) environment mapping using multimodal sensing.DETAILED DESCRIPTION OF EMBODIMENTS OF THE INVENTION

[0007] The disclosed invention comprises an integrated hardware- software system. As used herein, a “processor” or “computing unit” comprises hardware, comprising a central processing unit and associated random access memory and / or a specialized hardware, software executable in the hardware, or the like, or a combination thereof, e.g., an NVIDIA Jetson Orin processor.

[0008] In a first embodiment, referring generally to Fig. 1, modular, scalable integrated system for autonomous vehicle navigation and three-dimensional (3D) environment mapping using multi-modal sensing 1 comprises a plurality of sensors 10, each sensor comprising a modal sensor data output; perception processor 20, comprising object processor 22 which, in turn, comprises two-dimensional (2D) object detector 22a operatively in communication with stereo sensor 10a of the plurality of sensors 10; object tracker 22b operatively in communication with 2D object detector 22a; stereo depth processor 23 operatively in communication with stereo sensor 10a of the plurality of sensors 10 which may further comprise a 3D vision stereo camera; and pose{NATAER0000066575.0} Page 2 of 19estimator 24 operatively in communication with object tracker 22b and stereo depth processor 23; application programming interface (API) wrapper 30 operatively in communication with stereo sensor 10a of the plurality of sensors 10 and with inertial measurement unit (IMU) sensor 10c of the plurality of sensors 10, the API wrapper comprising real-time appearance-based mapping (RTAB) map core 30a, first sessions database 32 operatively in communication with RTAB map core 30a, and API database manager 30c operatively in communication with RTAB map core 30a and first sessions database 32; simultaneous localization and mapping (SLAM) integrator 40 comprising a predetermined plurality of SLAM implementations 41a, 41b, 41c, 41d and operatively in communication with IMU sensor 10c and with LiDAR sensor 10d of the plurality of sensors 10; second sessions database 42 operatively in communication with SLAM integrator 40; ephemeral processor 50 operatively in communication with IMU sensor 10c and SONAR sensor 10e of the plurality of sensors 10; third sessions database 52 operatively in communication with ephemeral processor 50; real time synchronizer 60 operatively in communication with first sessions database 32, second sessions database 42, and third sessions database 52; output aggregator 62 operatively in communication with pose estimator 24 and synchronizer 60; data manager 70, comprising world map database 72 operatively in communication with synchronizer 60, shared memory / buffer 74 operatively in communication with output aggregator 62 and world map database 72, format converter 75 operatively in communication with shared memory / buffer 74, message queue manager 76 operatively in communication with format converter 75; and data consumer 80 comprising database store 82 which is operatively in communication with world map database 72, navigator 84 operatively in communication with database store 82; visualization client 86 operatively in communication with database store 82, and graphics display interface (GDI) post processor 88 operatively in communication with database store 82.{NATAER0000066575.0} Page 3 of 19

[0009] In embodiments, the plurality of sensors 10 provides sensed data to perception processor 20 via a plurality of parallel pipelines. These predetermined set of parallel pipelines may comprise a plurality of real-time processing pipelines, comprising a visual processing pipeline utilizing data from 2D object detector 22a, object track processor 22b, stereo depth processor 23, and pose estimator 24; a core processor pipeline utilizing data from API wrapper 30 and stateful processor; and a SLAM pipeline utilizing data from ephemeral processor 50 and stateful processor.

[0010] The plurality of sensors 10 may further comprise monocular sensor 10b; RGB-D sensor 10f, and environmental sensor 10g.

[0011] In embodiments pose estimator 54 comprises final six degree of freedom (6-DOF) pose 54a and path planning processor 54b operatively in communication with final 6-DOF pose 54a. Typically, path planning processor 54b comprises a dynamic window.

[0012] In embodiments, multiple SLAM implementations comprise: LiDAR-inertial odometry package (FAST-LIO2) SLAM 41a for a LIDAR-based SLAM; universal Front-End workflow SLAM (BALM) 41b for additional mapping capabilities; continuous time iterative closest point (CT-ICP) SLAM 41c for point cloud processing; MultiBeam SLAM 41d; Single Beam SLAM 41e; and integrator 43 comprising an OpenNav and Ephemeral System.

[0013] In embodiments, ephemeral processor 50 comprises MultiBeam SLAM processor 51a, first OpenNav system 51b adapted to process a Single Beam SLAM, and second OpenNav system 51c adapted process both a MultiBeam SLAM and a Single Beam SLAM.

[0014] Typically, 2D object detector 22a is configured to identify an object in a camera data set; object tracker 22b is configured to follow an identified object from 2D object detector 22a across a predetermined set of video frames; stereo sensor 10a comprises a stereo camera; stereo{NATAER0000066575.0} Page 4 of 19depth detector 23 is configured to process data from the stereo camera and obtain depth information; and pose estimator 54 is configured to combine data from 2D object detector Ila, object track processor 22b, and stereo depth detector 23 and process the combined data to determine a position and orientation of the identified object.

[0015] In embodiments, message queue manager 76 comprises message queue broker 77, message queuing telemetry transport (MQTT) broker 78 operatively in communication with message queue broker 76 and world map database 72, and MQTT topics processor 79 operatively in communication with MQTT broker 78.

[0016] In embodiments, format converter 75 is configured to provide synchronized websocket data useful for real-time communication.

[0017] In the operation of exemplary methods, referring still to Fig. 1, autonomous vehicle navigation and three-dimensional (3D) environment mapping using multi-modal sensing may be provided using modular, scalable integrated system 1 as described herein above to plan a path for an autonomous vehicle by obtaining data from the plurality of sensors 10 in real-time; using world map data from world map database 72 set to maintain an environmental model; using perception processor 20 to use stereo depth detector 23 to process data, including obtaining depth information, from stereo camera 10a of the plurality of sensors 10; using monocular camera 10b of the plurality of sensors 10 for basic visual input; using IMU sensor 10c of the plurality of sensors 10 for motion tracking; using LIDAR sensor 10d of the plurality of sensors 10 for precise distance measurement; using SONAR sensor 10e of the plurality of sensors 10 for obstacle detection; and using an RGB-D camera 10f of the plurality of sensors 10 for combined color and depth data; using SLAM integrator 40 to improve localization accuracy by combining multiple SLAM approaches in parallel and provide real-time switching between SLAM methods based on{NATAER0000066575.0} Page 5 of 19environmental conditions; using perception processor 20 to integrate the data obtained from the plurality of sensors 10 in real-time and persist the data in sessions; generating a real-time dynamic path trajectory for the autonomous vehicle by using real-time 3D reconstruction by using pose estimator 24 to combine data from 2D object detector 22a, object track processor 22b, and stereo depth detector 23 and process the combined data using parallel processing pipelines to determine position and orientation of an identified object; cross-validating sensor readings; maintaining system state data across sessions; handling dynamic environment updates; and managing temporal data consistency.

[0018] In embodiments, a predetermined usage policy for stereo depth processing may be implemented to optimize computational resources and a dynamic weighting system used that automatically adjusts sensor input importance based on environmental conditions combined with a smart fallback system when certain sensors 10 of the plurality of sensors 10 fail or become unreliable.

[0019] Integrating data from the plurality of sensors 10 in real-time may comprise using parallel processing pipelines, efficient data synchronization, and low-latency communication through MQTT.

[0020] In embodiments, an intelligent state manager may be used to create and / or maintain session persistence to maintain environmental awareness across system restarts. In these embodiments, databases 32,42,52,72,82 may be used to enable rapid state recovery and provide continuity and reliability.

[0021] In embodiments, an enhanced path planning system comprising pose estimator 24 may be implemented by combining dynamic window A* with real-time pose estimation to integrate pose estimation with path planning in a single coherent pipeline. As those of skill in these{NATAER0000066575.0} Page 6 of 19arts understands, A* is an algorithm that can be used to find a shortest path and a dynamic A* (sometimes referred to as D*) is an incremental heuristic search algorithm that combines ideas of A* and Dynamic SWSF-FP.

[0022] In embodiments, an advanced data management architecture may be present using ephemeral processor 50 by combining a graphics database manager for topological relationships, a second database manager, e.g., PostgreSQL with PostGIS, for spatial data, and a third database manager, e.g., Apache Cassandra, for time-series data.

[0023] In embodiments where message queue manager 76 comprises message queue broker 77 and message queuing telemetry transport (MQTT) broker 78 operatively in communication with message queue broker 77 and world map database 72, real-time synchronizer 60 may be used to handle multi-sensor data alignment by integrating the MQTT with websocket data for real-time data streaming.

[0024] In embodiments, output aggregator 62 may combine multiple data streams using a MQTT and Websocket protocols for real-time data distribution.

[0025] As will be apparent to those of ordinary skill, modular, scalable integrated system 1 provides robust navigation comprising multi-modal sensor fusion, real-time SLAM implementation, dynamic obstacle avoidance, and autonomous docking capability; environmental adaptation comprising automatic exposure / gain control, sensor weighting based on conditions, and fault detection and recovery; enhanced performance metrics comprising a 30Hz update rate, 2-5cm localization accuracy, 10Hz path planning, and < 100ms control latency; and safety systems comprising emergency stop protocols, health monitoring, collision prediction, and failsafe behaviors. In addition, the disclosed invention provides cross-platform compatibility (ROV / AUV / drone / forklift).{NATAER0000066575.0} Page 7 of 19

[0026] Compared to existing systems modular, scalable integrated system 1 provides superior environmental adaptation via multi-sensor fusion as opposed to single sensor dependency, real-time adaptation to visibility conditions, and cross-platform capability (remotely operated vehicle (ROV) / drone / forklift); enhanced performance via real-time 3D reconstruction vs postprocessing; advanced autonomy via dynamic path planning vs fixed routes, autonomous docking vs manual intervention, active obstacle avoidance vs passive detection, and multi-modal SLAM vs single-source mapping; operational benefits including 60% reduction in manual intervention, 40% faster inspection speeds, 30% lower operational costs, and cross-environment functionality.

[0027] In most embodiments, modular, scalable integrated system 1 comprises a scalable and modular architecture comprising modular component design, multiple database options for different data types, and distributed processing capability. This architecture provides a comprehensive solution for autonomous navigation with robust perception capabilities, efficient data management, and real-time processing. Modular, scalable integrated system 1 can handle complex environments while maintaining reliability and performance through its multi-layered approach and redundant systems.

[0028] In its embodiments, modular, scalable integrated system 1 comprises superior reliability, multiple redundant systems, intelligent failover capabilities, cross-validation of sensor data, enhanced accuracy, a multi-SLAM approach which improves localization accuracy, advanced sensor fusion which reduces error rates, and real-time pose estimation which improves navigation precision. It can lead to better performance via optimized resource usage, efficient data management, and real-time processing capabilities.

[0029] The foregoing disclosure and description of the inventions are illustrative and explanatory. Various changes in the size, shape, and materials, as well as in the details of the{NATAER0000066575.0} Page 8 of 19illustrative construction and / or an illustrative method may be made without departing from the spirit of the invention.{NATAER0000066575.0} Page 9 of 19

Claims

CLAIMS1) A modular, scalable integrated system for autonomous vehicle navigation and three-dimensional (3D) environment mapping using multi-modal sensing (1), comprising:a) a plurality of sensors (10), each sensor (10) comprising a modal sensor data output; b) a perception processor (20), comprising:i) an object processor (22), comprising(1) a two-dimensional (2D) object detector (22a) operatively in communication with a stereo sensor (10a) of the plurality of sensors (10);(2) an object tracker (22b) operatively in communication with the 2D object detector (22a);(3) a stereo depth processor (23) operatively in communication with the stereo sensor (10a) of the plurality of sensors (10); and(4) a pose estimator (24) operatively in communication with the object tracker (22b) and the stereo depth processor (23);ii) an application programming interface (API) wrapper (30) operatively in communication with the stereo sensor (10a) of the plurality of sensors (10) and with an inertial measurement unit (IMU) sensor (10c) of the plurality of sensors (10), the API wrapper (30) comprising:(1) a real-time appearance-based mapping (RTAB) map core (30a);(2) a first sessions database (32) operatively in communication with the RTAB map core (30a); and(3) an API database manager (30c) operatively in communication with the RTAB map core (30a) and the first sessions database (32);{NATAER0000066575.0} Page 10 of 19iii) a simultaneous localization and mapping (SLAM) integrator (40) comprising a predetermined plurality of SLAM implementations (41a, 41b, 41c, 41d) and operatively in communication with the IMU sensor (10c) and with a LiDAR sensor (10d) of the plurality of sensors (10);iv) a second sessions database (42) operatively in communication with the SLAM integrator (40);v) an ephemeral processor (50) operatively in communication with the IMU sensor (10c) and a SONAR sensor (10e) of the plurality of sensors (10);vi) a third sessions database (52) operatively in communication with the ephemeral processor (50);vii)a real time synchronizer (60), operatively in communication with the first sessions database (32), the second sessions database (42), and the third sessions database (52); andviii) an output aggregator (62) operatively in communication with the pose estimator (24) and the real time synchronizer (60);c) a data manager (70), comprising:i) a world map database (72) operatively in communication with the real time synchronizer (60);ii) a shared memory / buffer (74) operatively in communication with the output aggregator (62) and the world map database (72);iii) a format converter (75) operatively in communication with the shared memory / buffer (74);{NATAER0000066575.0} Page 11 of 19iv) a message queue manager (76) operatively in communication with the format converter (75); andd) a data consumer (80), comprising:i) a database store (82), operatively in communication with the world map database (72); ii) a navigator (84) operatively in communication with the database store (82);iii) a visualization client (86) operatively in communication with the database store (82);andiv) a graphics display interface (GDI) post processor (88) operatively in communication with the database store (82).2) The system of Claim 1, wherein the plurality of sensors (10) provides sensed data to the perception processor (20) via a plurality of parallel pipelines.3) The system of Claim 2, wherein the predetermined set of parallel pipelines comprises a plurality of real-time processing pipelines, comprising:a) a visual processing pipeline utilizing data from the 2D object detector (22a), the object track processor, the stereo depth processor, and the pose estimator;b) a core processor pipeline utilizing data from the API wrapper and the stateful processor;andc) a SLAM pipeline utilizing data from the ephemeral processor (50) and the stateful processor.4) The system of Claim 1, wherein the plurality of sensors (10) further comprises:a) a monocular sensor (10b);b) an RGB-D sensor (10f); andc) an environmental sensor (10g).{NATAER0000066575.0} Page 12 of 195) The system of Claim 1, wherein the stereo sensor (10a) comprises a 3D vision stereo camera.6) The system of Claim 1, wherein the pose estimator (54) comprises:a) a final six degree of freedom (6-DOF) pose (54a); andb) a path planning processor (54b) operatively in communication with the final 6-DOF pose, the path planning processor (54b) further comprising a dynamic window7) The system of Claim 1, wherein:a) the multiple SLAM implementations comprise:i) a LiDAR-inertial odometry package (FAST-LIO2) SLAM (41a) for a LIDAR-based SLAM;ii) a universal Front-End workflow SLAM (BALM) (41b) for additional mapping capabilities;iii) a continuous time iterative closest point (CT-ICP) SLAM (41c) for point cloud processing;iv) a MultiBeam SLAM (41d);v) a Single Beam SLAM (41e); andb) an integrator (53) comprising an OpenNav and Ephemeral System.8) The system of Claim 1, wherein the ephemeral processor (50) comprises:a) a MultiBeam SLAM processor (51a);b) a first OpenNav system (51b) adapted to process a Single Beam SLAM; andc) a second OpenNav system (51c) adapted process both a MultiBeam SLAM and a Single Beam SLAM.9) The system of Claim 1, wherein:{NATAER0000066575.0} Page 13 of 19a) the 2D object detector (22a) is configured to identify an object in a camera data set; b) the object tracker (22b) is configured to follow an identified object from the 2D object detector across a predetermined set of video frames;c) the stereo sensor (10a) comprises a stereo camera;d) the stereo depth detector (23) is configured to process data from the stereo camera and obtain depth information; ande) the pose estimator (54) is configured to combine data from the 2D object detector (22a), the object track processor (22b), and the stereo depth detector (23) and process the combined data to determine position and orientation of the identified object.10) The system of Claim 1, wherein the message queue manager (76) comprises:a) a message queue broker (77);b) a message queuing telemetry transport (MQTT) broker (78) operatively in communication with the message queue broker (77) and the world map database; andc) an MQTT topics processor (79) operatively in communication with the MQTT broker. 11) The system of Claim 1, wherein the format converter (75) is configured to provide synchronized websocket data useful for real-time communication.12) A method of providing autonomous vehicle navigation and three-dimensional (3D) environment mapping using multi-modal sensing to plan a path for an autonomous vehicle using a modular, scalable integrated system for autonomous vehicle navigation and three-dimensional (3D) environment mapping using multi-modal sensing (1) comprising a plurality of sensors (10), each sensor (10) comprising a modal sensor data output; a perception processor (20) comprising an object processor (22) which comprises a two-dimensional (2D) object detector (22a) operatively in communication with a stereo sensor (10a) of the plurality of sensors (10), an object tracker (22b){NATAER0000066575.0} Page 14 of 19operatively in communication with the 2D object detector (22a), a stereo depth processor (23) operatively in communication with the stereo sensor (10a) of the plurality of sensors (10), and a pose estimator (24) operatively in communication with the object tracker (22b) and the stereo depth processor (23); an application programming interface (API) wrapper (30) operatively in communication with the stereo sensor (10a) of the plurality of sensors (10) and with an inertial measurement unit (IMU) sensor (10c) of the plurality of sensors (10) where the API wrapper (30) comprises a real-time appearance-based mapping (RTAB) map core (30a), a first sessions database (32) operatively in communication with the RTAB map core (30a), and an API database manager (30c) operatively in communication with the RTAB map core (30a) and the first sessions database (32); a simultaneous localization and mapping (SLAM) integrator (40) comprising a predetermined plurality of SLAM implementations (41a,41b,41c, 41d) and operatively in communication with the IMU sensor (10c) and with a LiDAR sensor (10d) of the plurality of sensors (10); a second sessions database (42) operatively in communication with the SLAM integrator (40); an ephemeral processor (50) operatively in communication with the IMU sensor (10c) and a SONAR sensor (10e) of the plurality of sensors (10); a third sessions database (52) operatively in communication with the ephemeral processor (50); a real time synchronizer (60), operatively in communication with the first sessions database (32), the second sessions database (42), and the third sessions database (52); and an output aggregator (62) operatively in communication with the pose estimator (24) and the real time synchronizer (60); a data manager (70) comprising a world map database (72) operatively in communication with the real time synchronizer (60), a shared memory / buffer (74) operatively in communication with the output aggregator (62) and the world map database (72), a format converter (75) operatively in communication with the shared memory / buffer (74), a message queue manager (76) operatively{NATAER0000066575.0} Page 15 of 19in communication with the format converter (75), and a data consumer (80) comprising a database store (82) operatively in communication with the world map database (72), a navigator (84) operatively in communication with the database store (82), a visualization client (86) operatively in communication with the database store (82), and a graphics display interface (GDI) post processor (88) operatively in communication with the database store (82), the method comprising:a) obtaining data from the plurality of sensors (10) in real-time;b) using world map data from the world map database (72) to maintain an environmental model;c) using the perception processor (20) to process sensor data, further comprising:i) using the stereo depth detector (23) to process data, including obtain depth information, from a stereo camera (10a) of the plurality of sensors (10);ii) using a monocular camera (10b) of the plurality of sensors (10) for basic visual input; iii) using an inertial measurement unit (IMU) sensor (10c) of the plurality of sensors (10) for motion tracking;iv) using a LIDAR sensor (10d) of the plurality of sensors (10) for precise distance measurement;v) using a SONAR sensor (10e) of the plurality of sensors (10) for obstacle detection; and vi) using an RGB-D camera (10f) of the plurality of sensors (10) for combined color and depth data;d) using the SLAM integrator (40) to improve localization accuracy by combining multiple SLAM approaches in parallel and provide real-time switching between SLAM methods based on environmental conditions; ande) using the perception processor (20) integrate the data obtained from the plurality of sensors{NATAER0000066575.0} Page 16 of 19(10) in real-time and persist the data in sessions; andf) generating a real-time dynamic path trajectory for the autonomous vehicle by using realtime 3D reconstruction, comprising:i) using the pose estimator (54) to combine data from the 2D object detector (22a), the object track processor (22b), and the stereo depth detector (23); andii) processing the combined data using parallel processing pipelines to determine a position and an orientation of an identified object.13) The method of Claim 12, further comprising:a) implementing a predetermined usage policy for stereo depth processing to optimize computational resources; andb) using a dynamic weighting system that automatically adjusts sensor input importance based on environmental conditions combined with a smart fallback system when certain sensors (10) fail or become unreliable.14) The method of Claim 12, wherein comprising integrating data from the plurality of sensors (10) in real-time comprises using parallel processing pipelines, efficient data synchronization, and low-latency communication through MQTT.15) The method of Claim 12, further comprising:a) enabling an intelligent state manager comprising session persistence to maintain environmental awareness across system restarts; andb) using the databases (32,42,52,72,82) to enable rapid state recovery and provide continuity and reliability.16) The method of Claim 12, further comprising implementing an enhanced path planning system comprising the pose estimator (54) by combining dynamic window A-star with real-time{NATAER0000066575.0} Page 17 of 19pose estimation to integrate pose estimation with path planning in a single coherent pipeline. 17) The method of Claim 12, further comprising implementing an advanced data management architecture comprising the ephemeral processor (50) by combining a graphics database manager for topological relationships, a second database manager for spatial data, and a third database manager for time-series data.18) The method of Claim 12, wherein:a) the message queue broker (77) comprises a message queue broker (77) and a message queuing telemetry transport (MQTT) broker (78) operatively in communication with the message queue broker (77) and the world map database (72); andb) the real-time real time synchronizer (60) handles multi-sensor data alignment by integrating the MQTT with websocket data for real-time data streaming.19) The method of Claim 12, further comprising using the output aggregator (62) to combine multiple data streams using a MQTT and Websocket protocols for real-time data distribution.{NATAER0000066575.0} Page 18 of 19

Images