Positioning method and system of mobile carrier, computer readable storage medium, device

Abstract

This disclosure relates to the field of positioning and navigation technology, providing a positioning method and system for a mobile vehicle, a computer-readable storage medium, and a device. The method includes: acquiring visual, inertial, GNSS, and visible light signal intensity data through time synchronization; transforming the GNSS and LED positions to a local navigation coordinate system using a first transformation, and aligning the visual-inertial trajectory using a second transformation including heading deviation, scale factor, and translation; constructing a GNSS position / velocity factor, a visible light distance factor, and a smoothing factor, and combining them with the visual and inertial factors to form a sliding window optimization objective function; minimizing this objective function to solve for the vehicle's state in the local navigation coordinate system; and reverting to continuous visual-inertial output when both GNSS and visible light fail, achieving highly continuous and robust positioning in complex scenarios such as tunnels. This disclosure ensures the accuracy and continuity of mobile vehicle positioning.

Description

Technical Field

[0001] This disclosure relates to the field of positioning and navigation technology, and in particular to a positioning method and system for a mobile carrier, a computer-readable storage medium, and a device. Background Technology

[0002] Large underground engineering spaces such as tunnels, underground chambers, engineering corridors, underground powerhouses, integrated utility tunnels, and mine roadways are generally characterized by their enclosed structures, long and narrow spaces, and severe electromagnetic interference. In these environments, Global Navigation Satellite System (GNSS) signals are often only briefly available in localized areas such as the entrance, and remain blocked for extended periods once inside, making it difficult to provide continuous and reliable absolute positioning references. Meanwhile, these scenarios typically feature continuous LED (Light Emitting Diode) lighting along the operational path, providing unique conditions for introducing visible light-assisted positioning. When personnel, vehicles, inspection robots, quadruped robots, or construction equipment operate within these environments for extended periods, they need to obtain a unified global coordinate reference within GNSS-available areas, while simultaneously maintaining high-frequency, low-drift, and globally consistent pose output in deep regions where GNSS is completely unavailable. This poses a significant challenge to existing positioning technologies.

[0003] It should be noted that the information disclosed in the background section above is only used to enhance the understanding of the background of this disclosure. Summary of the Invention

[0004] The purpose of this disclosure is to provide a positioning method, a positioning system, a computer-readable storage medium, and an electronic device for a mobile carrier, thereby overcoming, to at least a certain extent, the technical problem of simultaneously achieving global coordinate reference establishment and continuous, stable, and low-drift pose output under the condition that GNSS signals are unavailable for a long time in large underground engineering spaces due to limitations in related technologies.

[0005] Other features and advantages of this disclosure will become apparent from the following detailed description, or may be learned in part from practice of this disclosure.

[0006] According to a first aspect of this disclosure, a method for locating a mobile carrier is provided, comprising: The system acquires multi-source observation data of the mobile carrier at various times and synchronizes the multi-source observation data in time to form a synchronized observation set; the multi-source observation data includes visual image data, inertial measurement data, global navigation satellite system measurement data, and visible light received signal intensity data of light sources. Using a first transformation relationship from the geocentric-ground-fixed coordinate system to the local navigation coordinate system, the measurement data of the global navigation satellite system and the known positions of each light source in the geocentric-ground-fixed coordinate system are transformed to the local navigation coordinate system; using a second transformation relationship between the visual-inertial local world coordinate system and the local navigation coordinate system, the pose and velocity of the moving vehicle in the visual-inertial local world coordinate system are transformed to the local navigation coordinate system; the second transformation relationship is characterized by heading deviation, scale factor and three-dimensional translation deviation, wherein the heading deviation is a single-axis rotation parameter about the direction of gravity; Based on the position and velocity observations in the global navigation satellite system measurement data, residuals are constructed by comparing them with the corresponding components of the pose and velocity in the local navigation coordinate system after transformation by the second transformation relationship. The residuals are then weighted according to the covariances of the position and velocity observations to form position and velocity factors. Based on the known position of the light source in the local navigation coordinate system, the visible light ranging value calculated from the visible light received signal intensity data of the light source, and the geometric relationship between the pose of the moving vehicle in the visual-inertial local world coordinate system, a visible light distance factor is constructed; the position factor, the velocity factor, and the visible light distance factor are used together to constrain the heading deviation, the scale factor, and the three-dimensional translation deviation in the second transformation relationship; Based on the difference in heading deviation and the difference in scale factor between adjacent times within the sliding window, heading deviation smoothing factor and scale smoothing factor are constructed respectively. A joint optimization objective function is constructed within the sliding window; the joint optimization objective function includes a visual factor constructed based on the visual image data, an inertial factor constructed based on the inertial measurement data, a position factor, a velocity factor, a visible light distance factor, a heading deviation smoothing factor, and a scale smoothing factor; The state estimate of the mobile vehicle is solved by minimizing the joint optimization objective function; wherein the state estimate includes the positioning parameters of the mobile vehicle in the local navigation coordinate system. During periods when both the global navigation satellite system measurement data and the visible light received signal intensity data of the light source are unavailable, continuous positioning output is maintained based on the visual image data and the inertial measurement data.

[0007] In an exemplary embodiment of this disclosure, the step of constructing residuals based on the position and velocity observations from the global navigation satellite system measurement data, respectively, with the corresponding components of the pose and velocity transformed by the second transformation relationship in the local navigation coordinate system, and weighting the residuals according to the covariances corresponding to the position and velocity observations to form position and velocity factors, includes: The position residual is obtained by subtracting the coordinates of the global navigation satellite system position observation in the local navigation coordinate system from the position component of the pose obtained by mapping through the second transformation relationship in the local navigation coordinate system. The position residual is then weighted based on the covariance corresponding to the position observation to form the position factor. The velocity residual is obtained by subtracting the velocity component of the global navigation satellite system velocity observation in the local navigation coordinate system from the velocity component of the velocity obtained by mapping through the second transformation relationship in the local navigation coordinate system. The velocity residual is then weighted based on the covariance corresponding to the velocity observation to form the velocity factor.

[0008] In an exemplary embodiment of this disclosure, the step of constructing a visible light distance factor based on the known position of the light source in the local navigation coordinate system, the visible light ranging value calculated from the visible light received signal intensity data of the light source, and the geometric relationship between the pose of the moving vehicle in the visual-inertial local world coordinate system includes: Based on the visible light received signal intensity data of the light source and the pre-calibrated visible light propagation model, the visible light ranging value is calculated inversely. Calculate the Euclidean distance between the position of the mobile carrier in the local navigation coordinate system and the known positions of each light source in the local navigation coordinate system. The difference between the Euclidean distance and the visible light ranging value is taken as the distance residual. The distance residual is weighted based on the observation variance corresponding to the visible light ranging value to form the visible light distance factor.

[0009] In an exemplary embodiment of this disclosure, after obtaining the visible light ranging value through inverse calculation, the method further includes: Using the pose maintained in the sliding window, the fixed normal vector of the photoelectric receiver in the carrier coordinate system is transformed to the local navigation coordinate system to obtain the receiver normal vector; The visible light incident angle is calculated based on the receiver normal vector and the direction vector from the moving carrier to the light source. The radiation angle is calculated based on the optical axis direction of the light source and the direction vector from the light source to the moving carrier. Substituting the visible light incident angle and the radiation angle into the visible light propagation model, the visible light ranging value is geometrically corrected.

[0010] In an exemplary embodiment of this disclosure, the method further includes: The position factor, velocity factor, and visible light distance factor within the sliding window are subjected to validity screening, and only the factors that pass the validity screening are included in the joint optimization objective function.

[0011] In an exemplary embodiment of this disclosure, the step of performing effectiveness screening on the position factor, velocity factor, and visible light distance factor within the sliding window, and including only the factors that pass the effectiveness screening in the joint optimization objective function, includes: When the position observation uncertainty of the global navigation satellite system measurement data is less than a first preset threshold, the position factor is determined to be valid; When the velocity observation uncertainty of the global navigation satellite system measurement data is less than a second preset threshold, and the velocity modulus of the mobile carrier is greater than the minimum effective velocity, the velocity factor is determined to be effective. When the relative rate of change of the signal corresponding to the visible light received signal intensity data of the light source is less than a third preset threshold, the visible light ranging value is within a preset effective distance range, and the visible light incident angle is less than the maximum allowable incident angle, the corresponding visible light distance factor is determined to be valid.

[0012] In an exemplary embodiment of this disclosure, the method further includes: When the global navigation satellite system measurement data is unavailable but the visible light received signal intensity data of the light source is available, continuous positioning is maintained by the visible light distance factor, the visual factor and the inertial factor; When the visible light received signal intensity data of the light source is unavailable but the measurement data of the global navigation satellite system is available, continuous positioning is maintained by the position factor, the velocity factor, the visual factor and the inertial factor.

[0013] According to a second aspect of this disclosure, a positioning system for a mobile carrier is provided, comprising: The data acquisition module is used to acquire multi-source observation data of the mobile carrier at various times, and to synchronize the multi-source observation data in time to form a synchronized observation set; the multi-source observation data includes visual image data, inertial measurement data, global navigation satellite system measurement data, and visible light received signal intensity data of light sources; The transformation relationship determination module is used to transform the global navigation satellite system measurement data and the known positions of each light source in the geocentric-ground-fixed coordinate system to the local navigation coordinate system using a first transformation relationship from the geocentric-ground-fixed coordinate system to the local navigation coordinate system; and to transform the pose and velocity of the moving vehicle in the visual-inertial local world coordinate system to the local navigation coordinate system using a second transformation relationship between the visual-inertial local world coordinate system and the local navigation coordinate system; the second transformation relationship is characterized by heading deviation, scale factor and three-dimensional translation deviation, wherein the heading deviation is a single-axis rotation parameter about the direction of gravity; The first factor construction module is used to construct residuals based on the position and velocity observations in the global navigation satellite system measurement data, and the corresponding components of the pose and velocity transformed by the second transformation relationship in the local navigation coordinate system, respectively. The residuals are then weighted according to the covariances of the position and velocity observations to form position factors and velocity factors. The second factor construction module is used to construct a visible light distance factor based on the known position of the light source in the local navigation coordinate system, the visible light ranging value calculated from the visible light received signal intensity data of the light source, and the geometric relationship between the pose of the moving vehicle in the visual-inertial local world coordinate system; the position factor, the velocity factor, and the visible light distance factor are used together to constrain the heading deviation, the scale factor, and the three-dimensional translation deviation in the second transformation relationship; The third factor construction module is used to construct the heading deviation smoothing factor and the scale smoothing factor based on the heading deviation difference and the scale factor difference between adjacent times within the sliding window, respectively. The objective function construction module is used to construct a joint optimization objective function within the sliding window; the joint optimization objective function includes a visual factor constructed based on the visual image data, an inertial factor constructed based on the inertial measurement data, a position factor, a velocity factor, a visible light distance factor, a heading deviation smoothing factor, and a scale smoothing factor; The positioning module is used to solve for the state estimate of the mobile vehicle by minimizing the joint optimization objective function; wherein the state estimate includes the positioning parameters of the mobile vehicle in the local navigation coordinate system; and to maintain continuous positioning output based on the visual image data and the inertial measurement data during periods when both the global navigation satellite system measurement data and the visible light received signal intensity data of the light source are unavailable.

[0014] According to a third aspect of this disclosure, a computer-readable storage medium is provided, on which a computer program is stored, which, when executed by a processor, implements the positioning method for a mobile carrier described in the first aspect.

[0015] According to a fourth aspect of this disclosure, an electronic device is provided, comprising: a processor; and a memory for storing executable instructions of the processor; wherein the processor is configured to perform the positioning method of the mobile carrier described in the first aspect by executing the executable instructions.

[0016] As can be seen from the above technical solutions, the positioning method for a mobile carrier, the positioning system for a mobile carrier, the computer-readable storage medium, and the electronic device in the exemplary embodiments of this disclosure have at least the following advantages and positive effects: In the technical solutions provided by some embodiments of this disclosure, on the one hand, by explicitly modeling the second transformation relationship between the visual inertial local world coordinate system and the local navigation coordinate system as a parameterized form including heading deviation, scale factor and three-dimensional translation deviation, and by using GNSS position / velocity observation and visible light ranging observation to jointly constrain the transformation, the cumulative drift and scale uncertainty of visual inertial odometry are effectively corrected, and the consistency and accuracy of positioning results in the global navigation coordinate system are improved. On the other hand, by jointly optimizing the visual factor, inertial factor, GNSS factor, visible light distance factor and heading / scale smoothing factor within a sliding window, the tight coupling fusion of multi-source heterogeneous observations under the same objective function is achieved, avoiding information loss and error propagation caused by step-by-step estimation in traditional loosely coupled methods. Furthermore, when GNSS and visible light observations are unavailable, the system can seamlessly retreat to a continuous motion estimation mode based on visual image data and inertial measurement data, maintaining local trajectory output and ensuring positioning continuity in complex engineering scenarios such as tunnels and underground spaces.

[0017] It should be understood that the foregoing general description and the following detailed description are exemplary and explanatory only, and are not intended to limit this disclosure. Attached Figure Description

[0018] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this disclosure and, together with the description, serve to explain the principles of this disclosure. It is obvious that the drawings described below are merely some embodiments of this disclosure, and those skilled in the art can obtain other drawings based on these drawings without any inventive effort.

