Quadruped robot motion planning method and system based on height self-adaption

Abstract

The invention relates to the field of mobile robot motion planning, and discloses a quadruped robot motion planning method and system based on height self-adaption. The method comprises the following steps: constructing 3D and 2D local obstacle maps according to coordinate transformation, and calculating a grid vertical distance through ray casting to generate a 2.5 D local obstacle map; determining an initial control sampling space by combining speed, acceleration and height constraints; discretizing a sampling space and simulation time, deducing a control value sequence and recursively calculating a local trajectory; then calculating a shaft alignment bounding box in a directed bounding box of the robot, screening grid units in the shaft alignment bounding box, comparing the height of a machine body with the allowable height of grids, and rejecting collision trajectories to obtain a feasible trajectory set; and finally selecting an optimal track by the evaluation function, extracting a control value and sending the control value to hardware for execution. According to the method, the 2.5 D local obstacle map containing height information is constructed, the four-dimensional control sampling space is designed, and the robot height self-adaptive adjustment is realized in combination with the collision detection and evaluation function.

Description

Technical Field

[0001] This application relates to the field of motion planning for mobile robots, and more particularly to a motion planning method and system for a quadruped robot based on height adaptation. Background Technology

[0002] With the development of mobile robot technology, quadruped robots, with their superior mobility and terrain adaptability, have a promising future in autonomous navigation in complex industrial environments such as pipe corridors, equipment warehouses, and machine rooms. Traditional two-dimensional path planning algorithms, such as the dynamic window method, perform well in flat environments with no height changes, but in real-world scenarios, there are vertical height constraints, and the height adjustment capabilities of quadruped robots are not fully utilized, limiting their mobility.

[0003] Existing technologies have significant limitations in height-restricted environments: While the dynamic window method offers high computational efficiency and real-time performance, and is widely used for 2D planar planning in wheeled robots, it only evaluates the trajectory in a 2D plane, neglecting vertical posture adjustment and thus failing to adapt to the height-adjustable characteristics of quadruped robots. The elastic band method improves path smoothness and avoids dynamic obstacles, but is limited to 2D translation and orientation planning, lacking height information modeling and unable to meet the needs of low-ceilinged spaces. Path planning methods based on 3D point clouds, while possessing strong spatial representation capabilities and suitable for platforms like UAVs, suffer from high computational complexity and low efficiency, making real-time deployment on ground robots difficult. Furthermore, they do not incorporate the height adjustment capabilities of quadruped robots to optimize posture, limiting their practicality. These limitations impact the operational coverage and safety of quadruped robots in height-restricted environments.

[0004] To address the aforementioned issues, this application proposes a height-adaptive quadruped robot motion planning method and system. By constructing a 2.5D obstacle map containing height information, designing a four-dimensional control sampling space, and combining collision detection and evaluation functions, height adaptive adjustment is achieved, thereby improving the quadruped robot's ability to navigate in complex environments. Summary of the Invention

[0005] This application provides a height-adaptive quadruped robot motion planning method and system. By constructing a 2.5D obstacle map containing height information, designing a four-dimensional control sampling space, and combining collision detection and evaluation functions, height adaptive adjustment is achieved, thereby improving the quadruped robot's ability to navigate in complex environments.

[0006] In a first aspect, this application provides a height-adaptive quadruped robot motion planning method, the method comprising: Step S101: Based on the coordinate transformation relationship provided by the robot's underlying drive and SLAM algorithm, construct 3D and 2D local obstacle maps, calculate the vertical distance of each grid cell through ray projection, and generate a 2.5D local obstacle map containing plane occupancy information and z-axis gap height. Step S102: Define a four-dimensional control vector containing linear velocity, angular velocity and body height. Combine the robot's velocity, acceleration and height constraints, and determine the initial control sampling space through the constraint intersection. Step S103: Discretize the initial control sampling space and simulation time, derive the control value sequence, and recursively calculate the local trajectory including spatial coordinates, yaw angle and the body height using the omnidirectional kinematic model; Step S104: Based on the local trajectory, calculate the axis-aligned bounding box within the robot's directed bounding box, filter all grid cells within the axis-aligned bounding box, and compare the robot's height with the grid's allowable height to eliminate collision trajectories and obtain a set of feasible trajectories. Step S105: With the goal of maximizing the preset evaluation function, select the optimal trajectory from the set of feasible trajectories, extract the corresponding four-dimensional control vector as the optimal control value, and send it to the robot hardware for execution.

[0007] Optionally, step S101 includes: establishing a transformation relationship between the LiDAR coordinate system and the map coordinate system based on the coordinate transformation relationship provided by the robot's underlying driver and the SLAM framework; transforming the LiDAR point cloud to the map coordinate system based on the transformation relationship, constructing a 3D local obstacle map through ray projection, and marking areas exceeding a set range as free space; compressing the 3D point cloud along the z-axis to generate a 2D point cloud, constructing a 2D local obstacle map through ray projection, and marking areas exceeding a set range as free space; constructing an initial 2.5D local obstacle map by fusing the planar occupancy information of the 2D local obstacle map and the vertical height information of the 3D local obstacle map; and forming the 2.5D local obstacle map by combining the planar occupancy status and the vertical distance information of the obstacles above each grid cell in the initial 2.5D local obstacle map.

[0008] Optionally, step S101 further includes: if the point cloud density of the lidar is lower than a preset threshold, a frame interpolation mechanism is triggered to supplement the missing area by interpolating the point clouds of two adjacent frames, so as to ensure the integrity and accuracy of the 2.5D local obstacle map.

[0009] Optionally, step S102 includes: defining, according to the robot motion planning requirements, a set of... x Axial velocity, yThe four-dimensional control vectors of axial velocity, angular velocity, and body height form the basic parameter framework of the control sampling space; based on the robot hardware performance parameters, the following parameters are set respectively. x Axial velocity, the aforementioned y The minimum and maximum values ​​of the axial velocity and the angular velocity are used to construct a first subset of the control sampling space under velocity constraints; based on the robot's kinematic characteristics, the following settings are made: x Axial acceleration, y The maximum values ​​of axial acceleration and angular acceleration are combined with the current measured linear velocity and angular velocity values ​​to calculate the variable range of each velocity parameter during the simulation time, thus constructing a second control sampling space subset under acceleration constraints. Based on the robot's mechanical structure parameters, the minimum and maximum heights of the robot's highest point from the ground are set to construct a third control sampling space subset under height constraints. By taking the intersection of the control sampling space subsets under velocity, acceleration, and height constraints, the initial control sampling space that simultaneously satisfies all three types of constraints is generated.

[0010] Optionally, step S103 includes: transferring the initial control sampling space... x Axis speed parameters y The axial velocity parameters, angular velocity parameters, and body height parameters are discretized to generate multiple discrete control vectors. The total trajectory simulation time is discretized according to a preset elastic estimation step size, transforming the continuous simulation time into multiple discrete time segments. For each discrete control vector, based on the robot's current measured velocity and acceleration constraints, the control value sequence corresponding to each time step is derived step by step according to the discrete time step size. Based on the omnidirectional kinematic model, the spatial coordinates and yaw angle changes corresponding to each time step are recursively calculated in combination with the control value sequence to generate a complete local trajectory.

[0011] Optionally, step S104 includes: initializing the collision state by setting the collision state of each local trajectory to non-collision; generating a directed bounding box in the map coordinate system by calling TransformFootprint for the pose of each point on the trajectory, and then obtaining an axis-aligned bounding box by ComputeAABB; enumerating all mesh cells within the axis-aligned bounding box and retaining the cells located inside the directed bounding box as determined by PointInOBB; reading the allowable passage height of the retained cells from the 2.5D local obstacle map and comparing it with the body height of the current trajectory one by one; if the body height is greater than or equal to the allowable height of any cell, marking the trajectory as a height collision; removing all trajectories marked as height collisions, and the remaining trajectories constitute the feasible trajectory set.

