Obstacle marking method based on curve fitting

Abstract

The present application discloses an obstacle marking method based on curve fitting, comprising: step S1, a robot controls a laser sensor to collect obstacle contour points at a current position point by rotating and scanning the laser probe; step S2, the robot uses the obstacle contour points collected at the current position point to perform curve fitting to generate an obstacle fitting curve; step S3, the robot sets a plurality of line segments on a side close to the obstacle fitting curve; step S4, the robot converts the position points passed by each line segment in the obstacle fitting curve into a grid map, and then selects a valid obstacle grid based on the occupancy probability value corresponding to the grid where the passed position point is located; step S5, the robot sequentially splices the selected valid obstacle grids into the effective contour of the obstacle in the grid map along the target planning direction, and then configures the direction of the effective contour of the obstacle in the grid map as the direction in which the robot walks along the contour of the obstacle.

Description

Technical Field

[0001] The present invention relates to the technical field of laser scanning objects, and in particular to an obstacle marking method based on curve fitting. Background Art

[0002] Currently, most sweeping robots use the obstacle grid marked in the grid map during navigation and obstacle avoidance operations. The obstacle outline is represented by the grid in the grid map. Due to the resolution of the cells in the grid map, when a single grid is used to represent a position point actually occupied by an obstacle, there is a distance error consistent with the size corresponding to the resolution (for example, if the grid resolution is 10mm, a 10mm error will be generated when it is used directly to locate the obstacle). Therefore, planning the path directly according to the grid map will cause grid errors and often lose some obstacle contour points that should have been collected. After the laser point cloud collected by the robot through the laser sensor is directly converted to the grid map, the marked obstacle grid is less representative, resulting in the direction of the contour line spliced ​​together in sequence being unable to guide the robot to bypass the obstacle where the contour line is located. Summary of the Invention

[0003] This application discloses an obstacle marking method based on curve fitting. The specific technical solution is as follows:

[0004] An obstacle marking method based on curve fitting includes: step S1, where a robot controls a laser sensor to collect obstacle contour points at a current position by rotating and scanning the laser probe; step S2, where the robot performs curve fitting using the obstacle contour points collected at the current position to generate an obstacle fitting curve; step S3, where the robot sets a plurality of line segments on a side of the robot close to the obstacle fitting curve so that the set line segments pass through the obstacle fitting curve; step S4, where the robot converts the position points passed by each line segment in the obstacle fitting curve into a grid map, and then selects valid obstacle grids based on the occupancy probability values ​​corresponding to the grids where the position points passed by each line segment in the obstacle fitting curve are located; step S5, where the robot sequentially stitches the selected valid obstacle grids into a valid outline of the obstacle in the grid map along a target planning direction, and then configures the direction of the valid outline of the obstacle in the grid map as the direction in which the robot moves along the outline of the obstacle.

[0005] The technical effects of the aforementioned steps S1 to S5 are as follows: Step S2 uses the obstacle contour points to perform curve fitting to fit the obstacle fitting curve and its trajectory equation, thereby achieving smoothing of the obstacle contour; compared with the prior art that directly uses grid or point cloud data to splice the trajectory, the present technical solution can improve the accuracy and smoothness of the fitted trajectory; then, Step S3 converts the position points passed by each line segment in the obstacle fitting curve into a grid map; Step S4 selects corresponding valid obstacle grids from the grid map based on the occupancy probability values ​​of the grids where the position points passed by the line segment in the obstacle fitting curve are located, so as to splice them into the effective contour of the obstacle in the grid map in Step S5, and configures the direction of the effective contour of the obstacle in the grid map as the direction in which the robot walks along the contour of the obstacle, thereby guiding the robot to bypass the obstacle where the contour line is located. Therefore, the aforementioned steps S1 to S5 select a valid obstacle grid (belonging to the obstacle grid) based on the occupancy probability value corresponding to the grid where the obstacle contour point is located. By limiting each line segment and occupancy probability value within the grid area, representative obstacle grids can be selected in sequence from the smoothed obstacle fitting curve while consuming less coordinate conversion and grid search. To a certain extent, this can compensate for the grid error caused by the obstacle contour points that should have been collected, and provide reliable direction parameter and distance parameter support for the robot to walk along the outline of the obstacle.

[0006] Furthermore, step S2 includes: determining the starting point and the end point of a Bezier curve along the target planning direction from the obstacle contour points collected by the robot at the same position; the robot sequentially marks each obstacle contour point distributed along the target planning direction between the starting point and the end point of the Bezier curve as an intermediate point required for fitting the Bezier curve; generating a trajectory equation of the obstacle fitting curve according to the starting point, the end point, and the intermediate point of the Bezier curve, and determining to generate the obstacle fitting curve; wherein the sum of the number of the starting point, the end point, and the intermediate point of the Bezier curve is equal to the sum of the order of the Bezier curve and the value 1. This technical solution, while determining each obstacle contour point as the starting point, end point, and intermediate point, fits a Bezier curve distributed along the obstacle contour along the target planning direction, offsetting the grid errors existing in the position points originally marked using the raster map, and filling the contour sampling points that may be lost when using the raster map mark, playing a role in leveling the errors. For example, a relatively rough wall can be flattened by curve fitting, and the final mark is a smooth trajectory, which smoothes the obstacle contour.

[0007] Furthermore, the order of the Bezier curve is represented by n; the trajectory equation of the obstacle fitting curve is:

[0008]

[0009] Among them, the trajectory equation of an obstacle fitting curve represents the trajectory equation required to fit an obstacle fitting curve; B x (t) is the horizontal coordinate of the point in the obstacle fitting curve, B y (t) is the ordinate of the point in the obstacle fitting curve, i represents the sequence number of the control point required for the obstacle fitting curve, and t is equal to the ratio between i and (n+1); the starting point, end point and middle point of the Bezier curve are all control points required for Bezier curve fitting; P ix Represented as the control point P i The horizontal coordinate of the fitting target point, P iy Represented as the control point P i The vertical coordinate of the fitting target point; P 0x The horizontal coordinate of the starting point of the Bezier curve, P 0y represents the ordinate of the starting point of the Bezier curve; nx The abscissa of the end point of the Bezier curve, P ny This technical solution uses a specific Bezier curve mathematical model to accurately fit the obstacle fitting curve, improving the accuracy of reconstructing the contours of irregularly shaped obstacles and providing important parameters such as direction and distance for subsequent obstacle avoidance decisions.

[0010] Furthermore, the obstacle fitting curve is a Bezier curve; the number of values ​​of t is equal to the number of points inserted to fit the obstacle fitting curve; during the nth-order Bezier curve fitting process, t increases unidirectionally from 0 to 1, extending from the starting point of the Bezier curve to the end point of the Bezier curve, and the inserted points are sequentially connected to form the obstacle fitting curve; the tangent lines connecting the points forming the obstacle fitting curve are used to plan the direction required for the robot to walk along the outline of the currently scanned obstacle. This allows the robot to track side obstacles based on the trajectory points of the obstacle fitting curve.

[0011] Furthermore, when t = 0, the starting point of the Bezier curve is obtained by substituting the trajectory equation of the obstacle fitting curve into the equation; when t = 1, the end point of the Bezier curve is obtained by substituting the trajectory equation of the obstacle fitting curve into the equation. The starting point of the Bezier curve is the obstacle contour point with the smallest coordinate value in the target planning direction among the obstacle contour points collected by the robot at the same position, forming the obstacle contour point closest to the robot body center in the same step S1. The end point of the Bezier curve is the obstacle contour point with the largest coordinate value in the target planning direction among the obstacle contour points collected by the robot at the same position, forming the obstacle contour point farthest from the robot body center in the same step S1. The obstacle contour point is the edge position of the obstacle scanned by the robot in a two-dimensional plane. Subsequently, the robot can be controlled to mark valid obstacle grids in the grid map in a direction from the starting point of the Bezier curve closest to the robot body to the end point of the Bezier curve, thereby determining the position of the side of the obstacle closest to the robot body.

