Equipment pose prediction and safety monitoring system and method for rescue robot

Abstract

The invention relates to an equipment pose prediction and safety monitoring system and method for a rescue robot, and belongs to the technical field of rescue robots, and the system comprises a hardware sensing layer which is responsible for original data acquisition and comprises a laser radar, an inertial measurement unit and a joint encoder. The data processing layer is a core calculation engine, comprises an SLAM module and is responsible for fusing sensor data, constructing an environment point cloud map in real time and estimating the pose of an equipment body; and meanwhile, a nonlinear optimization solver is included and is used for executing stable pose prediction. The intelligent decision-making layer is the brain of the system, comprises a parallel pose prediction and risk map generation module and is responsible for carrying out rapid stability evaluation on a front area; meanwhile, the system comprises a gravity center dynamic sensing and overturning early warning evaluation module which is responsible for whole machine stability monitoring and risk early warning based on the real-time gravity center position.

Description

Technical Field

[0001] This invention relates to the field of rescue robot technology, specifically to an equipment pose prediction and safety monitoring system and method for rescue robots. Background Technology

[0002] In emergency rescue and other scenarios, rescue robots, as core equipment, need to perform tasks in complex, unstructured terrain environments such as gravel, gullies, and steep slopes. The accuracy and safety of their posture control directly determine operational efficiency and safety. When existing mobile equipment operates in complex terrain environments, posture control mainly relies on the operator's subjective experience and visual judgment, lacking scientific decision support based on terrain physical information, resulting in many technical shortcomings.

[0003] First, existing terrain perception methods based on two-dimensional maps or low-precision elevation data are unable to accurately reconstruct the complex real three-dimensional geometric structures such as gravel, gullies, and steep slopes at the work site, making it impossible to accurately determine the contact state between the equipment and the ground. Furthermore, the lack of effective modeling of the dynamic coupling relationship between the equipment's physical structural properties and terrain geometric features makes it impossible to predict the equipment's pose before operation, ensuring both stability requirements and practical execution. Existing systems are mostly limited to static passability analysis and cannot perform real-time, online pose prediction and optimization based on the equipment's movement intentions. This makes it easy for equipment to overturn or sink when operating in unknown or dangerous terrain, and the lack of effective pre-operational warning capabilities severely restricts operational efficiency and threatens operational safety.

[0004] A search revealed two existing patents: a rollover warning device based on real-time attitude data (patent number: CN117360412A) and an intelligent engineering machinery operation attitude prediction method (patent number: CN117668429A). The former focuses on dynamic rollover monitoring and warning for multi-axle vehicles, while the latter emphasizes attitude prediction optimization for engineering machinery. Both involve attitude data acquisition, dynamic modeling, and filtering prediction, but neither fully integrates 3D point cloud terrain information with nonlinear optimization for pre-position prediction, resulting in insufficient coupled modeling and real-time warning capabilities in complex terrain.

[0005] Finally, existing systems are mostly limited to static mobility analysis and cannot perform real-time, online pose prediction and optimization based on the equipment's intended movement. In unknown or dangerous terrain, the attitude changes of equipment during movement are dynamic, and static analysis cannot respond to these changes in a timely manner, making the equipment prone to accidents such as overturning and sinking. Furthermore, the lack of effective early warning capabilities severely restricts operational efficiency and threatens operational safety. Summary of the Invention

[0006] The present invention aims to solve the above-mentioned technical problems by providing an equipment pose prediction and safety monitoring system and method for rescue robots.

[0007] To solve the above-mentioned technical problems, the technical solution provided by the present invention is as follows:

[0008] A system for predicting the pose and monitoring the safety of rescue robots, including...

[0009] The hardware perception layer is used to synchronously collect raw data from multiple sources, including lidar, inertial measurement unit and joint encoder; the lidar is used to acquire three-dimensional point cloud data of the environment, the inertial measurement unit is used to collect the equipment's own angular velocity, acceleration and orientation attitude data, and the joint encoder is used to read the joint angle data of the robotic arm.

[0010] The data processing layer, which communicates with the hardware perception layer, includes a SLAM module and a nonlinear optimization solver. The SLAM module is used to fuse data from lidar and inertial measurement units to construct a high-precision 3D point cloud map in real time and calculate the six-degree-of-freedom pose of the equipment in the global coordinate system. It is also used to downsample and optimize the 3D point cloud map to generate a lightweight point cloud map. The nonlinear optimization solver is used to construct and solve a nonlinear optimization model with multiple physical constraints based on the lightweight point cloud map and the equipment's grounding feature points to obtain the predicted stable pose and the predicted stability margin.

[0011] The intelligent decision-making layer, communicating with the data processing layer, includes a parallel pose prediction and risk map generation module, a center of gravity dynamic perception and overturning warning assessment module, and a safety decision and control execution module. The parallel pose prediction and risk map generation module samples feasible areas ahead to generate a pre-travel position, performs pose prediction in parallel, and integrates the results to generate a risk map. The center of gravity dynamic perception and overturning warning assessment module calculates the vehicle's global center of gravity and stability margin to achieve real-time overturning warning. The safety decision and control execution module integrates the risk map and warning information to output safety control commands.

