A path planning method and system for a quadruped robot based on improved DWA

Abstract

This application discloses a path planning method and system for a quadruped robot based on an improved Direct Weakening (DWA) algorithm. The method includes: acquiring a global path point sequence and the turning state of the previous control cycle; fusing multi-source sensor data to construct a local environment map and the robot's own state set; evaluating the scene complexity coefficient and determining the current tracking point; sampling candidate velocity combinations and predicting the trajectory; filtering candidates that meet stability conditions based on the turning state of the previous cycle, and eliminating collision trajectories; dynamically adjusting the evaluation function weights according to the tracking point distance, heading deviation, and scene complexity to select the optimal trajectory; smoothing the trajectory before execution, and monitoring and correcting deviations in real time. This application avoids attitude fluctuations caused by frequent switching through turning state constraints, achieves accurate approximation through distance grading and heading deviation linkage, and improves scene adaptability through multi-source fusion and dynamic weights. It solves the problems of poor attitude stability, low tracking accuracy, and insufficient robustness in complex scenes found in existing DWA algorithms.

Description

Technical Field

[0001] This application relates to the field of robotics technology, and in particular to a path planning method and system for a quadruped robot. Background Technology

[0002] Quadruped robots, with their superior terrain adaptability and mobility, have become important intelligent equipment for performing tasks such as inspection, rescue, and transportation in complex environments. In dynamic, unstructured environments, real-time obstacle avoidance and path planning are core technologies for ensuring the safe and efficient movement of quadruped robots.

[0003] An existing solution for dynamic obstacle avoidance and path planning of quadruped robots based on the DWA algorithm has the following core implementation process: First, based on physical constraints such as the robot's maximum linear velocity, maximum yaw rate, maximum linear acceleration, and maximum angular acceleration, several sets of velocity combinations are randomly sampled within the current feasible velocity space. Then, for each sampled velocity, the trajectory within the next 0.5–1.0 seconds is predicted using a differential flat model or forward kinematics solution. Next, combined with obstacle information output by the environmental perception module, it is determined whether the predicted trajectory will collide with an obstacle, and risky velocity combinations are eliminated. Then, the remaining feasible trajectories are quantitatively scored using preset obstacle avoidance safety distance, target approach degree, and path smoothness evaluation indicators. The weight coefficients of the evaluation indicators are mostly fixed values ​​or manually adjusted values ​​based on simple rules. Finally, the velocity combination with the highest score is selected as the optimal instruction for the current control cycle and converted into gait instructions to drive the robot's movement. At the same time, the actual pose state is collected by sensors to achieve closed-loop feedback and cyclic updates.

[0004] However, existing improvement solutions mainly focus on path planning, but none of these improvements systematically optimize for the unique turning stability constraints and precise tracking point approximation requirements of quadruped robots, nor do they fully consider the dynamic scene adaptability under multi-sensor fusion. Specifically, the following points are noted:

[0005] (1) Frequent turning switching leads to attitude instability: The existing solution only predicts the trajectory based on obstacle removal, and then selects the optimal path only based on obstacle avoidance safety distance, target proximity and path smoothness. However, it does not pay attention to the change of yaw rate of the optimal path, which is prone to alternating positive and negative angular velocities in multiple consecutive control cycles. This leads to large fluctuations in the roll / pitch angle of the quadruped robot, disordered gait, and a significant increase in energy consumption. In addition, the risk of side wall collision is aggravated in narrow passage scenarios.

[0006] (2) Insufficient "position-attitude" coordination in the approach to the tracking point: The change in the yaw rate of the optimal path in the existing scheme leads to a large heading deviation when reaching the tracking point and requires additional time for attitude adjustment. At the same time, it results in insufficient speed for long-distance approach and untimely attitude correction at close distance.

[0007] (3) Poor scene robustness: Existing technologies only use a single visual sensor for perception, which is easily affected by environmental interference such as rain, fog, and low light, leading to trajectory prediction deviations. Furthermore, they do not fully consider the impact of the movement trend of dynamic obstacles on steering decisions, which can easily lead to collision risks.

[0008] (4) The evaluation function is not flexible enough. Traditional fixed weights or simple segmented adjustment methods are difficult to achieve the optimal balance between obstacle avoidance safety, path efficiency and attitude stability, resulting in a decline in overall motion performance. Summary of the Invention

[0009] Based on this, and in response to the aforementioned technical problems, a path planning method and system for quadruped robots based on improved DWA is provided. This method solves the problems of attitude instability caused by frequent switching of yaw angular velocity, insufficient coordination between position and attitude when the tracking point approaches, poor perception robustness in complex scenarios, and difficulty in adapting to dynamic scenarios due to fixed evaluation function weights.

[0010] Firstly, a path planning method for a quadruped robot based on an improved DWA (Directed Path Analysis) is provided, the method comprising:

[0011] Step S1: Obtain the pre-planned global path point sequence and the final target point coordinates; and obtain the turning state of the initial control cycle as the turning state of the previous control cycle; the turning state includes: the actual value of the yaw rate of the previous control cycle, the turning direction of the previous control cycle, and the time when the turning direction switch occurred last time;

[0012] Step S2: Real-time acquisition and fusion of data collected by multiple environmental perception sensors to output a local environmental map containing obstacle information; Simultaneously, acquisition of data collected by the robot's own state sensor to construct the robot's own state set, which includes at least the robot's current position, current heading angle, current linear velocity, and current yaw rate.

[0013] Step S3: Based on the local environment map, evaluate the environmental complexity of the area in front of the robot to obtain the scene complexity coefficient;

[0014] Step S4: Within the preset speed and yaw rate constraints, the linear velocity and yaw rate are sampled in combination to obtain candidate linear velocity and candidate yaw rate; and the predicted trajectory is obtained based on the combination of each set of candidate linear velocity and candidate yaw rate.

