Towel heating robot capable of navigating and tracing and control method thereof

Abstract

The invention relates to the technical field of tracing, and discloses a towel heating robot capable of navigating and tracing and a control method thereof. Comprising a detection unit, a tracing unit, a judgment unit and a tracing adjustment unit, the detection unit obtains current environment information to construct a three-dimensional environment model, the tracing unit plans tracing path information for current position coordinates and target position coordinates according to the three-dimensional environment model, the judgment unit judges the tracing path information, and the tracing adjustment unit adjusts the tracing path information. When it is judged that the tracing path information exists in the historical tracing path information set, the tracing path similarity is determined according to the historical tracing path information, a first tracing path is determined according to the tracing path similarity, and when it is judged that the tracing path information does not exist in the historical tracing path information set, a second tracing path is determined according to the judgment result, the tracing adjusting unit corrects the robot according to the real-time environment information, the first tracing route or the second tracing route. The method has the characteristic of establishing the three-dimensional environment model to optimize the tracing route.

Description

Technical Field

[0001] The present invention relates to the field of tracking technology, and in particular to a towel heating robot capable of navigating and tracking and a control method thereof. Background Art

[0002] As people's awareness of hygiene increases, restaurants, bars, hotels and other crowded places pay more and more attention to providing customers with hot towel services. With the development of robot technology, navigation and tracking systems are applied to the field of robot tracking. Towel heating robots need to automatically transport hot towels along the tracking path in complex environments such as restaurants, bars and hotels, while avoiding obstacles and dynamically adjusting the path. However, there are still some shortcomings in practical applications, which limit their wide application and performance optimization. On the one hand, although the three-dimensional environmental model can accurately describe obstacles and structures in static environments, when dealing with dynamic environments, the three-dimensional environmental model will lag behind, resulting in the failure of robot path planning and the inability to avoid obstacles in time. Towel heating robots need to establish a complete three-dimensional environmental model based on dynamic environments. On the other hand, traditional tracking technologies mostly rely on pre-set physical trajectories. Towel heating robots can only travel along fixed trajectories, and it is impossible to establish new tracking routes to navigate and track the towel heating robots, which leads to reduced execution of towel heating robots.

[0003] Therefore, how to provide a towel heating robot that can navigate and track and a control method thereof is a technical problem that technical personnel in this field urgently need to solve. Summary of the invention

[0004] In view of this, the present invention proposes a towel heating robot capable of navigating and tracing and a control method thereof, aiming to solve the problem of how to establish a complete three-dimensional environment model and how to optimize the existing tracing route.

[0005] In one aspect, the present invention provides a towel heating robot capable of navigation and tracking, comprising:

[0006] A detection unit, a tracking unit, a judgment unit and a tracking adjustment unit;

[0007] The detection unit acquires current environment information and constructs a three-dimensional environment model. The detection unit generates point cloud data from the current environment information, extracts feature points of data from the point cloud data and puts them into the three-dimensional environment model.

[0008] The tracking unit obtains the current position coordinates and the target position coordinates of the towel heating robot in the three-dimensional environment model, and the tracking unit plans tracking path information for the current position coordinates and the target position coordinates according to the three-dimensional environment model;

[0009] The judgment unit is configured to make a judgment on the tracking path information;

[0010] When it is determined that the tracking path information exists in the historical tracking path information set, determining the tracking route similarity according to the historical tracking path information, and determining the first tracking route according to the tracking route similarity;

[0011] When it is determined that the tracking path information does not exist in the historical tracking path information set, determining a tracking distance priority value and an obstacle avoidance priority value according to the tracking path information;

[0012] The judging unit judges the tracking distance priority value and the obstacle avoiding priority value to obtain a judgment result, and determines the second tracking route according to the judgment result;

[0013] The tracking adjustment unit corrects the towel heating robot according to real-time environmental information, the first tracking route or the second tracking route.

[0014] Furthermore, the current environment information includes:

[0015] Obstacle information, distance information, object information, dynamic information and posture information;

[0016] The obstacle information includes static obstacles and dynamic obstacles existing in the current environment;

[0017] The distance information is, on the one hand, the distance between the current position coordinates of the robot and the static obstacle, and on the other hand, the distance between the current position coordinates of the robot and the dynamic obstacle;

[0018] The object information is the shape and size of the static obstacle and the shape and size of the dynamic obstacle;

[0019] The dynamic information is the moving position and speed of the dynamic obstacle during the movement;

[0020] The posture information is the movement posture of the dynamic obstacle during the movement.

[0021] Furthermore, the feature points of the point cloud data and the extracted data include:

[0022] The point cloud data is a collection of points of the static obstacles and the dynamic obstacles. The feature point of the extracted data is the normal vector of each point calculated in the point cloud data. The normal vector Q is obtained by the following formula:

[0023]

[0024] Where: P i Represents any point in the set, Pi The coordinates of (x i ,y i , z i ), P j P i The surrounding neighboring points, P j The coordinates of (x j ,y j , z j ), d(P i , P j ) is P i and P j distance between, select P i The surrounding k adjacent points form a neighborhood set N i ;