[0012] Optionally, step S105 includes: constructing a comprehensive evaluation function that includes target tracking, path following, obstacle avoidance distance, and height adjustment terms, wherein the height adjustment term is based on the robot's normal gait height and is used to quantify the deviation between the current height and the normal height; calculating the corresponding comprehensive evaluation function value for each trajectory in the feasible trajectory set, and selecting the trajectory with the highest comprehensive evaluation function value as the optimal trajectory by comparing the comprehensive evaluation function values ​​of all trajectories; extracting the four-dimensional control vector corresponding to the optimal trajectory as the optimal control value; and sending the optimal control value to the quadruped robot hardware to drive the robot to move along the optimal trajectory, thus completing a single planning closed loop.

[0013] Optionally, step S105 further includes: real-time monitoring of the deviation between the robot's actual height and the expected height in the optimal trajectory; when the deviation continues to exceed the tolerance range, immediately restarting the motion planning process to achieve dynamic replanning.

[0014] Secondly, this application provides a height-adaptive quadruped robot motion planning system, the system comprising: The map generation module receives the coordinate transformation relationship between the robot's underlying drive and the SLAM algorithm, constructs 3D and 2D local obstacle maps, calculates the vertical distance of the grid through ray projection, and generates a 2.5D local obstacle map including the plane occupancy and z-axis gap height. The sampling space determination module is used to define a four-dimensional control vector containing linear velocity, angular velocity, and body height. Combined with robot velocity, acceleration, and height constraints, the initial control sampling space is determined through the constraint intersection. The local trajectory extrapolation module is used to discretize the initial control sampling space and simulation time, derive the control value sequence, and recursively extrapolate the local trajectory containing spatial coordinates, yaw angle, and body height using an omnidirectional kinematic model. The collision trajectory elimination module is used to obtain local trajectories, calculate the axis alignment of the robot within the bounding box, filter all grid cells within the axis alignment bounding box, compare the robot height with the allowed height of the grid, eliminate collision trajectories, and obtain a set of feasible trajectories. The optimal control output module is used to select the optimal trajectory from the set of feasible trajectories with the goal of maximizing the preset evaluation function, extract the corresponding four-dimensional control vector as the optimal control value, and send it to the robot hardware.

[0015] Thirdly, a height-adaptive quadruped robot motion planning device is provided, comprising: a memory and at least one processor, wherein the memory stores instructions; the at least one processor invokes the instructions in the memory to cause the height-adaptive quadruped robot motion planning device to execute the aforementioned height-adaptive quadruped robot motion planning method.

[0016] This application proposes a height-adaptive quadruped robot motion planning method and system, applicable to complex industrial environments with vertical height constraints such as pipe corridors, equipment warehouses, and machine rooms. It solves the problems of existing path planning methods being unable to adapt to the height-adjustable characteristics of quadruped robots and having insufficient mobility in height-restricted scenarios. Compared with existing technologies, the beneficial effects of this application's technical solution are at least as follows: First, by constructing a 2.5D local obstacle map that combines two-dimensional occupancy grid information with lidar point cloud height data, the vertical gap between the robot and the obstacles above can be assessed. This overcomes the shortcomings of traditional dynamic window methods and elastic band methods, which can only plan in two-dimensional planes and lack height information modeling, enabling quadruped robots to adapt to height-restricted scenarios.

[0017] Secondly, a four-dimensional control sampling space including linear velocity, angular velocity, and body height is designed, and a trajectory calculation and height collision detection mechanism are combined with an omnidirectional kinematic model. This avoids the problems of high computational complexity and difficulty in real-time deployment of three-dimensional point cloud path planning methods, and makes full use of the height adjustment capability of quadruped robots to improve their passage safety in complex environments.

[0018] Third, constructing an evaluation function with a height adjustment term can guide the robot to quickly return to its normal height after obstacle avoidance, solving the problem that existing technologies do not consider the height energy consumption optimization of quadruped robots, and reducing the robot's excess energy consumption during the height adjustment process.

[0019] Fourth, it supports real-time resampling of control vectors and trajectory updates in dynamic environments, and the framework has no strong dependence on the underlying drive model of quadruped robots. It can be applied to a variety of legged mobile platforms with high adjustability, thus expanding the applicability and robustness of motion planning systems. Attached Figure Description

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

[0021] Figure 1This is a flowchart of a height-adaptive quadruped robot motion planning method according to this application; Figure 2 This is a schematic diagram of the structure of a height-adaptive quadruped robot motion planning system according to this application; Figure 3 This is a schematic block diagram of a height-adaptive quadruped robot motion planning device according to this application. Detailed Implementation

[0022] This application provides a height-adaptive quadruped robot motion planning method and system. The terms "first," "second," "third," "fourth," etc. (if present) in the specification, claims, and accompanying drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments described herein can be implemented in a sequence other than that illustrated or described herein. Furthermore, the terms "comprising" or "having" and any variations thereof are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or device that includes a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or devices.

[0023] This application relates to the field of mobile robot motion planning, and discloses a method and system for motion planning of a quadruped robot based on height adaptation. The method includes: constructing 3D and 2D local obstacle maps based on coordinate transformation; generating a 2.5D local obstacle map by calculating the vertical distance of the grid through ray projection; determining the initial control sampling space by combining velocity, acceleration, and height constraints; discretizing the sampling space and simulation time, deriving the control value sequence, and recursively calculating the local trajectory; then calculating the axis-aligned bounding box within the robot's directed bounding box, filtering the grid cells within the axis-aligned bounding box, comparing the robot's height with the allowed grid height, and eliminating collision trajectories to obtain a set of feasible trajectories; finally, selecting the optimal trajectory using an evaluation function, extracting the control values, and sending them to the hardware for execution. This application achieves adaptive height adjustment of the robot by constructing a 2.5D local obstacle map containing height information, designing a four-dimensional control sampling space, and combining collision detection and evaluation functions, thereby improving the robot's mobility.

[0024] For ease of understanding, the specific process of the embodiments of this application is described below. Please refer to [link / reference]. Figure 1 One embodiment of a height-adaptive quadruped robot motion planning method in this application includes: Step S101: Based on the coordinate transformation relationship provided by the robot's underlying drive and SLAM algorithm, construct 3D and 2D local obstacle maps, calculate the vertical distance of each grid cell through ray projection, and generate a 2.5D local obstacle map containing plane occupancy information and z-axis gap height.

[0025] In one specific embodiment, the process of performing step S101 may specifically include the following steps: Based on the coordinate transformation relationship provided by the robot's underlying drive and SLAM framework, a transformation relationship between the LiDAR coordinate system and the map coordinate system is established. Based on the aforementioned transformation relationship, the lidar point cloud is transformed into a map coordinate system, a 3D local obstacle map is constructed through light projection, and areas outside the set range are marked as free space. 3D point cloud is compressed along the z-axis to generate 2D point cloud. A 2D local obstacle map is constructed by ray projection, and areas outside the set range are marked as free space. An initial 2.5D local obstacle map is constructed by fusing the planar occupancy information of the 2D local obstacle map and the vertical height information of the 3D local obstacle map. For each grid cell in the initial 2.5D local obstacle map, the 2.5D local obstacle map is formed by combining its planar occupancy status and the vertical distance information of the obstacles above.