[0015] Step S5: Based on the steering state of the previous control cycle, the yaw rate in the candidate speed combination is screened, and candidates that do not meet the steering stability conditions are eliminated; and the predicted trajectory that collides with the obstacle is eliminated using the local environment map to obtain the final candidate predicted trajectory.

[0016] Step S6: Based on the distance from the robot's current position to the tracking point in the current period of the global path point sequence, the deviation between the current heading angle and the desired heading angle, and the scene complexity coefficient, adjust the weights of each dimension in the pre-constructed evaluation function; and score the candidate predicted trajectories to select the optimal trajectory with the highest score; the desired heading angle is the direction of the line connecting the current heading angle to the tracking point;

[0017] Step S7: Smooth the optimal trajectory and send the smoothed optimal trajectory to the motor for execution;

[0018] Step S8: Acquire the position and attitude information of the UAV in real time, and calculate the deviation between the position and attitude information of the UAV and the optimal trajectory. If the deviation is greater than a certain value, return to step S5; record the current turning state as the current turning state, and repeat steps S2-S7 until the final target point is reached.

[0019] Optionally, in the above scheme, the step of acquiring and fusing data from multiple environmental perception sensors in real time to output a local environmental map containing obstacle information includes:

[0020] The system acquires point cloud data collected by lidar and depth image data collected by vision camera in real time, and performs filtering processing on the point cloud data and depth image data.

[0021] The filtered point cloud data and image data are spatiotemporally synchronized and fused to generate a local environment map that also contains obstacle information.

[0022] In the above scheme, optionally, the step of filtering the yaw rate of the candidate speed combinations based on the steering state of the previous control cycle and eliminating candidates that do not meet the steering stability conditions includes:

[0023] For the currently sampled candidate yaw rate, determine whether its turning direction is consistent with the turning direction of the turning state in the previous control cycle. If they are consistent, retain this candidate yaw rate.

[0024] If the steering direction of the current candidate yaw rate is inconsistent with the steering direction of the steering state in the previous control cycle, calculate whether the time difference between the current moment and the moment when the steering direction switch occurred is less than the preset switching time interval threshold. If yes, the candidate yaw rate is removed; otherwise, it is retained.

[0025] If the turning direction of the current candidate yaw rate is inconsistent with the turning direction of the turning state in the previous control cycle, it is determined whether the candidate yaw rate is greater than the preset switching yaw rate threshold. If so, the candidate yaw rate is removed; otherwise, it is retained.

[0026] If the turning direction of the current candidate yaw rate is inconsistent with the turning direction of the previous control cycle, the yaw angle acceleration is calculated based on the candidate yaw rate and the actual value of the yaw rate in the previous control cycle. It is then determined whether the yaw angle acceleration is greater than the preset switching angle acceleration threshold. If it is, the candidate yaw rate is removed; otherwise, it is retained.

[0027] Optionally, in the above scheme, the evaluation function is:

[0028] ;

[0029] Where S is the safe distance indicator; G is the target approach indicator; and P is the path smoothness indicator. For distance adaptation metrics, This is an attitude correction index.

[0030] In the above scheme, optionally, the safety distance index S is calculated using the following formula:

[0031] ;

[0032] in, To predict the minimum distance from the trajectory to the obstacle; This is the safe distance threshold;

[0033] The target approximation index G is calculated using the following formula:

[0034] ;

[0035] Where L is the remaining length from the robot's current position to the final target point. This represents the total length of the global path.

[0036] The path smoothing index P is calculated using the following formula:

[0037] ;

[0038] in, To predict the maximum curvature value in the trajectory, Maximum permissible curvature;

[0039] The distance adaptation index Calculated using the following formula:

[0040] ;

[0041] The attitude correction index Calculated using the following formula:

[0042] in, This represents the current yaw angle deviation.

[0043] In the above scheme, optionally further, adjusting the weights of each dimension in the pre-constructed evaluation function based on the distance from the robot's current position to the tracking point, the deviation between the current heading angle and the desired heading angle, and the scene complexity coefficient includes:

[0044] Calculate the straight-line distance from the current robot position to the current tracking point in real time, and remember it as the tracking point distance; calculate the deviation between the current heading angle and the desired heading angle, and remember it as the heading deviation angle;

[0045] Determine the distance level of the tracking point to obtain distance level information; the distance level includes at least long distance, medium distance, and short distance;

[0046] Determine whether the heading deviation angle exceeds the threshold to be corrected or the threshold to meet the standard, and obtain the heading angle deviation judgment result;

[0047] The evaluation function coefficients are calculated through simulation and reasoning based on distance level information, heading angle deviation judgment results, and scene complexity coefficients. as well as The size of W4; wherein, the distance level information is used to adjust the size of W4, and the heading angle deviation judgment result is used to adjust the size of W4. The size of W1, W2, and W3 is used to adjust the size of W1, W2, and W3.

[0048] Optionally, in the above scheme, smoothing the optimal trajectory includes:

[0049] The optimal curve is fitted using a 3rd-order B-spline curve to obtain the smoothed trajectory curve;

[0050] Determine whether the step size, body turning angle, and pitch angle corresponding to the smoothed trajectory curve are less than the preset safety values. If they exceed the preset safety values, readjust the parameters and perform fitting.

[0051] Optionally, in the above scheme, if the deviation exceeds a certain value, trajectory correction or switching of tracking points is performed:

[0052] Secondly, a quadruped robot path planning system based on an improved DWA (Design for Robots) is provided, the system comprising:

[0053] Initialization module: used to obtain the pre-planned global path point sequence and the coordinates of the final target point; and to obtain the turning state of the initial control cycle and record it as the turning state of the previous control cycle; the turning state includes: the actual value of the yaw rate of the previous control cycle, the turning direction of the previous control cycle, and the time when the turning direction switch occurred last time;

[0054] Data acquisition module: used to acquire and fuse data collected by multiple environmental perception sensors in real time, and output a local environmental map containing obstacle information; at the same time, it acquires data collected by the body state sensor to construct the robot's own state set, which includes at least the robot's current position, current heading angle, current linear velocity and current yaw rate.

[0055] Tracking point determination module: used to evaluate the environmental complexity of the area in front of the robot based on the local environment map to obtain the scene complexity coefficient;

[0056] Velocity space sampling module: Used to combine and sample linear velocity and yaw rate within a preset speed and yaw rate constraint range to obtain candidate linear velocity and candidate yaw rate; and to obtain the predicted trajectory based on each combination of candidate linear velocity and candidate yaw rate.

[0057] Predicted trajectory elimination module: Based on the steering state of the previous control cycle, it filters the yaw rate in the candidate speed combination and eliminates candidates that do not meet the steering stability conditions; and uses the local environment map to eliminate predicted trajectories that collide with obstacles, thus obtaining the final candidate predicted trajectory.

[0058] The optimal trajectory selection module is used to adjust the weights of each dimension in the pre-constructed evaluation function based on the distance from the robot's current position to the tracking point in the current period of the global path point sequence, the deviation between the current heading angle and the desired heading angle, and the scene complexity coefficient; and to score the candidate predicted trajectories and select the optimal trajectory with the highest score; the desired heading angle is the direction of the line connecting the current heading angle to the tracking point;

[0059] Smoothing module: used to smooth the optimal trajectory and send the smoothed optimal trajectory to the motor for execution;

[0060] Feedback execution module: used to acquire the position and attitude information of the UAV in real time, and calculate the deviation between the position and attitude information of the UAV and the optimal trajectory. If the deviation is greater than a certain value, it returns to the predicted trajectory elimination module; records the current turning state as the current turning state, and executes the tracking point determination module, velocity space sampling module, predicted trajectory elimination module, optimal trajectory filtering module and smoothing module in a loop until the final target point is reached.

[0061] Thirdly, a computer device includes a memory and a processor, the memory storing a computer program, and the processor executing the computer program to implement the steps of the improved DWA-based quadruped robot path planning method described in the first aspect.

[0062] Fourthly, a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the quadruped robot path planning method based on the improved DWA described in the first aspect.

[0063] This application has at least the following beneficial effects:

[0064] This application records the actual yaw rate, steering direction, and steering switching time of the previous control cycle. During the candidate velocity sampling phase, yaw rates that do not meet the steering stability conditions are screened, effectively avoiding body posture fluctuations caused by alternating positive and negative yaw rates within continuous control cycles and reducing the risk of sidewall collisions in narrow passage scenarios. By evaluating the scene complexity coefficient in real time and determining the current tracking point accordingly, the distance from the robot to the tracking point and the deviation between the current heading angle and the desired heading angle are calculated. Combined with the scene complexity coefficient, the weights of each dimension of the evaluation function are dynamically adjusted, achieving coordinated optimization of distance hierarchical approximation and heading deviation correction, shortening the target docking time and improving posture accuracy. By fusing multi-source environmental perception sensor data to construct a local environmental map, the reliability of environmental perception is enhanced. By smoothing the optimal trajectory and monitoring execution deviations in real time, closed-loop trajectory correction is achieved, ensuring the continuity and stability of the motion process. Attached Figure Description

[0065] Figure 1 A flowchart illustrating a path planning method for a quadruped robot based on an improved DWA, provided as an embodiment of this application;

[0066] Figure 2 This is a detailed flowchart illustrating a path planning method for a quadruped robot based on an improved DWA, provided as an embodiment of this application. Detailed Implementation

[0067] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0068] In the current field of dynamic obstacle avoidance and path planning for quadruped robots, the traditional DWA algorithm has become the mainstream technical solution due to its strong real-time performance and low computational complexity. Its core implementation logic is as follows:

[0069] 1. Velocity space sampling: based on the robot's physical constraints (maximum linear velocity) Maximum yaw rate Maximum linear acceleration Maximum angular acceleration Randomly sample several groups within the feasible velocity space. Speed ​​combination;

[0070] 2. Trajectory Prediction: For each set of sampled velocities, based on the robot's kinematics model (differential flat model or forward kinematics solution), predict the motion trajectory within the next 0.5-1.0 seconds;

[0071] 3. Obstacle collision detection: Based on the obstacle information output by the environmental perception module, determine whether the predicted trajectory will collide with the obstacle, and eliminate the speed combination corresponding to the collision risk trajectory;

[0072] 4. Evaluation function selection: The remaining feasible trajectories are scored using preset evaluation indicators (obstacle avoidance safety distance S, target approach degree G, path smoothness P). The weights are fixed or manually adjusted, and the speed combination with the highest score is selected as the control command.

[0073] 5. Control Execution and Feedback: The optimal speed combination is converted into gait commands to drive the robot's movement. Sensors collect the actual state, and the path planning results are updated iteratively. Furthermore, existing improvement schemes mostly focus on optimizing evaluation function weights (such as weight adjustment based on fuzzy logic) or obstacle prediction (such as Kalman filtering to predict the position of dynamic obstacles), but they do not systematically optimize "steering stability" and "accurate tracking point approximation (position + attitude dual targets)," and do not fully consider scene adaptability under multi-sensor fusion.

[0074] This application requires the explanation of some terms:

[0075] 1. DWA: Dynamic Window Approach, a local path planning algorithm based on robot physical constraints (maximum velocity, acceleration), which determines the path in the velocity space (linear velocity v, yaw rate). The trajectory is sampled and predicted, and the optimal motion command is selected by combining the evaluation function.

