Safety Margin-Based Navigation Path Optimization Method and Related Equipment

Abstract

This application discloses a navigation path optimization method and related equipment based on safety margin, relating to the field of robot path planning technology. The navigation path optimization method based on safety margin includes: obtaining the first pixel coordinates of the target area and the second pixel coordinates of the robot's current position; identifying obstacle areas based on a pre-stored indoor scene map, expanding the obstacle areas, and calculating the Euclidean clearance distance from each point in free space to the nearest expanded obstacle area; based on the Euclidean clearance distance and a preset safety margin, performing a path search with clearance penalty, starting from the second pixel coordinates and ending at the first pixel coordinates, to obtain a safe path; and optimizing the path points of the safe path based on preset sparse optimization rules to obtain the target navigation path. This application solves the technical problem of poor navigation path planning performance for indoor quadruped robots in related technologies.

Description

Technical Field

[0001] This application relates to the field of robot path planning technology, and in particular to a navigation path optimization method and related equipment based on safety margin. Background Technology

[0002] Quadruped robots are increasingly used in indoor delivery and inspection scenarios due to their good terrain adaptability and mobility. In related technologies, autonomous navigation of quadruped robots in indoor fixed map environments is usually achieved based on occupancy grid maps. Specifically, the indoor environment is divided into regular grids, obstacle areas and free space are marked, and a path search is performed with the robot's current position as the starting point and the target point as the ending point to obtain a navigation path that the robot can pass through.

[0003] However, path search in related technologies typically aims for the shortest path length, resulting in paths that tend to closely follow obstacle edges. During actual walking, quadruped robots have a certain width, and gait swaying and positioning errors can cause the actual distance between the robot and obstacles to be less than the planned distance. This leads to collisions with walls, tables, chairs, door frames, and other obstacles, compromising navigation safety and resulting in poor path planning performance for indoor quadruped robots. Summary of the Invention

[0004] The main purpose of this application is to provide a navigation path optimization method and related equipment based on safety margin, which aims to solve the technical problem of poor navigation path planning effect of indoor quadruped robots in related technologies.

[0005] To achieve the above objectives, this application proposes a navigation path optimization method based on safety margin, the method comprising: Obtain the first pixel coordinate of the target area and the second pixel coordinate of the robot's current position; Based on the pre-stored indoor scene map, the obstacle area is identified, the obstacle area is expanded, and the Euclidean distance from each point in free space to the nearest expanded obstacle area is calculated. Based on the Euclidean clearance distance and the preset safety margin, a path search with clearance distance penalty is performed starting from the second pixel coordinates and ending at the first pixel coordinates to obtain a safe path. The safe path is optimized by path point optimization based on preset sparse optimization rules to obtain the target navigation path.

[0006] In one embodiment, the step of performing a path search with a clearance penalty, starting from the second pixel coordinates and ending at the first pixel coordinates, based on the Euclidean clearance distance and a preset safety margin, to obtain a safe path includes: Compare the European-style clearance distance with the preset safe clearance distance threshold; When the Euclidean clearance distance is less than the safe clearance distance threshold, a penalty cost is determined based on the difference between the Euclidean clearance distance and the safe clearance distance threshold, and the penalty cost is added to the basic movement cost of the path search to obtain the path search cost. When the Euclidean clearance distance is not less than the safe clearance distance threshold, the basic movement cost is used as the path search cost; Based on the path search cost, a path search is performed starting from the second pixel coordinates and ending at the first pixel coordinates to obtain a safe path.

[0007] In one embodiment, the step of expanding the barrier region includes: Obtain the robot's body dimensions, gait swing range, and positioning error; Determine the basic expansion radius based on the aforementioned fuselage dimensions; The swing margin is determined based on the gait swing range, and the error margin is determined based on the positioning error; The expansion radius is determined based on the basic expansion radius, the swing margin, and the error margin; Using the boundary of the obstacle region as a reference, the area is expanded outward according to the expansion radius to obtain the expanded obstacle region.

[0008] In one embodiment, the step of optimizing the safe path based on preset sparse optimization rules to obtain the target navigation path includes: Perform line visual detection on the path points in the safe path to determine whether the line connecting two path points crosses the obstacle area. When the connecting line does not cross the obstacle area, the redundant path point between the two path points is deleted; Calculate the segment length between adjacent path points remaining after deleting the redundant path points; When the segment length exceeds the preset maximum segment length, insert an intermediate point along the line segment direction between the adjacent path points; When the distance between the intermediate point and the first pixel coordinate is less than a preset nearest neighbor threshold, the intermediate point is replaced by the midpoint between the adjacent path points; The target navigation path is obtained based on the inserted or replaced path points.

[0009] In one embodiment, the step of obtaining the first pixel coordinates of the target area and the second pixel coordinates of the robot's current position includes: Read the first pixel coordinates pre-stored in the target region; Obtain the robot's current map coordinates; Obtain the preset map origin offset, map resolution, and grid height; Based on the map origin offset, the map resolution, and the grid height, the current map coordinates are converted into second pixel coordinates.