[0025] N i = {P j ∣d(P i , P j};

[0026] Select any point P in the neighborhood set o The coordinates are (x o ,y o , z o ), calculate the mean μ of the neighborhood points in the neighborhood set, where k represents P i The number of neighboring points around it;

[0027]

[0028] The coordinates of the mean μ are (μ x , μ y , μ z ), construct the covariance matrix C according to the mean μ;

[0029]

[0030] Where T represents the transpose of the matrix, expanding the elements of the variance matrix C:

[0031]

[0032] in:

[0033]

[0034]

[0035]

[0036]

[0037]

[0038]

[0039] The covariance matrix C is subjected to eigenvalue decomposition, and the decomposition is obtained by the following characteristic equation:

[0040] det(C-λI)=0;

[0041] Where λ is the eigenvalue, I is the identity matrix, solve the eigenvalues ​​λ1, λ2, λ3 and the corresponding eigenvectors v1, v2, v3;

[0042] Q=v max ;

[0043] The normal vector Q is the largest eigenvector corresponding to the eigenvalue.

[0044] Furthermore, the tracking unit plans tracking path information for the current position coordinates and the target position coordinates according to the three-dimensional environment model, including:

[0045] The tracking unit runs weighted (A, B), where A represents a node set, B represents an edge set, and the edge weight is W (α, β), where α represents the node of the shortest path to be determined, and β represents the node connected to α. The current position coordinate is set to S, and the tracking path information is obtained by the following formula:

[0046] Initial settings:

[0047] α = argmind(β);

[0048] Calculate the distance Z from S to β:

[0049] Z = d(α) + d{W(α, β)};

[0050]

[0051] The tracing unit substitutes all nodes and calculates the tracing path information.

[0052] Further, the judging unit determines the similarity of the tracing routes according to the historical tracing path information, and determining the first tracing route according to the tracing route similarity includes:

[0053] The similarity of the tracking route is obtained by the following formula:

[0054] The tracking path information is F, and the historical tracking path information is G;

[0055] F={F1,F2,…F n};

[0056] G={G1,G2,…G n};

[0057]

[0058] Among them, L represents the similarity of the tracking route, F i represents the i-th coordinate point in the tracking path information set F, G i represents the i-th coordinate point in the historical tracking path information set G, d(F i , G i ) represents point F i and point G i The Euclidean distance between max is the maximum value between the coordinate points in set F and the coordinate points in set G;

[0059]

[0060] When L is greater than or equal to 0.5, the judgment unit determines the first tracing route according to the historical tracing path information;

[0061] When L is less than 0.5, the judgment unit corrects the similarity of the tracking route.

[0062] Furthermore, when L is less than 0.5, the judgment unit corrects the similarity of the tracking route by:

[0063] The judgment unit inserts new coordinate points into the path with fewer coordinate points according to the tracking path information F and the historical tracking path information G, and the new coordinate points are evenly spaced from the original coordinate points, and the number of coordinate points in the tracking path information F set is consistent with the number of coordinate points in the historical tracking path information G set;

[0064] The judgment unit introduces an error tolerance ∈ when adding a new coordinate point. When the interval between the original coordinate points is less than the set threshold, the error tolerance ∈ considers that there is no interval between the original coordinate points. At this time, the similarity of the tracing route is:

[0065]

[0066] Among them, L represents the similarity of the tracking route, F i represents the i-th coordinate point in the tracking path information set F, G i represents the i-th coordinate point in the historical tracking path information set G, d(F i , G i ) represents point F i and point G i The Euclidean distance between maxis the maximum value between the coordinate points in set F and the coordinate points in set G;

[0067] The determination unit determines the first tracking route according to the correction result.

[0068] Further, the determining unit determines the tracking distance priority value and the obstacle avoidance priority value according to the tracking path information, including:

[0069] The tracking distance priority value is obtained by the following formula:

[0070]

[0071] Wherein, D represents the tracking distance priority value, dM represents the distance between the current position coordinates and the target position, and dF represents the tracking path length;

[0072] The obstacle avoidance priority value is obtained by the following formula:

[0073]

[0074] Wherein, R represents the obstacle avoidance priority value, E s represents the minimum safe distance from the static obstacle and the dynamic obstacle, E o Indicates the shortest distance between the tracking path and the static obstacle and the dynamic obstacle.

[0075] Further, the judging unit judges the tracking distance priority value and the obstacle avoiding priority value to obtain a judgment result, and determining the second tracking route according to the judgment result includes:

[0076] The judgment unit introduces γ and δ weight coefficients when making judgments, where γ represents the time urgency and δ represents the safety importance.

[0077] When a towel heating robot that can navigate and track is urgently needed to arrive, the value of γ is 1;

[0078] When there is no urgent need for the towel heating robot to arrive, the value of γ is 0.5;

[0079] When the towel heating robot that can navigate and track needs to pass through obstacles, the δ value is 1;

[0080] When the towel heating robot that can navigate and track does not need to pass through obstacles, the δ value is 0.5;

[0081] When γD+δR≥0.5, the second tracing route passes through a place with obstacles;

[0082] When γD+δR<0.5, the second tracing route does not pass through a place with obstacles.

[0083] Furthermore, the tracking adjustment unit corrects the towel heating robot according to the real-time environmental information, the first tracking route or the second tracking route, including:

[0084] The tracking adjustment unit uses a PID controller to adjust the three-dimensional posture and position of the towel heating robot in the three-dimensional environment model;

[0085] The tracking adjustment unit uses a PID controller to adjust the three-dimensional posture and position of the towel heating robot in the three-dimensional environment model;

[0086] The tracking adjustment unit obtains the current position coordinates;

[0087] The tracking adjustment unit compares the real-time environment information with the first tracking route or the second tracking route to obtain an adjustment result;

[0088] The adjustment result is an instantaneous three-dimensional coordinate adjustment;

[0089] The tracking adjustment unit corrects the x-axis, y-axis, and z-axis coordinates of the towel heating robot in the three-dimensional environment model until the towel heating robot reaches the target position coordinates.

[0090] Compared with the prior art, the beneficial effects of the present invention are as follows: the detection unit obtains the current environment information and constructs a three-dimensional environment model, the towel heating robot can perceive the environment information around it in real time, ensure that the towel heating robot can accurately identify obstacles and target positions in a complex environment, and through the feature point extraction of point cloud data, the towel heating robot can obtain more accurate environment modeling in three-dimensional space, thereby improving the accuracy of planning the tracking route, ensuring that the robot can approach the target position with the optimal tracking route, the tracking unit and the judgment unit work together, so that the towel heating robot can flexibly respond to different environmental conditions, and when it is judged that the tracking path information exists in the historical tracking path information set, the tracking route of the historical tracking path information can be reused, reducing the consumption of computing resources. When it is judged that the tracking path information does not exist in the historical tracking path information set, the second tracking route can be determined according to the judgment result, thereby enhancing the adaptability of the towel heating robot, the tracking adjustment unit corrects the towel robot, and the towel heating robot is dynamically adjusted through the PID controller, thereby improving the stability of the operation of the towel heating robot and reducing the deviation of the towel heating robot during operation.

[0091] On the other hand, the present application also provides a control method for a towel heating robot capable of navigation and tracing, which is used to apply the above-mentioned towel heating robot capable of navigation and tracing, comprising:

[0092] Obtain current environment information and build a three-dimensional environment model;

[0093] Generate point cloud data from the current environment information, extract feature points of data from the point cloud data and put them into the three-dimensional environment model;

[0094] Obtaining the current position coordinates and the target position coordinates of the robot, planning the tracking path information according to the current position coordinates and the target position coordinates, and making a judgment on the tracking path information;

[0095] When it is determined that the tracking path information exists in the historical tracking path information set, determining the tracking route similarity according to the historical tracking path information, and determining the first tracking route according to the tracking route similarity;

[0096] When it is determined that the tracking path information does not exist in the historical tracking path information set, determining a tracking distance priority value and an obstacle avoidance priority value according to the tracking path information;

[0097] Determine a result of the tracing distance priority value and the obstacle avoidance priority value, and determine a second tracing route according to the result of the tracing distance priority value and the obstacle avoidance priority value;

[0098] An adjustment result is obtained by comparing the real-time environmental information with the first tracing route or the second tracing route, and the x-axis, y-axis, and z-axis coordinates of the towel heating robot in the three-dimensional environmental model are corrected according to the adjustment result until the towel heating robot reaches the target position coordinates.

[0099] It is understandable that the above-mentioned navigation and tracking towel heating robot and control method thereof have the same beneficial effects, which will not be described in detail here. BRIEF DESCRIPTION OF THE DRAWINGS

[0100] Various other advantages and benefits will become apparent to those of ordinary skill in the art by reading the detailed description of the preferred embodiments below. The accompanying drawings are only for the purpose of illustrating the preferred embodiments and are not to be considered as limiting the present invention. Moreover, the same reference symbols are used throughout the accompanying drawings to represent the same components. In the accompanying drawings:

[0101] Figure 1 A functional block diagram of a towel heating robot capable of navigation and tracking provided by an embodiment of the present invention;

[0102] Figure 2 A flowchart of a control method for a towel heating robot capable of navigation and tracking is provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0103] Exemplary embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although exemplary embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments described herein. On the contrary, these embodiments are provided in order to enable a more thorough understanding of the present disclosure and to fully convey the scope of the present disclosure to those skilled in the art. It should be noted that, in the absence of conflict, the embodiments of the present invention and the features described in the embodiments can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0104] In some embodiments of the present application, see Figure 1 As shown, a towel heating robot capable of navigation and tracing comprises: a detection unit, a tracing unit, a judgment unit and a tracing adjustment unit. The detection unit obtains current environment information and constructs a three-dimensional environment model. The detection unit generates point cloud data from the current environment information, extracts feature points of the data according to the point cloud data and puts them into the three-dimensional environment model. The tracing unit obtains the current position coordinates and the target position coordinates of the towel heating robot in the three-dimensional environment model. The tracing unit plans tracing path information for the current position coordinates and the target position coordinates according to the three-dimensional environment model. The judgment unit is configured to judge the tracing path information. When the judgment unit is When the tracing path information is judged to exist in the historical tracing path information set, the tracing route similarity is determined according to the historical tracing path information, and the first tracing route is determined according to the tracing route similarity; when the tracing path information is judged not to exist in the historical tracing path information set, the tracing distance priority value and the obstacle avoiding priority value are determined according to the tracing path information, the judging unit judges the tracing distance priority value and the obstacle avoiding priority value to obtain a judging result, and determines the second tracing route according to the judging result; the tracing adjustment unit corrects the towel heating robot according to the real-time environmental information, the first tracing route and the second tracing route.

