Method and device for positioning mapping of strong-shading orchard environment, equipment and medium

Abstract

The application relates to a positioning mapping method, device, equipment and medium for a strong-shading orchard environment, and the method comprises the following steps: calculating a real-time dynamic differential positioning weight through a continuous mapping function, scaling a position covariance according to the real-time dynamic differential positioning weight, and generating a dynamic covariance matrix; adding a real-time dynamic differential positioning observation position as a global constraint into a factor graph optimization framework, jointly optimizing a residual error of a visual inertial odometer, and obtaining a global pose estimation result of a mobile platform in a global coordinate system; when the mobile platform drives out of a fruit tree shading area, the solving state of the real-time dynamic differential positioning is recovered from a degraded solving state to a fixed solving state, the real-time dynamic differential positioning weight is gradually increased, the real-time dynamic differential positioning correction amount is smoothly absorbed, and the drift of the visual inertial odometer accumulated in the shading area is corrected. The application realizes high precision, continuity, low drift and global consistency of positioning in a strong-shading orchard environment.

Description

Technical Field

[0001] This application relates to the field of orchard location mapping, and in particular to a location mapping method, corresponding device, electronic equipment and computer-readable storage medium for orchards with strong shading. Background Technology

[0002] In precision agriculture and agricultural remote sensing research, to achieve spatial alignment between data collected at different times, on different platforms, and from different sensors, it is typically necessary to establish accurate GNSS spatial location indexes for target crops, target areas, or target plots. By using a unified geographic coordinate benchmark, heterogeneous information such as 3D point clouds, RGB remote sensing imagery, hyperspectral data, yield data, and agricultural management data can be correlated to the same spatial location, thereby supporting applications such as fruit tree growth monitoring, canopy structure analysis, pest and disease identification, yield estimation, and agricultural machinery operation management.

[0003] However, in orchard environments, GNSS signals are easily blocked by tree canopies, branches, and leaves, accompanied by multipath reflections and signal attenuation, leading to unstable RTK-GNSS positioning and making it difficult to maintain a fixed centimeter-level solution over long periods. When relying solely on GNSS positioning, the system is prone to positioning jumps or data interruptions; while relying solely on visual-inertial odometry can obtain continuous local motion estimates, cumulative drift occurs in large-scale, long-distance operations, failing to guarantee the consistency of global geographic coordinates.

[0004] In summary, to address the challenges posed by existing technologies in simultaneously meeting the requirements for high-precision synchronous acquisition and continuous positioning in orchard scenarios, which demand high accuracy, continuity, low drift, and global coordinate consistency, the applicant has undertaken corresponding explorations to resolve these issues. Summary of the Invention

[0005] The purpose of this application is to solve the above-mentioned problems by providing a method for positioning and mapping in a heavily obscured orchard environment, a corresponding device, electronic equipment, and a computer-readable storage medium.

[0006] To achieve the various objectives of this application, the following technical solution is adopted:

[0007] A method for locating and mapping a heavily shaded orchard environment, proposed to meet one of the purposes of this application, includes:

[0008] The mobile platform acquires multi-source sensor data during its movement in the orchard environment. The multi-source sensor data includes at least binocular images acquired by a binocular vision sensor mounted on the mobile platform, inertial measurement unit data acquired by an inertial measurement unit, and real-time dynamic differential positioning data output by a satellite positioning module.

[0009] Visual inertial odometry is constructed based on the binocular images and the inertial measurement unit data to obtain the local pose estimation result of the mobile platform in the local coordinate system;

[0010] Based on the solution state, position covariance, and position residual between the real-time dynamic differential positioning observation position and the local pose estimation result in the real-time dynamic differential positioning data, a positioning quality evaluation index is constructed. Based on the positioning quality evaluation index, the real-time dynamic differential positioning weight of the real-time dynamic differential positioning observation in the factor graph optimization is calculated through a continuous mapping function, and the position covariance is scaled according to the real-time dynamic differential positioning weight to generate a dynamic covariance matrix.

[0011] The real-time dynamic differential positioning observation position is added as a global constraint to the factor graph optimization framework and optimized together with the residual of the visual inertial odometry to obtain the global pose estimation result of the mobile platform in the global coordinate system.

[0012] When the mobile platform moves out of the fruit tree shading area, as the real-time dynamic differential positioning solution state recovers from the degraded solution state to the fixed solution state, the real-time dynamic differential positioning weight is gradually increased, so that the real-time dynamic differential positioning correction amount is smoothly absorbed, thereby correcting the drift accumulated by the visual inertial odometry in the shading area.

[0013] Optionally, the step of constructing a visual inertial odometry based on the binocular images and the inertial measurement unit data to obtain the local pose estimation result of the mobile platform in the local coordinate system includes:

[0014] Visual features are extracted and tracked from the binocular images to obtain feature matching relationships between adjacent frames;

[0015] Stereo matching is performed on the binocular images to obtain visual geometric constraints with known scale;

[0016] The inertial measurement unit data is pre-integrated to obtain motion increment constraints between adjacent keyframes;

[0017] The visual geometric constraints and the motion increment constraints are incorporated into a sliding window optimization framework, and the local pose estimation results of the mobile platform in the local coordinate system, including position, velocity, attitude, accelerometer bias, and gyroscope bias, are estimated through nonlinear optimization.

[0018] Optionally, the step of constructing a positioning quality evaluation index based on the solution state, position covariance, and position residual between the real-time dynamic differential positioning observation position and the local pose estimation result in the real-time dynamic differential positioning data includes:

[0019] The solution status in the real-time dynamic differential positioning data is obtained, the solution status is quantized and assigned a value, and a state quantization value is generated. The solution status includes fixed solution status, floating solution status, single-point solution status and invalid solution status.

[0020] Obtain the position covariance matrix from the real-time dynamic differential positioning data, and calculate the trace of the position covariance matrix as a quantification of positioning uncertainty.

[0021] The real-time dynamic differential positioning observation position and the local pose estimation result are obtained. The position residual of the real-time dynamic differential positioning observation position relative to the local pose estimation result is calculated, and the norm of the position residual is calculated as a quantitative value of observation consistency.

[0022] Using the state quantization value, the positioning uncertainty quantization value, and the observation consistency quantization value as inputs, a positioning quality evaluation index is calculated according to a preset comprehensive evaluation function. The positioning quality evaluation index is used to characterize the reliability of real-time dynamic differential positioning observations at the current moment. The higher the state quantization value, the lower the positioning uncertainty quantization value, and the lower the observation consistency quantization value, the higher the positioning quality evaluation index.

[0023] Optionally, the step of calculating the real-time dynamic differential positioning weights of real-time dynamic differential positioning observations in factor graph optimization using a continuous mapping function includes:

[0024] Obtain the positioning quality evaluation index at the current moment;

[0025] Using the positioning quality evaluation index as an input variable, the real-time dynamic differential positioning weight at the current moment is calculated through a monotonically increasing continuous mapping function, wherein the real-time dynamic differential positioning weight continuously increases as the positioning quality evaluation index increases and continuously decreases as the positioning quality evaluation index decreases.

[0026] The output range of the continuous mapping function is limited to a preset interval, and the continuous mapping function is continuously differentiable within its domain, so that the real-time dynamic differential positioning weights transition continuously with the changes in the positioning quality evaluation index.

[0027] Optionally, the step of scaling the position covariance according to the real-time dynamic differential positioning weights to generate a dynamic covariance matrix includes:

[0028] Obtain the real-time dynamic differential positioning weights at the current moment;

[0029] Using the sum of the real-time dynamic differential positioning weights and the fractional value used to prevent division by zero as the denominator, calculate its corresponding reciprocal to obtain the scaling factor;

[0030] The original position covariance matrix in the real-time dynamic differential positioning data is multiplied by the scaling factor to generate a dynamic covariance matrix. The larger the weight of the real-time dynamic differential positioning, the smaller the scaling factor, the smaller the dynamic covariance matrix, and the stronger the constraint strength of the real-time dynamic differential positioning observation in factor graph optimization. Conversely, the smaller the weight of the real-time dynamic differential positioning, the larger the scaling factor, the larger the dynamic covariance matrix, and the weaker the constraint strength of the real-time dynamic differential positioning observation in factor graph optimization.

[0031] Optionally, the step of incorporating the real-time dynamic differential positioning observation position as a global constraint into the factor graph optimization framework, and optimizing it together with the visual inertial odometry residuals to obtain the global pose estimation result of the mobile platform in the global coordinate system, includes:

[0032] Construct a visual reprojection residual, wherein the visual reprojection residual represents the difference between the observed position of a feature point in a binocular image and the predicted position of the feature point projected onto the image plane according to the current estimated pose;

[0033] Construct an inertial measurement pre-integration residual, wherein the inertial measurement pre-integration residual represents the difference between the relative motion increment obtained by pre-integration of inertial measurement data between adjacent keyframes and the relative motion increment calculated by estimated pose;

[0034] Construct a real-time dynamic differential positioning position residual, wherein the real-time dynamic differential positioning position residual represents the difference between the real-time dynamic differential positioning observation position and the visual inertial odometry predicted position;

[0035] Construct a multi-source constraint joint optimization function, which is the weighted sum of squares of the visual reprojection residual, the inertial measurement pre-integration residual, and the real-time dynamic differential positioning position residual;

[0036] The multi-source constraint joint optimization function is solved by nonlinear optimization to obtain the global pose estimation result in the global coordinate system.

[0037] Optionally, when the mobile platform leaves the area obscured by fruit trees, and the real-time dynamic differential positioning solution returns from a degraded solution state to a fixed solution state, the real-time dynamic differential positioning weight is gradually increased to smoothly absorb the real-time dynamic differential positioning correction amount, thereby correcting the drift accumulated by the visual inertial odometry in the obscured area. This step includes:

[0038] Obtain the positioning quality evaluation index at the current moment, and calculate the target weight at the current moment based on the positioning quality evaluation index. The deterioration solution state includes floating solution state, single-point solution state and invalid solution state.

[0039] Obtain the actual weight of the previous time step, and use the weighted average of the actual weight of the previous time step and the target weight of the current time step as the actual weight of the current time step. The actual weight of the previous time step is assigned a first smoothing factor, and the target weight of the current time step is assigned a second smoothing factor. The sum of the first smoothing factor and the second smoothing factor is 1.

[0040] During the period of the fixed solution state, the above steps are repeated so that the actual weight gradually approaches the target weight from the value of the previous moment, and the real-time dynamic differential positioning correction is gradually absorbed in multiple optimization windows as the actual weight gradually increases.

[0041] When the actual weight reaches the target weight, the real-time dynamic differential positioning observation is added to the factor graph optimization with the highest constraint strength to correct the drift accumulated by the visual inertial odometry in the occluded area.

[0042] A positioning and mapping device for a heavily shaded orchard environment, provided for another purpose of this application, includes:

[0043] The data synchronization acquisition module is configured to acquire multi-source sensor data during the movement of the mobile platform in the orchard environment. The multi-source sensor data includes at least binocular images acquired by the binocular vision sensor mounted on the mobile platform, inertial measurement unit data acquired by the inertial measurement unit, and real-time dynamic differential positioning data output by the satellite positioning module.

[0044] The visual inertial estimation module is configured to construct a visual inertial odometry based on the binocular images and the inertial measurement unit data, and obtain the local pose estimation result of the mobile platform in the local coordinate system;

[0045] The positioning weight generation module is configured to construct a positioning quality evaluation index based on the solution state, position covariance, and position residual between the real-time dynamic differential positioning observation position and the local pose estimation result in the real-time dynamic differential positioning data; based on the positioning quality evaluation index, the module calculates the real-time dynamic differential positioning weight of the real-time dynamic differential positioning observation in factor graph optimization through a continuous mapping function, and scales the position covariance according to the real-time dynamic differential positioning weight to generate a dynamic covariance matrix;

[0046] The joint optimization solution module is configured to incorporate the real-time dynamic differential positioning observation position as a global constraint into the factor graph optimization framework, and optimize it together with the residual of the visual inertial odometry to obtain the global pose estimation result of the mobile platform in the global coordinate system.

[0047] The progressive drift correction module is configured to gradually increase the weight of the real-time dynamic differential positioning when the mobile platform moves out of the fruit tree shading area and the real-time dynamic differential positioning solution state recovers from the degraded solution state to the fixed solution state, so that the real-time dynamic differential positioning correction amount is smoothly absorbed, thereby correcting the drift accumulated by the visual inertial odometry in the shading area.

[0048] An electronic device provided for another purpose of this application includes a central processing unit and a memory, the central processing unit being configured to invoke and run a computer program stored in the memory to perform the steps of the location mapping method for a heavily shaded orchard environment described in this application.

[0049] A computer-readable storage medium is provided for another purpose of this application, which stores, in the form of computer-readable instructions, a computer program implemented according to the location mapping method for the heavily shaded orchard environment, which, when invoked by a computer, executes the steps included in the corresponding method.

[0050] Compared to existing technologies, this application addresses the problem that existing technologies cannot simultaneously meet the requirements of high-precision synchronous acquisition and continuous positioning in orchard scenarios, which require high precision, continuity, low drift, and global coordinate consistency. This application includes, but is not limited to, the following beneficial effects:

[0051] Firstly, this application constructs an integrated dynamic weight evaluation function based on three factors: the real-time dynamic differential positioning solution state, the position covariance, and the position residual between the real-time dynamic differential positioning observation position and the local pose estimation result. This function allows the real-time dynamic differential positioning constraint strength to be adjusted in real time according to the actual positioning quality. When the real-time dynamic differential positioning accuracy is high, the system enhances global constraints to correct the cumulative drift of the visual inertial odometry (VIO). When the real-time dynamic differential positioning signal deteriorates, the system reduces the constraint weights and degenerates into VIO-dominated positioning. Compared to traditional methods that directly use the original covariance output by the satellite positioning module or simply switch weights based on the solution state, this application achieves a continuous and smooth transition of fusion strength without human intervention, effectively avoiding the erroneous influence of low-quality real-time dynamic differential positioning observations on system state estimation.

[0052] Secondly, this application comprehensively considers three factors: the real-time dynamic differential positioning solution state, the position covariance, and the position residual between the real-time dynamic differential positioning observation position and the local pose estimation result, to construct a positioning quality evaluation index, and calculates the real-time dynamic differential positioning weights through a continuous mapping function. Compared with the step switching method, this application makes the weights change continuously when the solution state changes from fixed solution state to floating solution state to single-point solution state, avoiding trajectory jumps and optimization oscillations caused by sudden changes in the constraint strength of real-time dynamic differential positioning.

[0053] Thirdly, this application scales the inverse of the position covariance based on the real-time dynamic differential positioning weights to generate a dynamic covariance matrix. When the real-time dynamic differential positioning quality is high, the weights are large, the scaling factor is small, the dynamic covariance matrix is ​​small, and the constraint strength of real-time dynamic differential positioning observations in factor graph optimization is large; when the real-time dynamic differential positioning quality is poor, the weights are small, the scaling factor is large, the dynamic covariance matrix is ​​large, and the constraint strength of real-time dynamic differential positioning observations in factor graph optimization is small.

[0054] Fourth, when real-time dynamic differential positioning recovers from a degraded solution state to a fixed solution state, this application does not directly add high-weight real-time dynamic differential positioning observations to the optimization. Instead, it gradually increases the weight of real-time dynamic differential positioning, so that the real-time dynamic differential positioning correction is smoothly absorbed. Compared with traditional methods, which may cause instantaneous trajectory jumps and optimization oscillations at the moment of recovery, this application ensures the smoothness and continuity of camera pose output and achieves seamless correction of visual inertial odometry cumulative drift.

[0055] Furthermore, this application provides centimeter-level global positioning accuracy by fixing the solution state through real-time dynamic differential positioning; the visual inertial odometry maintains continuous positioning during real-time dynamic differential positioning occlusion to avoid data interruption; real-time dynamic differential positioning global constraint correction of visual inertial odometry cumulative drift ensures trajectory convergence in the global coordinate system; and the fusion strategy is dynamically adjusted according to the real-time dynamic differential positioning signal quality to adapt to the strong occlusion environment of the orchard. Attached Figure Description

[0056] The above and / or additional aspects and advantages of this application will become apparent and readily understood from the following description of the embodiments taken in conjunction with the accompanying drawings, wherein:

[0057] Figure 1 This is an exemplary hardware architecture used in the positioning and mapping system for a heavily shaded orchard environment in the embodiments of this application;

[0058] Figure 2 This is a flowchart illustrating the method for locating and mapping a heavily obstructed orchard environment in an embodiment of this application.

[0059] Figure 3 This is a schematic diagram illustrating the process of constructing positioning quality evaluation indicators in the embodiments of this application;

[0060] Figure 4 This is a schematic diagram illustrating the process of correcting the drift accumulated by the visual inertial odometer in the obstructed area in an embodiment of this application;

[0061] Figure 5 This is a schematic diagram of the positioning and mapping device for a strongly obscured orchard environment in an embodiment of this application;

[0062] Figure 6This is a schematic diagram of the structure of the computer device in the embodiments of this application. Detailed Implementation

[0063] The embodiments of this application are described in detail below. Examples of these embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain this application, and should not be construed as limiting this application.

[0064] Those skilled in the art will understand that, unless specifically stated otherwise, the singular forms “a,” “an,” “the,” and “the” used herein may also include the plural forms. It should be further understood that the term “comprising” as used in this application means the presence of the stated features, integers, steps, operations, elements, and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof. It should be understood that when we say an element is “connected” or “coupled” to another element, it can be directly connected or coupled to the other element, or there may be intermediate elements. Furthermore, “connected” or “coupled” as used herein can include wireless connections or wireless coupling. The term “and / or” as used herein includes all or any units and all combinations of one or more associated listed items.

[0065] Those skilled in the art will understand that, unless otherwise defined, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains. It should also be understood that terms such as those defined in general dictionaries should be understood to have the same meaning as in the context of the prior art, and should not be interpreted in an idealized or overly formal sense unless specifically defined as herein.

[0066] Those skilled in the art will understand that the terms "client," "terminal," and "terminal device" as used herein include both devices that receive wireless signals, devices that only possess wireless signal receiver capabilities without transmission capabilities, and devices with receiving and transmitting hardware, devices that have receiving and transmitting hardware capable of bidirectional communication over a bidirectional communication link. Such devices may include: cellular or other communication devices such as personal computers or tablets, having single-line displays, multi-line displays, or cellular or other communication devices without multi-line displays; PCS (Personal Communications Service) that can combine voice, data processing, fax, and / or data communication capabilities; PDAs (Personal Digital Assistants) that may include radio frequency receivers, pagers, internet / intranet access, web browsers, notebooks, calendars, and / or GPS (Global Positioning System) receivers; and conventional laptops and / or handheld computers or other devices that have and / or include radio frequency receivers. As used herein, "client," "terminal," and "terminal device" can be portable, transportable, installed in a means of transportation (air, sea, and / or land), or suitable and / or configured to operate locally and / or in a distributed manner, operating in any other location on Earth and / or in space. "Client," "terminal," and "terminal device" as used herein can also be a communication terminal, an internet access terminal, or a music / video playback terminal, such as a PDA, a MID (Mobile Internet Device), and / or a mobile phone with music / video playback capabilities, or a smart TV, set-top box, etc.