[0019] Figure 1 A flowchart illustrating the positioning method of a mobile carrier in an embodiment of this disclosure is shown. Figure 2 A schematic diagram illustrating the key coordinate systems involved in the mobile carrier positioning system and their interrelationships in an embodiment of this disclosure is shown. Figure 3This illustration shows how, in an embodiment of the present disclosure, position and velocity observations from global navigation satellite system measurement data are used to construct residuals with the corresponding components of pose and velocity in the local navigation coordinate system after transformation by a second transformation relationship, and the residuals are weighted according to the covariance of the position and velocity observations to form position and velocity factors. Figure 4 A schematic diagram illustrating how the visible light distance factor is constructed in an embodiment of this disclosure is shown. Figure 5 This diagram illustrates the working principle of the visible light propagation model in an embodiment of this disclosure. Figure 6 This diagram illustrates a process for geometrically correcting visible light ranging values ​​in an embodiment of the present disclosure. Figure 7 This diagram illustrates how to construct an objective function by combining multiple factors in an embodiment of this disclosure; Figure 8 This diagram illustrates how to effectively filter the position factor, velocity factor, and visible light distance factor within a sliding window in an embodiment of this disclosure. Figure 9 This diagram illustrates the overall process of how to effectively filter the position factor, velocity factor, and visible light distance factor within a sliding window in an embodiment of this disclosure. Figure 10 This diagram illustrates the overall architecture of the positioning method for a mobile carrier in an embodiment of this disclosure. Figure 11 This illustration shows a schematic diagram of the multi-source fusion positioning operation process of a mobile carrier under different typical environments in the embodiments of this disclosure; Figure 12 A schematic diagram of the structure of the positioning system for a mobile carrier in an exemplary embodiment of this disclosure is shown; Figure 13 A schematic diagram of the structure of an electronic device in an exemplary embodiment of this disclosure is shown. Detailed Implementation

[0020] Example embodiments will now be described more fully with reference to the accompanying drawings. However, example embodiments can be implemented in many forms and should not be construed as limited to the examples set forth herein; rather, these embodiments are provided to make this disclosure more comprehensive and complete, and to fully convey the concept of the example embodiments to those skilled in the art. The described features, structures, or characteristics can be combined in any suitable manner in one or more embodiments. In the following description, numerous specific details are provided to give a full understanding of embodiments of this disclosure. However, those skilled in the art will recognize that the technical solutions of this disclosure can be practiced with one or more of the specific details omitted, or other methods, components, apparatus, steps, etc., can be employed. In other instances, well-known technical solutions are not shown or described in detail to avoid obscuring various aspects of this disclosure.

[0021] The terms “a,” “an,” “the,” and “the” are used in this specification to indicate the presence of one or more elements / components / etc.; the terms “including” and “having” are used to indicate an open-ended inclusion and to mean that there may be other elements / components / etc. in addition to the listed elements / components / etc.; the terms “first” and “second” are used only as markings and are not a limitation on the number of objects.

[0022] Furthermore, the accompanying drawings are merely illustrative of this disclosure and are not necessarily drawn to scale. The same reference numerals in the drawings denote the same or similar parts, and therefore repeated descriptions of them will be omitted. Some block diagrams shown in the drawings are functional entities and do not necessarily correspond to physically or logically independent entities.

[0023] Large underground engineering spaces such as tunnels, underground chambers, engineering corridors, underground powerhouses, integrated utility tunnels, and mine roadways are generally characterized by enclosed structures, long and narrow spaces, and severe electromagnetic interference. In these environments, Global Navigation Satellite System (GNSS) signals are often only briefly available in localized areas such as the entrance, and remain blocked for extended periods once inside, making it difficult to provide continuous and reliable absolute positioning references. Meanwhile, these scenarios typically feature continuous LED lighting along the operational path, providing unique conditions for introducing visible light-assisted positioning. When personnel, vehicles, inspection robots, quadruped robots, or construction equipment operate within these environments for extended periods, they need to obtain a unified global coordinate reference within GNSS-available areas, while simultaneously maintaining high-frequency, low-drift, and globally consistent pose output in deep regions where GNSS is completely unavailable. This poses a significant challenge to existing positioning technologies.

[0024] Positioning schemes relying solely on GNSS are highly susceptible to positioning jumps or even failures in transition sections at tunnel entrances and inside tunnels due to signal interruptions, multipath effects, and observation noise, failing to meet continuity requirements. While visual-inertial odometry (VIO) can provide high-frequency local pose estimation, it is limited by inherent heading uncertainties and scale drift problems. Without external constraints, errors accumulate with distance, making it difficult to support the global consistency requirements of long-distance engineering tasks. Although visible light positioning can provide absolute distance information using existing lighting facilities, its ranging results are easily affected by factors such as obstruction, non-line-of-sight propagation, changes in incident angle, receiver attitude disturbances, and signal attenuation over long distances. When used alone, it lacks robustness and is unsuitable as a primary positioning source.

[0025] Current mainstream multi-source fusion methods mostly employ loosely coupled switching or post-fusion strategies. This means that GNSS positioning is enabled when GNSS is available, and switched to inertial or visual-inertial modes when it fails, while visible light positioning is often treated in isolation. This fragmented architecture results in different positioning sources operating in heterogeneous coordinate systems, easily leading to trajectory discontinuities or jumps during switching. More importantly, the system lacks the ability to dynamically correct inherent errors in visual-inertial systems, especially heading deviation and scale drift. Even if a small number of valid visible light distance observations exist, they are not continuously integrated into the state estimation process to suppress long-term VIO drift, resulting in a waste of valuable absolute constraint information.

[0026] Furthermore, existing methods generally lack a unified perception and discrimination mechanism for the quality of multi-source observations. Faced with complex conditions such as GNSS signal degradation, visible light obstruction, or non-line-of-sight anomalies, the system often fails to identify and eliminate substandard observations in a timely manner, leading to the contamination of optimization results by abnormal factors and a significant decrease in positioning accuracy and stability. Therefore, it is urgent to construct a tightly coupled unified optimization framework that jointly models multi-source information such as visual, inertial, GNSS position and velocity, and visible light distance within the same sliding window, and achieves parameter co-optimization through heading and scale smoothing constraints. This will enable seamless, continuous, stable, and globally consistent robust positioning in GNSS-available areas, transition zones, and completely denied zones.

[0027] In the embodiments of this disclosure, a positioning method for a mobile carrier is first provided, which at least to some extent overcomes the shortcomings of related technologies in scenarios where GNSS signals are unavailable for a long time or only available intermittently in large underground engineering spaces, making it difficult to simultaneously achieve the establishment of a global coordinate reference and continuous, stable, low-drift pose output.

[0028] Figure 1 The flowchart of the positioning method of the mobile carrier in the present disclosure is shown. The subject executing the positioning method of the mobile carrier can be the positioning system of the mobile carrier.

[0029] refer to Figure 1 A method for locating a mobile carrier according to an embodiment of the present disclosure includes the following steps: Step S110: Acquire multi-source observation data of the mobile carrier at various times, and synchronize the multi-source observation data in time to form a synchronized observation set; the multi-source observation data includes visual image data, inertial measurement data, global navigation satellite system measurement data, and visible light received signal intensity data of light sources; Step S120: Using the first transformation relationship from the geocentric-ground-fixed coordinate system to the local navigation coordinate system, the measurement data of the global navigation satellite system and the known positions of each light source in the geocentric-ground-fixed coordinate system are transformed to the local navigation coordinate system; using the second transformation relationship between the visual-inertial local world coordinate system and the local navigation coordinate system, the pose and velocity of the moving vehicle in the visual-inertial local world coordinate system are transformed to the local navigation coordinate system; the second transformation relationship is characterized by heading deviation, scale factor and three-dimensional translation deviation, wherein the heading deviation is a single-axis rotation parameter about the direction of gravity; Step S130: Based on the position and velocity observations in the global navigation satellite system measurement data, construct residuals with the corresponding components of the pose and velocity in the local navigation coordinate system after transformation by the second transformation relationship, and weight the residuals according to the covariances of the position and velocity observations to form position factors and velocity factors. Step S140: Based on the known position of the light source in the local navigation coordinate system, the visible light ranging value calculated from the visible light received signal intensity data of the light source, and the geometric relationship between the pose of the moving vehicle in the visual-inertial local world coordinate system, a visible light distance factor is constructed; the position factor, velocity factor, and visible light distance factor are used together to constrain the heading deviation, scale factor, and three-dimensional translation deviation in the second transformation relationship; Step S150: Based on the difference in heading deviation and the difference in scale factor between adjacent times within the sliding window, construct the heading deviation smoothing factor and the scale smoothing factor respectively. Step S160: Construct a joint optimization objective function within a sliding window; the joint optimization objective function includes a visual factor constructed based on visual image data, an inertial factor constructed based on inertial measurement data, a position factor, a velocity factor, a visible light distance factor, a heading deviation smoothing factor, and a scale smoothing factor. Step S170: Solve for the state estimate of the mobile vehicle by minimizing the joint optimization objective function; wherein, the state estimate includes the positioning parameters of the mobile vehicle in the local navigation coordinate system; In step S180, during the period when both global navigation satellite system measurement data and visible light received signal intensity data are unavailable, continuous positioning output is maintained based on visual image data and inertial measurement data.

[0030] exist Figure 1 The technical solution provided in the illustrated embodiment, on the one hand, explicitly models the second transformation relationship between the visual-inertial local world coordinate system and the local navigation coordinate system as a parameterized form including heading deviation, scale factor, and three-dimensional translation deviation, and uses GNSS position / velocity observation and visible light ranging observation to jointly constrain this transformation, effectively correcting the cumulative drift and scale uncertainty of the visual-inertial odometry, and improving the consistency and accuracy of the positioning results in the global navigation coordinate system; on the other hand, by jointly optimizing the visual factor, inertial factor, GNSS factor, visible light distance factor, and heading / scale smoothing factor within a sliding window, tight coupling fusion of multi-source heterogeneous observations under the same objective function is achieved, avoiding information loss and error propagation caused by step-by-step estimation in traditional loosely coupled methods; furthermore, when GNSS and visible light observations are unavailable, the system can seamlessly retreat to a continuous motion estimation mode based on visual image data and inertial measurement data, maintaining local trajectory output and ensuring positioning continuity in complex engineering scenarios such as tunnels and underground spaces.

[0031] The following are Figure 1 The specific implementation process of each step in the process will be explained in detail: It should be noted that the mobile carrier in this disclosure refers to a mobile platform capable of autonomous or controlled movement within large enclosed or semi-enclosed engineering spaces such as tunnels, underground chambers, engineering corridors, underground factories, integrated pipe corridors, and mine roadways, and equipped with cameras, inertial measurement units (IMUs), Global Navigation Satellite System (GNSS) receivers, and visible light receivers. Typical forms may include, but are not limited to: unmanned vehicles, inspection robots, quadruped robots (such as robotic dogs), construction equipment, or intelligent terminal devices carried / beared by personnel. This mobile carrier integrates multi-source sensors to establish a global coordinate reference when GNSS signals are available, and maintains continuous and stable pose estimation based on visual, inertial, and visible light information when GNSS is limited or unavailable.

[0032] Before step S110, the various coordinate systems involved in this disclosure will first be explained. (See reference...) Figure 2 , Figure 2 This diagram illustrates the key coordinate systems and their interrelationships involved in the mobile carrier positioning system in this embodiment of the present disclosure, such as... Figure 2 As shown: Carrier coordinate system (b): Fixed on the moving carrier, with its origin located at the vehicle's center of gravity or the sensor mounting reference point. It is typically a right-handed Cartesian coordinate system, with the x-axis pointing towards the front of the vehicle, the y-axis pointing to the right, and the z-axis pointing vertically upward. This coordinate system is used to describe the carrier's own motion state.

[0033] Camera coordinate system (c): Originating at the camera's optical center, the x-axis points horizontally to the right, the y-axis points vertically downwards, and the z-axis points outwards along the optical axis. This is achieved through parameter calibration. It is associated with the IMU coordinate system to achieve spatial alignment between visual observation and inertial data.

[0034] IMU coordinate system (i): The origin is at the center of the IMU sensor; the x-axis typically points forward, the y-axis points to the right, and the z-axis points upward. This is determined through calibration parameters. A fixed transformation relationship is established with the carrier coordinate system to fuse inertial measurement data.

[0035] Visual-inertial local world coordinate system (w): A local reference frame initialized by visual-inertial odometry (VIO) and updated as the trajectory expands. The origin is usually set to the initial position of the vehicle. This coordinate system is used to represent local motion trajectories without global scale and heading deviation, and is the internal coordinate framework of the visual-inertial system.

[0036] Local navigation coordinate system (n): adopts the East-North-Up (ENU) right-handed coordinate system. The origin can be set as the initial GNSS positioning point or engineering reference point. It is used to output geographically significant position, velocity and attitude information and is the expression benchmark for the final positioning result of the system.

[0037] The Earth-Centered, Earth-Fixed (ECEF) coordinate system (t) is a coordinate system fixed at the Earth's center. Its origin is at the Earth's center, the x-axis points to the intersection of the Prime Meridian and the equator, the y-axis is perpendicular to the x-axis and lies in the equatorial plane, and the z-axis points to the North Pole. GNSS observations are obtained in this coordinate system, which serves as the reference for absolute position.

[0038] The dashed arrows in the diagram represent coordinate transformation relationships between different coordinate systems, while the solid arrows represent alignment / estimation relationships. Specifically, these include: and These are the static extrinsic parameters of the camera and IMU relative to the carrier; and The rotation matrix and translation vector from the visual inertial local world coordinate system w to the local navigation coordinate system n together constitute the second transformation relationship; scale factor Used to correct for potential scale errors in visual inertial systems, ensuring that local trajectories are consistent in length with GNSS / visible light observations.