[0105] Specifically, a towel heating robot capable of navigation and tracking includes a heating control device and a tracking device. The tracking device includes: a detection unit, a tracking unit, a judgment unit and a tracking adjustment unit. The detection unit is responsible for obtaining environmental information around the towel heating robot, generating point cloud data based on the obtained environmental information to construct a three-dimensional environmental model. The detection unit is configured with a front detection camera, a rear detection camera and a front camera. These camera probes provide data in different forms. The front camera is a laser radar, which accurately obtains the relative position and distance of surrounding obstacles through laser reflection distance measurement. The front detection camera and the rear detection camera are depth cameras. The depth camera is used to capture the depth information of the current scene to form more accurate spatial data. In addition, the front camera is also equipped with ultrasonic and infrared sensors to supplement distance measurement and detect obstacles at a short distance. Through these camera probes, the detection unit can generate point cloud data in real time. The detection unit will extract feature points based on the point cloud data to construct a three-dimensional environmental model. The three-dimensional environmental model contains all obstacles in the current environment and the three-dimensional information of the towel heating robot itself. The tracking unit identifies its own position coordinates and target position coordinates according to the three-dimensional environment model. The target position coordinates are the destination coordinates determined by the task target confirmed or preset by the user. The tracking unit generates tracking path information from the current position coordinates to the target position coordinates through the path planning algorithm according to the current three-dimensional environment model. The judgment unit is responsible for analyzing and judging the tracking path information. When the towel heating robot reads the tracking path information and drives, the judgment unit checks whether the current tracking path information is similar to a certain path information in the historical tracking path information set. The judgment unit improves the tracking path information by calculating the similarity of the tracking route, and formulates the first tracking route according to the improved tracking path information. If the current tracking path information does not match the path information in the historical tracking path information set, the judgment unit determines the second tracking route according to the calculated tracking distance priority value and the obstacle avoidance priority value. The tracking adjustment unit is responsible for adjusting the path of the towel heating robot in real time to ensure the tracking accuracy of the towel heating robot. When the robot encounters a new obstacle during the movement, the tracking adjustment unit will correct the first tracking route and the second tracking route according to the real-time environmental information.

