EKF-SLAM processing method and system based on electronic map

Abstract

The invention discloses an EKF-SLAM processing method and system based on an electronic map, and the method comprises the steps: manually setting a path point for the autonomous cruise of an unmanned breeding ship through an electronic map module of an upper computer, and carrying out the coordinate system conversion of the path point through a coordinate conversion module; constructing a motion model of the unmanned breeding ship about an input rudder angle and a control time interval on an X axis, a Y axis and a steering angle; constructing an observation model of the unmanned culture ship; the motion state of the unmanned culture ship is predicted by adopting an EKF-SLAM algorithm, feature points and observation points of an electronic map are associated according to the mahalanobis distance, and state variables and covariance thereof are updated according to feature points existing in the map.

Description

Technical Field

[0001] The present invention relates to the technical field of map construction, and in particular to an EKF-SLAM processing method and system based on an electronic map. Background Art

[0002] Unmanned aquaculture vessels, as a major new type of aquaculture equipment, play a vital role in aquaculture processes such as feeding, pesticide application, and monitoring. However, existing aquaculture equipment still faces shortcomings such as limited functionality, high costs, low mechanization penetration, low overall intelligence levels, and specialized equipment operation. For example, most unmanned aquaculture vessels utilize high-performance processors to address computing power issues, relying on their powerful computing power to process information collected by various sensors in real time. However, the market price of unmanned aquaculture vessels equipped with high-performance processors is generally high, which has hindered the purchase of intelligent equipment by businesses and farmers.

[0003] Simultaneous Localization and Mapping (SLAM) algorithms are key technologies for achieving autonomous intelligent robot mobility. SLAM algorithms can be categorized by sensor type into visual SLAM, laser SLAM, and multi-sensor fusion SLAM. Sensor-based SLAM algorithms typically use monocular, binocular, RGBD cameras, 2D LiDAR, or 3D LiDAR as front-end data acquisition units. The front-end odometry uses feature extraction and matching to estimate pose. The back-end optimizes using algorithms such as the Extended Kalman Filter (EKF) and graph optimization to obtain accurate pose estimates. Feature-based matching methods are then used for loop detection to eliminate accumulated errors. Furthermore, dedicated acceleration chips for sensor-based SLAM algorithms are primarily FPGAs and ASICs. Theoretically, higher sensor accuracy leads to higher accuracy in robot pose estimation. The greater the computing power of the acceleration chip, the more real-time the robot's response, but this translates to higher investment costs. As an intelligent robot that performs tasks at a lower speed in an environment with fewer features, unmanned aquaculture vessels choose vision or lidar as front-end sensors and FPGA and other acceleration chips as processors. These are not ideal choices in terms of application or cost. Summary of the Invention

[0004] The embodiment of the present application provides an EKF-SLAM processing method and system based on an electronic map. The system introduces path points set by the electronic map into the EKF-SLAM algorithm, and uses an acceleration chip with an STM32 single-chip microcomputer as the core to process the real-time positioning data obtained by the Beidou positioning module in real time. This solves the technical problem that the SLAM algorithm in the prior art has high requirements for characteristic environments and is greatly affected by environmental factors during the application of vision and lidar technologies. The present application can provide sufficient computing power and precise positioning, while greatly reducing R&D costs, so as to enable unmanned aquaculture ships to perform aquaculture tasks in an autonomous manner, improve the positioning accuracy and intelligence level of unmanned aquaculture ships, and reduce the overall R&D costs of unmanned aquaculture ships.

[0005] The present invention provides an EKF-SLAM processing method based on an electronic map, comprising the following steps: Step S1, manually setting the path points of the autonomous cruising of the unmanned aquaculture vessel using the electronic map module of the host computer, and performing the path point coordinate system conversion through the coordinate transformation module; Step S2, constructing a motion model of the unmanned aquaculture vessel in the X-axis, Y-axis, and steering angle with respect to the input rudder angle and the control time interval; Step S3, constructing an observation model of the unmanned aquaculture vessel; Step S4, using the EKF-SLAM algorithm to predict the motion state of the unmanned aquaculture vessel, correlating the feature points and observation points of the electronic map according to the Mahalanobis distance, and updating the state variables and their covariances according to the feature points already existing in the map.

