CN-121977535-A - Four-foot robot positioning and mapping method, device, terminal and medium under complex terrain environment

CN121977535ACN 121977535 ACN121977535 ACN 121977535ACN-121977535-A

Abstract

The invention discloses a method, a device, a terminal and a medium for positioning and mapping a quadruped robot in a complex terrain environment, wherein the method comprises the steps of acquiring laser radar point cloud, inertial measurement data and leg motor data of the quadruped robot, constructing a state estimation observation model of the quadruped robot, and realizing estimation of the motion state of the quadruped robot to obtain a state estimation result; and based on a state estimation result, the point cloud motion compensation, the point cloud registration and the point cloud state estimation based on the state estimation result, the compensated laser radar point cloud and the global map of the self-adaptive voxel structure, outputting a three-dimensional voxel map, and taking the three-dimensional voxel map as a constraint updating foot elevation based on a confidence level perceived plane constraint foot elevation mechanism to optimize the subsequent state estimation flow of the quadruped robot. The invention can effectively solve the problems of positioning drift, matching loss, map construction blurring, registration failure and the like caused by the flexible motion state of the quadruped robot under complex terrain.

Inventors

ZHANG XING
ZHAO XINGE
LI QINGQUAN

Assignees

深圳大学

Dates

Publication Date: 20260505
Application Date: 20260403

Claims (10)