[0039] The coordinate system definitions and transformation relationships described above constitute the basic spatial semantic model of the multi-source tightly coupled optimization framework disclosed herein. Through estimation... , and The system can unify visual inertial trajectory with GNSS position, velocity and visible light distance observations into the same local navigation coordinate system, thereby achieving high-precision, high-consistency and continuous positioning output.

[0040] In step S110, multi-source observation data of the mobile carrier at each time moment are acquired, and the multi-source observation data are time-synchronized to form a synchronized observation set; the multi-source observation data includes visual image data, inertial measurement data, global navigation satellite system measurement data, and visible light received signal intensity data of light sources.

[0041] In this step, multi-source observation data from the mobile platform at various times can be acquired, and the multi-source observation data can be synchronized in time to form a synchronized observation set. This step aims to provide a unified, structured, and time-aligned multi-source observation input for subsequent tightly coupled positioning optimization. Its core task is to collect raw data output from various sensors on the mobile platform during operation, and to form a synchronized observation set based on visual keyframes by synchronizing asynchronous observations in time.

[0042] Specifically, the aforementioned multi-source observation data can include the following five types of raw information: Visual image data : The image captured by the camera at the k-th keyframe; Inertial measurement data : Represents the inertial synchronization data associated with the k-th keyframe, in the form of , This represents the angular velocity measured by the gyroscope. This indicates the specific force or acceleration observations measured by the accelerometer, which are used to support motion pre-integration calculations between adjacent keyframes; GNSS location data and GNSS velocity data The raw measurements output by the GNSS receiver are often not perfectly aligned with the visual keyframes. GNSS observation uncertainty and : These represent the covariance of position and velocity observations, respectively, and are used for subsequent factor weighting; Visible light received signal strength data : Indicates that at the current keyframe moment, from the th keyframe... The signal strength received by each LED light source; LED light source spatial position : This refers to pre-calibrated and stored static map information, bound to the corresponding light source ID; the superscript g indicates the coordinate system to which it belongs, namely the geographic coordinate system or the engineering unified coordinate system.

[0043] The aforementioned sensors have different sampling frequencies. For example, IMUs typically operate at high frequencies of several hundred hertz, while GNSS and visible light receivers update more slowly, and cameras trigger keyframes at a fixed frame rate. Therefore, this disclosure employs a synchronization strategy based on visual keyframes: GNSS and visible light observations are aligned to the keyframe time using nearest neighbor matching or linear interpolation. The continuous measurement sequence of the IMU is used to construct the pre-integral quantity from the previous keyframe to the current keyframe. This pre-integral quantity is implicit in the inertial constraints and is associated with the current keyframe state.

[0044] Ultimately, each keyframe k corresponds to a set of synchronized observations: All elements in this set are aligned to the same time base, forming a unified input basis for subsequent coordinate transformations, factorization, and sliding window optimization.

[0045] Instead of treating various sensors as independent positioning sources and fusing them post-hoc, this design uses imagery, inertial data, GNSS velocity, GNSS position, visible light RSS, and LED spatial position as unified observation inputs for a single, continuous positioning problem. This approach enables collaborative modeling of multi-source information from the outset, providing a solid data foundation for subsequently constructing a joint optimization objective function that integrates visual factors, inertial factors, GNSS factors, visible light distance factors, and a sliding window.

[0046] This step successfully established a unified observation stream covering four types of information: visual, inertial, GNSS, and visible light. It realized the technical prerequisite of establishing a global coordinate reference in GNSS-available areas and maintaining continuous positioning with visible light assistance in GNSS-unavailable environments, significantly improving the robustness and consistency of positioning in large underground engineering spaces.

[0047] Following step S110, it should be noted that this disclosure can be based on synchronized camera image data. With inertial measurement data A visual-inertial pose estimation model (VIO model) is constructed to generate a continuous, high-frequency, and internally geometrically consistent motion trajectory. This trajectory serves as the basis for fusing absolute observations such as GNSS and visible light, and is used to maintain positioning continuity during periods when GNSS signals are unavailable or visible light observations are interrupted.

[0048] In large underground engineering spaces, GNSS signals are typically only available at the entrance area, and visible light observations may be interrupted due to obstruction or distance limitations. Without a reliable local motion model, the system cannot maintain continuous state during periods of absolute observation loss. To address this, this disclosure constructs a visual-inertial pose estimation model (VIO model) based on synchronized multi-source observation data, which outputs in real-time continuous pose and velocity estimates of the moving vehicle in the visual-inertial local world coordinate system (denoted as w).

[0049] The VIO model employs a sliding window optimization framework, achieving high-precision, low-drift state estimation by jointly minimizing visual reprojection error and IMU pre-integration constraints. The system state vector to be optimized is defined as: ,in: This represents the state of the mobile carrier corresponding to the i-th keyframe; N represents the number of keyframes within the sliding window; This represents the extrinsic parameters between the camera coordinate system c and the carrier coordinate system b; Represents the inverse depth of the j-th visual feature point (used for efficient parameterization of spatial points); M represents the number of visual feature points involved in the optimization; ψ、ξ、 These are global transformation parameters introduced in advance for subsequent alignment with the local navigation coordinate system, corresponding to: heading deviation, scale factor, and translation deviation between the visual inertial local world coordinate system w and the local navigation coordinate system n.

[0050] The state of the moving vehicle at the i-th keyframe is further refined as follows: ,in: Indicates the first The moving vehicle in the visual-inertial local world coordinate system at each keyframe The lower position; Represents the moving vehicle in the visual inertial local world coordinate system The speed of the downward movement; Represents the carrier coordinate system Relative to the visual inertial local world coordinate system The posture quaternion; This indicates the accelerometer bias (zero bias estimate). Indicates gyroscope bias; superscript Indicates accelerometer-related items, superscript This indicates items related to the gyroscope.

[0051] The extrinsic parameters between the camera and the carrier are represented as: ,in: Representing the camera coordinate system The origin is in the carrier coordinate system The lower position; Representing the camera coordinate system Relative to the carrier coordinate system The posture quaternion; During the optimization process, the system constructs two types of core residual factors: Visual factor: generated by the reprojection error of feature points extracted from the image between adjacent keyframes, used to constrain camera motion and 3D structure; Inertia factor: obtained by pre-integrating the raw IMU measurement data between adjacent keyframes, used to constrain the kinematic state (position, velocity, attitude) of the carrier and the IMU zero bias.

[0052] By jointly minimizing the nonlinear cost function composed of the aforementioned factors, this disclosure can output a smooth, continuous, and high-frequency pose and velocity sequence in real time, forming a complete visual-inertial trajectory. Although the trajectory is initially located in the visual-inertial local world coordinate system w, its internal geometry is highly consistent and already includes dynamic states such as velocity and IMU zero bias, providing a reliable motion prior and state basis for subsequent steps (such as coordinate system alignment in S120 and multi-source factor construction in S130).

[0053] In step S120, the first transformation relationship from the geocentric-ground-fixed coordinate system to the local navigation coordinate system is used to transform the global navigation satellite system measurement data and the known positions of each light source in the geocentric-ground-fixed coordinate system to the local navigation coordinate system; the second transformation relationship between the visual-inertial local world coordinate system and the local navigation coordinate system is used to transform the pose and velocity of the moving vehicle in the visual-inertial local world coordinate system to the local navigation coordinate system; the second transformation relationship is characterized by heading deviation, scale factor and three-dimensional translation deviation, wherein the heading deviation is a single-axis rotation parameter about the direction of gravity.

[0054] In this step, two key coordinate system transformation relationships can be established, and these relationships can be used to uniformly express multi-source observation data in the local navigation coordinate system: On one hand, the first transformation relation is defined as a fixed affine transformation from the geocentric-fixed coordinate system (ECEF, denoted as e) to the local navigation coordinate system (denoted as n). This transformation is based on pre-determined reference points in the target engineering space (such as known control points at the tunnel entrance), and its function is to transform absolute observations with global geographical significance (including GNSS measurement data and LED light source positions) into a local, stable engineering reference frame. Specifically, this first transformation relation includes a fixed rotation matrix. and a fixed translation vector (That is, the coordinates of the reference point under ECEF), its mathematical implementation is as follows: The GNSS position at the k-th keyframe is expressed in the local navigation coordinate system as follows: ,in, Indicates the first The GNSS position at each key frame time in the local navigation coordinate system The coordinates below; Indicates the first The GNSS position at each key frame time in the Earth-centered Earth-fixed coordinate system The coordinates below; This indicates that the selected reference point is in the Earth-centered Earth-fixed coordinate system. The coordinates below; Represents the geocentric and geofixed coordinate system To local navigation coordinate system The rotation matrix.

[0055] No. The positions of the LED light sources in the local navigation coordinate system are: ,in, Indicates the first An LED light source in the local navigation coordinate system The lower position; Indicates the first An LED light source in the Earth-centered Earth-fixed coordinate system The position below, or the coordinates after transformation from the engineering unified coordinate system to the geocentric coordinate system; the meanings of the other symbols are the same as those mentioned above.

[0056] GNSS velocity in the local navigation coordinate system is expressed as: ,in, This represents the GNSS velocity in the local navigation coordinate system at the k-th keyframe. This represents the expression of GNSS velocity in a geocentric coordinate system. This represents the rotation matrix from the geocentric coordinate system to the local navigation coordinate system.

[0057] In the above transformation and The transformation remains unchanged during system operation, therefore the first transformation relationship is a static, one-time calibrated global-local mapping.

[0058] On the other hand, the second transformation relation is defined as a parametric affine transformation from the visual-inertial local world coordinate system (denoted as w) to the local navigation coordinate system n. Unlike the first transformation, this relation is not fixed, but rather structurally constrained based on the observability characteristics of the visual-inertial odometry: since the IMU can provide high-precision gravity orientation, the pitch and roll angles are effectively constrained, leaving only the heading degree of freedom about the gravity orientation (i.e., the Z-axis of the local navigation frame) uncertain. Therefore, this disclosure explicitly characterizes the second transformation relation as the following three parameters to be estimated: heading deviation : Represents the single-axis rotational parameter about the direction of gravity; scale factor Used to correct scale drift in VIO trajectories; 3D translation deviation : Indicates the position offset of the origin of the visual inertial local world coordinate system in the local navigation coordinate system.

[0059] Based on this parameterized model, the state of the mobile vehicle at the k-th keyframe is expressed in the local navigation coordinate system as follows: ,in, and These represent the mobile carrier at the [number]th [year]. Each keyframe moment is in the visual inertial local world coordinate system. The lower position; and Due to heading deviation The only certainty:

[0060] It should be noted that in the second transformation relationship , and Instead of being a fixed value, it is incorporated as a state variable into the sliding window optimization process and estimated online along with the carrier pose, velocity, and IMU bias. This design enables the system to dynamically adapt to the initial randomness of the visual-inertial local world coordinate system, changes in GNSS conditions at the tunnel entrance, and scale drift during long-distance operation.

[0061] In summary, the first transformation relation provides a stable global-local mapping, while the second transformation relation provides an adaptively correctable local VIO-local navigation alignment mechanism. Together, they form the geometric basis for fusing multi-source heterogeneous observations within the same coordinate frame.

[0062] In step S130, based on the position and velocity observations in the global navigation satellite system measurement data, residuals are constructed with the corresponding components of the pose and velocity in the local navigation coordinate system after transformation by the second transformation relationship. The residuals are then weighted according to the covariances of the position and velocity observations to form position factors and velocity factors.

[0063] In this step, residuals can be constructed based on the position and velocity observations in the global navigation satellite system measurement data, respectively, and the corresponding components of the pose and velocity in the local navigation coordinate system after transformation by the second transformation relationship. The residuals are then weighted according to the covariances of the position and velocity observations to form position factors (also known as GNSS position factors) and velocity factors (also known as GNSS velocity factors).

[0064] For details, please refer to Figure 3 , Figure 3 This embodiment of the present disclosure illustrates how, based on position and velocity observations from Global Navigation Satellite System measurement data, residuals are constructed with the corresponding components of pose and velocity in the local navigation coordinate system after transformation by a second transformation relationship, and the residuals are weighted according to the covariance of the position and velocity observations to form position and velocity factors. The flowchart includes steps S301-S302. In step S301, the coordinates of the global navigation satellite system position observations in the local navigation coordinate system are subtracted from the position components of the pose obtained by mapping through the second transformation relationship in the local navigation coordinate system to obtain the position residual. The position residual is then weighted based on the covariance corresponding to the position observations to form the position factor.

[0065] In this step, when the mobile vehicle is in a GNSS-available area, a transition section at the entrance of a tunnel, or a region where GNSS is intermittently available, and the GNSS position observation meets the quality requirements, the system uses this observation to perform global position correction on the visual-inertial trajectory. Specifically, the original GNSS position observation is first... Transform to the local navigation coordinate system n using the first transformation relationship to obtain... Simultaneously, the position of the moving vehicle estimated by visual inertia in the visual inertial local world coordinate system w is determined. The coordinates are mapped to the same local navigation coordinate system through the second transformation relationship.

[0066] The second transformation relationship is determined by the scaling factor. Rotation matrix Translational deviation The common representation serves to transform the state in the visual-inertial local world coordinate system to the local navigation coordinate system. Therefore, the transformed position is expressed as: The difference between the two constitutes the residual of the GNSS position factor, defined as: ,in: Indicates the first GNSS position residuals at each key frame; This indicates that the GNSS position observations have been transformed to the local navigation coordinate system using the first transformation relation. The coordinates after; Indicates the scale factor; Represents the visual inertial local world coordinate system To local navigation coordinate system rotation matrix; This represents the position of the moving vehicle in the visual-inertial local world coordinate system w, as estimated by visual inertialism. This represents the translational deviation of the origin of the visual inertial local world coordinate system in the local navigation coordinate system n.

