Mobile robot navigation method and system based on rotary imaging radar and satellite positioning

By fusing multi-source data from rotating imaging radar, satellite positioning module, and inertial measurement unit, the navigation accuracy and reliability issues of mobile robots in complex environments are solved, achieving high-precision and high-reliability navigation results.

CN121977532APending Publication Date: 2026-05-05SHANGHAI ENZUO TECH CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202610361263.8
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2026-03-24
Publication Date
2026-05-05

AI Technical Summary

Technical Problem

Existing mobile robot navigation methods struggle to achieve high-precision and high-reliability navigation in complex environments, especially when satellite signals are blocked or data is lost, failing to provide continuous and reliable positioning information.

Method used

By combining a rotating imaging radar, a satellite positioning module, and an inertial measurement unit, the system achieves synchronous acquisition and fusion of multi-source data. The rotating imaging radar acquires 3D point cloud data, satellite positioning provides global position reference, and inertial measurement senses the robot's motion state. The system integrates and generates positioning attitude data, and when satellite signals fail, it supplements the data through dead reckoning to generate a dynamic local environment map for path planning and adjustment.

Benefits of technology

It improves the navigation accuracy and reliability of mobile robots in complex environments, ensures continuous and stable navigation even in the absence of satellite signals, and enhances the robustness and adaptability of navigation.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN121977532A_ABST
    Figure CN121977532A_ABST
Patent Text Reader

Abstract

The invention provides a mobile robot navigation method and system based on a rotary imaging radar and satellite positioning, and relates to the technical field of mobile robot navigation. Synchronously acquiring surrounding environment three-dimensional point cloud data, satellite positioning data and inertial measurement data; feature information is extracted and integrated to generate positioning attitude data; distinguishing static and dynamic characteristics according to the three-dimensional point cloud data, and generating a dynamic local environment map; when the satellite positioning signal fails, dead reckoning is carried out by combining the three-dimensional point cloud data, the inertial measurement data and the positioning attitude data before failure, and the positioning attitude data is updated; and combining the new positioning attitude data and the dynamic local environment map with the target driving data to generate an initial navigation control instruction, continuously collecting a data dynamic adjustment instruction, and controlling the robot to complete navigation. The method can adapt to a complex dynamic environment and realize high-precision reliable navigation.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] This invention relates to the field of mobile robot navigation technology, and more specifically, to a mobile robot navigation method and system based on rotating imaging radar and satellite positioning. Background Technology

[0002] With the rapid development of mobile robot technology, navigation capabilities have become a key factor determining the breadth of their applications and their performance. Currently, common mobile robot navigation methods mainly rely on data from a single type of sensor.

[0003] For example, while satellite-based navigation methods can provide relatively accurate global location information for mobile robots in open environments, satellite signals are easily blocked in some special scenarios, such as urban canyons with tall buildings, indoor environments, or underground spaces, causing the positioning signal to fail and making it impossible to provide continuous and reliable positioning information for the robot, which seriously affects the accuracy and stability of navigation.

[0004] While navigation methods based on sensors such as LiDAR can acquire local information about the robot's surrounding environment and build a local map for navigation, these sensors can usually only acquire environmental data in a specific direction or within a limited range. In complex and ever-changing dynamic environments, it is difficult to fully and timely perceive surrounding dynamic interference factors. Furthermore, during robot movement, data loss or error accumulation is prone to occur, which in turn affects the accuracy and safety of navigation.

[0005] Furthermore, existing navigation methods often lack effective data fusion and collaborative processing mechanisms when processing sensor data, failing to fully utilize the advantages of different sensor data, resulting in the inability to achieve high-precision and high-reliability navigation in complex environments. Summary of the Invention

[0006] In view of the aforementioned problems, and in conjunction with the first aspect of the present invention, embodiments of the present invention provide a mobile robot navigation method based on rotating imaging radar and satellite positioning, the method comprising: The rotating imaging radar, satellite positioning module, and inertial measurement unit on the mobile robot are activated. The rotating imaging radar enters a continuous omnidirectional rotating scanning state to collect three-dimensional point cloud data of the environment around the mobile robot. The satellite positioning module simultaneously receives positioning signals transmitted by the satellite to generate satellite positioning data. The inertial measurement unit simultaneously senses the motion state of the mobile robot to generate inertial measurement data. Feature information is extracted from the three-dimensional point cloud data, satellite positioning data, and inertial measurement data, and coordinate system transformation is completed. The interrelationship of feature information is established, and all the associated feature information is integrated to generate the positioning posture data of the mobile robot. Based on the three-dimensional point cloud data continuously collected by the rotating imaging radar, static environmental feature data and dynamic interference feature data of the environment around the mobile robot are extracted. After removing the dynamic interference feature data, the remaining static environmental feature data are integrated. The integrated static environmental feature data are arranged according to the actual spatial location to generate a dynamic local environment map of the environment around the mobile robot. The satellite positioning signal transmission status is identified based on the satellite positioning data continuously collected by the satellite positioning module. When the satellite positioning signal fails, dead reckoning is performed based on the three-dimensional point cloud data continuously collected by the rotating imaging radar and the inertial measurement data continuously collected by the inertial measurement unit, combined with the positioning attitude data generated before the satellite positioning signal failed. Dead reckoning positioning data is generated and added to the positioning attitude data, replacing the failed satellite positioning-related data in the positioning attitude data to form new positioning attitude data. The new positioning attitude data and dynamic local environment map are combined with the target driving data of the mobile robot to perform path planning and driving control processing to generate initial navigation control commands. Subsequent 3D point cloud data, satellite positioning data, and inertial measurement units are continuously collected through rotating imaging radar, satellite positioning module, and inertial measurement unit. The aforementioned processing steps are repeated to generate updated positioning attitude data and updated dynamic local environment map. Based on the updated positioning attitude data and updated dynamic local environment map, the initial navigation control commands are dynamically adjusted to generate adjusted navigation control commands and control the mobile robot to drive according to the adjusted navigation control commands to complete the navigation operation.

[0007] Furthermore, embodiments of the present invention also provide a mobile robot navigation system based on rotating imaging radar and satellite positioning, characterized in that it includes: A processor; a machine-readable storage medium for storing machine-executable instructions of the processor; wherein the processor is configured to execute the aforementioned mobile robot navigation method based on rotating imaging radar and satellite positioning by executing the machine-executable instructions.

[0008] In another aspect, embodiments of the present invention also provide a computer program product, the computer program product including machine-executable instructions, the machine-executable instructions being stored in a computer-readable storage medium, a processor of a mobile robot navigation system based on rotating imaging radar and satellite positioning reading the machine-executable instructions from the computer-readable storage medium, the processor executing the machine-executable instructions, causing the mobile robot navigation system based on rotating imaging radar and satellite positioning to execute the aforementioned mobile robot navigation method based on rotating imaging radar and satellite positioning.

[0009] Based on the above, by simultaneously activating the rotating imaging radar, satellite positioning module, and inertial measurement unit (IMU) mounted on the mobile robot, synchronous acquisition of multi-source data was achieved. The 3D point cloud data acquired by the rotating imaging radar through continuous omnidirectional rotation scanning reflects the 3D structural information of the mobile robot's surrounding environment; the satellite positioning module synchronously receives positioning data generated from satellite signals, providing a global position reference for the robot; and the inertial measurement unit senses the robot's motion state, generating inertial measurement data that helps understand the robot's motion trends and attitude changes. By extracting feature information from the aforementioned multi-source data and performing coordinate system transformation, establishing the interrelationships of feature information, and integrating them to generate the mobile robot's positioning and attitude data, the advantages of different sensor data are fully utilized, achieving effective data fusion and improving the accuracy and reliability of positioning. Based on data collected by a rotating imaging radar, static environmental features and dynamic interference features are distinguished, generating a dynamic local environment map. This map can reflect changes in the robot's surrounding environment in a timely and accurate manner, effectively eliminating the impact of dynamic interference on navigation. When satellite positioning signals fail, dead reckoning is performed based on data from the rotating imaging radar and inertial measurement unit, combined with the positioning attitude data before the failure. This replenishes and updates the positioning attitude data, ensuring continuous and stable navigation for the robot even in the absence of satellite signals, thus enhancing navigation robustness. Finally, the new positioning attitude data and dynamic local environment map are combined with target driving data for path planning and driving control. By continuously collecting data and dynamically adjusting navigation control commands, the robot can adapt to environmental changes in real time, achieving high-precision and high-reliability navigation operations. This significantly improves the navigation performance and application range of mobile robots in complex dynamic environments. Attached Figure Description

[0010] Figure 1 This is a schematic diagram of the execution flow of the mobile robot navigation method based on rotating imaging radar and satellite positioning provided in an embodiment of the present invention.

[0011] Figure 2 This is a schematic diagram of exemplary hardware and software components of a mobile robot navigation system based on rotating imaging radar and satellite positioning provided in an embodiment of the present invention. Detailed Implementation

[0012] Figure 1 This is a flowchart illustrating a mobile robot navigation method based on rotating imaging radar and satellite positioning, provided in one embodiment of the present invention. A detailed description follows.

[0013] Step S110: Activate the rotating imaging radar, satellite positioning module, and inertial measurement unit on the mobile robot. The rotating imaging radar enters a continuous omnidirectional rotating scanning state to collect three-dimensional point cloud data of the environment around the mobile robot. The satellite positioning module synchronously receives positioning signals transmitted by the satellite to generate satellite positioning data. The inertial measurement unit synchronously senses the motion state of the mobile robot to generate inertial measurement data.

[0014] In this embodiment, mobile robot navigation in an indoor warehousing environment is used as the application scenario. When the mobile robot receives a start command, its internal control unit first sends start signals to the rotating imaging radar, satellite positioning module, and inertial measurement unit. Upon receiving the signal, the rotating imaging radar's drive motor starts working, causing the radar antenna to continuously rotate 360 ​​degrees at a preset angular velocity. The scanning frequency is set to A times per second, and the scanning range covers a spatial area with a radius of B meters around the robot, thereby collecting real-time three-dimensional point cloud data of environmental objects within this area. This data includes the three-dimensional coordinate information of each point. Simultaneously, the satellite positioning module starts receiving positioning signals emitted by multiple satellites. By analyzing these signals, it calculates the mobile robot's position information in the global coordinate system, generating satellite positioning data containing timestamps, longitude, latitude, and elevation, with a sampling frequency of C times per second. The accelerometer and gyroscope inside the inertial measurement unit also start working simultaneously. The accelerometer senses the robot's acceleration in the three coordinate axes in real time, while the gyroscope measures the robot's angular velocity. Based on these data, inertial measurement data containing acceleration, angular velocity, and timestamps are generated. The sampling frequency is D times per second, where the value of D is greater than A and C to ensure the real-time nature of motion state perception.

[0015] Step S120: Extract feature information from the three-dimensional point cloud data, satellite positioning data, and inertial measurement data, complete coordinate system transformation, establish the interrelationship of feature information, and integrate all the associated feature information to generate the positioning attitude data of the mobile robot.

[0016] In the aforementioned indoor warehousing scenario, after activating its sensors, the mobile robot needs to process the multi-source data it collects to obtain positioning and attitude data. First, it is necessary to extract feature information that reflects the robot's position and attitude from different types of data. Since these data come from different sensors and their coordinate systems differ, a unified transformation must be performed. Then, the aforementioned feature information is correlated and integrated to ultimately form positioning and attitude data that accurately describes the robot's current position and attitude.

[0017] Step S121: Acquire the 3D point cloud data collected by the rotating imaging radar, the satellite positioning data generated by the satellite positioning module, and the inertial measurement data sensed by the inertial measurement unit. Mark the acquisition time of the 3D point cloud data, satellite positioning data, and inertial measurement data to make the acquisition time of the 3D point cloud data, satellite positioning data, and inertial measurement data form a synchronized time reference.

[0018] In indoor warehousing scenarios, the mobile robot's control unit acquires data from the rotating imaging radar, satellite positioning module, and inertial measurement unit via data interfaces. For the 3D point cloud data acquired by the rotating imaging radar, each frame contains a timestamp generated by the radar's internal clock module. Satellite positioning data also includes a timestamp based on satellite time during generation. Inertial measurement data similarly has a timestamp recorded by its internal clock. Because the clocks of different sensors may have slight differences, the control unit calibrates the timestamps of each sensor's data against the robot's internal master clock to ensure data synchronization. Specifically, the control unit records the deviation between each sensor's timestamp and the master clock, and then corrects the timestamps of the sensor data based on these deviations. This ensures that the timestamps of the 3D point cloud data, satellite positioning data, and inertial measurement data are all aligned with the master clock's time reference, achieving time synchronization. For example, if the rotating imaging radar's timestamp is E milliseconds behind the master clock, then E milliseconds are added to the timestamps of all the radar's data to match the master clock.

[0019] Step S122: Extract environmental contour feature data from the 3D point cloud data after time reference synchronization and alignment. Analyze the 3D point cloud data point by point after time reference synchronization and alignment to extract edge feature points, surface feature points, and vertex feature points of static objects in the environment around the mobile robot. Group and organize all extracted feature points according to the actual spatial position of the corresponding static objects to form a set of environmental contour feature points.