[0010] In one embodiment, after the step of optimizing the safe path based on a preset sparse optimization rule to obtain the target navigation path, the method further includes: Determine the target heading angle based on adjacent path points in the target navigation path; Obtain the robot's current heading angle; Calculate the heading difference between the target heading angle and the current heading angle; When the heading difference is less than a preset angle threshold, the robot maintains its current heading angle; When the heading difference is not less than the angle threshold, the heading angle of the robot is adjusted to the target heading angle.

[0011] Furthermore, to achieve the above objectives, this application also proposes a navigation path optimization device based on safety margin, the navigation path optimization device based on safety margin comprising: The acquisition module is used to acquire the first pixel coordinates of the target area and the second pixel coordinates of the robot's current position; The calculation module is used to identify obstacle areas based on a pre-stored indoor scene map, expand the obstacle areas, and calculate the Euclidean distance from each point in free space to the nearest expanded obstacle area. The planning module is used to perform a path search with a clearance penalty, starting from the second pixel coordinate and ending at the first pixel coordinate, based on the Euclidean clearance distance and a preset safety margin, to obtain a safe path. The optimization module is used to optimize the path points of the safe path based on preset sparse optimization rules to obtain the target navigation path.

[0012] Furthermore, to achieve the above objectives, this application also proposes a navigation path optimization device based on safety margin, the device comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, the computer program being configured to implement the steps of the navigation path optimization method based on safety margin as described above.

[0013] In addition, to achieve the above objectives, this application also proposes a storage medium, which is a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, it implements the steps of the navigation path optimization method based on safety margin as described above.

[0014] In addition, to achieve the above objectives, this application also provides a computer program product, which includes a computer program that, when executed by a processor, implements the steps of the navigation path optimization method based on safety margin as described above.

[0015] One or more technical solutions proposed in this application have at least the following technical effects: Compared with related technologies, this application obtains the first pixel coordinates of the target area and the second pixel coordinates of the robot's current position, identifies obstacle areas based on a pre-stored indoor scene map, and expands these obstacle areas. It calculates the Euclidean clearance distance from each point in free space to the nearest expanded obstacle area, performs path search with clearance penalty based on the Euclidean clearance distance and a preset safety margin to obtain a safe path, and optimizes the path points of the safe path based on preset sparse optimization rules to obtain the target navigation path. It is understood that this application improves path safety by calculating the Euclidean clearance distance from each point in free space to the nearest expanded obstacle area and using this distance as a penalty for path search, thus actively moving away from obstacle edges while avoiding obstacle areas. Furthermore, by optimizing the path points of the safe path based on preset sparse optimization rules, the path point density is matched with the robot's navigation protocol execution interface, thereby improving the stability of navigation execution and ultimately enhancing the navigation path planning effect of the indoor quadruped robot. Attached Figure Description

[0016] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application.

[0017] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0018] Figure 1 A flowchart illustrating an embodiment of the navigation path optimization method based on safety margin of this application; Figure 2 A system block diagram provided for an embodiment of the navigation path optimization method based on safety margin in this application; Figure 3 A system flowchart provided for an embodiment of the navigation path optimization method based on safety margin in this application; Figure 4 This is a schematic diagram of the module structure of the navigation path optimization device based on safety margin according to an embodiment of this application; Figure 5This is a schematic diagram of the device structure of the hardware operating environment involved in the navigation path optimization method based on safety margin in the embodiments of this application.

[0019] 19] The purpose, features and advantages of this application will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0020] It should be understood that the specific embodiments described herein are merely illustrative of the technical solutions of this application and are not intended to limit this application.

[0021] To better understand the technical solution of this application, a detailed description will be provided below in conjunction with the accompanying drawings and specific implementation methods.

[0022] The main solution in this application embodiment is: Obtain the first pixel coordinate of the target area and the second pixel coordinate of the robot's current position; Based on the pre-stored indoor scene map, the obstacle area is identified, the obstacle area is expanded, and the Euclidean distance from each point in free space to the nearest expanded obstacle area is calculated. Based on the Euclidean clearance distance and the preset safety margin, a path search with clearance distance penalty is performed starting from the second pixel coordinates and ending at the first pixel coordinates to obtain a safe path. The safe path is optimized by path point optimization based on preset sparse optimization rules to obtain the target navigation path.

[0023] In this embodiment, the application uses a navigation path optimization device based on safety margin as the execution subject. For ease of description, it will be referred to as "device" in detail.

[0024] Because path search in related technologies usually aims to minimize path length, the resulting path tends to be close to the edge of obstacles, causing quadruped robots to collide with obstacles during actual walking, resulting in insufficient navigation safety.

[0025] This application provides a solution that calculates the Euclidean distance in free space point by point and uses it as a penalty for path search, enabling the path to actively move away from the edge of obstacles while avoiding them, and optimizes the path points of the searched path to match the path point density with the robot navigation protocol.

[0026] Based on this, embodiments of this application provide a navigation path optimization method based on safety margin, referring to... Figure 1 , Figure 1 This is a flowchart illustrating the first embodiment of the navigation path optimization method based on safety margin of this application.

