Path planning and dynamic obstacle avoidance system and method applied to spmt moving and transporting

Abstract

The application discloses a path planning and dynamic obstacle avoidance system and method applied to SPMT (Shuttle People Mover Train) moving and transporting, and belongs to the technical field of moving and transporting control. The method comprises the following steps: a three-dimensional static environment digital base map is constructed through multi-source data fusion and a voxel grid method, and a preliminary safe path is calculated offline in combination with SPMT train set parameters; in the process of moving, millimeter wave radar and visual sensor data are fused, dynamic obstacles are identified by comparing with the base map, and a real-time environment map is generated; when obstacles intrude into a three-dimensional pipeline-shaped path safety area, a time elastic band algorithm is used to online re-plan a locally optimal obstacle avoidance path; through a model predictive control algorithm, a kinematics conversion model and PID control are combined to real-timely adjust train set axis turning and speed to track a trajectory until the train set reaches the end point through high-precision positioning determination; and the application realizes static risk avoidance in advance and dynamic obstacle real-time response, and guarantees the safety and efficiency of the whole SPMT moving and transporting process.

Description

Technical Field

[0001] This invention belongs to the field of transportation control technology, specifically a path planning and dynamic obstacle avoidance system and method applied to SPMT transportation. Background Technology

[0002] Self-Propelled Modular Transporters (SPMTs), as core equipment for transporting heavy equipment and large components, are widely used in engineering construction, intelligent manufacturing, and other fields. Their transport scenarios are often complex environments, presenting not only static risk sources such as underground pipelines, differences in foundation bearing capacity, and above-ground structures, but also dynamic interference factors such as temporary obstacles and relocated existing facilities. This places extremely high demands on the safety and dynamic adaptability of path planning. Existing SPMT transport path planning methods mostly construct static paths based on single-dimensional environmental data, lacking fusion modeling of multi-source static information from underground and above-ground sources. This can easily lead to safety hazards due to the omission of hidden risk sources. Dynamic obstacle recognition often relies on single sensors, resulting in insufficient data accuracy and reliability, making it difficult to accurately capture obstacles that are not pre-modeled or whose positions have changed. Path replanning algorithms lack flexibility, either failing to respond quickly to dynamic obstacles or exhibiting poor smoothness in replanned trajectories, affecting the stable operation of the vehicle. Furthermore, the trajectory tracking control of multi-axle SPMTs lacks deep coordination with path planning, making it difficult to achieve precise control of steering and speed, thus limiting transport accuracy and efficiency. Therefore, there is an urgent need for an integrated approach that takes into account static risk prediction, dynamic obstacle response, and precise trajectory planning and control, in order to solve the pain points of existing technologies and ensure the safety and efficiency of SPMT transportation throughout the entire process. Summary of the Invention

[0003] To address the shortcomings of existing technologies, this invention proposes a path planning and dynamic obstacle avoidance system and method for SPMT (Special Purpose Vehicle) transportation. It constructs a 3D static environment digital base map through multi-source data fusion and voxel grid method, and calculates a preliminary safe path offline using SPMT train parameters. During operation, it integrates millimeter-wave radar and visual sensor data, compares it with the base map to identify dynamic obstacles, and generates a real-time environment map. When an obstacle intrudes into the safe area of ​​the 3D pipeline-like path, it replans the locally optimal obstacle avoidance path online based on a time-elastic band algorithm. Through model predictive control algorithms, combined with kinematic transformation models and PID control, it adjusts the steering and speed of each axle of the train in real time to track the trajectory until the train reaches its destination through high-precision positioning. This invention achieves early avoidance of static risks and real-time response to dynamic obstacles, ensuring safe and efficient SPMT transportation throughout the entire process.

[0004] To achieve the above objectives, the present invention provides the following technical solution:

[0005] Path planning and dynamic obstacle avoidance methods applied to SPMT transportation include:

[0006] S1: Construct a three-dimensional static environment digital base map; the three-dimensional static environment digital base map defines all known static risk areas;

[0007] S2: Based on the three-dimensional static environment digital base map, combined with the axle load, geometric shape and passability parameters of the SPMT train, calculate an initial safe path to avoid known static risk areas offline;

[0008] S3: During the SPMT trainset’s journey along the initial safety path, real-time point cloud and image data are acquired through deployed millimeter-wave radar and vision sensors. The real-time point cloud and image data are then fused and compared with a three-dimensional static environment digital base map to identify obstacles that have not been pre-modeled or whose positions have changed, and to generate a real-time environment map containing dynamic information.

[0009] S4: When an obstacle is detected to have entered the safe zone of the path, the local optimal obstacle avoidance path is replanned online based on the real-time environment map, with the current position of the vehicle crew as the starting point and the original target point as the ending point.

[0010] S5: The optimal obstacle avoidance path is sent to the SPMT train control system. The SPMT train control system uses a model predictive control algorithm to track the trajectory with virtual reference points. By adjusting the steering and speed of each axle in real time, the train is driven to travel along the optimal obstacle avoidance path.

[0011] S6: Throughout the entire transfer process, S3 to S5 are executed in a loop until the train arrives at its destination.

[0012] Specifically, the process of constructing the three-dimensional static environment digital base map includes:

[0013] The precise burial depth and direction of underground pipelines are obtained through the digital processing of engineering drawings, generating pipeline direction grid data. The mechanical properties of the foundation soil layer are obtained through ground-penetrating radar detection, generating foundation bearing capacity layered isosurface data. The surface geometric information of above-ground structures is obtained through three-dimensional laser scanning, generating surface point cloud and triangular mesh models.

[0014] The pipeline routing grid data, the stratified isosurface data of foundation bearing capacity, and the surface point cloud and triangular mesh model are uniformly registered to the same geographic coordinate system to obtain the registered multi-source data.

[0015] The voxel raster method is used to fuse the registered multi-source data, and semantic category attributes and corresponding risk level labels are assigned based on the fused data source to generate a three-dimensional static environmental digital base map containing spatial, semantic and risk information.

[0016] Specifically, real-time point cloud and image data are fused and compared with a 3D static environment digital base map to identify obstacles that have not been pre-modeled or whose positions have changed, generating a real-time environment map containing dynamic information, including:

[0017] The real-time point cloud data acquired by millimeter-wave radar is preprocessed to obtain candidate obstacle point cloud clusters; the preprocessing includes point cloud denoising, ground point filtering, and clustering segmentation.

[0018] The real-time image data acquired by the visual sensor is preprocessed, and the obstacle region in the real-time image is identified by a deep learning object detection network. The obstacle region is then transformed from the image coordinate system to the world coordinate system to obtain the transformed visual obstacle region.

[0019] The candidate obstacle point cloud clusters and the transformed visual obstacle regions are correlated and fused in the world coordinate system, and then compared with the three-dimensional static environment digital base map on a voxel-by-voxel basis to identify unmodeled obstacles or areas where the position of modeled obstacles has changed in the three-dimensional static environment digital base map, thereby generating a real-time environment map containing dynamic obstacle information.

[0020] Specifically, the path safety zone is defined based on the geometry and passability parameters of the SPMT trainset, including:

[0021] Using the centerline of the initial safe path or the locally optimal obstacle avoidance path as a reference, a safe distance is extended horizontally to both sides. The safe distance is composed of the actual width of the vehicle group plus a preset margin, forming a two-dimensional safe corridor. The two-dimensional safe corridor is extended vertically upward to the maximum height of the goods transported by the vehicle group and downward to the underground depth considering the influence of foundation deformation, thereby forming a three-dimensional path pipeline-shaped safe area. When any obstacle intrudes into this three-dimensional pipeline-shaped safe area, the online path replanning of S4 is triggered.

[0022] Specifically, the online replanning of the locally optimal obstacle avoidance path is implemented based on the time elastic band algorithm, including:

[0023] Extract a local segment of the preliminary safe path ahead of the current position from the real-time environment map as the initial trajectory band;

[0024] The relative position of obstacles to the initial trajectory zone in the real-time environment map is detected. For obstacles in the safe area of ​​the intrusion path, the intrusion depth is calculated based on the surface points, and a repulsive potential field proportional to the intrusion depth is constructed along the surface normal direction.

[0025] A nonlinear optimization problem is constructed with trajectory smoothness, following degree of the initial trajectory band, and sum of obstacle repulsion potential as the core objective functions. A numerical optimization algorithm is used to iteratively solve the nonlinear optimization problem. By adjusting the position of the path points on the initial trajectory band, the objective function is minimized, and the local optimal obstacle avoidance path that can avoid dynamic obstacles is output.

[0026] Specifically, the model predictive control algorithm is as follows:

[0027] The local optimal obstacle avoidance path is preprocessed into a parameterized curve with arc length as the parameter;

[0028] At the beginning of each control cycle, the current actual state of the geometric center of the SPMT train is used as the initial state. A dynamic distance related to the current speed of the train is pre-aimed on the parameterized curve. The corresponding pre-aimed point is determined as the virtual reference point for this control cycle, and its expected pose and speed are obtained.

[0029] A nonlinear dynamic model containing the geometric center position coordinates, heading angle, and steering angle of each axle is established as a prediction model to predict the state evolution of the train in the finite time domain.

[0030] Based on the current initial state and prediction model, a rolling time-domain optimization problem is constructed, which includes trajectory tracking error term, control command increment term and physical constraints of each actuator. The rolling time-domain optimization problem is solved online to obtain the optimal steering angle command sequence and speed command sequence of each axis in the control time domain.

[0031] The first element of the optimal steering angle command and speed command sequence obtained from the solution is sent to the train actuator to control the movement of the train.

[0032] Specifically, the process of adjusting the steering and speed of each axis in real time is as follows:

[0033] The SPMT train control system has a built-in kinematic transformation model from joint space to wheel space. The kinematic transformation model takes the expected motion speed and instantaneous turning radius of the overall geometric center of the train as input, and combines the fixed position coordinates of each independent wheel set relative to the geometric center. Through forward kinematic calculation, the target steering angle and target linear velocity of each wheel set are calculated in real time.