[0106] It is understandable that the towel heating robot can perceive the surrounding environment information in real time. Through continuous monitoring by the detection unit, the tracking adjustment unit obtains the latest environmental information and updates the three-dimensional environmental model. New obstacles will be taken into consideration. The tracking adjustment unit will make real-time corrections to the first tracking route and the second tracking route, and dynamically adjust the deviated path or unexpected situations encountered to guide the towel heating robot to avoid obstacles and improve the stability and safety of navigation and tracking.

[0107] In some embodiments of the present application, the current environment information includes: obstacle information, distance information, object information, dynamic information and posture information. The obstacle information is the static obstacles and dynamic obstacles existing in the current environment. The distance information is the distance between the robot's current position coordinates and the static obstacles on the one hand, and the distance between the robot's current position coordinates and the dynamic obstacles on the other hand. The object information is the shape and size of the static obstacles and the shape and size of the dynamic obstacles. The dynamic information is the moving position and speed of the dynamic obstacles during the movement. The posture information is the movement posture of the dynamic obstacles during the movement.

[0108] It can be understood that obstacle information refers to all obstacles existing in the current environment, which are divided into static obstacles and dynamic obstacles. Static obstacles are objects that are fixed in position and do not move in the environment, and dynamic obstacles are objects that move in the environment, such as pedestrians, animals and other robots. The distance information is divided into two parts: the distance between the towel heating robot and the static obstacle and the distance between the towel heating robot and the dynamic obstacle. By detecting the distance between the towel heating robot and the static obstacle and the distance between the dynamic obstacle, these obstacles can be avoided in time when planning the tracking path information to avoid collision. Object information refers to the characteristics of all obstacles in the current environment, including the shape and size of static and dynamic obstacles. Obtaining the shape and size of static and dynamic obstacles helps the towel heating robot evaluate the relationship with obstacles when planning the tracking path information. Dynamic information refers to the position and speed of dynamic obstacles during the movement process. It records the position changes of dynamic obstacles at different time points and the speed of dynamic obstacles, thereby ensuring the security of planning the tracking path information and ensuring the safety of the towel heating robot itself. The posture information contains the dynamic characteristics of the movement direction and inclination angle of the dynamic obstacle during the movement.

[0109] In some embodiments of the present application, the feature points of the point cloud data and the extracted data include:

[0110] The point cloud data is a collection of points of static obstacles and dynamic obstacles. The feature points of the extracted data are calculated by calculating the normal vector of each point in the point cloud data. The normal vector Q is obtained by the following formula:

[0111]

[0112] Where: P i Represents any point in the set, P i The coordinates of (x i ,y i , z i ), P j P i The surrounding neighboring points, P j The coordinates of (xj ,y j , z j ), d(P i , P j ) is P i and P j distance between, select P i The surrounding k adjacent points form a neighborhood set N i ;