[0027] In this embodiment, the navigation path optimization method based on safety margin includes steps S10~S40: Step S10: Obtain the first pixel coordinates of the target area and the second pixel coordinates of the robot's current position; It should be noted that the target area is the delivery or inspection destination specified by the user in the indoor scene, including but not limited to rooms, meeting rooms, rest areas, service points, etc.; the first pixel coordinate of the target area is the pixel coordinate of the target area in the indoor floor plan; the second pixel coordinate of the robot's current position is the pixel coordinate of the robot's current position in the indoor floor plan.

[0028] Understandably, by unifying the target area and the robot's current position into pixel coordinates, subsequent path searching can be performed in a unified pixel coordinate system.

[0029] Step S20: Identify obstacle areas based on the pre-stored indoor scene map, expand the obstacle areas, and calculate the Euclidean distance from each point in free space to the nearest expanded obstacle area. It should be noted that the indoor scene map is a pre-stored indoor planar grid map, including but not limited to grayscale occupancy grid maps; the obstacle area is the area marked as an obstacle in the indoor scene map, such as walls, fixed furniture, and impassable areas; the expansion process is the process of expanding the obstacle area outward by a preset distance according to its boundary, and the expanded obstacle area is the area that the robot cannot enter when actually walking; the free space is the area in the indoor scene map other than the expanded obstacle area; the Euclidean distance is the Euclidean distance from each point in the free space to the nearest expanded obstacle area.

[0030] Understandably, this step uses dilation to reserve a safety margin for the robot's body size, gait swing, and positioning error, and uses Euclidean distance quantification to quantify the distance from each point to the obstacle, providing a basis for subsequent safe path search.

[0031] Step S30: Based on the Euclidean clearance distance and the preset safety margin, perform a path search with clearance distance penalty starting from the second pixel coordinates and ending at the first pixel coordinates to obtain a safe path; It should be noted that the preset safety margin is a pre-set standard used to measure the safe distance between the path and the obstacle; the path search with clearance penalty is a search method that superimposes the clearance penalty on the movement cost of the path search. When the Euclidean clearance of a certain position is less than the preset safety margin, the movement cost of that position increases with the degree of clearance deficiency, thereby causing the searched path to actively move away from the obstacle.

[0032] Understandably, this step uses clearance penalty to ensure that the path avoids obstacles while prioritizing free space with a larger safe clearance, thus reducing the risk of the robot colliding with obstacles.

[0033] Step S40: Optimize the path points of the safe path based on the preset sparse optimization rules to obtain the target navigation path.

[0034] It should be noted that the preset sparsity optimization rules are pre-defined rules used to adjust the density of path points; path point optimization is an optimization process that performs sparsification and encryption processing on path points in a secure path.

[0035] Understandably, this step optimizes path points to match the density of path points with the execution interface of the robot navigation protocol, balancing path simplicity with navigation execution stability.

[0036] This embodiment provides a navigation path optimization method based on safety margin. By expanding the obstacle area, calculating the Euclidean clearance distance, and searching the clearance distance penalty path, the path actively moves away from obstacles, and the path points of the safe path are optimized, which solves the technical problem of poor navigation path planning effect of indoor quadruped robots.

[0037] In one feasible implementation, the step of performing a path search with a clearance penalty, starting from the second pixel coordinates and ending at the first pixel coordinates, based on the Euclidean clearance distance and a preset safety margin, to obtain a safe path includes: Compare the European-style clearance distance with the preset safe clearance distance threshold; When the Euclidean clearance distance is less than the safe clearance distance threshold, a penalty cost is determined based on the difference between the Euclidean clearance distance and the safe clearance distance threshold, and the penalty cost is added to the basic movement cost of the path search to obtain the path search cost. When the Euclidean clearance distance is not less than the safe clearance distance threshold, the basic movement cost is used as the path search cost; Based on the path search cost, a path search is performed starting from the second pixel coordinates and ending at the first pixel coordinates to obtain a safe path.

[0038] It should be noted that the safe clearance threshold is a pre-set preferred safe clearance distance, which is the minimum distance expected to be maintained between a point on the path and an obstacle; the basic movement cost is the basic cost of moving between adjacent nodes in the path search; and the penalty cost is an additional cost determined based on the degree of clearance insufficiency when the Euclidean clearance distance is less than the safe clearance threshold. It can be understood that this implementation compares the Euclidean clearance distance with the safe clearance threshold. When the Euclidean clearance distance is less than the safe clearance threshold, the penalty cost increases with the degree of clearance insufficiency, ensuring that the path prioritizes passing through safe areas with sufficient clearance while maintaining reachability.

[0039] In one feasible implementation, the step of expanding the barrier region includes: Obtain the robot's body dimensions, gait swing range, and positioning error; Determine the basic expansion radius based on the aforementioned fuselage dimensions; The swing margin is determined based on the gait swing range, and the error margin is determined based on the positioning error; The expansion radius is determined based on the basic expansion radius, the swing margin, and the error margin; Using the boundary of the obstacle region as a reference, the area is expanded outward according to the expansion radius to obtain the expanded obstacle region.