[0012] Preferably, the SLAM module, through the collaborative work of feature extraction and data association at the front end and pose graph optimization and loop closure detection at the back end, outputs two key results: first, the precise pose of the equipment body in the global coordinate system. Secondly, a high-precision 3D point cloud map of the environment. .

[0013] Preferably, the core parameters of the nonlinear optimization model are configured as follows:

[0014] Optimization variable: Equipment at a given horizontal position The height below Roll angle Pitch angle ;

[0015] Cost function: ,in, For contact and adhesion weight, For non-penetration penalty weights, Suspension suppression weight, To address the contact bonding residual, This is a non-penetration penalty item. This is the term for minimizing the dangling condition;

[0016] Constraints: The equipment must have at least three ground contact points that are in effective contact with the terrain, and the projection point of the vehicle's center of gravity must be located inside the supporting polygon;

[0017] Solution Algorithm: A fast numerical algorithm using a nonlinear optimization solver is employed to predict the stable pose of the equipment. And the prediction stability residual corresponding to this attitude. .

[0018] The The value ranges from 1.0 to 5.0. The value ranges from 10 to 100. The value ranges from 0.1 to 1.0.

[0019] Preferably, the parallel pose prediction and risk map generation module adopts a multi-threaded parallel architecture, with the main thread generating a discrete set of pre-movement positions by sampling the feasible area ahead based on the equipment's kinematic model. The position is dynamically allocated to multiple worker threads; each worker thread independently executes the pose prediction process and outputs the position at each location. optimal pose and stability residual After integration, a forward-looking regional risk map is generated.

[0020] Preferably, the center of gravity dynamic perception and rollover early warning assessment module receives the precise position and orientation of the chassis. and the joint angle of the robotic arm By using the forward kinematics and coordinate transformation of a multi-rigid-body system, the center of gravity of each rigid-body component in the world coordinate system is calculated, and then the global center of gravity of the entire vehicle is calculated according to the formula. The formula is as follows:

[0021] ;

[0022] in, The total mass of the vehicle. For the first The mass of a rigid body The position of the rigid body's center of mass in the world coordinate system is obtained through forward kinematics and coordinate transformation.

[0023] Real-time calculation of the vehicle's global center of gravity The global center of gravity is vertically projected onto the horizontal plane containing the support polygon formed by all grounding points of the chassis, thus obtaining the center of gravity projection point. And calculate the stability margin. The formula is as follows:

[0024] ;

[0025] To support the boundaries of the polygon, To support any point on the polygon boundary, The distance is Euclidean.

[0026] This invention also provides a method for equipment pose prediction and safety monitoring for rescue robots, comprising the following steps:

[0027] S1. Multi-source sensor data acquisition:

[0028] The hardware sensing layer uses LiDAR, inertial measurement unit, and joint encoder to synchronously collect 3D environmental information, equipment motion posture data, and robotic arm joint angle data.

[0029] S2. Environmental Modeling and Self-Localization:

[0030] By fusing the data collected in step S1 through the SLAM module, a high-precision 3D point cloud map is constructed in real time and the six-degree-of-freedom pose of the equipment body is estimated. After lightweight processing by the point cloud optimization unit, a lightweight point cloud map is obtained.

[0031] S3. Stable pose prediction:

[0032] The nonlinear optimization solver extracts a subset of local terrain point clouds at the target location from a lightweight point cloud map, constructs a nonlinear optimization model containing a cost function and constraints, and solves for the predicted stable pose and predicted stability residual of the target location.

[0033] S4. Regional Risk Assessment:

[0034] The parallel pose prediction and risk map generation module adopts a multi-threaded parallel approach, performing the pose prediction process of step S3 on multiple pre-travel positions in the feasible area ahead, and integrating the results to generate a forward-looking area risk map.

[0035] S5. Real-time overturning warning:

[0036] The center of gravity dynamic perception and rollover warning assessment module calculates the global center of gravity of the vehicle and projects it onto the horizontal plane where the supporting polygon is located. It calculates the stability margin and compares it with the safety threshold, triggering the corresponding warning.

[0037] S6. Security Decision Execution:

[0038] The safety decision-making and control execution module integrates risk maps and early warning information, outputs path planning suggestions, attitude adjustment commands, or emergency braking commands, and completes one processing cycle.

[0039] Preferably, the lightweight processing of the point cloud optimization unit is as follows: using three-dimensional voxelization downsampling technology, while preserving the integrity of the terrain geometric features, the number of original point clouds is reduced from 1.38 million points to 180,000 points to generate a lightweight point cloud map.

[0040] Preferably, the extraction method of the local terrain point cloud subset is as follows: based on the equipment geometry, a dynamic spatial window clipping strategy is adopted to extract the local terrain point cloud subset centered on the target location from the lightweight point cloud map.

[0041] Preferably, the safety threshold is determined by measuring the minimum stability margin of the equipment under different operating states through tilt table tests and robotic arm disturbance tests, and taking its 90th percentile value as the safety threshold.

[0042] By adopting the above system and method, the present invention has the following advantages:

[0043] 1. This invention improves terrain perception accuracy by constructing a high-precision 3D point cloud map through tightly coupled LiDAR-IMU SLAM technology. Combined with lightweight point cloud processing, it improves computational efficiency while ensuring the integrity of terrain features, thus solving the problem of inaccurate terrain reconstruction in existing technologies.

[0044] 2. This invention achieves accurate pose prediction. Based on a nonlinear optimization model with multiple physical constraints, it integrates constraints such as contact fit, non-penetration, and suspension suppression to accurately predict the stable attitude of the equipment at the target position, thus solving the defect of weak dynamic coupling modeling.

[0045] 3. This invention adopts real-time regional risk assessment and a multi-threaded parallel architecture to quickly complete the pose prediction of multiple locations in the foreground area and generate a risk map, achieving a breakthrough from single-point prediction to regional assessment and solving the problem of lag in static analysis.

[0046] 4. This invention calculates the global center of gravity through multi-rigid-body kinematics and combines stability margin quantitative assessment of stability to achieve accurate early warning, reduce the risk of overturning, and improve operational safety.

[0047] The above overview is for illustrative purposes only and is not intended to be limiting in any way. In addition to the illustrative aspects, embodiments, and features described above, further aspects, embodiments, and features of the invention will become readily apparent from the accompanying drawings and the following detailed description. Attached Figure Description

[0048] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0049] Figure 1 This is a system overall architecture block diagram of the present invention;

[0050] Figure 2 This is a flowchart of the overall method of the present invention;

[0051] Figure 3 This is a block diagram illustrating the environmental perception and point cloud optimization principle of the present invention;

[0052] Figure 4 This is a flowchart of the stable pose prediction process of the present invention;

[0053] Figure 5 This is a typical equipment chassis grounding feature point definition diagram of the present invention;

[0054] Figure 6 This is a diagram of the parallel regional risk assessment architecture of the present invention;

[0055] Figure 7 This is a schematic diagram of the principle of center of gravity sensing and overturning warning of the present invention;

[0056] Figure 8 This is the point cloud data map before optimization according to the present invention;

[0057] Figure 9 This is the optimized point cloud data of the present invention;

[0058] Figure 10 This is a schematic diagram of the posture contact of the equipment of the present invention on the terrain;

[0059] Figure 11 This is a three-dimensional trajectory curve of the center of gravity (CoM) of the equipment of the present invention during the motion process;

[0060] Figure 12 This is a visualization of the attitude adjustment results of the equipment of the present invention on complex terrain. Detailed Implementation

[0061] Specific embodiments of the invention will now be described in detail. Although the invention is described in conjunction with these specific embodiments, it should be understood that the invention is not intended to be limited to these specific embodiments. Rather, these embodiments are intended to cover alternative, modified, or equivalent embodiments that may be included within the spirit and scope of the invention as defined by the claims. In the following description, numerous specific details are set forth in order to provide a thorough understanding of the invention. The invention may be practiced without some or all of these specific details. In other instances, well-known processes have not been described in detail so as not to unnecessarily obscure the invention.

[0062] When used in conjunction with the terms "comprising," "method comprising," or similar language in this specification and appended claims, the singular forms "a," "some," and "the" include plural references unless the context clearly indicates otherwise. Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0063] The present invention will now be described in further detail with reference to the full text.

[0064] Combined with appendix Figures 1-12 This invention discloses an equipment pose prediction and safety monitoring system and method for rescue robots based on 3D point cloud and nonlinear optimization. It aims to achieve accurate prediction and optimization of equipment pose under complex unstructured terrain conditions by deeply fusing 3D laser point cloud data with equipment structural models, effectively overcoming the problems of low perception accuracy, weak dynamic coupling modeling, and lagging early warning mechanisms in existing technologies. The core objective of this invention is to reconstruct the real operating terrain based on high-precision point cloud data and identify potential equipment-terrain contact relationships, establishing a nonlinear optimization framework for equipment stability and terrain constraints. This allows for the real-time generation of six-degree-of-freedom pose schemes (including 3D position and attitude) that meet safety margins, and integrates a tilt risk early warning mechanism to proactively trigger intervention in potentially unstable states. This method and system effectively overcome over-reliance on operator experience, achieving a technological leap from manual visual judgment to model calculation and proactive early warning, significantly improving the terrain passability, operational stability, and operational safety of emergency rescue equipment in complex and dangerous environments.

[0065] In specific implementations of this invention, such as Figure 1As shown, through a closed-loop technology chain of "perception-modeling-prediction-assessment-early warning," the system enables equipment to leap from passive reaction to proactive anticipation in safety decision-making. The system consists of a hardware perception layer, a data processing layer, and an intelligent decision-making layer, employing a three-layer architecture. The hardware perception layer is responsible for raw data acquisition, including LiDAR (for acquiring 3D environmental information), an inertial measurement unit (for sensing its own motion and attitude), and joint encoders (for reading the joint angles of the robotic arm). The data processing layer is the core computing engine, containing a SLAM (Simultaneous Localization and Mapping) module, responsible for fusing sensor data, constructing a real-time environmental point cloud map, and estimating the equipment's pose; it also includes a nonlinear optimization solver for performing stable pose prediction. The intelligent decision-making layer is the system's brain, containing a parallel pose prediction and risk map generation module, responsible for rapid stability assessment of the area ahead; it also includes a dynamic center of gravity perception and overturning warning assessment module, responsible for overall stability monitoring and risk warning based on the real-time center of gravity position.