[0034] The optimal steering angle command and speed command sequence are sent to the local controller of each wheel set through a real-time industrial Ethernet fieldbus network. Each local controller is equipped with a steering PID control submodule and a drive PID control submodule. The steering PID control submodule receives the target steering angle command and its output is connected to the hydraulic steering servo valve to control the displacement of the wheel set steering cylinder. The drive PID control submodule receives the target linear speed command and its output is connected to the frequency converter or servo driver of the drive motor to control the motor speed.

[0035] Specifically, the internal configurations of the steering PID control submodule and the drive PID control submodule include:

[0036] The steering PID control submodule takes the difference between the target steering angle and the actual steering angle fed back by the encoder as input. After proportional, integral and derivative operations, it outputs an analog voltage signal to the amplifier of the electro-hydraulic servo valve to control the hydraulic oil flow and thus drive the steering cylinder to move.

[0037] The drive PID control submodule takes the difference between the target linear speed and the actual linear speed calculated from the feedback of the motor encoder as input. After PID calculation, it outputs an analog or pulse signal to the frequency converter or servo driver that drives the motor to adjust the motor torque and speed.

[0038] Specifically, the criteria for determining whether the train crew has arrived at its destination are as follows:

[0039] The three-dimensional coordinates of the geometric center of the SPMT train are monitored in real time by a high-precision GNSS positioning system or a total station. When the geometric center coordinates of the train enter a three-dimensional sphere with the target point coordinates as the center and a radius of preset accuracy tolerance, and remain stable for a preset period of time, it is determined that the train has accurately arrived at the destination, the entire transfer task is completed, the control system disconnects the drive power, and maintains the parking brake state.

[0040] A path planning and dynamic obstacle avoidance system for SPMT transportation includes: a path planning module, an environmental perception module, a dynamic obstacle avoidance module, a motion control module, and a task monitoring module;

[0041] The path planning module, based on a preset three-dimensional static environment digital base map, plans a preliminary safe path from the starting point to the end point for the SPMT train set.

[0042] The environmental perception module acquires real-time point cloud and image data through millimeter-wave radar arrays and stereo vision cameras deployed at the front and sides of the vehicle during the SPMT train's journey along the initial safe path, and generates a real-time environmental map that integrates static base maps and dynamic obstacles.

[0043] The dynamic obstacle avoidance module monitors the relative position of dynamic obstacles and the three-dimensional pipe-shaped safety area centered on the initial safe path and expanded according to the vehicle's geometry and safety margin in real time, and uses a time elastic band algorithm to replan a locally optimal obstacle avoidance path online.

[0044] The motion control module receives the local optimal obstacle avoidance path, parameterizes it into a curve with arc length as the parameter, and outputs the optimal steering angle command sequence and speed command sequence.

[0045] The task monitoring module, relying on a high-precision GNSS positioning system, displays the position, attitude, speed, and planned path of the SPMT vehicle group on the three-dimensional digital base map in real time. When the system determines through GNSS data that the geometric center of the vehicle group has entered the end-point tolerance range and remains stable for more than a preset time, it determines that the transfer task is completed and the control system automatically switches to the safe parking state.

[0046] Compared with the prior art, the beneficial effects of the present invention are:

[0047] 1. This invention proposes a path planning and dynamic obstacle avoidance system for SPMT transportation, and optimizes and improves its architecture, operation steps and processes. The system has the advantages of simple process, low investment and operation costs and low production and operation costs.

[0048] 2. This invention proposes a path planning and dynamic obstacle avoidance method for SPMT (Special Purpose Vehicle) transport. By fusing multi-source data, a three-dimensional static environmental digital base map containing spatial, semantic, and risk information is constructed. Combined with SPMT train parameters, a preliminary safe path is planned in advance, enabling precise avoidance of static risks such as underground pipelines, foundation limitations, and above-ground structures. Simultaneously, by integrating millimeter-wave radar and visual sensor data, dynamic obstacles that are not pre-modeled or whose positions are changing can be efficiently identified. With the precise definition of the three-dimensional pipeline-like path safety zone, path replanning can be triggered in a timely manner. From static prediction to dynamic response, collision risk is reduced in all aspects, significantly improving the safety of the SPMT transport process.

[0049] 3. This invention proposes a path planning and dynamic obstacle avoidance method for SPMT (Special Purpose Vehicle) transportation. Utilizing an online replanning approach based on a time-elastic band algorithm, it can avoid dynamic obstacles while maintaining trajectory smoothness and initial path following accuracy, reducing trainset adjustment costs. Combined with model predictive control algorithms, kinematic transformation models, and PID control, it enables precise real-time control of steering and speed on each axle, ensuring trajectory tracking accuracy. Full-process cyclical dynamic monitoring and optimization, coupled with a high-precision positioning endpoint determination mechanism, ensures the trainset efficiently and accurately reaches the target location, effectively adapting to complex transportation environments and improving the reliability and execution efficiency of SPMT transportation tasks. Attached Figure Description

[0050] Figure 1 This is a schematic diagram of the path planning and dynamic obstacle avoidance method of the present invention applied to SPMT transportation;

[0051] Figure 2 This is a flowchart illustrating the principle of the path planning and dynamic obstacle avoidance method applied to SPMT transportation according to the present invention. Detailed Implementation

[0052] Example 1:

[0053] Please see Figure 1 and Figure 2 One embodiment of the present invention provides a path planning and dynamic obstacle avoidance method applied to SPMT transportation, specifically including:

[0054] S1: Construct a three-dimensional static environment digital base map; the three-dimensional static environment digital base map defines all known static risk areas;

[0055] Furthermore, taking the relocation project of a reactor module within a large chemical plant as an example, the relocation path needs to traverse a pipe gallery area, a soft soil foundation area, and pass through a height-restricted gantry. The specific process of constructing a three-dimensional static environment digital base map includes: First, obtaining the precise burial depth and direction of underground pipelines through the digital processing of engineering drawings, generating pipeline direction grid data; obtaining the mechanical properties of the foundation soil layers in the relocation area through ground-penetrating radar and static cone penetration testing, generating stratified isosurface data of foundation bearing capacity, and identifying the local soft soil area labeled Zone-Soft-02; scanning the above-ground pipe gallery, gantry, and surrounding buildings using a three-dimensional laser scanner, generating a surface point cloud and triangular mesh model with millimeter-level precision; subsequently, uniformly registering the above three sets of multi-source data to the project's independent coordinate system, such as taking the southwest corner of the plant area as the origin; finally, using the voxel raster method to fuse the registered data, discretizing the space into a cubic voxel array with a side length of 0.2 meters, recording the center coordinates of each voxel, and assigning semantic attributes and risk levels based on the fused data source. For example, voxels located within 1 meter directly above underground pipelines are marked as high-risk - pipelines; voxels located within Zone-Soft-02 with a bearing capacity below a preset threshold, such as 80 kPa, are marked as medium-risk - soft soil; and voxels located below height-restricted gantry with a height below the total height of transported goods, such as 12.5 meters, are marked as high-risk - height-restricted. Ultimately, a three-dimensional static environmental digital base map integrating spatial, semantic, and multi-level risk information is generated.

[0056] S2: Based on the three-dimensional static environment digital base map, combined with the axle load, geometric shape and passability parameters of the SPMT train, calculate an initial safe path to avoid known static risk areas offline;

[0057] Furthermore, in this embodiment, the SPMT trainset consists of 4 axles and 256 independent steering wheels. The transported reactor module measures 15m long × 8m wide × 12m high, with a total weight of 800 tons. Based on this, the path planning algorithm searches on a three-dimensional static environment digital base map, using the trainset's starting and ending points as the starting and ending points, such as assembly area A and installation foundation B. The path planning algorithm simplifies the trainset into a three-dimensional envelope considering the turning radius, where the minimum turning radius of this trainset is 15 meters. Multiple constraints are imposed during the search: the path must completely avoid all voxels marked as high-risk, such as directly above pipelines or height-restricted areas; when the path crosses a medium-risk soft soil area, it must ensure that the ground pressure of each axle of the trainset is lower than the allowable bearing capacity recorded by the voxel in that area. If necessary, the path planning algorithm automatically plans a gentler gradient to reduce axle load. Finally, a preliminary safe path L0 with a length of approximately 350 meters, including 3 curves, successfully bypasses all known static risk areas and is stored as a series of dense path point sequences.

[0058] Furthermore, the specific steps of S2 include:

[0059] (1) Based on the axle load, geometric shape and passability parameters of the SPMT train set, and combined with the risk level and attributes of the voxels recorded in the three-dimensional static environment digital base map, the rigid constraints and cost model of the path planning are defined. The rigid constraints include the minimum turning radius, maximum climbing and downhill angle of the train set. At the same time, the axle load of the train set is transformed into the requirements of the foundation bearing capacity to form dynamic constraints. That is, the foundation bearing capacity of the area through which the planned path passes must be sufficient to support the load of the train set and prevent foundation failure. The cost model quantifies and evaluates the cost of traversing voxels of different risk levels, such as high-risk, medium-risk and low-risk areas, as well as the cost of performing different motion actions, such as going straight, turning and climbing. Finally, the physical and safety constraints are transformed into computer-processable rules and quantitative indicators.

[0060] (2) Guided by the defined rigid constraints and cost model, the three-dimensional static environment digital base map is processed and converted into a discrete data structure suitable for graph search. Specifically, based on the two-dimensional plane projection of voxels in the three-dimensional static environment digital base map, each voxel or its center point is regarded as a node of the graph. According to the defined cost model, each node is assigned a passage cost, which is directly related to the comprehensive risk level label recorded in its digital base map. For example, the prohibited passage area is set to unlimited cost, and the high-risk area is set to high cost. At the same time, connecting edges are established between adjacent nodes. The weight of each edge, i.e. the movement cost, is calculated by the defined cost model related to the movement of the vehicle group. For example, the turning cost considering the turning angle and the slope cost considering the elevation change are obtained. Thus, a passage cost map covering the global environment is obtained, with nodes and edges having clear cost weights.