[0067] The hardware referred to by the names "server," "client," and "service node" in this application is essentially an electronic device with the equivalent capabilities of a personal computer. It is a hardware device with the necessary components revealed by the von Neumann architecture, such as a central processing unit (including an arithmetic logic unit and a control unit), memory, input devices, and output devices. The computer program is stored in its memory, and the central processing unit loads the program stored in the secondary storage into the main memory to run it, execute the instructions in the program, and interact with the input and output devices to complete specific functions.

[0068] It should be noted that the concept of "server" used in this application can also be extended to the case of server clusters. Based on the network deployment principles understood by those skilled in the art, the servers should be logically divided. Physically, these servers can be independent of each other but accessible through interfaces, or they can be integrated into a single physical computer or a computer cluster. Those skilled in the art should understand this flexibility and should not use it to constrain the implementation of the network deployment method in this application.

[0069] One or more of the technical features of this application, unless explicitly specified herein, can be deployed on a server and accessed by a client remotely calling the online service interface provided by the server, or can be directly deployed and run on a client for access.

[0070] Unless otherwise specified, the neural network models referenced or potentially referenced in this application may be deployed on a remote server and invoked remotely on the client, or deployed on a client with the capability to invoke directly. In some embodiments, when running on the client, the corresponding intelligence may be acquired through transfer learning in order to reduce the requirements on the client's hardware resources and avoid excessive consumption of the client's hardware resources.

[0071] Unless otherwise specified, all data involved in this application may be stored remotely on a server or on a local terminal device, as long as it is suitable for use by the technical solution of this application.

[0072] Those skilled in the art will understand that although the various methods in this application are described based on the same concept and thus present commonality among them, they can be performed independently unless otherwise specified. Similarly, the various embodiments disclosed in this application are all based on the same inventive concept; therefore, concepts expressed in the same way, as well as concepts that are appropriately changed for convenience but are expressed differently, should be understood equivalently.

[0073] Unless otherwise expressly stated, the various embodiments disclosed in this application can be combined in a cross-cutting manner to flexibly construct new embodiments, as long as such combination does not depart from the inventive spirit of this application and can meet the needs of the prior art or solve a certain deficiency in the prior art. Those skilled in the art should be aware of such modifications.

[0074] Please see Figure 1 The location mapping method for heavily shaded orchard environments in this application can be based on, for example, Figure 1The positioning and mapping system for a heavily obstructed orchard environment is implemented. The hardware of the positioning and mapping system for a heavily obstructed orchard environment integrates a Global Navigation Satellite System (GNSS) positioning and timing module, a visual inertial sensing module including a binocular vision sensor and an inertial measurement unit, and a hardware synchronization trigger. It also integrates an anti-electromagnetic interference shielding structure and a standardized quick-release module.

[0075] In some embodiments, the hardware synchronization trigger synchronizes the visual and inertial sensors by parsing the Global Navigation Satellite System (GNSS) second pulse signal, achieving spatiotemporal alignment of multimodal heterogeneous data. The quick-release structure ensures efficient cross-platform reuse of the system. On the algorithm side, a loosely coupled framework between Visual Inertial Odometry (VIO) and GNSS is constructed based on an improved VINS-Fusion architecture. This framework innovatively introduces a dynamic confidence evaluation mechanism based on positioning covariance, quantifying GNSS positioning accuracy in real time and adaptively adjusting global optimization weights. This application can effectively suppress trajectory discontinuities caused by GNSS degradation and accumulated errors in Visual Inertial Odometry (VIO) in complex occlusion environments, improving global consistency, local continuity, and positioning robustness in large-scene data acquisition and pose estimation processes.

[0076] In some embodiments, the Global Navigation Satellite System (GNSS) positioning and timing module uses the u-blox ZED-F9P as the system's real-time dynamic differential positioning (RTK) GNSS receiver. This module supports multi-frequency, multi-constellation satellite signal reception and can be combined with an external 4G / LTE communication module to acquire RTCM differential correction data transmitted from the base station, achieving centimeter-level real-time dynamic differential positioning (RTK). Simultaneously, the ZED-F9P can output a high-precision pulse-per-second (PPS) signal, which serves as the system's global hardware time reference, driving subsequent synchronization triggers to achieve unified time alignment between the GNSS, visual sensors, and inertial measurement units. In this system, the GNSS positioning and timing module not only provides absolute geographic location but also provides a system-wide hardware time reference. This design enables the GNSS to simultaneously function as both a global positioning source and a global time reference source, providing a unified spatiotemporal reference for subsequent visual inertial odometry (VIO) / real-time dynamic differential positioning (RTK) data fusion.

[0077] In some embodiments, multi-sensor systems typically rely on their respective internal clocks for data acquisition. Each device's internal clock operates independently, resulting in frequency deviations and initial time differences. Visual inertial odometry (VIO) systems depend on the precise time correspondence between image frames and inertial measurement unit (IMU) measurements. Inputting data with completely different timestamps into the system will prevent it from fusing multi-source data. While software synchronization methods such as timestamp interpolation, interpolation, and buffer alignment can align the time of multi-source data at the algorithmic level, the measured data obtained by these methods comes from estimation processing rather than native measurements, thus introducing certain errors.

[0078] This application uses an STM32 microcontroller as the hardware synchronization trigger. This module receives the PPS synchronization signal from the Global Navigation Satellite System (GNSS) positioning and timing module. The STM32 controls the pulse frequency and pulse width of the output signal, and the peripheral circuitry implements pulse amplitude conversion, frequency division modulation, and level conversion. This trigger converts the periodic high-level square wave pulse PPS signal (pulse frequency 1Hz, pulse width 50μs, pulse amplitude 3.3V) input from the GNSS positioning and timing module into a low-level binocular camera pulse trigger signal (pulse period 0.5Hz, pulse width 50μs, pulse amplitude 1.8V), ensuring time synchronization of multiple sensors through hardware interrupts.

[0079] By employing a hardware-synchronized triggering method, the acquisition timing of multiple sensors is unified through an external trigger signal, achieving microsecond-level time alignment. This ensures that the acquisition data from the binocular vision sensor and inertial measurement unit (IMU) are aligned with the Global Navigation Satellite System (GNSS) time, and guarantees that the input values ​​to the system are all true measurements, improving accuracy while simplifying the algorithm. Furthermore, the synchronization signal can be used to achieve synchronized triggering when subsequently adding cameras, radar, and other multi-source sensors.

[0080] In some embodiments, this application uses an Intel RealSense D457 binocular camera with external hardware triggering capability as the system's visual inertial perception module. The camera integrates a binocular infrared vision sensor, an RGB image sensor, and an inertial measurement unit (IMU), and provides an external hardware synchronization interface. Unlike the normal mode, which relies on the camera's internal clock to autonomously output image frames, this application configures the D457 as a slave camera responding to external triggering. This allows it to receive pulse trigger signals from a hardware synchronization trigger and output binocular images and IMU information according to an external unified clock. The synchronization trigger signal is generated by the hardware synchronization trigger from the PPS signal output by the Global Navigation Satellite System (GNSS) positioning and timing module, thereby establishing a deterministic correspondence between the camera image acquisition time and the GNSS time reference.

[0081] In normal mode, when camera data arrives at ROS or the host computer, the timestamp may be affected by USB transmission, driver caching, operating system scheduling, and ROS message queues. Even if the camera's actual exposure time is relatively stable, the host computer's reception time may still fluctuate. The advantage of using external signals triggered from camera mode is that the acquisition action is determined by the external pulse at the sensor end, and the backend software is mainly responsible for reading and recording data, rather than determining the sampling time. The system maps the image data, inertial measurement unit (IMU) data, and real-time dynamic differential positioning (RTK) positioning data output by the D457 to a unified timeline at the host computer or fusion algorithm end, reducing data mismatches caused by internal clock drift, software timestamp jitter, and communication delays, and improving the robustness and accuracy of the visual inertial odometry (VIO) system.

[0082] In some embodiments, the high-precision Global Navigation Satellite System (GNSS) antenna is extremely sensitive to radio frequency noise. Furthermore, the visual-inertial sensing module, USB 3.0 high-speed data bus, 4G / LTE communication unit, hardware synchronization trigger, power lines, and multiple signal lines within the system may generate broadband electromagnetic harmonics or near-field radiated noise during operation, interfering with GNSS satellite signal reception and consequently affecting the acquisition and maintenance of the real-time dynamic differential positioning (RTK) fixed solution. To improve the system's robustness in complex electromagnetic environments and multi-platform environments, this application incorporates an integrated conductive shielding structure at the GNSS antenna base as an electromagnetic interference shielding structure. This structure can be made of aluminum foil, copper foil, metal plates, metal plating, or composite conductive materials and forms a stable electrical connection with the system structural components or grounding point. The conductive shielding structure is arranged between the GNSS antenna and the underlying electronic module to isolate and discharge near-field electromagnetic interference from the visual-inertial sensing module, communication module, USB data line, and power line.

[0083] In some embodiments, to enable rapid deployment and calibration reuse of the system across different mobile platforms, this application introduces a rigid quick-release mechanism based on the Arca-Swiss Standard as a standardized quick-release module at the bottom of the structural component. This quick-release module relies on a standardized dovetail groove structure design, using a 45° inclined plane to convert lateral clamping force into vertical compressive force, resulting in a high-rigidity, low-play mechanical connection after installation. This structure reduces rotational clearance and repetitive installation errors caused by traditional bolt installations or ordinary quick-release structures, offering engineering advantages such as high versatility, high system deployment efficiency, and rapid assembly and disassembly without auxiliary tools.

[0084] In some embodiments, before operating the positioning and mapping system in a heavily obscured orchard environment, the host computer of the mobile platform must first be connected via USB to the D457 binocular camera, the Global Navigation Satellite System (GNSS) module, and the synchronization trigger of the device to complete the initialization of each subsystem and establish data transmission links between the host computer and each sensor. After the system starts, the following process is executed sequentially:

[0085] The system first activates the Global Navigation Satellite System (GNSS) positioning node, configures the NTRIP account of the ZED-F9P module, and puts the receiver into Real-Time Dynamic Differential Positioning (RTK) mode. The receiver accesses the differential correction data service of RTK through its built-in 4G communication unit, receives RTCM data streams from the base station, and simultaneously begins searching and tracking multi-frequency, multi-constellation satellite signals until the system status bit detects that the positioning mode has entered a centimeter-level high-precision fixed solution state. After the GNSS module completes positioning convergence, it outputs a stable PPS signal. The hardware synchronization trigger uses this PPS signal as a time reference, and the STM32 microcontroller performs frequency division and level conversion, converting the 1Hz pulse frequency, 50μs pulse width, and 3.3V pulse amplitude PPS signal into a low-level trigger signal with a 0.5Hz pulse period, 50μs pulse width, and 1.8V pulse amplitude, establishing the global time reference for the entire system.

[0086] The host computer activates the binocular camera node and configures the Intel RealSense D457 camera in "slave" mode triggered by external hardware. It receives the synchronization trigger signal from the hardware synchronization trigger via the control interface. At the rising edge of the trigger level, the camera's underlying hardware forces the exposure of an image frame and simultaneously latches the motion attitude data of the internal six-axis inertial measurement unit (IMU), achieving absolute time axis alignment between visual and inertial data and Global Navigation Satellite System (GNSS) data.

[0087] Furthermore, the host computer synchronously receives three data streams from the Global Navigation Satellite System (GNSS) module, the hardware synchronization trigger, and the D457 binocular camera via a serial communication interface, and performs data parsing and topic publishing in the ROS node. While receiving GNSS positioning data, visual images, and IMU inertial data, the host computer simultaneously records the arrival time of the PPS trigger signal. By comparing the timestamps of each sensor data frame with the PPS trigger time, the time deviation of each device channel is calculated online. This deviation value is used as a compensation parameter for multi-source data alignment in the upper-layer fusion algorithm, ensuring accurate time synchronization of sensor data before entering the fusion framework from a software perspective.

[0088] The host computer simultaneously starts the multi-sensor adaptive fusion positioning algorithm node and enables the ROS topic data recording function. It stores the Global Navigation Satellite System (GNSS) positioning data, visual image data, inertial measurement unit (IMU) inertial data, and synchronization signal status in a complete time series as an offline data package for subsequent offline dense mapping and algorithm performance evaluation.

[0089] Furthermore, system initialization is performed in two layers:

[0090] The first layer is the visual-inertial odometry initialization. The system first constructs a visual-inertial odometry system using data from a binocular camera and an inertial measurement unit (IMU). The binocular camera establishes inter-frame feature matching relationships through visual feature tracking and recovers feature point depth through stereo matching, providing visual geometric constraints with known scale. The IMU provides motion increment constraints between adjacent keyframes through pre-integration. The algorithm incorporates the visual geometric constraints and IMU motion increment constraints into a sliding window optimization framework. Through nonlinear optimization, it completes IMU zero-bias estimation, gravity direction alignment, and initial local coordinate system establishment. It estimates the system's position, velocity, attitude, and IMU bias in the local coordinate system, providing the system with high-frequency, continuous, and short-term stable local motion estimation. This enables the system to maintain stable positioning even when GNSS signals are temporarily blocked or degraded.

[0091] The second layer is the alignment of the Global Navigation Satellite System (GNSS) global coordinate system. The system uses the position of the first real-time dynamic differential positioning (RTK) fixed solution as the global reference origin, and uses the pose graph optimization framework to calibrate the rotation-translation transformation parameters between the visual inertial odometry (VIO) local coordinate system and the GNSS global coordinate system online, thus completing the four-degree-of-freedom accurate registration of the local and global coordinate systems.

[0092] Furthermore, after initialization, the system enters an adaptive fusion operation state. It analyzes the solution state and position covariance carried in the Global Navigation Satellite System (GNSS) positioning data in real time, calculates the residual between the real-time dynamic differential positioning (RTK) observation position and the visual inertial odometry (VIO) local pose estimation result, and uses the solution state, position covariance, and residual as inputs to construct a positioning quality evaluation index. This positioning quality evaluation index calculates the real-time dynamic differential positioning weights of the RTK observations in factor graph optimization using a sigmoid continuous mapping function, and scales the inverse of the position covariance according to the real-time dynamic differential positioning weights to generate a dynamic covariance matrix. In the factor graph framework of global optimization of visual inertial odometry (VIO) and global navigation satellite system (GNSS), the dynamic covariance matrix is ​​used to weight the position residual of real-time dynamic differential positioning (RTK), and together with the visual reprojection residual and the inertial measurement unit (IMU) pre-integration residual, it constitutes a multi-source constrained joint optimization function. The global pose estimation result in the global coordinate system is obtained by solving the nonlinear optimization.

[0093] Simultaneously, the system monitors the real-time dynamic differential positioning (RTK) solution status in real time: when the covariance indicates high positioning accuracy, a global tight constraint is applied to the Global Navigation Satellite System (GNSS) factor to correct the visual inertial odometry (VIO) cumulative drift; when the real-time dynamic differential positioning (RTK) degrades to a floating solution, a single-point solution, or an invalid solution, the GNSS constraint at that moment is automatically released or significantly weakened, allowing the positioning system to seamlessly degrade to the VIO-dominated mode; when the mobile platform leaves the fruit tree obstruction area, and the real-time dynamic differential positioning (RTK) recovers from the degraded solution state to the fixed solution state, the system gradually increases the real-time dynamic differential positioning weight through a recursive smooth update method, so that the real-time dynamic differential positioning (RTK) correction is gradually absorbed in multiple optimization windows, avoiding trajectory jumps and achieving seamless correction of VIO cumulative drift.

[0094] During system operation, RGB image data from the binocular camera, Global Navigation Satellite System (GNSS) positioning data, and pose information output by the fusion algorithm are recorded synchronously through rosbag. All of the above data carry a unified timestamp, forming a spatiotemporally aligned multimodal data sequence.

[0095] Furthermore, after the acquisition is completed, the original images, inertial measurement unit (IMU) data, real-time dynamic differential positioning (RTK)-Global Navigation Satellite System (GNSS) data, and fused pose can be exported to the server as input for subsequent 3D reconstruction, point cloud stitching, or multi-source remote sensing data registration.

[0096] Based on the above exemplary scenario, please refer to Figure 2 In one embodiment of the method for locating and mapping a heavily shaded orchard environment, this application includes:

[0097] Step S10: Acquire multi-source sensor data during the movement of the mobile platform in the orchard environment, wherein the multi-source sensor data includes at least binocular images collected by the binocular vision sensor mounted on the mobile platform, inertial measurement unit data collected by the inertial measurement unit, and real-time dynamic differential positioning data output by the satellite positioning module.

[0098] The positioning and mapping system for a heavily obstructed orchard environment in the terminal device can acquire multi-source sensor data during the movement of the mobile platform in the orchard environment. The multi-source sensor data includes at least binocular images collected by a binocular vision sensor mounted on the mobile platform, inertial measurement unit data collected by an inertial measurement unit, and real-time dynamic differential positioning data output by a satellite positioning module.

[0099] In some embodiments, the binocular images and the inertial measurement unit data are synchronously triggered for acquisition after being converted by a synchronization trigger from the second pulse signal output by the satellite positioning module, specifically including:

[0100] Step S101: The satellite positioning module outputs a second pulse signal with a period of 1 second, a pulse width of 50 microseconds, and an amplitude of 3.3 volts;

[0101] Step S102: The synchronous trigger receives the second pulse signal, modulates it into a trigger beat with a period of 0.5 seconds by the microcontroller, and adjusts the pulse amplitude to a trigger level that is compatible with the binocular vision sensor and the inertial measurement unit by level conversion;

[0102] Step S103: The binocular vision sensor and the inertial measurement unit synchronously perform image exposure and inertial data latching on the rising edge of the trigger signal, so as to establish a deterministic time correspondence between the acquisition time and the second pulse signal.

[0103] Step S20: Construct a visual inertial odometry based on the binocular images and the inertial measurement unit data to obtain the local pose estimation result of the mobile platform in the local coordinate system;

[0104] After acquiring multi-source sensor data during the movement of the mobile platform in the orchard environment, a visual inertial odometry is constructed based on the binocular images and the inertial measurement unit data to obtain the local pose estimation result of the mobile platform in the local coordinate system.

[0105] In some embodiments, the step of constructing a visual inertial odometry based on the binocular images and the inertial measurement unit data to obtain the local pose estimation result of the mobile platform in the local coordinate system includes:

[0106] Step S201: Perform visual feature extraction and tracking on the binocular images to obtain feature matching relationships between adjacent frames;

[0107] Step S202: Perform stereo matching on the binocular images to obtain visual geometric constraints with known scale;

[0108] Step S203: Pre-integrate the inertial measurement unit data to obtain motion increment constraints between adjacent keyframes;

[0109] Step S204: Add the visual geometric constraints and the motion increment constraints to the sliding window optimization framework, and estimate the local pose estimation results of the mobile platform in the local coordinate system, including position, velocity, attitude, accelerometer bias and gyroscope bias, through nonlinear optimization.

[0110] Specifically, the system first constructs a visual-inertial odometry using data from a binocular camera and an inertial measurement unit (IMU). The binocular camera provides scale-known visual geometric constraints, while the IMU provides high-frequency motion prediction and short-term attitude constraints. The algorithm estimates the system's local pose in the local coordinate system, including position, velocity, attitude, accelerometer bias, and gyroscope bias, through visual feature tracking, binocular stereo matching, IMU pre-integration, and sliding window nonlinear optimization.

[0111] The local state of a visual inertial odometry (VIO) system can be represented as:

[0112]

[0113] in, Indicates the first Location at any given moment Indicates speed, Represents attitude quaternions, and These represent the accelerometer bias and the gyroscope bias, respectively.

[0114] The main function of this stage is to provide the system with high-frequency, continuous, and short-term stable local motion estimation, so that the system can maintain stable positioning even when the Global Navigation Satellite System (GNSS) signal is blocked or degraded for a short time.

[0115] Step S30: Based on the solution state, position covariance, and position residual between the real-time dynamic differential positioning observation position and the local pose estimation result in the real-time dynamic differential positioning data, construct a positioning quality evaluation index; based on the positioning quality evaluation index, calculate the real-time dynamic differential positioning weight in factor graph optimization through a continuous mapping function, and scale the position covariance according to the real-time dynamic differential positioning weight to generate a dynamic covariance matrix;

[0116] After constructing a visual inertial odometry system based on the binocular images and the inertial measurement unit data to obtain the local pose estimation result of the mobile platform in the local coordinate system, a positioning quality evaluation index is constructed based on the solution state, position covariance, and position residual between the real-time dynamic differential positioning observation position and the local pose estimation result in the real-time dynamic differential positioning data. Based on the positioning quality evaluation index, the real-time dynamic differential positioning weight in factor graph optimization is calculated through a continuous mapping function, and the position covariance is scaled according to the real-time dynamic differential positioning weight to generate a dynamic covariance matrix.

[0117] In some embodiments, please refer to Figure 3The step of constructing a positioning quality evaluation index based on the solution state, position covariance, and position residual between the real-time dynamic differential positioning observation position and the local pose estimation result in the real-time dynamic differential positioning data includes:

[0118] Step S31: Obtain the solution status in the real-time dynamic differential positioning data, quantize and assign values ​​to the solution status, and generate a state quantization value. The solution status includes a fixed solution status, a floating solution status, a single-point solution status, and an invalid solution status.

[0119] Step S32: Obtain the position covariance matrix in the real-time dynamic differential positioning data, and calculate the trace of the position covariance matrix as a quantification of positioning uncertainty.

[0120] Step S33: Obtain the real-time dynamic differential positioning observation position and the local pose estimation result, calculate the position residual of the real-time dynamic differential positioning observation position relative to the local pose estimation result, and calculate the norm of the position residual as a quantification value of observation consistency.

[0121] Step S34: Using the state quantization value, the positioning uncertainty quantization value, and the observation consistency quantization value as inputs, calculate the positioning quality evaluation index according to a preset comprehensive evaluation function. The positioning quality evaluation index is used to characterize the reliability of the real-time dynamic differential positioning observation at the current moment. The higher the state quantization value, the lower the positioning uncertainty quantization value, and the lower the observation consistency quantization value, the higher the positioning quality evaluation index.

[0122] Specifically, the system reads the solution status, position covariance matrix, and real-time dynamic differential positioning observation position from the real-time dynamic differential positioning data in real time, and calculates the positioning observation residual by combining it with the current local pose estimation result. The system calculates the position residual between the real-time dynamic differential positioning observation position and the local pose estimation result, which is expressed as:

[0123]

[0124] in, This indicates the real-time dynamic differential positioning observation position. This represents the position component in the local pose estimation result after coordinate system alignment. This represents the position residual between the real-time dynamic differential positioning observation position and the local pose estimation result.

[0125] Positioning quality evaluation indicators can be expressed as:

[0126]

[0127] in, This indicates the solution status, including fixed solution status, floating solution status, single-point solution status, and invalid solution status. The position covariance matrix represents the quantified value of the positioning uncertainty output by the satellite positioning module; This represents the residual between the real-time dynamic differential positioning observation position and the local pose estimation result.

[0128] Furthermore, the system uses the above information as a basis for judgment:

[0129] When in a fixed solution state, the current real-time dynamic differential positioning has centimeter-level high positioning accuracy; when in a floating solution state, the positioning accuracy is moderate; when in a single-point solution state or an invalid solution state, the reliability of real-time dynamic differential positioning observation is low.

[0130] The system reads the quantitative value of positioning uncertainty output by the satellite positioning module. When the position covariance is small, it indicates that the current real-time dynamic differential positioning observation uncertainty is low; when the position covariance is large, it indicates that the current real-time dynamic differential positioning observation uncertainty is high.

[0131] The system calculates the position residual between the real-time dynamic differential positioning observation position and the local pose estimation result. When the residual is less than the preset threshold, it indicates that the real-time dynamic differential positioning observation and the visual inertial odometry estimation results are in good agreement; when the residual is greater than the preset threshold, it indicates that the real-time dynamic differential positioning observation may have abnormal jumps or be subject to interference.

[0132] In a further embodiment, the step of calculating the real-time dynamic differential positioning weights of real-time dynamic differential positioning observations in factor graph optimization using a continuous mapping function includes:

[0133] Step S301: Obtain the positioning quality evaluation index at the current moment;

[0134] Step S302: Using the positioning quality evaluation index as an input variable, calculate the real-time dynamic differential positioning weight at the current moment through a monotonically increasing continuous mapping function, wherein the real-time dynamic differential positioning weight continuously increases as the positioning quality evaluation index increases and continuously decreases as the positioning quality evaluation index decreases.

[0135] Step S303: The output range of the continuous mapping function is limited to a preset interval, and the continuous mapping function is continuously differentiable within its domain, so that the real-time dynamic differential positioning weights transition continuously with the changes in the positioning quality evaluation index.

[0136] Specifically, based on the positioning quality evaluation index output in step S34, the system constructs a Global Navigation Satellite System (GNSS) real-time dynamic differential positioning weight allocation function, mapping the real-time dynamic differential positioning (RTK) reliability to real-time dynamic differential positioning weights, i.e.:

[0137]

[0138] in, This represents the weight of real-time dynamic differential positioning observations in factor graph optimization. When the comprehensive evaluation index is high, Taking a larger value results in stronger constraints from real-time dynamic differential positioning observations during optimization; however, when the comprehensive evaluation index is low... Taking a smaller value results in a lower constraint strength for real-time dynamic differential positioning observations during optimization.

[0139] To achieve a smooth mapping from comprehensive evaluation indicators to real-time dynamic differential positioning weights, the system employs a sigmoid continuous mapping function, which is expressed as:

[0140]

[0141] in, This indicates the slope of the weight change, controlling the response speed of the real-time dynamic differential positioning weight when the comprehensive evaluation index changes; This represents the quality threshold, which determines the intermediate dividing point of the comprehensive evaluation index. When the comprehensive evaluation index is higher than the threshold, the real-time dynamic differential positioning weight continuously moves towards the high value area; when the comprehensive evaluation index is lower than the threshold, the real-time dynamic differential positioning weight continuously moves towards the low value area.

[0142] This continuous mapping function has the following properties:

[0143] Monotonically increasing: The weight of real-time dynamic differential positioning increases continuously as the comprehensive evaluation index increases and decreases continuously as the comprehensive evaluation index decreases, ensuring that the higher the comprehensive evaluation index, the greater the constraint weight.

[0144] Output range limitation: The function output range is limited to the interval [0,1] or (0,1) to ensure that the real-time dynamic differential positioning weights are always within a reasonable range of constraint strength.

[0145] Continuous differentiability: The function is continuously differentiable everywhere in its domain, so that the weight of real-time dynamic differential positioning changes continuously with the changes of the comprehensive evaluation index.

[0146] Based on the aforementioned continuous mapping characteristics, when the real-time dynamic differential positioning (RTK) solution state switches between fixed solution, floating solution, single-point solution and invalid solution, the real-time dynamic differential positioning weights will not change abruptly, but will transition smoothly, thereby avoiding trajectory jumps and optimization oscillations caused by abrupt changes in the constraint strength of the Global Navigation Satellite System (GNSS).

[0147] In a further embodiment, the step of scaling the position covariance according to the real-time dynamic differential positioning weights to generate a dynamic covariance matrix includes:

[0148] Step S3001: Obtain the real-time dynamic differential positioning weights at the current moment;

[0149] Step S3002: Using the sum of the real-time dynamic differential positioning weight and the value of the fractional division to prevent division by zero as the denominator, calculate its corresponding reciprocal to obtain the scaling factor;

[0150] Step S3003: Multiply the original position covariance matrix in the real-time dynamic differential positioning data by the scaling factor to generate a dynamic covariance matrix. The larger the weight of the real-time dynamic differential positioning, the smaller the scaling factor, the smaller the dynamic covariance matrix, and the stronger the constraint strength of the real-time dynamic differential positioning observation in factor graph optimization. Conversely, the smaller the weight of the real-time dynamic differential positioning, the larger the scaling factor, the larger the dynamic covariance matrix, and the weaker the constraint strength of the real-time dynamic differential positioning observation in factor graph optimization.

[0151] Specifically, the system uses the real-time dynamic differential positioning weights output in step S302. The original position covariance matrix is ​​adaptively scaled to generate a dynamic covariance matrix for the injection factor graph optimization framework.

[0152] The Global Navigation Satellite System (GNSS) real-time dynamic differential positioning (RTK) weight allocation function maps RTK reliability to RTK weights. The system then further generates an optimized dynamic covariance matrix based on these weights, expressed as follows:

[0153]

[0154] in, This is expressed as a scaling factor; The dynamic covariance matrix injected into the optimizer, The original position covariance matrix of the receiver. To prevent small values ​​from being divided by zero, the value is set to an extremely small positive number, used only for weighting in real-time dynamic differential positioning. This prevents the denominator from being zero, which could lead to numerical overflow.

