CN-121994244-A - Autonomous stair climbing navigation system and method based on four-wheel foot robot

CN121994244ACN 121994244 ACN121994244 ACN 121994244ACN-121994244-A

Abstract

The invention discloses an autonomous stair climbing navigation system and method based on a four-wheel foot robot, which belong to the field of autonomous navigation and motion control of robots, and comprise the steps of obtaining laser radar data to construct a probability voxel map, and eliminating the influence of dynamic obstacles by updating occupation probability; searching step-by-step expansion nodes on a map, eliminating nodes violating vertical increment and safety height constraint to generate candidate paths, performing parameterization fitting on the candidate paths, performing screening and optimization based on a multi-element cost function to obtain an optimal track, analyzing a stair climbing state, calculating horizontal motion compensation quantity containing vertical deviation and speed included angle information, superposing the horizontal motion compensation quantity to a plane motion instruction, and performing closed-loop adjustment by utilizing tightly-coupled pose information. The invention adopts a light constraint search and motion coupling compensation strategy, can generate smooth and feasible tracks in a complex stair environment, and improves the terrain adaptability and autonomous navigation precision of the four-wheel foot robot.

Inventors

PAN HONGSHENG
XU YIHANG
LI KEPIN

Assignees

辛顿人工智能科技有限公司

Dates

Publication Date: 20260508
Application Date: 20260209

Claims (9)

