Intelligent service robot positioning and navigation system for complex environment

Abstract

The invention relates to an intelligent service robot positioning and navigation system oriented to a complex environment, which is applied to the technical field of positioning and navigation, and realizes the positioning and navigation of an intelligent service robot through robot robust positioning oriented to indoor and outdoor switching scenes, machine vision based on deep scene semantic analysis and understanding, robot autonomous cognition based on big data analysis, a navigation key map and the like. Therefore, autonomous navigation obstacle avoidance of the intelligent service robot in different scenes is realized, and compared with the prior art, the dependence on human intervention is greatly reduced, and large-scale development and application of the intelligent service robot are facilitated; besides, when the service robot moves in a relatively determined area for a long time, the relevance of some information in the environment can be analyzed through a deep learning or probabilistic method by collecting the environment information in the operation process, then the environment information in the current operation state is judged, and then follow-up navigation is optimized.

Description

Technical Field

[0001] The invention relates to an intelligent service robot positioning and navigation system, and in particular to an intelligent service robot positioning and navigation system for complex environments and applied in the field of navigation technology. Background Art

[0002] For service robots, autonomous positioning and navigation technology has become one of the core and focus of the product, and its role is directly related to the intelligence level of the robot. Although some common intelligent service robots can achieve positioning and navigation functions in specific environments through established programs, such as household sweeping robots that can complete cleaning tasks at home, their application scope is limited to indoors. The service robots currently on the market do not have the following functions:

[0003] 1) The ability to automatically adjust when the environment switches to keep the system stable;

[0004] 2) Confirm the robot's location in a complex environment, so as to realize the ability to judge the surrounding environment;

[0005] 3) Deeply mine effective information in an unknown environment to determine the robot's location, thereby achieving functions such as optimal path planning.

[0006] This inevitably leads to shortcomings such as the singleness of the robot's functions and high dependence on human intervention, which seriously limits the large-scale development and application of intelligent service robots. Summary of the invention

[0007] In view of the above-mentioned prior art, the technical problem to be solved by the present invention is that the existing service robots have single functions and are highly dependent on human intervention.

[0008] To solve the above problems, the present invention provides an intelligent service robot positioning and navigation system for complex environments, comprising an industrial computer and a motion controller installed on the robot chassis, the industrial computer and the motion controller are connected by signal, and the motion controller comprises a motor drive module, an ultrasonic module, a gyroscope module installed on the robot chassis, a Bluetooth module, and a digital tube module installed on the upper surface of the robot chassis;

[0009] The motor drive module includes a power module, a current acquisition module and two brushless DC motors;

[0010] The ultrasonic module includes multiple ultrasonic detectors installed around the robot chassis;

[0011] The navigation system also includes a perception basic module, which includes a 3D laser radar, a 2D laser radar, an IMU module and a camera installed on the robot;

[0012] The navigation system also includes a complex scene positioning module, which includes a GPS positioning module and a UWB positioning module installed in the robot chassis;

[0013] The method for positioning and navigating a robot in a complex environment based on a navigation system includes the following steps:

[0014] S1. Autonomous navigation, obstacle detection and positioning of service robots;

[0015] S2. Real-time path planning of service robots in dynamic environments: Realize real-time path planning by combining global path planning and local path planning;

[0016] S3. The service robot locates itself in complex scenes: the robot measures the environment through its own perception basic module. When the environmental measurement result exceeds the given range, it determines that the current scene is actually outdoors, and switches the positioning algorithm based on this to achieve the switching between indoor and outdoor GPS positioning and UWB positioning, as well as the switching between indoor and outdoor scenes;

[0017] S4, based on machine vision analysis of image deep semantic segmentation, a deep semantic map of the proposed environment;

[0018] S5. Autonomous cognitive navigation of robots based on big data: The robot moves for a long time in a relatively certain area, collects environmental information during operation, analyzes the correlation between various information in the environment with deep learning or probabilistic methods, and then determines the environmental information in the current operating state to achieve autonomous cognitive navigation and optimize subsequent navigation.

[0019] As a further improvement of the present application, the specific operation of step S1 is as follows:

[0020] S11. Generate height map:

[0021] According to the walking speed of the robot, considering the obstacles within 5m in front of the robot can meet the requirements of real-time obstacle detection and avoidance of the robot. The range of 5m in front, 3m in width and 2m in height is set as the area of ​​interest, and points beyond this area will be ignored;

[0022] First, divide the region of interest into cubes with a bottom side length of c along the X and Z directions, and then divide each cube into grids with a height of b along the Y direction; assuming that (X0, Y0, Z0) and (Xmax, Ymax, Zmax) are the two boundary points of the region of interest, and P = (X, Y, Z) is any observation point in space, then the index (u, v, w) of the grid where the point is located can be obtained according to the following formula:

[0023]

[0024] After mapping all the measurement points to the grid, we can find the highest grid containing the observation point in each regular tetrahedron and record the height of the grid, thus obtaining the height distribution map in the space of interest;

[0025] S12, search for the initial point:

[0026] A graph-based breadth search method is used to find all obstacle grids and grids that the robot can pass through, and a spiral search algorithm is used to determine a point that the robot can pass through as the initial point of the search;

[0027] The specific method for determining the search point is:

[0028] Given a grid (u j ,v j ), define a search window with a side length of l around it, and use the grid as a passable point to be found when the grid meets the following conditions:

[0029] There are observed 3D data points in the grid, and the grid is not at the boundary of the space of interest;

[0030] There are data points in the four grids around the grid, that is, (u i -1,v j ), (u i +1,v j ), (u i ,v j -1) and (u i ,v j +1);

[0031] There are enough valid grids in the window;

[0032] The difference between the height of other grids in the window and the height of this grid is less than a certain constant;

[0033] The grid (u0, v0) 0.5m in front of the robot is used as the first grid to be searched to determine whether the grid meets the above conditions; if not, a spiral search is performed until the initial point through which the robot can pass is found;