[0113] N i = {P j ∣d(P i , P j};

[0114] Select any point P in the neighborhood set o The coordinates are (x o ,y o , z o ), calculate the mean μ of the neighborhood points in the neighborhood set, where k represents P i The number of neighboring points around it;

[0115]

[0116] The coordinates of the mean μ are (μ x , μ y , μ z ), construct the covariance matrix C according to the mean μ;

[0117]

[0118] Where T represents the transpose of the matrix, expanding the elements of the variance matrix C:

[0119]

[0120] in:

[0121]

[0122]

[0123]

[0124]

[0125]

[0126]

[0127] The covariance matrix C is subjected to eigenvalue decomposition, which is given by the following characteristic equation:

[0128] det(C-λI)=0;

[0129] Where λ is the eigenvalue, I is the identity matrix, solve the eigenvalues ​​λ1, λ2, λ3 and the corresponding eigenvectors v1, v2, v3;

[0130] Q=v max ;

[0131] The normal vector Q is the largest eigenvector corresponding to the eigenvalue.

[0132] It can be understood that the detection unit obtains the geometric features of all obstacles in the current environment information as point cloud data. The point cloud data needs to extract feature points before it can be put into the three-dimensional environment model. The feature points of the extracted data need to calculate the normal vector of each point in the point cloud data. First, it is necessary to select the surrounding k adjacent points to construct a neighborhood set N i , then select any point in the neighborhood set to calculate the mean μ of the neighborhood points; the mean μ represents the center position of all points in the neighborhood set, and the covariance matrix C is established based on the mean μ, (P o -μ) is point P o For the deviation of the mean μ, T represents the transposition to form an outer product, which reflects the distribution of the neighborhood points through transposition, and the elements of the covariance matrix represent the covariance between different dimensions, C xx , C yy , C zz , C xy , C xz and C yz These elements describe the linear relationship between the coordinate axes. A higher covariance value indicates that the points of the corresponding dimension have a stronger correlation in that direction. Then the covariance matrix is ​​decomposed by eigenvalues ​​to obtain the eigenvalues ​​λ1, λ2, λ3 and the corresponding eigenvectors v1, v2, v3, and then the normal vector is obtained. The normal vector represents the main direction of the local surface. The normal vector can effectively describe the distribution characteristics of the points in the point cloud data, and then accurately represent the plane, surface and edge geometric characteristics of static obstacles and dynamic obstacles in the three-dimensional environment model.

[0133] In some embodiments of the present application, the tracking unit plans the tracking path information for the current position coordinates and the target position coordinates according to the three-dimensional environment model, including: the tracking unit runs weighted (A, B), where A represents a node set, B represents an edge set, and the edge weight is W (α, β), where α represents a node of the shortest path to be determined, β represents a node connected to α, the current position coordinates are set to S, and the tracking path information is obtained by the following formula:

[0134] Initial settings:

[0135] α = argmind(β);

[0136] Calculate the distance Z from S to β:

[0137] Z = d(α) + d{W(α, β)};

[0138]

[0139] The tracing unit substitutes all nodes and calculates the tracing path information.

[0140] It can be understood that for the current position coordinate S, the shortest path distance is set to 0, and the distances for other nodes are set to infinity. In this way, the current position coordinate can be used as the starting point, and the initial distances of other nodes are all infinite. The node α closest to the current position coordinate S is selected from the node set, and the current distance is recorded. For each β connected to α, the distance from the current position coordinate S to β is calculated. If the distance from the current position coordinate S to β is less than the recorded current distance, the distance from the current position coordinate S to β is updated. When all nodes are processed, the tracking path information d(β) is obtained. By initializing the shortest path and selecting the nearest node to be determined, the distances and predecessor information of the connected nodes are updated, and finally the tracking path information is obtained.

[0141] In some embodiments of the present application, the judgment unit determines the similarity of the tracing routes according to the historical tracing path information, and the determination of the first tracing route according to the tracing route similarity includes:

[0142] The similarity of the tracking route is obtained by the following formula:

[0143] The tracking path information is F, and the historical tracking path information is G;

[0144] F={F1,F2,…F n};

[0145] G={G1,G2,…G n};

[0146]

[0147] Among them, L represents the similarity of the tracking route, F i represents the i-th coordinate point in the tracking path information set F, G i represents the i-th coordinate point in the historical tracking path information set G, d(F i , G i ) represents point F i and point G i The Euclidean distance between max is the maximum value between the coordinate points in set F and the coordinate points in set G;

[0148]

[0149] When L is greater than or equal to 0.5, the judgment unit determines a first tracing route according to the historical tracing path information;

[0150] When L is less than 0.5, the judgment unit corrects the similarity of the tracking route.

[0151] It can be understood that the tracking path information F is composed of several coordinate points F1, F2, ... F n The historical tracking path information G is also composed of several coordinate points G1, G2, ...G n The Euclidean distance is used to calculate the spatial distance difference between the corresponding points in the tracking path information and the historical tracking path information. The smaller the value, the closer the distance between the two points is, and the larger the value, the greater the deviation between the two points. i , G i ) divided by d max , normalize the distance to ensure that the distance results are within the range of [0, 1], and then average the distance results of all coordinate points, where n is the total number of coordinate points in the tracking path information set. The tracking route similarity is calculated by distance deviation. If the current tracking path information is very close to the historical tracking path information, the distance between each coordinate point in the two sets will be very small, and the similarity L will approach 1. On the contrary, if the current tracking path information is very different from the historical tracking path information, the obtained Euclidean distance is also large, and the similarity value will decrease and approach 0. 0.5 is the middle value of the similarity. When L is greater than or equal to 0.5, it means that the current tracking path information is less different from the historical tracking path information. Therefore, the judgment unit believes that the current tracking path information is consistent with the historical tracking path information, and determines to use the historical tracking path information as the first tracking route. When L is less than 0.5, it means that the current tracking path information is very different from the historical tracking path information. The judgment unit will correct the tracking route similarity to ensure that the robot can navigate according to the original historical tracking path information.

[0152] In some embodiments of the present application, when L is less than 0.5, the judgment unit corrects the similarity of the tracking route, including: the judgment unit inserts new coordinate points into the path with fewer coordinate points according to the tracking path information F and the historical tracking path information G, and the insertion of new coordinate points and the original coordinate points are evenly spaced, and the number of coordinate points in the tracking path information set is consistent with the number of coordinate points in the historical tracking path information set;

[0153] The judgment unit introduces error tolerance ∈ when adding new coordinate points. When the interval between the original coordinate points is less than the set threshold, the error tolerance ∈ considers that there is no interval between the original coordinate points. At this time, the similarity of the tracking route is:

[0154]

[0155] Among them, L represents the similarity of the tracking route, F i represents the i-th coordinate point in the tracking path information set F, G i represents the i-th coordinate point in the historical tracking path information set G, d(F i , G i ) represents point F i and point G i The Euclidean distance between max is the maximum value between the coordinate points in set F and the coordinate points in set G;

[0156] The judgment unit determines the first tracing route according to the correction result.

[0157] It is understandable that the number of coordinate points in the tracking path information F and the historical tracking path information G may be inconsistent, or the positions of their corresponding coordinate points are not well aligned, so it is necessary to insert new coordinate points into the path information with fewer coordinate points to keep the number of coordinate points consistent with the other path information, and ensure that the intervals between the inserted new coordinate points and the original coordinate points are uniform, which can make the coordinate points of the two path information more aligned. When inserting new coordinate points, the error tolerance ∈ is introduced. When the distance between the coordinate points is less than the threshold, they can be considered to be the same point. The determination of the threshold needs to be determined based on the current environmental information obtained in the current environment. The length of the static obstacles and dynamic obstacles in the environment is greater than the distance between the coordinate points. The distance between the coordinate points is much smaller than the length of the static obstacles and the dynamic obstacles. The error between the coordinate points can be completely ignored. The error tolerance ∈ assumes that there is no interval between the original coordinate points. At this time, the similarity of the corrected tracking route will approach 1. At this time, the robot navigates according to the original historical tracking path information.