1. The method for positioning and mapping the quadruped robot in the complex terrain environment is characterized by comprising the following steps: Acquiring laser radar point cloud, inertial measurement data and leg motor data of a quadruped robot, constructing a state estimation observation model of the quadruped robot based on the inertial measurement data and the leg motor data, and realizing estimation of the motion state of the quadruped robot based on the state estimation observation model to obtain a state estimation result, wherein the leg motor data comprises joint angle data and foot contact detection data, and the state estimation observation model comprises a foot position observation equation, a foot speed observation equation and a rotational drift correction observation equation of the inertial measurement data; compensating point cloud motion based on the state estimation result to obtain compensated laser radar point cloud, and based on the state estimation result, the compensated laser radar point cloud and a global map of the self-adaptive voxel structure, realizing point cloud registration and point cloud state estimation, and outputting a three-dimensional voxel map; and taking the three-dimensional voxel map as a constraint updating foot elevation based on a plane constraint foot updating mechanism of confidence perception so as to optimize a subsequent state estimation flow of the quadruped robot.
2. The method for positioning and mapping a quadruped robot in a complex terrain environment of claim 1, further comprising: Connecting a laser radar with a quadruped robot through a customized structural member, wherein a joint motor and an inertia measurement unit are arranged in the quadruped robot; the coordinate system of the inertial measurement unit is used as a machine body reference system of the quadruped robot, the coordinate system of the first frame of inertial measurement unit is used as a global coordinate system, the relative position relation between the machine body reference system and the laser radar coordinate system is determined, and an external parameter transformation matrix between the machine body reference system and the laser radar coordinate system is determined.
3. The method for positioning and mapping a quadruped robot in a complex terrain environment of claim 2, further comprising: and carrying out time soft synchronization on the laser radar point cloud, the inertial measurement data and the leg motor data of the quadruped robot, and ensuring the consistency of the multi-source data in the time dimension.
4. The method for positioning and mapping a quadruped robot in a complex terrain environment according to claim 3, wherein constructing a state estimation observation model of the quadruped robot based on the inertial measurement data and the leg motor data, and realizing estimation of a motion state of the quadruped robot based on the state estimation observation model, and obtaining a state estimation result comprises: defining a motion state vector of the four-foot robot, constructing a forward kinematics model for a single leg of the four-foot robot, and solving the position of a foot end in a body reference frame of the four-foot robot through joint angle data and leg connecting rod length; Respectively establishing a foot end position observation equation and a foot end speed observation equation based on the inertial measurement data and the leg motor data by combining gait perception; Constructing a rotational drift correction observation equation based on the inertial measurement data; Binding the foot end position observation equation, the foot end speed observation equation and the rotation drift correction observation equation with defined motion state vectors to obtain the state estimation observation model; Based on the state estimation observation model, the inertial measurement data and the leg motor data are fused to realize the estimation of the motion state of the quadruped robot, so as to obtain a state estimation result, wherein the state estimation result reflects the body core state parameters of the quadruped robot under a global coordinate system, and the body core state parameters comprise a position, a speed, a transformation matrix from a body reference system to the global coordinate system, a rotation matrix from the body reference system to the global coordinate system, an offset correction value of the inertial measurement data and a gravity vector estimation value.
5. The method for positioning and mapping a quadruped robot in a complex terrain environment according to claim 4, wherein the compensating the point cloud based on the state estimation result to obtain the compensated laser radar point cloud comprises: And based on the state estimation result, combining the external parameter transformation matrix, uniformly aligning the laser radar point clouds acquired at different moments in the same scanning period to a radar reference coordinate system at the scanning ending moment, and realizing point cloud motion compensation to obtain the compensated laser radar point clouds.
6. The method for positioning and mapping a quadruped robot in a complex terrain environment according to claim 5, wherein based on the state estimation result, the compensated lidar point cloud and the global map of the adaptive voxel structure, the method for realizing point cloud registration and point cloud state estimation and outputting a three-dimensional voxel map comprises: fusing the prior distribution of the state estimation result with likelihood items constructed by laser radar scanning, and integrating the body sensing and the exogenous sensing information into a probability frame updated sequentially; transforming the compensated laser radar point cloud to a global coordinate system, and constructing a registration observation model by combining a global map of the self-adaptive voxel structure and taking the distance from the point to the plane as a residual error item; optimizing residual terms through a coarse-to-fine hierarchical alignment strategy, and completing point cloud registration to obtain registered laser radar point clouds; Updating a state estimation value of the laser radar based on the registered laser radar point cloud, merging the registered laser radar point cloud into a global map of the self-adaptive voxel structure, and outputting a three-dimensional voxel map.
7. The method for positioning and mapping a quadruped robot in a complex terrain environment according to claim 1, wherein the plane constraint foot-end updating mechanism based on confidence perception updates foot-end elevation by using the three-dimensional voxel map as a constraint, so as to optimize a subsequent state estimation flow of the quadruped robot, and the method comprises the following steps: based on confidence perception, uncertainty modeling is carried out on the foot end position, error covariance of the foot end position under a global coordinate system is solved, and reliability of foot end observation is quantized; Searching a root voxel where the foot end position is and a local plane in a sub voxel by using the three-dimensional voxel map, calculating uncertainty of the distance from the foot end to the plane, completing plane consistency check through a self-adaptive threshold, and screening an effective plane; Performing projection calculation on an effective plane on the foot end to finish physical feasibility constraint inspection; Updating foot elevation based on the plane projection points passing all the tests, and correcting elevation drift caused by abnormal movement to optimize the subsequent state estimation flow of the quadruped robot.
8. A four-foot robot positioning and mapping system in a complex terrain environment, wherein the system is configured to implement the steps of the four-foot robot positioning and mapping method in a complex terrain environment of any of claims 1-7, the system comprising: The motion state estimation module is used for acquiring laser radar point cloud, inertial measurement data and leg motor data of the quadruped robot, constructing a state estimation observation model of the quadruped robot based on the inertial measurement data and the leg motor data, and realizing estimation of the motion state of the quadruped robot based on the state estimation observation model to obtain a state estimation result, wherein the leg motor data comprises joint angle data and foot contact detection data, and the state estimation observation model comprises a foot position observation equation, a foot speed observation equation and a rotational drift correction observation equation of the inertial measurement data; The point cloud compensation and registration module is used for carrying out point cloud motion compensation based on the state estimation result to obtain a compensated laser radar point cloud, and carrying out point cloud registration and point cloud state estimation based on the state estimation result, the compensated laser radar point cloud and a global map of the self-adaptive voxel structure to output a three-dimensional voxel map; the foot end updating module is used for updating foot end elevation by taking the three-dimensional voxel map as a constraint based on a plane constraint foot end updating mechanism of confidence perception so as to optimize the subsequent state estimation flow of the quadruped robot.
9. A terminal comprising a memory, a processor and a four-foot robot positioning and mapping program stored in the memory and operable on the processor, wherein the processor performs the steps of the four-foot robot positioning and mapping method in a complex terrain environment as claimed in any one of claims 1 to 7 when executing the four-foot robot positioning and mapping program in a complex terrain environment.
10. A computer readable storage medium, wherein the computer readable storage medium stores a four-foot robot positioning and mapping program under a complex terrain environment, and the four-foot robot positioning and mapping program under the complex terrain environment implements the steps of the four-foot robot positioning and mapping method under the complex terrain environment according to any one of claims 1-7 on the computer readable storage medium.