[0026] Specifically, the origin of the robot's body coordinate system (base) is defined as the projection point of the origin of the lidar coordinate system (lidar) onto the ground. The underlying driver calculates the transformation matrix from the base coordinate system to the lidar coordinate system based on the robot's kinematics. The SLAM framework provides robot pose estimation through a high-frequency laser inertial odometry (LIO) algorithm, which estimates the pose using a transformation matrix from the map coordinate system to the base coordinate system. This is represented by the transformation matrix from the lidar coordinate system to the map coordinate system, based on the transitivity of coordinate transformation. can be It is found that the transformation matrix is ​​used to transform the point cloud data collected by LiDAR from the LiDAR coordinate system to the map coordinate system, providing a unified coordinate reference for the subsequent construction of multi-dimensional obstacle maps, and solving the problem of fusion of data from different sensors due to the difference in coordinate systems.

[0027] Based on the above transformation relationship, the LiDAR point cloud is transformed to the map coordinate system to construct a 3D local obstacle map. For each LiDAR point cloud frame... (The lidar uses a Livox MID-360 hybrid solid-state lidar with a scanning frequency of 10Hz and approximately 20,000 points per frame.) In the lidar coordinate system, through a transformation matrix... The inverse transformation converts it into a point cloud in map coordinates. The conversion formula is: Subsequently, a ray casting algorithm was used to construct a 3D local obstacle map by simulating the ray propagation path and marking the spatial unit states. Let the origin of the lidar in the map coordinate system be o = ( o x , o y , o z )(Depend on (The translation components are determined), for Each point in p =( x , y , z ), from the origin o to point p A ray of light is projected, and the 3D octree mesh cells that the ray passes through are marked as occupied; simultaneously, the radius of the region of interest is set. =5m (based on the working range of the quadruped robot in the industrial pipe gallery), and all Euclidean distances from the origin o in the 3D octree map are greater than 5m. The grid cells are marked as free space to complete the 3D local obstacle map. The construction of this map solves the technical problem that two-dimensional maps cannot represent vertical height obstacles in the background technology, and provides three-dimensional spatial data support for subsequent height constraint assessment.

[0028] Perform 3D point cloud z Axis compression generates 2D point clouds, which are then used to construct 2D local obstacle maps. 3D point cloud in map coordinate system implement z Shaft compression operation, compression rules are as follows ,in =0.05m =0.15m (set according to the height range of the ground on which the quadruped robot walks, to ensure that the compressed point cloud can accurately reflect the distribution of obstacles on the ground), and the 3D point cloud is projected onto the ground using this rule. z A 2D point cloud is obtained by using a 2D plane with a value of 0. The origin of the 2D projection of the lidar in the map coordinate system. (from the origin o) x , y Coordinates and z =0 (determined), using the same ray casting algorithm as 3D map construction, from... o 2D Towards Each point in ( x ,y , 0) Project a ray of light, and the 2D raster grid cells that the ray passes through are marked as occupied; similarly, cells in the 2D raster map that are occupied are marked as occupied. o 2D The Euclidean distance is greater than A grid cell with a height of 5m is marked as free space, resulting in a 2D local obstacle map. This map retains the advantages of traditional two-dimensional planning algorithms in efficiently representing planar obstacles, while avoiding the problem of excessive computational complexity caused by directly using 3D maps, thus balancing the accuracy of environmental representation with computational efficiency.

[0029] An initial 2.5D local obstacle map is constructed by fusing planar occupancy information from a 2D local obstacle map and vertical height information from a 3D local obstacle map. Initialization of the 2.5D local obstacle map. For 2D local obstacle maps Each grid cell in i , j , 0), first of all Assigned value ,in This indicates the occupancy status of the grid cell in the 2D map (0 indicates free, 1 indicates occupied). This is the initial vertical clearance height (temporarily replaced by the radius of the region of interest, to be updated with the actual measured value later). Then, regarding... Each grid cell in i , j ,0), calculate its physical coordinates based on the resolution of the 2D raster (set to 0.1m / grid). x The specific calculation method is as follows: (y=0) , ,in( , (Origin coordinates of the 2D raster map); in the 3D local obstacle map The function `CastRay` from the octree library is called to perform ray casting, with the starting point as the input. x , y , 0) T , Light ray direction vector (0,0,1) T (Along the positive z-axis) and the radius of the region of interest The output is the point of collision between the light beam and the obstacle above. ,Right now .like If not empty, calculate the collision point. and the starting point ( x , y Euclidean distance between (,0) The calculation formula is: (Because the starting point and the collision point are at) x , y The coordinates are the same, but the actual calculation is as follows: ,in for of z (coordinates), and Updated to ( );like If empty, then keep The initial values ​​are set, thus completing the construction of the 2.5D local obstacle map.

[0030] In one specific embodiment, if the point cloud density of the lidar is lower than a preset threshold, a frame interpolation mechanism is triggered to fill in the missing area by interpolating the point clouds of two adjacent frames, so as to ensure the integrity and accuracy of the 2.5D local obstacle map. Specifically, this includes the following steps: If the point cloud density of the lidar is lower than a preset threshold, a frame interpolation mechanism is triggered to fill in the missing area by interpolating the point clouds of two adjacent frames, so as to ensure the integrity and accuracy of the 2.5D local obstacle map.

[0031] Specifically, if the LiDAR point cloud density is lower than a preset threshold (set at 5 points / square meter, based on the perception accuracy requirements of the industrial pipe gallery environment), a frame interpolation mechanism is triggered. This mechanism uses a linear interpolation algorithm to fill in missing areas by interpolating point clouds from two adjacent frames: Let the current frame point cloud be... The previous frame's point cloud is (The two frames are spaced 0.1 seconds apart, consistent with the LiDAR scanning frequency.) First, calculate the overlapping area of ​​the two point clouds in the map coordinate system. The overlapping area is determined by the midpoint of the two point clouds. x , y The coordinate differences are all less than 0.2m; the location of missing points within the overlapping area ( x,y,z ),according to Corresponding point and Corresponding point (Two points satisfied) and Perform linear interpolation; the interpolation formula is: , This interpolation calculation supplements the missing point cloud data, ensuring the integrity and accuracy of the 2.5D local obstacle map. It avoids deviations in the calculation of the vertical gap height of obstacles above due to insufficient point cloud density. This frame-filling mechanism solves the problem of missing point clouds that may occur in complex industrial environments for LiDAR, and further improves the ability of the 2.5D map to express highly constrained environments.

[0032] Step S102: Define a four-dimensional control vector containing linear velocity, angular velocity, and body height. Combine the robot's velocity, acceleration, and height constraints, and determine the initial control sampling space through the constraint intersection.

[0033] In one specific embodiment, the process of performing step S102 may specifically include the following steps: Based on the robot's motion planning requirements, define the following: x Axial velocity, y The four-dimensional control vectors of axial velocity, angular velocity, and body height form the basic parameter framework of the control sampling space; Based on the robot's hardware performance parameters, the minimum and maximum values ​​of the x-axis velocity, the y-axis velocity, and the angular velocity are set respectively to construct a first control sampling space subset under velocity constraints; Based on the robot's kinematic characteristics, set x Axial acceleration, y The maximum values ​​of axial acceleration and angular acceleration are combined with the current measured linear velocity and angular velocity values ​​to calculate the variable range of each velocity parameter during the simulation time, and to construct a second control sampling space subset under acceleration constraints. Based on the robot's mechanical structure parameters, the minimum and maximum heights of the robot's highest point from the ground are set, and a third control sampling space subset under height constraints is constructed. The initial control sampling space that simultaneously satisfies all three types of constraints is generated by taking the intersection of subsets of the control sampling space under velocity, acceleration, and height constraints.

[0034] Specifically, based on the robot's motion planning requirements, the definition includes... x Axial velocity, y The four-dimensional control vectors of axial velocity, angular velocity, and body height form the basic parameter framework of the control sampling space. These four-dimensional control vectors are expressed as follows: ,in for x Axial velocity (unit: m / s) for y Axial velocity (unit: m / s) ω Angular velocity (unit: rad / s) h The height of the robot (unit: m) is defined based on the operational requirements of quadruped robots in industrial environments such as pipe racks, equipment warehouses, and machine rooms, where they need to simultaneously adjust their planar motion state and vertical height posture. This directly addresses the shortcomings of background technologies such as the Dynamic Window Method (DWA) and the Flexible Band Method (TEB), which only use two-dimensional pose. To address the technical challenge of controlling variables and adapting to the adjustable height characteristics of quadruped robots, a new height dimension was added. h To expand vertical control capabilities.

[0035] Based on the robot's hardware performance parameters, set respectively x Axial velocity, y The minimum and maximum values ​​of axial velocity and angular velocity are used to construct the first control sampling space subset under velocity constraints. The robot's hardware performance parameters were read in real-time from the underlying drivers by the onboard computer (Intel Core i9-11900H CPU, 16GB RAM, running Ubuntu 20.04 system and ROS Noetic framework). These parameters were then combined with the robot's motion capability limits in industrial inspection scenarios to set... The range of values ​​is ,in =-0.5m / s (the negative sign indicates that the robot moves along...) x (Motion in the negative direction of the axis) =1.0m / s; The range of values ​​is ,in =-0.3m / s =0.3m / s; ω The range of values ​​is ,in =-1.5rad / s =1.5 rad / s. Based on this, the first control sampling space subset... as follows: The construction of this subset is based on the motion limit parameters that the robot hardware can stably output, ensuring that the velocity and angular velocity parameters in the control vector do not exceed the hardware's carrying capacity, and avoiding motor overload or motion instability caused by control commands exceeding limits.

[0036] Based on the robot's kinematic characteristics, set x Axial acceleration, y The maximum values ​​of axial acceleration and angular acceleration, combined with the currently measured linear and angular velocity values, are used to calculate the variable range of each velocity parameter during the simulation time, thus constructing a second control sampling space subset under acceleration constraints. The robot's kinematic characteristics follow the laws of uniformly accelerated linear motion in physics, and its original formula is: (in the formula) v The final velocity, The initial velocity is... a For acceleration, t (For motion time), due to the maximum limit of acceleration, the changes in velocity parameters must remain within the range of "current measured value ± maximum acceleration × simulation time" within the simulation time to ensure the smoothness of the motion process. Specifically, set... for xMaximum linear acceleration in the axial direction (value taken as 0.8 m / s²). This represents the maximum linear acceleration in the y-axis direction (value 0.6 m / s²). α This is the maximum angular acceleration (value 1.2 rad / s²). The time for the DWA-H forward modeling simulation is 0.5s. These are obtained from the robot drive module. x axis, y Current measured linear velocity of the shaft To obtain the current measured angular velocity from the robot drive module, then x The variable range of the axial velocity is , y The variable range of the axial velocity is The variable range of angular velocity is A specific calculation example illustrates this: if the current actual measurement... =0.3m / s, then substituting into the formula, we get x The variable range of the axis speed is 0.3-0.8×0.5=-0.1m / s to 0.3+0.8×0.5=0.7m / s; if the current actual measurement =0.1m / s, then the variable range of the velocity along the y-axis is 0.1-0.6×0.5=-0.2m / s to 0.1+0.6×0.5=0.4m / s; if the current measured value is 0.1m / s, then the variable range of the velocity along the y-axis is 0.1-0.6×0.5=-0.2m / s to 0.1+0.6×0.5=0.4m / s; =0.2 rad / s, then the variable range of angular velocity is from 0.2 - 1.2 × 0.5 = -0.4 rad / s to 0.2 + 1.2 × 0.5 = 0.8 rad / s. Based on the above variable range, the second control sampling space subset Build as , The construction of this subset ensures that the velocity change process corresponding to the control vector conforms to the physical laws of robot kinematics, avoiding body swaying or gait disorder caused by excessive acceleration.

[0037] Based on the robot's mechanical structure parameters, the minimum and maximum heights of the robot's highest point above the ground are set, and a third control sampling space subset under height constraints is constructed. The mechanical structure parameters of the robot are determined by the quadruped robot design drawings, taking into account its need for passage in low-ceilinged industrial spaces (such as under pipes and equipment). This is the minimum height of the highest point of the aircraft above the ground. The maximum height of the aircraft's highest point above the ground. h Given the target height of the highest point of the aircraft above the ground in the control vector, the third control sampling space subset is: The construction of this subset directly addresses the technical problem that traditional two-dimensional planning algorithms lack the ability to model height information and cannot meet the passage requirements in low-ceilinged spaces, by using a single height parameter. h Instead of using a complex spatial modeling method based on 3D point clouds, we model the state in the vertical direction, thus reducing computational complexity while ensuring the ability to express height constraints.

[0038] An initial control sampling space that simultaneously satisfies all three types of constraints is generated by taking the intersection of subsets of the control sampling space under velocity, acceleration, and height constraints. This intersection operation originates from the fundamental operational rules of set theory in mathematics, and its logical meaning is to select pairs of elements that simultaneously satisfy... Hardware speed limits kinematic acceleration laws The set of control vectors for the mechanical height range. From the data correspondence, The input data consists of robot hardware performance parameters (such as maximum linear velocity and maximum angular velocity), and the output is velocity parameters. The feasible domain; The input data is the current measured speed. Maximum acceleration and simulation time The output is the dynamic feasible region of the velocity parameters during the simulation time; The input data is the robot's mechanical structure parameters. The output is the height parameter ( h The feasible regions of the three are taken as inputs and the initial control sampling space is taken as outputs. Each control vector in this space All of them possess hardware feasibility, kinematic rationality, and high adaptability, providing effective and reliable control inputs for subsequent local trajectory extrapolation.

[0039] Step S103: Discretize the initial control sampling space and simulation time, derive the control value sequence, and recursively calculate the local trajectory including spatial coordinates, yaw angle and the aircraft height using the omnidirectional kinematic model.

[0040] In one specific embodiment, the process of executing step S103 may specifically include the following steps: The initial control sampling space x Axis speed parameters y The axis velocity parameters, angular velocity parameters, and body height parameters are discretized to generate multiple discrete control vectors; The total time of trajectory simulation is discretized according to the preset elastic estimation step size, and the continuous simulation time is converted into multiple discrete time segments. For each discrete control vector, based on the robot's current measured velocity and acceleration constraints, the sequence of control values ​​corresponding to each time step is derived step by step according to the discrete time step. Based on the omnidirectional kinematics model, and combined with the control value sequence, the spatial coordinates and yaw angle changes corresponding to each time step are recursively calculated to generate a complete local trajectory.

[0041] Specifically, the initial control sampling space x Axis speed parameters y The axial velocity, angular velocity, and body height parameters are discretized to generate multiple discrete control vectors. Specifically, the intersection of velocity, acceleration, and height constraints in the initial control sampling space is discretized, with parameters set based on the accuracy requirements of the robot's motion planning and the computing power capacity of the onboard computer. x Axis velocity search space size (i.e., for) The feasible region was sampled in 5 equal parts. y Axis velocity search space size =3, angular velocity search space size Size of the search space for the height of the machine According to the formula for calculating the number of discretizations. Substituting the parameters yields n =5×3×4×3=180 discrete control vectors, each of which is... The specific values ​​can be chosen, for example, one of the discrete control vectors is (0.4m / s, 0.1m / s, 0.3rad / s, 0.5m). This discretization process transforms the continuous control space into a finite number of discrete vectors, avoiding the problem of excessive computational complexity caused by continuous space sampling in three-dimensional path planning methods.

[0042] The total trajectory simulation time is discretized based on a preset elastic estimation step size, transforming the continuous simulation time into multiple discrete time segments. The total trajectory simulation time... Set to 0.5s, with flexible calculation step size. According to the time discretization formula Δ Substituting the parameters yields That is, the duration of each discrete time segment is 0.05s, and the continuous simulation time is converted into 10 discrete time steps, providing a time reference for the subsequent derivation of the control value sequence and trajectory by time step.

[0043] For each discrete control vector, based on the robot's current measured velocity and acceleration constraints, the sequence of control values ​​for each time step is derived progressively according to the discrete time step. The robot's current measured velocity includes values ​​obtained from the underlying driver. x Measured linear velocity of shaft =0.3m / s y Measured linear velocity of shaft =0.1m / s and measured angular velocity The acceleration constraint follows the method in step S102. The derivation of the control value sequence follows the laws of uniformly accelerated motion in physics, and is adapted by combining the discrete time step: For x Axis speed control value sequence If the current step The target is smaller than the discrete control vector. Then the next step (Total acceleration must be within) The acceleration is uniformly distributed within each step, with an increment of acceleration per step. );like Greater than the target ,but ; y Axis speed control value sequence With angular velocity control value sequence The derivation logic is consistent, and the sequence of body height control values ​​is consistent. Because of the discrete control vector h Since it is a fixed value, Taking the discrete control vector (0.5m / s, 0.2m / s, 0.5rad / s, 0.4m) as an example, the derivation is as follows. :initial =0.3m / s, 0.08m / s, then =0.3 + 0.08 = 0.38 m / s =0.38 + 0.08 = 0.46 m / s =0.48+0.08=0.54m / s (exceeds the target by 0.5m / s, so the next step is to decrease). =0.54-0.08=0.46m / s, until the sequence converges to the target. Nearby, forming a complete Sequence; similarly, it can be derived , sequence sum The derivation process ensures that the control value sequence smoothly transitions from the current measured speed to the target speed, avoiding sudden speed changes that could cause robot attitude instability.