[0158] In some embodiments of the present application, the determination unit determines the tracking distance priority value and the obstacle avoidance priority value according to the tracking path information, including:

[0159] The tracking distance priority value is obtained by the following formula:

[0160]

[0161] Where D represents the tracking distance priority value, dM represents the distance between the current position coordinates and the target position, and dF represents the tracking path length;

[0162] The obstacle avoidance priority value is obtained by the following formula:

[0163]

[0164] Among them, R represents the obstacle avoidance priority value, E s Indicates the minimum safe distance from static obstacles and dynamic obstacles, E o Indicates the shortest distance between the tracking path and static obstacles and dynamic obstacles.

[0165] It can be understood that dM represents the ideal shortest straight-line distance from the current position coordinates to the target position coordinates, and dF is the length of the actual tracking path. By calculating dF-dM, the redundant part of the current tracking path for the shortest path can be obtained. s is an ideal safety distance within which the towel heating robot can operate safely without collision. o is the distance between the tracking path of the towel heating robot and the nearest obstacle in the actual environment. By calculating E s -E o The safety margin of the tracking path and obstacles can be obtained.

[0166] In some embodiments of the present application, the judgment unit judges the tracking distance priority value and the obstacle avoiding priority value to obtain a judgment result, and determines the second tracking route according to the judgment result, including: introducing γ and δ weight coefficients when the judgment unit makes a judgment, γ represents the time urgency, δ represents the safety importance, when the navigable and traceable towel heating robot is urgently needed to arrive, the γ value is 1, when the navigable and traceable towel heating robot is not urgently needed to arrive, the γ value is 0.5, when the navigable and traceable towel heating robot needs to pass through obstacles, the δ value is 1, when the navigable and traceable towel heating robot does not need to pass through obstacles, the δ value is 0.5, when γD+δR≥0.5, the second tracking route passes through places with obstacles, and when γD+δR<0.5, the second tracking route does not pass through places with obstacles.

[0167] It can be understood that when γD+δR≥0.5, it indicates that the towel heating robot needs to reach the target position quickly, and the second tracking route will pass through places with obstacles to facilitate the towel heating robot to arrive quickly. When γD+δR<0.5, it indicates that there is no requirement for the arrival time of the towel heating robot, and the second tracking route avoids places with obstacles and chooses a safer way to reach the target position.

[0168] In some embodiments of the present application, the tracking adjustment unit uses a PID controller to adjust the three-dimensional posture and position of the towel heating robot in the three-dimensional environment model, the tracking adjustment unit obtains the current position coordinates, and the tracking adjustment unit compares the real-time environmental information with the first tracking route or the second tracking route to obtain an adjustment result. The adjustment result is an instantaneous three-dimensional coordinate adjustment. The tracking adjustment unit corrects the x-axis, y-axis, and z-axis coordinates of the towel heating robot in the three-dimensional environment model until the towel heating robot reaches the target position coordinates.

[0169] It is understandable that in the three-dimensional environment model, the tracking adjustment unit uses a PID controller to control and adjust the three-dimensional posture and position of the towel heating robot. The PID controller can adjust the control amount so that the towel heating robot reaches the expected goal. Through PID control, the driving wheels of the towel heating robot enable the towel heating robot to achieve smooth forward, backward and turning operations, ensuring that the towel heating robot can stably follow the set tracking route. The tracking adjustment unit continuously obtains the current position coordinates of the towel heating robot in the three-dimensional environment model and continuously monitors the current position coordinates. For new obstacles that may appear in the environment, the tracking adjustment unit compares the real-time environmental information with the first tracking route or the second tracking route to obtain an adjustment result. The adjustment result is a correction of the instantaneous three-dimensional coordinates, indicating that the driving wheels of the towel heating robot need to be corrected in a certain direction along the three axes of x, y, and z to ensure that the towel heating robot can accurately reach the target position.

[0170] In summary, the beneficial effects of the present invention are as follows: the detection unit obtains the current environmental information and constructs a three-dimensional environmental model, the towel heating robot can perceive the environmental information around it in real time, ensure that the towel heating robot can accurately identify obstacles and target positions in a complex environment, and through the feature point extraction of point cloud data, the towel heating robot can obtain more accurate environmental modeling in three-dimensional space, thereby improving the accuracy of planning the tracking route, ensuring that the robot can approach the target position with the optimal tracking route, the tracking unit and the judgment unit work together, so that the towel heating robot can flexibly respond to different environmental conditions, and when it is judged that the tracking path information exists in the historical tracking path information set, the tracking route of the historical tracking path information can be reused to reduce the consumption of computing resources. When it is judged that the tracking path information does not exist in the historical tracking path information set, the second tracking route can be determined according to the judgment result, thereby enhancing the adaptability of the towel heating robot, the tracking adjustment unit corrects the towel robot, and the towel heating robot is dynamically adjusted through the PID controller, thereby improving the stability of the operation of the towel heating robot and reducing the deviation of the towel heating robot during operation.

[0171] In another preferred embodiment based on the above embodiment, refer to Figure 2 As shown, this embodiment provides a control method for a towel heating robot capable of navigation and tracing, which is used to apply the above-mentioned towel heating robot capable of navigation and tracing, and includes:

[0172] S100: Acquire current environment information and construct a three-dimensional environment model;