[0155] As shown in the above calculation formula, the weights of real-time dynamic differential positioning (RTK) are inversely proportional to the covariance matrix. When the quality of real-time dynamic differential positioning (RTK) is high, the weights of real-time dynamic differential positioning... Take the larger value, scaling factor Smaller, resulting in a smaller dynamic covariance matrix. The constraints on real-time dynamic differential positioning (RTK) observations are relatively strong when the quality of RTK is poor; however, the RTK weights are relatively small when the quality of RTK is low. Taking a smaller value results in a larger scaling factor, leading to a larger dynamic covariance matrix and weaker constraint strength of real-time dynamic differential positioning observations in factor graph optimization.

[0156] Furthermore, use This represents the dynamic information matrix of the Global Navigation Satellite System (GNSS) position factors in the optimization problem. Its magnitude reflects the strength of the constraint imposed by GNSS observations on the state estimation, i.e.:

[0157]

[0158] Among them, dynamic information matrix With dynamic covariance matrix They are inverse matrices, when the dynamic covariance matrix is... When the dynamic information matrix is ​​small, A larger dynamic differential positioning observation results in a stronger constraint on optimization; conversely, a smaller dynamic covariance matrix results in a weaker constraint. When it is large, the dynamic information matrix The constraints on real-time dynamic differential positioning observations during optimization are relatively small.

[0159] Step S40: The real-time dynamic differential positioning observation position is added as a global constraint to the factor graph optimization framework and optimized together with the residual of the visual inertial odometry to obtain the global pose estimation result of the mobile platform in the global coordinate system.

[0160] Based on the positioning quality evaluation index, the real-time dynamic differential positioning weights of the real-time dynamic differential positioning observations in the factor graph optimization are calculated through a continuous mapping function. The position covariance is then scaled according to the real-time dynamic differential positioning weights to generate a dynamic covariance matrix. The real-time dynamic differential positioning observation positions are then added as global constraints to the factor graph optimization framework and optimized together with the residuals of the visual inertial odometry to obtain the global pose estimation results of the mobile platform in the global coordinate system.

[0161] In some embodiments, the step of incorporating the real-time dynamic differential positioning observation position as a global constraint into the factor graph optimization framework, and jointly optimizing it with the visual inertial odometry residuals to obtain the global pose estimation result of the mobile platform in the global coordinate system includes:

[0162] Step S401: Construct visual reprojection residual, wherein the visual reprojection residual represents the difference between the observed position of a feature point in the binocular image and the predicted position of the feature point projected onto the image plane according to the current estimated pose;

[0163] In visual inertial odometry, the system extracts visual feature points from binocular images and reconstructs their 3D spatial positions using triangulation. For 3D feature points in a pre-built map, the system reprojects them onto the image plane based on the currently estimated camera pose, obtaining predicted pixel coordinates. The deviation between these predicted coordinates and the actual observed pixel coordinates of the feature points is the visual reprojection residual.

[0164] When pose estimation is accurate, the reprojection residual is small; when pose estimation has biases, the reprojection residual will increase accordingly. This residual is weighted by the visual observation covariance matrix and used to constrain the pose estimation of the mobile platform.

[0165] Step S402: Construct the inertial measurement pre-integration residual, wherein the inertial measurement pre-integration residual represents the difference between the relative motion increment obtained by pre-integration of inertial measurement data between adjacent keyframes and the relative motion increment calculated by estimated pose;

[0166] The system pre-integrates all inertial measurement unit (IMU) data between two keyframes, compressing it into a relative motion increment constraint that includes relative rotation, velocity increment, and position increment. Simultaneously, it calculates the relative motion increment between adjacent keyframes based on the currently estimated pose state. The deviation between these two values ​​is the IMU pre-integration residual.

[0167] The residuals are weighted using a pre-integrated covariance matrix to constrain the velocity, attitude, and inertial measurement unit (IMU) bias estimation of the mobile platform. The residuals are small when the IMU data is accurate; they increase when the IMU bias estimation is inaccurate or when the motion is violent.

[0168] Step S403: Construct the real-time dynamic differential positioning position residual, wherein the real-time dynamic differential positioning position residual represents the difference between the real-time dynamic differential positioning observation position and the visual inertial odometry predicted position;

[0169] The system compares the real-time dynamic differential positioning (RTK) observation position with the visual inertial odometry (VIO) predicted position at the current moment, and calculates the three-dimensional position deviation between the two, i.e.:

[0170]

[0171] in, This indicates the real-time dynamic differential positioning observation position. This represents the position component in the local pose estimation result after coordinate system alignment. This represents the position residual between the real-time dynamic differential positioning observation position and the local pose estimation result.

[0172] When the real-time dynamic differential positioning (RTK) is accurate and the visual inertial odometry (VIO) does not drift, the residual is small; when the real-time dynamic differential positioning (RTK) is affected by multipath effects and the position changes, or when the visual inertial odometry (VIO) accumulates drift over a long period of time, the residual will increase accordingly.

[0173] The residuals are weighted by a dynamic covariance matrix. The dynamic covariance matrix is ​​generated in real time by step S3003 based on the positioning quality evaluation index: when the positioning quality is good, the covariance is small and the weight is high; when the positioning quality is poor, the covariance is large and the weight is low.

[0174] Step S404: Construct a multi-source constraint joint optimization function, wherein the multi-source constraint joint optimization function is the weighted sum of squares of the visual reprojection residual, the inertial measurement pre-integration residual, and the real-time dynamic differential positioning position residual;

[0175] The multi-source constrained joint optimization function is the weighted sum of squares of the visual reprojection residual, the inertial measurement pre-integration residual, and the real-time dynamic differential positioning position residual, expressed as:

[0176]

[0177] The weights of the visual reprojection residual, the inertial measurement pre-integration residual, and the real-time dynamic differential positioning (RTK) position residual are independent of each other and do not affect each other. The weights of the visual and inertial measurement units (IMUs) are determined by the noise characteristics of the sensors themselves and are basically fixed after system calibration; only the weight of the real-time dynamic differential positioning (RTK) is dynamically changing and determined by the real-time positioning quality. This design allows the system to fully utilize global constraints to correct drift when the RTK signal is good, and automatically reduce its impact when the RTK signal deteriorates, avoiding low-quality observations from contaminating the estimation results.

[0178] Through this optimization model, the system can automatically adjust the Global Navigation Satellite System (GNSS) constraint strength according to the positioning quality evaluation index. When the real-time dynamic differential positioning (RTK) accuracy is high, the GNSS constraint is strengthened to correct the visual inertial odometry (VIO) cumulative drift; when the positioning quality evaluation index decreases, the GNSS constraint is weakened, and the system mainly relies on the visual inertial odometry (VIO) to maintain the continuity of local motion.

[0179] Step S405: Perform nonlinear optimization on the multi-source constraint joint optimization function to obtain the global pose estimation result in the global coordinate system.

[0180] The system performs nonlinear optimization on the multi-source constraint joint optimization function constructed in step S404. The optimization variables are the state vectors of all key frames within the sliding window, including position, velocity, attitude, accelerometer bias, and gyroscope bias.

[0181] By minimizing the weighted sum of squares of the three types of residuals, the optimizer jointly optimizes the visual reprojection constraint, the inertial measurement unit (IMU) pre-integration constraint, and the real-time dynamic differential localization constraint. The visual reprojection constraint mainly relates to the relative pose between adjacent frames, the IMU pre-integration constraint mainly relates to the motion coherence between adjacent keyframes, and the real-time dynamic differential localization constraint provides a global absolute position reference. These three types of constraints mutually restrain each other during the optimization process, ultimately yielding a set of optimal state estimates that minimize the overall weighted sum of squares of all observation residuals within the sliding window, thus obtaining a convergent global pose estimation result in the global coordinate system.

[0182] Step S50: When the mobile platform moves out of the fruit tree shading area, when the real-time dynamic differential positioning solution state recovers from the degraded solution state to the fixed solution state, the real-time dynamic differential positioning weight is gradually increased so that the real-time dynamic differential positioning correction amount is smoothly absorbed, so as to correct the drift accumulated by the visual inertial odometry in the shading area.

[0183] The real-time dynamic differential positioning observation position is added as a global constraint to the factor graph optimization framework and optimized together with the residual of the visual inertial odometry. After obtaining the global pose estimation result of the mobile platform in the global coordinate system, when the mobile platform leaves the fruit tree occlusion area, as the solution state of the real-time dynamic differential positioning recovers from the degraded solution state to the fixed solution state, the weight of the real-time dynamic differential positioning is gradually increased so that the real-time dynamic differential positioning correction is smoothly absorbed, thereby correcting the drift accumulated by the visual inertial odometry in the occlusion area.

[0184] In some embodiments, please refer to Figure 4When the mobile platform leaves the area obscured by fruit trees, and the real-time dynamic differential positioning solution returns from a degraded solution state to a fixed solution state, the real-time dynamic differential positioning weight is gradually increased to smoothly absorb the real-time dynamic differential positioning correction amount, thereby correcting the drift accumulated by the visual inertial odometry within the obscured area. This step includes:

[0185] Step S501: Obtain the positioning quality evaluation index at the current moment, and calculate the target weight at the current moment based on the positioning quality evaluation index. The deterioration solution state includes floating solution state, single-point solution state and invalid solution state.

[0186] Step S502: Obtain the actual weight of the previous time step, and use the weighted average of the actual weight of the previous time step and the target weight of the current time step as the actual weight of the current time step. The actual weight of the previous time step is assigned a first smoothing factor, and the target weight of the current time step is assigned a second smoothing factor. The sum of the first smoothing factor and the second smoothing factor is 1.

[0187] Step S503: During the period of the fixed solution state, the above steps are repeated so that the actual weight gradually approaches the target weight from the value of the previous moment, and the real-time dynamic differential positioning correction is gradually absorbed in multiple optimization windows as the actual weight gradually increases.

[0188] Step S504: When the actual weight reaches the target weight, the real-time dynamic differential positioning observation is added to the factor graph optimization with the highest constraint strength to correct the drift accumulated by the visual inertial odometry in the occluded area.