[0012] Furthermore, in step S3, the robot sets a plurality of mutually parallel line segments along the target planning direction on its side close to the obstacle fitting curve, and each line segment is configured to extend toward the obstacle fitting curve; wherein, the distance between the extension starting point of each line segment and the body center of the robot in the preset detection direction is less than the body radius of the robot, and the distance between the extension end point of each line segment and the body center of the robot in the preset detection direction is greater than the body radius of the robot; wherein, the distance between the extension starting point of each line segment and the edge of the robot on the side close to the obstacle fitting curve is less than the distance between the extension end point of the same line segment and the edge of the robot on the side close to the obstacle fitting curve; wherein, the preset detection direction is perpendicular to the target planning direction; and the body radius of the robot represents half of the body width of the robot.

[0013] The obstacle fitting curve fitted in step S2 provides specific direction parameters and distance parameters for the obstacle contour along which the robot detects. Since the set line segment passes through the edge of the robot and the contour of the obstacle along which it is detected, each line segment within the grid map area defined by the multiple line segments set in step S3 is used to orderly obtain each position point in the obstacle fitting curve along the preset detection direction. This is equivalent to screening out each obstacle point with representative direction from the obstacle fitting curve.

[0014] Therefore, this technical solution is based on the target planning direction and the extension direction of the parallel line segments, so that the robot can obtain the direction of the effective obstacle grid more clearly, avoiding step S4 to select the effective obstacle grid in any direction, and thus can quickly obtain the effective obstacle grid that can connect the actual direction of the obstacle outline.

[0015] Furthermore, among the multiple parallel line segments set by the robot, the vertical distance from the robot's body center to the line segment farthest from the robot's body center is equal to the robot's body radius; the vertical distance from the robot's body center to the line segment closest to the robot's body center is equal to 0; and among the multiple parallel line segments set by the robot, the robot configures the spacing between two adjacent line segments to be equal to the resolution of a single grid. Therefore, setting multiple parallel line segments is equivalent to sequentially setting multiple equally spaced line segments along the target planning direction. Each line segment can be considered to extend along a preset detection direction in order to pass through the obstacle fitting curve, and is distributed according to the grid resolution so that the offset error caused by the grid resolution is taken into account between the points where the obstacle fitting curve passes between two adjacent line segments.

[0016] Furthermore, in step S4, the method for selecting valid obstacle grids based on the occupancy probability values ​​corresponding to the grids where each line segment passes through the obstacle fitting curve is located includes: for a position point passed by the same line segment in the obstacle fitting curve, selecting the grid with the highest occupancy probability value from the grids hit by the same position point in the grid map, and then marking the grid with the highest occupancy probability value as the valid obstacle grid passed by the same line segment in the grid map. After the robot obtains the grid where the obstacle point with the highest occupancy probability value matches a line segment in step S4, it accurately locates the contour point of the obstacle passed by the line segment at the current moment. Furthermore, by inducing grid entry probability information, the amount of marking is reduced and the error problem of the same obstacle point being marked / hit simultaneously in multiple grids is overcome. If the robot walks along the contour formed by the valid obstacle grids, the stability of the robot's edge walking can be guaranteed.

[0017] Furthermore, after an obstacle point hits a grid within the grid map, the hit grid is marked as the grid where the obstacle point is located, and the grid where the obstacle point is located is assigned a hit probability value. Each point along the obstacle fitting curve where a line segment passes is marked as an obstacle point. Whenever an obstacle point hits a corresponding grid, the hit probability values ​​previously assigned to the corresponding grid are accumulated to obtain an occupancy probability value for the corresponding grid, so that the occupancy probability value for the same grid is updated when the number of hits by the obstacle point changes. The accumulated hit probability value is then updated to the occupancy probability value, so that the hit probability value assigned to the grid where the obstacle point is located is updated to the occupancy probability value. By using the probability value accumulation result, the accuracy of obstacle marking within the grid map is improved, allowing the grid occupancy probability value to more accurately reflect the distribution of obstacles around the robot.

[0018] Furthermore, after the robot collects the obstacle contour point in step S1, the position coordinates of the obstacle contour point are set as the local coordinates formed by the obstacle contour point relative to the current position point of the robot, wherein the local coordinates are located in the robot coordinate system, the current position point of the robot is the origin of the robot coordinate system, and the current position point of the robot is represented by the body center of the robot; when the robot converts the position coordinates of the position point passed by each line segment in the obstacle fitting curve into the global map coordinate system used to construct the grid map, the grid coordinates of the obstacle point are obtained, and the grid where the obstacle point is located is determined in the grid map to form the corresponding grid in the obstacle point hit grid map; the position point passed by each line segment in the obstacle fitting curve is marked as an obstacle point; wherein the preset detection direction is parallel to the wheel axle of the robot, and the line segment set in step S2 is parallel to the preset detection direction; the positive direction of the ordinate axis of the robot coordinate system is the same as the target planning direction, and the positive direction of the abscissa axis of the robot coordinate system is the same as the preset detection direction. A standard scanning direction is provided for the obstacle points that the line segment passes through. When obtaining grid coordinates, the corresponding coordinate axis directions can be used as the preset detection direction and target planning direction, respectively, to reduce the amount of rotation calculations on the angle. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 The present invention is a flowchart of an obstacle marking method based on curve fitting disclosed in an embodiment.

[0020] Figure 2 An embodiment discloses a schematic diagram of a robot marking effective obstacle grids in a grid map based on setting multiple line segments passing through obstacle fitting curves. DETAILED DESCRIPTION

[0021] In order to more clearly illustrate the present invention, specific embodiments are provided below for further explanation. In the following description, for the purpose of illustration rather than limitation, specific details such as specific system structures and technologies are provided to provide a thorough understanding of the embodiments of the present application. However, it will be clear to those skilled in the art that the present application can also be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present application.

[0022] It should be understood that when used in this application, the term "comprising" indicates the presence of the described features, integers, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or their collections. It should also be understood that the term "and / or" used in this application refers to any combination of one or more of the associated listed items and all possible combinations, including these combinations.

[0023] As used in this application, the term "if" can be interpreted as "when" or "upon" or "in response to determining" or "in response to detecting," depending on the context. Similarly, the phrase "if it is determined" or "if [described condition or event] is detected" can be interpreted as meaning "upon determination" or "in response to determining" or "upon detection of [described condition or event]" or "in response to detecting [described condition or event]," depending on the context.

[0024] In addition, in the description of the present application, the terms "first", "second", "third", etc. are only used to distinguish the description and are not to be understood as indicating or implying relative importance. References to "one embodiment" or "some embodiments" etc. described in the specification of the present application mean that one or more embodiments of the present application include specific features, structures or characteristics described in conjunction with the embodiment. Therefore, the statements "in one embodiment", "in some embodiments", "in some other embodiments", "in some other embodiments", etc. that appear in different places in this specification do not necessarily refer to the same embodiment, but mean "one or more but not all embodiments", unless otherwise specifically emphasized in other ways. The terms "including", "comprising", "having" and their variations all mean "including but not limited to", unless otherwise specifically emphasized in other ways.

[0025] As an embodiment, a curve fitting-based obstacle marking method is disclosed in conjunction with the accompanying drawings. The execution subject of the obstacle marking method is a self-moving device waiting to walk along the outline of the obstacle, such as a robot. The robot may include a sweeping robot, a guide robot, a patrol robot, a lawn mowing robot, etc. The robot's body shape can be Figure 2 The center of the disc-shaped body located on the left side of the obstacle L1 is represented by position point R1, which is the center of the projection area of ​​the robot's body on the two-dimensional plane; the robot's body is equipped with a laser sensor, such as a lidar, which is used to detect obstacles in a rotating scanning manner, and supports the robot to build a grid map of the surrounding environment scanned, and mark the contour information of the robot and the obstacles around it according to the grid map.

[0026] like Figure 1 As shown, the obstacle marking method includes:

[0027] In step S1, the robot controls the laser sensor to collect obstacle contour points at its current location using a rotating laser probe. The laser sensor, equipped with a rotatable laser probe, collects obstacle contour points while the robot is positioned at its current location. The laser probe emits a laser beam and rotates 360 degrees around the center of the laser sensor, collecting obstacle contour points. The robot then proceeds to step S2. It should be noted that obstacle contour points are reflection points formed by the laser beam emitted by the laser sensor reflecting off the surface of an obstacle near the robot's side. These reflection points provide the two-dimensional coordinate and angle information of the detected (or scanned) obstacle.

[0028] Since it is the result of the laser probe's rotation scanning, the robot can collect multiple obstacle contour points at one time; in step S1, each time the laser probe rotates one circle, it provides the robot with multiple point cloud points, which is recorded as the robot collecting a frame of point cloud. Each point cloud point can correspond to an obstacle contour point at an angle, so the robot can collect obstacle contour points according to the point cloud frame. In some embodiments, the robot obtains the grid where the obstacle contour point is located and its corresponding occupancy probability value in the grid map. The position coordinates of the obstacle contour point can be the coordinates determined by the robot coordinate system (rectangular coordinate system) with the robot's body center as the origin, and are obtained by converting the point cloud points output by the laser sensor from the laser coordinate system. The laser coordinate system is a polar coordinate system with the center of the laser sensor (also regarded as the rotation center of the laser probe) as the center. It is calculated by the rigid connection relationship between the center of the laser sensor and the body center of the robot. The trigonometric function transformation relationship that can be mastered by those skilled in the art is used to establish a rigid connection relationship between the center of the laser sensor and the center of the robot body. The rigid connection relationship and the specific transformation equations between the robot coordinate system and the laser coordinate system involve the transformation between the polar coordinate system and the rectangular coordinate system, and the transformation between polar coordinate systems. These are all technical means commonly used by those skilled in the art and will not be elaborated here.

[0029] In step S2, the robot performs curve fitting using the obstacle contour points collected at its current position. Specifically, the robot performs curve fitting on the obstacle contour points collected in step S1 to generate an obstacle fitting curve. The robot then proceeds to step S3. The robot performs curve fitting in step S3 at its current position within the robot coordinate system, thereby generating a trajectory equation for the obstacle fitting curve within the robot coordinate system. In order to curve fit a smooth curve for the robot to select trajectory points, this embodiment uses Bezier curves for curve fitting. Generally, since the obstacle contour points are pre-represented using a grid map, and the robot directly bypasses the obstacle based on the grid map, grid error will be introduced (determined by the resolution of the grid map. Here, the grid resolution is 10mm, and there will be a 10mm positioning coordinate error when it is directly used to bypass the obstacle). As a result, the laser point cloud data detected during the movement of differential robots such as sweeping robots will continue to jitter. Therefore, the curve fitting performed in this embodiment is equivalent to smoothing the obstacle contour points. The smoothness of the fitted curve is less than the grid resolution, thereby leveling the error and filling the obstacle contour points lost due to the grid resolution. This makes the connected obstacle contour smoother, which is suitable for robots planned to walk along the edge parallel to it.