[0173] Generate point cloud data from the current environment information, extract feature points from the point cloud data and put them into the 3D environment model.

[0174] S200: Obtain the current position coordinates and the target position coordinates of the robot, plan the tracking path information according to the current position coordinates and the target position coordinates, and make a judgment on the tracking path information;

[0175] When it is determined that the tracing path information exists in the historical tracing path information set, determining the tracing route similarity according to the historical tracing path information, and determining the first tracing route according to the tracing route similarity;

[0176] When it is determined that the tracking path information does not exist in the historical tracking path information set, determining a tracking distance priority value and an obstacle avoidance priority value according to the tracking path information;

[0177] S300: determining a determination result based on a tracking distance priority value and an obstacle avoidance priority value, and determining a second tracking route according to the determination result;

[0178] S400: Compare the real-time environmental information with the first tracing route or the second tracing route to obtain an adjustment result, and correct the x-axis, y-axis, and z-axis coordinates of the towel heating robot in the three-dimensional environmental model according to the adjustment result until the towel heating robot reaches the target position coordinates.

[0179] It is understandable that by obtaining the current environmental information and generating point cloud data for building a three-dimensional environmental model, the coordinates of the robot's current position and target position can be obtained, and the tracking path information can be planned based on this information to determine whether the current tracking path information is similar to the historical tracking path information. If similar, the first tracking route is selected. A comprehensive judgment is then made on the tracking distance priority value and the obstacle avoidance priority value to determine the second tracking route. Finally, the towel heating robot makes dynamic corrections based on real-time environmental information and tracking routes to ensure that the towel heating robot reaches the target position smoothly, improves the tracking efficiency and safety of the towel heating robot, and adapts to the navigation and tracking needs in various complex environments.

[0180] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program codes.

[0181] The present application is described with reference to flowcharts and / or block diagrams of methods, devices (systems) and computer program products according to embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0182] These computer program instructions may also be stored in a computer-readable storage device that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable storage device produce a product including an instruction device that implements the process. Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0183] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0184] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, ordinary technicians in the relevant field should understand that the specific implementation methods of the present invention can still be modified or replaced by equivalents. Any modification or equivalent replacement that does not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.

Claims

1. A towel heating robot capable of navigation and tracking, characterized in that: include: A detection unit, a tracking unit, a judgment unit and a tracking adjustment unit; The detection unit acquires current environment information and constructs a three-dimensional environment model. The detection unit generates point cloud data from the current environment information, extracts feature points of data from the point cloud data and puts them into the three-dimensional environment model. The tracking unit obtains the current position coordinates and the target position coordinates of the towel heating robot in the three-dimensional environment model, and the tracking unit plans tracking path information for the current position coordinates and the target position coordinates according to the three-dimensional environment model; The judgment unit is configured to make a judgment on the tracking path information; When it is determined that the tracking path information exists in the historical tracking path information set, determining the tracking route similarity according to the historical tracking path information, and determining the first tracking route according to the tracking route similarity; When it is determined that the tracking path information does not exist in the historical tracking path information set, determining a tracking distance priority value and an obstacle avoidance priority value according to the tracking path information; The judging unit judges the tracking distance priority value and the obstacle avoiding priority value to obtain a judgment result, and determines the second tracking route according to the judgment result; The tracking adjustment unit corrects the towel heating robot according to real-time environmental information, the first tracking route or the second tracking route.

2. The navigable and traceable towel heating robot according to claim 1, characterized in that: The current environment information includes: Obstacle information, distance information, object information, dynamic information and posture information; The obstacle information includes static obstacles and dynamic obstacles existing in the current environment; The distance information is, on the one hand, the distance between the current position coordinates of the robot and the static obstacle, and on the other hand, the distance between the current position coordinates of the robot and the dynamic obstacle; The object information is the shape and size of the static obstacle and the shape and size of the dynamic obstacle; The dynamic information is the moving position and speed of the dynamic obstacle during the movement; The posture information is the movement posture of the dynamic obstacle during the movement.

3. The towel heating robot capable of navigation and tracking according to claim 2, characterized in that: The feature points of the point cloud data and the extracted data include: The point cloud data is a collection of points of the static obstacle and the dynamic obstacle. The feature point of the extracted data is the normal vector of each point calculated in the point cloud data. The normal vector Q is obtained by the following formula: Where: P i Represents any point in the set, P i The coordinates of (x i ,y i , z i ), P j P i The surrounding neighboring points, P j The coordinates of (x j ,y j , z j ), d(P i , P j ) is P i and P j distance between, select P i The surrounding k adjacent points form a neighborhood set N i ; N i ={P j ∣d(P i ,P j )}; Select any point P in the neighborhood set o The coordinates are (x o ,y o , z o ), calculate the mean μ of the neighborhood points in the neighborhood set, where k represents P i The number of neighboring points around it; The coordinates of the mean μ are (μ x , μ y , μ z ), construct the covariance matrix C according to the mean μ; Where T represents the transpose of the matrix, expanding the elements of the variance matrix C: in: The covariance matrix C is subjected to eigenvalue decomposition, and the decomposition is obtained by the following characteristic equation: det(C-λI)=0; Where λ is the eigenvalue, I is the identity matrix, solve the eigenvalues ​​λ1, λ2, λ3 and the corresponding eigenvectors v1, v2, v3; Q=v max ; The normal vector Q is the largest eigenvector corresponding to the eigenvalue.