[0066] A system for predicting the pose and monitoring the safety of rescue robots, including...

[0067] The hardware perception layer is used to synchronously collect raw data from multiple sources, including lidar, inertial measurement unit and joint encoder; the lidar is used to acquire three-dimensional point cloud data of the environment, the inertial measurement unit is used to collect the equipment's own angular velocity, acceleration and orientation attitude data, and the joint encoder is used to read the joint angle data of the robotic arm.

[0068] The data processing layer, which communicates with the hardware perception layer, includes a SLAM module and a nonlinear optimization solver. The SLAM module is used to fuse data from lidar and inertial measurement units to construct a high-precision 3D point cloud map in real time and calculate the six-degree-of-freedom pose of the equipment in the global coordinate system. It is also used to downsample and optimize the 3D point cloud map to generate a lightweight point cloud map. The nonlinear optimization solver is used to construct and solve a nonlinear optimization model with multiple physical constraints based on the lightweight point cloud map and the equipment's grounding feature points to obtain the predicted stable pose and the predicted stability margin.

[0069] In specific implementations of this invention, such as Figure 3 As shown, the SLAM module, through the collaborative work of feature extraction and data association at the front end and pose graph optimization and loop closure detection at the back end, outputs two key results: first, the precise pose of the equipment body in the global coordinate system. Secondly, a high-precision 3D point cloud map of the environment. .

[0070] Specifically, the raw data from the SLAM module's LiDAR and IMU (Inertial Measurement Unit) are input into the SLAM processing module. Through the collaborative work of its front-end (feature extraction and data association) and back-end (pose map optimization and loop closure detection), the SLAM module outputs two key results: first, the precise pose of the equipment body in the global coordinate system. Secondly, a high-precision 3D point cloud map of the environment. The original point cloud map data is massive (e.g., 1.38 million points), making direct use for subsequent calculations inefficient. Therefore, the system employs a three-dimensional systematic downsampling technique to process the point cloud. Processing is performed to generate a lightweight point cloud map. (e.g., 180,000 points). This technical solution significantly improves the processing speed of subsequent algorithms while ensuring the integrity of terrain geometric features. It enables accurate acquisition of the environmental model and provides a spatial reference for pose prediction.

[0071] The intelligent decision-making layer, communicating with the data processing layer, includes a parallel pose prediction and risk map generation module, a center of gravity dynamic perception and overturning warning assessment module, and a safety decision and control execution module. The parallel pose prediction and risk map generation module samples feasible areas ahead to generate a pre-travel position, performs pose prediction in parallel, and integrates the results to generate a risk map. The center of gravity dynamic perception and overturning warning assessment module calculates the vehicle's global center of gravity and stability margin to achieve real-time overturning warning. The safety decision and control execution module integrates the risk map and warning information to output safety control commands.

[0072] In specific implementations of this invention, such as Figure 4 As shown, the core parameter configuration of the nonlinear optimization model is as follows:

[0073] Optimization variable: Equipment at a given horizontal position The height below Roll angle Pitch angle ;

[0074] Cost function: ,in, For contact and adhesion weight, For non-penetration penalty weights, Suspension suppression weight, To address the contact bonding residual, This is a non-penetration penalty item. This is the term for minimizing the dangling condition;

[0075] Constraints: The equipment must have at least three ground contact points that are in effective contact with the terrain, and the projection point of the vehicle's center of gravity must be located inside the supporting polygon;

[0076] Solution Algorithm: A fast numerical algorithm using a nonlinear optimization solver is employed to predict the stable pose of the equipment. And the prediction stability residual corresponding to this attitude. .

[0077] The The value ranges from 1.0 to 5.0. The value ranges from 10 to 100. The value ranges from 0.1 to 1.0.

[0078] The parallel pose prediction and risk map generation module adopts a multi-threaded parallel architecture. The main thread samples and generates a discrete set of pre-movement positions in the feasible area ahead based on the equipment's kinematic model. The position is dynamically allocated to multiple worker threads; each worker thread independently executes the pose prediction process and outputs the position at each location. optimal pose and stability residual After integration, a forward-looking regional risk map is generated.

[0079] Specifically, the technical solution aims to address the key problem of "predicting the stable attitude of equipment when stationary at a given target horizontal position." This prediction, based on physical models and terrain geometry, provides a direct basis for risk assessment.

[0080] First, the system inputs a target horizontal position. Next, based on the equipment's geometric dimensions, a dynamic spatial window pruning strategy is employed to prune the global lightweight point cloud map. Extract a subset of local terrain point clouds centered on the target point. This subset contains key terrain information that determines the equipment's attitude.

[0081] Then, a nonlinear optimization model for attitude prediction is constructed. The core of this model is to find an equipment attitude that is most compatible with terrain geometry and physical constraints.