[0006] Preferably, the path point coordinate system conversion is specifically: converting the GCJ02 longitude and latitude coordinates provided by the AutoNavi electronic map into the WGS84 coordinates of the Beidou positioning module.

[0007] Preferably, the motion model in step S2 is: ;

[0008] in, 、 and They represent the unmanned aquaculture ship on the 2D plane. Axis and Coordinates in the axis direction and the current steering angle; Indicates the movement speed; To control the input rudder angle, To control the time interval.

[0009] Preferably, the observation model in step S3 is: ; in, Indicates the observation value of the Beidou positioning module at the current moment. is the observation noise.

[0010] Preferably, the method of using the EKF-SLAM algorithm to predict the motion state of the unmanned aquaculture vessel specifically includes: Step S41, using a nonlinear system to describe the unmanned aquaculture vessel system; Step S42, estimating the motion state of the unmanned aquaculture vessel at the next moment; Step S43: using the maximum steering angle and the maximum steering speed as two parameters to limit the steering angle and adjust the steering of the unmanned aquaculture vessel.

[0011] Preferably, the step S43 is specifically as follows: ;

[0012] in, is the steering angle, is the maximum steering angle, is the maximum turning speed, To control the time interval.

[0013] Preferably, in step S4, the characteristic points and observation points of the electronic map are associated according to the Mahalanobis distance as follows: If the Mahalanobis distance is less than the maximum distance of the coordinates existing in the map, it is considered to be a feature point existing in the map; If the Mahalanobis distance is greater than the minimum distance of the new feature, the current observation is considered to be a new observation and the new observation needs to be added to the map.

[0014] This application also proposes an EKF-SLAM processing system based on an electronic map, including a processor and a host computer; The processor includes: a wireless communication module, a Beidou positioning module, an inertial measurement unit, and an STM32 single-chip microcomputer; the wireless communication module is used to exchange data with the host computer; the Beidou positioning module is used to collect Beidou positioning in real time; the inertial measurement unit is used to collect parameters of the unmanned aquaculture vessel in real time; The host computer includes: a communication setting module, a parameter setting module, an electronic map module, a track planning module, a coordinate transformation module, a data transmission module, a data management module, and a data processing module; The communication setting module is used to set the connection parameters required for serial communication and establish a communication connection with the processor; the parameter setting module is used to perform parameter settings; the electronic map module is used to provide latitude and longitude coordinates under GCJ02 coordinates; the track planning module is used to manually set the path points for autonomous cruising of the unmanned aquaculture vessel; the coordinate transformation module is used to perform path point coordinate system conversion; the data transmission module is used to send the planned track coordinates to the processor through the wireless communication module; the data management module is used to store data such as the planned track coordinates and the processor-estimated posture; the data processing module is used to predict the motion state of the unmanned aquaculture vessel, associate the feature points and observation points of the electronic map according to the Mahalanobis distance, and update the state variables and their covariances according to the feature points already existing in the map.

[0015] Preferably, the wireless communication module includes two wireless communication units, one wireless communication unit is connected to the processor circuit board, and the other wireless communication unit is connected to the host computer through a USB interface.

[0016] One or more technical solutions provided in the embodiments of this application have at least the following technical effects or advantages: 1. Using electronic maps and Beidou positioning data to provide waypoints and observation signals to the EKF-SLAM algorithm respectively, it effectively solves the technical problem of inaccurate positioning of unmanned aquaculture vessels due to the lack of environmental features when sailing in large water environments. It enables unmanned aquaculture vessels to perform aquaculture tasks in an autonomous manner, improving the positioning accuracy and intelligence level of unmanned aquaculture vessels.

[0017] 2. The innovative invention of this application uses the STM32 microcontroller to design the EKF-SLAM algorithm processing system, which has high cost performance, good scalability and a wide range of market applications, good performance, and low cost.

[0018] 3. This application uses the STM32F29IGT6 microcontroller as the core processing unit, which solves the problems of high R&D cost, great development difficulty and insufficient scalability of acceleration chips for existing SLAM algorithms. The design and development difficulty of this application is relatively low, reducing R&D costs and risks.