[0189] Specifically, in an orchard environment, real-time dynamic differential positioning (RTD) may degrade from a fixed solution state to a floating solution state, a single-point solution state, or an invalid solution state due to occlusion by tree canopy, branches, or leaves. When the mobile platform leaves the occluded area, RTD may revert to a fixed solution state. If the RTD observation position factor is immediately set to the highest weight at the moment of recovery, the visual inertial odometry (VIO) trajectory may be momentarily pulled towards the RTD observation position, resulting in trajectory jumps or backend optimization oscillations.

[0190] To address the aforementioned issues, this application employs a smooth weight recovery mechanism during the real-time dynamic differential positioning recovery phase. When the real-time dynamic differential positioning state recovers from a degraded solution state to a fixed solution state, the system does not immediately adopt the highest constraint weight. Instead, it gradually increases the weight of the real-time dynamic differential positioning position factor over multiple consecutive optimization windows, allowing the position residual between the visual inertial odometry (VIO) and real-time dynamic differential positioning to be gradually absorbed by the back-end optimization.

[0191] The real-time dynamic differential positioning weights are updated using a recursive smoothing method, and are expressed as follows:

[0192]

[0193] in, This represents the real-time dynamic differential positioning weights at the current moment. This represents the real-time dynamic differential positioning weights from the previous moment. Indicates the smoothing factor; This represents the target weight obtained based on the current real-time dynamic differential positioning quality evaluation.

[0194] When the real-time dynamic differential positioning just returns to a fixed solution state, the system increases the real-time dynamic differential positioning weights by a small amount. This gradually strengthens the real-time dynamic differential positioning constraints; when the fixed solution state remains stable... By gradually approaching the target weight, the real-time dynamic differential positioning observation can fully correct the accumulated drift of the visual inertial odometry (VIO). This mechanism does not directly replace the VIO pose with the real-time dynamic differential positioning position, but rather gradually increases the weight of the real-time dynamic differential positioning position factor in the back-end optimization. This allows the positional deviation between the VIO and the real-time dynamic differential positioning to be continuously absorbed in multiple sliding windows, thereby maintaining the smoothness and continuity of the camera pose output.

[0195] As can be seen from the above embodiments, compared with the prior art, this application addresses the problem that the prior art is unable to simultaneously meet the requirements of high-precision synchronous acquisition and continuous positioning in orchard scenarios, which require high precision, continuity, low drift, and global coordinate consistency. This application has, but is not limited to, the following beneficial effects:

[0196] Firstly, this application constructs an integrated dynamic weight evaluation function based on three factors: the real-time dynamic differential positioning solution state, the position covariance, and the position residual between the real-time dynamic differential positioning observation position and the local pose estimation result. This function allows the real-time dynamic differential positioning constraint strength to be adjusted in real time according to the actual positioning quality. When the real-time dynamic differential positioning accuracy is high, the system enhances global constraints to correct the cumulative drift of the visual inertial odometry (VIO). When the real-time dynamic differential positioning signal deteriorates, the system reduces the constraint weights and degenerates into VIO-dominated positioning. Compared to traditional methods that directly use the original covariance output by the satellite positioning module or simply switch weights based on the solution state, this application achieves a continuous and smooth transition of fusion strength without human intervention, effectively avoiding the erroneous influence of low-quality real-time dynamic differential positioning observations on system state estimation.

[0197] Secondly, this application comprehensively considers three factors: the real-time dynamic differential positioning solution state, the position covariance, and the position residual between the real-time dynamic differential positioning observation position and the local pose estimation result, to construct a positioning quality evaluation index, and calculates the real-time dynamic differential positioning weights through a continuous mapping function. Compared with the step switching method, this application makes the weights change continuously when the solution state changes from fixed solution state to floating solution state to single-point solution state, avoiding trajectory jumps and optimization oscillations caused by sudden changes in the constraint strength of real-time dynamic differential positioning.

[0198] Thirdly, this application scales the inverse of the position covariance based on the real-time dynamic differential positioning weights to generate a dynamic covariance matrix. When the real-time dynamic differential positioning quality is high, the weights are large, the scaling factor is small, the dynamic covariance matrix is ​​small, and the constraint strength of real-time dynamic differential positioning observations in factor graph optimization is large; when the real-time dynamic differential positioning quality is poor, the weights are small, the scaling factor is large, the dynamic covariance matrix is ​​large, and the constraint strength of real-time dynamic differential positioning observations in factor graph optimization is small.

[0199] Fourth, when real-time dynamic differential positioning recovers from a degraded solution state to a fixed solution state, this application does not directly add high-weight real-time dynamic differential positioning observations to the optimization. Instead, it gradually increases the weight of real-time dynamic differential positioning, so that the real-time dynamic differential positioning correction is smoothly absorbed. Compared with traditional methods, which may cause instantaneous trajectory jumps and optimization oscillations at the moment of recovery, this application ensures the smoothness and continuity of camera pose output and achieves seamless correction of visual inertial odometry cumulative drift.

[0200] Furthermore, this application provides centimeter-level global positioning accuracy by fixing the solution state through real-time dynamic differential positioning; the visual inertial odometry maintains continuous positioning during real-time dynamic differential positioning occlusion to avoid data interruption; real-time dynamic differential positioning global constraint correction of visual inertial odometry cumulative drift ensures trajectory convergence in the global coordinate system; and the fusion strategy is dynamically adjusted according to the real-time dynamic differential positioning signal quality to adapt to the strong occlusion environment of the orchard.

[0201] Please see Figure 5A positioning and mapping device for a heavily obscured orchard environment, provided to meet one of the purposes of this application, includes a data synchronization acquisition module 1100, a visual inertial estimation module 1200, a positioning weight generation module 1300, a joint optimization solution module 1400, and a progressive drift correction module 1500. The data synchronization acquisition module 1100 is configured to acquire multi-source sensor data during the movement of the mobile platform in the orchard environment. The multi-source sensor data includes at least binocular images acquired by a binocular vision sensor mounted on the mobile platform, inertial measurement unit (IMU) data acquired by an IMU, and real-time dynamic differential positioning data output by a satellite positioning module. The visual-inertial estimation module 1200 is configured to construct a visual-inertial odometry based on the binocular images and the IMU data to obtain a local pose estimation result of the mobile platform in a local coordinate system. The positioning weight generation module 1300 is configured to construct a positioning quality evaluation index based on the solution state, position covariance, and position residual between the real-time dynamic differential positioning observation position and the local pose estimation result in the real-time dynamic differential positioning data; and to generate a positioning quality evaluation index based on the positioning quality evaluation index. The system employs a multi-level optimization (MLEM) module 1400 to calculate the real-time dynamic differential positioning weights of the real-time dynamic differential positioning observations in the factor graph optimization using a continuous mapping function. The module then scales the position covariance based on these weights to generate a dynamic covariance matrix. A joint optimization solution module 1400 incorporates the real-time dynamic differential positioning observation positions as global constraints into the factor graph optimization framework, optimizing them together with the residuals of the visual inertial odometry (VIO) to obtain the global pose estimation result of the mobile platform in the global coordinate system. A progressive drift correction module 1500 is configured to gradually increase the real-time dynamic differential positioning weights as the solution state of the real-time dynamic differential positioning recovers from a degraded solution state to a fixed solution state after the mobile platform leaves the fruit tree occlusion area. This allows the real-time dynamic differential positioning correction amount to be smoothly absorbed, correcting the drift accumulated by the VIO in the occlusion area.

[0202] Based on any embodiment of this application, please refer to Figure 6 Another embodiment of this application also provides an electronic device, which can be implemented by a computer device, such as... Figure 6The diagram shows the internal structure of a computer device. This computer device includes a processor, a computer-readable storage medium, a memory, and a network interface connected via a system bus. The computer-readable storage medium stores an operating system, a database, and computer-readable instructions. The database may store control information sequences. When executed by the processor, the computer-readable instructions enable the processor to implement a method for locating and mapping a heavily shaded orchard environment. The processor provides computational and control capabilities, supporting the operation of the entire computer device. The memory stores computer-readable instructions, which, when executed by the processor, enable the processor to execute the method for locating and mapping a heavily shaded orchard environment as described in this application. The network interface of the computer device is used for communication with a terminal. Those skilled in the art will understand that… Figure 6 The structure shown is merely a block diagram of a portion of the structure related to the present application and does not constitute a limitation on the computer device to which the present application is applied. Specific computer devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0203] In this embodiment, the processor is used to execute... Figure 5 The specific functions of each module are described, and the memory stores the program code and various data required to execute these modules. The network interface is used for data transmission between the user terminal and the server. In this embodiment, the memory stores the program code and data required to execute all modules in the positioning and mapping device for heavily obstructed orchard environments of this application. The server can call the server's program code and data to execute the functions of all modules.

[0204] This application also provides a storage medium storing computer-readable instructions, which, when executed by one or more processors, cause the one or more processors to perform the steps of the location mapping method for a heavily shaded orchard environment as described in any embodiment of this application.

[0205] This application also provides a computer program product, including a computer program / instructions that, when executed by one or more processors, implement the steps of the location mapping method for a heavily shaded orchard environment as described in any embodiment of this application.

[0206] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments of this application can be implemented by a computer program instructing related hardware. This computer program can be stored in a computer-readable storage medium, and when executed, it can include the processes of the embodiments of the methods described above. The aforementioned storage medium can be a magnetic disk, optical disk, read-only memory (ROM), or random access memory (RAM), etc.

[0207] The above description is only a partial embodiment of this application. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of this application, and these improvements and modifications should also be considered within the scope of protection of this application.

Claims