[0082] The optimization variable is the equipment at a given horizontal position. Below, the three degrees of freedom in attitude determined by the terrain: height Roll angle Pitch angle These three quantities together describe the feasible attitude space of the equipment in its current planar position. This is the yaw angle.

[0083] Simultaneously, the system internally calls upon pre-set "grounding feature points" on the equipment chassis. These "grounding feature points" refer to the key geometric points on the equipment chassis used to make initial contact with the terrain. Typical engineering machinery equipment (tracked chassis, four-wheeled chassis) has "grounding feature points" such as... Figure 5As shown, these points represent the effective support profile of the equipment and are key reference points for judging contact, suspension, and stability:

[0084] The cost function is designed as a weighted combination of multiple physical constraint terms, and its overall structure is as follows:

[0085] ;

[0086] Each sub-item corresponds to a physical mechanism, with weights... The rules are as follows, but can still be adjusted based on experience or task requirements. The construction logic is as follows:

[0087] 1. Contact bonding residual Minimize the distance between the equipment's "grounding feature point" and the surface of the terrain point cloud, so that the equipment chassis naturally lands on the real terrain. This item ensures "geometric feasibility".

[0088] 2. Non-penetration penalty item : Apply a high penalty to any posture that causes the equipment model to penetrate the terrain, ensuring that the predicted posture conforms to physical constraints. This item ensures "no geometric conflict".

[0089] 3. Minimize the hanging term The feature point that "should be grounded but is suspended" is penalized, making the optimization tend to find a stable posture where all grounding points fall on the support surface. This promotes "multi-point support" and stability.

[0090] 4. Contact Adhesion Weight The sensitivity of the control optimization to the "fit of the equipment chassis to the terrain" is usually set to a relatively large value (such as 1.0–5.0) to ensure accurate geometric contact.

[0091] 5. Non-penetration penalty weight This is used to forcibly prevent equipment models from entering the terrain and is the most stringent constraint. Its weight is typically very large (e.g., 10–100) to ensure the optimizer prioritizes eliminating any non-physical poses.

[0092] 6. Suspension Suppression Weight This is designed to prevent certain grounding feature points from being left floating, making optimization more inclined towards multi-point support. Typical values ​​are small (e.g., 0.1–1.0) for fine-tuning balance.

[0093] The minimum cost function corresponds to the most natural and likely stable pose in a physical sense. The constraints require that the predicted pose must satisfy static stability, that is, at least three ground feature points are in effective contact with the terrain, and the center of gravity projection lies within the supporting polygon.

[0094] After the problem is formulated, it is solved by a nonlinear optimization solver. The solution process essentially involves searching for the physically and geometrically most feasible equipment posture, as detailed below:

[0095] 1. Initialization:

[0096] Input the initial pose and obtain a subset of local terrain point clouds centered on that pose;

[0097] 2. Iterative updates:

[0098] In each step, the solver updates the attitude variables based on the gradient of the cost function and the constraints, gradually approximating a more reasonable attitude.

[0099] 3. Conflict detection and correction:

[0100] During iteration, check in real time whether terrain penetration exists; if so, push the pose away from unreasonable areas by penalizing the gradient.

[0101] 4. Convergence criterion:

[0102] The solver returns the final solution when the change in the cost function is less than the threshold and all mechanical constraints are satisfied.

[0103] Ultimately, the model outputs the predicted stable pose of the equipment at the target location. (Include , , ), and the predicted stability residual corresponding to this attitude. The latter directly quantifies the stability risk at that location.

[0104] To achieve real-time early warning, this invention extends time-consuming single-point pose prediction to efficient regional risk assessment, with the architecture as follows: Figure 6 As shown. The main thread first generates a set of discrete pre-travel positions by sampling within the feasible region in front of the equipment, based on the equipment's kinematic model. Subsequently, the main thread treats these positions as independent optimization tasks and dynamically assigns them to multiple worker threads. Each worker thread internally runs a complete pose prediction process (such as...). Figure 4 (As shown), but it processes the respective assigned positions. The corresponding local terrain. All worker threads perform nonlinear optimization calculations in parallel and synchronously. After all threads have completed their calculations, the system collects data for each location. optimal pose and stability residual Ultimately, this data is integrated to generate a forward-looking regional risk map, which clearly identifies the stability levels of different areas ahead, providing a direct basis for path planning and risk avoidance. This technical solution distributes computationally intensive tasks through parallelization, enabling stability simulation of large-scale scenarios within a very short timeframe.

[0105] In specific implementations of this invention, such as Figure 7 As shown:

[0106] The center of gravity dynamic perception and overturning early warning assessment module monitors the stability of the equipment's current state in real time. This system receives chassis pose data from SLAM / IMU in real time. and the joint angle of the robotic arm from the joint encoder (Robot arm joint angle vector). Through the forward kinematics and coordinate transformation of the multi-rigid-body system, the center of gravity of each rigid body component (including the chassis and each robot arm link) in its own coordinate system is calculated. Then, through coordinate transformation, it is unified to the world coordinate system to obtain... Then, according to the formula:

[0107] ;

[0108] In the formula: As the overall center of gravity of the vehicle, The total mass of the vehicle. For the first The mass of a rigid body The position of the rigid body's center of mass in the world coordinate system is obtained through forward kinematics and coordinate transformation.