[0061] (3) Perform a heuristic search on the traffic cost graph to obtain the original path, including: using the constructed traffic cost graph as the search space, using the starting and ending points of the vehicle group's movement as the starting and target nodes of the search, and using a heuristic search algorithm to iteratively explore adjacent nodes starting from the starting node. In each exploration, the heuristic search algorithm calculates the cumulative actual cost from the starting point to the current node, and uses a heuristic function, such as the Euclidean distance from the current node to the target node, to estimate the remaining cost, and prioritizes expanding the node with the minimum total cost; throughout the search process, strictly verify the defined rigid constraints: for any candidate Select a path segment, check whether its turning radius is less than the minimum turning radius, whether the slope exceeds the maximum limit, and verify whether the foundation bearing capacity of all relevant voxel records below the path meets the requirements by simulating the axle pressure distribution of the train. Any node or path segment that violates the constraints will be immediately eliminated. When the heuristic search algorithm successfully reaches the target node, a polyline trajectory consisting of graph nodes connecting the start and end points is generated by backtracking. This is the preliminary safe path that satisfies all safety and motion constraints. The heuristic search algorithm is prior art in this field and is not an inventive solution of this application. It will not be described in detail here.

[0062] S3: During the SPMT trainset’s journey along the initial safety path, real-time point cloud and image data are acquired through deployed millimeter-wave radar and vision sensors. The real-time point cloud and image data are then fused and compared with a three-dimensional static environment digital base map to identify obstacles that have not been pre-modeled or whose positions have changed, and to generate a real-time environment map containing dynamic information.

[0063] Furthermore, when the vehicle travels along path L0 to the edge of the utility tunnel area, approximately 120 meters from the starting point, four millimeter-wave radars and two stereo vision cameras deployed at the front and sides of the vehicle begin operation. The point cloud data generated by the millimeter-wave radar is first denoised and ground-filtered, and then segmented into multiple candidate obstacle point cloud clusters through Euclidean clustering. Simultaneously, the real-time images acquired by the stereo vision cameras are processed by the YOLOv5 network for target detection, identifying objects such as construction workers, toolboxes, and temporary barriers. Through stereo matching and coordinate transformation, the three-dimensional bounding boxes of these objects in the world coordinate system are obtained. The data fusion module associates the radar point cloud clusters with the visual three-dimensional bounding boxes, confirming a temporary barrier obstacle O1 located approximately 20 meters ahead of the path and not marked on the base map. Then, the fused dynamic obstacle information is compared voxel by voxel with the three-dimensional static environment digital base map, marking the space voxels occupied by O1 as dynamic obstacles, thereby generating a real-time environment map superimposed with the dynamic obstacle O1.

[0064] S4: When an obstacle is detected to have entered the safe zone of the path, the local optimal obstacle avoidance path is replanned online based on the real-time environment map, with the current position of the vehicle crew as the starting point and the original target point as the ending point.

[0065] Furthermore, the path safety zone is defined according to the parameters of this train set: based on the path centerline, it extends horizontally to both sides, with the specific extension length being half of the vehicle width of 8m plus a safety margin of 1.5m, which is 5.5m, forming a safety corridor with a width of 11m; this corridor extends vertically upward to the cargo height of 12.5m and downward to a depth of 1m underground considering the foundation effect, forming a three-dimensional pipe-like area. The system detects that a temporary obstacle O1 has partially encroached upon the safe area. Subsequently, the system performs local replanning based on the time-elastic band algorithm. Taking the path segment L0, which is 50 meters ahead of the current position, as the initial trajectory band, the system calculates the intrusion depth of the temporary obstacle O1 based on its surface points and constructs a repulsive potential field along its surface normal. Based on the time-elastic band algorithm, an optimization problem is constructed, the objective function of which simultaneously minimizes: the curvature of the new trajectory, the deviation of the new trajectory from the initial safe path L0, and the sum of the repulsive potential fields from O1 experienced by all points on the new trajectory. Through numerical optimization, the system outputs a locally optimal obstacle avoidance path L1 that smoothly bypasses O1 within milliseconds based on the time-elastic band algorithm. After bypassing O1, L1 gradually converges back to the original initial safe path L0.

[0066] S5: The optimal obstacle avoidance path is sent to the SPMT train control system. The SPMT train control system uses a model predictive control algorithm to track the trajectory with virtual reference points. By adjusting the steering and speed of each axle in real time, the train is driven to travel along the optimal obstacle avoidance path.

[0067] Furthermore, the motion control module parameterizes the locally optimal obstacle avoidance path L1 as a curve with arc length s as the parameter. At the start of each control cycle, such as 100ms, the model predictive control algorithm uses the current actual pose (X coordinate, Y coordinate, heading angle θ) of the vehicle's geometric center, i.e., the GPS antenna phase center, as the initial state, where X represents the horizontal axis and Y represents the vertical axis. Based on the current vehicle speed, such as 0.3 meters per second, the model predictive control algorithm anticipates a dynamic distance on the parameterized curve L1, such as the anticipation time. A virtual reference point R is determined. The model predictive control algorithm establishes a predictive model that includes the vehicle's kinematics and a nonlinear tire model. Within a finite time domain, such as the next 5 seconds, a state point is predicted every 0.1 seconds to predict the evolution of the vehicle's state. Subsequently, a rolling time domain optimization problem is solved, with the goal of minimizing the error between the future state of the vehicle and the reference trajectory, while constraining the steering angular velocity and driving acceleration of each wheel set within physical limits. The optimal steering angle and speed command sequence for each axle in the future time domain is obtained, and only the first command in the sequence, i.e., the optimal command for the current control cycle, is issued.

[0068] Furthermore, after the command is issued, the joint-space to wheel-space kinematic model constructed within the SPMT train control system begins to operate. This model takes the desired longitudinal and lateral velocities of the train's overall geometric center, provided by the MPC, as input. Combined with the fixed position coordinates of each independent wheel set relative to the geometric center, it calculates the target steering angle and target linear velocity for each wheel set in real time through forward kinematics calculations. The target steering angle command is sent to the local controller of each wheel set via real-time Ethernet. The steering PID control submodule of the local controller receives the target steering angle, compares it with the actual steering angle fed back by the encoder, and outputs an analog voltage signal to the electro-hydraulic servo valve after PID calculation, precisely controlling the displacement of the steering cylinder. Similarly, the drive PID control submodule receives the target linear velocity, compares it with the actual linear velocity converted from the motor encoder feedback, and outputs a control signal to the frequency converter after PID calculation, adjusting the speed and torque of the drive motor. Through the coordinated action of all wheel sets, the train, tens of meters long and weighing hundreds of tons, is driven precisely along the locally optimal obstacle avoidance path L1, smoothly bypassing the temporary barrier O1.

[0069] S6: Throughout the entire transfer process, S3 to S5 are executed in a loop until the train arrives at its destination.

[0070] The criteria for determining whether the train crew has reached its destination are:

[0071] The three-dimensional coordinates of the geometric center of the SPMT train are monitored in real time by a high-precision GNSS positioning system or a total station. When the geometric center coordinates of the train enter a three-dimensional sphere with the target point coordinates as the center and a radius of preset accuracy tolerance, and remain stable for a preset period of time, it is determined that the train has accurately arrived at the destination, the entire transfer task is completed, the control system disconnects the drive power, and maintains the parking brake state.

[0072] The process of constructing the three-dimensional static environment digital base map includes:

[0073] S1.1: Obtain the precise burial depth and direction of underground pipelines through digital processing of engineering drawings, generate pipeline direction grid data, obtain the mechanical properties of foundation soil layers through ground-penetrating radar detection, generate foundation bearing capacity layered isosurface data, obtain the surface geometric information of above-ground structures through three-dimensional laser scanning, and generate surface point cloud and triangular mesh models.

[0074] S1.2: Register the pipeline routing grid data, the stratified isosurface data of foundation bearing capacity, and the surface point cloud and triangular mesh model to the same geographic coordinate system to obtain the registered multi-source data;

[0075] Furthermore, the specific steps in S1.2 include:

[0076] (1) Establish a unified geographical reference benchmark. Specifically, based on the scope and accuracy requirements of the project's relocation operations, adopt the geodetic coordinate system and elevation benchmark. Under the selected benchmark, obtain high-level control points that cover the entire relocation area and whose three-dimensional coordinates have been accurately determined by professional surveying and mapping units.

[0077] (2) Different types of source data are converted to the established unified benchmark to form data with preliminary geographic coordinates. Specifically, this includes parallel processing of three types of data: First, for pipeline routing grid data, the geometric and attribute information of the pipeline is obtained by digitizing and vector extracting the design drawings. Then, using the feature points known on the drawings and corresponding to the control points in the first step, the coordinate transformation parameters are calculated to convert the pipeline data from the drawing coordinate system to the unified geographic coordinate system. Second, for foundation bearing capacity data, the location of the on-site geological exploration points is surveyed. This process involves connecting the exploration points with control points in a unified geographic coordinate system to obtain precise geographic coordinates for each exploration point. Using these coordinates as control, a layered isosurface of foundation bearing capacity with geographic coordinates is generated through spatial interpolation. Furthermore, for the surface point cloud obtained by 3D laser scanning, the common targets between multiple stations are first used to perform relative stitching in the scanner coordinate system. Then, a total station is used to measure the precise 3D coordinates of these common targets in a unified geographic coordinate system. Using these coordinates as control points, spatial transformation parameters are calculated to transform the complete stitched point cloud and its derived triangular mesh model to a unified geographic coordinate system.

[0078] (3) Eliminate such biases through cross-validation and minor adjustments between data. Specifically, identify and select multiple pairs of corresponding, clear geometric features between data from different sources. For example, valve center points that are clearly visible in the point cloud model and clearly marked on the pipeline design drawing, or terrain feature lines that are reflected in the surface point cloud and described in the geological exploration profile. Use the coordinate difference of these corresponding feature pairs in their respective data sources as constraints, calculate a set of optimal correction parameters through methods such as constrained least squares adjustment, and make overall minor adjustments to the coordinates of some or all datasets, thereby minimizing the overlay error between different data sources and ensuring that the pipeline depth matches the geological stratification and that the surface structures are aligned with the underground model.

[0079] (4) The pipeline orientation grid data, foundation bearing capacity layered isosurface data, surface point cloud and triangular mesh model that have been completed with high precision spatial alignment, together with their respective attribute information, are integrated and output as registered multi-source data.

[0080] S1.3: The registered multi-source data is fused using the voxel raster method, and semantic category attributes and corresponding risk level labels are assigned based on the fused data source to generate a three-dimensional static environmental digital base map containing spatial, semantic and risk information.