4. The towel heating robot capable of navigation and tracking according to claim 1, characterized in that: The tracking unit plans tracking path information for the current position coordinates and the target position coordinates according to the three-dimensional environment model, including: The tracking unit runs weighted (A, B), where A represents a node set, B represents an edge set, and the edge weight is W (α, β), where α represents the node of the shortest path to be determined, and β represents the node connected to α. The current position coordinate is set to S, and the tracking path information is obtained by the following formula: Initial setting: d(S) = 0, d(β) = ∞, α = argmind(β); Calculate the distance Z from S to β: Z = d(α) + d{W(α, β)}; The tracing unit substitutes all nodes and calculates the tracing path information.

5. The towel heating robot capable of navigation and tracking according to claim 2, characterized in that: The judging unit determines the similarity of the tracing routes according to the historical tracing path information, and determining the first tracing route according to the tracing route similarity includes: The similarity of the tracking route is obtained by the following formula: The tracking path information is F, and the historical tracking path information is G; F={F1,F2,…F n }; G={G1,G2,…G n }; Among them, L represents the similarity of the tracking route, F i represents the i-th coordinate point in the tracking path information set F, G i represents the i-th coordinate point in the historical tracking path information set G, d(F i , G i ) represents point F i and point G i The Euclidean distance between max is the maximum value between the coordinate points in set F and the coordinate points in set G; When L is greater than or equal to 0.5, the judgment unit determines the first tracing route according to the historical tracing path information; When L is less than 0.5, the judgment unit corrects the similarity of the tracking route.

6. The towel heating robot capable of navigation and tracking according to claim 5, characterized in that: When L is less than 0.5, the judgment unit corrects the similarity of the tracking route including: The judgment unit inserts new coordinate points into the path with fewer coordinate points according to the tracking path information F and the historical tracking path information G, and the new coordinate points are evenly spaced from the original coordinate points, and the number of coordinate points in the tracking path information F set is consistent with the number of coordinate points in the historical tracking path information G set; The judgment unit introduces an error tolerance ∈ when adding a new coordinate point. When the interval between the original coordinate points is less than the set threshold, the error tolerance ∈ considers that there is no interval between the original coordinate points. At this time, the similarity of the tracing route is: Among them, L represents the similarity of the tracking route, F i represents the i-th coordinate point in the tracking path information set F, G i represents the i-th coordinate point in the historical tracking path information set G, d(F i , G i ) represents point F i and point G i The Euclidean distance between max is the maximum value between the coordinate points in set F and the coordinate points in set G; The determination unit determines the first tracking route according to the correction result.

7. The towel heating robot capable of navigation and tracking according to claim 5, characterized in that: The determining unit determines the tracking distance priority value and the obstacle avoidance priority value according to the tracking path information, including: The tracking distance priority value is obtained by the following formula: Wherein, D represents the tracking distance priority value, dM represents the distance between the current position coordinates and the target position, and dF represents the tracking path length; The obstacle avoidance priority value is obtained by the following formula: Wherein, R represents the obstacle avoidance priority value, E s represents the minimum safe distance from the static obstacle and the dynamic obstacle, E o Indicates the shortest distance between the tracking path and the static obstacle and the dynamic obstacle.

8. The towel heating robot capable of navigation and tracking according to claim 7, characterized in that: The judging unit judges the tracking distance priority value and the obstacle avoiding priority value to obtain a judgment result, and determining the second tracking route according to the judgment result includes: The judgment unit introduces γ and δ weight coefficients when making judgments, where γ represents the time urgency and δ represents the safety importance. When a towel heating robot that can navigate and track is urgently needed to arrive, the value of γ is 1; When there is no urgent need for the towel heating robot to arrive, the value of γ is 0.5; When the towel heating robot that can navigate and track needs to pass through obstacles, the δ value is 1; When the towel heating robot that can navigate and track does not need to pass through obstacles, the δ value is 0.5; When γD+δR≥0.5, the second tracing route passes through a place with obstacles; When γD+δR<0.5, the second tracing route does not pass through a place with obstacles.

9. The towel heating robot capable of navigation and tracking according to claim 1, characterized in that: The tracking adjustment unit corrects the towel heating robot according to the real-time environmental information, the first tracking route or the second tracking route, including: The tracking adjustment unit uses a PID controller to adjust the three-dimensional posture and position of the towel heating robot in the three-dimensional environment model; The tracking adjustment unit obtains the current position coordinates; The tracking adjustment unit compares the real-time environment information with the first tracking route or the second tracking route to obtain an adjustment result; The adjustment result is an instantaneous three-dimensional coordinate adjustment; The tracking adjustment unit corrects the x-axis, y-axis, and z-axis coordinates of the towel heating robot in the three-dimensional environment model until the towel heating robot reaches the target position coordinates.

10. A control method for a towel heating robot capable of navigation and tracing, used for applying the towel heating robot capable of navigation and tracing as claimed in any one of claims 1 to 9, characterized in that: include: Obtain current environment information and build a three-dimensional environment model; Generate point cloud data from the current environment information, extract feature points of data from the point cloud data and put them into the three-dimensional environment model; Obtaining the current position coordinates and the target position coordinates of the robot, planning the tracking path information according to the current position coordinates and the target position coordinates, and making a judgment on the tracking path information; When it is determined that the tracking path information exists in the historical tracking path information set, determining the tracking route similarity according to the historical tracking path information, and determining the first tracking route according to the tracking route similarity; When it is determined that the tracking path information does not exist in the historical tracking path information set, determining a tracking distance priority value and an obstacle avoidance priority value according to the tracking path information; Determine a result of the tracing distance priority value and the obstacle avoidance priority value, and determine a second tracing route according to the result of the tracing distance priority value and the obstacle avoidance priority value; An adjustment result is obtained by comparing the real-time environmental information with the first tracing route or the second tracing route, and the x-axis, y-axis, and z-axis coordinates of the towel heating robot in the three-dimensional environmental model are corrected according to the adjustment result until the towel heating robot reaches the target position coordinates.

Images