[0020] The synchronized 3D point cloud data contains point cloud information of static objects such as shelves, floors, and columns in the indoor storage environment. The control unit analyzes each frame of point cloud data point by point, identifying edge feature points of static objects through edge detection algorithms. These points are usually located at the boundaries of object surfaces, where their normal vectors change significantly. Surface feature points are determined by calculating the curvature of each point in the point cloud; points with higher curvature are considered surface feature points, reflecting the unevenness of the object's surface. Vertex feature points are obtained by finding extreme points in local areas of the point cloud, such as points at the corners of shelves or the tops of columns. After extracting these feature points, the control unit groups feature points belonging to the same static object according to their spatial coordinate relationships. For example, edge feature points, surface feature points, and vertex feature points belonging to the same shelf are grouped together, forming an environmental contour feature point set. Each element in this set is a group containing multiple feature points, and each group corresponds to a static object.

[0021] Step S123: Extract global position coordinate data from the satellite positioning data after time reference synchronization and alignment. Perform signal analysis on the satellite positioning data after time reference synchronization and alignment to extract the three-dimensional position data of the mobile robot in the global spatial coordinate system. The three-dimensional position data includes the horizontal and vertical coordinates representing the horizontal position and the elevation data representing the vertical position. Integrate and organize the extracted three-dimensional position data to form complete global position coordinate information.

[0022] The time-synchronized satellite positioning data contains various information, such as satellite ephemeris and pseudorange. The control unit performs signal analysis on this data and calculates the mobile robot's three-dimensional position data in a global spatial coordinate system (such as the WGS84 coordinate system) using a positioning algorithm. The horizontal axis is typically represented by longitude, the vertical axis by latitude, and the elevation data reflects the robot's height above sea level. In indoor warehousing environments, satellite signal interference may affect positioning accuracy, but it still provides a general global position reference. The control unit integrates the analyzed longitude, latitude, and elevation data, removing potential noise and outliers. For example, it processes multiple sets of positioning data using a moving average filter to obtain smoothed three-dimensional position data, thus forming complete global position coordinate information.

[0023] Step S124: Extract attitude change feature data from the inertial measurement data after time reference synchronization and alignment, perform motion state analysis on the inertial measurement data after time reference synchronization and alignment, extract pitch angle change data, roll angle change data and heading angle change data during the mobile robot's motion, record the continuous change trajectory of each type of angle change data and integrate the three types of angle change data with the corresponding trajectory.

[0024] The time-synchronized inertial measurement data contains the acceleration and angular velocity information of the mobile robot during its movement. The control unit analyzes this data to interpret its motion state, converting the angular velocity data into angle change data through integration. Specifically, pitch angle change data reflects the robot's rotational angle change around the lateral axis, roll angle change data reflects the robot's rotational angle change around the longitudinal axis, and yaw angle change data reflects the robot's rotational angle change around the vertical axis. The control unit records the continuous change trajectory of these three types of angle change data over time, for example, arranging the angle values ​​at each sampling moment in chronological order to form three angle change curves. Then, these three types of angle change data and their corresponding change trajectories are integrated to form attitude change feature data, which reflects the robot's attitude changes during movement.

[0025] Step S125: Based on the spatial reference system corresponding to the global position coordinate information, define a unified spatial coordinate system. The establishment of the spatial coordinate system includes determining its origin position, coordinate axis direction and length unit.

[0026] In indoor warehousing scenarios, a unified spatial coordinate system needs to be defined to uniformly process data from different sensors. Using the WGS84 coordinate system corresponding to the global position coordinate information as a reference, the control unit selects a fixed point inside the warehouse as the origin of the unified spatial coordinate system, such as a corner of the warehouse. The coordinate axes are set as follows: the X-axis points along the length of the warehouse, the Y-axis points along the width of the warehouse, and the Z-axis is perpendicular to the ground and pointing upwards. The unit of length is meters, consistent with the unit of length in the global position coordinate information. Through this definition, the unified spatial coordinate system can transform data collected by different sensors into the same coordinate framework, facilitating subsequent data processing and fusion.

[0027] Step S126: Compare the local spatial coordinate system parameters corresponding to the set of environmental contour feature points with the unified spatial coordinate system parameters, calculate the origin deviation and coordinate axis direction deviation between the local spatial coordinate system and the unified spatial coordinate system, construct a coordinate transformation relationship based on the calculated deviation values, and substitute the coordinates of all feature points in the set of environmental contour feature points into the coordinate transformation relationship to complete the transformation.

[0028] The set of environmental contour feature points is acquired in the local spatial coordinate system of the rotating imaging radar. The origin of this local coordinate system is usually located at the center of the radar, and the coordinate axis directions are related to the radar's installation direction. To transform these feature points to a unified spatial coordinate system, a coordinate transformation is required. First, the control unit acquires the parameters of the local spatial coordinate system corresponding to the set of environmental contour feature points, including the origin coordinates, coordinate axis extension directions, and coordinate scale intervals. Simultaneously, it also acquires the complete parameters of the unified spatial coordinate system. Then, the parameters of the two systems are compared item by item. Regarding the origin deviation calculation, the deviation values ​​of the origin in the X, Y, and Z directions are obtained by comparing the coordinates of the local coordinate system origin in the unified coordinate system with the coordinates of the unified coordinate system origin. The coordinate axis direction deviation is determined by calculating the angles between each coordinate axis of the local coordinate system and the corresponding coordinate axis of the unified coordinate system. The scaling factor for the coordinate scale interval is the ratio of the scale interval of the local coordinate system to the scale interval of the unified coordinate system. Based on these deviation values ​​and scaling factors, a coordinate transformation relationship is constructed, which includes three steps: origin translation, coordinate axis rotation, and scale scaling. Finally, the coordinates of each feature point in the set of environmental contour feature points are substituted into the transformation relationship, and translation, rotation and scaling operations are performed in sequence to obtain the coordinates of the feature points in a unified spatial coordinate system.

[0029] Step S1261: Obtain the set of environmental contour feature points and a unified spatial coordinate system. Extract the corresponding local spatial coordinate system parameters from the set of environmental contour feature points. Extract the origin coordinates, coordinate axis extension directions and coordinate scale intervals of the local spatial coordinate system to form complete parameters of the local spatial coordinate system.

[0030] In indoor warehousing scenarios, a rotating imaging radar is installed at a specific location on a mobile robot, with the origin of its local spatial coordinate system located at the radar's physical center. The control unit, by reading the radar's technical parameters and installation location information, determines the position of the origin of the local spatial coordinate system within the robot's body coordinate system. Then, by combining the relationship between the robot's body coordinate system and the unified spatial coordinate system, it indirectly obtains an initial estimate of the origin of the local coordinate system in the unified coordinate system. Regarding the coordinate axis directions, the radar's X-axis typically points directly in front of the robot, the Y-axis points to the robot's left, and the Z-axis points vertically upwards. The coordinate scale interval is determined by the radar's measurement accuracy; for example, each scale unit represents an actual distance of F meters. Using this information, the complete parameters of the local spatial coordinate system are extracted.

[0031] Step S1262: Extract the complete parameters from the unified spatial coordinate system. Extract the origin coordinates, coordinate axis extension directions, and coordinate scale intervals of the unified spatial coordinate system to form the complete parameters of the unified spatial coordinate system.

[0032] As mentioned earlier, the origin of the unified spatial coordinate system is set at the corner of the warehouse, with coordinates (0, 0, 0) in the unified coordinate system. The X-axis points along the length of the warehouse, the Y-axis points along the width of the warehouse, and the Z-axis points vertically upwards. The coordinate scale interval is 1 meter, meaning each coordinate unit represents 1 meter of actual distance. The control unit directly extracts these complete parameters from the preset system parameters.

[0033] Step S1263: Compare the complete parameters of the local spatial coordinate system with the complete parameters of the unified spatial coordinate system item by item. First, compare the origin coordinates of the two systems, calculate and record the three-dimensional coordinate deviation of the origin coordinates of the local spatial coordinate system relative to the origin coordinates of the unified spatial coordinate system.

[0034] The control unit compares the initial estimated value (X1, Y1, Z1) of the origin of the local spatial coordinate system in the unified coordinate system with the origin (0, 0, 0) of the unified coordinate system, calculates the three-dimensional coordinate deviation value (ΔX, ΔY, ΔZ), where ΔX=X1-0=X1, ΔY=Y1-0=Y1, ΔZ=Z1-0=Z1, and stores the above deviation value in memory.

[0035] Step S1264: Continue to compare the extension directions of the coordinate axes of the local spatial coordinate system and the unified spatial coordinate system, calculate and record the angular deviation values ​​of each coordinate axis of the local spatial coordinate system relative to the corresponding coordinate axis of the unified spatial coordinate system.

[0036] For the X-axis deviation, the control unit determines it by measuring the angle θx between the local coordinate system X-axis and the unified coordinate system X-axis; similarly, it calculates the angle θy between the Y-axis and the angle θz between the Z-axis. The above angle deviation values ​​can be obtained by the attitude sensor installed on the robot or by using known radar installation angle information, and are recorded.

[0037] Step S1265: Compare the coordinate scale intervals of the local spatial coordinate system and the unified spatial coordinate system, calculate the proportional coefficient between the coordinate scale interval of the local spatial coordinate system and the coordinate scale interval of the unified spatial coordinate system, and record it.

[0038] The scale interval of the local spatial coordinate system is F meters, and the scale interval of the unified coordinate system is 1 meter. Therefore, the scaling factor k = F / 1 = F. This scaling factor should be recorded.

[0039] Step S1266: Construct a coordinate transformation relationship based on the recorded coordinate system origin deviation value, coordinate axis angle deviation value, and coordinate scale scaling coefficient. This coordinate transformation relationship includes origin translation rules, coordinate axis rotation rules, and scale scaling rules, and is integrated into a complete coordinate transformation formula.

[0040] The construction of coordinate transformation relationships begins with a translation of the origin, shifting the point (x, y, z) in the local coordinate system by (ΔX, ΔY, ΔZ) to obtain the translated coordinates (x+ΔX, y+ΔY, z+ΔZ). Next, coordinate axis rotation is performed, using a rotation transformation matrix based on the angular deviations θx, θy, and θz. Finally, scaling is applied, multiplying the rotated coordinates by a scaling factor k to obtain the coordinates in the unified coordinate system. These rules are integrated into a complete coordinate transformation formula used to transform the coordinates of feature points in the local coordinate system to the unified coordinate system.

[0041] Step S1267: Extract the local spatial coordinates of all feature points from the set of environmental contour feature points, extract the three-dimensional coordinate data of each feature point separately and arrange them in order to form a sequence of local coordinates of feature points.

[0042] The control unit traverses each feature point group in the set of environmental contour feature points, extracting the three-dimensional coordinates (x_i, y_i, z_i) of all feature points in the local spatial coordinate system from each group, where i represents the index of the feature point. These coordinates are then arranged according to the order of the feature points in the set, forming a sequence of local coordinates for the feature points.

[0043] Step S1268: Substitute the coordinates of each feature point in the local coordinate sequence into the constructed coordinate transformation formula, and calculate the coordinates of each feature point in sequence according to the origin translation rule, coordinate axis rotation rule, and scale scaling rule.

[0044] For each coordinate (x_i, y_i, z_i) in the local coordinate sequence of a feature point, the origin translation rule is first applied to obtain (x_i+ΔX, y_i+ΔY, z_i+ΔZ). Then, the translated coordinates are substituted into the rotation transformation matrix to perform rotation calculations, obtaining the rotated coordinates. Finally, the rotated coordinates are multiplied by the scaling factor k to obtain the coordinates (X_i, Y_i, Z_i) of the feature point in a unified spatial coordinate system.

[0045] Step S1269: Based on the preset effective coordinate range of the unified spatial coordinate system, verify the three-dimensional coordinate data of each feature point obtained by calculation. For coordinate data that exceeds the preset effective coordinate range, perform iterative calculation according to the coordinate transformation formula until it falls into the effective coordinate range.

[0046] The preset effective coordinate range of the unified spatial coordinate system is set according to the actual size of the indoor storage environment. For example, the X-axis range is 0 to G meters, the Y-axis range is 0 to H meters, and the Z-axis range is 0 to I meters. For the calculated feature point coordinates (X_i, Y_i, Z_i), if its X-coordinate is less than 0 or greater than G, its Y-coordinate is less than 0 or greater than H, or its Z-coordinate is less than 0 or greater than I, then the coordinates are determined to be outside the effective range. At this time, the control unit will check whether there are errors in the coordinate transformation process, such as recalculating the origin deviation value or the angle deviation value, and resubmitting it into the coordinate transformation formula for calculation until the feature point coordinates fall within the effective range.

[0047] Step S12610: Re-associate the corrected unified coordinate data of feature points with the feature point grouping information in the environmental contour feature point set, so that the unified coordinate data of each feature point corresponds to the original static object group, and integrate and organize all the associated data to complete the coordinate transformation of the environmental contour feature point set.

[0048] After correcting the feature point coordinates, the control unit associates the unified coordinate data (X_i, Y_i, Z_i) of each feature point with its original grouping information in the environmental contour feature point set. For example, the unified coordinate data of feature points belonging to shelf A is still grouped under shelf A. Then, the unified coordinate data of feature points in all groups are integrated and organized to form a transformed environmental contour feature point set, in which the coordinates of all feature points are unified to a unified spatial coordinate system.