[0044] Based on the omnidirectional kinematics model, and by recursively calculating the changes in spatial coordinates and yaw angle at each time step using a control value sequence, a complete local trajectory is generated. The omnidirectional kinematics model is a classic model in robot kinematics, used to describe the pose changes of a robot capable of omnidirectional planar motion. This application extends the body height parameter based on this model to form a model that includes spatial coordinates. Yaw angle ψ and body height h The trajectory calculation formulas are shown in formulas (1) to (4) below: (1) In the formula For the first i time step x Axis coordinates (unit: m) For discrete time steps, The first i time step x axis, y Axis speed control value, ψ For the first i Yaw angle at time step (unit: rad); (2) In the formula and The first time step y Axis coordinates (unit: m); (3) In the formula For the first i+ 1 time step yaw angle, For the first i Time step angular velocity control value; (4) In the formula The first The body height at the time step (unit: m) remains unchanged during trajectory extrapolation because h is a fixed value in the current discrete control vector.

[0045] The trajectory extrapolation process is illustrated using a specific calculation example: The initial pose is set as... x (0) = 2.0m y (0) = 1.5m ψ (0) = 0 rad (along) x (positive axis direction) h (0) = 0.4m, the first i The control value sequence at time step 0 is: =0.38m / s =0.16m / s =0.28rad / s =0.05s.