Description

Four-foot robot positioning and mapping method, device, terminal and medium under complex terrain environment Technical Field The invention relates to the technical field of measuring robots, in particular to a method, a device, a terminal and a medium for positioning and mapping a quadruped robot in a complex terrain environment. Background The acquisition and expression of three-dimensional space information plays a key role in the fields of infrastructure construction and operation monitoring, disaster early warning and emergency response, resource exploration and the like. The above application scenario is often in complex and high-risk environments, such as steep mountain areas, underground spaces (e.g., tunnels, pipe galleries, transportation hubs, etc.), mine areas, dense forests, post-disaster buildings, industrial plants, etc. When the manual field operation is carried out in such an environment, various security threats such as geological disasters (such as landslide, collapse, rock burst and the like), harmful gases, radiation, limited terrains and the like can be faced, and meanwhile, the efficiency and the environment adaptability of traditional manual mapping are obviously restricted. The superposition of the safety and operation constraint factors highlights the urgent need for developing an unmanned three-dimensional mapping technology with high reliability and safety guarantee capability in a complex high-risk scene. The instant localization and mapping (SLAM, simultaneous Localization AND MAPPING) technique enables the mobile platform to complete its pose estimation and environment map construction in the absence of prior environmental information. The current SLAM system is mainly carried on platforms such as unmanned aerial vehicles, wheeled or crawler-type unmanned ground vehicles and the like. The platform forms a mature control strategy and operation stability in a structured environment, but the maneuverability of the platform is obviously limited under the condition of a closed space or complex terrain. For example, unmanned aerial vehicles face problems such as space constraints, navigation safety risks, insufficient cruising ability and the like in limited spaces such as indoors or underground, while wheeled or crawler robots have limited throughput in stairs, ruins and rugged terrains. The mobility constraint limits the application range of the system in disaster relief and complex scene mapping tasks, so that a substitute mobile platform with stronger obstacle crossing capability and complex terrain adaptability is needed to meet the operation requirements under the environments of unstructured, dense obstacle and obvious terrain fluctuation. The quadruped robot benefits from the bionic gait structural design, has remarkable advantages in the aspects of terrain adaptability and autonomous mobility, and can realize stable walking in rugged, limited and unstructured environments, so that the operation boundary of the mapping system under extreme conditions is expanded. The method has strong obstacle crossing capability and endurance capability, so that the method has good application potential in three-dimensional mapping tasks in high-risk environments. However, achieving the positioning and mapping of a quadruped robot in a complex environment still faces technical challenges. For example, four-legged robots have various movement patterns, frequent gait switching is required during operation, foot contact is unstable, and slipping or unbalanced wrestling is liable to occur. This can cause problems such as data distortion and state estimation drift accumulation of the mounted sensor, and finally affect the stability and accuracy of the conventional SLAM system. Even in the dedicated SLAM framework for four-legged robots, state estimation instability is one of the key issues. The root of the method is that the inherent constraint of gait is underutilized, and the fusion of the body perception and the exogenous information is insufficient. In existing systems, foot end position is typically estimated based on a robot kinematic model and ground support conditions are determined using contact sensors. However, most fusion strategies treat each leg foot measurement as being independent of each other in time and space, ignoring inherent motion constraint characteristics in foot-type motions, such as simultaneous support of multiple feet and quasi-static characteristics of the foot end at approximately rest during the touchdown to touchdown phase. Resulting in insufficient modeling and utilization of the space-time consistency and gait-related constraints of the leg system. In addition, many systems use a factor graph optimization framework to fuse motion with sensed data to enhance robustness in the event of foot slip, sensor degradation, or severe vibration of the body, etc. However, the leg odometer factor and the external perception factors such as LiDAR (a system integrating three