[0081] Furthermore, the specific steps in S1.3 include:

[0082] (1) Based on the pipeline routing grid data, foundation bearing capacity layered isosurface data, and the spatial range covered by the surface point cloud and triangular mesh model that have been uniformly registered with coordinates, a cubic space that can completely contain all the above data is determined. The cubic space is uniformly divided in the three orthogonal dimensions of length, width, and height according to the preset fixed side length, thereby generating a three-dimensional voxel array composed of many small cubic units of equal volume. Each unit of the three-dimensional voxel array is called a voxel, and its position is uniquely determined by its three-dimensional index in the array, forming a regular discretized basic space container that covers the entire transportation environment.

[0083] (2) Traverse each voxel in the constructed three-dimensional voxel array, and determine its spatial association and attribute label one by one. For pipeline routing grid data, determine whether the voxel is occupied by any pipeline model geometry. If so, mark the voxel as underground pipeline semantic category and assign it an initial risk label according to pipeline attributes. For foundation bearing capacity layer isosurface data, determine which soil layer isosurface defines the space where the voxel center point is located, and mark the soil layer type and bearing capacity attribute to the voxel. At the same time, assign it an initial risk label according to the bearing capacity value. For surface point cloud and triangular mesh model, determine whether the voxel is occupied by the surface of the ground or structure it represents. If so, mark the voxel as surface or above-ground structure semantic category and assign it an initial risk label according to whether the voxel height is lower than the vehicle clearance height. Accordingly, each voxel is assigned one or more semantic categories and corresponding initial risk labels mapped from the three types of data sources.

[0084] (3) Based on the semantic category and initial risk label set obtained for each voxel mapping, the system calls the preset risk assessment rule base to make a comprehensive risk judgment for each voxel. The risk assessment rule base specifies different semantic categories, such as underground pipelines and above-ground structures, as well as the combination logic and priority between different initial risk labels. The system calculates each voxel according to the risk assessment rule base: if the voxel is marked as an underground pipeline or above-ground structure, which represents the semantic category of entity occupation, then regardless of its foundation attributes, its final risk level is directly judged as prohibiting passage; if the voxel is not marked by entity occupation semantics, but its foundation bearing capacity initial risk label shows high risk, then its final risk level is judged as high risk; in other cases, it is judged as medium risk or low risk according to the foundation conditions, and finally outputs a unique comprehensive final risk level label for each voxel.

[0085] (4) Integrate the spatial framework of the constructed three-dimensional voxel array, the semantic category attribute of each voxel obtained by mapping, and the final risk level label of each voxel, and generate a standardized data unit for each voxel in the three-dimensional voxel array. The data unit shall contain at least its three-dimensional index, semantic category attribute and final risk level label. All standardized data units of voxels are organized in the order of their three-dimensional indexes to form a complete three-dimensional structured data field. The three-dimensional structured data field and its metadata describing the spatial range, voxel size and coordinate system together constitute the final three-dimensional static environment digital base map.

[0086] By fusing and comparing real-time point cloud and image data with a 3D static environment digital base map, obstacles that were not pre-modeled or whose positions have changed are identified, generating a real-time environment map containing dynamic information, including:

[0087] S3.1: Preprocess the real-time point cloud data acquired by millimeter-wave radar to obtain candidate obstacle point cloud clusters; the preprocessing includes point cloud denoising, ground point filtering, and clustering segmentation;

[0088] Furthermore, the point cloud denoising process is as follows: For each original point cloud data point, the system searches for multiple points within a certain radius or nearest neighbors in its three-dimensional spatial neighborhood. By calculating the spatial distribution characteristics of the point cloud within this neighborhood, such as calculating the average distance from the point to all its neighboring points, the system evaluates its dispersion. If the number of neighboring points of any point is too small, or if the average distance from any point to its neighboring points is significantly greater than the average of the entire scene, then the point can be determined as a noise point because it lacks reasonable spatial continuity with its surrounding points and does not conform to the characteristics of a continuous physical surface. The system traverses all points and identifies and removes these statistically isolated points.

[0089] Furthermore, this application employs an adaptive plane fitting and segmentation method based on a random sampling consensus algorithm to filter out ground points. Specifically, a small number of points are randomly selected from the point cloud to fit an initial three-dimensional plane model. Then, the vertical distance from all points to this hypothetical three-dimensional plane is calculated. Next, a distance threshold is set, and points with a vertical distance less than the threshold are marked as interior points, while those greater than the threshold are marked as exterior points. This process is repeated multiple times with random sampling and fitting. Finally, the plane model with the most interior points is selected as the ground model. All points classified as interior points of this plane model are filtered out. Ultimately, most points representing continuous ground are removed, leaving mainly point clouds of objects above the ground, such as vehicles, equipment, personnel, curbs, and guardrails.

[0090] Furthermore, this application employs a clustering algorithm based on Euclidean distance when performing point cloud clustering and segmentation. However, the clustering algorithm based on Euclidean distance is prior art in this field and is not an inventive solution of this application, so it will not be described in detail here.

[0091] S3.2: Preprocess the real-time image data acquired by the vision sensor, use a deep learning object detection network to identify obstacle regions in the real-time image, and transform the obstacle regions from the image coordinate system to the world coordinate system to obtain the transformed visual obstacle regions;

[0092] Furthermore, the specific steps in S3.2 include:

[0093] (1) Acquire real-time image data output by the visual sensor, perform noise reduction and enhancement processing on the real-time image, and normalize the size of the processed image to a preset fixed resolution, such as 640 pixels wide and 480 pixels high, to form a standardized image that conforms to the input format of the deep learning model.

[0094] (2) Identifying obstacle regions through a pre-trained object detection network, specifically including: inputting the obtained standardized image into a pre-trained deep learning-based object detection network model, which has been trained on a large amount of industrial scene image data and has the ability to identify typical obstacles such as vehicles, equipment, and personnel. The object detection network model performs forward inference operations on the input standardized image, extracts image features, predicts and outputs information on all detected obstacles in the image; for each detected obstacle, the object detection network model outputs its category label, a confidence score representing the reliability of detection, and the two-dimensional position range of the obstacle in the image. This position range is usually defined by a rectangular bounding box in the image pixel coordinate system. The bounding box is determined by the horizontal and vertical coordinates of its upper left corner point, as well as the width and height. The final output is an obstacle detection result containing the category, confidence score, and image pixel coordinate bounding box. The object detection network model is the prior art in this field and is not an inventive solution of this application, so it will not be described in detail here.

[0095] (3) Receive all the output obstacle detection results and set a confidence threshold. Compare the confidence score of each obstacle detection result with the confidence threshold. Only the detection results with a confidence score greater than or equal to the confidence threshold are retained as reliable obstacle detection boxes. All detection results with a confidence score lower than the confidence threshold are discarded. The output is a set of high-confidence obstacle detection boxes and their corresponding category information after filtering.

[0096] (4) For each high-confidence obstacle detection box output, a mapping from two-dimensional image coordinates to three-dimensional world coordinates is performed. This transformation depends on the camera intrinsic matrix and the camera extrinsic matrix relative to the world coordinate system, which are obtained in advance through camera calibration. For each obstacle detection box, its depth information needs to be obtained first. It is usually assumed that the bottom of the obstacle is in contact with the ground plane. By solving the pixel coordinates of the midpoint of the bottom boundary of the detection box in the image, and combining the spatial geometric relationship between the camera and the ground plane defined by the known camera extrinsic matrix, the three-dimensional depth value corresponding to the point is calculated. Then, the inverse projection of the camera imaging model is used. Based on the principle of image processing, and combining the camera intrinsic parameter matrix and the calculated 3D depth value, the midpoint of the bottom boundary is transformed from 2D image pixel coordinates to 3D camera coordinates. Finally, the camera extrinsic parameter matrix is ​​applied to transform the coordinates of this point in the 3D camera coordinate system to a unified world coordinate system. This coordinate point can then represent the approximate horizontal position of the obstacle in the world coordinate system. The final output is a visual obstacle region with 3D position and spatial range estimation in the world coordinate system. The camera intrinsic parameter matrix and the camera extrinsic parameter matrix relative to the world coordinate system are existing technologies in this field and are not inventive solutions of this application, and will not be elaborated here.

[0097] Furthermore, the calculation process for the three-dimensional depth value includes:

[0098] (1) Load two key parameters obtained in advance through the camera calibration process: the camera intrinsic matrix and the camera extrinsic matrix. The camera intrinsic matrix describes the internal optical characteristics of the camera and is used for the transformation between the image pixel coordinate system and the camera coordinate system. The camera extrinsic matrix describes the position and attitude of the camera in the world coordinate system. At the same time, obtain a known ground height value in the world coordinate system, which defines the vertical position of the ground plane.

[0099] (2) Based on the obtained camera extrinsic matrix, the specific position of the camera optical center in the world coordinate system is determined. Combined with the standard direction of vertical upward in the world coordinate system as the normal direction of the ground plane, and the known ground height value, a mathematical plane equation that can completely describe the position and direction of the ground plane in the world coordinate system is obtained. That is, the ground plane equation is determined by using the known ground height and normal vector. The ground plane equation is the prior art in this field and is not the inventive solution of this application. It will not be elaborated here.

[0100] (3) Extract the pixel coordinates of the point to be measured from the image and calculate its ray direction in the camera coordinate system. Specifically, this includes: receiving the obstacle detection box output by the visual target detection network, selecting the midpoint of the bottom boundary of the obstacle detection box, recording its pixel coordinates in the image, using the inverse matrix of the camera intrinsic parameter matrix to perform mathematical transformation on the pixel coordinates, transforming it from the two-dimensional image pixel coordinate system to the normalized plane in the three-dimensional camera coordinate system, and outputting a three-dimensional vector. This three-dimensional vector represents the direction of the ray that starts from the origin of the camera optical center and passes through the image pixel in the camera coordinate system, i.e., the direction vector.

[0101] (4) With the camera optical center as the origin and the direction vector as the direction, establish a spatial ray parameter equation in the camera coordinate system. The spatial ray parameter equation uses the product of the scale parameter and the direction vector to represent the coordinates of any point on the ray. Then, use the obtained camera extrinsic matrix to transform this spatial ray parameter equation from the camera coordinate system to the world coordinate system to obtain the mathematical expression of the ray in the world coordinate system, that is, the spatial ray equation in the world coordinate system.