[0076] 2. Yaw angular velocity (turning angular velocity) represents the robot's rotational speed around an axis perpendicular to the ground. Positive / negative values ​​correspond to right / left turns, respectively, and zero values ​​correspond to straight-line movement.

[0077] 3. Tracking Point: The target node (including intermediate guide points and the final target point) in the path point sequence generated by global path planning, used to guide the robot to approach the final destination step by step;

[0078] 4. Steering bidirectional switching constraints: based on timing constraints and dynamic constraints. The alternating positive and negative signs apply restrictions to prevent frequent reversals of the turning direction within a short period;

[0079] 5. Heading deviation angle The absolute difference between the robot's current heading angle and the expected heading angle along the line connecting its own center of mass and the tracking point reflects the attitude alignment accuracy.

[0080] 6. Tracking point distance classification: Based on the robot's kinematic characteristics and scene complexity, the straight-line distance between the "current position and the tracking point" is divided into three intervals: long distance, medium distance, and short distance, to realize differentiated path planning strategies;

[0081] 7. Multi-sensor fusion: Through the collaborative work of LiDAR (obstacle distance detection), IMU (inertial measurement unit, attitude and motion state acquisition), RGB-D vision camera (terrain and obstacle type recognition), and RTK localization module (position information acquisition), a comprehensive perception of the environment and robot status is achieved;

[0082] 8. Closed-loop control: By collecting the deviation between the robot's actual motion state (position, attitude, speed) and the planned target in real time, the control commands are dynamically corrected to ensure path tracking accuracy and attitude stability;

[0083] 9. Complex Scenarios: Including dynamic obstacles (movement speed / direction varies over time), narrow passages (width ≤ twice the robot's wheelbase), and unstructured terrain (slope). It has comprehensive application scenarios with protrusions / depressions.

[0084] In one embodiment, such as Figure 1 and 2 As shown, a path planning method for a quadruped robot based on an improved DWA is provided, the method comprising:

[0085] Step S1: Obtain the pre-planned global path point sequence and the final target point coordinates; and obtain the turning state of the initial control cycle as the turning state of the previous control cycle; the turning state includes: the actual value of the yaw rate of the previous control cycle, the turning direction of the previous control cycle, and the time when the turning direction switch occurred last time.

[0086] In step S1, the initialization phase takes time. ): Load the global path point sequence and the coordinates of the final target point At the same time, it specifies the parameters to be used later, such as the switching time interval threshold. Switching angular velocity threshold Switching angular acceleration threshold close range Mid-range Long distance Threshold to be corrected , threshold ), initialize the sensor, filtering algorithm parameters and steering state cache.

[0087] Step S2: Real-time acquisition and fusion of data collected by multiple environmental perception sensors to output a local environmental map containing obstacle information; simultaneously, acquisition of data collected by the robot's own state sensor to construct the robot's own state set, which includes at least the robot's current position, current heading angle, current linear velocity, and current yaw rate.

[0088] In step S2, multi-sensor data is collected, filtered, spatiotemporally synchronized, and fused using EKF, and then the local grid map, obstacle status, and robot state set are output. (Horizontal axis, vertical axis, acceleration, yaw rate, roll angle, pitch angle) Tracking point generation stage (cycle) ).

[0089] Step S3: Based on the local environment map, evaluate the environmental complexity of the area in front of the robot to obtain the scene complexity coefficient.

[0090] In step S3, the scene complexity coefficient K is calculated, and the current cycle tracking point is adaptively selected based on K. .

[0091] Step S4: Within the preset speed and yaw rate constraints, the linear velocity and yaw rate are sampled in combination to obtain candidate linear velocity and candidate yaw rate; and the predicted trajectory is obtained based on the combination of each candidate linear velocity and candidate yaw rate.

[0092] In step S4, the physical constraints of this application are based on velocity. yaw rate Thirty combinations of samples (velocity, yaw rate) were collected. For each sampled velocity, the robot's trajectory within the next 0.5-1.0 seconds was predicted based on the robot's kinematic model (differential flat model or forward kinematics solution).

[0093] Step S5: Based on the steering state of the previous control cycle, the yaw rate in the candidate speed combination is screened, and candidates that do not meet the steering stability conditions are eliminated; and the predicted trajectory that collides with the obstacle is eliminated using the local environment map to obtain the final candidate predicted trajectory.

[0094] In step S5, specifically, candidates that do not meet the steering stability conditions are eliminated. First, the constraint parameters must be calibrated in advance. Generally, this is based on the gait dynamics characteristics of the quadruped robot (trot gait period 0.3-0.5s, attitude stability limit angular acceleration). The design incorporates a triple constraint of "timing + angular velocity + angular acceleration" to avoid frequent steering switching.

[0095] (1) The constraint parameters are calibrated as follows:

[0096] 1) Switching time interval threshold T: based on gait cycle set up, (Ensure that steering shifts match gait phase to avoid posture conflicts);

[0097] 2) Switch angular velocity threshold Based on attitude stability limit settings, (Avoid excessive steering angular velocity that could cause body tilting);

[0098] 3) Switching angular acceleration threshold Based on motor drive capability settings, (To avoid joint impact caused by sudden changes in angular acceleration);

[0099] (2) Eliminate candidates that do not meet the steering stability condition:

[0100] 1) Steering State Buffer: Stores key states from the previous control cycle (Δt=0.1s) in real time: the sign of the yaw rate (indicating the steering direction). ,like Turn right. Left turn, 0 = straight ahead), last turn change time The actual value of the yaw rate actually executed in the previous cycle. ;

[0101] 2) Switching feasibility verification: Verify the candidate yaw rate sampled in the current period. Determine whether a turnaround is allowed based on the following logic:

[0102] a) Sign consistency judgment: If the current yaw rate is in the direction of change... =Last turning direction (With the steering direction unchanged), proceed directly to the collision detection stage;

[0103] b) Timing constraint verification: If the current yaw rate is in the direction of change... ≠ last turning direction Calculate the current time Compared to the last steering switch time The difference, if Switching time interval threshold T, rejecting the candidate yaw rate. ;

[0104] c) Angular velocity constraint check: If the current yaw angular velocity Reject the candidate ;

[0105] d) Angular acceleration constraint verification: Calculate angular acceleration ,like Reject the candidate ;

[0106] This step also includes: based on the local grid map, eliminating combinations of trajectories that collide with obstacles.

[0107] Step S6: Based on the distance from the robot's current position to the tracking point in the current period of the global path point sequence, the deviation between the current heading angle and the desired heading angle, and the scene complexity coefficient, adjust the weights of each dimension in the pre-constructed evaluation function; and score the candidate predicted trajectories to select the optimal trajectory with the highest score; the desired heading angle is the direction of the line connecting the current heading angle to the tracking point.

[0108] In step S6, the following calculations are first required: the distance from the robot's current position to the tracking point, the deviation between the current heading angle and the desired heading angle, and the scene complexity coefficient.

[0109] 1) The distance from the robot's current position to the tracking point is denoted as the tracking point distance L: calculated using the Euclidean distance formula based on the fused position data. Where x is the x-coordinate of the current position and y is the y-coordinate of the current position. Tracking point x-coordinate, The vertical coordinate of the tracking point.

[0110] 2) Heading deviation angle Desired heading angle (Based on the direction of the line connecting the robot's current position and the tracking point), current heading angle Taken from IMU fused data, ;

[0111] 3) Scene complexity coefficient K: Dynamic obstacle density based on multi-sensor fusion output and terrain slope calculate, (Normalization to) .

[0112] Secondly, determine the grading parameters:

[0113] 1) Distance classification threshold: long distance Mid-range close range ;

[0114] 2) Heading deviation threshold: Threshold to be corrected (Posture adjustment should be prioritized), threshold for achieving the target (Posture meets requirements);

[0115] Then, the evaluation function is dynamically optimized:

[0116] 1) Five-dimensional evaluation function (safety, efficiency, smoothness, distance adaptation, attitude correction), with weights adaptively adjusted based on L, Δθ, and K, as shown in the following formula:

[0117]

[0118] Where S is the safe distance indicator; G is the target approach indicator; and P is the path smoothness indicator. For distance adaptation metrics, Attitude correction index; These are the weighting coefficients;

[0119] The safe distance index S is calculated using the following formula: ;in, To predict the minimum distance from the trajectory to the obstacle; For the safe distance threshold (this application) ), normalized to ;

[0120] The target approximation index G is calculated using the following formula: Where L is the remaining length from the robot's current position to the final target point. (This represents the total global path length, normalized to...) ;

[0121] The path smoothing index P is calculated using the following formula: ;in, To predict the maximum curvature value in the trajectory, For the maximum permissible curvature (this application) ), normalized to ;

[0122] Distance adaptation index Calculated using the following formula: Weight Dynamically adjusted according to distance classification;

[0123] Attitude correction index Calculated using the following formula: ,in, This represents the current yaw angle deviation. Weighting. Dynamically adjusted based on heading deviation;

[0124] Finally, based on the weight adjustment rules (based on fuzzy logic reasoning), Table 1 shows the final weight adjustment method:

[0125] Specifically: when the distance L from the tracking point is greater than the distance to the farthest point. Heading deviation angle Greater than the threshold to be corrected Scene complexity coefficient ,but ;

[0126] When the distance L from the tracking point is greater than the distance D3, the heading deviation angle Not greater than the threshold to be corrected Scene complexity coefficient ,but ;

[0127] When the tracking point distance L is less than or equal to the long distance D3 and greater than the medium distance D2, the heading deviation angle is... Greater than the threshold to be corrected If the scene complexity coefficient K is less than 0.5, then ;

[0128] When the tracking point distance L is less than or equal to the long distance D3 and greater than the medium distance D2, the heading deviation angle is... Greater than the threshold to be corrected If the scene complexity coefficient K is greater than or equal to 0.5, then ;

[0129] When the tracking point distance L is less than or equal to the long distance D3 and greater than the medium distance D2, the heading deviation angle is... Less than or equal to the threshold to be corrected Scene complexity coefficient ,but ;

[0130] When the distance L from the tracking point is less than or equal to the midpoint distance D2, the heading deviation angle Greater than the threshold to be corrected Scene complexity coefficient ,but ;

[0131] When the distance L from the tracking point is less than or equal to the midpoint distance D2, the heading deviation angle Greater than the threshold to be corrected Scene complexity coefficient ,but ;

[0132] Table 1

[0133]

[0134] Step S7: Smooth the optimal trajectory and send the smoothed optimal trajectory to the motor for execution.

[0135] In this application, the optimal trajectory selected by the evaluation function is post-processed to ensure that it matches the gait characteristics of the quadruped robot:

[0136] 1) Trajectory Smoothing: A cubic B-spline curve is used for fitting, with the number of vertices controlled at 5-8 (adapted to the trajectory length). The curvature of the fitted trajectory is continuous and... To avoid sudden changes at the inflection point;

[0137] 2) Kinematic constraint verification: Based on the quadruped robot DH parameter model (hip, knee, and ankle joint link lengths are 0.3m, 0.3m, and 0.2m respectively), the step length corresponding to the fitted trajectory is verified. Body turning angle Pitch angle If the constraints are exceeded, the B-spline control points are readjusted until the requirements are met.