1. An autonomous stair climbing navigation system based on a four-wheeled foot robot, the system comprising: the probability voxel map construction module is used for acquiring laser radar point cloud data and current pose of the four-wheeled foot robot, reducing occupation probability of laser passing through voxels and improving occupation probability of terminal position voxels under a world coordinate system, and constructing a probability voxel map; the constraint candidate path generation module is used for searching step expansion nodes on the probability voxel map, comparing the vertical height increment with a climbing capacity threshold value of a joint and eliminating overrun nodes in the node expansion process, comparing the gravity alignment direction distance with a safety height interval and eliminating out-of-range nodes to generate candidate paths; The track optimization module is used for performing track parameterization on the candidate paths and fitting by using a quintic polynomial, generating track control points at intervals of time and assigning speed vectors and motion time, calculating smoothness, motion feasibility, obstacle avoidance distance and climbing limit to construct a cost function, and performing screening and nonlinear optimization to obtain an optimized stair climbing track; The horizontal motion compensation module is used for analyzing the gesture direction of the four-wheel foot robot and the vertical axial change trend of the optimized stair climbing track, calculating the vertical deviation between the current control point and the position of the robot and the included angle between the speed vector and the gravity vector to generate a horizontal motion compensation amount, and superposing the horizontal motion compensation amount to the plane motion controller to output a plane motion instruction; And the pose tight coupling closed-loop adjustment module is used for acquiring real-time pose information by utilizing the tight coupling inertial measurement unit and the laser radar, executing feedback closed-loop coupling adjustment on the plane motion instruction based on the optimized stair climbing track and the real-time pose information, and outputting an autonomous stair climbing driving instruction.
2. The four-wheeled foot robot-based autonomous stair climbing navigation system of claim 1, wherein the probabilistic voxel map building module comprises: The point cloud de-distortion and clipping unit is used for receiving the laser radar point cloud data, performing de-distortion processing and space clipping, and locking the perception range of the four-wheel foot robot; And the voxel occupation probability updating unit is used for executing the value decreasing updating of the occupation probability on the path voxels penetrated by the laser and executing the value increasing updating of the occupation probability on the laser terminal position voxels in the perception range to obtain the probability voxel map.
3. The four-wheeled foot robot-based autonomous stair climbing navigation system of claim 1, wherein the constrained candidate path generation module comprises: The node expansion unit is used for executing path node iteration on the probability voxel map in a search step mode to generate a first node to be detected; The vertical increment constraint unit is used for calculating the vertical height increment of the first node to be detected, eliminating the node of which the vertical height increment is larger than the climbing capacity threshold of the maximum joint travel of the four-wheel-foot robot, and generating a second node to be detected; And the safety height constraint unit is used for calculating the gravity alignment direction distance from the second node to be detected to the surface of the shortest obstacle, eliminating nodes of which the gravity alignment direction distance is not in a safety height interval of the foot falling space of the four-wheel foot robot, and generating a candidate path.
4. The four-wheeled foot robot-based autonomous stair climbing navigation system of claim 1, wherein the trajectory optimization module comprises: The track parameterization generating unit is used for executing fitting on the candidate paths through a penta polynomial, executing discretization processing by utilizing a time interval, endowing a speed vector and a motion time to the track control points, and constructing a space-time track control point sequence; And the multi-element cost optimizing unit is used for calculating a smoothness item, a motion feasibility item, an obstacle avoidance distance item and a height climbing limit item to construct a multi-element cost function, and performing screening and nonlinear optimization on the space-time track control point sequence through the multi-element cost function to output an optimized stair climbing track.
5. The four-wheeled foot robot-based autonomous stair climbing navigation system of claim 4, wherein the constructing the multiple cost function includes: And calculating a weighted proportion factor for the smoothness item, the motion feasibility item, the obstacle avoidance distance item and the height climbing limit item, and executing weighted multiplication and linear accumulation operation to construct a multi-element cost function.
6. The four-wheeled foot robot-based autonomous stair climbing navigation system of claim 1, wherein the horizontal motion compensation module comprises: The stair climbing state identification unit is used for analyzing the gesture direction of the four-wheel foot robot and the change trend of the optimized stair climbing track in the vertical axial direction and generating a stair climbing state activation instruction; The compensation parameter calculation unit is used for responding to the stair climbing state activation instruction, calculating the vertical deviation between the current control point and the position of the robot and the included angle between the speed vector of the current control point and the vertical gravity vector, and executing operation by combining with a preset compensation gain factor to generate a horizontal motion compensation quantity; And the command superposition coupling unit is used for mapping and superposing the horizontal motion compensation quantity to a plane speed command of the plane motion controller and outputting the plane motion command.
7. The four-wheel-foot robot-based autonomous stair climbing navigation system of claim 6, wherein the generating the horizontal motion compensation amount comprises: and calculating an included angle between the speed vector and the vertical gravity vector to obtain a tangent value, and performing continuous multiplication operation through the tangent value, the vertical deviation and the compensation gain factor to output horizontal motion compensation quantity.
8. The four-wheeled foot robot-based autonomous stair climbing navigation system of claim 1, wherein the pose tight coupling closed loop adjustment module comprises: The real-time pose acquisition unit is used for acquiring sensing data by utilizing the inertial measurement unit and the laser radar, performing multi-sensor tight coupling calculation and outputting real-time pose information; And the closed-loop coupling driving unit is used for mapping the real-time pose information to the optimized stair climbing track to calculate deviation, executing feedback closed-loop coupling adjustment on the plane motion instruction and outputting an autonomous stair climbing driving instruction.
9. An autonomous stair climbing navigation method based on a four-wheel foot robot, applied to the autonomous stair climbing navigation system based on the four-wheel foot robot as set forth in any one of claims 1-8, the method comprising: acquiring laser radar point cloud data and current pose of the four-wheeled foot robot, reducing occupation probability of laser passing through voxels and improving occupation probability of terminal position voxels under a world coordinate system, and constructing a probability voxel map; Searching step expansion nodes on the probability voxel map, comparing the vertical height increment with a climbing capacity threshold value of a joint and eliminating overrun nodes in the node expansion process, comparing the gravity alignment direction distance with a safety height interval and eliminating out-of-range nodes to generate candidate paths; performing track parameterization on the candidate paths, fitting by using a penta polynomial, generating track control points at intervals of time, assigning a speed vector and a movement time, calculating smoothness, movement feasibility, obstacle avoidance distance and climbing limit to construct a cost function, and performing screening and nonlinear optimization to obtain an optimized stair climbing track; Analyzing the gesture direction of the four-wheel foot robot and the vertical axial change trend of the optimized stair climbing track, calculating the vertical deviation between the current control point and the position of the robot and the included angle between the speed vector and the gravity vector to generate a horizontal motion compensation amount, and superposing the horizontal motion compensation amount to a plane motion controller to output a plane motion instruction; And acquiring real-time pose information by utilizing a tightly coupled inertial measurement unit and a laser radar, performing feedback closed-loop coupling adjustment on the plane motion instruction based on the optimized stair climbing track and the real-time pose information, and outputting an autonomous stair climbing driving instruction.