[0034] S13. Obstacle analysis:

[0035] After obtaining the initial search point, the breadth-optimal search strategy is used to obtain all obstacle grids; for each grid, determine whether the four grids in its 4-neighborhood are obstacles;

[0036] Assume that the height of the current grid is h, and the height of the grid to be judged is h'. If the following conditions are met, the grid to be judged is considered to be an obstacle:

[0037] Among them, hrobot is the maximum height that the robot can cross, and θrobot is the maximum slope of the slope that the robot can walk on. After obtaining all obstacle grids, all points in the grid are marked as obstacles, and the remaining points are passable areas.

[0038] Then the obtained obstacle mesh is projected onto the XZ plane, and the minimum circumscribed quadrilateral outline of the projection curve is extracted;

[0039] The length and width of the quadrilateral correspond to the length and width of the obstacle, and the highest grid in the quadrilateral corresponds to the height of the obstacle; the shortest distance from the robot to the quadrilateral is the distance from the robot to the obstacle, and the coordinates of the points of the quadrilateral connected by the shortest distance are the coordinates of the obstacle;

[0040] S14. Obstacle map update:

[0041] It is planned to compare the point cloud information in the obstacle space with the global point cloud map established over a long period of time to determine whether the detected obstacle is generated after the dynamic change of the environment or is an inherent obstacle of the site; form an obstacle map for all obstacle point clouds, and perform Bayesian probability update on each obstacle area;

[0042] Assume that the prior probability of all regions being static obstacles under the initial conditions is P. After measuring an area A, the probability P of the area being of category C is obtained. C And the probability information P of the corresponding area being a static obstacle A C , the probability P' that the area is a static obstacle is obtained by using Bayes' theorem:

[0043] As a further improvement of the present application, the specific operation of global path planning in step S2 is:

[0044] Combine the effective information of Dijkstra algorithm (starting from the initial point) and BFS algorithm (starting from the target point) to construct two costs: the cost h(n) from the initial point to any node n and the heuristic evaluation cost w(n) from any node n to the target point;

[0045] When moving from the initial point to the target point, the heuristic function is constructed by weighing the two costs to continuously guide the robot to search for the global optimal path;

[0046] Among them, the heuristic function is: f(n) = μh(n) + γw(n);

[0047] The specific operations of local path planning are:

[0048] The essence of robot local path planning is to start from the starting point and reach the target area by moving between feasible areas. The navigation process can be described as: Path = Lmap org，free →L→Lmap n，free →L→Lmap goal，free ;

[0049] The local path is optimized based on the purpose of planning a path that is closer and closer to the target, with a faster driving speed and as far away from obstacles as possible;

[0050] G(v,w)=α*heading(v,w)+β*dist(v,w)+γ*vel(v,w),

[0051] Where heading is used to evaluate the angle between the robot’s current driving direction and the target position; dist represents the distance between the robot and the obstacle, that is, a safety margin must be set, and a certain gap must be kept between the robot and the obstacle, and the gap increases linearly with the increase in speed; the safe speed refers to the maximum speed at which the robot can stop before colliding with the obstacle. Suppose the actual speed of the robot is (v a , w a ), the actual speed of the moving obstacle is (v b , w b ),

[0052]

[0053] The dynamic window method is used to sample the speed in the speed space, and the speed of random sampling is limited to reduce the number of samples, so that less calculation is introduced when evaluating the cost function, while ensuring its effectiveness; let the sampling time interval be t, considering the dynamic acceleration of the robot, downsample the search space to the dynamic window, and only retain the speed that can be reached with the current acceleration. The speed in the dynamic window should satisfy:

[0054] V2={v,w|v=v,w|v∈[v a -v z*t , v a +v k*t ]∩[w a -w z*t , w a +w k*t ]}

[0055] Finally, in the path interval Lmap nfree The safe speed set Φ obtained above n =V1∩V2, the final cost function vel is based on the safe speed set Φ n Calculated.

[0056] As a further improvement of the present application, the positioning signal of outdoor GPS positioning is processed by using a Kalman filter method to enhance robustness;

[0057] Assume that the positioning result received at time k is x0, y0, z0, then:

[0058] Where x, y, z are ideal state variables ε x , ε y , ε z is the first-order Markov national policy process error, ω x ,ω y ,ω z is the measurement error;

[0059] Build a linear Kalman model of the observables:

[0060] X(k|k-1)=Φ1(k|k-1)X(k-1)

[0061] X(k)=X(k|k-1)+K(k)[B(k)-H(k)X(k|k-1)]

[0062] K(k)=P(k|k-1)H T (k)[H(k)P(k|k-1))H T (k)+R(k)] -1

[0063] P(k|k-1))=Φ(k|k-1)P(k|k-1)Φ T (k|k-1)+Q(k|k-1)

[0064] In the above formula, Φ1(k|k-1)={Φ 1x (k|k-1)Φ 1y (k|k-1)Φ 1z (k|k-1)}

[0065] Where Q(k) is the noise cosquare difference of the system, and Q is the discretization matrix.

[0066] As a further improvement of the present application, the indoor UWB positioning ranging measurement is symbolized as r UWB , symbolize the LiDAR-based ranging measurement as r LiD , let x t =[p 0，x，t , p 0，y，t , v 0，x，t , v 0，y，t ] T ,

[0067]

[0068] make Represents the location of the robot beacon and the speed of the beacon The competition state related to UWB is composed of the following dynamic model evolution state X t :X t =F t X t-1 +G t w t ,

[0069] The transfer matrix Ft is: Gt is:

[0070] The state noise wt has zero mean and covariance And δ is the sampling interval;

[0071] make is the UWB-based range measured at time t, from the robot to the beacon and beacon to beacon range They are nonlinear functions of pt:

[0072]