[0138] Step S8: Acquire the position and attitude information of the UAV in real time, and calculate the deviation between the position and attitude information of the UAV and the optimal trajectory. If the deviation is greater than a certain value, return to step S5; record the current turning state as the current turning state, and repeat steps S2-S7 until the final target point is reached.

[0139] In step S8, which is the closed-loop feedback stage (period 0.1s): the position deviation is calculated. and attitude deviation Based on the triggering conditions, perform corrections or switch tracking points; if the final target point is reached ( and If the condition is met, the algorithm terminates; otherwise, return to step S2 and repeat the process.

[0140] The aforementioned path planning method for a quadruped robot based on improved DWA records the actual yaw rate, turning direction, and turning switch time of the previous control cycle. During the candidate velocity sampling stage, yaw rates that do not meet the turning stability conditions are filtered out, effectively avoiding the body posture fluctuation problem caused by the alternation of positive and negative yaw rates within continuous control cycles and reducing the risk of sidewall collisions in narrow passage scenarios. By evaluating the scene complexity coefficient in real time and determining the current tracking point accordingly, the distance from the robot to the tracking point and the deviation between the current heading angle and the desired heading angle are calculated. Combined with the scene complexity coefficient, the weights of each dimension of the evaluation function are dynamically adjusted, achieving coordinated optimization of distance hierarchical approximation and heading deviation correction, shortening the target docking time and improving posture accuracy. By fusing multi-source environmental perception sensor data to construct a local environment map, the reliability of environmental perception is enhanced. By smoothing the optimal trajectory and monitoring the execution deviation in real time, closed-loop trajectory correction is achieved, ensuring the continuity and stability of the motion process.

[0141] In one embodiment, the real-time acquisition and fusion of data collected by multiple environmental perception sensors to output a local environmental map containing obstacle information includes:

[0142] The system acquires point cloud data collected by lidar and depth image data collected by vision camera in real time, and performs filtering processing on the point cloud data and depth image data.

[0143] The filtered point cloud data and image data are spatiotemporally synchronized and fused to generate a local environment map that also contains obstacle information.

[0144] In one embodiment, adjusting the weights of each dimension in the pre-constructed evaluation function based on the distance from the robot's current position to the tracking point, the deviation between the current heading angle and the desired heading angle, and the scene complexity coefficient includes:

[0145] Calculate the straight-line distance from the current robot position to the current tracking point in real time, and remember it as the tracking point distance; calculate the deviation between the current heading angle and the desired heading angle, and remember it as the heading deviation angle;

[0146] Determine the distance level of the tracking point to obtain distance level information; the distance level includes at least long distance, medium distance, and short distance;

[0147] Determine whether the heading deviation angle exceeds the threshold to be corrected or the threshold to meet the standard, and obtain the heading angle deviation judgment result;

[0148] The evaluation function coefficients and their magnitudes are calculated through simulation reasoning based on the distance level information, the heading angle deviation judgment result, and the scene complexity coefficient; wherein, the distance level information is used to adjust the magnitude of W4, the heading angle deviation judgment result is used to adjust the magnitude of W1, W2, and W3, and the scene complexity coefficient is used to adjust the magnitude of W1, W2, and W3.

[0149] The advantages of this application are:

[0150] 1. Significantly improved steering stability and energy efficiency: through The dual constraint mechanism of bidirectional switching reduces the frequency of steering switching. The fluctuation range of the roll / pitch angle of the quadruped robot's body The incidence of gait disorder decreased. The above reduces exercise energy consumption. In narrow passage scenarios, the collision rate is significantly higher than that of traditional DWA. Down to the following;

[0151] 2. Improved accuracy of "position-attitude" dual targets in tracking point approximation: Through distance grading and heading deviation correction mechanisms, the average time for the robot to reach the tracking point is shortened. The heading and attitude deviation is stably controlled within No need for secondary adjustments, improved exercise efficiency above;

[0152] 3. Enhanced robustness in complex scenarios: Multi-sensor fusion technology improves environmental perception accuracy to [percentage missing]. The above features can adapt to complex scenarios such as dynamic obstacles, narrow passages, and unstructured terrain, thus expanding the scope of scenario adaptation compared to traditional single-sensor solutions. ;

[0153] 4. Optimization of the evaluation function's adaptive capability: The dynamic weight adjustment mechanism achieves a multi-objective balance between obstacle avoidance safety, path efficiency, and attitude stability, improving the overall performance (safety rate × efficiency × stability) under different complexity scenarios. above;

[0154] 5. Strong compatibility and portability: The solution is based on the traditional DWA algorithm and does not change the core framework. It can directly replace the path planning module of the existing quadruped robot without reconstructing the hardware system, reducing development costs by 30%, and is compatible with mainstream quadruped robot platforms (such as Boston Dynamics Spot and Unitree H1).

[0155] 6. Improved Dynamic Obstacle Adaptability: By adjusting the scene complexity coefficient and velocity sampling window in tandem, the obstacle avoidance response time in dynamic obstacle scenarios is improved. Collision rate reduced above.

[0156] In one embodiment, a quadruped robot path planning system based on improved DWA includes:

[0157] Initialization module: used to obtain the pre-planned global path point sequence and the coordinates of the final target point; and to obtain the turning state of the initial control cycle and record it as the turning state of the previous control cycle; the turning state includes: the actual value of the yaw rate of the previous control cycle, the turning direction of the previous control cycle, and the time when the turning direction switch occurred last time;

[0158] Data acquisition module: used to acquire and fuse data collected by multiple environmental perception sensors in real time, and output a local environmental map containing obstacle information; at the same time, it acquires data collected by the body state sensor to construct the robot's own state set, which includes at least the robot's current position, current heading angle, current linear velocity and current yaw rate.

[0159] Tracking point determination module: used to evaluate the environmental complexity of the area in front of the robot based on the local environment map to obtain the scene complexity coefficient;

[0160] Velocity space sampling module: Used to combine and sample linear velocity and yaw rate within a preset speed and yaw rate constraint range to obtain candidate linear velocity and candidate yaw rate; and to obtain the predicted trajectory based on each combination of candidate linear velocity and candidate yaw rate.

[0161] Predicted trajectory elimination module: Based on the steering state of the previous control cycle, it filters the yaw rate in the candidate speed combination and eliminates candidates that do not meet the steering stability conditions; and uses the local environment map to eliminate predicted trajectories that collide with obstacles, thus obtaining the final candidate predicted trajectory.

[0162] The optimal trajectory selection module is used to adjust the weights of each dimension in the pre-constructed evaluation function based on the distance from the robot's current position to the tracking point in the current period of the global path point sequence, the deviation between the current heading angle and the desired heading angle, and the scene complexity coefficient; and to score the candidate predicted trajectories and select the optimal trajectory with the highest score; the desired heading angle is the direction of the line connecting the current heading angle to the tracking point;

[0163] Smoothing module: used to smooth the optimal trajectory and send the smoothed optimal trajectory to the motor for execution;

[0164] Feedback execution module: used to acquire the position and attitude information of the UAV in real time, and calculate the deviation between the position and attitude information of the UAV and the optimal trajectory. If the deviation is greater than a certain value, it returns to the predicted trajectory elimination module; records the current turning state as the current turning state, and executes the tracking point determination module, velocity space sampling module, predicted trajectory elimination module, optimal trajectory filtering module and smoothing module in a loop until the final target point is reached.

[0165] Specific limitations regarding the quadruped robot path planning system based on improved DWA can be found in the limitations of the quadruped robot path planning method based on improved DWA mentioned above, and will not be repeated here. Each module in the aforementioned quadruped robot path planning system based on improved DWA can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in or independent of the processor in a computer device in hardware form, or stored in the memory of a computer device in software form, so that the processor can call and execute the operations corresponding to each module.

[0166] In one embodiment, a computer device is provided, which may be a server. The computer device includes a processor, memory, and a network interface connected via a system bus. The processor provides computing and control capabilities. The memory includes a non-volatile storage medium and internal memory. The non-volatile storage medium stores an operating system, computer programs, and a database. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage medium. The network interface is used to communicate with external terminals via a network connection. When the computer program is executed by the processor, it implements the aforementioned improved DWA-based quadruped robot path planning method.

[0167] In one embodiment, a computer-readable storage medium is also provided, on which a computer program is stored relating to all or part of the processes in the methods of the above embodiments.

[0168] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the methods described above. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, or optical storage, etc. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM), etc.