[0019] 4. This application uses the Lora wireless communication module adapter circuit of the STM32F103C8T6 microcontroller to solve the problem that the acceleration chip of the existing SLAM algorithm uses WiFi communication, which is greatly affected by environmental factors and has high energy consumption, reducing battery life. This application can realize long-distance, high-speed communication between the host computer and the processor in a large water surface environment where there is a lack of network infrastructure. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1Flowchart of the EKF-SLAM processing method based on electronic map in Example 1 of the present application; Figure 2 This is a schematic diagram of the path point settings for the autonomous cruising of the unmanned aquaculture vessel in Example 1 of the present application; Figure 3 This is a schematic diagram of the cruising trajectory of the unmanned boat in Example 1 of this application; Figure 4 This is a schematic diagram of the test results of the unmanned boat in Example 1 of this application; Figure 5 This is a structural diagram of the SLAM processing system based on the STM32 single-chip microcomputer in Example 2 of this application; Figure 6 This is a rendering of the processor circuit board in Example 2 of this application; Figure 7 This is a rendering of the wireless communication module core board in Example 2 of this application; Figure 8 This is a schematic diagram of the upper computer structure in Example 2 of this application. DETAILED DESCRIPTION

[0021] The present invention provides an EKF-SLAM processing method and system based on electronic maps. By jointly optimizing the hardware structure, communication system, and algorithm architecture, it effectively solves the problems of high R&D costs, poor communication stability, and insufficient environmental adaptability in existing solutions. The overall design concept of the technical solution is as follows: By using an STM32 microcontroller as the core processing unit, which boasts strong computing power and a rich set of peripheral resources, supports C / C++ programming, and easily accesses various open-source libraries, this design significantly reduces system development complexity. Compared to dedicated acceleration chip solutions, this design significantly reduces R&D time, manpower, and capital costs, while also shortening the development cycle and effectively mitigating R&D risks. This addresses the technical challenges of existing SLAM algorithm acceleration chips, which suffer from high R&D costs, difficulty, and limited scalability.

[0022] By designing an STM32-based Lora wireless communication module adapter circuit, we achieved long-distance, high-speed communication between the host computer and the processor in large-scale water environments lacking network infrastructure. This solution monitors the processor's operating status in real time and reliably transmits path point information preset by the host computer, while significantly reducing system power consumption. This addresses the technical issue of existing SLAM algorithm acceleration chips using WiFi communication, which is significantly affected by environmental factors, resulting in high energy consumption and reduced battery life.

[0023] By integrating pathpoint information provided by electronic maps with observation data from BeiDou positioning, an EKF-SLAM algorithm framework is constructed. The electronic map and BeiDou positioning data provide pathpoints and observation signals to the EKF-SLAM algorithm, respectively. This method is particularly suitable for environments with sparse features, such as large water surfaces. While maintaining positioning accuracy, it significantly reduces reliance on specialized sensors and environmental features. This overcomes the technical issues of vision-based or lidar-based SLAM algorithms, such as their high cost and high requirements for feature-rich environments.

[0024] In order to better understand the above technical solution, the above technical solution will be described in detail below with reference to the accompanying drawings and specific implementation methods.

[0025] Example 1 like Figure 1 As shown, this application proposes an EKF-SLAM processing method based on an electronic map, comprising the following steps: Step S1: manually set the path points of the autonomous cruising of the unmanned aquaculture vessel using the electronic map module of the host computer, and perform the path point coordinate system conversion through the coordinate transformation module.

[0026] First, use the electronic map module of the host computer to manually set the path points for the autonomous cruising of the unmanned aquaculture boat, such as Figure 2 Because the Amap electronic map provides GCJ02 longitude and latitude coordinates, and the Beidou positioning module uses the WGS84 coordinate system, all set path points need to undergo coordinate transformation before being sent to the processor via the wireless communication module, which can also save processor computing resources.

[0027] Step S2, constructing a motion model of the unmanned aquaculture vessel on the X-axis, Y-axis, and steering angle with respect to the input rudder angle and control time interval.

[0028] The dynamic model of the unmanned aquaculture vessel is defined as: (1) In formula (1), 、 and They represent the unmanned aquaculture ship on the 2D plane. Axis and The coordinates in the axis direction and the current steering angle. If the unmanned aquaculture vessel maintains a constant moving speed when performing the task , then the motion model of the ship is: (2) In formula (2), To control the input rudder angle, To control the time interval.