[0049] Step S127: Compare the inertial coordinate system parameters corresponding to the attitude change feature data with the unified spatial coordinate system parameters, calculate the origin deviation and coordinate axis direction deviation between the inertial coordinate system and the unified spatial coordinate system, construct another set of coordinate transformation relationships based on the calculated deviation values, and substitute all the angle change data in the attitude change feature data into the coordinate transformation relationship to complete the transformation.

[0050] The inertial measurement unit (IMU) also has its own inertial coordinate system. The origin of this IMU is typically located at the center of the unit, and the coordinate axes are related to the unit's installation orientation. Similar to step S126, the control unit first acquires the parameters of the IMU, including the origin coordinates, the coordinate axis extension directions, and the coordinate scale interval (for angle data, the scale interval is usually in angle units). Then, it compares these parameters with those of the unified spatial coordinate system to calculate the origin deviation and coordinate axis direction deviation. Since inertial measurement data primarily consists of angle variation data, its coordinate transformation mainly involves rotation of the coordinate axis directions. After constructing the corresponding coordinate transformation relationship, the pitch, roll, and yaw angle variation data from the attitude change characteristic data are substituted into this transformation relationship to convert them to angle representations in the unified spatial coordinate system.

[0051] Step S128: Connect the set of environmental contour feature points after coordinate transformation with the global position coordinate information, and map each feature point in the set of environmental contour feature points to a specific coordinate point in the global position coordinate information to form a one-to-one correspondence between feature points and coordinate points and generate feature coordinate association data.

[0052] After coordinate transformation, the feature point coordinates in the set of environmental contour feature points are in a unified spatial coordinate system, and the global position coordinate information is also the position of the mobile robot in a unified coordinate system. The control unit associates the feature point coordinates with the robot's global position coordinates to determine the positional relationship of each feature point relative to the robot. For example, by calculating the distance and direction between the feature point coordinates and the robot coordinates, the feature point is mapped to a specific coordinate point in the global coordinate information, thus forming feature coordinate association data. This feature coordinate association data records the position of each feature point in the global coordinate system and its relative positional relationship with the robot.

[0053] For example, step S1281: Obtain the set of environmental contour feature points after coordinate transformation and the extracted global position coordinate information, extract the unified spatial coordinate system coordinate data of all feature points from the set of environmental contour feature points, and arrange them according to static objects to form a sequence of unified coordinate data of feature points.

[0054] The control unit extracts the unified spatial coordinates (X_i, Y_i, Z_i) of each feature point from the transformed set of environmental contour feature points, and arranges them according to the static objects to which these feature points belong. For example, the coordinates of all feature points belonging to shelf A are arranged in sequence, followed by the coordinates of feature points belonging to shelf B, and so on, forming a unified coordinate data sequence of feature points.

[0055] Step S1282: Extract the real-time global coordinate data of the mobile robot from the global position coordinate information, and calculate and mark the global coordinate area within a preset range around the mobile robot in combination with the device size parameters of the mobile robot.

[0056] The global position coordinate information includes the real-time coordinates (X_robot, Y_robot, Z_robot) of the mobile robot in a unified spatial coordinate system. The device size parameters of the mobile robot include length L, width W, and height H. The control unit calculates the global coordinate region within a preset range around the robot's real-time coordinates (e.g., M meters in front, N meters behind, O meters to the left, P meters to the right, Q meters above, and R meters below). The coordinate range of this global coordinate region is from X_robot-N to X_robot+M, Y_robot-O to Y_robot+P, and Z_robot-R to Z_robot+Q, and then marks this global coordinate region.

[0057] Step S1283: Compare the coordinates of each feature point in the unified coordinate data sequence with the marked global coordinate region around the mobile robot to determine the spatial orientation and distance of each feature point relative to the mobile robot.

[0058] For each feature point coordinate (X_i, Y_i, Z_i) in the unified coordinate data sequence, the control unit determines whether it is located within the marked surrounding coordinate region. If it is within the region, the azimuth and distance of the feature point relative to the robot's real-time coordinates are calculated. The azimuth is obtained by calculating the angle between the projection of the line connecting the feature point and the robot's coordinates onto the horizontal plane and the X-axis, and the distance is obtained by calculating the Euclidean distance between the two points.

[0059] Step S1284: Using the real-time global coordinate data of the mobile robot as the position reference point, and combining the azimuth angle and straight-line distance of each feature point relative to the mobile robot, calculate the absolute coordinates of each feature point in the global spatial coordinate system.

[0060] Given the robot's real-time global coordinates (X_robot, Y_robot, Z_robot), the azimuth angle α and distance d of the feature point relative to the robot, the control unit calculates the absolute coordinates of the feature point in the global coordinate system using trigonometric functions. For example, X_i = X_robot + d * cos(α), Y_i = Y_robot + d * sin(α), and Z_i remains unchanged (assuming they are on the same height plane).

[0061] Step S1285: Based on the preset coordinate consistency rule, the absolute coordinates of the transformed feature points are verified. The coordinate consistency rule requires that the absolute coordinates of the same group of feature points should be continuously distributed within the preset spatial tolerance range. The absolute coordinates of the same group of feature points that do not meet the coordinate consistency rule are recalculated and corrected.

[0062] The preset spatial tolerance range is set based on the size and shape of the static object. For example, for a shelf, the coordinates of the same group of feature points should not deviate by more than S meters in the X, Y, and Z directions. The control unit checks whether the coordinates of the same group of feature points meet this rule. If the coordinates of a feature point deviate from the coordinates of other points in the group by more than S meters, it is considered that the coordinates may have errors. It is necessary to recalculate its azimuth and distance relative to the robot, and recalculate the absolute coordinates until the coordinate consistency rule is met.

[0063] Step S1286: Associate the corrected absolute coordinates of the feature points with the real-time global coordinate data of the mobile robot in the global position coordinate information, so that the absolute coordinates of each feature point correspond to the real-time position of the mobile robot.

[0064] The control unit stores the corrected absolute coordinates (X_i, Y_i, Z_i) of the feature points and the robot's real-time global coordinates (X_robot, Y_robot, Z_robot) in an association table, so that each feature point can be clearly mapped to the robot's position at that moment.

[0065] Step S1287: Combine the static object grouping information of the environmental contour feature point set, integrate the absolute coordinates of the feature points in the same group, and mark the static object type and overall spatial range corresponding to each group of feature points.

[0066] Based on the original static object grouping information in the set of environmental contour feature points, the control unit integrates the absolute coordinates of the feature points in the same group, determines the type of static object corresponding to each group of feature points, such as shelves, columns, etc., and calculates the overall spatial range of the object, such as the coordinate range of the smallest circumscribed cube.

[0067] Step S1288: Organize the absolute coordinates of each set of feature points and the corresponding static object information to form an environmental feature coordinate association table centered on the real-time global coordinates of the mobile robot.

[0068] The absolute coordinates, static object type, and spatial range of each set of feature points are organized into a table. The central reference point of the table is the real-time global coordinates of the mobile robot, which facilitates the subsequent search and use of feature point information.

[0069] Step S1289: Supplement the relative angle and relative distance information between each feature point and the mobile robot in the environmental feature coordinate association table, so that the association table can fully reflect the spatial positional relationship between the feature point and the mobile robot.

[0070] Add columns for relative angle and relative distance to the environmental feature coordinate association table, and fill in the azimuth angle and distance of each feature point relative to the robot calculated in step S1283 into the table, so that the association table contains more comprehensive spatial location information.

[0071] Step S12810: Format the environmental feature coordinate association table, sort the feature points from near to far according to their distance from the mobile robot, and retain the absolute coordinates, relative position information and object grouping information of all feature points to form feature coordinate association data.

[0072] The control unit sorts the data in the environmental feature coordinate association table from smallest to largest relative distance, and then integrates the absolute coordinates, relative angles, relative distances and object groupings in the table to form feature coordinate association data.

[0073] Step S129: Connect the attitude change feature data after coordinate transformation with the feature coordinate association data. Combine the angle change data and continuous change trajectory in the attitude change feature data to correct the deviation of the coordinate points in the feature coordinate association data to form the association correction data.

[0074] Attitude change feature data reflects the robot's attitude changes during movement, which cause changes in the relative positions of feature points in the robot's coordinate system. The control unit applies the pitch, roll, and yaw angle change data from the attitude change feature data to the coordinate points in the feature coordinate association data to correct for deviations. For example, when the robot's pitch angle changes, the Z-coordinate of a feature point will change accordingly. This change can be calculated from the attitude change data, thereby correcting the feature point coordinates and forming the associated correction data.

[0075] Step S1210: Based on the preset redundant data filtering rules, remove feature point data with identical coordinates from the associated correction data, and remove angle change data that exceeds the sensor range or has a rate of change exceeding a preset threshold. Integrate and organize the retained core data to generate the positioning posture data of the mobile robot.

[0076] The preset redundant data filtering rules include several aspects. First, for feature point data in the correlated correction data, if two or more feature points have exactly the same coordinates, only one is retained to remove duplicate data. Second, for angle change data, angle data exceeding the sensor's range (e.g., pitch angle range of -T to T degrees) is removed. Simultaneously, the rate of change of the angle change data is calculated, and angle data with a rate of change exceeding a preset threshold (e.g., U degrees per second) is also removed. After these filtering processes, the retained core data includes corrected feature point coordinate data and valid angle change data. Integrating and organizing this data forms positioning and attitude data that accurately describes the current position and attitude of the mobile robot.

[0077] Step S130: Based on the three-dimensional point cloud data continuously collected by the rotating imaging radar, extract the static environmental feature data and dynamic interference feature data in the environment surrounding the mobile robot. After removing the dynamic interference feature data, integrate the remaining static environmental feature data. Arrange the integrated static environmental feature data according to the actual spatial location distribution to generate a dynamic local environment map of the environment surrounding the mobile robot.

[0078] In indoor warehousing scenarios, the environment surrounding mobile robots includes not only static objects such as shelves and columns, but also dynamic objects such as other mobile robots and staff. The point cloud data of these dynamic objects can interfere with the construction of the environmental map. Therefore, it is necessary to extract static environmental feature data and dynamic interference feature data from the 3D point cloud data continuously collected by rotating imaging radar. After removing the dynamic interference, the static environmental feature data is arranged according to its actual spatial location to generate a dynamic local environmental map that can reflect changes in the surrounding environment in real time, providing environmental reference for robot navigation.

[0079] Step S131: Acquire the three-dimensional point cloud data continuously collected by the rotating imaging radar, arrange the three-dimensional point cloud data in chronological order of scanning time to form an ordered three-dimensional point cloud data sequence, perform signal filtering processing on the three-dimensional point cloud data sequence, extract feature data from the signal-filtered three-dimensional point cloud data sequence, analyze each point cloud data in the three-dimensional point cloud data sequence point by point, extract point cloud coordinate data and point cloud density data reflecting the characteristics of environmental objects, and integrate and organize the extracted point cloud coordinate data and point cloud density data to form the original environmental feature data.

[0080] The rotating imaging radar continuously acquires 3D point cloud data of the surrounding environment. The control unit arranges this data in chronological order of scanning time, forming a 3D point cloud data sequence. Since the radar data may contain noise points, such as dust in the air or random noise from the sensor, signal filtering is required. Common filtering methods include statistical filtering, which calculates the number of points within a certain range around each point cloud and removes points with fewer than a preset threshold as noise. Feature data extraction is performed on the filtered point cloud data sequence. The control unit analyzes each point cloud data point by point, extracting its 3D coordinate data (x, y, z), and simultaneously calculates the number of points within a fixed volume range around each point cloud as point cloud density data. The point cloud coordinate data and the corresponding density data are integrated to form the original environmental feature data.

[0081] Step S1311: Obtain the three-dimensional point cloud data sequence after signal filtering, analyze the three-dimensional point cloud data sequence point by point, read the three-dimensional spatial coordinate information of each point cloud data, which includes the horizontal coordinate value, the vertical coordinate value and the elevation value, and record the coordinate information of each point cloud data separately.

[0082] The control unit sequentially reads each point cloud data point from the filtered 3D point cloud data sequence and parses its 3D spatial coordinate information, namely the x-coordinate, y-coordinate, and elevation value z. For each point cloud data point, these three coordinate values ​​are stored separately in the corresponding fields of the data structure for subsequent processing.

[0083] Step S1312: Perform surrounding point cloud density statistics on each point cloud data in the three-dimensional point cloud data sequence. Delineate a fixed spatial region centered on each point cloud data and count the number of other point cloud data contained within this spatial region as the point cloud density value of that point cloud data.

[0084] For each point cloud data point (x_i, y_i, z_i), the control unit delineates a cubic spatial region centered on that point, for example, a cube with a side length of V meters. Then, the number of other point cloud data points contained within this cube is counted, and this count is used as the point cloud density value for that point cloud data point. Point cloud density can be used to distinguish different environmental objects; for example, high-density areas may correspond to solid objects, while low-density areas may correspond to open areas.

[0085] Step S1313: Associate the recorded point cloud coordinate information with the statistical point cloud density value one by one, so that the three-dimensional spatial coordinate information of each point cloud data corresponds to a unique point cloud density value, forming a data pair of point cloud coordinates and density.

[0086] The control unit associates the 3D coordinate information (x_i, y_i, z_i) of each point cloud data with its corresponding point cloud density value ρ_i, forming an associated data pair ((x_i, y_i, z_i), ρ_i). In this way, each point cloud data has both coordinate and density information, which facilitates subsequent feature extraction and classification.

