Robot dual-mode positioning intelligent switching and whole-process precise navigation control method

Abstract

The invention discloses a robot dual-mode positioning intelligent switching and full-process precise navigation control method, a robot device for realizing the control method comprises an airborne controller, a laser radar, a speedometer, an IMU and a driving unit, the airborne controller adopts a heterogeneous architecture, and each module interacts through an AXI-Lite bus; the specific control method comprises a lightning-steady state dual-mode intelligent switching positioning framework, a customized behavior tree three-segment navigation framework and an FPGA hard real-time sudden stop link framework. The control method provided by the invention realizes the core requirements of no infrastructure dependence, plug and play, high safety and high precision, solves the problems of poor scene adaptability, high deployment cost and difficulty in consideration of safety and precision in the prior art, is suitable for multiple fields of industrial inspection, warehouse logistics, emergency rescue, urban underground space and the like, and is high in universality.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation and positioning technology for mobile robots, and in particular to a method for intelligent switching between dual-mode positioning and precise navigation control throughout the entire process of a robot. Background Technology

[0002] Autonomous navigation and positioning technology for mobile robots is primarily applied in complex scenarios such as indoor and outdoor industrial inspection, warehousing and logistics, emergency rescue, and urban underground spaces. These scenarios are generally characterized by the lack of prior infrastructure, satellite signals, and long-term deployment conditions, placing extremely high demands on the rapid deployment, long-term stability, safety response speed, and accuracy of robot navigation.

[0003] Existing related technologies mainly include laser SLAM solutions based on NDT and ICP, visual SLAM solutions based on ORB-SLAM and VINS, and fusion localization solutions based on LIO-SAM and LVI-SAM. However, all of them have significant limitations: laser SLAM has slow initial convergence and is prone to failure in degraded environments such as corridors; visual SLAM is sensitive to illumination and requires robot movement coordination during the initialization phase; fusion localization solutions have high computational requirements and high hardware configuration requirements, resulting in high application barriers.

[0004] The core pain point currently facing the industry is the lack of a universal navigation framework that enables "instant power-on, instant positioning, instant safety, and instant accuracy." Existing technologies suffer from problems such as slow initial positioning (requiring manual marking or long-term scanning), poor environmental adaptability (causing laser / visual sensors to fail in scenarios such as corridors, glass, and metal walls), insufficient long-term operational accuracy (error > 0.3m / h without closed-loop correction), lagging safety response (software stack emergency stop > 100ms, excessive braking distance at high speeds), and high deployment costs (requiring advance deployment of infrastructure such as UWB, QR codes, and trackside labels). These issues fail to meet the demands for rapid deployment, no modification or adaptation, and a balance between high safety and high accuracy in complex scenarios. Summary of the Invention

[0005] In view of the above-mentioned deficiencies of the prior art, the technical problem to be solved by the present invention is how to improve the slow initial positioning, delayed safety response and lack of a general framework of "power-on, positioning, and safety".

[0006] To achieve the above objectives, the present invention provides a robot dual-mode positioning intelligent switching and full-process precise navigation control method. The robot device implementing the control method includes an onboard controller, a lidar, an odometer, an IMU, and a drive unit. The onboard controller adopts a heterogeneous architecture, and the modules interact through an AXI-Lite bus. Specific control methods include: a "lightning-steady-state" dual-mode intelligent switching positioning framework, a customized behavior tree three-segment navigation framework, and an FPGA hard real-time emergency stop link framework.

[0007] Furthermore, the lightning mode module consists of a front-end downsampling module, a pre-stored sub-map Msub, and a Polynet regressor. The functions and connections of each module are as follows: LiDAR outputs raw point cloud It connects directly to the front-end downsampling module to transmit raw point cloud data; the front-end downsampling module outputs sparse point cloud data. The data is transmitted to the pre-stored sub-map Msub and the NDT matching module respectively; the NDT matching module outputs the pose candidate set { }, where i≤64, are input to the Polynet regressor, and the regressor outputs the final initial pose. and confidence level .