[0073] where i=1,2,...,N t (N t +1) / 2 indexes the pairwise combination of UWB nodes, assuming [r t ] i ,i=1,2,...,N t (N t +1) / 2a all the equivalent ranges are iid destroyed, plus noise

[0074]

[0075] Then optimize the obtained data to improve the robustness of positioning data:

[0076]

[0077] in and P t|t They are the updated state estimate and covariance estimate respectively;

[0078] By deploying enough UWBs indoors and on robots and using the above model for positioning, a more accurate positioning effect can be achieved.

[0079] As a further improvement of the present application, in step S4, the method for establishing the deep semantic map is:

[0080] S41, estimating the position and posture of the camera in each frame image;

[0081] S42, converting each frame of the image in the previous step into a point cloud, then converting the point clouds into the same coordinate system, splicing them to form a point cloud map, and then generating an occupancy grid map based on the point cloud map;

[0082] S43. On the existing grid map, semantic information is assigned to 3D points in space, the semantic information containing the target is mapped into the spatial map, and semantic fusion is performed according to maximum likelihood estimation to obtain a semantic map.

[0083] As a further improvement of the present application, the generation of semantic information includes the following steps:

[0084] Sa. Determine the area where the target may exist;

[0085] Sb, identify the extracted area and determine the type of object in the area;

[0086] Sc. Segment the detected target area, extract the target from the background as accurately as possible, and distinguish the pixels belonging to the target from those belonging to the background.

[0087] As a further improvement of the present application, the autonomous cognitive navigation in step S5 is implemented by a model of deep learning change patterns, which can represent the correlation between path passability and use this correlation to make predictions in real time during navigation;

[0088] The deep learning method includes the following steps:

[0089] S51. Environmental Modeling:

[0090] During the robot operation, it is assumed that the set of traversability of the entire environment is E t , the robot cannot observe the feasibility of all paths, only some of them The unobservable part is denoted as U t , that is, the robot needs to pass Z t To analyze U t possibility, and formulate the problem as:

[0091]

[0092] Where η is the given current observation value Z t Normalizer, and p(E t ) is the joint probability distribution of the feasibility of paths in the environment; distribution p(E t) defines a probability function on the space of possible configurations and captures the correlation between the feasibility of the paths;

[0093] Then, the joint probability is represented by a tree-structured Bayesian network through the Chow-Liu tree approximation method; the joint probability distribution on path traversability is approximated by using a flexible but bounded factor graph representation;

[0094] S52, Factor Graph:

[0095] We define a unary factor node for each edge and a binary factor node for each pair of topological edges to model the correlation between the traversability of the edges, approximating the joint distribution on the edges as:

[0096]

[0097] By storing only |E|(|E|-1) / 2+|E|, the factor graph representation can approximate the probability of an environment configuration by capturing some correlation between path traversability, thus requiring only quadratic space to represent the number of edges, rather than exponential space required for the full joint probability distribution;

[0098] S53, observation and calculation:

[0099] The unary factors and binary factors of the factor graph are defined as:

[0100] φ i = p(e i )

[0101]

[0102] Where p(e i ) and p(e i ,e j ) are the unary and binary joint probabilities of a path being passable or blocked, respectively. Based on the definition of factor nodes, the approximate joint probability distribution is:

[0103]

[0104] According to the robot's previous running Z 1：t-1 , calculate the joint probability p(e i ) and p(e i ,e j ).

[0105] In summary, (1) the present application uses the robot's own sensors (lidar, camera, etc.) to measure the environment. When the environmental measurement results exceed a given range, it determines that the current scene is actually outdoors, and switches the positioning algorithm based on this.

[0106] (2) Develop machine vision analysis and understanding methods based on deep semantic segmentation of images to further optimize the robot's autonomous mapping, positioning navigation, and dynamic obstacle avoidance functions. The robot can build a deep semantic map of the environment through deep semantic analysis of the environment. The semantic map can improve the accuracy of the robot's positioning and navigation, and achieve more intelligent dynamic obstacle avoidance for different objects.

[0107] (3) To address the navigation problem of robots running for a long time in complex scenarios, a set of robot autonomous cognitive navigation methods based on big data analysis is designed and developed. The robot moves for a long time in a relatively certain area, collects environmental information during operation, analyzes the relevance of certain information in the environment using deep learning or probabilistic methods, and then determines the environmental information in the current operating state, thereby optimizing subsequent navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0108] Figure 1 A schematic diagram of a module for driving and controlling a service robot according to the present application;

[0109] Figure 2 A schematic diagram of the grid division of the area of ​​interest of this application;

[0110] Figure 3 A schematic diagram of the spiral search algorithm of the present application;

[0111] Figure 4 A schematic diagram of the actual scenario test for this application;

[0112] Figure 5 This is the principle diagram of indoor and outdoor positioning for this application;

[0113] Figure 6 Flowchart for constructing the semantic map for this application. DETAILED DESCRIPTION

[0114] An implementation of the present application is described in detail below with reference to the accompanying drawings.

[0115] like Figure 1 , an intelligent service robot positioning and navigation system for complex environments, including an industrial computer and a motion controller installed on the robot chassis, the industrial computer and the motion controller are connected by signal, and the motion controller includes a motor drive module, an ultrasonic module, a gyroscope module installed on the robot chassis, a Bluetooth module, and a digital tube module installed on the upper surface of the robot chassis;

[0116] The motor drive module includes a power module, a current acquisition module and two brushless DC motors;

[0117] The ultrasonic module includes multiple ultrasonic detectors installed around the robot chassis;

[0118] The navigation system also includes a perception basic module, which includes a 3D laser radar, a 2D laser radar, an IMU module and a camera installed on the robot;

[0119] The navigation system also includes a complex scene positioning module, which includes a GPS positioning module and a UWB positioning module installed in the robot chassis;

[0120] The method for positioning and navigating a robot in a complex environment based on a navigation system includes the following steps:

[0121] S1. Autonomous navigation, obstacle detection and positioning of service robots;