[0029] After discretizing formula (2), we can get: (3) in, and Respectively represent the ship's current and previous moments Speed ​​in direction, and Respectively represent the ship's current and previous moments Speed ​​in direction, and Represent the steering angle of the ship at the current moment and the previous moment respectively.

[0030] After sorting out formula (3) and introducing corresponding noise, its matrix form is: (4) Where, 、 and Respectively 、 The noise level of the direction, speed and steering angle.

[0031] Step S3: construct an observation model of the unmanned aquaculture vessel.

[0032] Since the autonomous cruising of unmanned aquaculture vessels relies solely on the BeiDou positioning module, the observation module is defined as: (5) in, Indicates the observation value of the Beidou positioning module at the current moment. is the observation noise.

[0033] Step S4, using the EKF-SLAM algorithm to predict the motion state of the unmanned aquaculture vessel, correlating the feature points and observation points of the electronic map according to the Mahalanobis distance, and updating the state variables and their covariances according to the feature points already existing in the map.

[0034] The nonlinear system is used to describe the unmanned aquaculture boat system as follows: (6) in, and is a nonlinear function, is the control vector related only to the steering angle, and represent process noise and observation noise, respectively.

[0035] Since the unmanned aquaculture vessel is in a large water surface environment and there are no available navigation marks, at the initial moment, the system state is that of an unmanned aquaculture vessel.

[0036] Step S5: predict the motion state of the unmanned aquaculture vessel and adjust the motion state according to the limited steering angle.

[0037] The unmanned aquaculture ship estimates the motion state of the ship at the next moment according to formula (4), namely: (7) In order to prevent the steering angle from overshooting, the maximum steering angle is introduced and maximum turning speed Two parameters limit the steering angle to prevent the ship from spinning in place or seriously deviating from the preset track when the steering angle is too large and the processor issues a new steering command before the ship has time to complete the steering adjustment. The adjustment strategy is: (8) The covariance of the estimated results is calculated according to the formula.

[0038] (9) in, (10) (11) Where, represents the state variable prediction covariance, represents the covariance of the state variables at the previous moment, is the Jacobian matrix of the state variables at the current moment, For control variables The Jacobian matrix of is the process noise covariance.

[0039] If the unmanned aquaculture vessel is in the process of autonomous cruising, the processor uses the Mahalanobis distance to determine whether the current observation is an existing coordinate in the map. At any moment, when the processor receives the positioning data returned by the Beidou positioning module, the positioning data needs to traverse all the coordinate data in the map and calculate the Mahalanobis distance between the two. If the Mahalanobis distance is less than the parameter ,parameter Indicates the maximum distance to be considered as a known feature, which is considered to be a feature point that already exists in the map. If the Mahalanobis distance is greater than the parameter ,parameter Indicates the minimum distance to be considered a new feature. The current observation is considered a new observation and needs to be added to the map. Calculate using the following formula: (12) in, For the current observation, is the observation matrix, is the covariance matrix, is the observation noise covariance matrix, Represents the normalization term. The EKF-SLAM algorithm updates the state variables and their covariances based on the feature points already existing in the map: (13) Where, represents the Kalman gain, is the observation state transfer matrix, is the observation noise covariance.

[0040] If new observations are generated during data association, the covariance matrix of the state variables needs to be further updated based on the newly added observations. The update method is as follows: (14) in, (15) To test the effectiveness of the designed EKF-SLAM processing system, the unmanned aquaculture boat was tested at a hairy crab farm in Yangcheng Lake. To ensure safety, the unmanned boat's cruising speed was kept low. Furthermore, due to its large size, the controller's control frequency could be kept low, as the boat's actual position might deviate significantly from the initial correction command after the controller issues one. Table 1 lists the parameters used in the unmanned boat testing. Figure 3 The image shows the unmanned boat’s cruising trajectory set before the test, with the red cross indicating the unmanned boat’s waypoint location. Figure 4 The test results are shown, where the yellow dots are the real-time Beidou positioning data and the blue lines are the actual tracks of the unmanned boat. Figure 4 It can be seen that when the positioning error of the Beidou positioning module is large, that is, when the Beidou positioning error exceeds the set threshold, the processor ignores the current positioning observation and only uses the predicted position to correct the heading. After a period of time, when new observations arrive, the unmanned ship can still cruise according to the set path points, which shows that the designed processor has a certain anti-interference ability.