[0087] Step S1314: Arrange all associated data pairs in the original order of the 3D point cloud data sequence, and remove associated data pairs whose point cloud density values ​​are lower than the preset effective density lower limit or higher than the preset effective density upper limit.

[0088] The preset effective density lower limit is ρ_min, and the upper limit is ρ_max. The control unit traverses all associated data pairs, and removes associated data pairs with point cloud density values ​​ρ_i < ρ_min or ρ_i > ρ_max from the sequence. This step can remove some outliers or noise points, because excessively low density may be isolated noise points, and excessively high density may be abnormal data caused by sensor malfunction.

[0089] Step S1315: Based on the clustering relationship of spatial coordinates, aggregate the associated data pairs whose Euclidean distance is less than the preset distance threshold into the same cluster. Each cluster corresponds to a potential environmental object region and is labeled as a cluster.

[0090] The control unit uses a clustering algorithm (such as K-means) to cluster the remaining associated data pairs. The Euclidean distance between any two associated data pairs is calculated; if the distance is less than a preset distance threshold d_th, they are clustered into the same cluster. Each cluster represents a potential environmental object region, and each cluster is uniquely labeled, such as Cluster 1, Cluster 2, etc.

[0091] Step S1316: Perform cluster feature calculation on each cluster-labeled associated data pair, calculate the three-dimensional coordinate mean vector, covariance matrix and average point cloud density of each cluster data, and associate the calculated cluster feature parameters with the cluster data.

[0092] For each cluster, the control unit calculates the mean vector of three-dimensional coordinates (μ_x, μ_y, μ_z) for all point cloud data within that cluster, where μ_x is the average of the x-coordinates of all points, and μ_y and μ_z are calculated similarly. The covariance matrix reflects the distribution of point cloud data within the cluster along the three coordinate axes. The average point cloud density is the average of the density values ​​of all point cloud data within the cluster. These cluster feature parameters are associated with the corresponding clusters and stored in the cluster's attribute information.

[0093] Step S1317: Integrate the point cloud data after associating all intra-cluster feature parameters, and retain all the original associated data pairs and corresponding intra-group feature parameters in each cluster data.

[0094] The control unit integrates the original associated data pairs of each cluster with its cluster feature parameters to form a data structure containing all point cloud data and feature information within the cluster, which facilitates subsequent classification and processing of different clusters.

[0095] Step S1318: Convert all integrated point cloud coordinate data into coordinate values ​​in a unified spatial coordinate system, and convert all point cloud density data into a unified statistical unit to ensure that the data are consistent in terms of coordinate reference and statistical unit.

[0096] Similar to step S126, the integrated point cloud coordinate data is transformed from the radar local coordinate system to a unified spatial coordinate system. The statistical unit of the point cloud density data is unified to the number of points per cubic meter. For density data previously statistically analyzed using other units, unit conversion is performed to ensure that the statistical units of all density data are consistent.

[0097] Step S1319: Rearrange the point cloud data after unifying units and coordinates according to spatial location from near to far, so that the data can reflect the distribution of the environment around the mobile robot from near to far.

[0098] The control unit calculates the distance from each point cloud data point to the current position of the mobile robot, and then rearranges all point cloud data points in order of distance from nearest to farthest. This allows for priority processing of nearby environmental features in subsequent processing, improving the real-time performance and accuracy of navigation.

[0099] Step S13110: Completely integrate the rearranged point cloud data, retaining the coordinate information, density information and spatial grouping information of all point cloud data to form the original environmental feature data.

[0100] The coordinate information, density information, and spatial grouping information of the rearranged point cloud data are integrated to form the original environmental feature data, which contains the basic feature information of potential objects in the surrounding environment.

[0101] Step S132: Based on the registration and comparison results of continuous multi-frame point cloud data, perform static and dynamic classification on the original environmental feature data, calculate the position movement vector of the environmental object corresponding to each point cloud data and the change of point cloud cluster morphology parameters within the continuous scanning cycle, and divide the original environmental feature data into static environmental feature data and dynamic interference feature data according to the preset static threshold and dynamic threshold.

[0102] The control unit acquires point cloud data from multiple consecutive frames (e.g., K consecutive frames), registers the point cloud data, and identifies corresponding point cloud clusters across different frames. For each point cloud cluster, it calculates its position movement vector within a continuous scanning cycle, i.e., the difference between the mean vector of the current frame cluster and the mean vector of the corresponding cluster in the previous frame. Simultaneously, it calculates changes in point cloud cluster morphological parameters, such as changes in the covariance matrix and average density. Preset static thresholds include a position movement vector threshold Δd and a morphological parameter change threshold Δm. If the position movement vector of a point cloud cluster is less than Δd and the morphological parameter change is less than Δm, it is classified as static environmental feature data; otherwise, it is classified as dynamic interference feature data.

[0103] Step S133: Continuously track the feature points corresponding to the dynamic interference feature data, record their spatial coordinate sequence within the continuous scanning period, calculate the instantaneous motion speed based on the time difference of the coordinate sequence, and filter out the feature points that are determined to be dynamic from the original environmental feature data.

[0104] For point cloud clusters classified as dynamic interference feature data, the control unit continuously tracks the feature points they contain. Using a feature point matching algorithm, the same dynamic feature point is identified in consecutive scan cycles, and its spatial coordinate sequence is recorded as (x_1, y_1, z_1, t_1), (x_2, y_2, z_2, t_2), ..., (x_n, y_n, z_n, t_n), where t is a timestamp. Then, the instantaneous velocity of the feature point is calculated using time difference, for example, v_x = (x_i - x_{i-1}) / (t_i - t_{i-1}), and v_y and v_z are calculated similarly. These dynamic feature points are then filtered out from the original environmental feature data to eliminate dynamic interference.

[0105] Step S134: Based on the preset point cloud geometric features and spatial distribution classification model, the pure static environment feature data after removing dynamic interference feature data is processed. The classification model divides the pure static environment feature data into ground feature data, wall feature data and fixed facility feature data according to the point cloud's normal vector, curvature, height and its clustering features in the vertical and horizontal directions.

[0106] The pre-defined point cloud geometric feature and spatial distribution classification model is a rule-based classification model. First, the normal vector of each static point cloud cluster is calculated. The normal vector of ground feature data is typically perpendicular to the ground (i.e., at a small angle to the Z-axis), while the normal vector of wall feature data is typically parallel to the ground (i.e., at a large angle to the Z-axis). Regarding curvature, the ground and walls generally have small curvature, while fixed facilities (such as shelves and columns) may have larger curvature variations. In terms of height, the elevation values ​​of ground feature data are low and relatively continuous, wall feature data has a certain vertical extension, and fixed facilities have a specific height range. Horizontal clustering features can distinguish between large, continuous areas of ground and walls, and locally clustered fixed facilities. Based on these features, the classification model divides purely static environmental feature data into three categories: ground, walls, and fixed facilities.

[0107] Step S135: Contour construction is performed on the classified ground feature data. All point cloud data in the ground feature data are arranged according to the spatial distribution of the actual ground. Continuous lines are used to connect the coordinate points corresponding to each point cloud data to form a ground contour that can reflect the undulation state and extension range of the ground.

[0108] The control unit sorts the point cloud data in the ground feature data according to their X and Y coordinates. Point cloud data with similar Z coordinates (elevation) are considered to belong to the same ground plane. Then, a contour extraction algorithm, such as the α-shape algorithm, is used to connect the above point cloud data into continuous lines to form a ground contour. This ground contour can reflect the undulation of the ground, such as whether there is slope, depression or bulge, and the extent of the ground.

[0109] Step S136: Contour construction is performed on the classified wall feature data and fixed facility feature data respectively. The point cloud data in the wall feature data is arranged and connected according to the spatial distribution of the actual wall to form the wall contour. The point cloud data in the fixed facility feature data is arranged and connected according to the spatial distribution of the actual fixed facility to form the fixed facility contour.

[0110] For wall feature data, the data is also arranged according to its spatial distribution. Since walls are usually planar structures perpendicular to the ground, the control unit can determine the planar equation of the wall using a planar fitting algorithm, then extract the boundary points of the point cloud data on that plane, and connect these boundary points to form the wall outline. For fixed facility feature data, based on the clustering pattern of its point cloud, the outermost layer of point cloud data is extracted as boundary points, and these boundary points are connected to form the outline of the fixed facility, such as the outer outline of a shelf or the circular outline of a column.

[0111] Step S137: Import the ground outline, wall outline, and fixed facility outline into a unified spatial coordinate system. According to the spatial positional relationship of the actual environmental objects corresponding to the three types of outlines, spatially superimpose and positionally calibrate the three to form a preliminary environmental outline set.

[0112] The coordinates of the constructed ground, wall, and fixture outlines are all transformed to a unified spatial coordinate system. Then, they are superimposed according to their spatial relationships in the actual environment; for example, walls typically stand on the ground, while fixtures are placed on the ground or against a wall. Position calibration ensures accurate relative positions between the different outlines, forming a preliminary environmental outline set. This preliminary environmental outline set contains the outline information of the main static objects in the indoor storage environment.

[0113] Step S138: Based on the new three-dimensional point cloud data continuously collected by the rotating imaging radar, repeat the aforementioned feature extraction and contour construction steps to generate new ground contour data, new wall contour data, and new fixed facility contour data, and supplement the new contour data into the preliminary environmental contour set.

[0114] As the mobile robot moves, the rotating imaging radar continuously acquires new 3D point cloud data. The control unit repeats steps S131 to S137 on the new point cloud data to generate new contour data for the ground, walls, and fixed structures. This new contour data is added to the initial environmental contour set, allowing the set to be continuously updated and containing more environmental information.

[0115] Step S139: Remove contour data that are far from the detection range of the mobile robot from the initial environmental contour set. Based on the preset contour smoothing and repair algorithm, process the environmental contour set after data supplementation and removal. The preset contour smoothing and repair algorithm is used to connect contour breakpoints smaller than a preset distance threshold and filter out contour protrusions with a size smaller than a preset noise threshold. The optimized contour data are fully integrated to generate a dynamic local environmental map.

[0116] During the movement of a mobile robot, some previously detected environmental objects may exceed the current detection range. The control unit calculates the distance from each contour in the initial environmental contour set to the robot's current position and removes contour data whose distance is greater than the radar's maximum detection range from the set. Then, contour smoothing and repair algorithms are applied. For discontinuities in the contours, if the distance between discontinuities is less than a preset distance threshold e, they are connected; for protrusions on the contours, if their size (such as area or length) is less than a preset noise threshold f, they are filtered out. After these processes, all optimized contour data are integrated to generate a dynamic local environment map, which can reflect the static environment around the robot in real time.

[0117] Step S140: Identify the transmission status of the satellite positioning signal based on the satellite positioning data continuously collected by the satellite positioning module. When the satellite positioning signal fails, perform dead reckoning processing based on the three-dimensional point cloud data continuously collected by the rotating imaging radar and the inertial measurement data continuously collected by the inertial measurement unit, combined with the positioning attitude data generated before the satellite positioning signal failed. Generate dead reckoning positioning data and supplement it to the positioning attitude data, replacing the failed satellite positioning-related data in the positioning attitude data to form new positioning attitude data.

[0118] In indoor warehousing environments, satellite positioning signals may be obstructed by warehouse roofs, shelves, etc., leading to signal failure or decreased accuracy. When satellite positioning signals fail, mobile robots need to rely on data from other sensors for positioning. This step identifies the transmission status of the satellite positioning signal. When the signal fails, it uses 3D point cloud data from a rotating imaging radar and inertial measurement data, combined with the positioning attitude data before the signal failure, to perform dead reckoning and generate new positioning data, ensuring the continuity and accuracy of robot positioning.

[0119] Step S141: Obtain satellite positioning data continuously collected by the satellite positioning module, extract signal features from the satellite positioning data, extract signal transmission strength data, signal transmission stability data and signal transmission continuity data from the satellite positioning data, and integrate and organize them to form satellite positioning signal feature data.

[0120] The control unit acquires satellite positioning data output by the satellite positioning module in real time. This data contains information related to the satellite signal. Signal transmission strength data can be characterized by the signal-to-noise ratio (SNR) in the satellite positioning data; the higher the SNR value, the stronger the signal. Signal transmission stability data can be obtained by calculating the variance of the SNR over multiple consecutive sampling periods; the smaller the variance, the more stable the signal. Signal transmission continuity data is determined by detecting whether signal interruptions occur and the frequency of these interruptions, such as the number of signal interruptions in the past G sampling periods. These signal characteristic data are then integrated to form satellite positioning signal characteristic data.

[0121] Step S142: Based on a preset signal quality assessment algorithm, process the satellite positioning signal feature data. The signal quality assessment algorithm is based at least on the deviation between the historical average and the current value of the signal transmission strength, the variance of the signal transmission stability, and the interruption duration and frequency of the signal transmission continuity, and outputs the transmission status determination result of the satellite positioning signal.

[0122] The preset signal quality assessment algorithm first calculates the historical average of the signal transmission strength, such as the average SNR μ_snr over the past H sampling periods. Then, it calculates the deviation of the current SNR value from μ_snr: d_snr = |current_snr - μ_snr| / μ_snr. The variance σ_snr of signal transmission stability has already been calculated. The interruption duration t_break and interruption frequency f_break of signal transmission continuity have also been extracted. The algorithm compares these parameters with preset thresholds. For example, if d_snr > d_th, σ_snr > σ_th, t_break > t_th, or f_break > f_th, the satellite positioning signal transmission status is determined to be failed; otherwise, it is determined to be normal.

