A quadruped robot energy-saving planning and control collaborative optimization method and device for unstructured environment and robot thereof
By constructing a global energy cost map and optimizing the path planning and motion control of the quadruped robot using deep reinforcement learning, the problem of insufficient endurance in unstructured environments was solved, resulting in a significant reduction in energy consumption and an improvement in environmental adaptability.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- JIANGSU UNIV
- Filing Date
- 2026-03-20
- Publication Date
- 2026-06-16
AI Technical Summary
Quadruped robots have insufficient endurance in unstructured environments, and existing path planning and motion control methods have failed to effectively reduce energy consumption, resulting in increased actual energy consumption.
A global energy cost map is constructed, and path planning is performed using an improved A* algorithm. The lower-level local motion controller is trained through deep reinforcement learning to optimize gait and reduce energy consumption.
It significantly reduces the energy consumption of robots in unstructured environments, improves endurance and environmental adaptability, and solves the problem of increased energy consumption in traditional methods.
Smart Images
Figure CN122219243A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of mobile robot path planning and motion control technology, specifically to a method, device, and robot for energy-saving planning and control collaborative optimization of quadruped robots in unstructured environments. Background Technology
[0002] Quadruped robots, with their unique legged structure, possess superior mobility and adaptability in unstructured terrain, and are widely used in fields such as field exploration, disaster relief, and material transportation. However, limited by the energy density of onboard batteries and the high power consumption of multi-joint motors, insufficient endurance has become a key bottleneck restricting quadruped robots from performing long-distance tasks. Therefore, how to reduce the robot's cost of transport (COT) through algorithm optimization has become a current research hotspot.
[0003] In existing technologies, navigation and control of quadruped robots typically employ a hierarchical architecture, but this presents several major problems: First, the upper-level path planning is disconnected from actual energy consumption. Existing global path planning methods (such as the traditional A* algorithm and Dijkstra's algorithm) usually optimize for the shortest path length or time. This strategy is effective in flat environments, but in unstructured environments with slopes or rough terrain, the geometrically shortest path often means traversing steep inclines or high-resistance areas, leading to a sharp increase in actual energy consumption. Second, the lower-level motion control lacks explicit energy optimization. Existing controllers based on model predictive control (MPC) or reinforcement learning primarily focus on motion stability and velocity tracking accuracy, failing to accurately establish a power model that includes motor heat loss. This results in the generated gait not being energy-optimal for long-distance walking. Third, there is a lack of coordination mechanisms between the upper and lower layers. The upper-level planner typically simplifies the robot to a point mass, failing to perceive the actual energy consumption cost of the lower-level controller under different terrains. This leads to the "theoretically optimal path" being a high-energy-consuming path in actual execution.
[0004] To address the aforementioned issues, a collaborative optimization method is needed that combines the macroscopic energy consumption perspective of upper-level global planning with the microscopic energy-saving gait of lower-level local control, in order to minimize the robot's system energy consumption while ensuring motion stability. Summary of the Invention
[0005] This invention provides a collaborative optimization method for energy-saving planning and control of quadruped robots in unstructured environments. The specific solution is as follows:
[0006] A collaborative optimization method for energy-saving planning and control of quadruped robots in unstructured environments, the method comprising the following steps:
[0007] Step 1: The environmental map is processed offline to generate a global elevation raster map. The terrain feature parameters of the raster are extracted and combined with the energy consumption weight coefficients pre-calibrated by the lower-level controller to construct a static global energy cost map.
[0008] Step 2: Based on the global energy cost map, define an evaluation function that includes cumulative energy consumption cost and heuristic estimated cost, use the improved A* algorithm to search for the global energy-optimal path, and generate the corresponding speed command sequence;
[0009] Step 3: Construct an instantaneous power model containing the quadruped robot and introduce it as a negative reward into a deep reinforcement learning framework. Train the lower-level local motion controller in a simulation environment so that it learns the energy-optimal gait.
[0010] Step 4: Migrate and deploy the trained lower-level local motion controller to the entity, receive the velocity command sequence generated in Step 2, and drive the robot to move along the planned energy-optimal path.
[0011] The specific implementation method of step 1 includes:
[0012] Step 1.1 Construct a global elevation raster map The detection area is divided into rectangular grid (where and (representing the length and width of the physical environment along the X and Y axes of the world coordinate system, respectively), setting the raster resolution to... For any grid node in the map coordinate system (in and (Representing the x and y coordinates of the node in a two-dimensional planar grid matrix, respectively), and its elevation value. Take all point cloud data within the grid area Average or maximum value of axis coordinates;
[0013] Step 1.2 Calculate the terrain slope angle for each grid node in the global map. To improve computational robustness, a local plane fitting method is employed: using the current grid node... Centered on, select or Using a neighborhood window, the local ground plane equation is fitted using the least squares method. (in To fit the normal vector components of the plane, (This is a constant term in the plane equation), which gives the terrain normal vector at that location. Then the slope angle of the grid node relative to the horizontal plane. The calculation formula is:
[0014]
[0015] In the formula, The coefficients of the fitted plane, .
[0016] Step 1.3 Calculate the terrain roughness of each grid node in the global map. This is used to quantify the degree of terrain undulation. The terrain roughness is defined as the residual standard deviation of the elevation values of each grid cell within the neighborhood window relative to the fitted plane described in step S1.2, and its calculation formula is:
[0017]
[0018] In the formula, This represents the total number of grid cells within the neighboring window. For the neighboring region The actual elevation value of each grid cell. This is the theoretical elevation value of the grid point projected onto the fitting plane.
[0019] Step 1.4 Establish an energy movement cost model between grids and generate a global energy cost map. Define the robot's starting point from the current node. Move to adjacent node Normalized energy cost The calculation formula is as follows:
[0020]
[0021] In the formula: The three-dimensional Euclidean distance between two nodes is calculated as follows: = ; These are the base energy consumption weight, slope energy consumption weight, and roughness energy consumption weight, respectively, and the weights are obtained through offline simulation calibration. The directional slope angle along the direction of movement; The terrain roughness of the target node; This is an asymmetric slope energy consumption factor function, used to characterize the energy consumption difference between uphill and downhill slopes. Its calculation formula is as follows:
[0022]
[0023] in, This is the downhill energy correction factor, with a range of values. ; For the current node Elevation value; Adjacent nodes The elevation value.
[0024] For nonlinear roughness drag factor function:
[0025]
[0026] in, This is the roughness sensitivity coefficient.
[0027] The specific implementation methods in step 2 include:
[0028] Step 2.1. Establish a path evaluation function based on energy consumption. Unlike the traditional A* algorithm, which only considers path length as the cost, this invention uses the quadruped robot's cumulative energy consumption as the core optimization metric. For any node in the global energy cost map... Its evaluation function is defined as:
[0029]
[0030] In the formula: Let be the actual energy consumption cost function, representing the energy consumption cost from the starting node. Reach the node along the currently generated path Cumulative energy consumption; Let be the heuristic energy consumption prediction function, representing the function from node . To the target node The estimated minimum energy consumption.
[0031] Step 2.2. Calculate the actual energy consumption cost function This value is calculated recursively, assuming a node... The parent node is Then its recursive calculation formula is:
[0032]
[0033] In the formula, ; This is the energy cost of a single-step movement between the parent node and the current node, obtained from the global energy cost map described in step 1. This formula ensures that the path search process strictly adheres to the physical energy consumption constraints imposed by terrain slope and roughness.
[0034] Step 2.3. Construct a heuristic energy consumption prediction function based on physical priors. To ensure the optimality of the global search and improve the convergence speed, the minimum transportation cost of a quadruped robot in ideal flat terrain is utilized. Based on prior physical knowledge, a heuristic energy consumption prediction function is constructed:
[0035]
[0036] In the formula, For the current node The three-dimensional Euclidean distance between the target node and the target node; For the overall quality of the robot; It is the acceleration due to gravity; The minimum dimensionless transportation cost for the robot to walk at an economical speed on a smooth, level surface is obtained through offline simulation calibration. These are heuristic weighting coefficients used to adjust the algorithm's sensitivity to goal orientation.
[0037] Step 2.4. Perform path search and trajectory smoothing. Construct an open list. It is used to store information about candidate raster nodes that have been discovered but have not yet been expanded by cost evaluation, constructing a closed list. It is used to store information about raster nodes that have undergone cost evaluation and will no longer be searched repeatedly, and is selected cyclically. The smallest node is expanded, and its corresponding parent node pointer is recorded when expanding adjacent nodes; the search stops when the current expanded node is the target node. Starting from the target node, the search backtracks along the recorded parent node pointers to the starting node, reversing the coordinates of the nodes along the way, thus generating the original path sequence composed of the center points of the discrete raster. Since the original path is a polygonal line, it would cause frequent acceleration and deceleration during robot movement, resulting in additional current surge losses. Therefore, the B-spline curve (B-Spline) algorithm is adopted. The discrete path points obtained from the backtracking are used as control vertices. By setting the basis functions of the B-spline and performing smooth interpolation fitting with the node vectors, the sharp angles in the original polygonal path are removed, thereby generating an energy-optimal reference trajectory with continuous curvature. And calculate the reference linear velocity of each point on the trajectory based on the robot's dynamic constraints. With angular velocity , as input commands for the lower-level controller.
[0038] The specific implementation method of step 3 includes:
[0039] Step 3.1: Construct a Markov Decision Process (MDP) for reinforcement learning. The motion control problem of the quadruped robot is modeled as a process consisting of tuples... Defined decision-making process.
[0040] state space It not only includes the robot's body perception information (base linear velocity, angular velocity, gravity vector, joint position and velocity), but also introduces a local terrain elevation map as an external perception input, enabling the lower-level local motion controller to perceive the terrain undulations under its feet (such as slope and steps), thereby making predictive energy-saving action adjustments.
[0041] Action space : Defined as the target position increment of a quadruped robot's four legs (left front, right front, left rear, and right rear), comprising a total of 12 joint motors (each leg includes a lateral swing joint, hip joint, and knee joint). Compared to directly controlling torque, the position control mode can utilize the stiffness characteristics of the low-level PD controller, resulting in higher stability during Sim-to-Real migration.
[0042] matrix : Represents the state transition probability matrix of the system under given states and actions.
[0043] reward function : Represents the reward function for environmental feedback.
[0044] Discount factor : This represents the discount factor used to weigh the current reward against the future reward.
[0045] Step 3.2: Establish a high-precision instantaneous power model for the quadruped robot. To overcome the shortcomings of traditional methods that only use the square of torque to roughly estimate energy consumption, this invention constructs a composite power model that includes mechanical work and Joule heat loss of the motor, serving as the physical benchmark for energy consumption optimization.
[0046]
[0047] In the formula, Indicates the first One joint motor; and These are the real-time output torque and angular velocity of the joint, respectively. For mechanical output power, adopt The operator means that only the energy consumption when the motor outputs positive power is counted, and the braking energy recovery is ignored (which is in line with the current hardware characteristics of quadruped robot actuators). Joule heat loss is caused by the internal resistance of the motor coil. The heat loss coefficient is related to the motor resistance and torque constant, and this coefficient is obtained by bench testing and calibration of the robot joint module.
[0048] Step 3.3: Design an energy-sensitive composite reward function To guide the policy network to minimize energy consumption while satisfying motion instructions, the reward function consists of a task reward term and an energy consumption penalty term:
[0049]
[0050] In the formula, The speed tracking reward is used to encourage the robot to follow the reference speed command generated in step 2 as accurately as possible; As an attitude stability reward, it is used to penalize unintended tilting of the fuselage; The core energy consumption penalty item, weight This determines the level of importance attached to energy conservation; As a penalty for motion smoothness, This indicates the weight of the smoothness penalty term for the action. and These represent the policy network in the current control period (time). ) and the previous control cycle (time) The output motion command (i.e., the target position increment of the 12 joint motors) further reduces the ineffective energy consumption of the motor due to frequent acceleration and deceleration by suppressing the high-frequency jitter of the motion command.
[0051] Step 3.4: Perform policy network training and Sim-to-Real transfer. The policy network for solving the Markov decision process is trained in large-scale parallel within a physical simulation environment using the Proximal Policy Optimization (PPO) algorithm. This policy network is used to establish the policy network from the state space. To the action space The nonlinear mapping relationship is used, and the trained entity serves as the lower-level local motion controller. To bridge the "reality gap" between simulation and the real environment, domain randomization is introduced during training: random perturbations are applied to the robot's physical parameters (such as link mass, center of mass position, and motor friction) and environmental parameters (such as ground friction coefficient and terrain noise). After hundreds of millions of iterations of training, a policy neural network with strong robustness and optimal energy is finally obtained. It serves as a lower-level local motion controller and is directly deployed to the onboard computer of the physical robot for inference control.
[0052] The specific implementation method of step 4 includes:
[0053] Step 4.1. Build the airborne hierarchical control system architecture. Construct a ROS 2-based hierarchical control system on the quadruped robot's onboard computer (such as the NVIDIA Jetson Orin platform).
[0054] Upper-level planning node: Run the improved A* algorithm described in step 2 to replan the global energy-optimal path based on real-time positioning information at a lower frequency (e.g., 1-2 Hz) and dynamically update the local reference trajectory;
[0055] Lower-level inference nodes: Deploy the policy neural network trained in step S3 Edge acceleration is achieved using TensorRT or ONNXRuntime to infer joint control signals driven by upper-layer instructions in real time at high frequencies (e.g., 50 Hz).
[0056] Step 4.2. Design instruction mapping logic based on path tracing. This is to map the global geometric path (i.e., the energy-optimal trajectory) generated by the upper layer. The observed speed commands are converted into the values required by the lower-level controller. Design a local guidance law. Calculate the robot's current position. The nearest target point on the trajectory Positional deviation between and heading deviation Generate the desired velocity and desired angular velocity commands:
[0057]
[0058]
[0059] In the formula, This is the rotation matrix from the world coordinate system to the fuselage coordinate system; To track gain; This is the feedforward velocity term calculated based on the curvature of the B-spline trajectory. This instruction is input into the policy network as a "User Command" vector.
[0060] Step 4.3. Execute Sim-to-Real policy reasoning and underlying driver. Read onboard IMU data (Euler angles, angular velocity) and joint encoder data (joint positions) through the robot's underlying development interface (such as Unitree SDK2). ,speed Combined with the local elevation map constructed by the airborne sensing system, a state vector is assembled. Input Policy Network Policy Network Output the target position increments of 12 joints Superimposed with the robot's nominal standing posture The final target joint angle is calculated. :
[0061]
[0062] In the formula, This is the action scaling factor.
[0063] Step 4.4. Closed-loop torque control and energy consumption monitoring. The calculated target joint angle... Field Oriented Control (FOC) actuators are transmitted to each joint via a high-bandwidth communication bus (Ethernet / EtherCAT). The actuators internally operate a high-frequency (>10 kHz) proportional-derivative (PD) control loop.
[0064]
[0065] In the formula, The target torque output by the motor. This is the proportional stiffness coefficient. The differential damping coefficient is... and These represent the expected joint position and expected joint angular velocity output by the policy network inference, respectively. and This provides feedback on the current actual joint position and actual joint angular velocity from the joint encoder.
[0066] The drive motor performs actions, while the voltage and current data fed back by the battery management system (BMS) are monitored in real time to calculate the actual cost of transport (COT) and feed it back to the upper-level planning node for online correction of the energy consumption weight coefficient in step 1, forming a complete closed loop of "perception-planning-control-evaluation".
[0067] The beneficial effects of this invention are:
[0068] 1. Reduced global transportation costs for robots. This invention constructs a global energy cost map that includes terrain slope and roughness features, and uses an improved A* algorithm with COT physical priors for planning, enabling robots to actively avoid high-energy-consuming terrains such as steep slopes and rugged roads. Compared with traditional geometric shortest path planning, this significantly reduces the overall energy consumption of the robot.
[0069] 2. Improved energy efficiency of local gait. This invention establishes an instantaneous power model that includes mechanical work and motor Joule heat loss, and introduces it as a negative reward into deep reinforcement learning training to guide the lower-level controller to learn an energy-saving gait with low torque and low heat loss, thus solving the problem of neglecting motor heat loss in traditional methods.
[0070] 3. Enhanced environmental adaptability and robustness of the robot. This invention introduces a local terrain elevation map into the controller state space and combines it with domain randomization technology for training, enabling the robot to perceive the undulations of the terrain beneath its feet and make predictive adjustments, effectively solving the adaptability problem when simulation strategies are transferred to real unstructured environments. Attached Figure Description
[0071] Figure 1 This is the overall flowchart of the present invention.
[0072] Figure 2 This is a schematic diagram of the hierarchical control system architecture based on ROS 2 in this invention.
[0073] Figure 3 This is a comparison chart of path planning and energy consumption indicators between the traditional algorithm and the algorithm of this invention in unstructured terrain in the embodiments of this invention. Detailed Implementation
[0074] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.
[0075] Hardware and Experimental Environment Setup: This embodiment selects the Unitree Go2-EDU quadruped robot as the physical experimental platform. The onboard computing unit uses NVIDIA Jetson Orin NX, which is responsible for running the upper-layer planning algorithm and the inference of the lower-layer policy network. The software environment is based on the Ubuntu 22.04 operating system and the ROS 2 (Humble) middleware architecture for communication management. Deep reinforcement learning training is completed on the workstation, and the simulation environment uses NVIDIA Isaac Sim5.1.
[0076] like Figure 1 As shown, this invention provides a collaborative optimization method for energy-saving planning and control of quadruped robots in unstructured environments.
[0077] The specific implementation process of this embodiment includes the following four main steps:
[0078] Implementation Step 1: Offline construction of a global energy cost map; such as Figure 1 As shown in step S1, this embodiment first imports pre-built elevation map information of the operating environment, which is stored as prior knowledge in the system storage unit. A global energy cost map is then constructed through offline calculation. The specific process is as follows:
[0079] S1.1 Constructing a Global Elevation Raster Map The detection area is divided into A rectangular grid (e.g., setting the test area as...) ), set the raster resolution to For any grid node in the map coordinate system Its elevation value Take all point cloud data within the grid area Average or maximum value of axis coordinates;
[0080] S1.2 Calculate the terrain slope angle of each grid node in the global map. To improve computational robustness, a local plane fitting method is employed: using the current grid node... Centered on, select or Using a neighborhood window, the local ground plane equation is fitted using the least squares method. This yields the terrain normal vector at that location. Then the slope angle of the grid node relative to the horizontal plane. The calculation formula is:
[0081]
[0082] In the formula, The coefficients of the fitted plane, .
[0083] S1.3 Calculate the terrain roughness of each grid node in the global map. This is used to quantify the degree of terrain undulation. The terrain roughness is defined as the residual standard deviation of the elevation values of each grid cell within the neighborhood window relative to the fitted plane described in step S1.2, and its calculation formula is:
[0084]
[0085] In the formula, This represents the total number of grid cells within the neighboring window. For the neighboring region The actual elevation value of each grid cell. This is the theoretical elevation value of the grid point projected onto the fitting plane.
[0086] S1.4 Establish an energy movement cost model between grids and generate a global energy cost map. By fusing slope and roughness weights, the original elevation grid is transformed into a cost map that characterizes energy consumption levels; darker colors represent higher energy costs in that area (e.g., steep slopes or obstacle zones). The robot's starting point is defined from the current node. Move to adjacent node Normalized energy cost The calculation formula is as follows:
[0087]
[0088] In the formula: The three-dimensional Euclidean distance between two nodes is calculated as follows: = ; These are the base energy consumption weight, slope energy consumption weight, and roughness energy consumption weight, respectively, and the weights are obtained through offline simulation calibration. The directional slope angle along the direction of movement; The terrain roughness of the target node; This is an asymmetric slope energy consumption factor function, used to characterize the energy consumption difference between uphill and downhill slopes.
[0089]
[0090] in, This is the downhill energy correction factor, with a range of values. ; For nonlinear roughness drag factor function:
[0091]
[0092] in, This is the roughness sensitivity coefficient.
[0093] Implementation Step 2: Global Energy Optimal Planning Based on the Improved A* Algorithm. Based on the global energy cost map constructed above, execute... Figure 1 Step S2. Compared to the traditional geometric shortest path that directly traverses high-energy-consuming areas, the path planned by the improved A* algorithm in this invention can actively bypass steep slopes and rugged areas, achieving optimal energy efficiency.
[0094] S2.1 Establish a path evaluation function based on energy consumption Unlike the traditional A* algorithm, which only considers path length as the cost, this invention uses the quadruped robot's cumulative energy consumption as the core optimization metric. For any node in the global energy cost map... Its evaluation function is defined as:
[0095]
[0096] In the formula: Let be the actual energy consumption cost function, representing the energy consumption cost from the starting node. Reach the node along the currently generated path Cumulative energy consumption; Let be the heuristic energy consumption prediction function, representing the function from node . To the target node The estimated minimum energy consumption.
[0097] S2.2 Calculate the actual energy consumption cost function This value is calculated recursively, assuming a node... The parent node is Then its recursive calculation formula is:
[0098]
[0099] In the formula, ; This is the energy cost of a single-step movement between the parent node and the current node, obtained from the global energy cost map described in step 1. This formula ensures that the path search process strictly adheres to the physical energy consumption constraints imposed by terrain slope and roughness.
[0100] S2.3 Constructing a heuristic energy consumption prediction function based on physical priors To ensure the optimality of the global search and improve the convergence speed, the minimum transportation cost of a quadruped robot in ideal flat terrain is utilized. As prior physical knowledge, construct a heuristic function:
[0101]
[0102] In the formula, For the current node The three-dimensional Euclidean distance between the target node and the target node; For the overall quality of the robot; It is the acceleration due to gravity; The minimum dimensionless transportation cost for the robot to walk at an economical speed on a smooth, level surface is obtained through offline simulation calibration. These are heuristic weighting coefficients used to adjust the algorithm's sensitivity to goal orientation.
[0103] S2.4 performs path search and trajectory smoothing. It maintains an open list ( ) and a closed list ( ), select cyclically Expand from the smallest node until the target node is reached, backtracking to generate the original path sequence composed of the center points of the discrete grid. Since the original path is a polygonal line, it causes frequent acceleration and deceleration during robot movement, resulting in additional current surge losses. Therefore, B-spline curves (B-Spline) are used to smooth the original path and interpolate it to generate an energy-optimal trajectory with continuous curvature. And calculate the reference linear velocity of each point on the trajectory based on the robot's dynamic constraints. With angular velocity , as input commands for the lower-level controller.
[0104] Example 3: Training the energy-optimal local controller based on deep reinforcement learning. Figure 1 Step S3. This invention constructs a deep reinforcement learning training framework that includes environmental interaction, state awareness, policy network inference, and reward calculation based on a power model, and trains the lower-level controller in the NVIDIA Isaac Sim physics simulation environment.
[0105] S3.1 Constructing a Markov Decision Process (MDP) for reinforcement learning. The motion control problem of the quadruped robot is modeled as a problem involving tuples. Defined decision-making process.
[0106] state space This includes the robot's base linear velocity, angular velocity, gravity vector, joint positions and velocities, and a local height map. The local height map is generated by analyzing the area around the robot. The terrain within the range was sampled and obtained at a resolution of [resolution missing]. .
[0107] Action space Defined as the target position increment of 12 joint motors. .
[0108] Policy Network Receive status input including local terrain elevation map and output the incremental motion of the joint position. The PPO algorithm optimizes network parameters in real time based on the feedback reward function.
[0109] S3.2 Establish a high-precision instantaneous power model for the quadruped robot. This embodiment abandons the traditional sum-of-squares torque model and constructs a composite power model that includes mechanical work and Joule heat loss:
[0110]
[0111] In the formula, Indicates the first One joint motor; and These are the real-time output torque and angular velocity of the joint, respectively. For mechanical output power, adopt Operator representations only count positive work; Joule heat loss is caused by the internal resistance of the motor coil. This is the heat loss coefficient. Based on the measured parameters of the Go2 robot's joint motors (phase resistance)... torque constant ), calculated .
[0112] S3.3 Design an energy-sensitive composite reward function To guide the policy network to minimize energy consumption while satisfying motion instructions, the reward function consists of a task reward term and an energy consumption penalty term:
[0113]
[0114] In the formula, The speed tracking reward is used to encourage the robot to follow the reference speed command generated in step 2 as accurately as possible; As an attitude stability reward, it is used to penalize unintended tilting of the fuselage; The core energy consumption penalty item, weight This determines the level of importance attached to energy conservation; As a penalty for motion smoothness, it further reduces the ineffective energy consumption of the motor caused by frequent acceleration and deceleration by suppressing high-frequency jitter of motion commands.
[0115] S3.4 Execution Policy Training and Sim-to-Real Transfer. The Proximal Policy Optimization (PPO) algorithm was used for parallel training in Isaac Sim across 4096 environments. Domain randomization was introduced to optimize the robot's link mass. Random disturbances, the coefficient of ground friction is Random sampling is performed. After 500 million steps (500M steps) of iterative training, the policy network is finally obtained. .
[0116] Example 4: Sim-to-Real Deployment and Collaborative Control of the Layered System Figure 1 As shown in step S4, the trained module is deployed to the physical robot. Figure 2 This embodiment demonstrates a hierarchical control system architecture based on ROS 2, forming a closed-loop system that includes upper-level planning and lower-level reasoning.
[0117] S4.1 Build an airborne hierarchical control system architecture. Build a ROS 2-based hierarchical control system on the quadruped robot's onboard computer (such as the NVIDIA Jetson Orin platform).
[0118] Upper-level planning node: Run the improved A* algorithm described in step S2, replan the global energy-optimal path at a frequency of 2 Hz based on real-time positioning information, and dynamically update the local reference trajectory;
[0119] Lower-level inference nodes: Deploy the policy neural network trained in step S3 It utilizes the TensorRT engine for acceleration, enabling real-time inference of joint control signals driven by upper-level instructions at a frequency of 50 Hz.
[0120] S4.2 designs instruction mapping logic based on path tracing. This is to map the global geometric path generated by the upper layer... The speed command observations are converted into the values required by the lower-level controller. Design a local guidance law. Calculate the robot's current position. The nearest target point on the trajectory Positional deviation between and heading deviation Generate the desired speed command:
[0121]
[0122]
[0123] In the formula, This is the rotation matrix from the world coordinate system to the fuselage coordinate system; To track gain; This is the feedforward velocity term calculated based on the curvature of the B-spline trajectory. This instruction is input into the policy network as a "User Command" vector.
[0124] S4.3 Executes Sim-to-Real policy reasoning and low-level driving. It reads onboard IMU data and joint encoder data through the robot's low-level development interface (Unitree SDK2); simultaneously, it uses the onboard Unitree 4DLiDAR L1 to scan the terrain around the robot in real time, constructing a local height map centered on the robot through rasterization. The above-mentioned body state and local terrain information are then assembled into a state vector. Input policy network.
[0125]
[0126] This drives the robot to move along the energy-optimal path.
[0127] The trained strategy was deployed to the robot. To verify the effectiveness of the invention, a comparative experiment was conducted in a simulation environment, and the results are as follows. Figure 3 As shown. Figure 3The figure above shows the path planning results. The dashed line represents the path planned by the traditional A* algorithm based on the shortest distance, which directly traverses the high-slope area. The solid line represents the path planned by the algorithm of this invention, in which the robot successfully identifies and bypasses the high-energy-consuming steep slope area. Figure 3 The figure illustrates a quantitative comparison of terrain elevation and energy consumption characteristics along the route. As can be seen from the figure, although the path length is slightly increased compared to traditional algorithms, the algorithm of this invention significantly reduces transportation costs (COT), achieving the expected energy-saving goal.
[0128] The embodiments of the present invention also include: a control device for a quadruped robot, wherein the control device deploys the aforementioned quadruped robot energy-saving planning and control collaborative optimization method for unstructured environments.
[0129] The embodiments of the present invention also include: a quadruped robot, which operates using the aforementioned quadruped robot energy-saving planning and control collaborative optimization method for unstructured environments.
[0130] The detailed descriptions listed above are merely specific descriptions of feasible embodiments of the present invention, and are not intended to limit the scope of protection of the present invention. All equivalent methods or modifications that do not depart from the technology of the present invention should be included within the scope of protection of the present invention.
Claims
1. A collaborative optimization method for energy-saving planning and control of quadruped robots in unstructured environments, characterized in that, include: Step 1: The environmental map is processed offline to generate a global elevation raster map. The terrain feature parameters of the raster are extracted and combined with the energy consumption weight coefficients pre-calibrated by the lower-level controller to construct a static global energy cost map. Step 2: Based on the global energy cost map, define an evaluation function that includes cumulative energy consumption cost and heuristic estimated cost, use the improved A* algorithm to search for the global energy-optimal path, and generate the corresponding speed command sequence; Step 3: Construct the instantaneous power model of the quadruped robot and introduce it as a negative reward into the deep reinforcement learning framework. Train the lower-level local motion controller in the simulation environment so that it learns the energy-optimal gait. Step 4: Based on the trained lower-level local motion controller, receive the velocity command sequence generated in Step 2 and drive the robot to move along the planned energy-optimal path.
2. The method for collaborative optimization of energy-saving planning and control of quadruped robots in unstructured environments according to claim 1, characterized in that, The specific implementation of step 1 includes: Step 1.1 Construct a global elevation raster map The detection area is divided into multiple rectangular grids, and the grid resolution is set to [value missing]. For any grid node in the map coordinate system Its elevation value Take all point cloud data within the grid area Average or maximum value of axis coordinates; Step 1.2 Calculate the terrain slope angle of each grid node in the global map using the local plane fitting method. : With the current grid node Centered on, select or Using a neighborhood window, the local ground plane equation is fitted using the least squares method. This yields the terrain normal vector at that location. Then the slope angle of the grid node relative to the horizontal plane. The calculation formula is: In the formula, The coefficients of the fitted plane, ; Step 1.3 Calculate the terrain roughness of each grid node in the global map. This is used to quantify the degree of undulation of the ground; the terrain roughness is defined as the residual standard deviation of the elevation values of each grid within the neighborhood window relative to the ground plane in step S1.2, and its calculation formula is: In the formula, This represents the total number of grid cells within the neighboring window. For the neighboring region The actual elevation value of each grid cell. This is the theoretical elevation value of the grid point projected onto the fitting plane; Step 1.4 Establish an energy movement cost model between grids and generate a global energy cost map. Define the robot starting from the current node. Move to adjacent node Normalized energy cost The calculation formula is as follows: In the formula: The three-dimensional Euclidean distance between two nodes is calculated as follows: ; These are the base energy consumption weight, slope energy consumption weight, and roughness energy consumption weight, respectively. The directional slope angle along the direction of movement; The terrain roughness of the target node; This is an asymmetric slope energy consumption factor function, used to characterize the energy consumption difference between uphill and downhill slopes. Its calculation formula is as follows: in, This is the downhill energy correction factor, with a range of values. ; The roughness drag factor is a nonlinear roughness drag factor function, and its calculation formula is as follows: in, This is the roughness sensitivity coefficient.
3. The method for collaborative optimization of energy-saving planning and control of quadruped robots in unstructured environments according to claim 2, characterized in that, The specific implementation of step 2 includes: Step 2.
1. Establish a path evaluation function based on energy consumption. Using the cumulative energy consumption of the quadruped robot as the core optimization metric, for any node in the global energy cost map... Its evaluation function is defined as: In the formula: Let be the actual energy consumption cost function, representing the energy consumption cost from the starting node. Reach the node along the currently generated path Cumulative energy consumption; Let be the heuristic energy consumption prediction function, representing the function from node . To the target node The estimated minimum energy consumption; Step 2.
2. Calculate the actual energy consumption cost function This value is calculated recursively, with nodes set as follows: The parent node is Then its recursive calculation formula is: In the formula, ; The energy cost of a single-step movement between the parent node and the current node is obtained from the global energy cost map described in step 1. Step 2.
3. Construct a heuristic function based on physical priors Using the minimum transportation cost of a quadruped robot in ideal flat terrain as prior physical knowledge, a heuristic function is constructed: In the formula, For the current node The three-dimensional Euclidean distance between the target node and the target node; For the overall quality of the robot; It is the acceleration due to gravity; The minimum dimensionless transportation cost when the robot moves at an economical speed on a smooth, level surface. These are heuristic weighting coefficients used to adjust the algorithm's sensitivity to target orientation; Step 2.
4. Perform global energy-optimal path search and trajectory smoothing; construct an open list. and a closed list Open list A closed list used to store information about candidate raster nodes that have been discovered but not yet expanded by cost evaluation. Used to store information about raster nodes that have undergone cost evaluation and will no longer be searched repeatedly, selected cyclically. Expand from the smallest node until the target node is reached, backtracking to generate the original path sequence composed of the center points of the discrete grid. The original path sequence is smoothed by interpolating B-spline curves to generate an energy-optimal trajectory with continuous curvature. And calculate the reference linear velocity of each point on the trajectory based on the robot's dynamic constraints. With angular velocity , as input commands for the lower-level controller.
4. The method for collaborative optimization of energy-saving planning and control of quadruped robots in unstructured environments according to claim 3, characterized in that, In step 2.4, the specific process of generating the original path sequence is as follows: Cyclic selection The smallest node is expanded, and its corresponding parent node pointer is recorded when expanding adjacent nodes; the search stops when the current expanded node is the target node. Starting from the target node, the search backtracks to the starting node along the recorded parent node pointers, reversing the coordinates of the nodes along the way, and finally generating the original path sequence composed of the center points of the discrete raster. .
5. The method for collaborative optimization of energy-saving planning and control of quadruped robots in unstructured environments according to claim 4, characterized in that, In step 2.4, the specific process of generating the energy-optimal trajectory is as follows: Using the discrete path points obtained through backtracking as control vertices, a smooth interpolation fitting is performed by setting the basis functions of the B-spline and the node vectors to remove sharp angles in the original polyline path, generating an energy-optimal reference trajectory with continuous curvature. .
6. The method for collaborative optimization of energy-saving planning and control of quadruped robots in unstructured environments according to claim 5, characterized in that, The specific implementation of step 3 includes: Step 3.1: Construct a Markov decision mechanism based on reinforcement learning; model the motion control problem of the quadruped robot as a problem involving tuples. The defined decision process, wherein: state space This includes the robot's own perception information (base linear velocity, angular velocity, gravity vector, joint position and velocity), as well as external perception input information (local terrain elevation map), enabling the controller to sense the terrain undulations under its feet and make predictive energy-saving adjustments. Action space : Defined as the target position increment of 12 joint motors. The 12 joint positions are defined as: the left front, right front, left rear, and right rear upper of the four legs of the quadruped robot, a total of 12 joint motors, each leg including the lateral swing joint, hip joint and knee joint; Step 3.2: Establish the instantaneous power model of the quadruped robot. This instantaneous power model includes a composite power model of mechanical work and motor Joule heat loss, which serves as the physical benchmark for energy consumption optimization. The model formula is as follows: In the formula, Indicates the first One joint motor; and These are the real-time output torque and angular velocity of the joint, respectively. For mechanical output power, adopt The operator indicates that only the energy consumption when the motor outputs positive power is counted, ignoring braking energy recovery; This refers to the Joule heat loss caused by the internal resistance of the motor coil. The heat loss coefficient is related to the motor resistance and torque constant. Step 3.3: Design the composite reward function for the energy-sensitive policy network. To guide the policy network to minimize energy consumption while satisfying motion instructions, the reward function consists of a task reward term and an energy consumption penalty term: In the formula, The speed tracking reward is used to encourage the robot to follow the reference speed command generated in step 2; As an attitude stability reward, it is used to penalize unintended tilting of the fuselage; The core energy consumption penalty item, weight This determines the level of importance attached to energy conservation; As a penalty for motion smoothness, it reduces the ineffective energy consumption of the motor caused by frequent acceleration and deceleration by suppressing high-frequency jitter in motion commands; Step 3.4: Perform policy network training and Sim-to-Real transfer; The policy network is trained in large-scale parallel operation in a physical simulation environment using a near-end policy optimization algorithm. Domain randomization is introduced during training: random perturbations are applied to the robot's physical and environmental parameters. Through iterative training, a robust and energy-optimal policy network is obtained. It serves as a lower-level local motion controller.
7. The method for collaborative optimization of energy-saving planning and control of quadruped robots in unstructured environments according to claim 6, characterized in that, The specific implementation of step 4 includes: Step 4.
1. Build an airborne hierarchical control architecture based on ROS 2; Upper-level planning node: Run the improved A* algorithm described in step 2 to replan the global energy-optimal path at a low frequency based on real-time positioning information and dynamically update the local reference trajectory; Lower-level inference nodes: Deploy the policy neural network trained in step 3. Edge acceleration is achieved using TensorRT or ONNXRuntime to infer joint control signals driven by upper-layer instructions at high frequency in real time. Step 4.
2. Design instruction mapping logic based on path tracing; in order to map the global geometric path generated by the upper-level planning... The speed command observations are converted into the values required by the lower-level controller. Design a local guidance law and calculate the robot's current position. The nearest target point on the trajectory Positional deviation between and heading deviation Generate the desired velocity and desired angular velocity: In the formula, This is the rotation matrix from the world coordinate system to the fuselage coordinate system; To track gain; This is the feedforward velocity term calculated based on the curvature of the B-spline trajectory; Step 4.
3. Execute Sim-to-Real policy reasoning and underlying driver; read airborne IMU data and joint encoder data, combine them with the local elevation map constructed by the airborne perception system, and assemble them into a state vector. Input Policy Network Policy Network Output the target position increments of 12 joints Superimposed with the robot's nominal standing posture The final target joint angle is calculated. : In the formula, This is the motion scaling factor; Step 4.
4. Closed-loop torque control and energy consumption monitoring; the calculated target joint angle The FOC driver is sent to each joint, and a high-frequency PD control loop runs inside the driver. While the drive motor performs the action, it monitors the voltage and current data fed back by the battery management system in real time, calculates the actual transportation cost (COT), and feeds it back to the upper planning node to correct the energy consumption weight coefficient in step 1 online, forming a complete closed loop of "perception-planning-control-evaluation".
8. The method for collaborative optimization of energy-saving planning and control of quadruped robots in unstructured environments according to claim 7, characterized in that, The PD control loop in step 4.4 is as follows: In the formula, The target torque output by the motor. This is the proportional stiffness coefficient. The differential damping coefficient is... and These represent the expected joint position and expected joint angular velocity output by the policy network inference, respectively. and This provides feedback on the current actual joint position and actual joint angular velocity from the joint encoder.
9. A control device for a robot, characterized in that, The control device deploys the energy-saving planning and control collaborative optimization method for quadruped robots in unstructured environments as described in any one of claims 1-8.
10. A quadruped robot, characterized in that, The robot operates using the energy-saving planning and control collaborative optimization method for quadruped robots oriented towards unstructured environments as described in any one of claims 1-8.