1. A method for locating and mapping a heavily obscured orchard environment, characterized in that, include: The mobile platform acquires multi-source sensor data during its movement in the orchard environment. The multi-source sensor data includes at least binocular images acquired by a binocular vision sensor mounted on the mobile platform, inertial measurement unit data acquired by an inertial measurement unit, and real-time dynamic differential positioning data output by a satellite positioning module. Visual inertial odometry is constructed based on the binocular images and the inertial measurement unit data to obtain the local pose estimation result of the mobile platform in the local coordinate system; Based on the solution state, position covariance, and position residual between the real-time dynamic differential positioning observation position and the local pose estimation result in the real-time dynamic differential positioning data, a positioning quality evaluation index is constructed. Based on the positioning quality evaluation index, the real-time dynamic differential positioning weight of the real-time dynamic differential positioning observation in the factor graph optimization is calculated through a continuous mapping function, and the position covariance is scaled according to the real-time dynamic differential positioning weight to generate a dynamic covariance matrix. The real-time dynamic differential positioning observation position is added as a global constraint to the factor graph optimization framework and optimized together with the residual of the visual inertial odometry to obtain the global pose estimation result of the mobile platform in the global coordinate system. When the mobile platform moves out of the fruit tree shading area, as the real-time dynamic differential positioning solution state recovers from the degraded solution state to the fixed solution state, the real-time dynamic differential positioning weight is gradually increased, so that the real-time dynamic differential positioning correction amount is smoothly absorbed, thereby correcting the drift accumulated by the visual inertial odometry in the shading area.

2. The method for locating and mapping a heavily shaded orchard environment according to claim 1, characterized in that, The step of constructing a visual inertial odometry based on the binocular images and the inertial measurement unit data to obtain the local pose estimation result of the mobile platform in the local coordinate system includes: Visual features are extracted and tracked from the binocular images to obtain feature matching relationships between adjacent frames; Stereo matching is performed on the binocular images to obtain visual geometric constraints with known scale; The inertial measurement unit data is pre-integrated to obtain motion increment constraints between adjacent keyframes; The visual geometric constraints and the motion increment constraints are incorporated into a sliding window optimization framework, and the local pose estimation results of the mobile platform in the local coordinate system, including position, velocity, attitude, accelerometer bias, and gyroscope bias, are estimated through nonlinear optimization.

3. The method for locating and mapping a heavily shaded orchard environment according to claim 1, characterized in that, The steps for constructing positioning quality evaluation indicators based on the solution state, position covariance, and position residual between the real-time dynamic differential positioning observation position and the local pose estimation result in the real-time dynamic differential positioning data include: The solution status in the real-time dynamic differential positioning data is obtained, the solution status is quantized and assigned a value, and a state quantization value is generated. The solution status includes fixed solution status, floating solution status, single-point solution status and invalid solution status. Obtain the position covariance matrix from the real-time dynamic differential positioning data, and calculate the trace of the position covariance matrix as a quantification of positioning uncertainty. The real-time dynamic differential positioning observation position and the local pose estimation result are obtained. The position residual of the real-time dynamic differential positioning observation position relative to the local pose estimation result is calculated, and the norm of the position residual is calculated as a quantitative value of observation consistency. Using the state quantization value, the positioning uncertainty quantization value, and the observation consistency quantization value as inputs, a positioning quality evaluation index is calculated according to a preset comprehensive evaluation function. The positioning quality evaluation index is used to characterize the reliability of real-time dynamic differential positioning observations at the current moment. The higher the state quantization value, the lower the positioning uncertainty quantization value, and the lower the observation consistency quantization value, the higher the positioning quality evaluation index.

4. The method for locating and mapping a heavily shaded orchard environment according to claim 1, characterized in that, The steps for calculating the real-time dynamic differential positioning weights of real-time dynamic differential positioning observations in factor graph optimization using continuous mapping functions include: Obtain the positioning quality evaluation index at the current moment; Using the positioning quality evaluation index as an input variable, the real-time dynamic differential positioning weight at the current moment is calculated through a monotonically increasing continuous mapping function, wherein the real-time dynamic differential positioning weight continuously increases as the positioning quality evaluation index increases and continuously decreases as the positioning quality evaluation index decreases. The output range of the continuous mapping function is limited to a preset interval, and the continuous mapping function is continuously differentiable within its domain, so that the real-time dynamic differential positioning weights transition continuously with the changes in the positioning quality evaluation index.

5. The method for locating and mapping a heavily shaded orchard environment according to claim 1, characterized in that, The step of scaling the position covariance according to the real-time dynamic differential positioning weights to generate a dynamic covariance matrix includes: Obtain the real-time dynamic differential positioning weights at the current moment; Using the sum of the real-time dynamic differential positioning weights and the fractional value used to prevent division by zero as the denominator, calculate its corresponding reciprocal to obtain the scaling factor; The original position covariance matrix in the real-time dynamic differential positioning data is multiplied by the scaling factor to generate a dynamic covariance matrix. The larger the weight of the real-time dynamic differential positioning, the smaller the scaling factor, the smaller the dynamic covariance matrix, and the stronger the constraint strength of the real-time dynamic differential positioning observation in factor graph optimization. Conversely, the smaller the weight of the real-time dynamic differential positioning, the larger the scaling factor, the larger the dynamic covariance matrix, and the weaker the constraint strength of the real-time dynamic differential positioning observation in factor graph optimization.

6. The method for locating and mapping a heavily shaded orchard environment according to claim 1, characterized in that, The steps of incorporating the real-time dynamic differential positioning observation position as a global constraint into the factor graph optimization framework, and optimizing it together with the visual inertial odometry residuals to obtain the global pose estimation result of the mobile platform in the global coordinate system include: Construct a visual reprojection residual, wherein the visual reprojection residual represents the difference between the observed position of a feature point in a binocular image and the predicted position of the feature point projected onto the image plane according to the current estimated pose; Construct an inertial measurement pre-integration residual, wherein the inertial measurement pre-integration residual represents the difference between the relative motion increment obtained by pre-integration of inertial measurement data between adjacent keyframes and the relative motion increment calculated by estimated pose; Construct a real-time dynamic differential positioning position residual, wherein the real-time dynamic differential positioning position residual represents the difference between the real-time dynamic differential positioning observation position and the visual inertial odometry predicted position; Construct a multi-source constraint joint optimization function, which is the weighted sum of squares of the visual reprojection residual, the inertial measurement pre-integration residual, and the real-time dynamic differential positioning position residual; The multi-source constraint joint optimization function is solved by nonlinear optimization to obtain the global pose estimation result in the global coordinate system.

7. The method for locating and mapping a heavily shaded orchard environment according to any one of claims 1 to 6, characterized in that, When the mobile platform leaves the area obscured by fruit trees, and the real-time dynamic differential positioning solution returns from a degraded solution state to a fixed solution state, the real-time dynamic differential positioning weight is gradually increased to smoothly absorb the real-time dynamic differential positioning correction amount, thereby correcting the drift accumulated by the visual inertial odometry within the obstructed area. This step includes: Obtain the positioning quality evaluation index at the current moment, and calculate the target weight at the current moment based on the positioning quality evaluation index. The deterioration solution state includes floating solution state, single-point solution state and invalid solution state. Obtain the actual weight of the previous time step, and use the weighted average of the actual weight of the previous time step and the target weight of the current time step as the actual weight of the current time step. The actual weight of the previous time step is assigned a first smoothing factor, and the target weight of the current time step is assigned a second smoothing factor. The sum of the first smoothing factor and the second smoothing factor is 1. During the period of the fixed solution state, the above steps are repeated so that the actual weight gradually approaches the target weight from the value of the previous moment, and the real-time dynamic differential positioning correction is gradually absorbed in multiple optimization windows as the actual weight gradually increases. When the actual weight reaches the target weight, the real-time dynamic differential positioning observation is added to the factor graph optimization with the highest constraint strength to correct the drift accumulated by the visual inertial odometry in the occluded area.

8. A positioning and mapping device for orchards with heavily obscured environments, characterized in that, include: The data synchronization acquisition module is configured to acquire multi-source sensor data during the movement of the mobile platform in the orchard environment. The multi-source sensor data includes at least binocular images acquired by the binocular vision sensor mounted on the mobile platform, inertial measurement unit data acquired by the inertial measurement unit, and real-time dynamic differential positioning data output by the satellite positioning module. The visual inertial estimation module is configured to construct a visual inertial odometry based on the binocular images and the inertial measurement unit data, and obtain the local pose estimation result of the mobile platform in the local coordinate system; The positioning weight generation module is configured to construct a positioning quality evaluation index based on the solution state, position covariance, and position residual between the real-time dynamic differential positioning observation position and the local pose estimation result in the real-time dynamic differential positioning data; based on the positioning quality evaluation index, the module calculates the real-time dynamic differential positioning weight of the real-time dynamic differential positioning observation in factor graph optimization through a continuous mapping function, and scales the position covariance according to the real-time dynamic differential positioning weight to generate a dynamic covariance matrix; The joint optimization solution module is configured to incorporate the real-time dynamic differential positioning observation position as a global constraint into the factor graph optimization framework, and optimize it together with the residual of the visual inertial odometry to obtain the global pose estimation result of the mobile platform in the global coordinate system. The progressive drift correction module is configured to gradually increase the weight of the real-time dynamic differential positioning when the mobile platform moves out of the fruit tree shading area and the real-time dynamic differential positioning solution state recovers from the degraded solution state to the fixed solution state, so that the real-time dynamic differential positioning correction amount is smoothly absorbed, thereby correcting the drift accumulated by the visual inertial odometry in the shading area.

9. An electronic device comprising a central processing unit and a memory, characterized in that, The central processing unit is used to invoke and run a computer program stored in the memory to perform the steps of the method as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, It stores, in the form of computer-readable instructions, a computer program implemented according to any one of claims 1 to 7, which, when invoked by a computer, executes the steps included in the corresponding method.

Images