[0040] It should be noted that the body size refers to the external dimensions of the quadruped robot; the gait swing range refers to the lateral swing range of the quadruped robot's body due to gait during walking; and the positioning error refers to the positional error introduced by the robot's positioning system. It is understood that this implementation method determines the expansion radius based on the body size, gait swing range, and positioning error, ensuring that the expanded obstacle area provides sufficient safety space for the robot's body, gait swing, and positioning error, preventing the path from being too close to obstacles.

[0041] In one feasible implementation, the step of optimizing the safe path based on preset sparse optimization rules to obtain the target navigation path includes: Perform line visual detection on the path points in the safe path to determine whether the line connecting two path points crosses the obstacle area. When the connecting line does not cross the obstacle area, the redundant path point between the two path points is deleted; Calculate the segment length between adjacent path points remaining after deleting the redundant path points; When the segment length exceeds the preset maximum segment length, insert an intermediate point along the line segment direction between the adjacent path points; When the distance between the intermediate point and the first pixel coordinate is less than a preset nearest neighbor threshold, the intermediate point is replaced by the midpoint between the adjacent path points; The target navigation path is obtained based on the inserted or replaced path points.

[0042] It should be noted that line visibility detection is a method of determining whether the line connecting two path points crosses an obstacle area; redundant path points are intermediate path points that can be directly reached by a straight line and do not need to be retained; maximum segment length is the maximum allowed distance between adjacent navigation points; and nearest neighbor threshold is the minimum allowed distance between an intermediate point and the target point. It can be understood that this implementation first removes redundant path points through line visibility detection, and then inserts intermediate points into path segments exceeding the maximum segment length, obtaining a path point sequence adapted to the robot navigation protocol. This avoids both frequent task switching caused by overly dense path points and navigation deviations caused by overly sparse path points.

[0043] In one feasible implementation, the step of obtaining the first pixel coordinates of the target area and the second pixel coordinates of the robot's current position includes: Read the first pixel coordinates pre-stored in the target region; Obtain the robot's current map coordinates; Obtain the preset map origin offset, map resolution, and grid height; Based on the map origin offset, the map resolution, and the grid height, the current map coordinates are converted into second pixel coordinates.

[0044] It should be noted that map coordinates refer to the robot's coordinates within the robot's map coordinate system; map origin offset is the offset of the robot's map coordinate system origin relative to the indoor floor plan coordinate system; map resolution is the actual physical size corresponding to each pixel; and grid height is the number of grids in the indoor scene map along the vertical axis. In essence, this implementation uses map origin offset, map resolution, and grid height to convert between robot map coordinates and pixel coordinates, enabling the robot to perform path searching between its current position and the target area within a unified pixel coordinate system.

[0045] In one feasible implementation, after the step of optimizing the safe path based on preset sparse optimization rules to obtain the target navigation path, the method further includes: Determine the target heading angle based on adjacent path points in the target navigation path; Obtain the robot's current heading angle; Calculate the heading difference between the target heading angle and the current heading angle; When the heading difference is less than a preset angle threshold, the robot maintains its current heading angle; When the heading difference is not less than the angle threshold, the heading angle of the robot is adjusted to the target heading angle.

[0046] It should be noted that the target heading angle is the orientation angle that the robot should maintain when moving along adjacent path points in the target navigation path; the current heading angle is the robot's current orientation angle; the heading difference is the difference between the target heading angle and the current heading angle; and the angle threshold is a preset threshold used to determine whether the heading angle needs to be adjusted. It is understood that this implementation maintains the current heading when the heading difference is less than the angle threshold, reducing the number of times the robot frequently adjusts its orientation due to small angle differences and improving the smoothness of the robot's movement.

[0047] For example, to help understand the implementation flow of the navigation path optimization method based on safety margin obtained by combining this embodiment with the above embodiment one, please refer to... Figure 2 , Figure 2 This is a system block diagram provided for an embodiment of the navigation path optimization method based on safety margin of this application, specifically: When a user clicks on "Meeting Room A" through the mobile app's target selection module and initiates a delivery task, the mobile app first displays the target area identifier, target name, and its first pixel coordinates on the floor plan corresponding to "Meeting Room A". The request is sent to the backend navigation server. The backend navigation server's status verification module immediately intercepts the request and performs pre-checks: if the robot is in a dormant state, it sends a wake-up command through the robot navigation command module and prompts the robot to initialize its position; if the positioning data is invalid, it rejects the task and returns a "reposition" prompt; if there is a navigation task in progress, it refuses to send the request again to prevent path conflicts.

[0048] After the status verification passes, the coordinate mapping module begins operation. This module obtains the robot's current position in the map coordinate system via the robot communication interface. (Unit: meters). Then, based on the preset map origin offset... Map resolution (meters / pixel) and grid height (pixels), using coordinate transformation formula: Convert the robot's current map coordinates to second-pixel coordinates. ,in, These are the converted pixel x and y coordinates; This represents the robot's current position in the map coordinate system. Offset from the preset map origin; Map resolution (meters per pixel); This represents the grid height in pixels.