[0123] Step S143: When the satellite positioning signal is determined to be in a failed state, obtain the positioning attitude data generated before the satellite positioning signal failed, extract the position coordinate data and attitude change data before the failure from the positioning attitude data before the failure, and integrate and organize them to form dead reckoning reference data.

[0124] Once the satellite positioning signal is determined to be faulty, the control unit immediately extracts the position coordinates (X0, Y0, Z0) and attitude change data before the failure from the most recent valid positioning and attitude data, including pitch angle θ0, roll angle φ0, and heading angle ψ0. This data is then integrated to form the baseline data for dead reckoning, which will be used as the basis for subsequent dead reckoning calculations.

[0125] Step S144: Obtain the three-dimensional point cloud data continuously collected by the rotating imaging radar after the satellite positioning signal fails, extract displacement features from the three-dimensional point cloud data, extract the spatial position change and travel direction change trajectory of the mobile robot within the continuous scanning cycle of the rotating imaging radar, and integrate and organize them to form relative displacement feature data.

[0126] After the satellite positioning signal fails, the rotating imaging radar continues to acquire 3D point cloud data. The control unit processes this point cloud data, registers it with previous point cloud data, and calculates the robot's spatial position change within a continuous scanning cycle. Simultaneously, it determines the trajectory of the changing direction of travel based on the position change, integrating the above information to form relative displacement feature data for dead reckoning.

[0127] Step S1441: Acquire the three-dimensional point cloud data continuously collected by the rotating imaging radar after the satellite positioning signal fails, divide the three-dimensional point cloud data according to the scanning cycle of the rotating imaging radar, and form a sequence of three-dimensional point cloud data units arranged according to the scanning cycle by treating the three-dimensional point cloud data collected in each scanning cycle as an independent data unit.

[0128] The scanning cycle of the rotating imaging radar is T seconds. The control unit divides the three-dimensional point cloud data acquired after the signal fails into a data unit every T seconds in time sequence, forming a three-dimensional point cloud data unit sequence, such as unit 1 (t0 to t0+T), unit 2 (t0+T to t0+2T), etc.

[0129] Step S1442: Extract three-dimensional point cloud data units from two adjacent scanning cycles from the three-dimensional point cloud data unit sequence, mark them as the point cloud data unit of the previous cycle and the point cloud data unit of the next cycle respectively, and import them synchronously.

[0130] The control unit extracts unit i and unit i+1 from the sequence, using them as point cloud data units for the previous and next cycles, respectively, and imports them into memory for processing.

[0131] Step S1443: Perform environmental feature matching on the point cloud data unit of the previous period and the point cloud data unit of the next period, extract the same static environmental feature points in the two data units as reference matching points, and record the coordinate data of each reference matching point in the two data units.

[0132] Using feature point matching algorithms (such as SIFT or ICP), identical static environmental feature points are found in the point cloud data cells of the previous and subsequent cycles. These points are fixed points in the robot's surrounding environment, such as corners of shelves or ground markings. The coordinate data (x_prev, y_prev, z_prev) and (x_curr, y_curr, z_curr) of these reference matching points in the two cells are recorded.

[0133] Step S1444: Using the coordinate data of the reference matching point as a reference, compare the relative positions of the reference matching point in the two data units, calculate the displacement components of the mobile robot along each coordinate axis in the unified spatial coordinate system from the previous scan cycle to the next scan cycle, and integrate all displacement components to obtain the single-cycle spatial displacement in adjacent scan cycles.

[0134] For each reference matching point, calculate its coordinate change in the next cycle relative to the previous cycle: Δx = x_curr - x_prev, Δy = y_curr - y_prev, Δz = z_curr - z_prev. Due to the robot's motion, these Δx, Δy, and Δz are actually manifestations of the robot's displacement in the opposite direction. Therefore, the robot's displacement components are dx = -Δx, dy = -Δy, and dz = -Δz. The average of the displacement components calculated for all reference matching points is taken as the single-cycle spatial displacement (dx_avg, dy_avg, dz_avg) within adjacent scanning cycles.

[0135] Step S1445: Repeat the aforementioned benchmark matching and displacement calculation steps for all adjacent period point cloud data units in the 3D point cloud data unit sequence, calculate the single-cycle spatial displacement of each adjacent scanning cycle, and arrange all single-cycle spatial displacements in the order of scanning cycles.

[0136] The control unit repeats steps S1443 and S1444 for each pair of adjacent units (unit 1 and unit 2, unit 2 and unit 3, etc.) in the three-dimensional point cloud data unit sequence, calculates the single-cycle spatial displacement of each adjacent cycle, and arranges them in chronological order to form a displacement sequence.

[0137] Step S1446: Extract the trajectory of the mobile robot's direction of travel from the arranged single-cycle spatial displacements, determine the direction of travel of the mobile robot in each scanning cycle based on the three-dimensional coordinate changes of each single-cycle spatial displacement, and connect the directions of travel of all scanning cycles in chronological order.

[0138] For each single-cycle spatial displacement (dx_avg, dy_avg, dz_avg), calculate its orientation angle α = arctan2(dy_avg, dx_avg) on ​​the horizontal plane. This orientation angle represents the robot's travel direction within that scan cycle. Connect the orientation angles of all scan cycles in chronological order to form the trajectory of the change in travel direction.

[0139] Step S1447: Based on the preset trajectory filtering algorithm, process the connected trajectory of changing driving direction, remove discrete points in the trajectory whose direction angle changes exceed the preset abrupt change threshold, and record the scanning period and spatial displacement corresponding to each direction change point.

[0140] The preset trajectory filtering algorithm can be either moving average filtering or Kalman filtering. For each direction angle α_i in the trajectory of changing driving direction, if |α_i-α_{i-1}|>Δα_th (preset abrupt change threshold), the point is considered a discrete point and is removed. Then, the scanning period and the spatial displacement of each direction change point in the filtered trajectory are recorded.

[0141] Step S1448: Associate the sequentially arranged single-cycle spatial displacement with the smoothed driving direction change trajectory, so that the single-cycle spatial displacement of each scanning cycle corresponds to a specific direction point in the driving direction change trajectory.

[0142] The control unit associates each displacement in the single-cycle spatial displacement sequence with the direction point of the corresponding scanning cycle in the trajectory of the change in driving direction, forming a (displacement, direction angle) association pair, which facilitates subsequent dead reckoning.

[0143] Step S1449: Based on the associated displacement and driving direction data, calculate the cumulative spatial position change of the mobile robot from the time the satellite positioning signal failed to the current moment, and integrate the cumulative spatial position change with the single-cycle spatial displacement and driving direction change trajectory.

[0144] Starting from the point where the satellite positioning signal fails, the single-cycle spatial displacement of each scanning cycle is accumulated to obtain the cumulative spatial position change (Σdx, Σdy, Σdz). The cumulative change is then integrated with the single-cycle displacement and the trajectory of the travel direction to form relative displacement characteristic data.

[0145] Step S14410: Convert all the integrated displacement data into a unified length unit, convert all the driving direction data into a unified angle unit, and format the displacement and direction data after unit unification to form relative displacement feature data.

[0146] Ensure that all displacement data is in meters and travel direction data is in degrees. Standardize the data format, for example, by storing displacement and direction angles in arrays or structures to form relative displacement characteristic data.

[0147] Step S145: Obtain inertial measurement data continuously collected by the inertial measurement unit after the satellite positioning signal fails, extract attitude increments from the inertial measurement data, extract the pitch angle increment, roll angle increment, and heading angle increment of the mobile robot in the continuous scanning cycle, record the numerical sequence of each angle increment over time, and integrate the various angle increments with their corresponding time sequences.

[0148] Even after signal failure, the inertial measurement unit continues to collect data. The control unit processes this data, extracting the pitch angle increment Δθ, roll angle increment Δφ, and yaw angle increment Δψ for each scan cycle through integral calculations of acceleration and angular velocity. The sequence of these angle increments over time is recorded, such as Δθ_1, Δθ_2, ..., Δθ_n (corresponding to different scan cycles), and the angle increment sequence is correlated and integrated with the time series.

[0149] Step S146: Substitute the spatial position change in the relative displacement feature data into the coordinate transformation rules of the unified spatial coordinate system, convert the spatial position change into displacement increment data with the same coordinate reference as the position coordinate data before failure, and associate the converted displacement increment data with the trajectory of the change in driving direction to form displacement increment data under the unified coordinate system.

[0150] The spatial position change in the relative displacement feature data may be in the radar local coordinate system, which needs to be transformed to a unified spatial coordinate system. The transformation method is similar to step S126. The transformed displacement increment data is associated with the trajectory of the change in driving direction to form displacement increment data in a unified coordinate system, ensuring that it is consistent with the coordinate reference of the position coordinate data before failure.

[0151] Step S147: Using the position coordinates before failure in the dead reckoning baseline data as the starting coordinates, the displacement increment data in the unified coordinate system in each scanning cycle is superimposed sequentially to calculate the instantaneous position coordinates of the mobile robot at the end of each scanning cycle, and all the calculated instantaneous position coordinates are arranged continuously in the order of the scanning cycles.

[0152] The pre-failure position coordinates in the dead reckoning baseline data are (X0, Y0, Z0). The displacement increment for the first scan cycle is (dx_1, dy_1, dz_1), so the instantaneous position coordinates at the end of this cycle are (X0+dx_1, Y0+dy_1, Z0+dz_1). The displacement increment for the second scan cycle is (dx_2, dy_2, dz_2), and the instantaneous position coordinates are the coordinates of the previous cycle plus the displacement increment of this cycle, and so on. All instantaneous position coordinates are arranged in scan cycle order.

[0153] Step S148: Combine the extracted attitude change increment data to perform attitude correction on each of the arranged instantaneous position coordinates. Adjust the attitude parameters of the corresponding instantaneous position coordinates according to the attitude change increment data in each scanning cycle so that each instantaneous position coordinate and its corresponding attitude parameters form a precise match.

[0154] The attitude change increment for each scan cycle is (Δθ_i, Δφ_i, Δψ_i). Starting with the attitude parameters (θ0, φ0, ψ0) before failure, the attitude parameters after the first scan cycle are (θ0+Δθ_1, φ0+Δφ_1, ψ0+Δψ_1), and for the second cycle, (θ0+Δθ_1+Δθ_2, ...), and so on. The instantaneous position coordinates of each scan cycle are matched with the corresponding attitude parameters to form (position coordinates, attitude parameters) pairs.

[0155] Step S149: Extract the real-time position coordinates and real-time attitude data of the mobile robot at the current moment from the matched instantaneous position coordinates and corresponding attitude parameters, and integrate and organize the extracted real-time position coordinates and real-time attitude data to generate dead reckoning positioning data.

[0156] The scanning period corresponding to the current moment is n. The instantaneous position coordinates (Xn, Yn, Zn) and attitude parameters (θn, φn, ψn) of the nth period are extracted from the matched correlation pairs. The above data are integrated together to generate dead reckoning positioning data.

[0157] Step S1410: Substitute the dead reckoning positioning data into the positioning attitude data after the satellite positioning signal fails, replace the failed satellite positioning-related position coordinate data and attitude data, retain other valid environmental contour feature data and motion state data in the positioning attitude data, and perform complete association and integration of the positioning attitude data after data replacement to form new positioning attitude data.

[0158] The control unit replaces the invalid satellite positioning-related parts of the positioning attitude data with the position coordinates and attitude parameters from the dead reckoning positioning data, while retaining other valid data such as environmental contour feature data. Then, the replaced data is correlated and integrated to ensure consistency and accuracy among the various data parts, forming new positioning attitude data for subsequent navigation control.

[0159] Step S150: Combine the new positioning attitude data and dynamic local environment map with the target driving data of the mobile robot, perform path planning and driving control processing to generate initial navigation control commands, continuously collect subsequent 3D point cloud data, satellite positioning data and inertial measurement data through rotating imaging radar, satellite positioning module and inertial measurement unit, repeat the above processing steps to generate updated positioning attitude data and updated dynamic local environment map, perform dynamic adjustment processing on the initial navigation control commands based on the updated positioning attitude data and updated dynamic local environment map, generate adjusted navigation control commands and control the mobile robot to drive according to the adjusted navigation control commands to complete the navigation operation.