[0109] Real-time calculation of the vehicle's global center of gravity Projecting the center of gravity vertically onto the horizontal plane containing the supporting polygon formed by all the chassis's ground contact points, we obtain the center of gravity projection point. .

[0110] ;

[0111] In the formula: To ensure stability margin, To support the boundaries of the polygon, To support any point on the polygon boundary, Let be the projection point of the center of gravity onto the ground. The distance is Euclidean.

[0112] The minimum stability margin of the equipment under different operating conditions was measured at the test site through tilt table tests, robotic arm disturbance tests, etc., and the 90th percentile value was taken as the safety threshold.

[0113] stability margin The projection point is calculated as the distance from the projection point to the boundary of the supporting polygon. The shortest distance. Finally, the stability assessment and early warning decision module continuously monitors... Value: When When the value is greater than the safety threshold, the equipment status is stable; when When the system approaches or falls below the safety threshold, it immediately issues visual or auditory warnings based on the risk level, or directly outputs attitude adjustment commands (such as retracting the robotic arm) to the control system to prevent tipping.

[0114] This invention also provides a method for equipment pose prediction and safety monitoring for rescue robots, comprising the following steps:

[0115] S1. Multi-source sensor data acquisition:

[0116] The hardware sensing layer uses LiDAR, inertial measurement unit, and joint encoder to synchronously collect 3D environmental information, equipment motion posture data, and robotic arm joint angle data.

[0117] S2. Environmental Modeling and Self-Localization:

[0118] By fusing the data collected in step S1 through the SLAM module, a high-precision 3D point cloud map is constructed in real time and the six-degree-of-freedom pose of the equipment body is estimated. After lightweight processing by the point cloud optimization unit, a lightweight point cloud map is obtained.

[0119] S3. Stable pose prediction:

[0120] The nonlinear optimization solver extracts a subset of local terrain point clouds at the target location from a lightweight point cloud map, constructs a nonlinear optimization model containing a cost function and constraints, and solves for the predicted stable pose and predicted stability residual of the target location.

[0121] S4. Regional Risk Assessment:

[0122] The parallel pose prediction and risk map generation module adopts a multi-threaded parallel approach, performing the pose prediction process of step S3 on multiple pre-travel positions in the feasible area ahead, and integrating the results to generate a forward-looking area risk map.

[0123] S5. Real-time overturning warning:

[0124] The center of gravity dynamic perception and rollover warning assessment module calculates the global center of gravity of the vehicle and projects it onto the horizontal plane where the supporting polygon is located. It calculates the stability margin and compares it with the safety threshold, triggering the corresponding warning.

[0125] S6. Security Decision Execution:

[0126] The safety decision-making and control execution module integrates risk maps and early warning information, outputs path planning suggestions, attitude adjustment commands, or emergency braking commands, and completes one processing cycle.

[0127] In specific implementations of this invention, such as Figure 2 As shown, the process of this invention begins with the synchronous acquisition of multi-source sensor data. Subsequently, it proceeds to the SLAM online mapping and self-localization step, fusing LiDAR and IMU data to construct a high-precision 3D environmental point cloud map in real time, and simultaneously calculating the six-degree-of-freedom pose of the equipment body in the global coordinate system. To improve subsequent processing efficiency, the generated point cloud undergoes 3D point cloud downsampling processing to reduce the data volume while retaining key terrain features. The above steps provide input for two core functions performed in parallel: firstly, parallel pose prediction and risk assessment, which, based on the current environmental map, rapidly predicts and scores the stable pose of multiple potential locations in front of the equipment, generating a risk map; secondly, real-time dynamic center of gravity calculation, real-time overturning warning, and attitude optimization, which, based on the equipment's global attitude and robotic arm configuration, calculates the vehicle's center of gravity position in real time and assesses the stability of the current attitude based on the supporting polygon theory, issuing an immediate warning upon detecting a risk. Finally, the safety decision and control execution module integrates the forward risk map and current attitude warning information to output the final safety command (such as path planning suggestions, attitude adjustment commands, or emergency braking), completing one processing cycle.

[0128] This invention addresses the perception blind spots and decision-making delays faced by heavy-duty equipment in unstructured environments. It proposes a pose prediction and safety early warning scheme based on 3D point clouds and nonlinear optimization. By constructing an accurate environmental model using SLAM technology and combining it with a nonlinear optimization method based on multi-physics constraints, the stable attitude of the equipment at the target location is predicted. This achieves a paradigm shift from traditional perception-response to prediction-decision, enabling the equipment to proactively anticipate terrain risks ahead.

[0129] To address the shortcomings of existing methods, such as the lack of quantitative stability assessment indicators and the difficulty in achieving accurate safety early warnings, this invention innovatively proposes a quantitative stability margin assessment system based on supporting polygon theory. By calculating the vehicle's center of gravity position in real time and its relationship with the stability domain boundary, it transforms stability judgments that previously relied on operational experience into precise data-driven decision-making. Combined with a parallel computing architecture for rapid risk mapping of the prospective region, this represents a technological breakthrough from single-point early warning to regional awareness, providing a novel technical guarantee for the intelligent operation safety of heavy-duty equipment under extreme conditions.