[0067] To reflect the reliability of GNSS position observations, this residual is based on its corresponding covariance matrix. Weighting is performed to form position factors: ,in: This is the activation variable for the location factor, set to 1 when GNSS location observations are reliable, and 0 otherwise.

[0068] This position factor allows the system to constrain the global position offset, scale error, and translation deviation of the visual-inertial trajectory online. In particular, it can quickly correct the inconsistency between the visual-inertial local world coordinate system and the global navigation reference in the early stage of GNSS signal recovery, providing high-precision and high-consistency absolute position priors for subsequent multi-source fusion.

[0069] In step S302, the velocity of the global navigation satellite system velocity observation value in the local navigation coordinate system is subtracted from the velocity component of the velocity obtained by mapping through the second transformation relationship in the local navigation coordinate system to obtain the velocity residual. The velocity residual is then weighted based on the covariance corresponding to the velocity observation value to form a velocity factor.

[0070] In this step, when the mobile vehicle is in a GNSS-available area, a transition section at the entrance of a tunnel, or a region where GNSS is intermittently available, and the GNSS velocity observation meets the quality requirements, the system uses this observation to constrain the dynamic state of the visual-inertial trajectory, improving kinematic consistency and heading estimation accuracy. Specifically, the raw GNSS velocity observation is first... Transform to the local navigation coordinate system n using the first transformation relationship to obtain... Simultaneously, the velocity of the moving vehicle estimated by visual inertia in the visual inertial local world coordinate system w is... The rotation component in the second transformation relation is mapped to the same local navigation coordinate system.

[0071] The rotation matrix in the second transformation relation Determined by the heading deviation ψ, it takes the form of a single-axis rotation about the direction of gravity, used to align the attitude between the visual-inertial local world coordinate system and the local navigation coordinate system. Since velocity is the first derivative of position, and the second transformation relation only includes attitude rotation (excluding scale factor), the transformed velocity is expressed as: ; The difference between the two constitutes the residual of the GNSS velocity factor, defined as: ,in: Indicates the first GNSS velocity residuals at each key frame; This indicates that the GNSS observation velocity has been transformed to the local navigation coordinate system using the first transformation relation. The speed after; The moving vehicle representing the visual-inertial estimation in the visual-inertial local world coordinate system The speed of the downward movement; Visual inertial local world coordinate system To local navigation coordinate system The rotation matrix is ​​determined by the heading deviation ψ, which is a single-axis rotation about the direction of gravity.

[0072] To reflect the reliability of GNSS velocity observations, this residual is based on its corresponding covariance matrix. Weighting is performed to form the objective function term for the velocity factor: ,in: This is the activation variable for the position factor, set to 1 when GNSS velocity observations are reliable, and 0 otherwise.

[0073] This velocity factor allows the system to constrain the velocity consistency of the visual-inertial trajectory online and utilizes GNSS velocity information for accurate estimation of the heading deviation ψ. Due to the rotation matrix... The ψ factor is determined solely by the heading deviation, while the velocity residual is directly sensitive to ψ. This factor is particularly effective in scenarios with weak GNSS signals or low-speed motion, and can significantly suppress heading drift, thereby improving the system's directional robustness in complex engineering spaces.

[0074] In one exemplary implementation, the aforementioned GNSS position factor and GNSS velocity factor together constitute the GNSS observation objective function term at the k-th key frame time: ,in: Indicates the first The objective function term of GNSS-related factors at each key frame time; This represents the activation variable for the GNSS position factor, which is set to 1 when the GNSS position observation is reliable and 0 otherwise. This represents the activation variable for the GNSS velocity factor, which is set to 1 when GNSS velocity observations are reliable, and 0 otherwise. Represents the covariance matrix of GNSS position observations; Represents the covariance matrix of GNSS velocity observations; and These are the Mahalanobis weighted norms, used to measure the magnitude of observation error relative to its uncertainty.

[0075] Through this joint factor structure, this disclosure achieves synergistic constraints on heading deviation, scale factor, and three-dimensional translation deviation in the second transformation relationship: the position factor mainly corrects for scale and translation, while the velocity factor mainly improves the heading estimation accuracy. This dual-action mechanism enables the system to quickly establish a globally consistent navigation reference in areas where GNSS signals are available, and maintains highly robust and continuous positioning performance in subsequent GNSS-rejected areas, making it particularly suitable for long-distance, weakly observable scenarios in large underground engineering spaces.

[0076] In step S140, a visible light distance factor is constructed based on the known position of the light source in the local navigation coordinate system, the visible light ranging value calculated from the visible light received signal intensity data of the light source, and the geometric relationship between the pose of the moving vehicle in the visual inertial local world coordinate system. The position factor, velocity factor and visible light distance factor are used together to constrain the heading deviation, scale factor and three-dimensional translation deviation in the second transformation relationship.

[0077] In this step, a visible light distance factor can be constructed based on the known position of the light source in the local navigation coordinate system, the visible light ranging value calculated from the visible light received signal intensity data of the light source, and the geometric relationship between the pose of the moving vehicle in the visual inertial local world coordinate system.

[0078] For details, please refer to Figure 4 , Figure 4This diagram illustrates the process of constructing the visible light distance factor in this embodiment of the present disclosure, including steps S401-S404: In step S401, the visible light ranging value is calculated by back-calculating the visible light received signal intensity data of the light source and the pre-calibrated visible light propagation model.

[0079] In this step, the photodetector on the mobile carrier collects visible light signal intensity data from each LED light source. This data reflects the attenuation characteristics of the visible light signal during propagation. To extract distance information, a pre-calibrated visible light propagation model is used for inverse calculation. This model comprehensively considers the radiation characteristics of the light source, the optical characteristics of the receiver, and geometric relationships; its expression can be: ,in: Indicates the first The LED light source is at the first The received signal strength at each key frame moment; Indicates the first The optical power calibration coefficients for each LED light source and receiver combination can be obtained through laboratory calibration. Indicates the first Lambert order of each LED light source; Indicates the first The radiation angle between each LED light source and the receiver; This indicates the Lambert order of the photodetector or the order of the receiver response; Indicates the first The first keyframe moment The visible light incidence angle corresponding to each LED light source; Represents the gain function of an optical filter; Represents the gain function of an optical condenser; Indicates the first The receiver at the first key frame moment and the first The spatial distance between each LED light source.

[0080] By transforming the above model, we can obtain the visible light ranging value calculated from the received signal strength: ,in, This indicates the mobile carrier calculated from the received signal strength and the... The estimated distance between the LED light sources is denoted as the "visible light ranging value". This value reflects the non-contact ranging result based on signal strength and is the basic input for subsequently constructing the visible light distance factor.

[0081] refer to Figure 5 , Figure 5 This diagram illustrates the working principle of the visible light propagation model in an embodiment of this disclosure, as shown below. Figure 5 As shown: In this schematic diagram, a photoelectric receiver is mounted on the mobile carrier to receive visible light signals from multiple LED light sources in the environment. Each LED light source has a pre-calibrated, known three-dimensional position and a vertical optical axis reference direction in space, and its radiated light intensity attenuation characteristics are fully described by the Lambert propagation model; a fixed unit normal vector exists on the surface of the photoelectric receiver. The orientation of the receiving plane is used to characterize the orientation of the visible light incident light intensity and is a core geometric parameter for calculating the intensity of visible light incident light.

[0082] The key parameters in the diagram are defined as follows: Height difference h: Represents the vertical height difference between the LED light-emitting end mounting plane and the photoelectric receiving end mounting plane. It is a fixed calibration parameter in engineering scenarios and is the basic geometric constant for calculating spatial propagation distance. Light vector : Represents the spatial vector pointing from the LED light-emitting end to the photoelectric receiver end, completely characterizing the propagation path of visible light. It is obtained by solving the coordinates of the LED light-emitting end and the photoelectric receiver end in the local navigation coordinate system; it corresponds to the visible light incident direction mentioned above. Radiation angle : Represents the vector of light emitted from the LED's light-emitting end and directed towards the photoelectric receiver. The angle between the LED and the vertical optical axis reference (vertical dashed line) reflects the directional emission characteristics of the LED light source. The larger this irradiance angle value, the faster the light intensity of the LED emitted attenuates; Angle of incidence : Represents the vector of light rays from the LED pointing towards the photodetector. Normal vector of the receiving end plane The angle between the incident angle and the incident angle directly determines the visible light reception efficiency of the photoelectric receiver. When the angle of incidence approaches 0 degrees, the received light intensity reaches its maximum value; as the incident angle increases, the received light intensity decreases according to the Lambert cosine law of cosM(ψ). Receiver plane normal vector : is a unit vector perpendicular to the mounting plane of the photoelectric receiver. Its fixed direction in the carrier coordinate system is pre-calibrated by the hardware mounting attitude. During the movement of the moving carrier, the normal vector itself has no displacement or rotation relative to the carrier, but it needs to rely on the carrier pose optimized within the sliding window, and be mapped to the local navigation coordinate system through coordinate transformation to complete the dynamic correction calculation of the irradiance angle and the incident angle.

[0083] In one exemplary embodiment, after obtaining the visible light ranging value through back-calculation, this disclosure can further perform geometric correction on the aforementioned visible light ranging value, thereby improving its measurement accuracy and robustness in complex dynamic environments. Specifically, refer to... Figure 6 , Figure 6 This diagram illustrates a process for geometrically correcting visible light ranging values ​​in an embodiment of the present disclosure, including steps S601-S604: In step S601, the fixed normal vector of the photoelectric receiver in the carrier coordinate system is transformed to the local navigation coordinate system using the pose maintained in the sliding window to obtain the receiver normal vector.

[0084] In this step, the photodetector has a fixed installation orientation in the carrier coordinate system b, and its surface normal vector is denoted as... Here, the superscript b represents the carrier coordinate system, and the subscript R represents the photoelectric receiver. This normal vector rotates as the attitude of the moving carrier changes. To obtain its orientation in the local navigation coordinate system, the pose information of the moving carrier maintained in the sliding window needs to be utilized. Specifically, the rotation matrix from the carrier coordinate system b to the local navigation coordinate system n at the k-th keyframe time is obtained using the pose output by the visual inertial odometry. It consists of two parts: ,in Let be the rotation matrix from the visual-inertial local world coordinate system w to the local navigation coordinate system n. This represents the rotation matrix from the carrier coordinate system b obtained from the visual inertial attitude to the visual inertial local world coordinate system w.

[0085] The above rotation matrix Applying this to the original normal vector yields the receiver normal vector of the photoelectric receiver in the local navigation coordinate system at the k-th keyframe moment: This vector can be used for subsequent incident angle calculations.

[0086] In step S602, the visible light incident angle is calculated based on the receiver normal vector and the direction vector from the moving carrier to the light source.

[0087] In this step, the first step is to determine the direction from the moving carrier to the first... The direction vector of each LED light source. When the position of the moving vehicle in the local navigation coordinate system is... , No. The known positions of the LED light sources in the local navigation coordinate system are: Then the direction vector from the moving vehicle to the light source is Since the actual direction of light propagation is from the light source to the photodetector, the incident direction is... Visible light incident angle Defined as the incident direction and the receiver normal vector The included angle between them satisfies: ,in, The magnitude of the receiver normal vector can be used to solve for the visible light incidence angle at the current moment. .

[0088] In step S603, the radiation angle is calculated based on the optical axis direction of the light source and the direction vector from the light source to the moving carrier.

[0089] In this step, the first Each LED light source has a pre-calibrated optical axis direction unit vector in the local navigation coordinate system. The direction vector from the light source to the moving vehicle is... (Same as above). Radiation angle Defined as the angle between the optical axis direction of the light source and the aforementioned pointing vector, which satisfies: ,in, Indicates the first An LED light source in the local navigation coordinate system The optical axis direction vector below; The magnitude of the LED optical axis direction vector is represented by this angle, which reflects the influence of the non-Lambertian radiation characteristics of the LED light source on the signal intensity and is a key parameter in the visible light propagation model.

[0090] In step S604, the visible light incident angle and radiation angle are substituted into the visible light propagation model to perform geometric correction on the visible light ranging value.

[0091] In this step, the visible light incident angle calculated above can be used... and radiation angle Substitute into the pre-calibrated visible light propagation model: ,in The data on the intensity of the visible light received signal from the light source collected by the photodetector. For calibration coefficients, Let M be the Lambert order of the LED light source, and M be the Lambert order of the photodetector. This is the gain function of the optical filter. This is the gain function of the optical condenser.

[0092] When calculating visible light ranging values, if we ignore... and Dynamic changes in these angles (e.g., assuming they are always zero) will lead to significant ranging errors. This embodiment calculates these two angles in real time and substitutes them into the model to geometrically correct the initially back-calculated ranging value, thereby obtaining a more accurate visible light ranging value. This correction mechanism eliminates the need for the system to maintain a horizontal attitude relying on the photoelectric receiver, making it suitable for high-precision positioning of mobile carriers under complex dynamic conditions such as slopes, turns, or vibrations.

[0093] After obtaining the visible light ranging value or the corrected visible light ranging value, you can then refer to... Figure 4 In step S402, the Euclidean distance between the moving carrier and each light source is calculated based on their positions in the local navigation coordinate system.

[0094] In this step, after completing the coordinate transformation between GNSS measurement data and visual-inertial state estimation, the position of the mobile vehicle in the local navigation coordinate system is known, denoted as . Simultaneously, the known positions of each LED light source in the Earth-centered, Earth-fixed coordinate system have been transformed to the same local navigation coordinate system through the first transformation relationship, denoted as... .