[0122] S11. Generate height map:

[0123] According to the walking speed of the robot, considering the obstacles within 5m in front of the robot can meet the requirements of real-time obstacle detection and avoidance of the robot. The range of 5m in front, 3m in width and 2m in height is set as the area of ​​interest, and points beyond this area will be ignored;

[0124] First, divide the region of interest into cubes with a bottom side length of c along the X and Z directions, and then

[0125] Divide each cube into grids of height b along the Y direction; assuming (X0, Y0, Z0), (Xmax, Ymax, Zmax)

[0126] are two boundary points of the region of interest, P = (X, Y, Z) is any observation point in space, then the index (u, v, w) of the grid where the point is located can be calculated according to the following formula:

[0127]

[0128] like Figure 2 ,After mapping all the measurement points to the grid, the highest grid containing the observation point in each regular tetrahedron can be found, and the height of the grid can be recorded, and the height distribution map in the space of interest can be obtained;

[0129] S12, search for the initial point:

[0130] A graph-based breadth search method is used to find all obstacle grids and grids that the robot can pass through, and a spiral search algorithm is used to determine a point that the robot can pass through as the initial point of the search;

[0131] The specific method for determining the search point is:

[0132] Given a grid (u j ,v j), define a search window with a side length of l around it, and use the grid as a passable point to be found when the grid meets the following conditions:

[0133] There are observed 3D data points in the grid, and the grid is not at the boundary of the space of interest;

[0134] There are data points in the four grids around the grid, that is, (u i -1,v j ), (u i +1,v j ), (u i ,v j -1) and (u i ,v j +1);

[0135] There are enough valid grids in the window;

[0136] The difference between the height of other grids in the window and the height of this grid is less than a certain constant;

[0137] like Figure 3 , take the grid (u0, v0) 0.5m in front of the robot as the first search grid to determine whether the grid meets the above conditions; if not, perform a spiral search until the initial point that the robot can pass is found;

[0138] S13. Obstacle analysis:

[0139] After obtaining the initial search point, the breadth-optimal search strategy is used to obtain all obstacle grids; for each grid, determine whether the four grids in its 4-neighborhood are obstacles;

[0140] Assume that the height of the current grid is h, and the height of the grid to be judged is h'. If the following conditions are met, the grid to be judged is considered to be an obstacle:

[0141] Among them, hrobot is the maximum height that the robot can cross, and θrobot is the maximum slope of the slope that the robot can walk on. After obtaining all obstacle grids, all points in the grid are marked as obstacles, and the remaining points are passable areas.

[0142] Then the obtained obstacle mesh is projected onto the XZ plane, and the minimum circumscribed quadrilateral outline of the projection curve is extracted;

[0143] The length and width of the quadrilateral correspond to the length and width of the obstacle, and the highest grid in the quadrilateral corresponds to the height of the obstacle; the shortest distance from the robot to the quadrilateral is the distance from the robot to the obstacle, and the coordinates of the points of the quadrilateral connected by the shortest distance are the coordinates of the obstacle;

[0144] S14. Obstacle map update:

[0145] It is planned to compare the point cloud information in the obstacle space with the global point cloud map established over a long period of time to determine whether the detected obstacle is generated after the dynamic change of the environment or is an inherent obstacle of the site; form an obstacle map for all obstacle point clouds, and perform Bayesian probability update on each obstacle area;

[0146] Assume that the prior probability of all regions being static obstacles under the initial conditions is P. After measuring an area A, the probability P of the area being of category C is obtained. C And the probability information P of the corresponding area being a static obstacle A C , the probability P' that the area is a static obstacle is obtained by using Bayes' theorem:

[0147] S2. Real-time path planning of service robots in dynamic environments: Realize real-time path planning by combining global path planning and local path planning;

[0148] S21. The specific operation of global path planning is:

[0149] Combine the effective information of Dijkstra algorithm (starting from the initial point) and BFS algorithm (starting from the target point) to construct two costs: the cost h(n) from the initial point to any node n and the heuristic evaluation cost w(n) from any node n to the target point;

[0150] When moving from the initial point to the target point, the heuristic function is constructed by weighing the two costs to continuously guide the robot to search for the global optimal path;

[0151] Among them, the heuristic function is: f(n) = μh(n) + γw(n);

[0152] S22. The specific operation of local path planning is:

[0153] The essence of robot local path planning is to start from the starting point and reach the target area by moving between feasible areas. The navigation process can be described as: Path = Lmap org，free →L→Lmap n，free →L→Lmap goal，free ;

[0154] Among them, free represents the connectivity analysis of the current local area. When constructing the evaluation function of the mobile trajectory, three factors are mainly considered: whether it is close to the target point or far away from the target point, the forward speed of the robot, and the distance to the next obstacle. That is, a path that is closer and closer to the target, faster in speed, and as far away from obstacles as possible is planned in the local map. Similar to the heuristic function, the local path is optimized by weighing these three costs;

[0155] The local path is optimized based on the purpose of planning a path that is closer and closer to the target, with a faster driving speed and as far away from obstacles as possible;

[0156] G(v,w)=α*heading(v,w)+β*dist(v,w)+γ*vel(v,w),

[0157] Where heading is used to evaluate the angle between the robot’s current driving direction and the target position; dist represents the distance between the robot and the obstacle, that is, a safety margin must be set, and a certain gap must be kept between the robot and the obstacle, and the gap increases linearly with the increase in speed; the safe speed refers to the maximum speed at which the robot can stop before colliding with the obstacle. Suppose the actual speed of the robot is (v a , w a ), the actual speed of the moving obstacle is (v b , w b ),

[0158]

[0159] The dynamic window method is used to sample the speed in the speed space, and the speed of random sampling is limited to reduce the number of samples, so that less calculation is introduced when evaluating the cost function, while ensuring its effectiveness; let the sampling time interval be t, considering the dynamic acceleration of the robot, downsample the search space to the dynamic window, and only retain the speed that can be reached with the current acceleration. The speed in the dynamic window should satisfy:

[0160] V2={v,w|v=v,w|v∈[v a -v z*t , v a +v k*t ]∩[w a -w z*t , w a +w k*t ]}Finally in the path interval Lmap nfree The safe speed set Φ obtained above n =V1∩V2, the final cost function vel is based on the safe speed set Φ nAlthough local path planning cannot guarantee the optimal solution, its calculation speed is fast, it can adapt to the ever-changing environment, and can ensure the real-time update of the local path, constantly guiding the robot to reach the target point safely.

[0161] In order to realize the autonomous inspection and navigation functions of mobile robots, multiple sensors, environmental perception and joint optimization are used to realize map construction and updating in dynamic environments. The fast search expansion tree algorithm is used to automatically obtain the frontier points of unknown boundaries, and the MeanShift algorithm is used to cluster similar frontier points to reduce the number of frontier points. The frontier points are then sent to the robot as targets to complete the process of autonomous map construction. Figure 4 The actual scene mapping test results are shown in Figure 2.

[0162] S3, such as Figure 5 , the service robot locates its own position in complex scenes: the robot measures the environment through its own perception basic module. When the environmental measurement result exceeds the given range, it determines that the current scene is actually outdoors, and switches the positioning algorithm based on this to realize the switching of indoor and outdoor GPS positioning and UWB positioning, as well as the switching of indoor and outdoor scenes;

[0163] S31. In outdoor GPS positioning, because the positioning signal obtained by the robot GPS receiver has satellite errors, propagation errors, reception errors, etc., it is necessary to process the positioning signal of outdoor GPS positioning using the Kalman filter method to enhance robustness;

[0164] Assume that the positioning result received at time k is x0, y0, z0, then:

[0165] Where x, y, z are ideal state variables ε x , ε y , ε z is the first-order Markov national policy process error, ω x ,ω y ,ω z is the measurement error;

[0166] Build a linear Kalman model of the observables:

[0167] X(k|k-1)=Φ1(k|k-1)X(k-1),

[0168] X(k)=X(k|k-1)+K(k)[B(k)-H(k)X(k|k-1)],

[0169] K(k)=P(k|k-1)H T (k)[H(k)P(k|k-1))H T(k)+R(k)] -1 ,

[0170] P(k|k-1))=Φ(k|k-1)P(k|k-1)Φ T (k|k-1)+Q(k|k-1),

[0171] In the above formula, Φ1(k|k-1)={Φ 1x (k|k-1)Φ 1y (k|k-1)Φ 1z (k|k-1)}

[0172] Where Q(k) is the noise cosquare difference of the system, and Q is the discretization matrix.

[0173] Assuming that the indoor threshold is k, the scanning radius of the radar is n, and n>k, we assume that a threshold m is selected between n and k. If more than half of the points scanned by the radar are greater than the threshold m, the robot is considered to be outdoors and the above-mentioned GPS technology is used. Otherwise, it is considered to be indoors and the UWB-Lidar positioning technology is used.

[0174] S32, symbolize the indoor UWB positioning ranging measurement into r UWB , symbolize the LiDAR-based ranging measurement as r LiD , let x t =[p 0，x，t , p 0，y，t , v 0，x，t , v 0，y，t ] T ,

[0175]

[0176] make Represents the location of the robot beacon and the speed of the beacon The competition state related to UWB is composed of the following dynamic model evolution state X t :X t =F t X t-1 +G t w t ,

[0177] The transfer matrix Ft is: Gt is:

[0178] The state noise wt has zero mean and covariance And δ is the sampling interval;

[0179] make is the UWB-based range measured at time t, from the robot to the beacon and beacon to beacon range They are nonlinear functions of pt:

[0180]

[0181] where i=1,2,...,N t (N t +1) / 2 indexes the pairwise combination of UWB nodes, assuming [r t ] i ,i=1,2,...,N t (N t +1) / 2a all the equivalent ranges are iid destroyed, plus noise

[0182]

[0183] Then optimize the obtained data to improve the robustness of positioning data:

[0184]

[0185] in and P t|t They are the updated state estimate and covariance estimate respectively;

[0186] By deploying enough UWBs indoors and on robots and using the above model for positioning, a more accurate positioning effect can be achieved.

[0187] S4, based on machine vision analysis of image deep semantic segmentation, proposes a deep semantic map of the environment, such as Figure 6 , the method of establishing a deep semantic map is:

[0188] S41. Estimate the position and posture of the camera in each frame. To complete this step, we can use two methods:

[0189] The first is to extract the feature points of the scene from multiple images, match the feature points in different frames, find the associated feature points, and then solve the position and posture transformation relationship between multiple frames based on the camera's imaging model and geometric transformation relationship;

[0190] Second, based on the point cloud data obtained from the depth map, the ICP algorithm is used to calculate the pose transformation between adjacent frames;

[0191] S42, converting each frame of the image in the previous step into a point cloud, then converting the point clouds into the same coordinate system, splicing them to form a point cloud map, and then generating an occupancy grid map based on the point cloud map;

[0192] S43. On the existing grid map, semantic information is assigned to 3D points in space, the semantic information containing the target is mapped into the spatial map, and semantic fusion is performed according to maximum likelihood estimation to obtain a semantic map.

[0193] Sb, identify the extracted area and determine the type of object in the area;

[0194] Sc. Segment the detected target area, extract the target from the background as accurately as possible, and distinguish the pixels belonging to the target from those belonging to the background.

[0195] S5. Robot autonomous cognitive navigation based on big data:

[0196] Autonomous cognitive navigation is achieved through a deep learning model of changing patterns that can represent the correlation between path traversability and use this correlation to make predictions in real time during navigation;

[0197] The deep learning method includes the following steps:

[0198] S51. Environmental Modeling:

[0199] During the robot operation, it is assumed that the set of traversability of the entire environment is E t , the robot cannot observe the feasibility of all paths, only some of them The unobservable part is denoted as U t , that is, the robot needs to pass Z t To analyze U t possibility, and formulate the problem as:

[0200]

[0201] Where η is the given current observation value Z t Normalizer, and p(E t ) is the joint probability distribution of the feasibility of paths in the environment; distribution p(E t ) defines a probability function on the space of possible configurations and captures the correlation between the feasibility of the paths;

[0202] Then, the joint probability is represented by a tree-structured Bayesian network through the Chow-Liu tree approximation method; the joint probability distribution on path traversability is approximated by using a flexible but bounded factor graph representation;

[0203] S52, Factor Graph:

[0204] A factor graph is a probabilistic graphical model that can represent a general decomposition of a function. It is constructed as an undirected graph with two kinds of nodes: variable nodes corresponding to random variables and factor nodes representing local functions of neighboring variable nodes. A factor graph representation is used, where the variable nodes are the edges of the topology E.

[0205] We define a unary factor node for each edge and a binary factor node for each pair of topological edges to model the correlation between the traversability of the edges, approximating the joint distribution on the edges as:

[0206]

[0207] By storing only |E|(|E|-1) / 2+|E|, the factor graph representation can approximate the probability of an environment configuration by capturing some correlation between path traversability, thus requiring only quadratic space to represent the number of edges, rather than exponential space required for the full joint probability distribution;

[0208] S53, observation and calculation:

[0209] Given the factor graph representation, we need to provide definitions of the factors φ and ψ so that the model corresponds to robot observations collected in previous runs and can be efficiently updated in an incremental manner as the robot acquires new observations.

[0210] The factor graph is inferred through the belief propagation algorithm, and the BP algorithm is used to make predictions based on the current data. The model parameters are estimated based on the robot's observations; the unary factors and binary factors of the factor graph are defined as:

[0211] φ i = p(e i )

[0212]

[0213] Where p(e i ) and p(e i ,e j ) are the unary and binary joint probabilities of a path being passable or blocked, respectively. Based on the definition of factor nodes, the approximate joint probability distribution is:

[0214]

[0215] According to the robot's previous running Z 1：t-1 , calculate the joint probability p(e i ) and p(e i ,e j). This is achieved by maintaining a counter of the number of times each unary and binary configuration has been observed. To prevent the probabilities from taking extreme values ​​of 0 or 1 in one observation, they are initialized with a uniform prior by assigning an equal number of positive occurrences to each configuration. After each run, the counters need to be updated based on the robot's observations and the probabilities are recomputed. Handling unary and binary joint probabilities allows us to update only the probabilities that correspond to the observed edges. Using this procedure, the parameters of the model are efficiently computed step by step based on the robot's observations. Another major advantage of this procedure is that it allows to easily incorporate partial observations of the environment.

[0216] In the above process, the robot moves for a long time in a relatively certain area, collects environmental information during the operation, analyzes the correlation between various information in the environment by deep learning or probabilistic methods, and then determines the environmental information in the current operating state to achieve autonomous cognitive navigation and optimize subsequent navigation.

[0217] In summary, the present application measures the environment through the robot's own sensors (lidar, camera, etc.). When the environmental measurement result exceeds a given range, it is determined that the current scene is actually outdoors, and the positioning algorithm is switched based on this.

[0218] Develop machine vision analysis and understanding methods based on deep semantic segmentation of images to further optimize the robot's autonomous mapping, positioning navigation, and dynamic obstacle avoidance functions. The robot can build a deep semantic map of the environment through deep semantic analysis of the environment. The semantic map can improve the accuracy of the robot's positioning and navigation, and achieve more intelligent dynamic obstacle avoidance for different objects;

[0219] Aiming at the navigation problem of robots running for a long time in complex scenes, a set of robot autonomous cognitive navigation methods based on big data analysis is designed and developed. The robot moves for a long time in a relatively certain area, collects environmental information during operation, analyzes the relevance of certain information in the environment by deep learning or probabilistic methods, and then determines the environmental information in the current running state, and then optimizes the subsequent navigation. Compared with the existing technology, this service robot can perform autonomous obstacle avoidance navigation in complex environments for a long time, reduce the service robot's dependence on human intervention, and facilitate the large-scale development and application of intelligent service robots.

[0220] In view of current practical needs, the above-mentioned implementation mode adopted in this application is not limited to the scope of protection. Various changes made within the knowledge scope of technical personnel in this field without departing from the concept of this application still fall within the scope of protection of the present invention.

Claims

1. Intelligent service robot positioning and navigation system for complex environments, characterized by: It includes an industrial computer and a motion controller installed on the robot chassis, the industrial computer is connected to the motion controller by signal, and the motion controller includes a motor drive module, an ultrasonic module, a gyroscope module installed on the robot chassis, a Bluetooth module, and a digital tube module installed on the upper surface of the robot chassis; The motor drive module includes a power supply module, a current acquisition module and two brushless DC motors; The ultrasonic module includes a plurality of ultrasonic detectors installed around the robot chassis; The navigation system also includes a perception basic module, which includes a 3D laser radar, a 2D laser radar, an IMU module and a camera installed on the robot; The navigation system further includes a complex scene positioning module, which includes a GPS positioning module and a UWB positioning module installed in the robot chassis; Based on the navigation system, the method for positioning and navigating the robot in a complex environment comprises the following steps: S1. Autonomous navigation, obstacle detection and positioning of service robots; S2. Real-time path planning of service robots in dynamic environments: Realize real-time path planning by combining global path planning and local path planning; S3. The service robot locates itself in complex scenes: the robot measures the environment through its own perception basic module. When the environmental measurement result exceeds the given range, it determines that the current scene is actually outdoors, and switches the positioning algorithm based on this to achieve the switching between indoor and outdoor GPS positioning and UWB positioning, as well as the switching between indoor and outdoor scenes; S4, based on machine vision analysis of image deep semantic segmentation, a deep semantic map of the proposed environment; S5. Autonomous cognitive navigation of robots based on big data: The robot moves for a long time in a relatively certain area, collects environmental information during operation, analyzes the correlation between various information in the environment with deep learning or probabilistic methods, and then determines the environmental information in the current operating state to achieve autonomous cognitive navigation and optimize subsequent navigation.