[0169] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0170] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the invention patent. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this patent application should be determined by the appended claims.

Claims

1. A path planning method for a quadruped robot based on an improved DWA, characterized in that, The method includes: Step S1: Obtain the pre-planned global path point sequence and the final target point coordinates; and obtain the turning state of the initial control cycle as the turning state of the previous control cycle; the turning state includes: the actual value of the yaw rate of the previous control cycle, the turning direction of the previous control cycle, and the time when the turning direction switch occurred last time; Step S2: Real-time acquisition and fusion of data collected by multiple environmental perception sensors to output a local environmental map containing obstacle information; Simultaneously, acquisition of data collected by the robot's own state sensor to construct the robot's own state set, which includes at least the robot's current position, current heading angle, current linear velocity, and current yaw rate. Step S3: Based on the local environment map, evaluate the environmental complexity of the area in front of the robot to obtain the scene complexity coefficient; Step S4: Within the preset speed and yaw rate constraints, the linear velocity and yaw rate are sampled in combination to obtain candidate linear velocity and candidate yaw rate; and the predicted trajectory is obtained based on the combination of each set of candidate linear velocity and candidate yaw rate. Step S5: Based on the steering state of the previous control cycle, the yaw rate in the candidate speed combination is screened, and candidates that do not meet the steering stability conditions are eliminated; and the predicted trajectory that collides with the obstacle is eliminated using the local environment map to obtain the final candidate predicted trajectory. Step S6: Based on the distance from the robot's current position to the tracking point in the current period of the global path point sequence, the deviation between the current heading angle and the desired heading angle, and the scene complexity coefficient, adjust the weights of each dimension in the pre-constructed evaluation function; and score the candidate predicted trajectories to select the optimal trajectory with the highest score; the desired heading angle is the direction of the line connecting the current heading angle to the tracking point; Step S7: Smooth the optimal trajectory and send the smoothed optimal trajectory to the motor for execution; Step S8: Acquire the position and attitude information of the UAV in real time, and calculate the deviation between the position and attitude information of the UAV and the optimal trajectory. If the deviation is greater than a certain value, return to step S5; record the current turning state as the current turning state, and repeat steps S2-S7 until the final target point is reached.

2. The quadruped robot path planning method based on improved DWA according to claim 1, characterized in that, The process of acquiring and fusing data from multiple environmental perception sensors in real time to output a local environmental map containing obstacle information includes: The system acquires point cloud data collected by lidar and depth image data collected by vision camera in real time, and performs filtering processing on the point cloud data and depth image data. The filtered point cloud data and image data are spatiotemporally synchronized and fused to generate a local environment map that also contains obstacle information.