[0130] The pose prediction and safety early warning method of this invention effectively improves the autonomous decision-making ability and operational safety of equipment in complex environments, and provides an innovative technical path for the intelligent development of heavy equipment.

[0131] Example 1:

[0132] like Figure 8 and Figure 9 As shown, the implementation of environmental perception and point cloud optimization follows a specific process:

[0133] Environmental point cloud acquisition: The equipment is equipped with a lidar system and uses a SLAM module to generate the original point cloud of the surrounding 3D environment in real time. For example... Figure 8 As shown in the figure, the initial point cloud suffers from problems such as noise, high dispersion, and blurred feature structures. The rescue robot is equipped with a LiDAR and uses a SLAM system to generate the original point cloud of the surrounding 3D environment in real time. The initial point cloud has problems such as noise, high dispersion, and blurred feature structures, with a point cloud of 1.38 million points, and the number of points continues to increase over time.

[0134] Point cloud optimization and geometric reconstruction: A 3D voxelization downsampling technique is used to lightweight the original point cloud, significantly improving subsequent computational efficiency while maintaining feature integrity. Figure 8 As shown in the figure, the original point cloud contains 1.38 million points, and this number continues to increase significantly over time. The downsampled point cloud obtained through 3D voxelization downsampling technology is shown below. Figure 9 As shown in the figure, the number of point clouds is only 180,000, and it remains basically unchanged over time.

[0135] The optimized point cloud exhibits more uniform density, significantly reduced noise points, and clearer terrain feature outlines. Inputting the optimized point cloud into the subsequent attitude prediction module improves computational efficiency by 10 times, and reduces contact determination error from ±0.15m to ±0.03m, providing a reliable basis for equipment stability prediction. The differences between the optimized and unoptimized point clouds are clearly visible; the optimized point cloud has more uniform density and significantly reduced noise points, making it more suitable for subsequent attitude prediction, contact determination, and path planning modules.

[0136] Therefore, this embodiment can achieve high-precision modeling of complex terrain and perform efficient and distortion-free processing, providing a reliable basis for equipment stability prediction.

[0137] Example 2:

[0138] like Figure 10-12 As shown, the equipment model and center of gravity calibration are performed by calculating the static center of gravity based on the equipment's three-dimensional structural model and updating the center of gravity position in real time according to joint angles, component movements, etc.

[0139] Center of gravity projection calculation and trajectory generation: During the movement of the equipment, the instantaneous center of gravity coordinates are projected onto the ground to obtain the trajectory of the center of gravity projection point; Figure 11 The three-dimensional curve shown is the trajectory of the continuous change of the center of gravity during the motion.

[0140] Real-time attitude: The real-time pose state during overall machine attitude adjustment.

[0141] The implementation results show that the center of gravity trajectory is smooth and can continuously reflect the dynamic stability changes of the equipment on uneven ground; it helps to identify the risk of overturning in advance and select a safer movement posture.

[0142] The present invention and its embodiments have been described above. This description is not restrictive, and the embodiments shown throughout are only one of the embodiments of the present invention. The actual structure is not limited to this. In conclusion, if those skilled in the art are inspired by this description and design similar structures and embodiments without departing from the spirit of the present invention, they should all fall within the protection scope of the present invention.

Claims

1. A system for predicting the pose and monitoring the safety of rescue robots, characterized in that, include: The hardware perception layer is used to synchronously collect raw data from multiple sources, including lidar, inertial measurement unit and joint encoder; the lidar is used to acquire three-dimensional point cloud data of the environment, the inertial measurement unit is used to collect the equipment's own angular velocity, acceleration and orientation attitude data, and the joint encoder is used to read the joint angle data of the robotic arm. The data processing layer, which communicates with the hardware perception layer, includes a SLAM module and a nonlinear optimization solver. The SLAM module is used to fuse data from lidar and inertial measurement units to construct a high-precision 3D point cloud map in real time and calculate the six-degree-of-freedom pose of the equipment in the global coordinate system. It is also used to downsample and optimize the 3D point cloud map to generate a lightweight point cloud map. The nonlinear optimization solver is used to construct and solve a nonlinear optimization model with multiple physical constraints based on the lightweight point cloud map and the equipment's grounding feature points to obtain the predicted stable pose and the predicted stability margin. The intelligent decision-making layer, communicating with the data processing layer, includes a parallel pose prediction and risk map generation module, a center of gravity dynamic perception and overturning warning assessment module, and a safety decision and control execution module. The parallel pose prediction and risk map generation module samples feasible areas ahead to generate a pre-travel position, performs pose prediction in parallel, and integrates the results to generate a risk map. The center of gravity dynamic perception and overturning warning assessment module calculates the vehicle's global center of gravity and stability margin to achieve real-time overturning warning. The safety decision and control execution module integrates the risk map and warning information to output safety control commands.

2. The equipment pose prediction and safety monitoring system for rescue robots according to claim 1, characterized in that: The SLAM module, through the collaborative work of feature extraction and data association at the front end and pose graph optimization and loop closure detection at the back end, outputs two key results: first, the precise pose of the equipment body in the global coordinate system. Secondly, a high-precision 3D point cloud map of the environment. .