2. The intelligent service robot positioning and navigation system for complex environments according to claim 1, characterized in that: The specific operation of step S1 is as follows: S11. Generate height map: According to the walking speed of the robot, considering the obstacles within 5m in front of the robot can meet the requirements of real-time obstacle detection and avoidance of the robot. The range of 5m in front, 3m in width and 2m in height is set as the area of ​​interest, and points beyond this area will be ignored; First, divide the region of interest into cubes with a bottom side length of c along the X and Z directions, and then Divide each cube into grids of height b along the Y direction; assume that (X0, Y0, Z0) and (Xmax, Ymax, Zmax) are two boundary points of the region of interest, and P = (X, Y, Z) is any observation point in space. The index (u, v, w) of the grid where the point is located can be obtained by the following formula: After mapping all the measurement points to the grid, we can find the highest grid containing the observation point in each regular tetrahedron and record the height of the grid, thus obtaining the height distribution map in the space of interest; S12, search for the initial point: A graph-based breadth search method is used to find all obstacle grids and grids that the robot can pass through, and a spiral search algorithm is used to determine a point that the robot can pass through as the initial point of the search; The method for determining the search point is specifically as follows: Given a grid (u j ,v j ), a search window with a side length of l is defined around it, and when the grid meets the following conditions, the grid is used as a passable point to be found: There are observed three-dimensional data points in the grid, and the grid is not at the boundary of the space of interest; There are data points in the four grids around the grid, that is, (u i -1,v j ), (u i +1,v j ), (u i ,v j -1) and (u i ,v j +1); There are enough valid grids in the window; The difference between the height of the other grids in the window and the height of the grid is less than a certain constant; The grid (u0, v0) 0.5m in front of the robot is used as the first grid to be searched to determine whether the grid meets the above conditions; if not, a spiral search is performed until the initial point through which the robot can pass is found; S13. Obstacle analysis: After obtaining the initial search point, the breadth-optimal search strategy is used to obtain all obstacle grids; for each grid, determine whether the four grids in its 4-neighborhood are obstacles; Assume that the height of the current grid is h, and the height of the grid to be judged is h'. If the following conditions are met, the grid to be judged is considered to be an obstacle: Among them, h robot is the maximum height that the robot can cross, θ robot is the maximum slope of the slope that the robot can walk on; after obtaining all obstacle grids, all points in the grid are marked as obstacles, and the remaining points are passable areas; Then the obtained obstacle mesh is projected onto the XZ plane, and the minimum circumscribed quadrilateral outline of the projection curve is extracted; The length and width of the quadrilateral correspond to the length and width of the obstacle, and the highest grid in the quadrilateral corresponds to the height of the obstacle; the shortest distance from the robot to the quadrilateral is the distance from the robot to the obstacle, and the coordinates of the points of the quadrilateral connected by the shortest distance are the coordinates of the obstacle; S14. Obstacle map update: It is planned to compare the point cloud information in the obstacle space with the global point cloud map established over a long period of time to determine whether the detected obstacle is generated after the dynamic change of the environment or is an inherent obstacle of the site; form an obstacle map for all obstacle point clouds, and perform Bayesian probability update on each obstacle area; Assume that the prior probability of all regions being static obstacles under the initial conditions is P. After measuring an area A, the probability P of the area being of category C is obtained. C And the probability information P of the corresponding area being a static obstacle A C , the probability P' that the area is a static obstacle is obtained by using Bayes' theorem:

3. The intelligent service robot positioning and navigation system for complex environments according to claim 1, characterized in that: The specific operation of the global path planning in step S2 is: Combine the effective information of the Dijkstra algorithm starting from the initial point and the BFS algorithm starting from the target point to construct two costs: the cost h(n) from the initial point to any node n and the heuristic evaluation cost w(n) from any node n to the target point; When moving from the initial point to the target point, the heuristic function is constructed by weighing the two costs to continuously guide the robot to search for the global optimal path; Among them, the heuristic function is: f(n) = μh(n) + γw(n); The specific operations of local path planning are: The essence of the local path planning of the robot is to start from the starting point and reach the target area by moving between feasible areas. The navigation process can be described as: Path = Lmap org，free →L→Lmap n，free →L→Lmap goal，free ; The local path is optimized based on the purpose of planning a path that is closer and closer to the target, with a faster driving speed and as far away from obstacles as possible; G(v,w)=α*heading(v,w)+β*dist(v,w)+γ*vel(v,w), Where heading is used to evaluate the angle between the robot’s current driving direction and the target position; dist represents the distance between the robot and the obstacle, that is, a safety margin must be set, and a certain gap must be kept between the robot and the obstacle, and the gap increases linearly with the increase in speed; the safe speed refers to the maximum speed at which the robot can stop before colliding with the obstacle. Suppose the actual speed of the robot is (v a , w a ), the actual speed of the moving obstacle is (v b , w b ), The dynamic window method is used to sample the speed in the speed space, and the speed of random sampling is limited to reduce the number of samples, so that less calculation is introduced when evaluating the cost function, while ensuring its effectiveness; let the sampling time interval be t, considering the dynamic acceleration of the robot, downsample the search space to the dynamic window, and only retain the speed that can be reached with the current acceleration. The speed in the dynamic window should satisfy: <h2 style=";text-align:left;direction:ltr">V2 = {v, w|v, v, w∈[v<h2 style=";text-align:left;direction:ltr"> a <h2 style=";text-align:left;direction:ltr"> -v<h2 style=";text-align:left;direction:ltr"> z*t <h2 style=";text-align:left;direction:ltr"> ,v<h2 style=";text-align:left;direction:ltr"> a <h2 style=";text-align:left;direction:ltr"> +v<h2 style=";text-align:left;direction:ltr"> k*t <h2 style=";text-align:left;direction:ltr"> ]∩[w<h2 style=";text-align:left;direction:ltr"> a <h2 style=";text-align:left;direction:ltr"> -w<h2 style=";text-align:left;direction:ltr"> z*t <h2 style=";text-align:left;direction:ltr"> ,w<h2 style=";text-align:left;direction:ltr"> a <h2 style=";text-align:left;direction:ltr"> +w<h2 style=";text-align:left;direction:ltr"> k*t <h2 style=";text-align:left;direction:ltr"> ]} Finally, in the path interval Lmap nfree The safe speed set Φ obtained above n =V1∩V2, the final cost function vel is based on the safe speed set Φ n Calculated.