[0095] Therefore, the mobile carrier and the first The Euclidean distance between individual LED light sources can be calculated using the following formula: This distance represents the geometric distance estimated based on the current pose of the mobile vehicle, and is used to compare it with the visible light ranging value.

[0096] In step S403, the difference between the Euclidean distance and the visible light ranging value is used as the distance residual.

[0097] In this step, the geometric Euclidean distance calculated in the previous step can be compared with the visible light ranging value calculated in S301 to construct a distance residual term, which is used to measure the consistency between the two.

[0098] Specifically, the k-th keyframe moment, the... The visible light distance residual corresponding to each LED light source is defined as: ,in: in, Indicates the first The first keyframe moment The visible light distance residual (scalar distance residual) corresponding to each LED light source reflects the degree of consistency between the current pose estimation of the mobile carrier and the visible light observation received by the photoelectric receiver. When the residual is large, it indicates that there is a deviation in the pose estimation, and the accuracy needs to be improved by optimizing and adjusting the second transformation relationship parameters. Indicates the mobile vehicle in the local navigation coordinate system The lower position; Indicates the first An LED light source in the local navigation coordinate system The lower position; It is the Euclidean distance between the moving carrier and the LED light source; This represents the visible light ranging value calculated from the received signal strength.

[0099] In step S404, the distance residual is weighted based on the observation variance corresponding to the visible light ranging value to form a visible light distance factor.

[0100] In this step, due to the visible light ranging value It is calculated from the received visible light signal intensity data of the light source, and its accuracy is affected by factors such as signal noise and ambient light interference, thus exhibiting uncertainty. Therefore, observation variance can be introduced. The distance residuals are weighted to reflect the reliability of different observations.

[0101] Specifically, the distance residual can be... Combined with the observation variance, construct the objective function term for the visible light distance factor: ,in: Indicates the first The objective function term for the visible light distance factor at each key frame moment; Indicates the first The set of LED light sources that can be observed by the receiver at each key frame moment; Indicates the first Each LED light source corresponds to an activation variable for the visible light distance factor. The value is 1 when the LED observation is reliable, and 0 otherwise. Indicates the first Variance of visible light ranging for each LED light source; This represents the reciprocal of the distance measurement variance, which is used as a scalar weight.

[0102] The visible light distance factor participates in the joint optimization objective function in the form of the square of the distance residual multiplied by the weights, directly constraining the visual-inertial trajectory. By minimizing this visible light distance factor, this disclosure can suppress the drift of the visual-inertial system using ranging information from a small number of LED light sources when GNSS is unavailable, thereby improving the positioning continuity and robustness in GNSS-constrained scenarios such as tunnels and underground spaces.

[0103] In step S150, a heading deviation smoothing factor and a scale smoothing factor are constructed based on the heading deviation difference and scale factor difference between adjacent times within the sliding window.

[0104] In this step, to avoid unreasonable abrupt changes in heading deviation and scale factor due to observation noise or brief loss of lock during GNSS signal interruption or transition, thereby affecting the stability of the overall positioning trajectory, this disclosure introduces a heading deviation smoothing factor and a scale smoothing factor in the sliding window optimization framework to constrain the temporal continuity of these two key parameters.

[0105] Specifically, based on the state estimation results between consecutive keyframes within the sliding window, the difference in heading deviation and the difference in scale factor at adjacent time points are calculated and incorporated as residual terms into the joint optimization objective function.

[0106] (1) Construction of heading deviation smoothing factor

[0107] Let the estimated heading deviation at the k-th keyframe be... The estimated heading deviation at the (k-1)th keyframe is: The time interval between the two is The heading smoothing residual at the k-th keyframe is defined as: ,in: Indicates the first Heading smoothing residuals at each keyframe moment; Indicates the first The estimated heading deviation at each key frame time; Indicates the first The estimated heading deviation at each key frame time; Indicates the first The key frame time and the first The time interval between keyframe moments. This residual reflects the trend of heading deviation over time. If its value is too large, it indicates that there are drastic fluctuations in heading deviation, which may be caused by abnormal GNSS jumps.

[0108] (2) Construction of scale smoothing factor

[0109] Let the scale factor estimated at the k-th keyframe time be... The scale factor estimated at the (k-1)th keyframe time is The scale-smoothing residual at the k-th keyframe is defined as: ,in: Indicates the first Scale-smoothed residuals at each keyframe moment; Indicates the first The scale factor estimated at each keyframe moment; Indicates the first The scale factor estimated at each keyframe moment; This represents the time interval between adjacent keyframe moments. This residual is used to suppress non-physical abrupt changes in the scale factor, especially when GNSS signals are intermittently available or blocked, to prevent the visual-inertial system from accumulating errors due to local scale drift.

[0110] In one exemplary implementation, the two smoothed residuals can also be combined into a single objective function term to constrain the optimization process: ,in: Let $\mathbf{k}$ represent the objective function term for the smoothing constraint at the $k$-th keyframe. and These are the weighting coefficients for the heading smoothing factor and the scale smoothing factor, respectively, which can be set based on prior knowledge of the system, such as dynamically adjusted based on GNSS measurement accuracy.

[0111] The smoothing factor is minimized during the optimization process, thereby making the changes in heading deviation and scale factor more gradual and avoiding overall trajectory jitter caused by instantaneous anomalies in GNSS.

[0112] Next, refer to Figure 1 In step S160, a joint optimization objective function is constructed within a sliding window. The joint optimization objective function includes a visual factor constructed based on visual image data, an inertial factor constructed based on inertial measurement data, a position factor, a velocity factor, a visible light distance factor, a heading deviation smoothing factor, and a scale smoothing factor.

[0113] In this step, a joint optimization objective function can be constructed within a sliding window. The joint optimization objective function includes a visual factor constructed based on visual image data, an inertial factor constructed based on inertial measurement data, a position factor, a velocity factor, a visible light distance factor, a heading deviation smoothing factor, and a scale smoothing factor.

[0114] Next, the specific implementation method for constructing the joint optimization objective function within a sliding window in this disclosure will be described: Within the same sliding window (formed at multiple keyframe moments) optimization framework, this disclosure jointly models the inertial factor, visual factor, GNSS position factor, GNSS velocity factor, heading smoothing factor, scale smoothing factor, and visible light distance factor to construct a unified, tightly coupled optimization objective function. This objective function achieves high-precision and robust estimation of the mobile vehicle's state by minimizing the weighted sum of squares of the multi-source observation residuals.

[0115] The joint optimization problem can be expressed as: ,in: This represents the optimal system state obtained after optimization; The system state to be optimized includes the pose, velocity, IMU deviation, visual feature point position of the mobile vehicle in the local navigation coordinate system, as well as the heading deviation, scale factor and three-dimensional translation deviation in the second transformation relationship. Represent the overall objective function; This represents the value of the variable that minimizes the objective function.

[0116] The overall objective function is defined as: ,in: This represents the prior terms obtained by marginalizing the sliding window, used to preserve historical states; Indicates the first The objective function term of the inertial factor corresponding to each key frame moment (for example, the construction process of the objective function term of the inertial factor may include: pre-integrating the measurement data of the IMU between two consecutive key frames to obtain the predicted values ​​of relative pose, velocity and attitude; comparing the predicted value with the corresponding quantity calculated from the current state variable to form the inertial residual; and then combining it with the covariance matrix determined by the IMU noise model to construct the weighted quadratic objective function term). Indicates the first The visual reprojection factor objective function term corresponding to each keyframe moment (for example, the visual factor objective function term can be constructed based on the reprojection error of feature points. Specifically, for spatial feature points observed by multiple keyframes within the sliding window, they are projected onto the image plane according to the currently estimated camera pose and camera intrinsic parameter model to obtain predicted pixel coordinates; the difference between the predicted coordinates and the actual detected feature point pixel coordinates is taken to form the reprojection residual; the visual factor objective function term is the weighted sum of squares of all effective reprojection residuals, and the weights are determined by the image observation noise covariance). This represents the sum of the two objective function terms: the GNSS position factor and the GNSS velocity factor. This represents the objective function term for the visible light distance factor; This represents the sum of the objective function terms for the heading smoothing factor and the scale smoothing factor; The summation symbol represents summing over all relevant keyframe moments or LED light sources within the sliding window.

[0117] Furthermore, each component of the objective function is expressed as follows: Priors ,in, This represents the prior residual, which is the difference between the historical state and the new state retained after the current window is marginalized; This represents the prior covariance matrix, reflecting the uncertainty of historical information. Inertia factor ,in, This represents the inertial pre-integration residual, calculated from IMU measurement data; This represents the inertial pre-integration covariance matrix, used to weight inertial constraints at different time intervals; Visual factor ,in, Indicates visual reprojection residual; Represents the visual observation covariance matrix; GNSS factor terms ,in, Indicates GNSS position residual; Indicates GNSS velocity residual; Represents the GNSS position covariance matrix; Represents the GNSS velocity covariance matrix; , These represent the activation variables for the GNSS position factor and velocity factor, respectively, used to dynamically control whether they participate in the optimization.

[0118] Visible light distance factor ,in, Represents the k-th keyframe moment. Visible light distance residual corresponding to each LED light source; This represents the variance of visible light ranging; This is the activation variable for the visible light distance factor, used to determine whether the observation is valid.

[0119] Smoothing factor ,in, Indicates the heading smoothing residual; Represents scale-smoothed residuals; Indicates the heading smoothing weight; This represents the scale smoothing weight, which is used to constrain the temporal continuity of heading deviation and scale factor to avoid anomalous jumps.

[0120] This disclosure unifies GNSS, visible light, and visual inertial systems into the same back-end optimization model, and dynamically controls whether each factor participates in the optimization by using observation quality activation variables, thus realizing an integrated tightly coupled positioning mechanism of "continuous motion estimation - absolute constraint correction - abnormal observation elimination".

[0121] refer to Figure 7 , Figure 7 This illustration shows how to construct an objective function by combining multiple factors in an embodiment of this disclosure, such as... Figure 7 As shown: The diagram uses keyframes (represented by elliptical nodes) as the core state variables. Each keyframe contains state variables such as the pose, velocity, IMU deviation, and heading deviation, scale factor, and translation deviation of the moving vehicle in the local navigation coordinate system, as well as in the second transformation relationship. Multiple keyframes are arranged in chronological order to form a sliding window, which serves as the basic unit for optimization.

[0122] Based on this, the system jointly introduces the following observation factors: The inertia factor connects adjacent keyframes and is constructed from the IMU pre-integrated residuals; Visual factors connect keyframes with their observed visual feature points, based on reprojection error; The GNSS position factor and GNSS velocity factor associate keyframes with the corresponding GNSS position and velocity observations, respectively. The visible light distance factor correlates keyframes with distance observations of LED light sources in the environment; The heading smoothing factor and scale smoothing factor act on the heading deviation and scale factor between keyframes, constraining their temporal changes.

[0123] All of the above factors together constitute a unified joint optimization objective function, which optimizes the keyframe state as a whole within a sliding window, achieving tight coupling and fusion of multi-source information from vision, inertia, GNSS and visible light.

[0124] In one exemplary embodiment, the present disclosure may further perform validity screening on the position factor, velocity factor and visible light distance factor within the sliding window, and only include the factors that pass the validity screening into the joint optimization objective function.

[0125] Specifically, you can refer to Figure 8 , Figure 8 This illustration shows a flowchart of how to effectively filter the position factor, velocity factor, and visible light distance factor within a sliding window in an embodiment of this disclosure, including steps S801-S803: In step S801, when the position observation uncertainty of the global navigation satellite system measurement data is less than a first preset threshold, the position factor is determined to be valid.

[0126] In this step, to improve the system's robustness in tunnels, portal transition sections, and underground engineering spaces, the quality of GNSS position observations needs to be dynamically evaluated. Specifically, this is achieved by analyzing the position observation uncertainties in the Global Navigation Satellite System measurement data. Determine whether the preset conditions are met and whether it can be used to construct a position factor.

[0127] The activation variable for the GNSS location factor is defined as follows: ,in: The activation variable represents the GNSS position factor at the k-th keyframe time. This represents the uncertainty of the GNSS position observation at the k-th keyframe (usually output by the GNSS receiver). This represents the threshold for uncertainty in GNSS position observations, which can be set according to the actual application scenario.

[0128] when When the value is 1, it indicates that the GNSS position observation quality at that moment is reliable, and its corresponding position factor can be included in the joint optimization objective function; otherwise, the factor is set to zero to avoid errors introduced by low-quality observations.

[0129] In step S802, when the velocity observation uncertainty of the global navigation satellite system measurement data is less than the second preset threshold and the velocity modulus of the mobile carrier is greater than the minimum effective velocity, the velocity factor is determined to be effective.

[0130] In this step, GNSS velocity observations also exhibit uncertainties, and their reliability decreases significantly at low speeds or when stationary. Therefore, it is necessary to consider both the uncertainty of velocity observations and the motion state of the moving vehicle.

[0131] The activation variable for the GNSS velocity factor is defined as: ,in: The activation variable represents the GNSS velocity factor at the k-th keyframe. Indicates the uncertainty of GNSS velocity observation; This represents the threshold for uncertainty in GNSS velocity observations. This represents the magnitude of the GNSS velocity in the local navigation coordinate system. This represents the minimum effective speed threshold set by the carrier, used to exclude stationary or low-speed states.

[0132] When the speed of the mobile vehicle is too low, the uncertainty in the GNSS velocity direction increases significantly, making it difficult to provide reliable heading constraints. Therefore, this disclosure uses a dual criterion: uncertainty meets the standard + speed is sufficiently high, to determine whether to activate the velocity factor.