[0102] (5) Combine the obtained spatial ray equation in the world coordinate system with the defined ground plane equation in the world coordinate system. By solving this system of equations, a linear equation in one variable about the scale parameter is obtained. Solving this equation, the obtained scale parameter value is the straight-line distance from the optical center of the camera to the point where the bottom of the obstacle contacts the ground plane. This distance is the three-dimensional depth value to be calculated.

[0103] Furthermore, the basic idea of ​​back projection is to regard the pixels in a two-dimensional image as projections of objects in three-dimensional space. By calculating the possible positions of each pixel in three-dimensional space, these positions are back-projected back onto the two-dimensional imaging plane, ultimately obtaining a three-dimensional model.

[0104] S3.3: The candidate obstacle point cloud clusters and the transformed visual obstacle regions are correlated and fused in the world coordinate system, and then compared with the three-dimensional static environment digital base map on a voxel-by-voxel basis to identify the areas of unmodeled obstacles or modeled obstacles in the three-dimensional static environment digital base map whose positions have changed, and generate a real-time environment map containing dynamic obstacle information.

[0105] The path safety zone is defined based on the geometry and passability parameters of the SPMT trainset, including:

[0106] A1: Based on the centerline of the initial safe path or the local optimal obstacle avoidance path, extend a safe distance horizontally to both sides. The safe distance is composed of the actual width of the vehicle group plus a preset margin, forming a two-dimensional safety corridor.

[0107] For example, the horizontal expansion distance of the two-dimensional safety corridor is calculated based on the actual width of the SPMT trainset. The core calculation logic is that the safety distance on one side is equal to half the actual width of the trainset plus a preset safety margin. After expansion on both sides, a complete two-dimensional safety corridor width is formed. In this embodiment, the actual width of the SPMT trainset transporting the reactor module is 8 meters, and the preset safety margin is 1.5 meters. Therefore, the safety distance for horizontal expansion on one side is 8 meters / 2 + 1.5 meters = 5.5 meters. After the center line of the trainset path expands horizontally by 5.5 meters on each side, a two-dimensional safety corridor with a width of 11 meters is finally formed. This width not only meets the geometric requirements of the actual driving of the trainset, but also provides sufficient safety buffer space through the preset margin to avoid the trainset from scraping and colliding with surrounding obstacles.

[0108] A2: Extend the two-dimensional safety corridor vertically upwards to the maximum height of the goods transported by the train crew, and downwards to the depth of the underground influence considering foundation deformation, thus forming a three-dimensional path pipeline-shaped safety zone. When any obstacle intrudes into this three-dimensional pipeline-shaped safety zone, the online path replanning of S4 is triggered.

[0109] The online replanning of the locally optimal obstacle avoidance path is implemented based on the time elastic band algorithm, including:

[0110] S4.1: Extract a local segment of the preliminary safe path ahead of the current position from the real-time environment map as the initial trajectory band;

[0111] Furthermore, the real-time environment map includes known static environment information and real-time perceived dynamic obstacle information.

[0112] S4.2: Detect the relative position of obstacles to the initial trajectory zone in the real-time environment map. For obstacles intruding into the safe area of ​​the intrusion path, calculate the intrusion depth based on the surface points and construct a repulsive potential field proportional to the intrusion depth along the surface normal direction.

[0113] Furthermore, the specific steps of S4.2 include:

[0114] (1) The system uses the centerline of the initial planned path as a reference, and combines the geometry and safety margin of the SPMT vehicle group to define a three-dimensional path safety area. The three-dimensional path safety area is a tubular space that surrounds the centerline of the path. The system traverses all obstacle representations in the real-time environment map, such as bounding boxes or occupied grids, calculates the spatial relationship between each obstacle and the three-dimensional path safety area, and identifies all obstacles that intersect with the safety area or whose distance is less than a preset threshold by calculating the minimum distance between the obstacle and the boundary of the safety area. It determines that they are intrusive obstacles and records their position and geometric information. The minimum distance is obtained by the Euclidean distance calculation formula. The Euclidean distance is the prior art in this field and is not an inventive solution of this application. It will not be elaborated here.

[0115] (2) For each identified intrusion obstacle, obtain its high-precision surface point cloud data. For each point in the point cloud, calculate its vertical distance to the center line of the initial planned path and compare this vertical distance with the safety radius defined by the path safety zone. If the vertical distance is less than the safety radius, it is determined that the point has intruded into the safety zone and its intrusion depth value is the safety radius minus the vertical distance. Then, traverse all surface points of the obstacle, calculate and record the intrusion depth value of each intrusion point, and extract the maximum intrusion depth of the entire surface of the obstacle as a representation of the threat level of the obstacle.

[0116] (3) For all identified intrusion points, calculate the local surface normal vector for each point. Specifically, select a certain number of nearest neighbor points in the spatial neighborhood of the point, and use these neighborhood points to fit a local tangent plane through principal component analysis. The normal direction of the local tangent plane is the surface normal direction, pointing to the outside of the obstacle. Principal component analysis is the prior art in this field and is not an inventive solution of this application, so it will not be described in detail here.

[0117] (4) Establish an independent repulsive potential field function for each intruding obstacle. The potential energy value of the repulsive potential field function at any point in space is determined by the distance from that point to the nearest intrusion point on the surface of the obstacle, the intrusion depth of that intrusion point, and the surface normal of that point. Specifically, the repulsive potential energy is highest near the surface of the obstacle and decreases outward along its surface normal direction. For the same obstacle, the greater the intrusion depth, the higher the intensity of the repulsive potential energy generated. The range of the repulsive potential field is limited to a predetermined distance around the obstacle. Beyond this range, the potential energy is zero.

[0118] (5) The independent repulsive potential fields generated for all intruding obstacles are vector superimposed in space to form a comprehensive obstacle repulsive potential field. At the same time, an attractive potential field is set at the target point of the path planning. The potential energy of this potential field is the lowest at the target point and increases with the distance. This target attractive potential field is superimposed with the comprehensive obstacle repulsive potential field to obtain the total potential field used for path search. In the total potential field, the area near the obstacle is a high potential energy peak and the target point is a low potential energy valley.

[0119] S4.3: Construct a nonlinear optimization problem with trajectory smoothness, following degree of the initial trajectory band, and sum of obstacle repulsion potential as the core objective functions. Use numerical optimization algorithm to iteratively solve the nonlinear optimization problem. By adjusting the position of the path points on the initial trajectory band, minimize the objective function and output the local optimal obstacle avoidance path that can avoid dynamic obstacles.

[0120] Furthermore, the specific implementation process of the locally optimal obstacle avoidance path includes:

[0121] (1) Taking the current position of the SPMT train as the starting point and the original target point as the ending point, a section of the trajectory in front of the current position is extracted from the initial trajectory band as the initial trajectory band to be optimized. The initial trajectory band consists of a series of ordered path points. At the same time, the total potential field formed by the superposition of the obstacle repulsion potential field and the target attraction potential field is obtained, and the spatial distribution of the potential field is known.

[0122] (2) Construct a core objective function for evaluating the quality of a trajectory. This core objective function consists of a weighted sum of three core terms:

[0123] The first term is the trajectory smoothness cost, which penalizes drastic changes in direction and distance between adjacent path points and encourages the generation of smooth and easy-to-follow trajectories. It is usually related to the curvature of the path point sequence.

[0124] The second item is the trajectory following cost, which is used to penalize the optimized trajectory for deviating too far from the initial trajectory band, ensuring that local replanning is carried out within the global framework and avoiding unnecessary large-scale detours. It is usually related to the distance between the optimized path point position and the corresponding point position on the initial trajectory band.

[0125] The third item is the obstacle potential field cost, which is used to guide the trajectory away from the obstacle. Its value is the integral or sum of the total potential field values ​​of all path points on the trajectory. If a path point falls into the obstacle repulsion zone with high potential energy, this cost will increase dramatically.

[0126] (3) Set constraints, which mainly include:

[0127] Geometric constraints: The optimized path points must be within the range allowed by the vehicle's kinematics, and the curvature of the entire path must be continuous and bounded;

[0128] Boundary constraints: Waypoints should not exceed the boundaries of the drivable area;

[0129] (4) The defined core objective function and the set constraints together constitute a nonlinear optimization problem. The sequential quadratic programming algorithm is used to iteratively solve the nonlinear optimization problem. The solution variables are the position coordinates of each path point on the initial trajectory. The sequential quadratic programming algorithm starts from the initial path point sequence. In each iteration, it calculates the total objective function value and its gradient corresponding to the current trajectory. Then, it fine-tunes the position of each path point along the direction that makes the objective function decrease, while ensuring that all constraints are met.

[0130] (5) After multiple iterations, when the decrease in the objective function value reaches the maximum number of iterations, the optimization process terminates. The resulting path point sequence is the trajectory that achieves the best overall performance in terms of smoothness, following accuracy and obstacle avoidance under the constraints. This is the locally optimal obstacle avoidance path generated by local replanning that can avoid real-time obstacles. Subsequently, this path is sent to the vehicle control system to guide real-time movement.

[0131] The specific model predictive control algorithm is as follows:

[0132] S5.1: Preprocess the local optimal obstacle avoidance path into a parameterized curve with arc length as the parameter;

[0133] Furthermore, the specific steps in S5.1 include:

[0134] (1) The system receives the local optimal obstacle avoidance path, and resamples the local optimal obstacle avoidance path using the equal arc length method to generate a new set of dense path point sequences with point spacing that meets the preset accuracy requirements;

[0135] (2) Starting from the starting point of the obtained dense path point sequence, calculate the straight-line distance between each pair of adjacent points in turn. Starting from the first point, set its cumulative arc length to zero; add the distance between the first point and the second point to the second point as the cumulative arc length of the second point; add the distance between the second point and the third point to the cumulative arc length of the second point to obtain the cumulative arc length of the third point; and so on, until the last point is calculated. Finally, assign a cumulative arc length value that monotonically increases from zero to the total length of the path to each path point in the dense path point sequence, and establish a one-to-one correspondence between the spatial position of each point and the distance traveled on the path.