[0008] Furthermore, the operation method of the lightning mode includes: S11: LiDAR acquires raw point cloud (Points|Δ|≤1 The front-end downsampling module employs a "voxel grid + random discarding" strategy. The voxel grid size is r=0.3m, and ≤20 points are randomly retained for voxels exceeding the upper limit, ultimately resulting in a sparse point cloud. (Points|Δ'|≤5×10³); S12: Generation method of pre-stored sub-map Msub: In the offline SLAM mapping stage, the global map is tiled into 10m×10m tiles, and the number of points in each sub-map is ≤1×1. PCA was used to retain principal components with a variance percentage ≥ 95%, resulting in a 64-dimensional feature vector with a storage volume ≤ 256kB / submap; Perform fast NDT matching with Msub to generate a set of 64 initial pose candidates. }; S13: The Polynet regressor is a fully connected network with layer widths of [64, 32, 16, 6], activation function PReLU, and weights quantized using 8-bit quantization. It requires ≤16kB of RAM during training. The loss function during training is L=||Egt-Epred|. +λ Cconfidence (λ=0.1) achieves data augmentation by adding uniform noise of ±0.5m and ±10° to the pose of the training set, with INT8 weight error ≤2% and FP32 accuracy; Input regressor, output 6-DoF pose and confidence level ; S14: Set the confidence threshold Cth1 = 0.65, if If the value is ≥0.65 and the total time t ≤ 2s, lock. Set the current pose; otherwise, expand the NDT search window by Δψ = ±45° and repeat S12-S14.

[0009] The lightning mode of this invention acquires the original point cloud using LiDAR, obtains a sparse point cloud through lightweight downsampling, performs fast NDT matching with a pre-stored sub-map to generate a pose candidate set, and inputs it into a lightweight Polynet regressor to output an accurate initial pose; thus meeting the requirements for rapid localization; steady-state mode: based on EKF, it fuses LiDAR, odometry, and IMU data, enables closed-loop detection and graph optimization, and corrects the global trajectory; thus solving the long-term drift problem; dual-mode switching logic: automatically triggers switching based on system runtime and pose confidence, taking into account both rapid initialization and long-term steady state.

[0010] The lightning mode delivers the following benefits: initial positioning time ≤ 2s, requiring no manual intervention and achieving "positioning upon power-on"; long-term positioning drift ≤ 0.05m / h, far superior to the existing technology's error level of 0.3m / h; economic benefits: no need for additional auxiliary positioning facilities, reducing scene modification and deployment costs.

[0011] Furthermore, the steady-state positioning mode comprises an EKF fusion module, a closed-loop detection submodule, and a graph optimization module; The connection relationships between the modules are as follows: the output data of the lidar, odometer, and IMU are connected to the EKF fusion module, the closed-loop detection submodule monitors the loop closure status of the sub-map in real time, and the trigger map optimization module corrects the global trajectory.

[0012] Furthermore, the operation mode of the steady-state positioning mode includes: S20: When the system runtime t ≥ 60s and there are 100 consecutive frames When Cth2 = 0.90, a mode switching command is triggered, and the system enters steady-state positioning mode. S31: In the EKF fusion module, the laser orientation correction weight Wlaser=40%, the odometry weight Wodom=50%, and the IMU weight Wimu=10%, realizing complementary fusion of multi-source data; S32: The global Monte Carlo sampling η is reduced from 1.0 to 0.001, reducing the computational load; the loop closure detection submodule detects loop closure events in the sub-map in real time. Once a loop closure is detected, the map optimization module is activated to correct the global trajectory and eliminate accumulated errors. S33: Output fused pose ξ fuse every 1s to ensure positioning drift εd≤0.05m / h.

[0013] Furthermore, the root node of the behavior tree structure is a sequential node “StartupAlign→CruiseDynamic→FinalRotate”, and each node has an independent function and is executed sequentially.

[0014] Furthermore, the operation mode of the customized behavior tree three-segment navigation includes: S41: StartupAlign node: Calls the path tangent orientation alignment algorithm, calculates the difference Δθ = |θcur - θtan| between the robot's current orientation θcur and the planned path tangent θtan. If Δθ > 3°, publishes the angular velocity ω = Kp. Δθ, until Δθ≤1°; S42: CruiseDynamic node: Enable local path cutter, set forward distance Lf=2.0m and backward distance Lb=0.4m, and cut local path Plocal in real time; combined with collision monitor, establish a safety protection circle 0.425m in front and 0.2m to the side, and immediately trigger an emergency stop command if the minimum distance dmin≤0.2m; S43: FinalRotate node: Executes a three-segment rotation curve (acceleration segment - constant velocity segment - deceleration segment), with a maximum angular velocity ωmax = 30° / s. The deceleration segment uses the exponential reaching law ω(t) = ωmax. exp (-3.8t), target orientation tolerance δθ=0.01°, settling time tsettle≤1.2s.

[0015] In this invention, the StartupAlign node corrects the deviation between the robot's current orientation and the planned path using a path tangent orientation alignment algorithm; the CruiseDynamic node captures local paths in real time, establishing forward and lateral safety protection zones to dynamically avoid obstacles; and the FinalRotate node employs a three-segment rotation curve of "acceleration-constant speed-deceleration" to accurately match the target orientation. In degraded scenarios, the positioning success rate of this mode is ≥99%, the orientation alignment error is ≤1°, the target orientation tolerance is ≤0.01°, and there is no mechanical oscillation. Therefore, it solves the problem of poor environmental adaptability and the failure of laser / vision sensors caused by degraded scenarios such as corridors, glass, and metal walls.

[0016] Furthermore, the FPGA hard real-time emergency stop link consists of a collision monitor, a PWM blocking module, and a MOSFET switch. The connection relationship is as follows: the point cloud data output by the lidar is connected to the collision monitor, the collision monitor outputs a trigger signal T1 to the PWM blocking module, and the PWM blocking module controls the MOSFET switch to turn off, thereby realizing the motor power-off braking.

[0017] Furthermore, the timing parameters of the FPGA hard real-time emergency stop link include: The laser point cloud is transmitted as a 32-byte UDP packet at a frequency of 1kHz via Gigabit Ethernet. The FPGA uses a 125MHz clock, and it takes 256clk = 2.048μs to parse the 32-byte data. The collision monitor runs the "surrounding sphere" algorithm, which compares the coordinates of 5×10³ points in parallel in a single frame, taking ≤500ns. If a collision risk is detected, it outputs a trigger signal T1. The trigger signal T1 is transmitted to the PWM blocking module, which drives the MOSFET to turn off. The total braking time tbrake = t1 + t2 ≤ 80 ns.

[0018] The Polynet regressor in this invention employs a [64,32,16,6] layer width design, PReLU activation function, and 8-bit quantization to reduce computational load and storage consumption. EKF fusion rationally allocates weights for laser (40%), odometry (50%), and IMU (10%) to balance positioning accuracy and real-time performance. The regressor parameter count is ≤32kB, and inference time is ≤5ms@600MHz ARMCortex-M7, resulting in low hardware requirements. The fused pose output frequency is 1Hz, with position error ≤±2mm and orientation error ≤±0.01°. This solves the problems of high computational load, high hardware requirements, and insufficient initial pose calculation accuracy in existing fusion positioning schemes.

[0019] Furthermore, the task completion determination in the control method includes: When the robot's pose simultaneously satisfies the position error εp≤±2mm and the orientation error εθ≤±0.01°, the task is considered complete and reported to the host computer.

[0020] Technical effect

[0021] The control method proposed in this invention achieves the core requirements of "no infrastructure dependency, plug and play, high security, and high precision", and solves the pain points of poor adaptability to existing technology scenarios, high deployment costs, and difficulty in balancing security and precision. It is applicable to multiple fields such as industrial inspection, warehousing and logistics, emergency rescue, and urban underground space, and has strong versatility.

[0022] Performance indicators: It has clear and excellent quantitative indicators, with initial positioning ≤2s, emergency stop delay ≤80ns, long-term drift ≤0.05m / h, position error ≤±2mm, and orientation error ≤±0.01°, which fully meets the operational requirements in complex scenarios; Production Implementation: The system adopts a "CPU+FPGA" heterogeneous architecture. The CPU core runs the positioning state machine and behavior tree, while the FPGA implements the hard real-time safety link. The modules are clearly divided, making it easy to integrate into existing robot onboard controllers. The lightweight algorithm lowers the hardware threshold, requiring no special customized hardware and no prior modification of the scene infrastructure. The deployment cost is low, and it can be quickly scaled up for application.

[0023] The following will further explain the concept, specific structure, and technical effects of the present invention in conjunction with the accompanying drawings, so as to fully understand the purpose, features, and effects of the present invention. Attached Figure Description

[0024] Figure 1 This is a schematic diagram of a dual-mode positioning switching state machine according to a preferred embodiment of the present invention; Figure 2 This is a flowchart of a preferred embodiment of the lightning positioning mode of the present invention; Figure 3 This is a diagram of a preferred embodiment of the steady-state positioning fusion framework of the present invention; Figure 4 This is a schematic diagram of a behavior tree structure according to a preferred embodiment of the present invention; Figure 5 This is a schematic diagram of a preferred embodiment of the present invention, showing local path interception and a security protection circle. Figure 6 This is a three-segment rotation curve diagram of a preferred embodiment of the present invention; Figure 7 This is a preferred embodiment of the controller heterogeneous architecture and emergency stop link diagram of the present invention. Detailed Implementation

[0025] The following description, with reference to the accompanying drawings, illustrates several preferred embodiments of the present invention to make its technical content clearer and easier to understand. The present invention can be embodied in many different forms, and the scope of protection of the present invention is not limited to the embodiments mentioned herein.

[0026] In the accompanying drawings, components with the same structure are indicated by the same numerical designation, and components with similar structures or functions are indicated by similar numerical designations. The dimensions and thicknesses of each component shown in the drawings are arbitrary, and the present invention does not limit the dimensions and thicknesses of each component. To make the illustrations clearer, the thickness of some components has been appropriately exaggerated in the drawings.

[0027] like Figure 1-7As shown, the robot navigation control method of the present invention consists of an airborne controller (700), a lidar (710), an odometer (730), an IMU (740), and a drive unit (760). The airborne controller adopts a heterogeneous architecture (CPUcore0 + CPU core1 + FPGA fabric), and the modules interact through the AXI-Lite bus (interaction period ≤ 1ms). Specifically, the following steps are executed: (I) Implementation details of lightning positioning mode (1) Module composition The module includes a front-end downsampling module (711), a pre-stored sub-map Msub (712), and a Polynet regressor (713). The functions and connections of each module are as follows: The lidar (710) outputs the original point cloud. It is directly connected to the front-end downsampling module (711) to transmit the original point cloud data; the front-end downsampling module (711) outputs sparse point cloud. The data is transmitted to the pre-stored sub-map Msub(712) and the NDT matching module, respectively; the NDT matching module outputs the pose candidate set { } (i≤64), input to Polynet regressor (713), the regressor outputs the final initial pose. and confidence level .

[0028] (2) Key parameters and operating procedures

[0029] S11: LiDAR (710) acquires raw point cloud (Points|Δ|≤1 The front-end downsampling module (711) adopts a "voxel grid + random discard" strategy. The voxel grid size is r=0.3m. For voxels exceeding the upper limit, ≤20 points are randomly retained to finally obtain a sparse point cloud. (Points|Δ'|≤5×10³); S12: Generation method of pre-stored sub-map Msub(712): In the offline SLAM mapping stage, the global map is sliced ​​into 10m×10m tiles, and the number of points in each sub-map is ≤1×1 PCA was used to retain principal components with a variance percentage ≥ 95%, resulting in a 64-dimensional feature vector with a storage volume ≤ 256kB / submap; Perform fast NDT matching with Msub to generate a set of 64 initial pose candidates. }; S13: The Polynet regressor (713) is a fully connected network with layer widths of [64, 32, 16, 6], activation function PReLU, and weights quantized using 8-bit. It requires ≤16kB of RAM during training. The loss function during training is L=||Egt-Epred|. +λ Cconfidence (λ=0.1) achieves data augmentation by adding ±0.5m and ±10° uniform noise to the training set poses, with INT8 weight error ≤2% and FP32 accuracy; Input regressor, output 6-DoF pose and confidence level ; S14: Set the confidence threshold Cth1 = 0.65, if If the value is ≥0.65 and the total time t ≤ 2s, lock. Set the current pose; otherwise, expand the NDT search window by Δψ = ±45° and repeat S12-S14.

[0030] (II) Implementation details of steady-state positioning mode

[0031] (1) Module composition

[0032] It includes an EKF fusion module (720), a closed-loop detection submodule (721), and a graph optimization module (722). The connection relationship is as follows: the output data of the lidar, odometer, and IMU are connected to the EKF fusion module (720). The closed-loop detection submodule (721) monitors the loop closure status of the sub-map in real time and triggers the graph optimization module (722) to correct the global trajectory.

[0033] (2) Key parameters and operating procedures

[0034] S20: When the system runtime t ≥ 60s and there are 100 consecutive frames When Cth2 = 0.90, a mode switching command is triggered, and the system enters steady-state positioning mode. S31: In the EKF fusion module (720), the laser orientation correction weight Wlaser=40%, the odometry weight Wodom=50%, and the IMU weight Wimu=10%, to achieve complementary fusion of multi-source data; S32: The global Monte Carlo sampling rate η is reduced from 1.0 to 0.001, reducing the computational load; the loop closure detection submodule (721) detects loop closure events in the submap in real time. Once a loop closure is detected, the graph optimization module (722) is activated to correct the global trajectory and eliminate accumulated errors. S33: Output fused pose ξfuse every 1s to ensure positioning drift εd≤0.05m / h.

[0035] (III) Implementation details of the three-stage navigation of behavior tree

[0036] (1) Behavior tree structure

[0037] The root node is a sequential node “StartupAlign→CruiseDynamic→FinalRotate”, and each node has independent functions and is executed sequentially. S41: StartupAlign node (orientation alignment): Calls the path tangent orientation alignment algorithm, calculates the difference Δθ = |θcur - θtan| between the robot's current orientation θcur and the planned path tangent θtan. If Δθ > 3°, publishes the angular velocity ω = Kp. Δθ, until Δθ≤1°; S42: CruiseDynamic node (Dynamic Cruise): Enables local path cutter, sets forward distance Lf=2.0m and backward distance Lb=0.4m, and cuts local path Plocal in real time; combined with collision monitor (522), establishes a safety protection circle 0.425m in front and 0.2m to the side, and immediately triggers emergency stop command if the minimum distance dmin≤0.2m; S43: FinalRotate node (Precise Rotation): Executes a three-segment rotation curve (acceleration segment - constant velocity segment - deceleration segment), with a maximum angular velocity ωmax = 30° / s. The deceleration segment uses the exponential reaching law ω(t) = ωmax. exp (-3.8t), target orientation tolerance δθ=0.01°, settling time tsettle≤1.2s.

[0038] (iv) Implementation details of FPGA hard real-time emergency stop link

[0039] (1) Link composition

[0040] The system includes a collision monitor (522), a PWM blocking module (751), and a MOSFET switch. The connection is as follows: the point cloud data output by the lidar is connected to the collision monitor (522), the collision monitor outputs a trigger signal T1 to the PWM blocking module (751), and the PWM blocking module controls the MOSFET switch to turn off, thereby realizing the motor power-off braking.

[0041] (2) Timing parameters

[0042] The laser point cloud is transmitted as a 32-byte UDP packet at a frequency of 1kHz via Gigabit Ethernet. The FPGA uses a 125MHz clock, and it takes 256clk = 2.048μs to parse the 32-byte data. The collision monitor (522) runs the "surrounding sphere" algorithm, which compares the coordinates of 5×10³ points in parallel in a single frame (32 DSPs process in parallel), taking ≤500ns. If a collision risk is detected (dmin≤0.2m), a trigger signal T1 is output (taking t1=35ns). The trigger signal T1 is transmitted to the PWM blocking module (751), which drives the MOSFET to turn off (time t2 = 45ns). The total braking time tbrake = t1 + t2 ≤ 80ns.

[0043] (v) Task completion determination

[0044] When the robot's pose simultaneously satisfies the conditions of position error εp≤±2mm and orientation error εθ≤±0.01°, the task is deemed complete and reported to the host computer.

[0045] The preferred embodiments of the present invention have been described in detail above. It should be understood that those skilled in the art can make numerous modifications and variations based on the concept of the present invention without creative effort. Therefore, all technical solutions that can be obtained by those skilled in the art based on the concept of the present invention through logical analysis, reasoning, or limited experimentation on the basis of existing technology should be within the scope of protection defined by the claims.

Claims

1. A method for intelligent switching between dual-mode positioning and precise navigation control throughout the entire process of a robot, characterized in that, The robot device for implementing the control method includes an onboard controller, a lidar, an odometer, an IMU, and a drive unit, wherein the onboard controller adopts a heterogeneous architecture and the modules interact through an AXI-Lite bus; Specific control methods include: a "lightning-steady-state" dual-mode intelligent switching positioning framework, a customized behavior tree three-segment navigation framework, and an FPGA hard real-time emergency stop link framework.

2. The robot dual-mode positioning intelligent switching and full-process precise navigation control method as described in claim 1, characterized in that, The Lightning mode module consists of a front-end downsampling module, a pre-stored sub-map Msub, and a Polynet regressor. The functions and connections of each module are as follows: LiDAR outputs raw point cloud It connects directly to the front-end downsampling module to transmit raw point cloud data; the front-end downsampling module outputs sparse point cloud data. The data is transmitted to the pre-stored sub-map Msub and the NDT matching module, respectively. The NDT matching module outputs a candidate set of poses. }, where i≤64, are input to the Polynet regressor, and the regressor outputs the final initial pose. and confidence level .

3. The robot dual-mode positioning intelligent switching and full-process precise navigation control method as described in claim 1, characterized in that, The operation method of the lightning mode includes: S11: LiDAR acquires raw point cloud (Points|Δ|≤1 The front-end downsampling module employs a "voxel grid + random discarding" strategy. The voxel grid size is r=0.3m, and ≤20 points are randomly retained for voxels exceeding the upper limit, ultimately resulting in a sparse point cloud. (Points|Δ'|≤5×10³); S12: Generation method of pre-stored sub-map Msub: In the offline SLAM mapping stage, the global map is tiled into 10m×10m tiles, and the number of points in each sub-map is ≤1×1. PCA was used to retain principal components with a variance percentage ≥ 95%, resulting in a 64-dimensional feature vector with a storage volume ≤ 256kB / submap; Perform fast NDT matching with Msub to generate a set of 64 initial pose candidates. }; S13: The Polynet regressor is a fully connected network with layer widths of [64, 32, 16, 6], activation function PReLU, and weights quantized using 8-bit quantization. It requires ≤16kB of RAM during training. The loss function during training is L=||Egt-Epred|. +λ Cconfidence (λ=0.1) achieves data augmentation by adding uniform noise of ±0.5m and ±10° to the pose of the training set, with INT8 weight error ≤2% and FP32 accuracy; Input regressor, output 6-DoF pose and confidence level ; S14: Set the confidence threshold Cth1 = 0.65, if If the value is ≥0.65 and the total time t ≤ 2s, lock. Set the current pose; otherwise, expand the NDT search window by Δψ = ±45° and repeat S12-S14.

4. The robot dual-mode positioning intelligent switching and full-process precise navigation control method as described in claim 1, characterized in that, The steady-state positioning mode is composed of an EKF fusion module, a closed-loop detection submodule, and a graph optimization module. The connection relationships between the modules are as follows: the output data of the lidar, odometer, and IMU are connected to the EKF fusion module, the closed-loop detection submodule monitors the loop closure status of the sub-map in real time, and the trigger map optimization module corrects the global trajectory.

5. The robot dual-mode positioning intelligent switching and full-process precise navigation control method as described in claim 1, characterized in that, The operation mode of the steady-state positioning mode includes: S20: When the system runtime t ≥ 60s and there are 100 consecutive frames When Cth2 = 0.90, a mode switching command is triggered, and the system enters steady-state positioning mode. S31: In the EKF fusion module, the laser orientation correction weight is Wlaser=40%, the odometry weight is Wodom=50%, and the IMU weight is Wimu=10%, achieving complementary fusion of multi-source data; S32: The global Monte Carlo sampling η is reduced from 1.0 to 0.001, reducing the computational load; the loop closure detection submodule detects loop closure events in the sub-map in real time. Once a loop closure is detected, the map optimization module is activated to correct the global trajectory and eliminate accumulated errors. S33: Output fused pose ξ fuse every 1s to ensure positioning drift εd≤0.05m / h.

6. The robot dual-mode positioning intelligent switching and full-process precise navigation control method as described in claim 1, characterized in that, The root node of the behavior tree structure is the sequential node "StartupAlign→CruiseDynamic→FinalRotate", and each node has an independent function and is executed sequentially.

7. The robot dual-mode positioning intelligent switching and full-process precise navigation control method as described in claim 1, characterized in that, The operation mode of the customized behavior tree three-segment navigation includes: S41: StartupAlign node: Calls the path tangent orientation alignment algorithm, calculates the difference Δθ = |θcur - θtan| between the robot's current orientation θcur and the planned path tangent θtan. If Δθ > 3°, publishes the angular velocity ω = Kp. Δθ, until Δθ≤1°; S42: CruiseDynamic node: Enable local path cutter, set forward distance Lf=2.0m and backward distance Lb=0.4m, and cut local path Plocal in real time; combined with collision monitor, establish a safety protection circle 0.425m in front and 0.2m to the side, and immediately trigger an emergency stop command if the minimum distance dmin≤0.2m; S43: FinalRotate node: Executes a three-segment rotation curve (acceleration segment - constant velocity segment - deceleration segment), with a maximum angular velocity ωmax = 30° / s. The deceleration segment uses the exponential reaching law ω(t) = ωmax. exp (-3.8t), target orientation tolerance δθ=0.01°, settling time tsettle≤1.2s.

8. The robot dual-mode positioning intelligent switching and full-process precise navigation control method as described in claim 1, characterized in that, The FPGA hard real-time emergency stop link consists of a collision monitor, a PWM blocking module, and a MOSFET switch. The connection relationship is as follows: the point cloud data output by the lidar is connected to the collision monitor, the collision monitor outputs a trigger signal T1 to the PWM blocking module, and the PWM blocking module controls the MOSFET switch to turn off, thereby realizing the motor power-off braking.

9. The robot dual-mode positioning intelligent switching and full-process precise navigation control method as described in claim 1, characterized in that, The timing parameters of the FPGA hard real-time emergency stop link include: The laser point cloud is transmitted as 32-byte UDP packets at a frequency of 1kHz via Gigabit Ethernet. The FPGA uses a 125MHz clock, and it takes 256clk = 2.048μs to parse the 32-byte data. The collision monitor runs the "surrounding sphere" algorithm, which compares the coordinates of 5×10³ points in parallel in a single frame, taking ≤500ns. If a collision risk is detected, it outputs a trigger signal T1. The trigger signal T1 is transmitted to the PWM blocking module, which drives the MOSFET to turn off. The total braking time tbrake = t1 + t2 ≤ 80 ns.

10. The robot dual-mode positioning intelligent switching and full-process precise navigation control method as described in claim 1, characterized in that, The task completion determination in the control method includes: When the robot's pose simultaneously satisfies the position error εp≤±2mm and the orientation error εθ≤±0.01°, the task is considered complete and reported to the host computer.

Images