[0041] Table 1 Parameter settings used in the unmanned ship test

[0042] Example 2 This application proposes a SLAM processing system based on STM32 single-chip microcomputer, which consists of two parts: processor and host computer. Figure 5As shown in the figure, the processor, with an STM32 microcontroller as its core, integrates a wireless communication module, a Beidou positioning module, an inertial measurement unit (IMU), and a storage unit. It primarily collects data from the Beidou positioning module and IMU in real time. This data, along with pre-set data in the memory, serves as input for the EKF-SLAM algorithm, estimating and outputting the unmanned aquaculture vessel's pose, such as position and heading. The processor provides control parameters to the controller based on the estimated pose and the orientation provided by the IMU. The wireless communication module serves as the data exchange channel between hardware and software, primarily enabling wireless transmission of trajectory planning data from the host computer to the hardware's memory. The processor's memory stores the estimated results and uploads them to the host computer for backup. The host computer manually plans the trajectory for autonomous missions using an online electronic map. The coordinates provided by the electronic map typically require conversion before being sent to the processor.

[0043] The core component of the processor circuit is the STM32F429IGT6 microcontroller, which primarily includes circuits for crystal oscillator clock, reset, IMU data reception, and Beidou data reception. The STM32F429IGT6 MCU boasts a clock speed of up to 180 MHz, providing up to 210 DMIPS of processing power, ensuring efficient data processing. It supports single-precision floating-point arithmetic, providing sufficient computing power for the EKF-SLAM algorithm. The crystal oscillator clock circuit is the heart of the circuit board, providing a stable clock signal to the chip, ensuring synchronous and accurate operation. The reset circuit is responsible for resetting the circuit board upon power-up, ensuring proper program initialization. The memory stores track coordinates transmitted by the software host computer via the wireless communication module. Figure 6 Shown is a 3D rendering of the processor circuit board.

[0044] The present invention has designed two wireless communication modules, one of which is connected to the processor circuit board, and the other is connected to the host computer via a USB interface. The wireless communication module can establish a communication link between the host computer and the processor. Each wireless communication module consists of a core board and a LoRa communication module. The core board uses an STM32F103C8T6 single-chip microcomputer as the processing unit and is designed with a light-emitting diode. When the transceiver board sends or receives data, the light-emitting diode will flash. Figure 7 This is a 3D rendering of the wireless communication module core board.

[0045] The host computer is developed using C# programming language and consists of modules such as communication settings, parameter settings, electronic maps, track planning, coordinate transformation, data transmission and data management. Figure 8As shown. Among them, the communication setting module mainly sets the connection parameters required for serial communication and establishes a communication connection with the processor. The parameter setting module mainly realizes the setting of the parameters required for the EKF-SLAM algorithm in the processor. The electronic map module uses the online AutoNavi electronic map, provides two-dimensional and satellite map preview modes, and mainly provides longitude and latitude coordinates under GCJ02 coordinates. The track planning module is used to manually plan the track coordinates of the unmanned aquaculture vessel when it performs tasks autonomously. The coordinate transformation module is used to convert the Mars coordinate system of the AutoNavi electronic map, that is, the GCJ02 coordinate system, to the WGS84 coordinate system of the Beidou positioning model. The coordinate transformation is usually completed during the data planning process. The data transmission module sends the planned track coordinates to the processor through the wireless communication module. The data management module is mainly used to store the planned track coordinates and data such as the processor's estimated posture to provide backup for subsequent algorithm debugging.

[0046] The embodiments of this specific embodiment are all preferred embodiments of the present invention, and are not intended to limit the scope of protection of the present invention. Therefore, any equivalent changes made according to the structure, shape, and principle of the present invention should be included in the scope of protection of the present invention. Although the preferred embodiments of the present invention have been described, those skilled in the art may make additional changes and modifications to these embodiments once they are aware of the basic creative concepts. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention. Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these changes and variations.

Claims