[0136] (3) Using the cumulative arc length value corresponding to each path point as the independent variable, the cubic spline interpolation method is used to construct three independent interpolation functions with the X, Y and Z coordinates of each path point as dependent variables. Together, they form a three-dimensional parametric curve with arc length as the parameter. Based on this, given an arc length value, the three-dimensional coordinates of a point on the curve can be uniquely determined by these three functions. The cubic spline interpolation method is the prior art in this field and is not the inventive solution of this application. It will not be described in detail here.

[0137] (4) Differentiate the three constructed coordinate interpolation functions with respect to the arc length parameter. Specifically, calculate the first and second derivatives of each coordinate interpolation function using numerical differentiation. The first derivative describes the tangent vector of the curve at the arc length parameter, representing the direction of the path. The second derivative, combined with the first derivative, can be used to calculate the curvature of the curve at that point, representing the degree of curvature of the path.

[0138] (5) The generated parameterized curves, i.e., the three coordinate interpolation functions, are encapsulated with the calculated derivative information to form a complete path curve object. A query interface is designed for this path curve object, with the main functions including: inputting an arc length parameter, the interface returns the three-dimensional coordinates, tangent vector, and curvature of the curve point; inputting a spatial coordinate point, the interface returns the arc length parameter corresponding to the nearest projection point on the curve through search calculation. This interface ensures that all queries meet the calculation speed requirements of real-time control.

[0139] S5.2: At the beginning of each control cycle, take the current actual state of the geometric center of the SPMT train as the initial state, aim at a dynamic distance related to the current speed of the train on the parameterized curve, determine the corresponding aiming point as the virtual reference point for this control cycle, and obtain its expected pose and speed.

[0140] Furthermore, the specific steps in S5.2 include:

[0141] (1) At the beginning of each control cycle, the current actual state of the geometric center of the SPMT train in the global coordinate system is obtained through the on-board sensor system, including its three-dimensional position, heading angle, yaw rate and linear velocity, and at the same time, the parameterized curve is received.

[0142] (2) Based on the current position of the geometric center of the train, perform a nearest point search on the parametric curve to obtain the arc length parameter value corresponding to the projection point of the current position of the train on the path curve, which is called the current arc length. Then, based on the magnitude of the current linear velocity of the train, multiply by a preset aiming time constant to calculate an aiming distance. Add this aiming distance to the current arc length to obtain the aiming arc length parameter value. If the calculated aiming arc length exceeds the total length of the path curve, limit it to the arc length value corresponding to the end point of the path.

[0143] (3) Take the calculated pre-aiming arc length parameter value as input, call the parameterized curve query interface, which returns the path point corresponding to this arc length parameter value, i.e. the pre-aiming point, and obtain its accurate three-dimensional coordinates, tangent direction vector and path curvature at the point.

[0144] (4) Based on the geometric information of the pre-aiming point obtained by query, calculate the expected state that the train should reach at the point. The expected position is the three-dimensional coordinate of the pre-aiming point. The expected heading angle is determined by the projection direction of the tangent direction vector of the pre-aiming point onto the horizontal plane. The magnitude of the expected speed is determined by a comprehensive calculation based on the geometric characteristics of the path, mainly the curvature at the pre-aiming point and the dynamic constraints of the train, to ensure driving stability. The direction of the expected speed is consistent with the tangent direction of the pre-aiming point. Finally, the expected position, expected heading angle and expected speed are encapsulated into the expected state vector of the cost control cycle.

[0145] (5) The generated desired state vector, together with the path curvature information of the target point, is used as the virtual reference point information for this control cycle and output to the model predictive controller.

[0146] S5.3: Establish a nonlinear dynamic model that includes the geometric center position coordinates, heading angle, and steering angle state of each axle of the train as a prediction model to predict the state evolution of the train in the finite time domain;

[0147] Furthermore, the specific steps in S5.3 include:

[0148] (1) Define the state variables, control inputs and external inputs of the model; the state variables include the position coordinates of the geometric center of the vehicle group in the global coordinate system, the heading angle, the linear velocity and the actual steering angle of each axis; the control inputs are the target steering angle command and the target linear velocity command issued to each axis; the external input is the current road surface adhesion coefficient;

[0149] (2) Establish the mathematical relationship between the rate of change of the geometric center position of the train set and the overall speed and heading angle of the train set. Specifically, the velocity vector in the car body coordinate system is transformed to the global coordinate system through coordinate transformation, so as to obtain the first-order differential equation of the position coordinates with respect to time.

[0150] (3) Establish a semi-empirical calculation model for the longitudinal and lateral forces on each tire. This semi-empirical calculation model expresses the tire force as a nonlinear function of tire slip ratio, tire slip angle, tire vertical load and road adhesion coefficient. The tire slip ratio and slip angle are calculated from the vehicle group linear velocity, the actual steering angle of each axle and the wheel rotation state.

[0151] (4) Establish the longitudinal and lateral dynamic equations of the vehicle body: According to Newton's second law, the longitudinal acceleration of the geometric center of the vehicle group is equal to the resultant force of all the longitudinal forces of the tires divided by the total mass of the vehicle group, and the lateral acceleration of the geometric center of the vehicle group is equal to the resultant force of all the lateral forces of the tires divided by the total mass of the vehicle group, plus the centripetal acceleration term composed of the product of the linear velocity and the yaw rate of the vehicle group.

[0152] (5) Establish the dynamic equation of heading angle: Based on Newton's second law, establish the relationship between the yaw acceleration of the train set and the yaw moment generated by the lateral forces of all tires acting on the car body, and the yaw moment of inertia of the train set. The yaw acceleration of the train set is equal to the sum of the calculated moments of all tire lateral forces about the geometric center divided by the yaw moment of inertia of the train set. Since the yaw rate is the first time derivative of the heading angle, the second differential equation of the heading angle with respect to time is established.

[0153] (6) Establish the dynamic equations of each axis steering actuator, specifically including: for the defined actual steering angle state variables of each axis, establish their dynamic equations respectively. The equations describe the relationship between the actual steering angle acceleration and the input steering angle control command, the inherent response hysteresis of the steering system, friction force and load torque.

[0154] (7) The time first-order differential equation, the longitudinal and lateral dynamic equations of the car body, the heading angle dynamic equation, and the dynamic equations of each axle steering actuator are merged and sorted to form a set of nonlinear differential equations with defined state variables as unknowns. This set of nonlinear differential equations is the continuous-time nonlinear dynamic prediction model used to describe the evolution of the car group's state.

[0155] (8) The nonlinear differential equation system is transformed into a discrete time difference equation system by using the numerical integration method. The train state at each sampling time in the preset finite time domain can be recursively predicted based on the current state measurement value and the control input sequence.

[0156] S5.4: Based on the current initial state and prediction model, construct a rolling time-domain optimization problem that includes trajectory tracking error term, control command increment term and physical constraints of each actuator, and solve the rolling time-domain optimization problem online to obtain the optimal steering angle command sequence and speed command sequence of each axis in the control time domain;

[0157] Furthermore, the specific steps in S5.4 include:

[0158] (1) The sequence of steering angle and speed commands for all sampling times and all axes in the prediction time domain is used as the sequence of control variables to be optimized. At the same time, the expected reference trajectory for each sampling time in the prediction time domain is obtained from the aiming module, including the expected position, heading angle and speed.

[0159] (2) Using the established discretized nonlinear dynamic prediction model, based on the current initial train state and the defined control variable sequence, the state of the train at each sampling time in the preset prediction time domain is recursively predicted. The position deviation, heading angle deviation and speed deviation between the predicted state and the expected reference state at each time are calculated respectively. The squares of these deviations are weighted and summed to form the trajectory tracking error cost term that runs through the entire prediction time domain.

[0160] (3) For the defined sequence of control variables, calculate the changes in steering angle command and speed command for all axes in the prediction time domain and between adjacent sampling times, and sum the squares of these increments in a weighted manner to form a control increment penalty term;

[0161] (4) Based on the physical limits of the steering and drive actuators of each axle of the SPMT train, set inequality constraints for the defined control variable sequence. The constraints of the control variable sequence include: the steering angle command of all axles at each sampling time shall not exceed the maximum and minimum steering angle range allowed by its mechanical structure; the change in steering angle command between adjacent times shall not exceed the maximum angular velocity capability of the steering actuator; the speed command of all axles at each sampling time shall not exceed the maximum and minimum speed range allowed by its drive system; the change in speed command between adjacent times shall not exceed the maximum acceleration capability of the drive system.

[0162] (5) To ensure driving safety and feasibility, constraints are set for the state variable sequence output by the prediction model. The constraints of the state variable sequence include: the predicted path point must be located within the safe driving area; the predicted vehicle attitude angle must not exceed the stability limit; and dynamic obstacle avoidance constraints are added based on environmental perception information.

[0163] (6) The constructed trajectory tracking error cost term and the constructed control increment penalty term are weighted and summed to form the total objective function that needs to be minimized. The constraints of the control variable sequence and the constraints of the state variable sequence are combined with the total objective function to form a constrained nonlinear optimization problem with the control variable sequence as the decision variable and the goal of minimizing the total objective function. In each control cycle, based on the current train state and the expected reference trajectory, the numerical optimization algorithm is used to solve this optimization problem online in real time to obtain the optimal control variable sequence that satisfies all constraints and minimizes the objective function in the prediction time domain. The numerical optimization algorithm is the prior art in this field and is not an inventive solution of this application. It will not be described in detail here.

[0164] (7) Extract all axis steering angle commands and speed commands corresponding to the first sampling time from the optimal control variable sequence obtained by the solution and output them to the actuator as the actual control commands of the current control cycle. Then, in the next control cycle, repeat the process from (1) to (7) with the new train state as the initial state and roll forward to solve the new optimization problem.

[0165] S5.5: Send the first element of the optimal steering angle command and speed command sequence obtained from the solution to the train actuator to control the movement of the train.

[0166] The process of adjusting the steering and speed of each axis in real time is as follows:

[0167] B1: The SPMT train control system has a built-in kinematic transformation model from joint space to wheel space. The kinematic transformation model takes the expected motion speed and instantaneous turning radius of the overall geometric center of the train as input, and combines the fixed position coordinates of each independent wheel set relative to the geometric center. Through forward kinematics calculation, the target steering angle and target linear velocity of each wheel set are calculated in real time. The forward kinematics is the prior art in this field and is not an inventive solution of this application. It will not be described in detail here.