3. The quadruped robot path planning method based on improved DWA according to claim 1, characterized in that, Based on the steering state of the previous control cycle, the yaw rate in the candidate speed combinations is screened, and candidates that do not meet the steering stability conditions are eliminated, including: For the currently sampled candidate yaw rate, determine whether its turning direction is consistent with the turning direction of the turning state in the previous control cycle. If they are consistent, retain this candidate yaw rate. If the steering direction of the current candidate yaw rate is inconsistent with the steering direction of the steering state in the previous control cycle, calculate whether the time difference between the current moment and the moment when the steering direction switch occurred is less than the preset switching time interval threshold. If yes, the candidate yaw rate is removed; otherwise, it is retained. If the turning direction of the current candidate yaw rate is inconsistent with the turning direction of the turning state in the previous control cycle, it is determined whether the candidate yaw rate is greater than the preset switching yaw rate threshold. If so, the candidate yaw rate is removed; otherwise, it is retained. If the turning direction of the current candidate yaw rate is inconsistent with the turning direction of the previous control cycle, the yaw angle acceleration is calculated based on the candidate yaw rate and the actual value of the yaw rate in the previous control cycle. It is then determined whether the yaw angle acceleration is greater than the preset switching angle acceleration threshold. If it is, the candidate yaw rate is removed; otherwise, it is retained.

4. The quadruped robot path planning method based on improved DWA according to claim 1, characterized in that, The evaluation function is: ； Where S is the safe distance indicator; G is the target approach indicator; and P is the path smoothness indicator. For distance adaptation metrics, This is an attitude correction index.

5. The quadruped robot path planning method based on improved DWA according to claim 4, characterized in that, The safe distance index S is calculated using the following formula: ； in, To predict the minimum distance from the trajectory to the obstacle; This is the safe distance threshold; The target approximation index G is calculated using the following formula: ； Where L is the remaining length from the robot's current position to the final target point. This represents the total length of the global path. The path smoothing index P is calculated using the following formula: ； in, To predict the maximum curvature value in the trajectory, Maximum permissible curvature; The distance adaptation index Calculated using the following formula: ； The attitude correction index Calculated using the following formula: in, This represents the current yaw angle deviation.

6. The quadruped robot path planning method based on improved DWA according to claim 4, characterized in that, The adjustment of the weights of each dimension in the pre-constructed evaluation function based on the distance from the robot's current position to the tracking point, the deviation between the current heading angle and the desired heading angle, and the scene complexity coefficient includes: Calculate the straight-line distance from the current robot position to the current tracking point in real time, and remember it as the tracking point distance; calculate the deviation between the current heading angle and the desired heading angle, and remember it as the heading deviation angle; Determine the distance level of the tracking point to obtain distance level information; the distance level includes at least long distance, medium distance, and short distance; Determine whether the heading deviation angle exceeds the threshold to be corrected or the threshold to meet the standard, and obtain the heading angle deviation judgment result; The evaluation function coefficients are calculated through simulation and reasoning based on distance level information, heading angle deviation judgment results, and scene complexity coefficients. as well as The size of W4; wherein, the distance level information is used to adjust the size of W4, and the heading angle deviation judgment result is used to adjust the size of W4. The size of W1, W2, and W3 is used to adjust the size of W1, W2, and W3.

7. The quadruped robot path planning method based on improved DWA according to claim 1, characterized in that, Smoothing the optimal trajectory includes: The optimal curve is fitted using a 3rd-order B-spline curve to obtain the smoothed trajectory curve; Determine whether the step size, body turning angle, and pitch angle corresponding to the smoothed trajectory curve are less than the preset safety values. If they exceed the preset safety values, readjust the parameters and perform fitting.

8. A path planning system for a quadruped robot based on an improved DWA, characterized in that, The system includes: Initialization module: used to obtain the pre-planned global path point sequence and the coordinates of the final target point; and to obtain the turning state of the initial control cycle and record it as the turning state of the previous control cycle; the turning state includes: the actual value of the yaw rate of the previous control cycle, the turning direction of the previous control cycle, and the time when the turning direction switch occurred last time; Data acquisition module: used to acquire and fuse data collected by multiple environmental perception sensors in real time, and output a local environmental map containing obstacle information; at the same time, it acquires data collected by the body state sensor to construct the robot's own state set, which includes at least the robot's current position, current heading angle, current linear velocity and current yaw rate. Tracking point determination module: used to evaluate the environmental complexity of the area in front of the robot based on the local environment map to obtain the scene complexity coefficient; Velocity space sampling module: Used to combine and sample linear velocity and yaw rate within a preset speed and yaw rate constraint range to obtain candidate linear velocity and candidate yaw rate; and to obtain the predicted trajectory based on each combination of candidate linear velocity and candidate yaw rate. Predicted trajectory elimination module: Based on the steering state of the previous control cycle, it filters the yaw rate in the candidate speed combination and eliminates candidates that do not meet the steering stability conditions; and uses the local environment map to eliminate predicted trajectories that collide with obstacles, thus obtaining the final candidate predicted trajectory. The optimal trajectory selection module is used to adjust the weights of each dimension in the pre-constructed evaluation function based on the distance from the robot's current position to the tracking point in the current period of the global path point sequence, the deviation between the current heading angle and the desired heading angle, and the scene complexity coefficient; and to score the candidate predicted trajectories and select the optimal trajectory with the highest score; the desired heading angle is the direction of the line connecting the current heading angle to the tracking point; Smoothing module: used to smooth the optimal trajectory and send the smoothed optimal trajectory to the motor for execution; Feedback execution module: used to acquire the position and attitude information of the UAV in real time, and calculate the deviation between the position and attitude information of the UAV and the optimal trajectory. If the deviation is greater than a certain value, it returns to the predicted trajectory elimination module; records the current turning state as the current turning state, and executes the tracking point determination module, velocity space sampling module, predicted trajectory elimination module, optimal trajectory filtering module and smoothing module in a loop until the final target point is reached.

9. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the computer program, it implements the steps of the method according to any one of claims 1 to 7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the method according to any one of claims 1 to 7.

Images