[0049] Subsequently, the grid security planning module loads the pre-stored grayscale map and performs obstacle identification and expansion processing. Specifically, it selects obstacles from the map that meet the following conditions. (For example The pixels identified are then used as the original obstacles. Next, the robot's body dimensions, gait swing range, and positioning error are obtained. The basic expansion radius is determined based on the body dimensions, the swing margin is determined based on the gait swing range, and the error margin is determined based on the positioning error. Finally, the expansion radius is determined by combining these factors. Based on the boundary of the original obstacle, according to the radius... Expand outwards to obtain the expanded barrier region. The nodes in the path search must not be passed through Any point in the free space. The module then calculates each pixel in free space. European clearance distance to the nearest obstacle Next, with Starting point Using the node as the destination, a safe path is generated using the eight-neighbor A* search algorithm. During the search, starting from the node... Move to node Path search cost From the formula It is confirmed that, among them, For the node Move to node The cost of path search; Based on the distance traveled; The preset preferred safety clearance distance; For the target node European clearance distance to the nearest obstacle; The weight for the distance penalty is determined when the target node satisfies... When the distance penalty is 0, the path is searched according to the shortest distance principle; when At that time, the cost increases linearly with the degree of insufficient clearance, which makes the A* algorithm tend to select pixels located in the center of the corridor and far away from the edge of the wall, thus introducing a safety margin in the planning stage.

[0050] The planned original path is a high-density grid point sequence, which is then processed by the path point processing module to adapt to the robot navigation protocol. This module performs two levels of optimization: first, it performs sparsification, then performs line visibility detection on the path points in the original path, and determines whether the line connecting two non-adjacent path points crosses the expanded obstacle region. If the connection does not cross an obstacle, all redundant intermediate points between the two points are deleted; then encryption is performed, and the segment length between the remaining adjacent path points after sparsification is calculated. When the segment length exceeds the preset maximum navigation segment length... When inserting a midpoint along the line segment direction, if the inserted midpoint is close to the target point... If the distance to the nearest neighbor threshold is less than the threshold, the midpoint of the adjacent path point is used to replace the insertion point, thus avoiding the generation of overly close or duplicate points. Ultimately, the processed path point sequence satisfies... ,in, and Adjacent path points; The distance is Euclidean. This is the preset maximum navigation segment length.

[0051] The coordinate mapping module intervenes again, converting the processed pixel path point sequence using a formula. Convert to coordinates in the robot's map coordinate system, where, The coordinates are the converted map coordinates; The pixel coordinates of the pixel path point; For map resolution; This represents the grid height. The coordinates are offset from the map origin. The transformed coordinates are sequentially sent by the robot navigation command module via UDP protocol. During transmission, non-final points are treated as transition points, and final points as target points. For each navigation segment, the system calculates the target heading angle based on adjacent path points. ,like If the angle is less than the preset threshold, the current heading will be maintained to reduce the frequent small-angle adjustments made by the quadruped robot.

[0052] During the robot's navigation task, the position and task status monitoring module polls the robot's real-time position and task status code at a fixed frequency. When the current position is detected to be close to the target point... When the Euclidean distance is less than the threshold, the robot is determined to have arrived at meeting room A. The voice and human-machine prompt module then triggers a voice announcement for package pickup and updates the mobile interface to display "Delivered". After a preset waiting time, the automatic return and repositioning module, using the robot's current position as the starting point and the preset origin as the ending point, again calls the grid occupancy safety planning module to generate a return path. After the robot returns to the origin, the command module issues an attitude adjustment command, causing the robot to adjust to the preset orientation and enter standby mode. Through the above modular collaboration, this embodiment realizes a complete, lightweight, and safety-marginalized delivery navigation closed loop from user order placement to robot repositioning.

[0053] For example, please refer to Figure 3 , Figure 3 The system flowchart provided for the navigation path optimization method based on safety margin of this application is as follows: In this scenario, a quadruped robot needs to deliver food from the kitchen to the private dining room "Peony Hall" within the restaurant. The target area, "Peony Hall," is defined on the mobile terminal's floor plan. After the user selects "Peony Hall" and initiates the task via the mobile terminal, the system enforces strict status checks and pre-emptive interception before formal planning: First, it checks the validity of the target area, confirming that the navigation pixel coordinates of "Peony Hall" are not invalid values; then, it checks the robot's sleep state, and if it is asleep, it is remotely woken up and asked to report its initial position; next, it checks the positioning status, and if positioning is lost, the task is immediately intercepted and a "repositioning" prompt is displayed; finally, it checks the navigation task cache, and if there is already a task being executed, it returns a "delivery in progress" prompt to the mobile terminal, rejecting the issuance of new tasks to avoid multi-task conflicts.

[0054] After successful verification, the coordinate mapping module reads the robot's current map coordinates and converts them into pixel coordinates. Simultaneously read the first pixel coordinates of "Peony Hall". Subsequently, the grid safety planning module performs path planning. Unlike Embodiment 1, this embodiment features more refined configuration of obstacle expansion and clearance distance calculations to adapt to the characteristics of a restaurant environment with numerous tables and chairs and narrow passageways. The system sets obstacle recognition thresholds. To identify darker table legs and walls in a grayscale map; expansion radius The system is configured to allow for greater margin of error in the robot body and positioning; the system is configured with a preferred safe clearance distance. Setting and clearing penalty weights This forces the path to stay as close to the centerline of the narrow passageway as possible to avoid scraping against the tables and chairs on either side. The A* algorithm then searches for a safe path from the kitchen to the "Peony Hall" in the pixel space.

[0055] The path refinement module optimizes the original path output by A*. First, redundant nodes are removed through line visual detection to identify key turning points. Then, the distance between adjacent path points is checked; if the segment length exceeds [a certain value], [the module will proceed accordingly]. The pixels are then inserted at the midpoint. In this scene, more than... The system automatically inserts a transition point at the midpoint of the segment, enabling the robot navigation command module to issue navigation targets in segments, thus preventing positioning drift due to excessively long single-segment distances.

[0056] The system converts the processed pixel path points into map coordinates and distributes them sequentially, employing a strategy of "distributing transition points first, then target points." As the robot moves towards the "Peony Hall," the position and task status monitoring module continuously polls. The system obtains the difference between the robot's current position and the currently distributed target point. When this difference is less than a threshold, the system automatically switches to the next path point and distributes the coordinates until the final target point, "Peony Hall," is marked as reached.

[0057] After the robot arrives at the "Peony Hall," a "Meal Pickup Reminder" voice announcement is triggered, the mobile device status is updated synchronously, and the system enters a waiting timer. After waiting, the automatic return-to-home module is activated. Similar to Embodiment 1, the system replans the return-to-home path. However, in this embodiment, if the return-to-home path planning fails, the system records an error log and returns a clear prompt, "Return-to-home failed, please intervene manually," enhancing long-term robustness. Throughout the robot's journey from the kitchen to the "Peony Hall," the system calculates the target heading angle in real time. The heading angle is determined by the adjacent path points using the following formula: in, For the first The target heading angle for each navigation segment; It is the arctangent function in the four quadrants; and The first The pixel coordinates of the start and end points of each navigation segment.

[0058] The system detected the last straight segment before entering the private room. The angle is only 2 degrees, which is less than the preset 5-degree angle threshold, therefore the current heading angle will be maintained. This consistent process prevents the robot from making unnecessary turning movements due to minor deviations, ensuring a stable walking posture. After successfully delivering the goods and returning to the origin, the system issues a "sit down" standby command, awaiting the next task. Through the above process control, this embodiment ensures that the quadruped robot can stably, safely, and efficiently complete delivery tasks in complex catering environments.

[0059] It should be noted that the above examples are only for understanding this application and do not constitute a limitation on the navigation path optimization method based on safety margin of this application. Any simple modifications based on this technical concept are within the protection scope of this application.

[0060] This application also provides a navigation path optimization device based on safety margin, please refer to... Figure 4 The navigation path optimization device based on safety margin includes: The acquisition module 10 is used to acquire the first pixel coordinates of the target area and the second pixel coordinates of the robot's current position; The calculation module 20 is used to identify obstacle areas based on a pre-stored indoor scene map, perform dilation processing on the obstacle areas, and calculate the Euclidean distance from each point in free space to the nearest dilated obstacle area. The planning module 30 is used to perform a path search with a clearance penalty based on the Euclidean clearance distance and a preset safety margin, starting from the second pixel coordinates and ending at the first pixel coordinates, to obtain a safe path. The optimization module 40 is used to optimize the path points of the safe path based on preset sparse optimization rules to obtain the target navigation path.

[0061] The safety margin-based navigation path optimization device provided in this application, employing the safety margin-based navigation path optimization method in the above embodiments, can solve the technical problem of poor navigation path planning performance for indoor quadruped robots in related technologies. Compared with the prior art, the beneficial effects of the safety margin-based navigation path optimization device provided in this application are the same as those of the safety margin-based navigation path optimization method provided in the above embodiments, and other technical features in the safety margin-based navigation path optimization device are the same as those disclosed in the methods of the above embodiments, and will not be repeated here.

[0062] This application provides a navigation path optimization device based on safety margin. The navigation path optimization device based on safety margin includes: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to execute the navigation path optimization method based on safety margin in the first embodiment described above.

[0063] The following is for reference. Figure 5 This document illustrates a structural schematic diagram of a safety margin-based navigation route optimization device suitable for implementing embodiments of this application. The safety margin-based navigation route optimization device in the embodiments of this application may include, but is not limited to, mobile terminals such as mobile phones, tablets, laptops, digital broadcast receivers, PDAs (Personal Digital Assistants), PMPs (Portable Media Players), in-vehicle terminals (e.g., in-vehicle navigation terminals), and fixed terminals such as digital televisions and desktop computers. Figure 5 The navigation path optimization device based on safety margin shown is merely an example and should not impose any limitations on the functionality and scope of use of the embodiments of this application.

[0064] like Figure 5As shown, the safety margin-based navigation path optimization device may include a processing unit 1001, which can perform various appropriate actions and processes according to a program stored in ROM 1002 or a program loaded from storage device 1003 into RAM 1004. RAM 1004 also stores various programs and data required for the operation of the safety margin-based navigation path optimization device. The processing unit 1001, ROM 1002, and RAM 1004 are interconnected via bus 1005. I / O interface 1006 is also connected to the bus. Typically, the following systems can be connected to I / O interface 1006: input devices 1007 including, for example, touch screens, touchpads, keyboards, mice, image sensors, microphones, accelerometers, gyroscopes, etc.; output devices 1008 including, for example, liquid crystal displays (LCDs), speakers, vibrators, etc.; storage devices 1003 including, for example, magnetic tapes, hard disks, etc.; and communication devices 1009. Communication device 1009 allows the safety margin-based navigation path optimization device to communicate wirelessly or wiredly with other devices to exchange data. While the figures show safety margin-based navigation path optimization devices with various systems, it should be understood that implementation or possession of all the systems shown is not required. More or fewer systems may be implemented alternatively.

[0065] Specifically, according to the embodiments disclosed in this application, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments disclosed in this application include a computer program product comprising a computer program carried on a computer-readable medium, the computer program containing program code for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via a communication device, or installed from storage device 1003, or installed from ROM 1002. When the computer program is executed by processing device 1001, it performs the functions defined in the methods of the embodiments disclosed in this application.

[0066] The safety margin-based navigation path optimization device provided in this application, employing the safety margin-based navigation path optimization method in the above embodiments, can solve the technical problem of poor navigation path planning performance for indoor quadruped robots in related technologies. Compared with the prior art, the beneficial effects of the safety margin-based navigation path optimization device provided in this application are the same as those of the safety margin-based navigation path optimization method provided in the above embodiments, and other technical features of this safety margin-based navigation path optimization device are the same as those disclosed in the previous embodiment method, and will not be repeated here.

[0067] It should be understood that the various parts disclosed in this application can be implemented using hardware, software, firmware, or a combination thereof. In the description of the above embodiments, specific features, structures, materials, or characteristics can be combined in any suitable manner in one or more embodiments or examples.

[0068] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

[0069] This application provides a computer-readable storage medium having computer-readable program instructions (i.e., a computer program) stored thereon, the computer-readable program instructions being used to execute the navigation path optimization method based on safety margin in the above embodiments.

[0070] The computer-readable storage medium provided in this application may be, for example, a USB flash drive, but is not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices, or any combination thereof. More specific examples of computer-readable storage media may include, but are not limited to: electrical connections having one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof. In this embodiment, the computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, system, or device. The program code contained on the computer-readable storage medium may be transmitted using any suitable medium, including but not limited to: wires, optical cables, RF (Radio Frequency), etc., or any suitable combination thereof.

[0071] The aforementioned computer-readable storage medium may be included in a safety margin-based navigation path optimization device; or it may exist independently and not assembled into a safety margin-based navigation path optimization device.

[0072] The aforementioned computer-readable storage medium carries one or more programs that, when executed by a safety margin-based navigation path optimization device, cause the safety margin-based navigation path optimization device to: acquire the first pixel coordinates of the target area and the second pixel coordinates of the robot's current position; Based on the pre-stored indoor scene map, the obstacle area is identified, the obstacle area is expanded, and the Euclidean distance from each point in free space to the nearest expanded obstacle area is calculated. Based on the Euclidean clearance distance and the preset safety margin, a path search with clearance distance penalty is performed starting from the second pixel coordinates and ending at the first pixel coordinates to obtain a safe path. The safe path is optimized by path point optimization based on preset sparse optimization rules to obtain the target navigation path.

[0073] Computer program code for performing the operations of this application can be written in one or more programming languages ​​or a combination thereof. These programming languages ​​include object-oriented programming languages ​​such as Java, Smalltalk, and C++, as well as conventional procedural programming languages ​​such as C or similar languages. The program code can be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving remote computers, the remote computer can be connected to the user's computer via any type of network—including a local area network (LAN) or a wide area network (WAN)—or can be connected to an external computer (e.g., via the Internet using an Internet service provider).

[0074] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of this application. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions.

[0075] The modules described in the embodiments of this application can be implemented in software or hardware. The names of the modules do not necessarily limit the functionality of the unit itself.

[0076] The readable storage medium provided in this application is a computer-readable storage medium that stores computer-readable program instructions (i.e., a computer program) for executing the above-described safety margin-based navigation path optimization method, which can solve the technical problem of poor navigation path planning performance of indoor quadruped robots in related technologies. Compared with the prior art, the beneficial effects of the computer-readable storage medium provided in this application are the same as the beneficial effects of the safety margin-based navigation path optimization method provided in the above embodiments, and will not be repeated here.

[0077] This application also provides a computer program product, including a computer program that, when executed by a processor, implements the steps of the navigation path optimization method based on safety margin as described above.

[0078] The computer program product provided in this application can solve the technical problem of poor navigation path planning performance for indoor quadruped robots in related technologies. Compared with the prior art, the beneficial effects of the computer program product provided in this application are the same as those of the navigation path optimization method based on safety margin provided in the above embodiments, and will not be repeated here.

[0079] All acquisition of signals, information, or actions in this application are carried out in compliance with the relevant data protection laws and policies of the country where the application is located, and with the authorization of the relevant device owner.

[0080] The above description is only a part of the embodiments of this application and does not limit the scope of protection of this application. All equivalent structural transformations made under the technical concept of this application and using the content of this application specification and drawings, or direct / indirect applications in other related technical fields, are included in the scope of protection of this application.

Claims

1. A navigation path optimization method based on safety margin, characterized in that, The method, applied to indoor quadruped robots, includes: Obtain the first pixel coordinate of the target area and the second pixel coordinate of the robot's current position; Based on the pre-stored indoor scene map, the obstacle area is identified, the obstacle area is expanded, and the Euclidean distance from each point in free space to the nearest expanded obstacle area is calculated. Based on the Euclidean clearance distance and the preset safety margin, a path search with clearance distance penalty is performed starting from the second pixel coordinates and ending at the first pixel coordinates to obtain a safe path. The safe path is optimized by path point optimization based on preset sparse optimization rules to obtain the target navigation path.

2. The method as described in claim 1, characterized in that, The step of performing a path search with a clearance penalty, starting from the second pixel coordinates and ending at the first pixel coordinates, based on the Euclidean clearance distance and a preset safety margin, to obtain a safe path includes: Compare the European-style clearance distance with the preset safe clearance distance threshold; When the Euclidean clearance distance is less than the safe clearance distance threshold, a penalty cost is determined based on the difference between the Euclidean clearance distance and the safe clearance distance threshold, and the penalty cost is added to the basic movement cost of the path search to obtain the path search cost. When the Euclidean clearance distance is not less than the safe clearance distance threshold, the basic movement cost is used as the path search cost; Based on the path search cost, a path search is performed starting from the second pixel coordinates and ending at the first pixel coordinates to obtain a safe path.

3. The method as described in claim 1, characterized in that, The step of expanding the obstacle region includes: Obtain the robot's body dimensions, gait swing range, and positioning error; Determine the basic expansion radius based on the aforementioned fuselage dimensions; The swing margin is determined based on the gait swing range, and the error margin is determined based on the positioning error; The expansion radius is determined based on the basic expansion radius, the swing margin, and the error margin; Using the boundary of the obstacle region as a reference, the area is expanded outward according to the expansion radius to obtain the expanded obstacle region.

4. The method as described in claim 1, characterized in that, The step of optimizing the safe path based on preset sparse optimization rules to obtain the target navigation path includes: Perform line visual detection on the path points in the safe path to determine whether the line connecting two path points crosses the obstacle area. When the connecting line does not cross the obstacle area, the redundant path point between the two path points is deleted; Calculate the segment length between adjacent path points remaining after deleting the redundant path points; When the segment length exceeds the preset maximum segment length, insert an intermediate point along the line segment direction between the adjacent path points; When the distance between the intermediate point and the first pixel coordinate is less than a preset nearest neighbor threshold, the intermediate point is replaced by the midpoint between the adjacent path points; The target navigation path is obtained based on the inserted or replaced path points.

5. The method as described in claim 1, characterized in that, The steps of obtaining the first pixel coordinates of the target area and the second pixel coordinates of the robot's current position include: Read the first pixel coordinates pre-stored in the target region; Obtain the robot's current map coordinates; Obtain the preset map origin offset, map resolution, and grid height; Based on the map origin offset, the map resolution, and the grid height, the current map coordinates are converted into second pixel coordinates.

6. The method as described in claim 1, characterized in that, After the step of optimizing the safe path based on preset sparse optimization rules to obtain the target navigation path, the method further includes: Determine the target heading angle based on adjacent path points in the target navigation path; Obtain the robot's current heading angle; Calculate the heading difference between the target heading angle and the current heading angle; When the heading difference is less than a preset angle threshold, the robot maintains its current heading angle; When the heading difference is not less than the angle threshold, the heading angle of the robot is adjusted to the target heading angle.

7. A navigation path optimization device based on safety margin, characterized in that, The device includes: The acquisition module is used to acquire the first pixel coordinates of the target area and the second pixel coordinates of the robot's current position; The calculation module is used to identify obstacle areas based on a pre-stored indoor scene map, expand the obstacle areas, and calculate the Euclidean distance from each point in free space to the nearest expanded obstacle area. The planning module is used to perform a path search with a clearance penalty, starting from the second pixel coordinate and ending at the first pixel coordinate, based on the Euclidean clearance distance and a preset safety margin, to obtain a safe path. The optimization module is used to optimize the path points of the safe path based on preset sparse optimization rules to obtain the target navigation path.

8. A navigation path optimization device based on safety margin, characterized in that, The device includes: a memory, a processor, and a computer program stored in the memory and executable on the processor, the computer program being configured to implement the steps of the navigation path optimization method based on safety margin as described in any one of claims 1 to 6.

9. A storage medium, characterized in that, The storage medium is a computer-readable storage medium, and a computer program is stored on the storage medium. When the computer program is executed by a processor, it implements the steps of the navigation path optimization method based on safety margin as described in any one of claims 1 to 6.

10. A computer program product, characterized in that, The computer program product includes a computer program that, when executed by a processor, implements the steps of the navigation path optimization method based on safety margin as described in any one of claims 1 to 6.

Images