[0168] B2: The optimal steering angle command and speed command sequence are sent to the local controller of each wheel set through a real-time industrial Ethernet fieldbus network. Each local controller is equipped with a steering PID control submodule and a drive PID control submodule. The steering PID control submodule receives the target steering angle command and its output is connected to the hydraulic steering servo valve to control the displacement of the wheel set steering cylinder. The drive PID control submodule receives the target linear speed command and its output is connected to the frequency converter or servo driver of the drive motor to control the motor speed. PID control is a prior art in this field and is not an inventive solution of this application, so it will not be described in detail here.

[0169] The internal configurations of the steering PID control submodule and the drive PID control submodule include:

[0170] B2.1: The steering PID control submodule takes the difference between the target steering angle and the actual steering angle fed back by the encoder as input. After proportional, integral and derivative operations, it outputs an analog voltage signal to the amplifier of the electro-hydraulic servo valve to control the hydraulic oil flow and drive the steering cylinder to move. PID control is a prior art in this field and is not an inventive solution of this application, so it will not be described in detail here.

[0171] Furthermore, the specific steps in B2.1 include:

[0172] (1) The steering PID control submodule receives the target steering angle command of the wheel set issued by the motion control module. The target steering angle of a single wheel set ranges from -30° to +30°. At the same time, the actual displacement data of the steering cylinder is collected by the high-precision photoelectric encoder mounted on the wheel set steering mechanism. The encoder sampling frequency is 1000 Hz and the resolution is 0.01° per pulse. Based on the linear correspondence between displacement and steering angle, the displacement data is converted into the actual steering angle of the wheel set.

[0173] (2) Using the encoder sampling period as the calculation period, the received target steering angle is subtracted from the converted actual steering angle to obtain a signed steering angle deviation value, which is used as the input data for each link of PID control.

[0174] (3) Multiply the steering angle deviation value with the adjusted first proportional coefficient KP1 to obtain the output value of the first proportional link, which is used as the basic control adjustment component. The first proportional coefficient KP1 is set to 2.5.

[0175] (4) The deviation value is continuously accumulated in units of deviation calculation period to obtain the deviation integral value. The deviation integral value is multiplied by the first integral coefficient KI1 after tuning to obtain the output value of the first integral link, which is used to eliminate the static deviation of the system. The first integral coefficient KI1 is set to 0.05.

[0176] (5) Calculate the change in steering angle deviation value between two adjacent cycles, divide the change by the deviation calculation cycle to obtain the deviation change rate, multiply the deviation change rate with the tuned first differential coefficient KD1 to obtain the output value of the first differential link, which is used to suppress system overshoot and oscillation. The first differential coefficient KD1 is set to 0.8.

[0177] (6) Add the output values ​​of the first proportional element, the first integral element, and the first derivative element to obtain the first total control quantity of the PID control.

[0178] (7) The first total control quantity is converted into an analog voltage signal in a linear proportional relationship through the built-in digital-to-analog conversion module of the PID control submodule. The digital-to-analog conversion accuracy is 12 bits and the output voltage range is 0 to 10 volts. The converted analog voltage signal is transmitted to the amplifier of the electro-hydraulic servo valve through a shielded cable. The analog-to-digital conversion is a prior art in this field and is not an inventive solution of this application. It will not be described in detail here.

[0179] (8) The proportional power amplifier of the electro-hydraulic servo valve receives the analog voltage signal and converts the voltage signal into control current with an amplification factor of 0.5 amperes / volts. The current is output to the solenoid coil of the electro-hydraulic servo valve. The magnitude of the control current determines the valve core opening of the electro-hydraulic servo valve.

[0180] (9) The hydraulic system supply pressure is 16 MPa, the rated flow of the electro-hydraulic servo valve is 30 liters / minute, the servo valve adjusts the hydraulic oil flow proportionally according to the valve core opening, the hydraulic oil enters the rodless chamber or rod chamber of the steering cylinder, pushes the cylinder piston to make linear motion, and through the rigid connection between the piston and the wheel set steering mechanism, drives the wheel set to complete the steering action, thereby realizing the adjustment of the wheel set steering angle;

[0181] (10) The photoelectric encoder continuously collects the actual displacement data after the steering cylinder is adjusted, converts it into the actual steering angle, and feeds it back to the steering PID control submodule in real time. The PID control submodule recalculates the steering angle deviation value and repeats (3)-(9) until the deviation value between the actual steering angle and the target steering angle is less than the accuracy threshold of 0.05°. At this time, the PID control submodule outputs a stable analog voltage signal to keep the opening of the electro-hydraulic servo valve core and the hydraulic oil flow stable, so that the wheel set is stable at the target steering angle position.

[0182] B2.2: The drive PID control submodule takes the difference between the target linear velocity and the actual linear velocity after conversion from the motor encoder feedback as input. After PID calculation, it outputs an analog or pulse signal to the frequency converter or servo driver that drives the motor to adjust the motor torque and speed. The parameters of the two PID controllers are tuned according to the response characteristics of the specific actuator.

[0183] Furthermore, the specific steps in B2.2 include:

[0184] (1) The drive PID control submodule receives the target linear velocity command of the wheel set issued by the motion control module. The target linear velocity of a single wheel set ranges from zero meters per second to one meter per second. The high-precision incremental encoder mounted on the wheel set drive motor collects the actual speed pulse data of the motor at a sampling frequency of two kilohertz and a resolution of 2,500 pulses per revolution. The drive PID control submodule converts the actual speed data of the motor into the actual linear velocity of the wheel set according to the fixed transmission ratio conversion relationship between the motor speed and the wheel set linear velocity.

[0185] (2) The drive PID control submodule uses the encoder sampling period as the calculation period, and performs a subtraction operation between the received target linear velocity and the converted actual linear velocity to obtain a signed linear velocity deviation value, which is used as the input data for each link of PID control.

[0186] (3) Multiply the linear velocity deviation value with the adjusted second proportional coefficient KP2 to obtain the output value of the second proportional link. This value is the basic adjustment component of the drive control. The second proportional coefficient KP2 is set to 3.0.

[0187] (4) The linear velocity deviation value is continuously accumulated in units of deviation calculation period to obtain the deviation integral value. The deviation integral value is multiplied by the tuned second integral coefficient KI2 to obtain the output value of the second integral link, which is used to eliminate the static deviation of the system. The second integral coefficient KI2 is set to 0.03.

[0188] (5) Calculate the change in linear velocity deviation within two adjacent cycles, divide the change by the deviation calculation period to obtain the deviation change rate, multiply the deviation change rate by the tuned second differential coefficient KD2 to obtain the output value of the second differential element, which is used to suppress system overshoot and oscillation. The second differential coefficient KD2 is set to 0.6.

[0189] (6) Add the output values ​​of the second proportional element, the second integral element, and the second derivative element to obtain the second total control quantity of the PID control;

[0190] (7) The drive PID control submodule converts the second total control quantity into an analog signal or a pulse signal in a linear proportional relationship through the built-in signal conversion module. The analog signal output range is 0 to 10 volts, and the pulse signal output frequency range is 0 to 10000 Hz. The signal conversion accuracy is 12 bits. The converted control signal is transmitted to the frequency converter or servo driver of the drive motor through a shielded cable.

[0191] (8) After receiving the control signal, the frequency converter or servo driver converts the signal into the corresponding motor drive parameters and adjusts the output power according to the magnitude of the control signal. When the control signal increases, the output frequency and voltage are increased synchronously, and the speed of the drive motor increases. When the control signal decreases, the output frequency and voltage are decreased synchronously, and the speed of the drive motor decreases. The output torque of the motor is dynamically matched and adjusted according to the load, thereby adjusting the actual linear speed of the wheel set to approach the target linear speed.

[0192] (9) The incremental encoder continuously collects the actual speed pulse data of the motor after adjustment, converts it into the actual linear speed of the wheel set, and feeds it back to the drive PID control submodule in real time. The PID control submodule recalculates the linear speed deviation value according to the new actual linear speed, repeats (3)-(8), until the deviation value between the actual linear speed and the target linear speed is less than the accuracy threshold of 0.001 meters per second. At this time, the drive PID control submodule outputs a stable control signal to keep the output power of the frequency converter or servo driver constant, so that the torque and speed of the drive motor are stable and the wheel set is accurately kept at the target linear speed position.

[0193] Furthermore, the PID parameters are dynamically tuned: the drive PID control submodule dynamically fine-tunes the proportional coefficient, integral coefficient, and derivative coefficient based on the real-time operating parameters of the drive motor, the output status of the frequency converter or servo driver, and the load changes of the wheel set. The proportional coefficient is tuned within the range of [2.0, 4.0], the integral coefficient within the range of [0.01, 0.05], and the derivative coefficient within the range of [0.4, 0.8], ensuring that the PID controller parameters match the real-time response characteristics of the actuator.

[0194] Example 2:

[0195] Another embodiment of the present invention provides a path planning and dynamic obstacle avoidance system for SPMT transportation, comprising: a path planning module, an environmental perception module, a dynamic obstacle avoidance module, a motion control module, and a task monitoring module;

[0196] The path planning module, based on a preset 3D static environment digital base map, plans a preliminary safe path from the starting point to the destination for the SPMT trainset;

[0197] The environmental perception module acquires real-time point cloud and image data through millimeter-wave radar arrays and stereo vision cameras deployed at the front and sides of the vehicle during the SPMT train's journey along the initial safe path, generating a real-time environmental map that integrates static base maps and dynamic obstacles.

[0198] The dynamic obstacle avoidance module monitors the relative position of dynamic obstacles to a three-dimensional pipe-shaped safety area centered on the initial safe path and expanded according to the vehicle's geometry and safety margin in real time, and uses a time elastic band algorithm to replan a locally optimal obstacle avoidance path online.

[0199] The motion control module receives the local optimal obstacle avoidance path, parameterizes it into a curve with arc length as the parameter, and outputs the optimal steering angle command sequence and speed command sequence.