[0046] Substitute into formula (1) to calculate ; Substitute into formula (2) to calculate ; Substitute into formula (3) to calculate ; Substituting into formula (4) yields .

[0047] Recursively calculate 10 time steps (covering) according to this logic. =0.5s), which can yield the following: , , , The complete local trajectory. This trajectory calculation process retains the planar motion accuracy of traditional two-dimensional programming algorithms, while also adding a body height parameter. h It achieves vertical state modeling; at the same time, the recursive calculation method only depends on the pose of the previous step and the current control value in each step, resulting in low computational complexity, which is far lower than the massive point cloud processing volume based on 3D point cloud planning methods.

[0048] Step S104: Based on the local trajectory, calculate the axis-aligned bounding box within the robot's directed bounding box, filter all grid cells within the axis-aligned bounding box, and compare the robot's height with the allowed grid height to eliminate collision trajectories and obtain a set of feasible trajectories.

[0049] In one specific embodiment, the process of executing step S104 may specifically include the following steps: Initialize the collision state by setting the collision state of each of the local trajectories to non-collision. The TransformFootprint function is called to generate a directed bounding box in the map coordinate system for the pose of each point on the trajectory, and then the ComputeAABB function is used to obtain the axis-aligned bounding box. Enumerate all mesh cells within the axis-aligned bounding box, and retain the cells located inside the directed bounding box as determined by PointInOBB; The permissible height of the reserved unit is read from the 2.5D local obstacle map and compared with the body height of the current trajectory one by one. If the body height is greater than or equal to the permissible height of any unit, the trajectory is marked as a height collision. All trajectories marked as high-collision are removed, and the remaining trajectories constitute the set of feasible trajectories.

[0050] Specifically, when filtering the set of feasible trajectories, the collision state is first initialized. The collision state of each local trajectory generated in step S103 is uniformly set to no collision, and the initial state value is recorded as collision=0 (0 represents no collision, 1 represents high collision).

[0051] For each local trajectory, the pose at each time step The `TransformFootprint` function is called to generate a directed bounding box (OBB) in the map coordinate system. The `TransformFootprint` function is based on the robot's body dimensions (quadruped robot body length 0.6m, width 0.4m, height detection range is the robot's height). ), combined with the yaw angle of the current pose Construct a directed bounding box in the map coordinate system that matches the aircraft's attitude. The boundary is determined by the aircraft dimensions and yaw angle, as shown in the formula: , (in the formula) l =0.3m is half the length of the fuselage. w =0.2m is half the width of the fuselage. The coordinates of each vertex are calculated using the vertex coordinate formula of a rotated rectangle in computer graphics. This is achieved by rotating the vertices in the local coordinate system of the fuselage (the rotation matrix is...). (Transformation to map coordinate system using translation). After generating the directed bounding box, the ComputeAABB function is called to calculate its axis-aligned bounding box (AABB). The AABB is the smallest rectangle containing the OBB, and its boundary is defined by the coordinates of all vertices of the OBB. x Minimum coordinates , x Maximum coordinates , y Minimum coordinates , y Maximum coordinates Determined, that is This calculation process is based on the definition of axis-aligned bounding boxes in computer graphics. It is achieved by traversing the OBB vertex coordinates and taking extreme values, which provides range constraints for subsequent mesh element enumeration, reduces the number of unnecessary element detections, and solves the efficiency problem caused by the complexity of 3D bounding box calculation in the background technology.

[0052] Enumerate axis-aligned bounding boxes All grid cells within the area are transformed according to the raster resolution (0.1m / cell) of the 2.5D local obstacle map. The transformation formula is as follows: , This formula converts the physical coordinates within the AABB range into map grid indices, resulting in the set of grid cells to be detected. For each grid cell in the set, call the PointInOBB function to determine whether its center point lies within the directed bounding box. Inside. The determination is made by checking whether the projection of the mesh center point onto the normal vectors of each edge of the OBB overlaps with the OBB projection. If they overlap, the cell is considered to be inside the OBB and is retained; otherwise, it is discarded. This results in a subset of mesh cells located inside the directed bounding box. This process combines AABB preliminary screening with OBB precise determination, reducing the number of mesh cells to be detected while ensuring detection accuracy, thus solving the problem of low efficiency in traditional point-by-point collision detection.