3. The equipment pose prediction and safety monitoring system for rescue robots according to claim 1, characterized in that: The core parameter configurations of the nonlinear optimization model are as follows: Optimization variable: Equipment at a given horizontal position The height below Roll angle Pitch angle ; Cost function: ,in, For contact and adhesion weight, For non-penetration penalty weights, Suspension suppression weight, To address the contact bonding residual, This is a non-penetration penalty item. This is the term for minimizing the dangling condition; Constraints: The equipment must have at least three ground contact points that are in effective contact with the terrain, and the projection point of the vehicle's center of gravity must be located inside the supporting polygon; Solution Algorithm: A fast numerical algorithm using a nonlinear optimization solver is employed to predict the stable pose of the equipment. and the prediction stability residual corresponding to this attitude. .

4. The equipment pose prediction and safety monitoring system for rescue robots according to claim 3, characterized in that: The The value ranges from 1.0 to 5.

0. The value ranges from 10 to 100. The value ranges from 0.1 to 1.

0.

5. The equipment pose prediction and safety monitoring system for rescue robots according to claim 2, characterized in that: The parallel pose prediction and risk map generation module adopts a multi-threaded parallel architecture. The main thread samples and generates a discrete set of pre-movement positions in the feasible area ahead based on the equipment's kinematic model. The position is dynamically allocated to multiple worker threads; each worker thread independently executes the pose prediction process and outputs the position at each location. optimal pose and stability residual After integration, a forward-looking regional risk map is generated.

6. The equipment pose prediction and safety monitoring system for rescue robots according to claim 2, characterized in that: The center of gravity dynamic perception and rollover early warning assessment module receives the precise position and orientation of the chassis. and the joint angle of the robotic arm By using the forward kinematics and coordinate transformation of a multi-rigid-body system, the center of gravity of each rigid-body component in the world coordinate system is calculated, and then the global center of gravity of the entire vehicle is calculated according to the formula. The formula is as follows: ； in, The total mass of the vehicle. For the first The mass of a rigid body The position of the rigid body's center of mass in the world coordinate system is obtained through forward kinematics and coordinate transformation. Real-time calculation of the vehicle's global center of gravity The global center of gravity is vertically projected onto the horizontal plane containing the support polygon formed by all grounding points of the chassis, thus obtaining the center of gravity projection point. And calculate the stability margin. The formula is as follows: ； To support the boundaries of the polygon, To support any point on the polygon boundary, The distance is Euclidean.

7. A method for equipment pose prediction and safety monitoring of rescue robots, characterized in that, Includes the following steps: S1. Multi-source sensor data acquisition: The hardware sensing layer uses LiDAR, inertial measurement unit, and joint encoder to synchronously collect 3D environmental information, equipment motion posture data, and robotic arm joint angle data. S2. Environmental Modeling and Self-Localization: By fusing the data collected in step S1 through the SLAM module, a high-precision 3D point cloud map is constructed in real time and the six-degree-of-freedom pose of the equipment body is estimated. After lightweight processing by the point cloud optimization unit, a lightweight point cloud map is obtained. S3. Stable pose prediction: The nonlinear optimization solver extracts a subset of local terrain point clouds at the target location from a lightweight point cloud map, constructs a nonlinear optimization model containing a cost function and constraints, and solves for the predicted stable pose and predicted stability residual of the target location. S4. Regional Risk Assessment: The parallel pose prediction and risk map generation module adopts a multi-threaded parallel approach, performing the pose prediction process of step S3 on multiple pre-travel positions in the feasible area ahead, and integrating the results to generate a forward-looking area risk map. S5. Real-time overturning warning: The center of gravity dynamic perception and rollover warning assessment module calculates the global center of gravity of the vehicle and projects it onto the horizontal plane where the supporting polygon is located. It calculates the stability margin and compares it with the safety threshold, triggering the corresponding warning. S6. Security Decision Execution: The safety decision-making and control execution module integrates risk maps and early warning information, outputs path planning suggestions, attitude adjustment commands, or emergency braking commands, and completes one processing cycle.

8. The method for equipment pose prediction and safety monitoring for rescue robots according to claim 7, characterized in that: The lightweight processing of the point cloud optimization unit is as follows: using three-dimensional voxelization downsampling technology, while preserving the integrity of terrain geometric features, the number of original point clouds is reduced from 1.38 million to 180,000, generating a lightweight point cloud map.

9. The method for equipment pose prediction and safety monitoring for rescue robots according to claim 7, characterized in that: The extraction method for the local terrain point cloud subset is as follows: based on the equipment geometry, a dynamic spatial window clipping strategy is adopted to extract the local terrain point cloud subset centered on the target location from the lightweight point cloud map.

10. The method for equipment pose prediction and safety monitoring for rescue robots according to claim 7, characterized in that: The safety threshold is determined by measuring the minimum stability margin of the equipment under different operating conditions through tilt table tests and robotic arm disturbance tests, and taking its 90th percentile value as the safety threshold.

Images