[0200] The task monitoring module, relying on a high-precision GNSS positioning system, displays the position, attitude, speed, and planned path of the SPMT vehicle group on the three-dimensional digital base map in real time. When the system determines through GNSS data that the geometric center of the vehicle group has entered the end-point tolerance range and remains stable for more than a preset time, the transfer task is determined to be completed, and the control system automatically switches to the safe parking state.

[0201] The embodiments of the present invention have been described above with reference to the accompanying drawings. However, the present invention is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments under the guidance of the present invention without departing from the spirit and scope of the present invention. All of these variations are within the protection scope of the present invention.

Claims

1. A path planning and dynamic obstacle avoidance method applied to SPMT transportation, characterized in that, include: S1: Construct a three-dimensional static environment digital base map; the three-dimensional static environment digital base map defines all known static risk areas; S2: Based on the three-dimensional static environment digital base map, combined with the axle load, geometric shape and passability parameters of the SPMT train, calculate an initial safe path to avoid known static risk areas offline; S3: During the SPMT trainset’s journey along the initial safety path, real-time point cloud and image data are acquired through deployed millimeter-wave radar and vision sensors. The real-time point cloud and image data are then fused and compared with a three-dimensional static environment digital base map to identify obstacles that have not been pre-modeled or whose positions have changed, and to generate a real-time environment map containing dynamic information. S4: When an obstacle is detected to have entered the safe zone of the path, the local optimal obstacle avoidance path is replanned online based on the real-time environment map, with the current position of the vehicle crew as the starting point and the original target point as the ending point. S5: The optimal obstacle avoidance path is sent to the SPMT train control system. The SPMT train control system uses a model predictive control algorithm to track the trajectory with virtual reference points. By adjusting the steering and speed of each axle in real time, the train is driven to travel along the optimal obstacle avoidance path. S6: Throughout the entire transfer process, S3 to S5 are executed in a loop until the train arrives at its destination.

2. The path planning and dynamic obstacle avoidance method for SPMT transportation as described in claim 1, characterized in that, The process of constructing the three-dimensional static environment digital base map includes: The burial depth and direction of underground pipelines are obtained through the digital processing of engineering drawings, and pipeline direction grid data is generated. The mechanical properties of the foundation soil layer are obtained through ground-penetrating radar detection, and foundation bearing capacity layer isosurface data is generated. The surface geometric information of above-ground structures is obtained through three-dimensional laser scanning, and surface point cloud and triangular mesh model are generated. The pipeline routing grid data, the stratified isosurface data of foundation bearing capacity, and the surface point cloud and triangular mesh model are uniformly registered to the same geographic coordinate system to obtain the registered multi-source data. The voxel raster method is used to fuse the registered multi-source data, and semantic category attributes and corresponding risk level labels are assigned based on the fused data source to generate a three-dimensional static environmental digital base map containing spatial, semantic and risk information.

3. The path planning and dynamic obstacle avoidance method for SPMT transportation as described in claim 2, characterized in that, By fusing and comparing real-time point cloud and image data with a 3D static environment digital base map, obstacles that were not pre-modeled or whose positions have changed are identified, generating a real-time environment map containing dynamic information, including: The real-time point cloud data acquired by millimeter-wave radar is preprocessed to obtain candidate obstacle point cloud clusters; the preprocessing includes point cloud denoising, ground point filtering, and clustering segmentation. The real-time image data acquired by the visual sensor is preprocessed, and the obstacle region in the real-time image is identified by a deep learning object detection network. The obstacle region is then transformed from the image coordinate system to the world coordinate system to obtain the transformed visual obstacle region. The candidate obstacle point cloud clusters and the transformed visual obstacle regions are correlated and fused in the world coordinate system, and then compared with the three-dimensional static environment digital base map on a voxel-by-voxel basis to identify unmodeled obstacles or areas where the position of modeled obstacles has changed in the three-dimensional static environment digital base map, thereby generating a real-time environment map containing dynamic obstacle information.

4. The path planning and dynamic obstacle avoidance method for SPMT transportation as described in claim 3, characterized in that, The path safety zone is defined based on the geometry and passability parameters of the SPMT trainset, including: Using the centerline of the initial safe path or the locally optimal obstacle avoidance path as a reference, a safe distance is extended horizontally to both sides. The safe distance is composed of the actual width of the vehicle group plus a preset margin, forming a two-dimensional safe corridor. The two-dimensional safe corridor is extended vertically upward to the maximum height of the goods transported by the vehicle group and downward to the underground depth considering the influence of foundation deformation, thereby forming a three-dimensional path pipeline-shaped safe area. When any obstacle intrudes into this three-dimensional pipeline-shaped safe area, the online path replanning of S4 is triggered.

5. The path planning and dynamic obstacle avoidance method for SPMT transportation as described in claim 4, characterized in that, The online replanning of the locally optimal obstacle avoidance path is implemented based on the time elastic band algorithm, including: Extract a local segment of the preliminary safe path ahead of the current position from the real-time environment map as the initial trajectory band; The relative position of obstacles to the initial trajectory zone in the real-time environment map is detected. For obstacles in the safe area of ​​the intrusion path, the intrusion depth is calculated based on the surface points, and a repulsive potential field proportional to the intrusion depth is constructed along the surface normal direction. A nonlinear optimization problem is constructed with trajectory smoothness, following degree of the initial trajectory band, and sum of obstacle repulsion potential as the core objective functions. A numerical optimization algorithm is used to iteratively solve the nonlinear optimization problem. By adjusting the position of the path points on the initial trajectory band, the objective function is minimized, and the local optimal obstacle avoidance path that can avoid dynamic obstacles is output.

6. The path planning and dynamic obstacle avoidance method for SPMT transportation as described in claim 5, characterized in that, The specific model predictive control algorithm is as follows: The local optimal obstacle avoidance path is preprocessed into a parameterized curve with arc length as the parameter; At the beginning of each control cycle, the current actual state of the geometric center of the SPMT train is used as the initial state. A dynamic distance related to the current speed of the train is pre-aimed on the parameterized curve. The corresponding pre-aimed point is determined as the virtual reference point for this control cycle, and its expected pose and speed are obtained. A nonlinear dynamic model containing the geometric center position coordinates, heading angle, and steering angle of each axle is established as a prediction model to predict the state evolution of the train in the finite time domain. Based on the current initial state and prediction model, a rolling time-domain optimization problem is constructed, which includes trajectory tracking error term, control command increment term and physical constraints of each actuator. The rolling time-domain optimization problem is solved online to obtain the optimal steering angle command sequence and speed command sequence of each axis in the control time domain. The first element of the optimal steering angle command and speed command sequence obtained from the solution is sent to the train actuator to control the movement of the train.

7. The path planning and dynamic obstacle avoidance method for SPMT transportation as described in claim 6, characterized in that, The process of adjusting the steering and speed of each axis in real time is as follows: The SPMT train control system has a built-in kinematic transformation model from joint space to wheel space. The kinematic transformation model takes the expected motion speed and instantaneous turning radius of the overall geometric center of the train as input, and combines the fixed position coordinates of each independent wheel set relative to the geometric center. Through forward kinematic calculation, the target steering angle and target linear velocity of each wheel set are calculated in real time. The optimal steering angle command and speed command sequence are sent to the local controller of each wheel set through a real-time industrial Ethernet fieldbus network. Each local controller is equipped with a steering PID control submodule and a drive PID control submodule. The steering PID control submodule receives the target steering angle command and its output is connected to the hydraulic steering servo valve to control the displacement of the wheel set steering cylinder. The drive PID control submodule receives the target linear speed command and its output is connected to the frequency converter or servo driver of the drive motor to control the motor speed.

8. The path planning and dynamic obstacle avoidance method for SPMT transportation as described in claim 7, characterized in that, The internal configurations of the steering PID control submodule and the drive PID control submodule include: The steering PID control submodule takes the difference between the target steering angle and the actual steering angle fed back by the encoder as input. After proportional, integral and derivative operations, it outputs an analog voltage signal to the amplifier of the electro-hydraulic servo valve to control the hydraulic oil flow and thus drive the steering cylinder to move. The drive PID control submodule takes the difference between the target linear speed and the actual linear speed calculated from the feedback of the motor encoder as input. After PID calculation, it outputs an analog or pulse signal to the frequency converter or servo driver that drives the motor to adjust the motor torque and speed.

9. The path planning and dynamic obstacle avoidance method for SPMT transportation as described in claim 8, characterized in that, The criteria for determining whether the train crew has reached its destination are: The three-dimensional coordinates of the geometric center of the SPMT train are monitored in real time by a high-precision GNSS positioning system or a total station. When the geometric center coordinates of the train enter a three-dimensional sphere with the target point coordinates as the center and a radius of preset accuracy tolerance, and remain stable for a preset period of time, it is determined that the train has accurately arrived at the destination, the entire transfer task is completed, the control system disconnects the drive power, and maintains the parking brake state.

10. A path planning and dynamic obstacle avoidance system for SPMT transportation, used to implement the path planning and dynamic obstacle avoidance method for SPMT transportation as described in any one of claims 1-9, characterized in that, include: The system includes a path planning module, an environmental perception module, a dynamic obstacle avoidance module, a motion control module, and a task monitoring module. The path planning module, based on a preset three-dimensional static environment digital base map, plans a preliminary safe path from the starting point to the end point for the SPMT train set. The environmental perception module acquires real-time point cloud and image data through millimeter-wave radar arrays and stereo vision cameras deployed at the front and sides of the vehicle during the SPMT train's journey along the initial safe path, and generates a real-time environmental map that integrates static base maps and dynamic obstacles. The dynamic obstacle avoidance module monitors the relative position of dynamic obstacles and the three-dimensional pipe-shaped safety area centered on the initial safe path and expanded according to the vehicle's geometry and safety margin in real time, and uses a time elastic band algorithm to replan a locally optimal obstacle avoidance path online. The motion control module receives the local optimal obstacle avoidance path, parameterizes it into a curve with arc length as the parameter, and outputs the optimal steering angle command sequence and speed command sequence. The task monitoring module, relying on a high-precision GNSS positioning system, displays the position, attitude, speed, and planned path of the SPMT vehicle group on the three-dimensional digital base map in real time. When the system determines through GNSS data that the geometric center of the vehicle group has entered the end-point tolerance range and remains stable for more than a preset time, it determines that the transfer task is completed and the control system automatically switches to the safe parking state.

Images