[0053] From 2.5D local obstacle map Reading the reserved unit subset Permissible passage height This parameter is stored in In each grid cell, the lowest height of the obstacle above the corresponding location is represented. The body height at this time step of the current trajectory is also considered. With each Compare them one by one, the comparison formula is: If any unit satisfies this condition, it indicates that the robot body will collide with the obstacle above at this position. The collision state of the trajectory is updated to collision=1 (height collision), and the detection of the trajectory in subsequent time steps is terminated.

[0054] Trajectories with a collision state of collision=1 are removed, and the remaining trajectories form a set of feasible trajectories. This process accurately represents the aircraft's spatial occupancy using directed bounding boxes and achieves height collision detection by combining the allowable passage height of the 2.5D map. This not only makes up for the shortcomings of traditional 2D collision detection in ignoring vertical constraints, but also avoids the need for processing massive amounts of point data in 3D point cloud collision detection.

[0055] Step S105: With the goal of maximizing the preset evaluation function, select the optimal trajectory from the set of feasible trajectories, extract the corresponding four-dimensional control vector as the optimal control value, and send it to the robot hardware for execution.

[0056] In one specific embodiment, the process of executing step S105 may specifically include the following steps: A comprehensive evaluation function is constructed that includes target tracking, path following, obstacle avoidance distance, and height adjustment terms. The height adjustment term is based on the robot's normal gait height and is used to quantify the deviation between the current height and the normal height. For each trajectory in the set of feasible trajectories, calculate its corresponding comprehensive evaluation function value. By comparing the comprehensive evaluation function values ​​of all trajectories, select the trajectory with the highest comprehensive evaluation function value as the optimal trajectory. Extract the four-dimensional control vector corresponding to the optimal trajectory as the optimal control value; The optimal control value is sent to the quadruped robot hardware, driving the robot to move along the optimal trajectory and complete a single planning closed loop.

[0057] Specifically, when selecting the optimal trajectory and outputting the optimal control value, a comprehensive evaluation function is first constructed, which includes target tracking, path following, obstacle avoidance distance, and height adjustment terms. This function aims to maximize the value and is used to quantify the comprehensive performance of each feasible trajectory. Its expression is shown in formula (5): in, traj This represents any trajectory in the set of feasible trajectories. For target tracking coefficient, This is the path following coefficient. The obstacle avoidance coefficient, The four coefficients are set according to the priority of "safety first, energy efficiency second, and route compliance" in height-restricted environments such as industrial utility tunnels and equipment warehouses. The specific values ​​are... Furthermore, the sum of all coefficients is 100, ensuring that the weight allocation meets the actual operational requirements; For target tracking items, For path following items, For obstacle avoidance distance, For the height adjustment item, the calculation formulas and sources are as follows: Target tracking term: Constructed based on the reciprocal of the Euclidean distance in mathematics, used to quantify the proximity between the trajectory endpoint and local target points. Provided by the global path planning module (approximately 1.5m to 2.0m from the robot's current position, and the total trajectory simulation time in step S103). And the robot's maximum speed matching), its expression is shown in formula (6): In the formula, For the endpoint of the trajectory x axis, y Axis coordinates (taken from the last time step of the trajectory) This formula originates from the basic principles of spatial distance calculation, using a reciprocal form to show that "the closer the trajectory endpoint is to the target point, the better." "The larger the value, the better" addresses the problems of weak target guidance and easy deviation from the global path in traditional algorithms.

[0058] Path following term: Constructed based on the reciprocal of the average distance from all points on the trajectory to the reference path, where the reference path is a segment of the global path within a local region (synchronously output by the SLAM framework, along with the region of interest of the 2.5D local obstacle map). (The range is consistent), and its expression is shown in formula (7): In the formula, N=10 is the total number of time steps of the trajectory. For the trajectory number i The coordinates of the step. For the corresponding number on the reference path i The formula quantifies the fit between the trajectory and the reference path by using the inverse of the average distance, thus preventing the trajectory from deviating significantly from the globally planned route and solving the problem of the traditional height adaptive algorithm becoming disconnected from the global path.