[0133] In step S803, when the relative rate of change of the signal corresponding to the visible light received signal intensity data of the light source is less than the third preset threshold, the visible light ranging value is within the preset effective distance range, and the visible light incident angle is less than the maximum allowable incident angle, the corresponding visible light distance factor is determined to be valid.

[0134] In this step, to ensure the reliability of the visible light ranging factor, a comprehensive evaluation of signal stability, geometric feasibility, and optical rationality is required. Specifically, the activation of the visible light ranging factor can be determined by combining the following three conditions.

[0135] First, define the relative change rate of the visible light signal as: ,in: Indicates the current time. The received signal strength of each LED light source; This indicates the received signal strength at the previous keyframe. To prevent positive numbers with a denominator of zero; It reflects the instantaneous fluctuation of the signal. If it is too large, it may be caused by multipath effect, ambient light interference or blockage.

[0136] Secondly, the activation variable for the visible light distance factor is defined as: ,in: Represents the k-th keyframe moment. Each LED light source corresponds to an activation variable for the visible light distance factor; This represents the threshold representing the relative rate of change of the received signal strength. and These represent the upper and lower limits of the effective ranging range of visible light, respectively. This represents the visible light ranging value (i.e., the estimated distance) obtained by inverse calculation from the received signal strength. Indicates the angle of incidence of visible light; This indicates the upper limit of the receiver's field of view or effective angle of incidence.

[0137] When all three conditions are met, the visible light distance factor is deemed valid and included in the optimization process; otherwise, it is set to zero to avoid incorrect constraints caused by signal abrupt changes, excessive distance, or excessive incident angle.

[0138] By dynamically managing the GNSS position factor, GNSS velocity factor, and visible light factor in the factor map based on observation quality as the trigger condition, the system can adaptively activate or suppress the corresponding observation factors under various complex scenarios such as GNSS availability, GNSS degradation, visible light obstruction, LED sparseness, and short-term absence of absolute observations. This avoids the trajectory jump or state discontinuity problems caused by hard switching of positioning modes in traditional methods, and achieves smooth transition and highly robust positioning of multi-source observation fusion.

[0139] refer to Figure 9 , Figure 9 This illustration shows the overall process of how to effectively filter the position factor, velocity factor, and visible light distance factor within a sliding window in an embodiment of this disclosure, as follows: Figure 9 As shown: like Figure 9 As shown, the process is divided into two parallel branches: the left side is the GNSS observation processing process, and the right side is the visible light observation processing process, which are used to determine whether to retain or remove the corresponding factors.

[0140] In the GNSS observation processing workflow, the system first inputs GNSS measurement data, including position, velocity and its uncertainty information.

[0141] First, determine whether the position uncertainty is less than or equal to the preset position accuracy threshold. If satisfied, proceed to the next step; otherwise, skip directly to "Remove GNSS position factor or velocity factor". Next, it is determined whether the speed uncertainty is less than or equal to the preset speed accuracy threshold. If the condition is met, proceed to the next step; otherwise, proceed to "Remove GNSS position factor or velocity factor". Finally, determine whether the current velocity modulus is greater than or equal to the preset minimum motion speed threshold. If satisfied, retain the GNSS position factor and velocity factor; otherwise, remove the GNSS position factor or velocity factor.

[0142] The above three steps constitute a comprehensive assessment of the GNSS observation quality: factors are retained for optimization only when the position and velocity are sufficiently reliable and the vehicle is in an effective motion state; otherwise, they are eliminated to prevent low-quality GNSS observations from introducing errors.

[0143] In the visible light observation and processing workflow, the system inputs the visible light received signal intensity (RSS signal intensity data) of multiple LED light sources.

[0144] First, signal mutation detection is performed to determine the amplitude of RSS change. Is it greater than the preset mutation threshold? If the threshold is exceeded, the signal is considered abnormal and the process proceeds to the "Dynamic Factor Removal" module. If no sudden change occurs, then proceed with non-line-of-sight / occlusion discrimination: determine whether the RSS signal strength data is less than the preset weak signal threshold. If the value is lower than the threshold, it is considered that there is non-line-of-sight propagation or occlusion, and the process will proceed to the "Dynamic Factor Removal" module. If it is not determined to be non-line-of-sight or obstructed, then distance measurement filtering is performed: determine whether the calculated distance d is within the preset valid range. If the factor is not within the specified range, proceed to the "Dynamic Factor Removal" module. If the distance is valid, further incident angle filtering is performed: the calculated incident angle is evaluated. Is it within the allowed range? If the condition is met, the LED distance factor is retained; otherwise, the "Factor Dynamic Removal" module is entered.

[0145] Ultimately, all observations that pass the above criteria will retain their corresponding visible light distance factors, while observations that fail to pass any criterion will have their GNSS and LED distance factors activated or removed by the "Factor Dynamic Activation / Removal" module based on the screening results.

[0146] Next, refer to Figure 1 In step S170, the state estimate of the mobile vehicle is solved by minimizing the joint optimization objective function; wherein the state estimate includes the positioning parameters of the mobile vehicle in the local navigation coordinate system.

[0147] In this step, after constructing and validating all factors within the sliding window, the system will jointly optimize the objective function described above. The input is fed into a nonlinear optimization solver (such as the Gauss-Newton method, the Levenberg-Marquardt algorithm, or iSAM2) for iterative minimization. The process aims to find the optimal system state that minimizes the overall objective function. ,Right now: ,in: This represents the set of system state variables to be optimized, including but not limited to: The pose (position and attitude) of the mobile vehicle in the local navigation coordinate system; The speed of the moving carrier; IMU bias (accelerometer bias, gyroscope bias); The three-dimensional position of visual feature points in the visual inertial local world coordinate system; The heading deviation ψ, scale factor ξ, and three-dimensional translation deviation in the second transformation relationship .

[0148] The final solution This is the optimized state estimate, which contains complete motion information of the mobile vehicle in the local navigation coordinate system.

[0149] After obtaining the optimized state estimate, it needs to be converted into a positioning result that can be directly used by the user. Specifically, the system outputs the position, velocity, and attitude of the mobile vehicle in the local navigation coordinate system.

[0150] The position output can be represented as: ,in: This indicates the optimized mobile vehicle in the local navigation coordinate system. The lower position; This represents the optimized scale factor (estimated value), used to correct the scale error of the visual inertial trajectory; Represents the optimized visual inertial local world coordinate system To local navigation coordinate system The rotation matrix reflects the global orientation alignment; This represents the optimized mobile vehicle in the visual-inertial local world coordinate system. The lower position; This represents the optimized translational deviation, used to correct the spatial offset between the visual-inertial local world coordinate system and the local navigation coordinate system; symbol This represents the optimized estimate.

[0151] By introducing joint optimization of scale factor, rotation matrix and translation bias, the problems of scale inconsistency and coordinate system deviation between visual inertial system and GNSS / visible light system are solved, significantly improving positioning accuracy and consistency.

[0152] In step S180, while both global navigation satellite system measurement data and visible light received signal intensity data are unavailable, continuous positioning output is maintained based on visual image data and inertial measurement data.

[0153] In this step, when the system is in extreme conditions where GNSS signals are completely unavailable and visible light observations (such as LED light sources) are also ineffective due to obstruction, sparseness, or ambient light interference (e.g., inside a tunnel, deep in an underground parking garage, or in a densely built-up area), the system will no longer rely on any absolute observation constraints, but instead provide continuous motion estimation through visual inertial odometry (VIO).

[0154] Specifically, an objective function containing the following factors can be constructed and optimized using only visual image data and inertial measurement data within a sliding window: Inertia factor It consists of IMU pre-integrated residuals and provides short-term, high-dynamic motion estimation. Visual factors It consists of feature point reprojection residuals, providing scale and pose constraints; Priors : Retain historical status information to ensure trajectory continuity.

[0155] By minimizing this simplified objective function, the system can continuously output the pose and velocity of the moving vehicle in the visual-inertial local world coordinate system, achieving pure visual-inertial continuous positioning.

[0156] In one exemplary implementation, when global navigation satellite system measurement data is unavailable but visible light received signal strength data is available, continuous positioning is maintained by visible light distance factor, visual factor and inertial factor.

[0157] Specifically, when GNSS signals are unavailable (e.g., during the transition section at the tunnel entrance) but visible light observation remains effective (e.g., LED light strips are not obstructed), the system activates the visible light distance factor, combining it with the existing visual and inertial factors to continue joint optimization. Specifically, the system will: Using visible light distance factor Provides additional geometric constraints to suppress scale drift of visual-inertial systems in the absence of GNSS; Continue to use visual factors and inertia factor Maintain the continuity of motion estimation; Do not activate GNSS-related factors (i.e. =0, =0), to avoid errors introduced by invalid observations.

[0158] In this mode, the system can smoothly transition to the "visible light-assisted visual inertial positioning" mode during GNSS degradation, significantly improving positioning accuracy and robustness.

[0159] In one exemplary implementation, when visible light signal strength data from a light source is unavailable but global navigation satellite system measurement data is available, continuous positioning is maintained using position factors, velocity factors, visual factors, and inertial factors.

[0160] When visible light observation is unavailable due to obstruction or sparseness (such as when a vehicle passes through a gap in LED lights) but the GNSS signal is still valid, the system continues to perform joint optimization by activating the GNSS position factor and GNSS velocity factor, combined with the visual factor and the inertial factor.

[0161] Specifically, the system will: Using GNSS location factors and GNSS velocity factor Provides global position and heading constraints; Continue to use visual factors and inertia factor Maintain the continuity of motion estimation; Do not activate the visible light distance factor (i.e.) =0), to avoid interference from invalid observations.

[0162] In this mode, the system can maintain high-precision positioning by relying on GNSS observations during periods of temporary absence of visible light, thus avoiding trajectory drift.

[0163] This disclosure does not interrupt positioning or restart mode during periods of missing absolute observation (GNSS and visible light), but maintains continuous output of the visual inertial system, and smoothly introduces new constraints through the same optimization framework after the absolute observation is restored, avoiding abrupt trajectory changes.

[0164] refer to Figure 10 , Figure 10 This diagram illustrates the overall architecture of the positioning method for a mobile carrier in an embodiment of this disclosure, as shown below. Figure 10 As shown: The system is based on four types of sensors: a camera, an inertial measurement unit (IMU), a Global Navigation Satellite System (GNSS) receiver, and an optoelectronic receiver. It simultaneously acquires visual image data, inertial measurement data, GNSS measurement data, and visible light signal intensity data from light sources. The camera and IMU constitute the visual inertial odometry (VIO) front end, which outputs high-frequency pose, velocity, and feature point trajectories in real time, forming a scale-free, locally consistent, but drift-prone initial motion estimate. This front end provides continuous state priors for subsequent optimization and is the core module for maintaining positioning output even when absolute observations are missing.

[0165] All observation information is uniformly fed into a sliding window tightly coupled optimization backend. Within this framework, the system constructs a joint objective function including inertia factor, visual reprojection factor, GNSS position / velocity factor, visible light distance factor, and heading and scale smoothing factor. The key innovation lies in the introduction of a factor effectiveness screening mechanism: based on multi-dimensional indicators such as GNSS observation uncertainty, vehicle velocity, visible light signal change rate, ranging range, and incident angle, corresponding factors are dynamically activated or eliminated. This mechanism enables the system to adaptively fuse high-quality observations while suppressing erroneous constraints under degradation conditions such as multipath, occlusion, non-line-of-sight, or weak signals, achieving robust optimization.

[0166] After coordinate transformation, the optimized results output the high-precision, globally consistent position, velocity, and attitude of the mobile vehicle in the local navigation coordinate system. The system supports seamless switching between three typical operating modes: global correction primarily using GNSS in GNSS-available areas; a smooth transition from GNSS to visible light-assisted navigation in tunnel transition sections; and absolute constraint maintained by visible light distance factors in GNSS-rejected areas such as tunnels or underground spaces, or retreating to pure visual-inertial continuous output when neither is available. The entire solution can directly reuse existing LED lighting facilities in scenarios such as tunnels, utility tunnels, and underground parking garages, eliminating the need for additional dedicated beacons, significantly reducing deployment costs, and demonstrating outstanding engineering practicality and scalability.

[0167] refer to Figure 11 , Figure 11 This illustration shows a schematic diagram of the multi-source fusion positioning operation process of a mobile carrier under different typical environments in embodiments of this disclosure, such as... Figure 11 As shown: This figure illustrates the operation process and multi-source fusion strategy of the mobile carrier positioning method provided in this disclosure under different typical environmental scenarios. The figure is divided horizontally into four typical areas: GNSS available area, tunnel entrance transition section, tunnel / engineering space (GNSS restricted area), and underground cavern / engineering corridor (GNSS unavailable area), which correspond to the continuous change of working conditions from outdoors to indoors and from open space to enclosed space, respectively.

[0168] Within the GNSS available area, the GNSS signal is stable and of good quality, allowing the system to acquire position and velocity observations normally. Simultaneously, there are sufficient visible light observations (from LED lighting facilities), and the visual inertial odometry (VIO) operates continuously at a high frequency of 200Hz. At this point, the optimization process directly incorporates the GNSS position and velocity constraints into the sliding window optimization model, achieving global high-precision correction and outputting continuous pose and trajectory in a unified local navigation coordinate system.

[0169] Upon entering the transition section at the tunnel entrance, GNSS signals fluctuate or are subject to multipath interference, leading to a decline in observation quality. Visible light observations are still obtainable, but their quantity may decrease. The system employs a dynamic factor screening mechanism to eliminate low-reliability GNSS observations, retaining only high-quality ones. Simultaneously, visible light distance constraints are introduced to achieve a smooth transition from GNSS-dominated to visible light-assisted observations, avoiding trajectory jumps or interruptions caused by mode switching.