[0160] In indoor warehousing scenarios, the goal of a mobile robot is to transport goods from its current location to a designated target location. This step combines new positioning and attitude data (reflecting the robot's current position and attitude), a dynamic local environment map (reflecting the surrounding environment), and target driving data (including the target location and driving requirements) to perform path planning and driving control, generating initial navigation control commands. Then, by continuously collecting and processing sensor data, the positioning attitude and environment map are constantly updated, and the initial commands are dynamically adjusted to ensure that the robot can accurately and safely reach the target location.

[0161] Step S151: Acquire new positioning attitude data, dynamic local environment map and target driving data of mobile robot, and perform spatiotemporal synchronization calibration of the three based on timestamp and unified spatial coordinate system to form calibrated navigation basic data. The target driving data includes target point coordinates, path key point coordinate sequence and target speed value.

[0162] The control unit acquires new positioning attitude data (including timestamp T1), a dynamic local environment map (including timestamp T2), and target travel data (including timestamp T3). Using the robot's internal master clock as a reference, these three timestamps are calibrated to ensure time synchronization. Simultaneously, the target point coordinates and path key point coordinate sequences in the target travel data are transformed to a unified spatial coordinate system, maintaining consistency with the coordinate reference of the positioning attitude data and the environment map. These synchronized and calibrated data are integrated to form the basic navigation data. The target point coordinates in the target travel data are (X_goal, Y_goal, Z_goal), and the path key point coordinate sequence is a series of ordered coordinate points (X1, Y1, Z1), (X2, Y2, Z2),..., used to guide the robot's movement, with a target speed value of v_goal.

[0163] Step S152: Extract the current position feature data and current attitude feature data of the mobile robot from the calibrated navigation base data. The current position feature data includes the real-time horizontal coordinate, real-time vertical coordinate and real-time elevation in a unified spatial coordinate system. The current attitude feature data includes the real-time pitch angle, real-time roll angle and real-time heading angle.

[0164] The current position feature data (X_current, Y_current, Z_current) and current attitude feature data (θ_current, φ_current, ψ_current) are extracted from the positioning and attitude data of the navigation basic data. These data accurately reflect the robot's current spatial position and attitude.

[0165] Step S153: Extract drivable area feature data and obstacle feature data from the dynamic local environment map in the calibrated navigation base data. The drivable area feature data includes the coordinates of the polygon vertices that constitute the boundary of the drivable area, and the obstacle feature data includes the coordinates of the bounding box vertices of all obstacles and the size of their circumscribed cubes.

[0166] The dynamic local environment map includes static environmental features such as the ground, walls, and fixed structures. The control unit extracts the drivable area from the map, typically the area on the ground excluding obstacles, and describes its boundaries using polygon vertex coordinates, such as (A1, B1), (A2, B2), ..., (An, Bn) (in a two-dimensional plane). Obstacle feature data includes the bounding box vertex coordinates of each obstacle, such as (C1, D1, E1), (C2, D2, E2), ..., as well as the length, width, and height dimensions of the circumscribed cube.

[0167] Step S154: Compare the current position feature data with the target driving position coordinates, calculate the driving direction and driving distance of the mobile robot from the current position to the target driving position, and combine the driving area feature data to delineate multiple candidate driving paths within the calculated driving direction and driving distance range.

[0168] Calculate the driving direction α_goal = arctan2(Y_goal - Y_current, X_goal - X_current) between the current position (X_current, Y_current) and the target position (X_goal, Y_goal), and the driving distance d_goal = sqrt((X_goal - X_current)^2 + (Y_goal - Y_current)^2). Within the drivable area, starting from the current position and ending at the target position, delineate multiple candidate driving paths based on the shape and size of the drivable area, such as a straight path, a path around the left obstacle, and a path around the right obstacle.

[0169] Step S155: Combine obstacle feature data to perform spatial position analysis on the multiple candidate driving paths, analyze the spatial position relationship between each candidate driving path and the obstacle, remove candidate driving paths that spatially overlap with the obstacle, and retain candidate driving paths that maintain a safe driving distance from all obstacles.

[0170] For each candidate driving path, the control unit analyzes its spatial relationship with all obstacles in the obstacle feature data. If the path spatially overlaps with the bounding box of an obstacle (i.e., a point on the path is located within the bounding box of an obstacle), the path is removed. For paths without overlap, the minimum distance between the path and each obstacle is calculated. If the minimum distance is greater than or equal to the preset safe driving distance s, the path is retained; otherwise, the path is removed.

[0171] Step S1551: Obtain multiple candidate driving path data and extracted obstacle feature data, extract single candidate driving path data from the multiple candidate driving path data, extract all contour coordinate points and path width data of the path and integrate them.

[0172] The control unit extracts a path from the candidate driving path data and extracts all its contour coordinate points (P1, P2, ..., Pm). These points are connected sequentially to form the centerline of the path. The path width data w represents the lateral space required for the robot to move.

[0173] Step S1552: Extract feature data of a single obstacle from the obstacle feature data, extract all contour coordinate points, position center coordinates and external dimensions of the obstacle and integrate them.

[0174] Extract an obstacle from the obstacle feature data and extract its contour coordinates (Q1, Q2, ..., Qk), position center coordinates (X_obs, Y_obs, Z_obs), and external dimensions (length L_obs, width W_obs, height H_obs).

[0175] Step S1553: Determine the spatial coverage of the path based on the contour coordinates and path width data of a single candidate driving path, and determine the spatial coverage of the obstacle based on the contour coordinates and dimensions data of the obstacle.

[0176] The spatial coverage of a path is a strip-shaped region of width w, centered on the path's centerline. The boundaries of this region can be determined by shifting the path's centerline to either side by a distance w / 2 and connecting the shifted contour points. The spatial coverage of an obstacle is its circumscribed cube, determined by its center coordinates and external dimensions. For example, the X range is from X_obs-L_obs / 2 to X_obs+L_obs / 2, the Y range is from Y_obs-W_obs / 2 to Y_obs+W_obs / 2, and the Z range is from Z_obs-H_obs / 2 to Z_obs+H_obs / 2.

[0177] Step S1554: Calculate the intersection area between the spatial coverage polygon of the path and the spatial coverage polygon of the obstacle. If the intersection area is greater than zero, it is determined that there is spatial overlap, and the candidate driving path is marked as an unqualified path.

[0178] Project both the spatial coverage of the path and the spatial coverage of the obstacles onto a two-dimensional plane (ignoring the Z-axis), forming two polygons. Calculate their intersection area using a polygon intersection algorithm. If the intersection area is greater than zero, it indicates spatial overlap between the path and the obstacles, and the path is marked as unacceptable.

[0179] Step S1555: When there is no spatial overlap between the path and the obstacle, calculate the minimum spatial distance between the path and the obstacle, and compare the calculated minimum spatial distance with the preset static safety distance threshold; if the minimum spatial distance is less than the static safety distance threshold, mark the candidate driving path as an unqualified path, and if the minimum spatial distance is greater than or equal to the safe driving distance standard, mark the candidate driving path as a qualified path.

[0180] If the path and obstacles do not spatially overlap, calculate the minimum distance from all points on the path to the obstacle boundary. This minimum spatial distance can be approximated by calculating the distance from the path centerline to the obstacle center, subtracting half the path width and half the obstacle width. Compare this distance to a static safety distance threshold s; if it is less than s, it is marked as unacceptable; if it is greater than or equal to s, it is marked as acceptable.

[0181] Step S1556: Repeat the aforementioned spatial position analysis and qualification judgment steps with all individual obstacles for a single candidate driving path data. If the judgment result with all obstacles is qualified, the path is retained; if any one of them is unqualified, the path is removed.

[0182] For a single candidate driving path, spatial position analysis and qualification assessment are required against all obstacles in the obstacle feature data. The path is retained only if the assessment results against all obstacles are qualified; if the assessment result against even one obstacle is unqualified, the path is removed.

[0183] Step S1557: Repeat all the aforementioned screening steps for all individual candidate driving path data in the multiple candidate driving path data, and complete the spatial position analysis and qualification judgment of each candidate driving path and all obstacles, and record the final screening result of each path.

[0184] The control unit performs the above screening steps on each candidate driving path and records the final result of whether each path is qualified.

[0185] Step S1558: Summarize all qualified paths from the screening results, extract complete data for all qualified paths, including path outline coordinates, path width data, and driving direction data, and integrate and organize them.

[0186] All data from qualified candidate driving paths are aggregated, including the path's outline coordinates, path width, and driving direction, and integrated for subsequent processing.

[0187] Step S1559: Convert the coordinate data of all qualified paths into coordinate values ​​in a unified spatial coordinate system to ensure that the coordinate reference of all qualified paths is consistent. Sort the qualified path data after coordinate calibration according to the driving distance from shortest to longest.

[0188] Ensure that the coordinate data of all qualified paths are in a unified spatial coordinate system, then calculate the driving distance (path length) of each path, and sort them from shortest to longest distance to facilitate the selection of the optimal path.

[0189] Step S156: Based on the preset path smoothness constraints and dynamic constraints, and combined with the target driving trajectory requirements and target driving speed related data in the current posture feature data and target driving data, adjust the retained candidate driving paths, calculate and adjust the curvature and heading angle change rate of each candidate driving path to meet the minimum turning radius limit of the mobile robot and match the steering capability under the target driving speed.

[0190] The preset path smoothness constraint requires the path curvature to change continuously and gently, avoiding sharp turns. Dynamic constraints include the mobile robot's minimum turning radius R_min and maximum steering angular velocity ω_max. The control unit adjusts the retained candidate travel paths based on these constraints. For example, for turning sections of the path, if the turning radius is less than R_min, the path is adjusted by increasing the turning radius; simultaneously, the rate of change of the path's heading angle is calculated to ensure it does not exceed ω_max, matching the robot's steering capability at the target travel speed v_goal.

[0191] Step S157: Select the path with the shortest driving distance, the smallest attitude adjustment range, and that fully meets the requirements of the target driving trajectory from the optimized candidate driving paths as the optimal driving path, and extract all contour coordinates and driving parameters of the optimal driving path.

[0192] Among the optimized candidate driving paths, the control unit comprehensively considers factors such as driving distance, attitude adjustment range (e.g., total change in heading angle), and whether it meets the target driving trajectory requirements (e.g., whether it passes through all path key points) to select the optimal driving path. The contour coordinates, driving direction, and driving speed of this path are extracted.

[0193] Step S158: Generate initial navigation control instructions based on the contour coordinates and driving parameters of the optimal driving path. The initial navigation control instructions include the mobile robot's driving direction adjustment data, driving speed control data, and driving angle correction data.

[0194] Based on the contour coordinates of the optimal driving path, the robot calculates the driving direction adjustment data (such as steering angle) for each path segment; based on the target driving speed and path curvature, driving speed control data (such as acceleration and deceleration commands) is generated; based on the current attitude characteristic data and path requirements, driving angle correction data (such as pitch and roll angle corrections) is generated. The above data are then integrated to form the initial navigation control commands.

[0195] Step S159: Continuously acquire subsequent 3D point cloud data, satellite positioning data, and inertial measurement data through rotating imaging radar, satellite positioning module, and inertial measurement unit, repeat all processing steps, and generate updated positioning attitude data and updated dynamic local environment map.

[0196] During the robot's movement, the sensors continuously collect data, and the control unit repeats steps S110 to S140 to continuously update the positioning posture data and dynamic local environment map to reflect the real-time changes in the robot's position and environment.

[0197] Step S1510: Correct the driving direction adjustment data and driving angle correction data in the initial navigation control command according to the updated positioning attitude data, adjust the driving speed control data and driving path planning data in the initial navigation control command according to the updated dynamic local environment map, integrate all the adjusted control data to generate the adjusted navigation control command and transmit it to the driving control component of the mobile robot.

[0198] The updated positioning and attitude data may deviate from the initial predicted position and attitude. The control unit corrects the driving direction and angle adjustment data based on these deviations, enabling the robot to accurately track the optimal path. The updated dynamic local environment map may detect new obstacles or changes in drivable areas. The control unit adjusts the driving speed control data (e.g., slowing down when encountering new obstacles) and path planning data (e.g., replanning a path to bypass new obstacles) accordingly. All adjusted control data are integrated to generate the final navigation control command, which is transmitted to the robot's driving control unit to control the robot to drive according to the command and complete the navigation operation.

[0199] In one exemplary embodiment, a mobile robot navigation system based on rotating imaging radar and satellite positioning is provided. This system can be a terminal, server, etc., and its internal structure diagram can be as follows: Figure 2 As shown, this mobile robot navigation system based on rotating imaging radar and satellite positioning includes a processor, memory, input / output interface, communication interface, display unit, and input device. The processor, memory, and input / output interface are connected via a system bus, and the communication interface, display unit, and input device are also connected to the system bus via the input / output interface. The processor provides computational and control capabilities. The memory includes a non-volatile storage medium and internal memory. The non-volatile storage medium stores the operating system and computer programs. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage medium. The input / output interface is used for exchanging information between the processor and external devices. The communication interface is used for wired or wireless communication with external terminals; wireless communication can be achieved through Wi-Fi, mobile cellular networks, near-field communication, or other technologies. When the computer program is executed by the processor, it implements a mobile robot navigation method based on rotating imaging radar and satellite positioning. The display unit is used to form a visually visible image and can be a display screen, projection device, or virtual reality imaging device. The display screen can be an LCD screen or an e-ink screen. The input device can be a touch layer covering the display screen, or a button, trackball, or touchpad set on the shell of a mobile robot navigation system based on rotating imaging radar and satellite positioning, or an external keyboard, touchpad, or mouse, etc.

[0200] It should be noted that, in order to simplify the description of the present invention and thus help to understand one or more embodiments of the invention, multiple features may sometimes be grouped into one embodiment, drawing or description thereof in the foregoing description of the embodiments of the present invention.

Claims

1. A mobile robot navigation method based on rotating imaging radar and satellite positioning, characterized in that, The method includes: The rotating imaging radar, satellite positioning module, and inertial measurement unit on the mobile robot are activated. The rotating imaging radar enters a continuous omnidirectional rotating scanning state to collect three-dimensional point cloud data of the environment around the mobile robot. The satellite positioning module simultaneously receives positioning signals transmitted by the satellite to generate satellite positioning data. The inertial measurement unit simultaneously senses the motion state of the mobile robot to generate inertial measurement data. Feature information is extracted from the three-dimensional point cloud data, satellite positioning data, and inertial measurement data, and coordinate system transformation is completed. The interrelationship of feature information is established, and all the associated feature information is integrated to generate the positioning posture data of the mobile robot. Based on the three-dimensional point cloud data continuously collected by the rotating imaging radar, static environmental feature data and dynamic interference feature data of the environment around the mobile robot are extracted. After removing the dynamic interference feature data, the remaining static environmental feature data are integrated. The integrated static environmental feature data are arranged according to the actual spatial location to generate a dynamic local environment map of the environment around the mobile robot. The satellite positioning signal transmission status is identified based on the satellite positioning data continuously collected by the satellite positioning module. When the satellite positioning signal fails, dead reckoning is performed based on the three-dimensional point cloud data continuously collected by the rotating imaging radar and the inertial measurement data continuously collected by the inertial measurement unit, combined with the positioning attitude data generated before the satellite positioning signal failed. Dead reckoning positioning data is generated and added to the positioning attitude data, replacing the failed satellite positioning-related data in the positioning attitude data to form new positioning attitude data. The new positioning attitude data and dynamic local environment map are combined with the target driving data of the mobile robot to perform path planning and driving control processing to generate initial navigation control commands. Subsequent 3D point cloud data, satellite positioning data, and inertial measurement units are continuously collected through rotating imaging radar, satellite positioning module, and inertial measurement unit. The aforementioned processing steps are repeated to generate updated positioning attitude data and updated dynamic local environment map. Based on the updated positioning attitude data and updated dynamic local environment map, the initial navigation control commands are dynamically adjusted to generate adjusted navigation control commands and control the mobile robot to drive according to the adjusted navigation control commands to complete the navigation operation.

2. The mobile robot navigation method based on rotating imaging radar and satellite positioning according to claim 1, characterized in that, The process of extracting feature information from the 3D point cloud data, satellite positioning data, and inertial measurement data, performing coordinate system transformation, establishing interrelationships among the feature information, and integrating all the correlated feature information to generate the positioning attitude data of the mobile robot includes: The system acquires 3D point cloud data collected by rotating imaging radar, satellite positioning data generated by satellite positioning module, and inertial measurement data sensed by inertial measurement unit. It then marks the acquisition time of the 3D point cloud data, satellite positioning data, and inertial measurement data to form a synchronized time reference. Environmental contour feature data is extracted from the 3D point cloud data after time reference synchronization and alignment. The 3D point cloud data after time reference synchronization and alignment is analyzed point by point to extract the edge feature points, surface feature points and vertex feature points of static objects in the environment around the mobile robot. All extracted feature points are grouped and organized according to the actual spatial position of the corresponding static objects to form an environmental contour feature point set. Global position coordinate data is extracted from the satellite positioning data after time reference synchronization and alignment. Signal analysis is performed on the satellite positioning data after time reference synchronization and alignment to extract the three-dimensional position data of the mobile robot in the global spatial coordinate system. The three-dimensional position data includes the horizontal and vertical coordinates representing the horizontal position and the elevation data representing the vertical position. The extracted three-dimensional position data is integrated and organized to form complete global position coordinate information. The attitude change feature data is extracted from the inertial measurement data after time reference synchronization and alignment. The motion state is analyzed from the inertial measurement data after time reference synchronization and alignment. The pitch angle change data, roll angle change data and heading angle change data of the mobile robot during the motion process are extracted. The continuous change trajectory of each angle change data is recorded and the three types of angle change data and corresponding trajectories are integrated. Based on the spatial reference system corresponding to the global position coordinate information, a unified spatial coordinate system is defined. The establishment of this spatial coordinate system includes determining its origin position, coordinate axis orientation, and length unit. The local spatial coordinate system parameters corresponding to the set of environmental contour feature points are compared with the unified spatial coordinate system parameters. The origin deviation and coordinate axis direction deviation between the local spatial coordinate system and the unified spatial coordinate system are calculated. Based on the calculated deviation values, a coordinate transformation relationship is constructed. The coordinates of all feature points in the set of environmental contour feature points are substituted into the coordinate transformation relationship to complete the transformation. The parameters of the inertial coordinate system corresponding to the attitude change feature data are compared with the parameters of the unified spatial coordinate system. The origin deviation and coordinate axis direction deviation between the inertial coordinate system and the unified spatial coordinate system are calculated. Based on the calculated deviation values, another set of coordinate transformation relationships is constructed. All angle change data in the attitude change feature data are substituted into this coordinate transformation relationship to complete the transformation. The set of environmental contour feature points after coordinate transformation is associated with the global position coordinate information. Each feature point in the set of environmental contour feature points is mapped to a specific coordinate point in the global position coordinate information, forming a one-to-one correspondence between feature points and coordinate points and generating feature coordinate association data. The attitude change feature data after coordinate transformation is linked with the feature coordinate association data. Combining the angle change data and continuous change trajectory in the attitude change feature data, the coordinate points in the feature coordinate association data are corrected for deviation to form association correction data. Based on preset redundant data filtering rules, feature point data with identical coordinates in the associated correction data are removed, and angle change data that exceeds the sensor's measurement range or has a rate of change exceeding a preset threshold are also removed. The remaining core data are then integrated and organized to generate the positioning posture data of the mobile robot.

3. The mobile robot navigation method based on rotating imaging radar and satellite positioning according to claim 1, characterized in that, The process involves identifying the transmission status of the satellite positioning signal based on the satellite positioning data continuously collected by the satellite positioning module. When the satellite positioning signal fails, dead reckoning is performed based on the three-dimensional point cloud data continuously collected by the rotating imaging radar and the inertial measurement data continuously collected by the inertial measurement unit, combined with the positioning attitude data generated before the satellite positioning signal failure. Dead reckoning positioning data is generated and added to the positioning attitude data, replacing the failed satellite positioning-related data in the positioning attitude data to form new positioning attitude data, including: The system acquires satellite positioning data continuously collected by the satellite positioning module, extracts signal features from the satellite positioning data, extracts signal transmission strength data, signal transmission stability data, and signal transmission continuity data from the satellite positioning data, and integrates and organizes them to form satellite positioning signal feature data. Based on a preset signal quality assessment algorithm, satellite positioning signal feature data is processed. The signal quality assessment algorithm is based at least on the deviation between the historical mean and the current value of signal transmission strength, the variance of signal transmission stability, and the interruption duration and frequency of signal transmission continuity, and outputs the transmission status determination result of the satellite positioning signal. When the satellite positioning signal is determined to be in a failed state, the positioning attitude data generated before the satellite positioning signal failed is obtained, the position coordinate data and attitude change data before the failure are extracted from the positioning attitude data before the failure, and then integrated and sorted to form dead reckoning reference data. The three-dimensional point cloud data continuously collected by the rotating imaging radar after the satellite positioning signal fails is acquired. Displacement features are extracted from the three-dimensional point cloud data to extract the spatial position change and travel direction change trajectory of the mobile robot within the continuous scanning cycle of the rotating imaging radar. The data is then integrated and organized to form relative displacement feature data. The inertial measurement unit continuously collects inertial measurement data after the satellite positioning signal fails. The attitude increment is extracted from the inertial measurement data. The pitch angle increment, roll angle increment and yaw angle increment of the mobile robot in the continuous scanning cycle are extracted. The numerical sequence of each angle increment changes over time is recorded and the various angle increments are integrated with their corresponding time sequences. Substitute the spatial position change in the relative displacement feature data into the coordinate transformation rules of a unified spatial coordinate system, convert the spatial position change into displacement increment data with the same coordinate reference as the position coordinate data before failure, and associate the converted displacement increment data with the trajectory of the change in driving direction to form displacement increment data under a unified coordinate system. Starting with the position coordinates before failure in the dead reckoning baseline data, the displacement increment data in the unified coordinate system in each scanning cycle is superimposed sequentially to calculate the instantaneous position coordinates of the mobile robot at the end of each scanning cycle. All the calculated instantaneous position coordinates are then arranged continuously in the order of the scanning cycles. The attitude of each instantaneous position coordinate is corrected by combining the extracted attitude change increment data. The attitude parameters of the corresponding instantaneous position coordinates are adjusted according to the attitude change increment data in each scanning cycle, so that each instantaneous position coordinate and its corresponding attitude parameters are accurately matched. The real-time position coordinates and real-time attitude data of the mobile robot at the current moment are extracted from the matched instantaneous position coordinates and corresponding attitude parameters. The extracted real-time position coordinates and real-time attitude data are integrated and organized to generate dead reckoning positioning data. Substitute dead reckoning positioning data into the positioning attitude data after satellite positioning signal failure, replace the failed satellite positioning-related position coordinate data and attitude data, retain other valid environmental contour feature data and motion state data in the positioning attitude data, and perform complete association and integration of the replaced positioning attitude data to form new positioning attitude data.

4. The mobile robot navigation method based on rotating imaging radar and satellite positioning according to claim 1, characterized in that, Based on the continuously acquired 3D point cloud data from the rotating imaging radar, static environmental feature data and dynamic interference feature data of the environment surrounding the mobile robot are extracted. After removing the dynamic interference feature data, the remaining static environmental feature data is integrated. The integrated static environmental feature data is then arranged according to its actual spatial distribution to generate a dynamic local environment map of the environment surrounding the mobile robot, including: The system acquires 3D point cloud data continuously collected by a rotating imaging radar, arranges the 3D point cloud data in chronological order of scanning time to form an ordered 3D point cloud data sequence, performs signal filtering on the 3D point cloud data sequence, extracts feature data from the filtered 3D point cloud data sequence, analyzes each point cloud data in the 3D point cloud data sequence point by point, extracts point cloud coordinate data and point cloud density data reflecting the characteristics of environmental objects, and integrates and organizes the extracted point cloud coordinate data and point cloud density data to form the original environmental feature data. Based on the registration and comparison results of continuous multi-frame point cloud data, static and dynamic classification is performed on the original environmental feature data. The position movement vector of the environmental object corresponding to each point cloud data and the change of point cloud cluster morphology parameters are calculated within the continuous scanning cycle. According to the preset static threshold and dynamic threshold, the original environmental feature data is divided into static environmental feature data and dynamic interference feature data. The feature points corresponding to the dynamic interference feature data are continuously tracked, and their spatial coordinate sequence within the continuous scanning period is recorded. The instantaneous motion velocity is calculated based on the time difference of the coordinate sequence, and the feature points that are determined to be dynamic are filtered out from the original environmental feature data. Based on a preset point cloud geometric features and spatial distribution classification model, the pure static environment feature data after removing dynamic interference feature data is processed. The classification model divides the pure static environment feature data into ground feature data, wall feature data and fixed facility feature data according to the point cloud's normal vector, curvature, height and its clustering features in the vertical and horizontal directions. Contour construction is performed on the classified ground feature data. All point cloud data in the ground feature data are arranged according to the spatial distribution of the actual ground. Continuous lines are used to connect the coordinate points corresponding to each point cloud data to form a ground contour that can reflect the undulation state and extension range of the ground. Contour construction is performed on the classified wall feature data and fixed facility feature data respectively. The point cloud data in the wall feature data is arranged and connected according to the spatial distribution of the actual wall to form the wall contour, and the point cloud data in the fixed facility feature data is arranged and connected according to the spatial distribution of the actual fixed facility to form the fixed facility contour. The ground contour, wall contour, and fixed facility contour are imported into a unified spatial coordinate system. According to the spatial positional relationship of the actual environmental objects corresponding to the three types of contours, the three are spatially superimposed and positionally calibrated to form a preliminary set of environmental contours. Based on the new three-dimensional point cloud data continuously collected by the rotating imaging radar, repeat the aforementioned feature extraction and contour construction steps to generate new ground contour data, new wall contour data, and new fixed facility contour data, and supplement the preliminary environmental contour set with the new contour data. Remove contour data that are far from the detection range of the mobile robot from the initial environmental contour set. Based on the preset contour smoothing and repair algorithm, process the environmental contour set after data supplementation and removal. The preset contour smoothing and repair algorithm is used to connect contour breakpoints smaller than a preset distance threshold and filter out contour protrusions with a size smaller than a preset noise threshold. Completely integrate all optimized contour data to generate a dynamic local environmental map.

5. The mobile robot navigation method based on rotating imaging radar and satellite positioning according to claim 1, characterized in that, The process involves combining new positioning attitude data and a dynamic local environment map with the target driving data of the mobile robot, performing path planning and driving control processing to generate initial navigation control commands, continuously collecting subsequent 3D point cloud data, satellite positioning data, and inertial measurement data through a rotating imaging radar, satellite positioning module, and inertial measurement unit, repeating the aforementioned processing steps to generate updated positioning attitude data and an updated dynamic local environment map, dynamically adjusting the initial navigation control commands based on the updated positioning attitude data and the updated dynamic local environment map, generating adjusted navigation control commands, and controlling the mobile robot to drive according to the adjusted navigation control commands to complete the navigation operation, including: Acquire new positioning attitude data, dynamic local environment map and target driving data of mobile robot, and perform spatiotemporal synchronization calibration of the three based on timestamp and unified spatial coordinate system to form calibrated navigation basic data. The target driving data includes target point coordinates, path key point coordinate sequence and target speed value. Extract the current position feature data and current attitude feature data of the mobile robot from the calibrated navigation base data. The current position feature data includes the real-time abscissa, real-time ordinate and real-time elevation in a unified spatial coordinate system. The current attitude feature data includes the real-time pitch angle, real-time roll angle and real-time heading angle. Driving area feature data and obstacle feature data are extracted from the dynamic local environment map in the calibrated navigation base data. The driving area feature data includes the coordinates of the polygon vertices that constitute the boundary of the driving area, and the obstacle feature data includes the coordinates of the bounding box vertices of all obstacles and the size of their circumscribed cubes. The current location feature data is compared with the target driving location coordinates to calculate the driving direction and distance of the mobile robot from the current location to the target driving location. Combined with the driving area feature data, multiple candidate driving paths are delineated within the calculated driving direction and distance range. By combining obstacle feature data, spatial position analysis is performed on multiple candidate driving paths. The spatial position relationship between each candidate driving path and the obstacle is analyzed. Candidate driving paths that overlap with the obstacle are removed, and candidate driving paths that maintain a safe driving distance from all obstacles are retained. Based on preset path smoothness constraints and dynamic constraints, and combined with the target driving trajectory requirements and target driving speed related data in the current posture feature data and target driving data, the retained candidate driving paths are adjusted. The curvature and heading angle change rate of each candidate driving path are calculated and adjusted to meet the minimum turning radius limit of the mobile robot and match the steering capability under the target driving speed. From the optimized candidate driving paths, the path with the shortest driving distance, the smallest attitude adjustment range, and that fully meets the requirements of the target driving trajectory is selected as the optimal driving path, and all contour coordinates and driving parameters of the optimal driving path are extracted. The initial navigation control command is generated based on the contour coordinates and driving parameters of the optimal driving path. The initial navigation control command includes the mobile robot's driving direction adjustment data, driving speed control data, and driving angle correction data. The system continuously collects subsequent 3D point cloud data, satellite positioning data, and inertial measurement data through a rotating imaging radar, satellite positioning module, and inertial measurement unit. All processing steps are repeated to generate updated positioning attitude data and updated dynamic local environment map. Based on the updated positioning and attitude data, the driving direction adjustment data and driving angle correction data in the initial navigation control command are corrected. Based on the updated dynamic local environment map, the driving speed control data and driving path planning data in the initial navigation control command are adjusted. All the adjusted control data are integrated to generate the adjusted navigation control command and transmitted to the driving control component of the mobile robot.

6. The mobile robot navigation method based on rotating imaging radar and satellite positioning according to claim 2, characterized in that, The process involves comparing the local spatial coordinate system parameters corresponding to the set of environmental contour feature points with the parameters of the unified spatial coordinate system, calculating the origin deviation and coordinate axis direction deviation between the local and unified spatial coordinate systems, constructing a coordinate transformation relationship based on the calculated deviation values, and substituting the coordinates of all feature points in the set of environmental contour feature points into this coordinate transformation relationship to complete the transformation. This includes: Obtain a set of environmental contour feature points and a unified spatial coordinate system. Extract the corresponding local spatial coordinate system parameters from the set of environmental contour feature points. Extract the origin coordinates, coordinate axis extension directions, and coordinate scale intervals of the local spatial coordinate system to form complete parameters of the local spatial coordinate system. Extract the complete parameters from the unified spatial coordinate system, including the origin coordinates, coordinate axis extension directions, and coordinate scale intervals of the unified spatial coordinate system. The complete parameters of the local spatial coordinate system are compared with the complete parameters of the unified spatial coordinate system item by item. First, the origin coordinates of the two are compared, and the three-dimensional coordinate deviation value of the origin coordinates of the local spatial coordinate system relative to the origin coordinates of the unified spatial coordinate system is calculated and recorded. Continue to compare the extension directions of the coordinate axes in the local spatial coordinate system and the unified spatial coordinate system, calculate and record the angular deviation values ​​of each coordinate axis in the local spatial coordinate system relative to the corresponding coordinate axis in the unified spatial coordinate system; Then compare the coordinate scale intervals of the local spatial coordinate system and the unified spatial coordinate system, calculate the proportionality coefficient between the coordinate scale interval of the local spatial coordinate system and the coordinate scale interval of the unified spatial coordinate system, and record it. Based on the recorded coordinate system origin deviation value, coordinate axis angle deviation value, and coordinate scale scaling coefficient, a coordinate transformation relationship is constructed. This coordinate transformation relationship includes origin translation rules, coordinate axis rotation rules, and scale scaling rules, and is integrated into a complete coordinate transformation formula. Extract the local spatial coordinates of all feature points from the set of environmental contour feature points, extract the three-dimensional coordinate data of each feature point separately and arrange them in order to form a sequence of local coordinates of feature points; Substitute the coordinates of each feature point in the local coordinate sequence into the constructed coordinate transformation formula, and calculate the coordinates of each feature point in turn according to the origin translation rule, coordinate axis rotation rule, and scale scaling rule; Based on the preset effective coordinate range of the unified spatial coordinate system, the three-dimensional coordinate data of each feature point is verified. For coordinate data that exceeds the preset effective coordinate range, iterative calculation is performed according to the coordinate transformation formula until it falls into the effective coordinate range. The corrected unified coordinate data of feature points are re-associated with the feature point grouping information in the environmental contour feature point set, so that the unified coordinate data of each feature point corresponds to the original static object group. All the associated data are integrated and organized to complete the coordinate transformation of the environmental contour feature point set.

7. The mobile robot navigation method based on rotating imaging radar and satellite positioning according to claim 3, characterized in that, The process involves acquiring 3D point cloud data continuously collected by the rotating imaging radar after satellite positioning signal failure, extracting displacement features from this 3D point cloud data, extracting the spatial position change and travel direction change trajectory of the mobile robot within the continuous scanning cycle of the rotating imaging radar, and integrating and organizing this data to form relative displacement feature data, including: The three-dimensional point cloud data continuously collected by the rotating imaging radar after the satellite positioning signal fails is acquired. The three-dimensional point cloud data is divided according to the scanning cycle of the rotating imaging radar. The three-dimensional point cloud data collected in each scanning cycle is formed as an independent data unit to form a sequence of three-dimensional point cloud data units arranged according to the scanning cycle. Extract three-dimensional point cloud data units from two adjacent scanning cycles from the three-dimensional point cloud data unit sequence, label them as the point cloud data unit of the previous cycle and the point cloud data unit of the next cycle respectively, and import them synchronously. Environmental feature matching is performed on the point cloud data units of the previous period and the point cloud data units of the next period. The same static environmental feature points in the two data units are extracted as reference matching points, and the coordinate data of each reference matching point in the two data units are recorded. Using the coordinate data of the reference matching point as a reference, the relative positions of the reference matching point in the two data units are compared, the displacement components of the mobile robot along each coordinate axis in the unified spatial coordinate system from the previous scanning cycle to the next scanning cycle are calculated, and all displacement components are integrated to obtain the single-cycle spatial displacement in adjacent scanning cycles. Repeat the aforementioned benchmark matching and displacement calculation steps for all adjacent period point cloud data units in the 3D point cloud data unit sequence, calculate the single-cycle spatial displacement of each adjacent scanning cycle, and arrange all single-cycle spatial displacements in the order of scanning cycles. The trajectory of the mobile robot's direction of travel is extracted from the arranged single-cycle spatial displacements. The direction of travel of the mobile robot in each scanning cycle is determined based on the three-dimensional coordinate changes of each single-cycle spatial displacement. The directions of travel of all scanning cycles are connected in chronological order. Based on the preset trajectory filtering algorithm, the connected driving direction change trajectory is processed, and discrete points in the trajectory whose direction angle changes exceed the preset sudden change threshold are removed. The scanning cycle and spatial displacement corresponding to each direction change point are recorded. The sequentially arranged single-cycle spatial displacements are associated with the smoothed driving direction change trajectory, so that the single-cycle spatial displacement of each scanning cycle corresponds to a specific direction point in the driving direction change trajectory. Based on the associated displacement and direction of travel data, the cumulative change in spatial position of the mobile robot from the time the satellite positioning signal failed to the current moment is calculated, and the cumulative change in spatial position is integrated with the single-cycle spatial displacement and the trajectory of the change in direction of travel. All the integrated displacement data are converted into a unified length unit, all the driving direction data are converted into a unified angle unit, and the format of the displacement and direction data after the unit is unified is standardized to form relative displacement feature data.

8. The mobile robot navigation method based on rotating imaging radar and satellite positioning according to claim 4, characterized in that, The process involves extracting feature data from the filtered 3D point cloud data sequence, analyzing each point cloud data point by point, extracting point cloud coordinate data and point cloud density data reflecting environmental object features, and integrating and organizing the extracted point cloud coordinate data and point cloud density data to form raw environmental feature data, including: The three-dimensional point cloud data sequence after signal filtering is obtained, and the three-dimensional point cloud data sequence is analyzed point by point. The three-dimensional spatial coordinate information of each point cloud data is read. The coordinate information includes the horizontal coordinate value, the vertical coordinate value and the elevation value. The coordinate information of each point cloud data is recorded separately. For each point cloud data in the 3D point cloud data sequence, the density of the surrounding point cloud is statistically analyzed. A fixed spatial region is defined with each point cloud data as the center, and the number of other point cloud data contained in this spatial region is counted as the point cloud density value of that point cloud data. The recorded point cloud coordinate information is associated one by one with the statistical point cloud density value, so that the three-dimensional spatial coordinate information of each point cloud data corresponds to a unique point cloud density value, forming a data pair of point cloud coordinates and density. Arrange all associated data pairs in the original order of the 3D point cloud data sequence, and remove associated data pairs whose point cloud density values ​​are lower than the preset effective density lower limit or higher than the preset effective density upper limit; Based on spatial coordinate clustering, associated data pairs with Euclidean distance less than a preset distance threshold are aggregated into the same cluster, and each cluster corresponds to a potential environmental object region and is labeled as a cluster. Cluster features are calculated for each cluster of labeled associated data pairs. The mean vector of three-dimensional coordinates, covariance matrix and average point cloud density of each cluster are calculated, and the calculated cluster feature parameters are associated with the cluster data. The point cloud data after associating all intra-cluster feature parameters are integrated, and all original associated data pairs and corresponding intra-group feature parameters in each cluster are retained; All integrated point cloud coordinate data are converted into coordinate values ​​in a unified spatial coordinate system, and all point cloud density data are converted into unified statistical units to ensure that the data are consistent in terms of coordinate reference and statistical units. The point cloud data, after unifying units and coordinates, is rearranged according to spatial location from near to far, so that the data can reflect the distribution of the environment around the mobile robot from near to far. The rearranged point cloud data is fully integrated, retaining the coordinate information, density information, and spatial grouping information of all point cloud data to form the original environmental feature data.

9. The mobile robot navigation method based on rotating imaging radar and satellite positioning according to claim 5, characterized in that, The process involves combining obstacle feature data to perform spatial position analysis on multiple candidate driving paths, analyzing the spatial relationship between each candidate driving path and obstacles, removing candidate driving paths that spatially overlap with obstacles, and retaining candidate driving paths that maintain a safe driving distance from all obstacles, including: Acquire multiple candidate driving path data and extracted obstacle feature data, extract single candidate driving path data from the multiple candidate driving path data, extract all contour coordinate points and path width data of the path and integrate them; Extract feature data of a single obstacle from obstacle feature data, extract all contour coordinate points, position center coordinates and external dimensions of the obstacle and integrate them; The spatial coverage of a path is determined based on the contour coordinates and path width data of a single candidate driving path, and the spatial coverage of an obstacle is determined based on the contour coordinates and dimensions data of the obstacle. The intersection area between the spatial coverage polygon of the path and the spatial coverage polygon of the obstacle is calculated. If the intersection area is greater than zero, it is determined that there is spatial overlap, and the candidate driving path is marked as an unqualified path. When there is no spatial overlap between the path and the obstacle, the minimum spatial distance between the path and the obstacle is calculated, and the calculated minimum spatial distance is compared with the preset static safety distance threshold. If the minimum spatial distance is less than the static safety distance threshold, the candidate driving path is marked as an unqualified path; if the minimum spatial distance is greater than or equal to the safe driving distance standard, the candidate driving path is marked as a qualified path. For a single candidate driving path data, repeat the aforementioned spatial position analysis and qualification judgment steps with all individual obstacles. If the judgment result with all obstacles is qualified, the path is retained; if any one of them is unqualified, the path is removed. Repeat all the aforementioned screening steps for all individual candidate driving path data in multiple candidate driving path data, and complete the spatial position analysis and qualification judgment of each candidate driving path and all obstacles, and record the final screening result of each path; Summarize all qualified paths from the screening results, extract complete data for all qualified paths, including path outline coordinates, path width data, and driving direction data, and integrate and organize them. The coordinate data of all qualified routes are uniformly converted into coordinate values ​​under a unified spatial coordinate system to ensure that the coordinate reference of all qualified routes is consistent. The qualified route data after coordinate calibration are sorted from shortest to longest driving distance.

10. A mobile robot navigation system based on rotating imaging radar and satellite positioning, characterized in that, include: processor; A machine-readable storage medium for storing machine-executable instructions of the processor; The processor is configured to execute the mobile robot navigation method based on rotating imaging radar and satellite positioning as described in any one of claims 1 to 9 by executing the machine-executable instructions.