Description

Autonomous stair climbing navigation system and method based on four-wheel foot robot Technical Field The invention relates to the field of autonomous navigation and motion control of robots, in particular to an autonomous stair climbing navigation system and method based on a four-wheel foot robot. Background The four-wheel foot robot has great application potential in the fields of urban search and rescue, industrial inspection, complex terrain exploration and the like. Compared with the traditional wheeled or crawler type robot, the four-wheel foot robot can adapt to a more complex unstructured environment by virtue of the unique motion structure. However, autonomous navigation and motion control of four-wheeled foot robots face more serious challenges than flat terrain when faced with complex structural environments with periodic elevation difference features, including stairs, etc., and particularly in continuous climbing tasks in multi-story building environments, place higher demands on environmental adaptability and control accuracy of the navigation system. The prior art has significant drawbacks in dealing with the above-described scenarios. Firstly, the traditional fully autonomous navigation algorithm generally relies on high-precision real-time map construction and optimization, has huge demand on computing resources, and is difficult to realize efficient deployment on a four-wheel-foot robot platform with limited resources. Secondly, the existing path planner is designed for a plane or an air environment, and when the stepped vertical height difference is processed, a feasible path meeting the requirements of the maximum climbing capacity constraint and gait stability of the robot is difficult to generate, and planning failure or movement instability is easy to cause. In addition, existing control systems often lack a mechanism for effectively coupling vertical displacement in a three-dimensional climbing trajectory to a bottom plane motion instruction, resulting in lower trajectory tracking accuracy during stair climbing, and difficulty in achieving smooth and accurate autonomous navigation. Disclosure of Invention In order to solve the problems, the invention provides an autonomous stair climbing navigation system and an autonomous stair climbing navigation method based on a four-wheel foot robot, which adopt a lightweight constraint search and motion coupling compensation strategy, can generate smooth and feasible tracks in a complex stair environment, and improve the terrain adaptability and autonomous navigation precision of the four-wheel foot robot. The above object can be achieved by the following scheme: An autonomous stair climbing navigation system based on a four-wheeled foot robot comprises a probability voxel map construction module, a constraint candidate path generation module, a track optimization module, a horizontal motion compensation module, a motion control module and a motion control module, wherein the probability voxel map construction module is used for acquiring laser radar point cloud data and current pose of the four-wheeled foot robot, reducing occupation probability of laser passing through voxels and improving occupation probability of terminal position voxels under a world coordinate system, constructing a probability voxel map, the constraint candidate path generation module is used for searching a stepping expansion node on the probability voxel map, comparing vertical height increment with a climbing capacity threshold of a joint and eliminating out-of-limit nodes in the node expansion process, comparing a gravity alignment direction distance with a safety height interval and eliminating out-of-limit nodes to generate a candidate path, the track optimization module is used for performing track parameterization on the candidate path, generating a track control point according to time intervals and assigning a speed vector and a motion time, calculating smoothness, motion feasibility, obstacle avoidance distance and climbing limit to construct a cost function, performing screening and nonlinear optimization, analyzing vertical axial change of the stair climbing trajectory of the four-wheeled foot robot, calculating a position control point and a motion control unit, performing motion control unit and a motion control unit, coupling the vertical motion control point with a motion control plane to a closed-loop motion control plane in real-time direction, and a closed-loop motion control unit to a motion control plane, and a motion control unit, and a motion control plane is coupled with the motion control unit in real-time direction, and a closed-loop motion control unit based on a motion control unit, and a real-time motion control instruction, and a closed-loop motion control unit, and a real-time motion control unit, and a closed control unit, outputting the autonomous stair climbing driving instruction. Optionally, the probability voxel map construction mod