[0170] In tunnels or engineering spaces (GNSS-restricted areas), GNSS signals are blocked or completely blocked, making effective observations impossible. However, with LED lighting in the environment, visible light observations can still be obtained, although the quantity may fluctuate with environmental changes. In this case, the system no longer relies on GNSS but instead utilizes the visible light distance factor provided by the LED light source as an absolute geometric constraint, incorporating it into the optimization process to suppress scale drift and heading deviation of the visual inertial trajectory.

[0171] In underground caverns or engineering corridors (GNSS unavailable areas), GNSS signals are completely ineffective, and LED light sources may be sparse or invisible. The system retreats to a pure visual-inertial continuous propagation mode, relying solely on VIO to maintain motion estimation, and combining existing LED constraints (if present) for local correction to ensure continuous trajectory output even without any external observations.

[0172] Ultimately, regardless of the environment, the system can output continuous position, velocity, attitude, and trajectory information in a unified local navigation coordinate system, achieving seamless positioning coverage from outdoors to indoors and from open spaces to complex enclosed spaces.

[0173] Based on the above technical solutions, this disclosure has at least the following technical effects: First, by unifying the visual inertial continuous estimation, GNSS position / velocity constraints, and visible light distance constraints into the same sliding window optimization framework, the tight coupling and fusion of multi-source observations is achieved. This avoids the problems of trajectory interruption, jump, or state discontinuity caused by hard switching of positioning modes due to GNSS failure or visible light obstruction in traditional methods. It significantly improves the positioning continuity and stability of mobile vehicles in complex engineering environments with limited GNSS, such as tunnels, underground spaces, and integrated utility tunnels.

[0174] Second, by simultaneously estimating the heading deviation, scale factor, and translation deviation between the visual inertial local world coordinate system and the local navigation coordinate system during the optimization process, and using GNSS position and velocity factors to correct them online, the visual inertial trajectory, which originally had no scale and no global reference, obtains a unified navigation coordinate reference. This effectively solves the global drift problem caused by coordinate system inconsistency during long-distance operation and improves the global consistency of positioning in a large-scale engineering space.

[0175] Third, by reusing existing LED lighting facilities in the environment to construct a visible light distance factor, it can still provide effective absolute geometric constraints in areas where GNSS signals are completely rejected or severely degraded (such as deep tunnels and underground garages), continuously suppressing the inherent heading drift, scale drift and position accumulation errors of visual inertial odometry, and significantly extending the system's high-precision positioning maintenance capability under GNSS-free conditions.

[0176] Fourth, by introducing the attitude prior information output by the visual inertial system, the photoelectric receiver normal vector, LED light source radiation angle and visible light incident angle are calculated in real time, and these are substituted into the visible light propagation model to dynamically and geometrically correct the ranging value. This breaks through the dependence of traditional visible light ranging on horizontal installation of equipment or static scenes, effectively reducing the impact of mobile carriers on ranging accuracy under dynamic conditions such as driving on slopes, turning, vibration or tilting of handheld terminals, and improving the practicality and adaptability of visible light assisted positioning.

[0177] Fifth, by establishing a dynamic activation mechanism based on observation quality, and comprehensively considering multi-dimensional criteria such as GNSS position / velocity uncertainty, mobile vehicle speed, relative rate of change of visible light signal, ranging effectiveness, and incident angle rationality, low-reliability observations are automatically screened and eliminated. This avoids erroneous data fusion under degradation conditions such as multipath effects, non-line-of-sight propagation, ambient light interference, and weak signals at long distances, significantly enhancing the robustness and reliability of the system in challenging scenarios such as complex urban canyons, indoor obstruction, and sparse LEDs.

[0178] Sixth, it eliminates the need for additional dedicated beacons or base stations, directly utilizing the LED lighting systems already widely installed in large infrastructures such as tunnels, underground caverns, and integrated utility tunnels as the positioning signal source. This significantly reduces system deployment costs and engineering modification difficulties, and possesses good scalability and practical engineering application value. It is especially suitable for scenarios with an urgent need for low-cost and robust positioning, such as smart transportation, intelligent construction, and emergency rescue.

[0179] This disclosure also provides a positioning system for a mobile carrier. Figure 12 This diagram illustrates the structure of a positioning system for a mobile carrier in an exemplary embodiment of this disclosure; as shown below. Figure 12 As shown, the positioning system 1200 for a mobile carrier may include a data acquisition module 1210, a transformation relationship determination module 1220, a first factor construction module 1230, a second factor construction module 1240, a third factor construction module 1250, an objective function construction module 1260, and a positioning module 1270. Wherein: The data acquisition module 1210 is used to acquire multi-source observation data of the mobile carrier at various times, and to synchronize the multi-source observation data in time to form a synchronized observation set; the multi-source observation data includes visual image data, inertial measurement data, global navigation satellite system measurement data, and visible light received signal intensity data of light sources. The transformation relationship determination module 1220 is used to transform the global navigation satellite system measurement data and the known positions of each light source in the geocentric-ground-fixed coordinate system to the local navigation coordinate system using a first transformation relationship from the geocentric-ground-fixed coordinate system to the local navigation coordinate system; and to transform the pose and velocity of the moving vehicle in the visual-inertial local world coordinate system to the local navigation coordinate system using a second transformation relationship between the visual-inertial local world coordinate system and the local navigation coordinate system; the second transformation relationship is characterized by heading deviation, scale factor and three-dimensional translation deviation, wherein the heading deviation is a single-axis rotation parameter about the direction of gravity; The first factor construction module 1230 is used to construct residuals based on the position and velocity observations in the global navigation satellite system measurement data and the corresponding components of the pose and velocity transformed by the second transformation relationship in the local navigation coordinate system, and to weight the residuals according to the covariances of the position and velocity observations to form position factors and velocity factors. The second factor construction module 1240 is used to construct a visible light distance factor based on the known position of the light source in the local navigation coordinate system, the visible light ranging value calculated from the visible light received signal intensity data of the light source, and the geometric relationship between the pose of the moving vehicle in the visual-inertial local world coordinate system; the position factor, the velocity factor, and the visible light distance factor are used together to constrain the heading deviation, the scale factor, and the three-dimensional translation deviation in the second transformation relationship; The third factor construction module 1250 is used to construct the heading deviation smoothing factor and the scale smoothing factor respectively based on the heading deviation difference and the scale factor difference between adjacent times within the sliding window. The objective function construction module 1260 is used to construct a joint optimization objective function within the sliding window; the joint optimization objective function includes a visual factor constructed based on the visual image data, an inertial factor constructed based on the inertial measurement data, the position factor, the velocity factor, the visible light distance factor, the heading deviation smoothing factor, and the scale smoothing factor; The positioning module 1270 is used to solve for the state estimate of the mobile vehicle by minimizing the joint optimization objective function; wherein the state estimate includes the positioning parameters of the mobile vehicle in the local navigation coordinate system; and to maintain continuous positioning output based on the visual image data and the inertial measurement data during periods when both the global navigation satellite system measurement data and the visible light received signal intensity data of the light source are unavailable.

[0180] In an exemplary embodiment of this disclosure, the first factor construction module 1230 constructs residuals based on the position and velocity observations in the global navigation satellite system measurement data, respectively, and the corresponding components of the pose and velocity transformed by the second transformation relationship in the local navigation coordinate system. The residuals are then weighted according to the covariances corresponding to the position and velocity observations to form position and velocity factors, including: The position residual is obtained by subtracting the coordinates of the global navigation satellite system position observation in the local navigation coordinate system from the position component of the pose obtained by mapping through the second transformation relationship in the local navigation coordinate system. The position residual is then weighted based on the covariance corresponding to the position observation to form the position factor. The velocity residual is obtained by subtracting the velocity component of the global navigation satellite system velocity observation in the local navigation coordinate system from the velocity component of the velocity obtained by mapping through the second transformation relationship in the local navigation coordinate system. The velocity residual is then weighted based on the covariance corresponding to the velocity observation to form the velocity factor.

[0181] In an exemplary embodiment of this disclosure, the second factor construction module 1240 constructs a visible light distance factor based on the known position of the light source in the local navigation coordinate system, the visible light ranging value calculated from the visible light received signal intensity data of the light source, and the geometric relationship between the pose of the moving carrier in the visual-inertial local world coordinate system, including: Based on the visible light received signal intensity data of the light source and the pre-calibrated visible light propagation model, the visible light ranging value is calculated inversely. Calculate the Euclidean distance between the position of the mobile carrier in the local navigation coordinate system and the known positions of each light source in the local navigation coordinate system. The difference between the Euclidean distance and the visible light ranging value is taken as the distance residual. The distance residual is weighted based on the observation variance corresponding to the visible light ranging value to form the visible light distance factor.

[0182] In an exemplary embodiment of this disclosure, after the visible light ranging value is obtained by inverse calculation, the second factor construction module 1240 is configured to: Using the pose maintained in the sliding window, the fixed normal vector of the photoelectric receiver in the carrier coordinate system is transformed to the local navigation coordinate system to obtain the receiver normal vector; The visible light incident angle is calculated based on the receiver normal vector and the direction vector from the moving carrier to the light source. The radiation angle is calculated based on the optical axis direction of the light source and the direction vector from the light source to the moving carrier. Substituting the visible light incident angle and the radiation angle into the visible light propagation model, the visible light ranging value is geometrically corrected.

[0183] In an exemplary embodiment of this disclosure, the objective function construction module 1260 is configured as follows: The position factor, velocity factor, and visible light distance factor within the sliding window are subjected to validity screening, and only the factors that pass the validity screening are included in the joint optimization objective function.

[0184] In an exemplary embodiment of this disclosure, the objective function construction module 1260 performs validity screening on the position factor, velocity factor, and visible light distance factor within the sliding window, and only includes factors that pass the validity screening into the joint optimization objective function, including: When the position observation uncertainty of the global navigation satellite system measurement data is less than a first preset threshold, the position factor is determined to be valid; When the velocity observation uncertainty of the global navigation satellite system measurement data is less than a second preset threshold, and the velocity modulus of the mobile carrier is greater than the minimum effective velocity, the velocity factor is determined to be effective. When the relative rate of change of the signal corresponding to the visible light received signal intensity data of the light source is less than a third preset threshold, the visible light ranging value is within a preset effective distance range, and the visible light incident angle is less than the maximum allowable incident angle, the corresponding visible light distance factor is determined to be valid.

[0185] In an exemplary embodiment of this disclosure, the positioning module 1270 is configured to: When the global navigation satellite system measurement data is unavailable but the visible light received signal intensity data of the light source is available, continuous positioning is maintained by the visible light distance factor, the visual factor and the inertial factor; When the visible light received signal intensity data of the light source is unavailable but the measurement data of the global navigation satellite system is available, continuous positioning is maintained by the position factor, the velocity factor, the visual factor and the inertial factor.

[0186] The specific details of each module in the aforementioned mobile carrier positioning system have been described in detail in the corresponding mobile carrier positioning methods, so they will not be repeated here.

[0187] It should be noted that although several modules or units for the device used to perform actions have been mentioned in the detailed description above, this division is not mandatory. In fact, according to embodiments of this disclosure, the features and functions of two or more modules or units described above can be embodied in one module or unit. Conversely, the features and functions of one module or unit described above can be further divided and embodied by multiple modules or units.

[0188] Furthermore, although the steps of the method in this disclosure are described in a specific order in the accompanying drawings, this does not require or imply that the steps must be performed in that specific order, or that all the steps shown must be performed to achieve the desired result. Additional or alternative steps may be omitted, multiple steps may be combined into one step, and / or a step may be broken down into multiple steps.

[0189] From the above description of the embodiments, those skilled in the art will readily understand that the exemplary embodiments described herein can be implemented by software or by combining software with necessary hardware. Therefore, the technical solutions according to the embodiments of this disclosure can be embodied in the form of a software product, which can be stored in a non-volatile storage medium (such as a CD-ROM, USB flash drive, external hard drive, etc.) or on a network, including several instructions to cause a computing device (such as a personal computer, server, mobile terminal, or network device, etc.) to execute the methods according to the embodiments of this disclosure.

[0190] This disclosure also provides a computer-readable storage medium, which may be included in the electronic device described in the above embodiments; or it may exist independently and not assembled into the electronic device.

[0191] Computer-readable storage media can be, for example—but not limited to—electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatuses, or devices, or any combination thereof. More specific examples of computer-readable storage media may include, but are not limited to: electrical connections having one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof. In this disclosure, a computer-readable storage medium can be any tangible medium that contains or stores a program that can be used by or in conjunction with an instruction execution system, apparatus, or device.

[0192] A computer-readable storage medium can be sent, propagated, or transmitted for use by or in connection with an instruction execution system, apparatus, or device. The program code contained on the computer-readable storage medium can be transmitted using any suitable medium, including but not limited to: wireless, wireline, optical fiber, RF, etc., or any suitable combination thereof.

[0193] A computer-readable storage medium carries one or more programs that, when executed by an electronic device, cause the electronic device to perform the methods described in the above embodiments.

[0194] Furthermore, this disclosure also provides an electronic device capable of implementing the above-described method.

[0195] Those skilled in the art will understand that various aspects of this disclosure can be implemented as a system, method, or program product. Therefore, various aspects of this disclosure can be specifically implemented in the following forms: a completely hardware implementation, a completely software implementation (including firmware, microcode, etc.), or a combination of hardware and software aspects, collectively referred to herein as a "circuit," "module," or "system."