1. An EKF-SLAM processing method based on electronic maps, characterized in that: The following steps are involved: Step S1, manually setting the path points of the autonomous cruising of the unmanned aquaculture vessel using the electronic map module of the host computer, and performing the path point coordinate system conversion through the coordinate transformation module; Step S2, constructing a motion model of the unmanned aquaculture vessel in the X-axis, Y-axis, and steering angle with respect to the input rudder angle and the control time interval; Step S3, constructing an observation model of the unmanned aquaculture vessel; Step S4, using the EKF-SLAM algorithm to predict the motion state of the unmanned aquaculture vessel, correlating the feature points and observation points of the electronic map according to the Mahalanobis distance, and updating the state variables and their covariances according to the feature points already existing in the map.

2. The EKF-SLAM processing method based on an electronic map as claimed in claim 1, wherein The path point coordinate system conversion is specifically as follows: the GCJ02 longitude and latitude coordinates provided by the AutoNavi electronic map are converted into the WGS84 coordinates of the Beidou positioning module.

3. The EKF-SLAM processing method based on electronic map as claimed in claim 1, wherein The motion model in step S2 is: ， in, 、 and They represent the unmanned aquaculture ship on the 2D plane. Axis and Coordinates in the axis direction and the current steering angle; Indicates the movement speed; To control the input rudder angle, To control the time interval.

4. The EKF-SLAM processing method based on electronic map as claimed in claim 1, wherein The observation model in step S3 is: ， in, Indicates the observation value of the Beidou positioning module at the current moment. is the observation noise.

5. The EKF-SLAM processing method of the electronic map as claimed in claim 1, wherein: The use of the EKF-SLAM algorithm to predict the motion state of the unmanned aquaculture vessel specifically includes: Step S41, using a nonlinear system to describe the unmanned aquaculture vessel system; Step S42, estimating the motion state of the unmanned aquaculture vessel at the next moment; Step S43: using the maximum steering angle and the maximum steering speed as two parameters to limit the steering angle and adjust the steering of the unmanned aquaculture vessel.

6. The EKF-SLAM processing method based on electronic map as claimed in claim 5, wherein The step S43 is specifically as follows: ， in, is the steering angle, is the maximum steering angle, is the maximum turning speed, To control the time interval.

7. The EKF-SLAM processing method based on electronic map as claimed in claim 1, wherein In step S4, the characteristic points and observation points of the electronic map are associated according to the Mahalanobis distance as follows: If the Mahalanobis distance is less than the maximum distance of the coordinates existing in the map, it is considered to be a feature point existing in the map; If the Mahalanobis distance is greater than the minimum distance of the new feature, the current observation is considered to be a new observation and the new observation needs to be added to the map.

8. An EKF-SLAM processing system based on electronic maps, characterized in that: Including processor and host computer; The processor includes: a wireless communication module, a Beidou positioning module, an inertial measurement unit, and an STM32 single-chip microcomputer; the wireless communication module is used to exchange data with the host computer; the Beidou positioning module is used to collect Beidou positioning in real time; the inertial measurement unit is used to collect parameters of the unmanned aquaculture vessel in real time; The host computer includes: a communication setting module, a parameter setting module, an electronic map module, a track planning module, a coordinate transformation module, a data transmission module, a data management module, and a data processing module; The communication setting module is used to set the connection parameters required for serial communication and establish a communication connection with the processor; the parameter setting module is used to perform parameter settings; the electronic map module is used to provide latitude and longitude coordinates under GCJ02 coordinates; the track planning module is used to manually set the path points for autonomous cruising of the unmanned aquaculture vessel; the coordinate transformation module is used to perform path point coordinate system conversion; the data transmission module is used to send the planned track coordinates to the processor through the wireless communication module; the data management module is used to store data such as the planned track coordinates and the processor-estimated posture; the data processing module is used to predict the motion state of the unmanned aquaculture vessel, associate the feature points and observation points of the electronic map according to the Mahalanobis distance, and update the state variables and their covariances according to the feature points already existing in the map.

9. The EKF-SLAM processing system based on electronic maps as claimed in claim 8, wherein The wireless communication module includes two wireless communication units, one wireless communication unit is connected to the processor circuit board, and the other wireless communication unit is connected to the host computer through a USB interface.

Images