4. The intelligent service robot positioning and navigation system for complex environments according to claim 1, characterized in that: The outdoor GPS positioning signal is processed by Kalman filtering to enhance the robustness; Assume that the positioning result received at time k is x0, y0, z0, then: Where x, y, z are ideal state variables ε x , ε y , ε z is the first-order Markov national policy process error, ω x ,ω y ,ω z is the measurement error; Build a linear Kalman model of the observables: X(k│k-1)=Φ1(k│k-1)X(k-1) X(k)=X(k│k-1)+K(k)[B(k)-H(k)X(k│k-1)] K(k)=P(k│k-1)H T (k)[H(k)P(k│k-1))H T (k)+R(k)] -1 P(k│k-1))=Φ(k│k-1)P(k│k-1)Φ T (k│k-1)+Q(k│k-1) In the above formula, Φ1(k│k-1)={Φ 1x (k│k-1) F 1y (k│k-1) F 1z (k│k-1)} Where Q(k) is the noise cosquare difference of the system, and Q is the discretization matrix.

5. The intelligent service robot positioning and navigation system for complex environments according to claim 4, characterized in that: The indoor UWB positioning ranging measurement is symbolized as r UWB , symbolize the LiDAR-based ranging measurement as r LiD , let x t =[p 0，x，t , p 0，y，t , v 0，x，t , v 0，y，t ] T , make Represents the location of the robot beacon and the speed of the beacon The competition state related to UWB is composed of the following dynamic model evolution state X t :X t =F t X t-1 +G t w t , The transfer matrix Ft is: Gt is: The state noise wt has zero mean and covariance And δ is the sampling interval; make is the UWB-based range measured at time t from the robot to the beacon and beacon to beacon range They are nonlinear functions of pt: where i=1,2,...,N t (N t +1) / 2 indexes the pairwise combination of UWB nodes, assuming [r t ] i ,i=1,2,...,N t (N t +1) / 2a all the equivalent ranges are iid destroyed, plus noise Then optimize the obtained data to improve the robustness of positioning data: P t|t-1 =F t P t-1|t-1 F t T +Q t in and P t|t They are the updated state estimate and covariance estimate respectively; By deploying enough UWBs indoors and on robots and using the above model for positioning, a more accurate positioning effect can be obtained.

6. The intelligent service robot positioning and navigation system for complex environments according to claim 1, characterized in that: In step S4, the method for establishing the deep semantic map is: S41, estimating the position and posture of the camera in each frame image; S42, converting each frame of the image in the previous step into a point cloud, then converting the point clouds into the same coordinate system, splicing them to form a point cloud map, and then generating an occupancy grid map based on the point cloud map; S43. On the existing grid map, semantic information is assigned to 3D points in space, the semantic information containing the target is mapped into the spatial map, and semantic fusion is performed according to maximum likelihood estimation to obtain a semantic map.

7. The intelligent service robot positioning and navigation system for complex environments according to claim 6, characterized in that: The generation of the semantic information comprises the following steps: Sa. Determine the area where the target may exist; Sb, identify the extracted area and determine the type of object in the area; Sc. Segment the detected target area, extract the target from the background as accurately as possible, and distinguish the pixels belonging to the target from those belonging to the background.

8. The intelligent service robot positioning and navigation system for complex environments according to claim 1, characterized in that: The autonomous cognitive navigation in step S5 is implemented by a model of deep learning change patterns, wherein the model can represent the correlation between path passability and use this correlation to make predictions in real time during navigation; The deep learning method comprises the following steps: S51. Environmental Modeling: During the robot operation, it is assumed that the set of traversability of the entire environment is E t , the robot cannot observe the feasibility of all paths, only some of them The unobservable part is denoted as U t , that is, the robot needs to pass Z t To analyze U t possibility, and formulate the problem as: Where η is the given current observation value Z t Normalizer, and p(E t ) is the joint probability distribution of the feasibility of paths in the environment; distribution p(E t ) defines a probability function on the space of possible configurations and captures the correlation between the feasibility of the paths; Then, the joint probability is represented by a tree-structured Bayesian network through the Chow-Liu tree approximation method; the joint probability distribution on path traversability is approximated by using a flexible but bounded factor graph representation; S52, Factor Graph: We define a unary factor node for each edge and a binary factor node for each pair of topological edges to model the correlation between the traversability of the edges, approximating the joint distribution on the edges as: By storing only |E|(|E|-1) / 2+|E|, the factor graph representation can approximate the probability of an environment configuration by capturing some correlations between path traversability, thus requiring only quadratic space to represent the number of edges, rather than exponential space required for the full joint probability distribution; S53, observation and calculation: The unary factors and binary factors of the factor graph are defined as: φ i =p(e i ) Where p(e i ) and p(e i ,e j ) are the unary and binary joint probabilities of a path being passable or blocked, respectively. Based on the definition of factor nodes, the approximate joint probability distribution is: According to the robot's previous running Z 1：t-1 , calculate the joint probability p(e i ) and p(e i ,e j ).

Images