[0196] The following reference Figure 13 To describe an electronic device 1300 according to such an embodiment of the present disclosure. Figure 13 The electronic device 1300 shown is merely an example and should not impose any limitation on the functionality and scope of use of the embodiments disclosed herein.

[0197] like Figure 13 As shown, the electronic device 1300 is presented in the form of a general-purpose computing device. The components of the electronic device 1300 may include, but are not limited to: at least one processor 1310, at least one memory 1320, a bus 1330 connecting different system components (including memory 1320 and processor 1310), and a display 1340.

[0198] The memory stores program code that can be executed by the processor 1310, causing the processor 1310 to perform the steps described in the "Exemplary Methods" section of this specification according to various exemplary embodiments of this disclosure. For example, the processor 1310 can perform actions such as... Figure 1As shown: Step S110, acquire multi-source observation data of the mobile carrier at various times, and synchronize the multi-source observation data in time to form a synchronized observation set; the multi-source observation data includes visual image data, inertial measurement data, global navigation satellite system measurement data, and visible light received signal intensity data of light sources; Step S120, using the first transformation relationship from the geocentric-fixed coordinate system to the local navigation coordinate system, transform the global navigation satellite system measurement data and the known positions of each light source in the geocentric-fixed coordinate system to the local navigation coordinate system; using the second transformation between the visual-inertial local world coordinate system and the local navigation coordinate system... The first transformation relationship transforms the pose and velocity of the mobile vehicle in the visual-inertial local world coordinate system to the local navigation coordinate system. The second transformation relationship is characterized by heading deviation, scale factor, and three-dimensional translation deviation, where the heading deviation is a single-axis rotation parameter about the direction of gravity. Step S130: Based on the position and velocity observations from the global navigation satellite system measurement data, residuals are constructed with the corresponding components of the pose and velocity transformed by the second transformation relationship in the local navigation coordinate system. The residuals are then weighted according to the covariance of the position and velocity observations to form the position factor and velocity factor. Step S140: Based on the known position of the light source in the local navigation coordinate system, the visible light ranging value calculated from the visible light received signal intensity data of the light source, and the geometric relationship between the pose of the moving vehicle in the visual-inertial local world coordinate system, a visible light distance factor is constructed. The position factor, velocity factor, and visible light distance factor are used together to constrain the heading deviation, scale factor, and three-dimensional translation deviation in the second transformation relationship. In step S150, a heading deviation smoothing factor and a scale smoothing factor are constructed respectively based on the difference in heading deviation and the difference in scale factor between adjacent moments within the sliding window. In step S160, a joint optimization objective is constructed within the sliding window. The objective function is defined as follows: the joint optimization objective function includes a visual factor constructed based on visual image data, an inertial factor constructed based on inertial measurement data, a position factor, a velocity factor, a visible light distance factor, a heading deviation smoothing factor, and a scale smoothing factor; step S170, by minimizing the joint optimization objective function, the state estimate of the mobile vehicle is solved; wherein, the state estimate includes the positioning parameters of the mobile vehicle in the local navigation coordinate system; step S180, during the period when both global navigation satellite system measurement data and visible light received signal intensity data are unavailable, continuous positioning output is maintained based on visual image data and inertial measurement data.

[0199] The memory 1320 may include a readable medium in the form of volatile storage, such as random access memory (RAM) 13201 and / or cache memory 13202, and may further include read-only memory (ROM) 13203.

[0200] The memory 1320 may also include a program / utility 13204 having a set (at least one) of program modules 13205, including but not limited to: an operating system, one or more application programs, other program modules, and program data, each or some combination of these examples may include an implementation of a network environment.

[0201] Bus 1330 can represent one or more of several types of bus structures, including a memory bus or memory controller, peripheral bus, graphics acceleration port, processor, or a local bus using any of the various bus structures.

[0202] Electronic device 1300 can also communicate with one or more external devices 1400 (e.g., keyboard, pointing device, Bluetooth device, etc.), one or more devices that enable a user to interact with electronic device 1300, and / or any device that enables electronic device 1300 to communicate with one or more other computing devices (e.g., router, modem, etc.). This communication can be performed via input / output (I / O) interface 1350. Furthermore, electronic device 1300 can also communicate with one or more networks (e.g., local area network (LAN), wide area network (WAN), and / or public networks, such as the Internet) via network adapter 1360. As shown, network adapter 1360 communicates with other modules of electronic device 1300 via bus 1330. It should be understood that, although not shown in the figures, other hardware and / or software modules can be used in conjunction with electronic device 1300, including but not limited to: microcode, device drivers, redundant processors, external disk drive arrays, RAID systems, tape drives, and data backup storage systems.

[0203] Other embodiments of this disclosure will readily occur to those skilled in the art upon consideration of the specification and practice of the invention disclosed herein. This disclosure is intended to cover any variations, uses, or adaptations of this disclosure that follow the general principles of this disclosure and include common knowledge or customary techniques in the art not disclosed herein. The specification and embodiments are to be considered exemplary only, and the true scope and spirit of this disclosure are indicated by the claims.

Claims

1. A method for locating a mobile carrier, characterized in that, include: Acquire multi-source observation data of the mobile carrier at various times, and synchronize the multi-source observation data in time to form a synchronized observation set; The multi-source observation data includes visual image data, inertial measurement data, global navigation satellite system measurement data, and visible light received signal intensity data from light sources. Using a first transformation relationship from the geocentric-ground-fixed coordinate system to the local navigation coordinate system, the measurement data of the global navigation satellite system and the known positions of each light source in the geocentric-ground-fixed coordinate system are transformed to the local navigation coordinate system; using a second transformation relationship between the visual-inertial local world coordinate system and the local navigation coordinate system, the pose and velocity of the moving vehicle in the visual-inertial local world coordinate system are transformed to the local navigation coordinate system; the second transformation relationship is characterized by heading deviation, scale factor and three-dimensional translation deviation, wherein the heading deviation is a single-axis rotation parameter about the direction of gravity; Based on the position and velocity observations in the global navigation satellite system measurement data, residuals are constructed by comparing them with the corresponding components of the pose and velocity in the local navigation coordinate system after transformation by the second transformation relationship. The residuals are then weighted according to the covariances of the position and velocity observations to form position and velocity factors. Based on the known position of the light source in the local navigation coordinate system, the visible light ranging value calculated from the visible light received signal intensity data of the light source, and the geometric relationship between the pose of the moving vehicle in the visual-inertial local world coordinate system, a visible light distance factor is constructed; the position factor, the velocity factor, and the visible light distance factor are used together to constrain the heading deviation, the scale factor, and the three-dimensional translation deviation in the second transformation relationship; Based on the difference in heading deviation and the difference in scale factor between adjacent times within the sliding window, heading deviation smoothing factor and scale smoothing factor are constructed respectively. A joint optimization objective function is constructed within the sliding window; the joint optimization objective function includes a visual factor constructed based on the visual image data, an inertial factor constructed based on the inertial measurement data, a position factor, a velocity factor, a visible light distance factor, a heading deviation smoothing factor, and a scale smoothing factor; The state estimate of the mobile vehicle is solved by minimizing the joint optimization objective function; wherein the state estimate includes the positioning parameters of the mobile vehicle in the local navigation coordinate system. During periods when both the global navigation satellite system measurement data and the visible light received signal intensity data of the light source are unavailable, continuous positioning output is maintained based on the visual image data and the inertial measurement data.

2. The method according to claim 1, characterized in that, The position and velocity observations from the global navigation satellite system measurement data are used to construct residuals with the corresponding components of the pose and velocity transformed by the second transformation relationship in the local navigation coordinate system. These residuals are then weighted according to the covariance of each position and velocity observation to form position and velocity factors, including: The position residual is obtained by subtracting the coordinates of the global navigation satellite system position observation in the local navigation coordinate system from the position component of the pose obtained by mapping through the second transformation relationship in the local navigation coordinate system. The position residual is then weighted based on the covariance corresponding to the position observation to form the position factor. The velocity residual is obtained by subtracting the velocity component of the global navigation satellite system velocity observation in the local navigation coordinate system from the velocity component of the velocity obtained by mapping through the second transformation relationship in the local navigation coordinate system. The velocity residual is then weighted based on the covariance corresponding to the velocity observation to form the velocity factor.

3. The method according to claim 1, characterized in that, The visible light distance factor is constructed based on the known position of the light source in the local navigation coordinate system, the visible light ranging value calculated from the visible light received signal intensity data of the light source, and the geometric relationship between the pose of the moving vehicle in the visual-inertial local world coordinate system, including: Based on the visible light received signal intensity data of the light source and the pre-calibrated visible light propagation model, the visible light ranging value is calculated inversely. Calculate the Euclidean distance between the position of the mobile carrier in the local navigation coordinate system and the known positions of each light source in the local navigation coordinate system. The difference between the Euclidean distance and the visible light ranging value is taken as the distance residual. The distance residual is weighted based on the observation variance corresponding to the visible light ranging value to form the visible light distance factor.

4. The method according to claim 3, characterized in that, After obtaining the visible light ranging value through reverse calculation, the method further includes: Using the pose maintained in the sliding window, the fixed normal vector of the photoelectric receiver in the carrier coordinate system is transformed to the local navigation coordinate system to obtain the receiver normal vector; The visible light incident angle is calculated based on the receiver normal vector and the direction vector from the moving carrier to the light source. The radiation angle is calculated based on the optical axis direction of the light source and the direction vector from the light source to the moving carrier. Substituting the visible light incident angle and the radiation angle into the visible light propagation model, the visible light ranging value is geometrically corrected.

5. The method according to claim 4, characterized in that, The method further includes: The position factor, velocity factor, and visible light distance factor within the sliding window are subjected to validity screening, and only the factors that pass the validity screening are included in the joint optimization objective function.

6. The method according to claim 5, characterized in that, The step of effectively filtering the position factor, velocity factor, and visible light distance factor within the sliding window, and including only the factors that pass the effectiveness filtering in the joint optimization objective function, includes: When the position observation uncertainty of the global navigation satellite system measurement data is less than a first preset threshold, the position factor is determined to be valid; When the velocity observation uncertainty of the global navigation satellite system measurement data is less than a second preset threshold, and the velocity modulus of the mobile carrier is greater than the minimum effective velocity, the velocity factor is determined to be effective. When the relative rate of change of the signal corresponding to the visible light received signal intensity data of the light source is less than a third preset threshold, the visible light ranging value is within a preset effective distance range, and the visible light incident angle is less than the maximum allowable incident angle, the corresponding visible light distance factor is determined to be valid.

7. The method according to any one of claims 1 to 6, characterized in that, The method further includes: When the global navigation satellite system measurement data is unavailable but the visible light received signal intensity data of the light source is available, continuous positioning is maintained by the visible light distance factor, the visual factor and the inertial factor; When the visible light received signal intensity data of the light source is unavailable but the measurement data of the global navigation satellite system is available, continuous positioning is maintained by the position factor, the velocity factor, the visual factor and the inertial factor.

8. A positioning system for a mobile carrier, characterized in that, include: The data acquisition module is used to acquire multi-source observation data of the mobile carrier at various times, and to synchronize the multi-source observation data in time to form a synchronized observation set; the multi-source observation data includes visual image data, inertial measurement data, global navigation satellite system measurement data, and visible light received signal intensity data of light sources; The transformation relationship determination module is used to transform the global navigation satellite system measurement data and the known positions of each light source in the geocentric-ground-fixed coordinate system to the local navigation coordinate system using a first transformation relationship from the geocentric-ground-fixed coordinate system to the local navigation coordinate system; and to transform the pose and velocity of the moving vehicle in the visual-inertial local world coordinate system to the local navigation coordinate system using a second transformation relationship between the visual-inertial local world coordinate system and the local navigation coordinate system; the second transformation relationship is characterized by heading deviation, scale factor and three-dimensional translation deviation, wherein the heading deviation is a single-axis rotation parameter about the direction of gravity; The first factor construction module is used to construct residuals based on the position and velocity observations in the global navigation satellite system measurement data, and the corresponding components of the pose and velocity transformed by the second transformation relationship in the local navigation coordinate system, respectively. The residuals are then weighted according to the covariances of the position and velocity observations to form position factors and velocity factors. The second factor construction module is used to construct a visible light distance factor based on the known position of the light source in the local navigation coordinate system, the visible light ranging value calculated from the visible light received signal intensity data of the light source, and the geometric relationship between the pose of the moving vehicle in the visual-inertial local world coordinate system; the position factor, the velocity factor, and the visible light distance factor are used together to constrain the heading deviation, the scale factor, and the three-dimensional translation deviation in the second transformation relationship; The third factor construction module is used to construct the heading deviation smoothing factor and the scale smoothing factor based on the heading deviation difference and the scale factor difference between adjacent times within the sliding window, respectively. The objective function construction module is used to construct a joint optimization objective function within the sliding window; the joint optimization objective function includes a visual factor constructed based on the visual image data, an inertial factor constructed based on the inertial measurement data, a position factor, a velocity factor, a visible light distance factor, a heading deviation smoothing factor, and a scale smoothing factor; The positioning module is used to solve for the state estimate of the mobile vehicle by minimizing the joint optimization objective function; wherein the state estimate includes the positioning parameters of the mobile vehicle in the local navigation coordinate system; and to maintain continuous positioning output based on the visual image data and the inertial measurement data during periods when both the global navigation satellite system measurement data and the visible light received signal intensity data of the light source are unavailable.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the positioning method of the mobile carrier according to any one of claims 1 to 7.

10. An electronic device, characterized in that, include: processor; as well as Memory for storing the executable instructions of the processor; The processor is configured to execute the positioning method of any one of claims 1 to 7 by executing the executable instructions.

Images