[0030] Step S3: The robot sets a plurality of line segments on the side close to the obstacle fitting curve so that the set line segments pass through the obstacle fitting curve; then the robot executes step S4. The line segments set in step S3 may pass through at least one trajectory point in the obstacle fitting curve, and the trajectory points passed through may be obstacle contour points. Figure 2 As shown in the figure, a plurality of arrowed line segments (excluding the coordinate axis Xr) are set on the right side of the robot body center R1, and the plurality of arrowed line segments pass through the obstacle fitting curve ( Figure 2 The intersection of the obstacle fitting curve and the line segment is represented by the black dot shown in the figure. In step S3, multiple line segments are set near one side of the obstacle fitting curve to define a specific obstacle-occupied area. Although there is spacing between different line segments, the length of each line segment is finite, forming a detection window traversed by line segments. Furthermore, the multiple line segments are given specific extension directions and can be regarded as rays within a certain range of the two-dimensional plane. This allows the search to identify the trajectory points on the obstacle fitting curve that need to be extracted in the specified direction.

[0031] Step S4: The robot converts the position points that each line segment passes through in the obstacle fitting curve into the grid map, and then selects the valid obstacle grid based on the occupancy probability value corresponding to the grid where the position points that each line segment passes through in the obstacle fitting curve are located; then the robot executes step S5. After each position point that each line segment passes through in the obstacle fitting curve is converted into the grid map, the grid where each position point is located is as follows: Figure 2 As shown in the black cells in . The number of position points that the same line segment passes through in the obstacle fitting curve can be at least one; when converted to the grid map, a position point that passes through can hit at least one grid, so it is necessary to further screen the corresponding occupancy probability values ​​of the grids to obtain a valid obstacle grid. Step S4 uses the technical means of coordinate system conversion to convert the position points that the same line segment passes through in the obstacle fitting curve from the robot coordinate system to the grid map (a map composed of multiple unit cells with a certain resolution) in real time, and obtain the grid where the position point passes, that is, the grid hit by the position point in the grid map. At the same time, the corresponding grid coordinates and the hit probability value of the position point in the grid are also obtained. The hit probability value of the position point in the grid is used to represent the possibility that the grid is occupied by an obstacle. The accumulated value of the hit probability value is updated to the occupancy probability value, so that the hit probability value assigned to the grid where the corresponding position point is located is updated to the occupancy probability value. The updated grid map more accurately reflects the position actually occupied by the obstacle.

[0032] In the implementation scenario of step S4, since the position point obtained by the robot in the robot coordinate system may have a coordinate offset error or an angle deflection error after being converted to the coordinate system of the grid map, an obstacle contour point or each trajectory point in the obstacle fitting curve obtained by rotating the laser probe one or more times may hit multiple grids after the coordinate system conversion. Figure 2 When the straight obstacle L1 is perpendicular to the line segment and the size of the straight obstacle L1 in the direction perpendicular to the straight obstacle L1 only occupies one grid size, in fact, the curve AB and the obstacle contour points collected by scanning the straight obstacle L1 may occupy at least two grids in the direction perpendicular to the straight obstacle L1 (corresponding to Figure 2 Thus, a position point actually occupied by a fixed obstacle or a trajectory point obtained by curve fitting in the robot coordinate system may be marked as consisting of multiple obstacle grids in the grid map.

[0033] Regardless of whether the grid occupancy probability value changes, the robot can be reduced to a point, which can be expressed as Figure 2The present embodiment utilizes the hit probability values ​​of different position points actually collected from the robot coordinate system falling into the same grid position of the grid map to continuously update the occupancy probability values ​​of the corresponding hit grids in the grid map. At this time, the grid map is updated. Therefore, compared with the prior art, representative obstacle grids can be selected in sequence from the smoothed obstacle fitting curve by limiting each line segment and occupancy probability value within the grid area while consuming less coordinate conversion and grid search.

[0034] It is worth noting that a grid map with a certain resolution allows a certain number of point clouds to hit, and is composed of a certain number of grids corresponding to the hits of the point clouds. In some embodiments, it is constructed from a probability grid [Pmin, Pmax] of 5cm*5cm size. Whether it is a two-dimensional grid map or a three-dimensional grid map, when the map is created, the occupancy probability value of the grid is less than Pmin, indicating that there is no obstacle at the grid position, between Pmin and Pmax, indicating unknown, and greater than Pmax, indicating that there is an obstacle at the grid position. Each frame of the laser point cloud will generate a batch of grids. In this batch of grids, each grid is assigned a probability value. If the grid already has a probability value, the probability value of the grid needs to be updated. The above-mentioned occupancy probability value can be used to indicate the possibility of the grid being occupied / hit by an obstacle. This embodiment can use the method in the relevant technology to determine the probability of each grid being hit by an obstacle at the current moment, which will not be described in detail here.

[0035] Step S5: The robot follows the target planning direction and sequentially stitches the selected effective obstacle grids into the effective outline of the obstacle in the grid map, and then configures the direction of the effective outline of the obstacle in the grid map as the direction the robot walks along the outline of the obstacle. Figure 2 In the target planning direction, the target planning direction can be regarded as following Figure 2 Walking from point A to point B can also be regarded as walking in the positive direction of the coordinate axis Yr.

[0036] The robot sequentially connects the valid obstacle grids selected in step S4 to form the effective outline of the obstacle in the grid map. This effective outline refers to the entire obstacle outline that guides the robot's movement direction and is smooth relative to the actual obstacle outline. Consequently, the robot can follow a trajectory parallel to the effective outline of the obstacle in the grid map, thus bypassing the obstacle.

[0037] The technical effects of the aforementioned steps S1 to S6 are as follows: Step S2 uses the obstacle contour points to perform curve fitting to fit the obstacle fitting curve and its trajectory equation, thereby achieving smoothing of the obstacle contour; compared with the prior art that directly uses grid or point cloud data to splice the trajectory, the present technical solution can improve the accuracy and smoothness of the fitted trajectory; then, Step S3 converts the position points passed by each line segment in the obstacle fitting curve into the grid map; Step S4 selects corresponding valid obstacle grids from the grid map based on the occupancy probability values ​​of the grids where the position points passed by the line segment in the obstacle fitting curve are located, so as to splice them into the effective contour of the obstacle in the grid map in Step S5, and configures the direction of the effective contour of the obstacle in the grid map as the direction in which the robot walks along the contour of the obstacle, thereby guiding the robot to bypass the obstacle where the contour line is located. Therefore, the aforementioned steps S1 to S5 select a valid obstacle grid (belonging to the obstacle grid) based on the occupancy probability value corresponding to the grid where the obstacle contour point is located. By limiting each line segment and occupancy probability value within the grid area, representative obstacle grids can be selected in sequence from the smoothed obstacle fitting curve while consuming less coordinate conversion and grid search. To a certain extent, this can compensate for the grid error caused by the obstacle contour points that should have been collected, and provide reliable direction parameter and distance parameter support for the robot to walk along the outline of the obstacle.

[0038] As an embodiment, the step S2 includes: determining the starting point and the end point of the Bezier curve along the target planning direction from the obstacle contour points collected by the robot at the same position point; when the curve fitting in step S2 is limited to Bezier curve fitting, the robot marks each obstacle contour point distributed along the target planning direction between the starting point and the end point of the Bezier curve as the intermediate point required for fitting the Bezier curve; the Bezier curve to be fitted is extended from the starting point of the Bezier curve to the end point of the Bezier curve, corresponding to Figure 2 In the figure, the starting point of the Bezier curve is point A, the end point of the Bezier curve is point B, and the target planning direction is the positive direction of the coordinate axis Yr shown in the figure.

[0039] This embodiment generates a trajectory equation for an obstacle fitting curve based on the starting point, endpoint, and intermediate points of the Bezier curve, and determines the generated obstacle fitting curve. Specifically, an obstacle fitting curve based on an n-th order Bezier curve is generated within the robot coordinate system. The sum of the number of starting points, endpoints, and intermediate points of the Bezier curve is equal to the sum of the order of the Bezier curve and 1. The order of the Bezier curve is represented by n.

[0040] In this embodiment, after determining each obstacle contour point as the starting point, end point, and intermediate point, a Bezier curve distributed along the obstacle contour is fitted along the target planning direction. This offsets the grid errors existing in the position points originally marked using the raster map, fills in the contour sampling points that may be lost when using the raster map marking, and plays a role in leveling the errors. For example, a relatively rough wall surface can be flattened by curve fitting, and the final marking is a smooth trajectory, so that relatively discrete obstacle contour points can be connected into a relatively continuous contour line, thereby achieving smoothing of the obstacle contour.

[0041] In some embodiments, the intermediate points required to fit the Bezier curve correspond to Figure 2 In the figure, the black dots arranged along the positive direction of the coordinate axis Yr between point A and point B are the trajectory points of the Bezier curve. Therefore, by sequentially connecting point A, the black discrete dots, and point B, the right side contour of obstacle L1 is obtained by curve fitting, i.e., the obstacle fitting curve. In the mathematical model, it is represented by the trajectory equation of the obstacle fitting curve (Bezier curve formula). Figure 2 In the figure, the black discrete dots arranged between point A and point B are not the middle points. The obstacle fitting curve is a Bezier curve extending along the target planning direction. Figure 2 It can be regarded as a Bezier curve extending along the positive direction of the coordinate axis Yr. If the Bezier curve is further translated to the side of the robot, the robot can walk along the translated Bezier curve, forming an obstacle avoidance trajectory in which the robot walks along the right contour of the obstacle L1.

[0042] Specifically, the order of the Bezier curve is represented by n; the trajectory equation of the obstacle fitting curve is:

[0043]

[0044] Among them, the trajectory equation of an obstacle fitting curve represents the trajectory equation required to fit an obstacle fitting curve; B x (t) is the horizontal coordinate of the point in the obstacle fitting curve, B y (t) is the ordinate of the point in the obstacle fitting curve, i represents the sorting number of the control points required for the obstacle fitting curve, and t is equal to the ratio between i and (n+1); the difference between the ordinate of the point with sorting number i and the ordinate of the point with sorting number (i+1) can be fixed; t is equal to the ratio between i and (n+1), which is used to represent the ratio of the sorting number of the most recently fitted point along the target planning direction to the total number of points that need to be fitted in the current execution of step S2, and the sorting number i represents the sorting of the control points required for the obstacle fitting curve along the target planning direction, which is regarded as being along the target planning direction. Figure 2The order of the coordinate axis Yr in the positive direction is shown, and it is configured to start with a value of 0 and increase along the target planning direction. The starting point, end point and middle point of the Bezier curve are all control points required for Bezier curve fitting; P ix Represented as control point P i The horizontal axis, P iy Represented as control point P i The vertical coordinate of P 0x The horizontal coordinate of the starting point of the Bezier curve, P 0y represents the ordinate of the starting point of the Bezier curve; nx The abscissa of the end point of the Bezier curve, P ny The ordinate of the end point of the Bezier curve. Indicates (P ix , P iy ) at a specific t value, similar to the binomial coefficient (number of combinations) in a binomial expansion, follows a Pascal's triangular distribution. Therefore, this embodiment uses a specific Bezier curve mathematical model to accurately fit the obstacle fitting curve, improving the accuracy of reconstructing the contours of irregularly shaped obstacles and facilitating the provision of important parameters such as direction and distance for subsequent obstacle avoidance decisions.

[0045] It should be noted that the trajectory equation of the obstacle fitting curve is a Bezier curve formula. A Bezier curve is a mathematical curve used in two-dimensional graphics applications; the curve is defined by a starting point (also called the initiation point), an end point (also called the termination point), and control points. Adjusting the control points changes the shape of the Bezier curve. Those skilled in the art will appreciate that when the starting and end points of a Bezier curve are locked, moving the midpoint on the line connecting two adjacent control points will change t. While a Bezier curve does not necessarily pass through all midpoints, it is guaranteed to pass through the starting and end points.

[0046] As an embodiment, the obstacle fitting curve belongs to a Bezier curve; the number of numerical types of t is equal to the number of points inserted to fit the obstacle fitting curve; thereby interpolating points with low continuity into points that appear relatively continuous. In the process of fitting an n-order Bezier curve, t increases unidirectionally from a value of 0 to a value of 1, and is regarded as extending from the starting point of the Bezier curve to the end point of the Bezier curve, and the inserted points are sequentially connected to form the obstacle fitting curve; in the target planning direction, when the relative positions of two adjacent control points change, the curvature (degree of curvature) of the Bezier curve changes. Two adjacent control points use P i and P i+1 Indicates that, where i∈[0,n-1], the starting point of the Bezier curve is represented by P0, and the end point of the Bezier curve is represented by P nIt means that i will gradually increase from the starting point during the Bezier curve fitting process until it reaches the end point, at which time t is equal to 1.

[0047] In this embodiment, the tangent direction of each point connected to the obstacle fitting curve is used to plan the direction (angular orientation) required for the robot to walk along the contour of the currently scanned obstacle, and is associated with the curvature (degree of curvature) of each point of the obstacle fitting curve; Bezier curve fitting can use the fitted trajectory points to fill in the contour points lost during the acquisition process and fit a curve that is closer to the direction of the obstacle contour, so that the robot can rely on the trajectory points of the obstacle fitting curve to track the side obstacles, and the direction required for the robot to walk to track the side obstacles is inclined to the target planning direction.

[0048] On the basis of the above embodiment, when t=0, the starting point of the Bezier curve is obtained by substituting the trajectory equation of the obstacle fitting curve; when t=1, the end point of the Bezier curve is obtained by substituting the trajectory equation of the obstacle fitting curve; the starting point of the Bezier curve is the obstacle contour point with the smallest coordinate value in the target planning direction among the obstacle contour points collected by the robot at the same position point, so as to form the obstacle contour point closest to the robot body center in the same step S1, corresponding to Figure 2 In the figure, the obstacle contour point with the smallest coordinate value on the coordinate axis Yr is represented by position point A. It is the detectable obstacle contour point closest to the current position of the robot (considered as the center of the robot body) in the obstacle fitting curve obtained by the current curve fitting. The end point of the Bezier curve is the obstacle contour point with the largest coordinate value in the target planning direction among the obstacle contour points collected by the robot at the same position, so as to form the obstacle contour point farthest from the center of the robot body in the same step S1, corresponding to Figure 2 In the center, point B represents the obstacle contour point with the largest coordinate value on the Yr coordinate axis. This is the detectable obstacle contour point farthest from the robot's current position (considered as the robot's body center) within the obstacle fitting curve obtained by the current curve fitting. After determining the starting point and endpoint of the Bezier curve, the robot subsequently marks valid obstacle grids within the grid map in step S4, following the direction from the starting point of the Bezier curve closest to the robot to the endpoint of the curve. This determines the position of the obstacle closest to the robot's body and prevents the robot from colliding with the obstacle.

[0049] It should be noted that the obstacle contour point is the edge position point of the obstacle scanned by the robot in the two-dimensional plane. It is a reflection point formed by the laser beam emitted by the laser sensor reflecting on the surface of the obstacle near the side of the robot. The two-dimensional coordinate information and angle information of the contour of the detected (or scanned) obstacle can be obtained through the reflection point, which can prompt the robot to plan the boundary of the obstacle avoidance area in advance.

[0050] Preferably, the difference between the coordinate values ​​of two adjacent control points in the target planning direction is equal to a preset parameter, and the preset parameter is preferably a value of 1 or a grid side length (the equivalent length of the side length of a cell in the grid map in the robot coordinate system); the coordinate values ​​of two adjacent control points in the target planning direction are preferably the vertical coordinate values ​​of the robot coordinate system.

[0051] As an embodiment, in step S3, the robot sets a plurality of mutually parallel line segments along the target planning direction on its side close to the obstacle fitting curve, and each line segment is configured to extend toward the obstacle fitting curve; for each line segment, the direction in which one endpoint extends toward the other endpoint can be regarded as extending from the center of the robot's body toward the obstacle fitting curve. The line segment set in step S3 is not necessarily perpendicular to the target planning direction; if the preset detection direction is parallel to the robot's wheel axle, and the preset detection direction is perpendicular to the target planning direction, then the line segment set in step S3 is not necessarily parallel to the robot's wheel axle, but will be parallel to the robot's travel plane, that is, parallel to the projection plane of the robot's body on its travel plane. Preferably, in Figure 2 In the embodiment, the line segments set in step S3 (except the line segments with arrows passing through the obstacle L1, except the coordinate axis Xr) can be considered to be parallel to the coordinate axis Xr shown in the figure, and each line segment is configured to extend toward the obstacle contour point fitted by the obstacle fitting curve. Figure 2 In the figure, it is represented as extending along the positive direction of the coordinate axis Xr so as to pass through the corresponding trajectory point of the obstacle fitting curve, and the preset detection direction is set to the positive direction of the coordinate axis Xr. The side of the robot close to the obstacle fitting curve is also the side of the preset detection direction of the body center R1, corresponding to Figure 2 In [1], the preset detection direction is the positive direction of coordinate axis Xr, and the target planning direction can be selected as the positive direction of coordinate axis Yr. Accordingly, coordinate axis Xr can be set as the horizontal axis of the coordinate system with the robot's body center R1 as the origin, and coordinate axis Yr can be set as the vertical axis of the coordinate system with the robot's body center R1 as the origin. The left endpoint of a line segment set in step S3 is the extension starting point of the line segment, and the right endpoint of the same line segment is the extension end point of the line segment. Directional information is assigned to the line segment set in step S3. On this basis, the line segment set in step S3 is equivalent to a vector line segment in the robot coordinate system.

[0052] In the line segments set in step S3, the distance between the extension starting point of each line segment and the robot body center in the preset detection direction is less than the robot body radius, and the distance between the extension end point of each line segment and the robot body center in the preset detection direction is greater than the robot body radius, so that the length of the line segment inside the body is less than the length of the line segment outside the body, wherein the robot body radius represents half of the robot body width. Figure 2 The distance between the left endpoint of the arrowed line segment (excluding coordinate axis Xr) passing through obstacle L1 and the robot's body center R1 in the positive direction of coordinate axis Xr is less than the robot's body radius. The maximum distance between the left endpoint of the line segment set in step S3 and the right edge of the robot in the positive direction of coordinate axis Xr is preferably 20 mm. This ensures that the line segment set in step S3 covers a small area of ​​the robot body near the obstacle. It should be noted that the side of the robot near the obstacle fitting curve is the side of the robot body closest to the obstacle fitted by the obstacle fitting curve.

[0053] In the step S3, the distance between the end point of each line segment and the center of the robot body in the preset detection direction is greater than the body radius of the robot, and the set line segment is extended to the outside of the robot body so that the line segment can pass through the obstacle fitting curve. The distance between the starting point of each line segment and the edge of the robot on the side close to the obstacle fitting curve is smaller than the distance between the end point of the same line segment and the edge of the robot on the side close to the obstacle fitting curve, that is, the distance between the starting point of each line segment and the edge position point of the robot body through which it passes is smaller than the distance between the end point of the same line segment and the edge position point of the robot body through which it passes; ensure that the coverage area of ​​the line segment set in step S3 on the outside of the body is larger than the coverage area on the inside of the body, corresponding to Figure 2 In the embodiment, the distance between the extension starting point (left endpoint) of the same line segment and the edge of the robot close to the obstacle fitting curve (the right edge position point passed by the same line segment) is smaller than the distance between the extension end point (right endpoint) of the same line segment and the edge of the robot close to the obstacle fitting curve (the right edge position point passed by the same line segment); among the line segments set in step S3, the minimum distance between the right endpoint of the line segment and the right edge of the robot in the positive direction of the coordinate axis Xr is preferably 150 mm, so that the line segment set in step S3 reserves a movable area between the robot body and the contour of the obstacle fitted by the adjacent obstacle fitting curve.

[0054] In summary, the obstacle fitting curve fitted in step S2 provides specific direction and distance parameters for the obstacle contour along which the robot is probing. Since the set line segments pass through the robot's edges and the contour of the obstacle along which it is probing, each line segment within the grid map area defined by the multiple line segments set in step S3 systematically acquires each position point in the obstacle fitting curve along the preset detection direction. This is equivalent to screening out individual obstacle points with directional significance from the obstacle fitting curve. Therefore, this embodiment, based on the target planning direction and the extension direction of the mutually parallel line segments, allows the robot to more clearly determine the directionality of the valid obstacle grid, avoiding the arbitrary direction selection of the valid obstacle grid in step S4. Consequently, a valid obstacle grid that can be connected to the actual direction of the obstacle contour can be quickly obtained.

[0055] As an embodiment of step S3, among the multiple parallel line segments set by the robot, the vertical distance from the robot's body center to the line segment farthest from the robot's body center is equal to the robot's body radius; and the vertical distance from the robot's body center to the line segment closest to the robot's body center is equal to 0. Among the line segments set in step S3, i.e., the arrowed line segments (excluding coordinate axis Xr) passing through obstacle L1 shown in the figure, the topmost arrowed line segment passing through obstacle L1 is the line segment farthest from the robot's body center R1, i.e., line segment E2F2 shown in the figure; the vertical distance from the robot's body center R1 to the line segment farthest from the robot's body center R1 is equal to the robot's body radius, which is the radius of the circle with point R1 as its center shown in the figure. The arrowed line segment passing through the bottom of the obstacle L1 is located in the coordinate axis Xr, and is the line segment closest to the robot's body center R1, that is, the line segment E1F1 shown in the figure; the vertical distance from the robot's body center R1 to the line segment closest to the robot's body center R1 is equal to the value 0.

[0056] Among the multiple parallel line segments set in step S3, the robot configures the spacing between two adjacent line segments to be equal to the resolution of a single grid. Preferably, the length of each line segment is equal, and the resolution of a single grid is equal to 10 mm. This achieves the goal of defining a regular area between line segment E2F2 and line segment E1F1 along the target planning direction according to the spacing. Since the preset detection direction is perpendicular to the target planning direction, step S3 is equivalent to sequentially setting multiple equally spaced line segments along the target planning direction starting from the body center R1. Each line segment can be considered to extend along the preset detection direction in order to pass through the obstacle fitting curve. Thus, multiple equally spaced line segments are used to represent a rectangular grid area, and are sequentially distributed in the grid map according to the grid resolution. The grid resolution is taken into account when two adjacent line segments pass through the obstacle fitting curve. This overcomes the influence of the offset error caused by the grid resolution after the intersection of the two adjacent line segments and the obstacle fitting curve is converted to the grid map. Therefore, setting up multiple parallel line segments is equivalent to setting up multiple equally spaced line segments in sequence along the target planning direction. Each line segment can be regarded as extending along the preset detection direction in order to pass through the obstacle fitting curve, and is distributed according to the resolution of the grid so that the offset error caused by the resolution of the grid is taken into account between the position points where the obstacle fitting curve passes.

[0057] As an embodiment, in step S4, the method of selecting a valid obstacle grid based on the occupancy probability value corresponding to the grid where the position point passed by each line segment in the obstacle fitting curve is located includes: for a position point passed by the same line segment in the obstacle fitting curve, selecting the grid with the largest occupancy probability value from the grids hit by the same position point, and then marking the grid with the largest occupancy probability value as the valid obstacle grid corresponding to the same line segment in the grid map, wherein the position point passed by each line segment set in step S3 in the obstacle fitting curve is marked as an obstacle point.

[0058] The number of position points that the same line segment passes through in the obstacle fitting curve can be one, two, or more, and all are marked as the number of obstacle points; the number of grids that the same position point hits in the grid map can be one, two, or more.

[0059] After converting the obstacle points passed by each line segment into the global map coordinate system, for one or more grids hit by the obstacle point, the grid with the largest occupancy probability value is selected from the grids hit by the same obstacle point, and marked as the valid obstacle grid corresponding to the line segment passing through in the grid map; after the robot obtains the valid obstacle grid corresponding to the line segment passing through in step S4, it forms reliable positioning information for a contour point divided by the line segment on the same obstacle fitting curve at the current moment, so that its tangent direction is directly configured as the robot's obstacle avoidance walking direction. If the robot walks along the contour composed of the valid obstacle grids, the stability of the robot's walking along the edge can be guaranteed.

[0060] On the other hand, step S5 sequentially combines the valid obstacle grids selected in step S4 into the effective outline of the obstacle in the grid map. Specifically, in the same step S5, the robot sequentially connects the valid obstacle grids set in the most recently executed step S4 to form a representative outline of the obstacle fitting curve in the grid map, which is equivalent to the obstacle outline formed by connecting at equal intervals along the target planning direction, and then configures the direction of the obstacle outline as the direction in which the robot walks along the outline of the obstacle; thereby saving the amount of marking by inducing probability information of entering the grid, and overcoming the error problem of the same obstacle point being marked / hit on multiple grids at the same time. The robot can ensure the stability of walking along the edge by walking along the outline area composed of the valid obstacle grids. When the valid obstacle grids represent the outline of a wall, multiple valid obstacle grids may be on the same straight line, which can ensure the stability of the robot walking along the wall.

[0061] It should be noted that the laser sensor is mounted on the robot body. The center of the laser sensor can be set at position R1 or position R1 is in the positive direction of the coordinate axis Yr. Preferably, the center of the robot body is set at Figure 2 At position point R1, the laser probe is rotatably installed in the laser sensor to support the robot in collecting obstacle contour points. The laser probe of the laser sensor detects the environment around it in a uniform rotation; in this embodiment, the laser sensor uses a laser beam to scan the partial area or the entire area passed by the multiple line segments set in step S3 at an angle, and each angle corresponds to a laser point. The laser points scanned in 360 degrees of rotation are added together to form a frame of point cloud. Among the detected obstacle contours, each angle corresponds to an obstacle contour point; the obstacle contour points collected by the robot are point clouds formed by the laser beam emitted by the laser probe being reflected in the obstacle contour when the laser probe rotates at least one circle; the laser points reflected from the surface of the obstacle exist in the point cloud, which are obstacle contour points in this embodiment.

[0062] When each position point passed by a line segment in the obstacle fitting curve is marked as an obstacle point, after the obstacle point hits at least one grid in the grid map, the hit grid is marked as the grid where the obstacle point is located, and the grid where the obstacle point is located is assigned a hit probability value. The first assigned hit probability value is the initial occupancy probability value. This embodiment can use methods in related technologies to determine the first hit probability value assigned to each grid at the current moment, which will not be described in detail here.

[0063] It should be noted that the grid map includes probability information indicating the existence of obstacles in specific locations or areas. The robot's laser sensor can be used to scan the object to be measured to obtain the corresponding position information of the object to be measured, and on this basis, the probability information of the object to be measured falling into / hitting the corresponding grid position of the map is calculated. This information serves as the hit probability value of the corresponding grid, which can be used to indicate the possibility that the grid where the obstacle contour point is located is occupied by an obstacle.

[0064] In some implementation scenarios, the robot may have inconsistent detection distances of a fixed obstacle (object to be measured) through a laser sensor at different times in the same posture. A frame of laser point cloud collected at one moment may involve a detection distance of 5m, while a frame of laser point cloud collected at another moment may involve a detection distance of 5.1m, thereby obtaining obstacle contour points at different positions. In order to avoid marking both the 5m and 5.1m obstacle contour points as the same obstacle contour point, it is necessary to use an occupied grid map construction algorithm to calculate the probability information of the aforementioned fixed obstacle falling into (hitting) the corresponding grid position of the map, and then determine the precise position of the aforementioned fixed obstacle in the map based on the size of the probability value.

[0065] In one embodiment, after an obstacle point hits a grid within the grid map, the hit grid is marked as the grid where the obstacle point is located, and the grid where the obstacle point is located is assigned a hit probability value. Each point along the obstacle fitting curve where a line segment passes is marked as an obstacle point. Whenever an obstacle point hits a corresponding grid, the hit probability values ​​previously assigned to the corresponding grid are accumulated to obtain an occupancy probability value for the corresponding grid. This can be achieved by accumulating the hit probability values ​​of the same grid at the previous moment to obtain the occupancy probability value for the grid, so that the occupancy probability value for the same grid is updated when the number of hits by the obstacle point changes. The accumulated hit probability value is updated to the occupancy probability value, so that the hit probability value assigned to the grid where the obstacle point is located is updated to the occupancy probability value. By using the probability value accumulation result, the accuracy of obstacle marking within the grid map is improved, so that the grid occupancy probability value more accurately reflects the distribution of obstacles around the robot.

[0066] If there are multiple obstacle points hitting the same grid when the laser probe rotates one circle, the hit probability values ​​corresponding to hitting the grid are accumulated in turn. If necessary, the accumulated result can be updated to the occupancy probability value each time, and finally the latest occupancy probability value of the hit grid when the laser probe scans one circle is obtained.

[0067] If only one obstacle point hits the same grid when the laser probe rotates one circle, the hit probability value corresponding to the grid is directly updated to the occupancy probability value, which serves as the occupancy probability value of the hit grid when the laser probe scans one circle.

[0068] If multiple obstacle points hit the same grid when the laser probe rotates at least two times (not limited to hitting in one scan or multiple scans), the hit probability values ​​corresponding to the hit grid are accumulated in sequence. If necessary, the accumulated result can be updated as the occupancy probability value each time, and finally the latest occupancy probability value of the hit grid when the laser probe scans at least two times is obtained. Preferably, at the current position of the robot, the more times the robot controls the laser probe to rotate, the more obstacle points hitting the same grid collected by the robot, and the occupancy probability value of the same grid hit by the selected obstacle point increases, so that the occupancy probability value of the same grid is updated as the number of rotations of the laser probe increases. When the laser sensor rotates to scan the environment, the surrounding environment can be updated in real time, so that the occupancy probability value of the grid more accurately reflects the status of the surrounding obstacles.

[0069] As an embodiment of step S4, the robot converts the position points that each line segment passes through in the obstacle fitting curve into the grid map as follows: first, after the robot collects the obstacle contour points in step S1, the robot sets the position coordinates of the obstacle contour points to the local coordinates of the obstacle contour points relative to the robot's current position point, wherein the local coordinates are located in the robot coordinate system, the robot's current position point is the origin of the robot coordinate system, and the robot's current position point is represented by the robot's body center. The local coordinates corresponding to the obstacle contour points and the local coordinates corresponding to each trajectory point in the obstacle fitting curve are all coordinates in the robot coordinate system. Then, to obtain the coordinates of the obstacle fitting curve and the line segment in the grid map, the robot converts the position coordinates of the position points that each line segment passes through in the obstacle fitting curve into the global map coordinate system used to construct the grid map, obtains the grid coordinates of the obstacle point, and determines the grid where the obstacle point is located in the grid map, forming the grid corresponding to the obstacle point hit in the grid map; wherein each position point that the line segment passes through in the obstacle fitting curve is marked as an obstacle point. In order to simplify the coordinate calculation and facilitate the determination of the robot's walking direction, the robot configures the preset detection direction to be parallel to the robot's wheel axle, and the line segment set in step S3 is parallel to the preset detection direction; the positive direction of the vertical axis of the robot coordinate system is the same as the target planning direction, and the positive direction of the horizontal axis of the robot coordinate system is the same as the preset detection direction; Figure 2 As shown, the preset detection direction is the positive direction of the coordinate axis Xr shown in the figure (the positive direction of the horizontal coordinate axis of the robot coordinate system), the preset detection direction is parallel to the wheel axis of the robot, and the line segment set in step S3 is parallel to the preset detection direction, that is, Figure 2 The arrowed line segments (excluding coordinate axis Xr) passing through obstacle L1 in the grid map are parallel to coordinate axis Xr, thereby forming a line segment scanning area parallel to the wheel axle within the grid map. Furthermore, the target planning direction is the positive direction of coordinate axis Yr of the robot coordinate system (the positive direction of the ordinate axis of the robot coordinate system). If the line segments set in step S3 are parallel to the abscissa axis of the robot coordinate system, a standard scanning direction is provided for the obstacle points through which the line segments pass. When acquiring grid coordinates, the corresponding coordinate axis directions can be used as the preset detection direction and target planning direction, respectively, reducing the amount of rotation calculations required for angles.

[0070] Step S4 uses the grid of the grid map to represent the position points that each line segment passes through in the obstacle fitting curve, so as to facilitate the subsequent selection of valid obstacle grids using the occupancy probability value, which is represented by Figure 2The black-filled cells are distributed along the target planning direction within the outline of obstacle L1. This allows for the creation of a valid outline within the grid map that is suitable for the robot's obstacle avoidance or circumvention, while minimizing grid search and computational effort. The direction of this valid outline can then be directly used to plan the robot's travel direction.

[0071] It should be noted that the rotation angle required for the local coordinate rotation transformation is equal to the angle between the coordinate axes of the same orientation (the same attribute coordinate axes) between the global map coordinate system and the robot coordinate system; the coordinate translation amount required for the local coordinate translation transformation is equal to the coordinate value of the robot's current position in the global map coordinate system, which also serves as the coordinate translation amount between the origin of the global map coordinate system and the origin of the robot coordinate system. The coordinate translation amount required for the local coordinate translation transformation in the horizontal coordinate axis direction of the robot coordinate system is equal to the horizontal coordinate value of the robot's current position in the global map coordinate system; the coordinate translation amount required for the local coordinate translation transformation in the vertical coordinate axis direction of the robot coordinate system is equal to the vertical coordinate value of the robot's current position in the global map coordinate system; the coordinate value of the robot's current position in the global map coordinate system includes the horizontal and vertical coordinate values ​​of the robot's current position in the global map coordinate system.

[0072] The trigonometric function transformation relationship mastered by those skilled in the art can be used to construct a coordinate system transformation equation using the rotation angle required for the local coordinate rotation transformation and the coordinate translation required for the local coordinate translation transformation. The position coordinates of the aforementioned obstacle point are then substituted into the coordinate system transformation equation to obtain the grid coordinates of the effective obstacle grid, thereby marking the effective outline of the obstacle fitted by the obstacle fitting curve in the grid of the grid map.

[0073] Preferably, the robot can adopt a body with a circular shape, such as a sweeping robot; the robot includes a semicircular head, a semicircular body and two symmetrically arranged wheels, the two symmetrically arranged wheels are connected by a wheel axle, and the wheel axle is set at the dividing line between the head and the body, such as Figure 2 As shown in the setting position of the coordinate axis Xr in the figure, the center of the wheel axle is the position point R1; the semicircular head and the semicircular body form a circular body; the body diameter is equal to the body width. The longest distance from the body center R1 of the robot to the body edge of the robot is expressed as the body radius of the robot, and the body radius is equal to half of the body width or half of the head width. The center of the wheel axle is the body center of the robot; the target planning direction is set perpendicular to the wheel axle, and the target planning direction is as shown in FIG. Figure 2The positive direction of the coordinate axis Yr in the image (the direction of the arrow on the coordinate axis Yr) is shown. The robot coordinate system is constructed based on the robot's body dimensions and its internal axles. Multiple parallel line segments can also be set to select the grid where the obstacle contour points are located, providing an appropriate direction for subsequent curve fitting operations.

[0074] Preferably, the preset detection direction is a direction toward a side obstacle of the robot and parallel to the wheel axle. Side obstacles are obstacles located outside the side of the robot body and are also obstacles fitted by the obstacle fitting curve. Side obstacles are obstacles located outside the side of the robot body, and the side of the robot body includes the left side or the right side of the body. Side obstacles are obstacles located on the left or right side of the robot and do not contact the robot. The robot's walking direction is pointing forward of the robot and is parallel to the target planning direction, which is perpendicular to the wheel axle.

[0075] In the embodiments provided in the present application, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. For example, the device embodiments described above are merely schematic. For example, the division of the units is merely a logical function division. In actual implementation, there may be other division methods, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. On the other hand, the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms. The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the scheme of this embodiment.

[0076] It will be understood by those skilled in the art that all or part of the processes in the above-mentioned embodiment methods can be implemented by instructing the relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, the references to memory, storage, database or other media used in the various embodiments provided in this application may include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory ROM, programmable memory PROM, electrically programmable memory DPROM, electrically erasable programmable memory DDPROM or flash memory. Volatile memory may include random access memory RAM or external cache memory.

[0077] The technical features of the above embodiments can be combined arbitrarily. To make the description concise, not all possible combinations 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.

[0078] The above embodiments merely represent several embodiments of the present invention, and their descriptions are relatively specific and detailed, but they should not be construed as limiting the scope of the invention. It should be noted that a person skilled in the art may make various modifications and improvements without departing from the concept of the present application, and these modifications and improvements are all within the scope of protection of the present application.

Claims

1. The obstacle marking method based on curve fitting is characterized by: The obstacle marking method comprises: Step S1: The robot controls the laser sensor to collect obstacle contour points at the current position by rotating and scanning the laser probe; Step S2: The robot performs curve fitting using the obstacle contour points collected at the current position to generate an obstacle fitting curve; Step S3: The robot sets a plurality of line segments on a side close to the obstacle fitting curve, so that the set line segments pass through the obstacle fitting curve; Step S4: The robot converts the position points that each line segment passes through in the obstacle fitting curve into a grid map, and then selects a valid obstacle grid based on the occupancy probability value corresponding to the grid where the position points that each line segment passes through in the obstacle fitting curve are located; Step S5: The robot moves along the target planning direction, sequentially stitching the selected valid obstacle grids into the effective outline of the obstacle in the grid map, and then configuring the direction of the effective outline of the obstacle in the grid map as the direction in which the robot walks along the outline of the obstacle.

2. The obstacle marking method according to claim 1, characterized in that: The step S2 comprises: From the obstacle contour points collected by the robot at the same position, the starting point and the end point of the Bezier curve are determined along the target planning direction; The robot marks each obstacle contour point distributed along the target planning direction between the starting point and the end point of the Bezier curve as the intermediate point required to fit the Bezier curve; Generate a trajectory equation of the obstacle fitting curve according to the starting point, the end point, and the middle point of the Bezier curve, and determine to generate the obstacle fitting curve; The sum of the number of the starting point, the end point, and the middle point of the Bezier curve is equal to the sum of the order of the Bezier curve and the value 1.

3. The obstacle marking method according to claim 2, characterized in that: The order of the Bezier curve is represented by n; the trajectory equation of the obstacle fitting curve is: Among them, the trajectory equation of an obstacle fitting curve represents the trajectory equation required to fit an obstacle fitting curve; B x (t) is the horizontal coordinate of the point in the obstacle fitting curve, B y (t) is the ordinate of a point in the obstacle fitting curve, i represents the sequence number of the control point required for the obstacle fitting curve, and t is equal to the ratio between i and (n+1); Among them, the starting point, end point and middle point of the Bezier curve are all control points required for Bezier curve fitting; Among them, P ix Represented as control point P i The horizontal axis, P iy Represented as control point P i The vertical coordinate of P 0x The horizontal coordinate of the starting point of the Bezier curve, P 0y represents the ordinate of the starting point of the Bezier curve; nx The abscissa of the end point of the Bezier curve, P ny The ordinate of the end point of the Bezier curve.

4. The obstacle marking method according to claim 3, characterized in that: The obstacle fitting curve is a Bezier curve; the number of values ​​of t is equal to the number of points inserted to fit the obstacle fitting curve; During the n-order Bezier curve fitting process, t increases unidirectionally from a value of 0 to a value of 1, extending from the starting point of the Bezier curve to the end point of the Bezier curve, and the inserted points are sequentially connected to form the obstacle fitting curve; The tangent directions of the points connected to form the obstacle fitting curve are used to plan the direction required for the robot to walk along the outline of the currently scanned obstacle.

5. The obstacle marking method according to claim 4, characterized in that: When t=0, the starting point of the Bezier curve can be obtained by substituting the trajectory equation of the obstacle fitting curve into the equation; when t=1, the end point of the Bezier curve can be obtained by substituting the trajectory equation of the obstacle fitting curve into the equation; The starting point of the Bezier curve is the obstacle contour point with the smallest coordinate value in the target planning direction among the obstacle contour points collected by the robot at the same position point, so as to form the obstacle contour point closest to the center of the robot body in the same step S1; The end point of the Bezier curve is the obstacle contour point with the largest coordinate value in the target planning direction among the obstacle contour points collected by the robot at the same position point, so as to form the obstacle contour point farthest from the center of the robot body in the same step S1; The obstacle contour points are edge position points of the obstacles scanned by the robot in a two-dimensional plane.

6. The obstacle marking method according to claim 2, characterized in that: In step S3, the robot sets a plurality of mutually parallel line segments along the target planning direction on a side of the robot close to the obstacle fitting curve, and each line segment is configured to extend toward the obstacle fitting curve; The distance between the starting point of each line segment and the center of the robot in the preset detection direction is less than the robot's body radius, and the distance between the end point of each line segment and the center of the robot in the preset detection direction is greater than the robot's body radius. The distance between the starting point of each line segment and the edge of the robot on the side close to the obstacle fitting curve is smaller than the distance between the end point of each line segment and the edge of the robot on the side close to the obstacle fitting curve. The preset detection direction is perpendicular to the target planning direction; the body radius of the robot represents half of the body width of the robot.

7. The obstacle marking method according to claim 6, characterized in that: Among the multiple parallel line segments set by the robot, the vertical distance from the robot's body center to the line segment farthest from the robot's body center is equal to the robot's body radius; the vertical distance from the robot's body center to the line segment closest to the robot's body center is equal to 0; Among the multiple parallel line segments set by the robot, the robot configures the distance between two adjacent line segments to be equal to the resolution of a single grid.

8. The obstacle marking method according to claim 6, characterized in that: In step S4, the method of selecting a valid obstacle grid based on the occupancy probability value corresponding to the grid where the position point passed by each line segment in the obstacle fitting curve is located includes: For a position point passed by the same line segment in the obstacle fitting curve, the grid with the largest occupation probability value is selected from the grids hit by the same position point in the grid map, and then the grid with the largest occupation probability value is marked as the valid obstacle grid corresponding to the same line segment in the grid map.

9. The obstacle marking method according to claim 8, characterized in that: After an obstacle point hits a grid in the grid map, the hit grid is marked as the grid where the obstacle point is located, and the grid where the obstacle point is located is assigned a hit probability value; wherein, the position point passed by each line segment in the obstacle fitting curve is marked as an obstacle point; Whenever an obstacle point hits the corresponding grid, the hit probability value assigned to the corresponding grid is accumulated to obtain the occupation probability value of the corresponding grid, so that the occupation probability value of the same grid is updated when the number of hits of the obstacle point changes; The accumulated value of the hit probability value is updated to the occupation probability value, so that the hit probability value assigned to the grid where the obstacle point is located is updated to the occupation probability value.

10. The obstacle marking method according to claim 9, characterized in that: After the robot collects the obstacle contour point in step S1, the position coordinates of the obstacle contour point are set as the local coordinates of the obstacle contour point relative to the current position of the robot, where the local coordinates are located in the robot coordinate system, the current position of the robot is the origin of the robot coordinate system, and the current position of the robot is represented by the center of the robot body; After the robot converts the position coordinates of the position points passed by each line segment in the obstacle fitting curve into the global map coordinate system used to construct the grid map, the grid coordinates of the obstacle point are obtained, and the grid where the obstacle point is located is determined in the grid map to form the corresponding grid in the grid map where the obstacle point hits. Among them, the preset detection direction is parallel to the wheel axle of the robot, and the line segment set in step S3 is parallel to the preset detection direction; the positive direction of the vertical axis of the robot coordinate system is the same as the target planning direction, and the positive direction of the horizontal axis of the robot coordinate system is the same as the preset detection direction.

Images