[0059] Obstacle avoidance distance: Constructed based on the average planar distance from all points on the trajectory to the nearest obstacle. The data comes from the 2.5D local obstacle map generated in step S101. Its expression is shown in formula (8): In the formula, For the trajectory number i Step coordinates arrive The plane distance of the most recently occupied grid in the middle; if this distance is less than the safety threshold of 0.3m (set according to the quadruped robot's body width of 0.4m to ensure sufficient obstacle avoidance clearance), then... A distance of 0.3m is set and an obstacle avoidance penalty is triggered. This formula directly relates to environmental obstacle information, ensuring that the trajectory maintains a safe obstacle avoidance distance and solving the problem of insufficient robustness in obstacle avoidance of two-dimensional programming algorithms.

[0060] Height adjustment: Based on the robot's normal gait height, quantify the deviation between the current trajectory height and the normal gait height. The design parameters of the quadruped robot taken from the embodiment are specifically: =0.6m, and its expression is shown in formula (9): In the formula, For the trajectory number i The body height of the step is calculated by taking the absolute value of the deviation and averaging it, so that the "trajectory height is closer to the target height" , The smaller the value, the larger its reciprocal. This formula originates from the basic method of deviation quantification in physics, guiding the robot to recover to the optimal energy-efficiency height as quickly as possible after obstacle avoidance, thus solving the problem of reduced endurance caused by not considering the energy consumption of altitude adjustment.

[0061] For each trajectory in the feasible trajectory set, calculate its corresponding comprehensive evaluation function value. The calculation is illustrated using two typical trajectories (trajectory A and trajectory B) as examples: Trajectory A: Endpoint coordinates Local target point ,but The average distance from each point on the trajectory to the reference path is 0.15m, then... If the average distance from each point to the nearest obstacle is 0.5m, then... The average height at each point is 0.58m, then... Substituting into the comprehensive evaluation function, we get: Trajectory B: Endpoint coordinates ,but The average distance from each point on the trajectory to the reference path is 0.2m, then... The average distance from each point to the nearest obstacle is 0.45m, then... The average height at each point is 0.45m, then... = Substituting into the comprehensive evaluation function, we get: By comparing the comprehensive evaluation function values ​​of all trajectories, the trajectory with the highest comprehensive evaluation function value (such as trajectory A in the example above) is selected as the optimal trajectory. The four-dimensional control vector corresponding to the optimal trajectory is extracted as the optimal control value. All parameters are within the constraints set in step S102. , , , To ensure hardware executability, the optimal control values ​​are sent to the quadruped robot hardware, specifically through the ROS topic communication interface of the onboard computer. The topic name is " / quadruped / control_cmd", and the data format uses the ROS standard message type "std_msgs / Float32MultiArray". The message data array is stored sequentially. The robot's underlying drive module subscribes to this topic, parses the optimal control values, and converts them into joint motor control signals. The linear velocity and angular velocity parameters are converted into gait period and step length commands for the leg joints, and the height parameter is converted into stroke commands for the leg extension motors, driving the robot to move along the optimal trajectory.

[0062] In one specific embodiment, the process of performing step S105 may further include the following steps: The deviation between the robot's actual height and the expected height in the optimal trajectory is monitored in real time. When the deviation continues to exceed the tolerance range, the motion planning process is immediately restarted to achieve dynamic replanning.

[0063] Specifically, the deviation between the robot's actual height and the expected height in the optimal trajectory is monitored in real time. When the deviation continues to exceed the tolerance range, the motion planning process is immediately restarted to achieve dynamic replanning, ensuring the robot's passage safety and posture stability in height-restricted scenarios.

[0064] Real-time monitoring is achieved through data fusion between the robot's inertial measurement unit (IMU) and lidar. The IMU, integrated into a laser inertial odometry system, has a sampling frequency of 100Hz and outputs real-time vertical displacement data of the robot's body in the base coordinate system. The lidar uses a Livox MID-360 hybrid solid-state lidar with a scanning frequency of 10Hz, and is based on the coordinate transformation relationship established in step S101. The ground point cloud data collected by the LiDAR is converted to a map coordinate system, and the vertical distance from the bottom of the robot to the ground is calculated as auxiliary correction data for the actual height. Specifically, the robot's actual height... The calculation uses a weighted fusion algorithm, and the formula is: in, The vertical displacement data output by the inertial measurement unit. The vertical distance from the bottom of the fuselage to the ground, calculated for the lidar. The weighting coefficients are determined based on measured data; the inertial measurement unit (IMU) has a higher sampling frequency and is given a higher weight to ensure real-time performance, while LiDAR data is more stable and is used to correct IMU drift. This multi-sensor complementarity improves the accuracy of actual height detection, solves the height measurement deviation problem caused by noise or obstruction from a single sensor, and provides a reliable data source for deviation calculation.

[0065] Expected height in the optimal trajectory The optimal trajectory selected in step S105 is taken, specifically the body height corresponding to each time step of the optimal trajectory. Based on the correspondence between the robot's real-time motion time and trajectory simulation time, the desired height value at the current moment is obtained through linear interpolation. For example, the optimal total trajectory simulation time... It contains 10 time steps (each step) If the robot has moved along the optimal trajectory for 0.12s, then the time between the second step (0.1s) and the third step (0.15s) of the trajectory at the current moment is calculated using the linear interpolation formula (10). : In the formula, The expected height for step 2 of the trajectory. For the desired height in step 3 of the trajectory, linear interpolation ensures that the desired height is synchronized with the robot's actual motion process.

[0066] Calculate the deviation between the actual height and the expected height. The formula is: .

[0067] in, For height deviation, the tolerance range is set to... ,when When the deviation is ≤0.05m, the height deviation is considered to be within the allowable range; when When the deviation is greater than 0.05m, the judgment deviation exceeds the tolerance range.

[0068] To avoid false triggering caused by transient noise, a deviation monitoring duration is set. The onboard computer's timer module records the duration of deviations exceeding the tolerance range. .when If the judgment deviation continues to exceed the tolerance range, the onboard computer sends a replanning trigger signal through the ROS service communication interface, immediately terminating the execution of the current optimal control value and restarting the complete motion planning process from steps S101 to S105: reconstructing the 2.5D local obstacle map to update the environmental information, regenerating the initial control sampling space, calculating the local trajectory, filtering the set of feasible trajectories and calculating the comprehensive evaluation function, and finally outputting the new optimal control value to the robot hardware to complete the dynamic replanning.

[0069] To illustrate with a specific scenario example: If the expected height of the optimal trajectory at the current moment... =0.58m, the robot's actual height is affected by ground protrusions. =0.64m, then =|0.64-0.58|=0.06m>0.05m, the deviation exceeds the tolerance range; the timer starts counting, if within the subsequent 10 inertial measurement unit sampling periods (0.1s), It was consistently maintained between 0.06m and 0.07m. If this occurs, dynamic replanning is triggered. When step S101 is re-executed, the lidar detects a raised area on the ground and updates the allowable passage height of the corresponding grid in the 2.5D local obstacle map; when generating the initial control sampling space in step S102, the height parameter is adjusted based on the updated environmental height constraints. h The feasible region; subsequent steps calculate a new optimal trajectory adapted to the ground protrusion, with an expected height. The height was adjusted to 0.64m to ensure that the deviation between the robot's actual height and the expected height falls within the tolerance range. This dynamic replanning mechanism addresses the problem that traditional planning algorithms rely solely on single planning results and cannot cope with dynamic environmental changes or body posture disturbances through real-time height monitoring and deviation judgment, ensuring that the robot maintains a safe posture in height-restricted environments. Simultaneously, the replanning trigger conditions are clearly defined through quantitative parameters, avoiding false triggers caused by subjective judgments. Furthermore, the replanning process reuses algorithm modules from previous steps, eliminating the need for additional new functionalities and ensuring the compatibility and operability of the solution.

[0070] The above describes a height-adaptive quadruped robot motion planning method according to an embodiment of this application. The following describes a height-adaptive quadruped robot motion planning system 200 according to an embodiment of this application. Please refer to [link to relevant documentation]. Figure 2 One embodiment of the height-adaptive quadruped robot motion planning system 200 in this application includes: The map generation module 201 is used to receive the coordinate transformation relationship between the robot's underlying drive and the SLAM algorithm, construct 3D and 2D local obstacle maps, calculate the vertical distance of the grid through ray projection, and generate a 2.5D local obstacle map including the plane occupancy and z-axis gap height. The sampling space determination module 202 is used to define a four-dimensional control vector containing linear velocity, angular velocity, and body height. Combined with robot velocity, acceleration, and height constraints, the initial control sampling space is determined by the constraint intersection. The local trajectory calculation module 203 is used to discretize the initial control sampling space and simulation time, derive the control value sequence, and recursively calculate the local trajectory containing spatial coordinates, yaw angle, and body height using an omnidirectional kinematic model. The collision trajectory elimination module 204 is used to obtain local trajectories, calculate the axis-aligned bounding box within the robot's directed bounding box, filter all grid cells within the axis-aligned bounding box, compare the robot's height with the grid's allowable height, eliminate collision trajectories, and obtain a set of feasible trajectories. The optimal control output module 205 is used to select the optimal trajectory from the set of feasible trajectories with the goal of maximizing the preset evaluation function, extract the corresponding four-dimensional control vector as the optimal control value, and send it to the robot hardware.

[0071] above Figure 2 The present invention provides a detailed description of a height-adaptive quadruped robot motion planning system from the perspective of modular functional entities. The following describes a height-adaptive quadruped robot motion planning device from the perspective of hardware processing.

[0072] Reference Figure 3This invention also provides a height-adaptive quadruped robot motion planning device 300, which can be a server, and its internal structure can be as follows: Figure 3 As shown, this height-adaptive quadruped robot motion planning device includes a processor 302, a memory 303, a display screen 304, an input device 305, a network interface 306, and a database 307 connected via a system bus 301. The processor, designed as a computer, provides computational and control capabilities. The memory of this height-adaptive quadruped robot motion planning device includes a non-volatile storage medium 3031 and internal memory 3032. The non-volatile storage medium stores the operating system and computer programs. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage medium. The database 307 of this height-adaptive quadruped robot motion planning device stores the corresponding data in this embodiment. The network interface 306 of this height-adaptive quadruped robot motion planning device is used for communication with external terminals via a network connection. The computer program is executed by the processor to implement the above-described method.

[0073] Those skilled in the art will understand that Figure 3 The structure shown is merely a block diagram of a portion of the structure related to the present invention and does not constitute a limitation on the height-adaptive quadruped robot motion planning device to which the present invention is applied.

[0074] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A height-adaptive motion planning method for a quadruped robot, characterized in that, Includes the following steps: Step S101: Based on the coordinate transformation relationship provided by the robot's underlying drive and SLAM algorithm, construct 3D and 2D local obstacle maps, calculate the vertical distance of each grid cell through ray projection, and generate a 2.5D local obstacle map containing plane occupancy information and z-axis gap height. Step S102: Define a four-dimensional control vector containing linear velocity, angular velocity and body height. Combine the robot's velocity, acceleration and height constraints, and determine the initial control sampling space through the constraint intersection. Step S103: Discretize the initial control sampling space and simulation time, derive the control value sequence, and recursively calculate the local trajectory including spatial coordinates, yaw angle and the body height using the omnidirectional kinematic model; Step S104: Based on the local trajectory, calculate the axis-aligned bounding box within the robot's directed bounding box, filter all grid cells within the axis-aligned bounding box, and compare the robot's height with the grid's allowable height to eliminate collision trajectories and obtain a set of feasible trajectories. Step S105: With the goal of maximizing the preset evaluation function, select the optimal trajectory from the set of feasible trajectories, extract the corresponding four-dimensional control vector as the optimal control value, and send it to the robot hardware for execution.

2. The motion planning method for a height-adaptive quadruped robot according to claim 1, characterized in that, Step S101 includes: Based on the coordinate transformation relationship provided by the robot's underlying drive and SLAM framework, a transformation relationship between the LiDAR coordinate system and the map coordinate system is established. Based on the aforementioned transformation relationship, the lidar point cloud is transformed into a map coordinate system, a 3D local obstacle map is constructed through light projection, and areas outside the set range are marked as free space. 3D point cloud is compressed along the z-axis to generate 2D point cloud. A 2D local obstacle map is constructed by ray projection, and areas outside the set range are marked as free space. An initial 2.5D local obstacle map is constructed by fusing the planar occupancy information of the 2D local obstacle map and the vertical height information of the 3D local obstacle map. For each grid cell in the initial 2.5D local obstacle map, the 2.5D local obstacle map is formed by combining its planar occupancy status and the vertical distance information of the obstacles above.

3. The method according to claim 2, characterized in that, If the point cloud density of the lidar is lower than a preset threshold, a frame interpolation mechanism is triggered to fill in the missing area by interpolating the point clouds of two adjacent frames, so as to ensure the integrity and accuracy of the 2.5D local obstacle map.

4. The motion planning method for a height-adaptive quadruped robot according to claim 1, characterized in that, Step S102 includes: Based on the robot's motion planning requirements, define the following: x Axial velocity, y The four-dimensional control vectors of axial velocity, angular velocity, and body height form the basic parameter framework of the control sampling space; Based on the robot's hardware performance parameters, the following settings are respectively configured: x Axial velocity, the aforementioned y The minimum and maximum values ​​of the axial velocity and the angular velocity are used to construct a first control sampling space subset under velocity constraints; Based on the robot's kinematic characteristics, set x Axial acceleration, y The maximum values ​​of axial acceleration and angular acceleration are combined with the current measured linear velocity and angular velocity values ​​to calculate the variable range of each velocity parameter during the simulation time, and to construct a second control sampling space subset under acceleration constraints. Based on the robot's mechanical structure parameters, the minimum and maximum heights of the robot's highest point from the ground are set, and a third control sampling space subset under height constraints is constructed. The initial control sampling space that simultaneously satisfies all three types of constraints is generated by taking the intersection of subsets of the control sampling space under velocity, acceleration, and height constraints.

5. The motion planning method for a height-adaptive quadruped robot according to claim 1, characterized in that, Step S103 includes: The initial control sampling space x Axis speed parameters y The axis velocity parameters, angular velocity parameters, and body height parameters are discretized to generate multiple discrete control vectors; The total time of trajectory simulation is discretized according to the preset elastic estimation step size, and the continuous simulation time is converted into multiple discrete time segments. For each discrete control vector, based on the robot's current measured velocity and acceleration constraints, the sequence of control values ​​corresponding to each time step is derived step by step according to the discrete time step. Based on the omnidirectional kinematics model, and combined with the control value sequence, the spatial coordinates and yaw angle changes corresponding to each time step are recursively calculated to generate a complete local trajectory.

6. The motion planning method for a height-adaptive quadruped robot according to claim 1, characterized in that, Step S104 includes: Initialize the collision state by setting the collision state of each of the local trajectories to non-collision. The TransformFootprint function is called to generate a directed bounding box in the map coordinate system for the pose of each point on the trajectory, and then the ComputeAABB function is used to obtain the axis-aligned bounding box. Enumerate all mesh cells within the axis-aligned bounding box, and retain the cells located inside the directed bounding box as determined by PointInOBB; The permissible height of the reserved unit is read from the 2.5D local obstacle map and compared with the body height of the current trajectory one by one. If the body height is greater than or equal to the permissible height of any unit, the trajectory is marked as a height collision. All trajectories marked as high-collision are removed, and the remaining trajectories constitute the set of feasible trajectories.

7. The height-adaptive quadruped robot motion planning method according to claim 1, characterized in that, Step S105 includes: A comprehensive evaluation function is constructed that includes target tracking, path following, obstacle avoidance distance, and height adjustment terms. The height adjustment term is based on the robot's normal gait height and is used to quantify the deviation between the current height and the normal height. For each trajectory in the set of feasible trajectories, calculate its corresponding comprehensive evaluation function value. By comparing the comprehensive evaluation function values ​​of all trajectories, select the trajectory with the highest comprehensive evaluation function value as the optimal trajectory. Extract the four-dimensional control vector corresponding to the optimal trajectory as the optimal control value; The optimal control value is sent to the quadruped robot hardware, driving the robot to move along the optimal trajectory and complete a single planning closed loop.

8. The motion planning method for a height-adaptive quadruped robot according to claim 1, characterized in that, Step S105 further includes: The deviation between the robot's actual height and the expected height in the optimal trajectory is monitored in real time. When the deviation continues to exceed the tolerance range, the motion planning process is immediately restarted to achieve dynamic replanning.

9. A height-adaptive quadruped robot motion planning system, characterized in that, The system is used to implement a height-adaptive quadruped robot motion planning method as described in any one of claims 1 to 8, the system comprising: The map generation module receives the coordinate transformation relationship between the robot's underlying drive and the SLAM algorithm, constructs 3D and 2D local obstacle maps, calculates the vertical distance of the grid through ray projection, and generates a 2.5D local obstacle map including the plane occupancy and z-axis gap height. The sampling space determination module is used to define a four-dimensional control vector containing linear velocity, angular velocity, and body height. Combined with robot velocity, acceleration, and height constraints, the initial control sampling space is determined through the constraint intersection. The local trajectory extrapolation module is used to discretize the initial control sampling space and simulation time, derive the control value sequence, and recursively extrapolate the local trajectory containing spatial coordinates, yaw angle, and body height using an omnidirectional kinematic model. The collision trajectory elimination module is used to obtain local trajectories, calculate the axis alignment of the robot within the bounding box, filter all grid cells within the axis alignment bounding box, compare the robot height with the allowed height of the grid, eliminate collision trajectories, and obtain a set of feasible trajectories. The optimal control output module is used to select the optimal trajectory from the set of feasible trajectories with the goal of maximizing the preset evaluation function, extract the corresponding four-dimensional control vector as the optimal control value, and send it to the robot hardware.

10. A height-adaptive quadruped robot motion planning device, characterized in that, It includes a memory and a processor, the memory storing a computer program that can run on the processor, and the processor executing the computer program to implement a height-adaptive quadruped robot motion planning method according to any one of claims 1 to 8.

Images