CN113885510A - Obstacle avoidance and pilot following method and system for four-legged robot - Google Patents
Obstacle avoidance and pilot following method and system for four-legged robot Download PDFInfo
- Publication number
- CN113885510A CN113885510A CN202111228506.4A CN202111228506A CN113885510A CN 113885510 A CN113885510 A CN 113885510A CN 202111228506 A CN202111228506 A CN 202111228506A CN 113885510 A CN113885510 A CN 113885510A
- Authority
- CN
- China
- Prior art keywords
- pilot
- quadruped robot
- point cloud
- obstacle avoidance
- path
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 54
- 238000007781 pre-processing Methods 0.000 claims abstract description 13
- 238000013507 mapping Methods 0.000 claims abstract description 8
- 230000006870 function Effects 0.000 claims description 12
- 239000011159 matrix material Substances 0.000 claims description 11
- 230000004888 barrier function Effects 0.000 claims description 8
- 238000001914 filtration Methods 0.000 claims description 8
- 230000008569 process Effects 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims description 4
- 230000008447 perception Effects 0.000 claims description 4
- 238000005259 measurement Methods 0.000 description 13
- 238000004891 communication Methods 0.000 description 11
- 230000007613 environmental effect Effects 0.000 description 5
- 210000002414 leg Anatomy 0.000 description 5
- 238000001514 detection method Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000004044 response Effects 0.000 description 4
- 230000002146 bilateral effect Effects 0.000 description 3
- 230000033001 locomotion Effects 0.000 description 3
- 239000003550 marker Substances 0.000 description 3
- 238000013178 mathematical model Methods 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000013528 artificial neural network Methods 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 210000004394 hip joint Anatomy 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 230000001788 irregular Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 241001455214 Acinonyx jubatus Species 0.000 description 1
- 229910000838 Al alloy Inorganic materials 0.000 description 1
- 241000124008 Mammalia Species 0.000 description 1
- 241001465754 Metazoa Species 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000000977 initiatory effect Effects 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000011664 nicotinic acid Substances 0.000 description 1
- 230000036632 reaction speed Effects 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0217—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with energy consumption, time reduction or distance reduction criteria
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a method and a system for obstacle avoidance and navigator following of a quadruped robot, wherein the method comprises the following steps: acquiring coordinate information of all base stations, the distance from a label to any one base station and original laser radar three-dimensional point cloud data; preprocessing original laser radar three-dimensional point cloud data; based on an improved ultra-wideband trilateral positioning algorithm, obtaining position information of the quadruped robot and a pilot according to coordinate information of all base stations and the distance from a tag to any one base station; mapping the preprocessed laser radar three-dimensional point cloud data to a two-dimensional plane grid map; the method comprises the steps of establishing a sliding window based on a two-dimensional plane grid map, the quadruped robot and position information of a pilot, conducting path search based on the sliding window and an autonomous navigation path planning algorithm, obtaining the shortest path after obstacle avoidance, finding the feasible shortest path through each search through the method, and having high search efficiency.
Description
Technical Field
The invention belongs to the technical field of artificial intelligence and robots, and particularly relates to a four-legged robot obstacle avoidance and pilot following method and system.
Background
The statements in this section merely provide background information related to the present disclosure and may not constitute prior art.
It is now well known that terrestrial surface area on earth is about 1.49 hundred million square meters, more than half of the terrestrial surface is a rugged, unstructured environment, and quadruped mammals can move efficiently in similar environments with great adaptability and mobility to dynamic unstructured environments. Therefore, a series of quadruped robots having quadruped animal motion characteristics, such as Bigdog, Hyq and Mini Cheetah, come out with the quadruped robots as bionic objects. In the research of the robot, the robot can be fused with people to realize natural interaction and adapt to complex environment. Therefore, research on the environment adaptability and the human-computer interaction capability of the four-footed robot becomes important. The current key technical problem is to solve the problems of obstacle avoidance and personnel tracking of the four-legged robot in a complex environment.
In the aspect of environment perception obstacle avoidance, the BigDog quadruped robot published in "Autonomous Navigation for BigDog" of IEEE International Conference on Robotics and Automation "in 2010 realizes obstacle avoidance Autonomous walking in a complex environment very early, obstacle information acquired by a plane laser scanner and a binocular camera is converted into a 2D grid diagram, and a path planning obstacle avoidance algorithm is introduced to realize Autonomous obstacle avoidance. In 2012, the LittleDog Quadruped Robot, published in "Obstacle Crossing with Stereo Vision for a Quadruped Robot" of IEEE International Conference on mechanics and Automation ", perceives environmental information through Stereo Vision, and realizes walking in rugged environment including irregular large obstacles. In addition, in 2013, the LS3 quadruped robot in the High fidelity day/night stereo mapping with the navigation and the negative attitude detection for the vision-in-the-loop walking published in IEEE/RSJ International Conference on Intelligent Robots and Systems can pass through a complex environment to a designated place without human intervention and realize autonomous traveling.
In the aspect of person identification and tracking, a method for identifying specific parts such as legs, faces, and trunk of a human body image is commonly used, and a method for extracting features of a learner through image identification is also used. However, there is currently little research on this aspect on four-footed machines, but some work has also been achieved by scholars. Leader tracking for a walking logistics, published in IEEE/RSJ International Conference on Intelligent Robots and Systems 2015, proposes a marker tracking system that uses near infrared cameras, retro-reflective markers and laser radars, allowing a particular user to designate himself as the pilot of a quadruped robot and to guide the robot along a desired path. In 2018, a Leader recognition and tracking for quadruped robots, published in IEEE International Conference on Information and Automation, distinguishes navigators of quadruped robots by using reflection markers, and adopts an algorithm of virtual expanded obstacle grids to plan paths, so that obstacle avoidance tracking is realized. In recent years, with the rapid development of image recognition technology, the students use the YOLOv3 neural network to detect And track the target person based on stereo vision, as published in 2019 in "Real-time target detection And tracking system based on stereo Control And Decision Conference" of Chinese Control And Decision Conference for squared up cameras.
In the above-described method, although the person tracking is possible by the human body specific recognition and the neural network target detection, in the case where the surrounding persons are numerous, the quadruped robot may lose the previously tracked person, and although the tracking target may be specified by pasting a specific reflective marker on the tracking target, the material of the pasted marker is easily lost by the influence of the light environment, and the large fluctuation occurs in the movement of the quadruped robot in the case of the multi-target tracking, and the tracking effect is poor.
Disclosure of Invention
In order to overcome the defects of the prior art, the invention provides a four-legged robot obstacle avoidance and navigator following method and system. Through the ultra-wideband positioning system, target personnel holding a specific label are positioned, on the basis, a 3D laser radar is further adopted to detect the barrier, a 2D grid map is constructed by combining positioning information and barrier information, a path planning algorithm is introduced, and finally obstacle avoidance and tracking of the quadruped robot are achieved.
In order to achieve the above object, one or more embodiments of the present invention provide the following technical solutions:
a barrier avoiding and pilot following method for a four-legged robot comprises the following steps:
acquiring coordinate information of all base stations, the distance from a label to any one base station and original laser radar three-dimensional point cloud data;
preprocessing original laser radar three-dimensional point cloud data;
based on an improved ultra-wideband trilateral positioning algorithm, obtaining position information of the quadruped robot and a pilot according to coordinate information of all base stations and the distance from a tag to any one base station;
mapping the preprocessed laser radar three-dimensional point cloud data to a two-dimensional plane grid map;
and establishing a sliding window based on a two-dimensional plane grid map, the quadruped robot and the position information of the pilot, and searching a path based on the sliding window and an autonomous navigation path planning algorithm to obtain the shortest path after obstacle avoidance.
Further, the preprocessing of the original laser radar three-dimensional point cloud data comprises point cloud filtering and ground point removal.
Further, the improved mode based on the improved ultra-wideband trilateration algorithm is to introduce a weighted compensation factor matrix when calculating the positions of the quadruped robot and the pilot.
Further, the two-dimensional plane grid map comprises a plurality of grids with the same size, the grids represent the working space of the robot, the environment is represented by a grid array, the grids comprise a white grid area and a black grid area, the white grid area represents free space, the black grid represents obstacle space, and the position of the black grid area in the white grid free space is the position of the obstacle in the environment where the quadruped robot is located.
Further, the sliding window construction includes:
in the two-dimensional plane grid map, a two-dimensional coordinate plane is established by taking the current position of the quadruped robot as an origin, and the area of the sliding window occupies four quadrants of the two-dimensional plane coordinate.
Further, the path search based on the sliding window and the autonomous navigation path planning algorithm comprises: setting the position of the quadruped robot as a starting point of path planning, and setting the position of a pilot as a target point; when the target point moves in the sliding window, an oblique path finding method is adopted while the path search is carried out based on the autonomous navigation path planning algorithm, and the optimal path is obtained.
Further, the autonomous navigation algorithm locks the target direction to search the shortest path through heuristic information contained in the cost function.
Further, during the search, two state lists are employed: an open list and a close list are used for storing relevant node information, the open list stores information of nodes which are generated but not accessed, and the close list stores information of nodes which are accessed.
Further, in finding a node, the node is selected by traversing the child nodes in multiple directions around the current location.
One or more embodiments provide a quadruped robot obstacle avoidance and pilot following system, which comprises a processor module and an environment sensing module connected with the processor module, wherein the environment sensing module comprises an ultra-wideband module and a 3D laser radar module, and the processor comprises:
a data acquisition module configured to: acquiring coordinate information of all base stations, the distance from a label to any one base station and original laser radar three-dimensional point cloud data;
a data pre-processing module configured to: preprocessing original laser radar three-dimensional point cloud data;
a quadruped robot and pilot position information calculation module configured to: introducing a weighted compensation factor matrix, and obtaining the position information of the quadruped robot and the pilot according to the coordinate information of all the base stations and the distance from the label to any one base station;
a path search module configured to: mapping the preprocessed laser radar three-dimensional point cloud data to a two-dimensional plane grid map; and establishing a sliding window based on a two-dimensional plane grid map, the quadruped robot and the position information of the pilot, and searching a path based on an autonomous navigation path planning algorithm to obtain the shortest path after obstacle avoidance.
The above one or more technical solutions have the following beneficial effects:
(1) the invention provides a quadruped robot obstacle avoidance and pilot following method, which is characterized in that a UWB positioning system of three base stations is built, UWB tags are carried on the quadruped robot and a pilot, wireless carrier communication between the base stations and the tags is realized by nanosecond-level non-sine wave narrow pulse transmission, distance information between the base stations and the tags is obtained, a trilateral positioning algorithm of the UWB is improved to complete resolving of tag coordinates, pilot position information and quadruped robot position information are determined, and positioning accuracy can be reduced to about 0.1m by the positioning method.
(2) The invention provides a barrier avoiding and pilot following method for a four-legged robot, which cannot ensure the consistency of the measurement distance precision from each label to a base station due to irresistible interference factors such as external environment and the like, and the difference of the distance measurement precision inevitably affects the final positioning.
(3) The method can more intuitively express the environmental information and more conveniently add a path planning obstacle avoidance algorithm, map the processed laser radar point cloud to a two-dimensional plane to construct a grid map of the path planning, project the laser radar three-dimensional point cloud to the two-dimensional plane, and construct a reference map of the path planning by adopting the grid method in the two-dimensional space, so that the reference map is simple, practical and convenient to operate.
(4) The invention provides a path planning algorithm based on a sliding window, which can find a feasible shortest path by each search by utilizing a constructed initiation type search function, has higher search efficiency, and solves the problems that the traditional path-finding algorithm is a static path planning algorithm, and a robot has low reaction speed and cannot adjust a path in time due to the fact that a new moving barrier is encountered after path planning.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this specification, are included to provide a further understanding of the invention, and are incorporated in and constitute a part of this specification, illustrate exemplary embodiments of the invention and together with the description serve to explain the invention and not to limit the invention.
Fig. 1 is an overall flowchart of a four-legged robot obstacle avoidance and pilot following method according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of a UWB ranging algorithm in accordance with an embodiment of the present invention;
fig. 3 is a schematic diagram of trilateration in accordance with an embodiment of the present invention.
FIG. 4 is a flowchart of an autonomous navigation based path planning algorithm according to a first embodiment of the present invention;
fig. 5 is an overall frame diagram of obstacle avoidance and pilot following for a quadruped robot in the second embodiment of the present invention.
Detailed Description
It is to be understood that the following detailed description is exemplary and is intended to provide further explanation of the invention as claimed. Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs.
It is noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of exemplary embodiments according to the invention. As used herein, the singular forms "a", "an" and "the" are intended to include the plural forms as well, and it should be understood that when the terms "comprises" and/or "comprising" are used in this specification, they specify the presence of stated features, steps, operations, devices, components, and/or combinations thereof, unless the context clearly indicates otherwise.
The embodiments and features of the embodiments of the invention may be combined with each other without conflict.
Example one
As shown in fig. 1, the embodiment discloses a method for obstacle avoidance and navigator following of a quadruped robot, which comprises the following steps:
s1: acquiring coordinate information of all base stations and the distance from the label to any one base station;
according to the invention, through building UWB positioning systems of three base stations, UWB tags are carried on the four-footed robot and the pilot, wireless carrier communication between the base stations and the tags is realized by nanosecond-level non-sine wave narrow pulse transmission, and distance information between the base stations and the tags is obtained.
All base station coordinates are (x)1,y1),(x2,y2),…,(xn,yn) The distances from all the tags (x, y) to any base station are d1,d2,...dn。
The method for calculating the distance from the tag to any base station comprises the following steps:
based on a bilateral time-of-flight two-way UWB ranging method, two groups of time differences on respective timestamps of any base station and two communication ends of a tag are measured.
As shown in fig. 2, as a specific embodiment, for example, it is assumed that communication ranging between a base station a and a tag B is to be implemented.
The base station A opens a TX serial port channel on a timestamp of the base station A to apply for sending a signal, and records the timestamp T at the moment after sending the signal to the tag Bas1The label B opens a serial port channel RX for receiving signals on the time stamp of the label B and receives the signals from the base station A, and the time stamp T at the moment is recorded immediately while the signals are receivedbr1；
After a period of time, the label B applies for response and receives the signal from the base station A, sends a response signal to the base station A by opening a sending serial port channel TX, and records a timestamp T when the response is sentbsThe base station A opens the receiving serial port drive RX to receive the response signal from the tag B and records the time stamp T at the momentar(ii) a After a certain time of delay processing, the base station A sends a final signal to the tag B again through the opened serial port TX channel, and records the time T of sending the final signalas2(ii) a The tag B receives the final signal from the base station A again and records the time stamp Tbr2。
Thus, four sets of time differences of base station A and tag B can be obtained as
Tround1＝Tar-Tas1 (1)
Tround2＝Tbr2-Tbs (2)
Treply1＝Tbs-Tbr1 (3)
Treply2＝Tas2-Tar (4)
Thus, the time of flight T of the wireless signal that enables base station A and tag B communicationpropComprises the following steps:
thus, a communication distance between any two can be obtained
di＝C×Tprop (6)
Wherein C is the speed of light.
Because the traditional method for two-way distance measurement based on unilateral flight time requires that the time stamps of two pieces of communication distance measurement equipment are required to be synchronous, the distance measurement precision in practical application is not very high, and accidental fluctuation exists, the method for two-way distance measurement based on bilateral flight time adopts the method for two-way distance measurement based on bilateral flight time, does not need time synchronization of two communication parties, simultaneously records two groups of time differences on respective time stamps of two communication ends, and greatly improves the distance measurement precision.
S2: and introducing a weighted compensation factor matrix, and obtaining the position information of the quadruped robot and the pilot according to the coordinate information of all the base stations and the distance from the label to any one base station.
As shown in fig. 3, the embodiment proposes an improved UWB trilateration algorithm to complete the solution of the tag coordinates and determine the position information of the pilot and the position information of the quadruped robot, and the positioning method can reduce the positioning accuracy to about 0.1 m.
The method comprises the following steps:
establishing an equation according to the coordinate information of all the base stations and the distance from the tag to any one base station:
solving the equation (1) by a least square method, and sequentially subtracting the last equation from the first n-1 equations of the equation set and arranging the equations into a matrix form:
the method is simplified to obtain:
AX＝b (9)
wherein the content of the first and second substances,
according to the principle of least squares, a least squares estimate of X can be obtained
Considering the external interference factor in practical application, introducing a weighted compensation factor matrix W, and performing the following transformation by using an equation (9):
WAX＝Wb (14)
w is a weighting matrix designed according to the distance error of the tag to be tested to each base station, and the matrix is usually a symmetric positive definite matrix. A least squares solution after label coordinate weighting is then obtained:
the traditional trilateral location algorithm obtains the position of the tag according to the position coordinates of the three base stations and the distance between the tag and the three base stations, the algorithm is visual, simple and convenient, and the trilateral location algorithm is a location algorithm which is common and easy to realize for a site model built by the three base stations. However, in practical applications, due to irresistible interference factors such as external environments, consistency of measurement distance accuracy between each tag and the base station cannot be guaranteed, and the difference of the distance measurement accuracy inevitably affects final positioning.
S3: acquiring complete original laser radar three-dimensional point cloud data, and preprocessing the original point cloud data;
in order to enable the quadruped robot to detect obstacles with different sizes as much as possible, and meanwhile, considering that in practical application, due to the fact that the laser radar is arranged above the quadruped robot, the radar plane has no obstacle, but the plane below the radar has the obstacle, based on the problems, the invention adopts the 16-line 3D laser radar, the influenced obstacles can be detected, and the robustness of obstacle detection is improved. Although the 16-line 3D laser radar can acquire a large amount of complete original point cloud information, some invalid points inevitably appear in the point cloud data due to external influences, and thus the original point cloud is preprocessed first.
Specifically, the preprocessing process of the original point cloud data comprises point cloud filtering and ground point removal.
(1) And point cloud filtering is to remove large-scale noise of the three-dimensional point cloud through a statistical filtering algorithm.
The main idea of the statistical filtering algorithm is that assuming that the average distance between each query point in the three-dimensional point cloud and its neighboring domain point satisfies the Gaussian distribution, a distance threshold is determined by its mean and variance, and points outside the distance threshold range are determined as outliers and removed.
The method comprises the following specific steps:
assuming that m neighborhood points exist in r neighborhood of a certain point in the three-dimensional point cloud, and the distance from r to each neighborhood point is djThus, the average distance d from the point r to its m neighborhood points can be calculatedaveIs composed of
Then, the average distance is statistically analyzed, the mean value μ and the standard deviation σ are calculated, and the distance threshold d is set according to the mean value μ and the standard deviation σthr
dthr＝μ±σ·α (17)
Where α is a threshold coefficient.
Finally, the average distance d of m neighbor neighborhood points of the point r is judgedaveWhether or not it is greater than a distance threshold dthr. If greater than the distance threshold dthrThen point r is an outlier and removed.
The laser point cloud map stores original scanning point clouds of a sensor to the environment, has the advantages of completely retaining information, and has the defects that when point cloud data are obtained, due to the influence caused by equipment precision and operator experience environmental factors, the diffraction characteristics of electromagnetic waves, the surface property change of a measured object and the influence of a data splicing and registering operation process, some discrete points and isolated points inevitably appear in the point cloud data, and the point cloud data cannot be directly used for navigation and obstacle avoidance, so point cloud filtering pretreatment is carried out.
According to the embodiment, the large-scale noise removal is carried out on the three-dimensional point cloud through a statistical filtering algorithm, so that the density of irregular point cloud data becomes smooth, and outliers and unnecessary noise data caused by problems such as occlusion are removed.
(2) Ground point removal
The ground point removing comprises the steps of performing height cutting on point clouds, cutting off excessive point cloud information, and finding out a plane representing the ground by adopting a RANSAC (random Sample consensus) random sampling consistency algorithm.
The core idea of the algorithm is as follows: the distribution of the whole point cloud data can be described by a mathematical model, three points are randomly selected from the original point cloud data set, and a plane model is determined according to the three points. Estimating other points by using the mathematical model, wherein the points which are adapted to the model are called local interior points, and the points which cannot be adapted to the model are local exterior points; and repeating the iteration for multiple times, wherein the model generated each time is discarded because the number of the local points is too small or is better than the existing model to be selected, and finally, an optimal plane mathematical model is determined.
The method comprises the following specific steps:
the normal plane equation of the space plane is
ax+by+cz＝l (18)
In the above formula, (a, b, c) is a unit normal vector of a plane, satisfying a2+b2+c 21, l denotes the distance between the plane and the origin.
Randomly taking three points q from a point cloud data set1(x1,y1,z1),q2(x2,y2,z2),q3(x3,y3,z3) And substituting the three parameters into the formula (18) to obtain a formula (19), solving three parameters a, b and c, and determining a plane equation S.
Judging whether other points in the point cloud data set are local points or not through the model, and taking the distance from the set point to the plane as a judgment criterion, see formula (20)
li＝|axi+byi+czi-l| (20)
Specifically, a proper distance threshold value theta is set according to actual point cloud data to approximate a fitting plane, points within the range of the threshold value theta are consistent with the model and are local points, points exceeding the distance threshold value theta are local points, and the number of the local points under the model is counted;
the above process needs to iterate many times, the group of points with the maximum number of local points is selected, and the maximum number of local points is fitted to obtain a final plane model, so that a proper iteration number k needs to be determined. The probability of random drawing to a local point is denoted by p and the confidence by c, which parameter represents the expected probability that the RANSAC algorithm will provide a useful result after running. n represents the number of data to be selected for calculating the model parameters, k represents the number of iterations, pnRepresenting the probability that the randomly drawn n points are all in-office points, 1-pnIndicating the probability of the presence of at least one outlier, (1-p)n)kIndicating the probability that at least one outlier exists for k iterations, which becomes significant enoughSmall, meaning that the higher the model confidence, the k iterations can achieve at least a set of correct model parameters with a probability of 1- (1-p)n)kThis probability is equal to the confidence parameter c, having
c＝1-(1-pn)k (21)
Taking the logarithm of the above formula, the value of k is deduced back to
k is the required iteration termination condition, and when the actual iteration number is more than k, the loop is exited.
The environment perception usually only interests obstacles on the road surface, and the ground point cloud is easy to affect the obstacle clustering, so the ground point cloud and the non-ground point cloud are usually separated before the obstacle clustering is carried out, and the ground and the non-ground are separated. Firstly, height clipping needs to be carried out on point clouds, and overhigh areas are ignored. The robot platform laser radar is installed at a height of about 0.5 m, and excessively high point cloud information is cut out, so that ground point cloud is segmented to reduce the influence of the laser radar on obstacle clustering. Then, a RANSAC (random Sample consensus) random sampling consistency algorithm is adopted to find out a plane representing the ground.
S4: mapping the preprocessed laser radar point cloud data to a two-dimensional plane grid map;
in the two-dimensional plane grid map, the environment is represented by grids, the robot working space is divided by grids with the same size, and the environment is represented by a grid array. Obstacles exist in the environment represented by the grid and occupy a certain number of grids. And a black grid represents the space of the obstacle, and a white grid represents the free space, so that the position of the black grid area in the white grid free space is the position of the obstacle in the environment where the four-footed robot is located.
In order to more intuitively represent environmental information and more conveniently add a path planning obstacle avoidance algorithm, the processed laser radar point cloud is mapped to a two-dimensional plane to construct a grid map of the path planning.
The method is characterized in that the height problem of objects in the three-dimensional space is not considered, the height of the obstacle is assumed to be within the height range which can be detected by the laser radar, the laser radar three-dimensional point cloud is projected to a two-dimensional plane, and a grid method is adopted in the two-dimensional space to construct a reference map for path planning, so that the reference map is simple, practical and convenient to operate. Therefore, the two-dimensional rasterization operation of the point cloud is performed by combining the obstacle point cloud information and the positioning information.
In practice, in order to describe the surrounding environment information more accurately, the accuracy represented by each mesh size to be divided is 0.1 m. Considering the operation speed of the processor and the efficiency of the path planning algorithm to traverse the mesh in the practical application, the size of the transformed grid map is defined as 200 × 200.
S5: and establishing a sliding window based on a two-dimensional plane grid map, the quadruped robot and the position information of the pilot, and searching a path based on the sliding window and an autonomous navigation path planning algorithm to obtain the shortest path after obstacle avoidance.
For the quadruped robot, the position of the quadruped robot is equivalent to the starting point of path planning, the position of a pilot is equivalent to the target point of the path planning, and as the quadruped robot walks along with the pilot, the ascertained environmental information can be updated in real time along with the change of the position of the quadruped robot, so that the starting point and the target point of the path planning are no longer fixed geographic coordinates.
The construction process of the sliding window specifically comprises the following steps:
(1) setting the position of the quadruped robot as a starting point of path planning, and setting the position of a pilot as a target point of the path planning;
(2) establishing a sliding window, wherein the establishing process of the sliding window comprises the following steps: and establishing a two-dimensional coordinate plane by taking the current position of the quadruped robot as an origin in a grid map, wherein the area of the sliding window occupies four quadrants of the two-dimensional plane coordinate.
When the target point moves in the sliding window, the current position of the quadruped robot is taken as a starting point, the current position of the movable target point is taken as an end point, the autonomous navigation algorithm is used for calculation, an optimal path is obtained, and meanwhile, an 8-direction oblique path finding method is adopted, so that the path searching efficiency is greatly improved.
The method for obtaining the shortest path after obstacle avoidance by searching through constructing a heuristic search function specifically comprises the following steps:
the construction process of the heuristic search function comprises the following steps:
the cost function is:
f(n)＝g(n)+h(n) (23)
in the formula: f (n) represents the total cost function from the initial position to the target node in the map, g (n) is used for representing the actual cost value of the robot from the node in the initial state to the current n node, and h (n) contains heuristic information and represents the minimum estimated cost value from the current n node to the node corresponding to the final target point.
Where g (n) is a constant value, so the solution of h (n) is crucial.
Assuming a heuristic function h*Is the optimal consumption value from node n to the end point.
h (n) and h*The relationship of (n) can be classified into the following three cases:
(1)h(n)>h*(n), a path to the destination may be found in a short time, but the path may not be the optimal solution.
(2)h(n)＝h*(n), namely, each expansion point is a point on the optimal path, and the processing result must have an optimal solution.
(3)h(n)<h*(n), the processing result must find an optimal path, but may pass through some invalid points.
In a special case, when h (n) is 0, f (n) is g (n), the autonomous navigation algorithm is converted into a Breadth-First Search algorithm (BFS), resulting in the algorithm losing heuristics.
Only in h (n) ≦ h*(n), the algorithm has the ability to complete and achieve the optimum. In practical applications, if an optimal path is desired to be obtained accurately, more heuristic information needs to be obtained, such as: the relation between the node to be selected and the end point, the weight of the node to be selected and the like. However, too many constraints increase the amount of computation and the time to compute the optimal path may be longer.
Preferably, when solving the suitable h (n) type, the heuristic function with the highest frequency is the manhattan distance. It takes into account the minimum cost from one site to another. The manhattan distance equation is as follows:
h(n)＝|xG-x1|+|yG-y1| (24)
wherein (x)1,y1) Position corresponding to the current robot coordinates in the path plan, (x)G,yG) Representing the coordinate position of the target point set in the path plan.
The flow chart of the autonomous navigation algorithm is shown in fig. 4.
(1) Putting the starting point of the robot into an open list;
(2) searching reachable nodes around the starting point of the robot, skipping over points in the close list, and taking the points as father nodes of the surrounding nodes;
(3) the starting point of the robot is deleted by the opening list and is put into a close list;
(4) calculating values in an open list;
(5) judging whether the cost function is minimum, if so, successfully searching the list, adding the point into the close list from the open list, and if not, adding the point into the close list;
(6) and (4) judging whether the point is a target point, if so, obtaining the shortest path, and otherwise, repeating the steps (1) - (5).
During the search, two state lists are mainly used: the open list and the close list are used for storing relevant node information.
Wherein the open list mainly stores information of nodes that have been generated but not yet accessed, and the close list mainly stores information of nodes that have been accessed.
When the search is started, the close list is empty, only one node, namely the starting node, is in the open list, and the search is started repeatedly to obtain the expected node.
If the node does not reach the target point, the adjacent nodes of the node are continuously searched, then the found new node is put into the open list, if the found new node is in the open list, the cost value of the new node is compared, and if the cost value is low, the information of the open list is updated until the target position is reached. If the open list is empty before reaching the destination point, it represents that no path exists. When a node is sought, the node is selected by traversing the child nodes in all directions around the current position.
The child node selection process comprises:
(1) if the child node is an obstacle, this node is ignored.
(2) If the node is in the open list, the node needs to be judged. And if the path cost of the selected current child node is less than the path cost of the covering parent node, setting the current node as the parent node of the child node. The f (n) and h (n) values of the child nodes are then modified, otherwise the child nodes are ignored.
(3) If the above situation is not met, a newly generated node is found and inserted into the open list.
Example two
The embodiment discloses a barrier is kept away to four-footed robot and pilot follows system, includes as shown in fig. 5: the robot comprises a quadruped robot platform, an environment sensing module and a processor module, wherein the quadruped robot platform comprises four legs and a robot platform connected with the legs, each leg consists of three brushless direct current motors of 48 nm and is divided into a roll hip joint with a corner of 180 degrees, a pitch hip joint with a corner of 180 degrees and a shank pitch joint with a corner of 150 degrees, and the legs are made of aluminum alloy connecting rods.
The environment sensing module comprises an Ultra Wide Band (UWB) module and a 3D laser radar module, the 3D laser radar module is arranged on the UWB module and arranged on the robot platform,
the robot lower computer controller adopts an Up Board to perform off-line motion calculation, uses a CAN bus to realize communication with a motor, and controls the robot to move.
All the devices are connected with a network switch through a network to form a ROS control network of the robot.
Preferably, the 3D laser radar module integrates 16 laser transceiving components in a hybrid solid-state laser radar mode, the measuring distance is up to 150 meters, the measuring precision is within 4cm, the number of output points per second is up to 300000 points, the horizontal angle measurement is 360 degrees, and the vertical angle measurement is-15 degrees.
In the specific implementation, the quadruped robot is required to avoid the front obstacle and track the front target person, so that the information of the obstacle behind the quadruped robot can be ignored, the horizontal view angle is set to 180 degrees, and the expected target can be completely reached.
The ultra-wideband module adopts a Link Track model, is a multifunctional system based on UWB technology, supports a local positioning function, and supports configuration into various roles such as a label and a base station. The local positioning is a real-time positioning mode supporting the integrated functions of positioning, navigation, time service and communication and is divided into three roles of a label, a base station and a console. The labels are measured in real time, the coordinates are calculated, information such as self distance measurement and coordinates is output, and the base station and the console output positioning information of all the labels in real time.
The method comprises the following steps:
a data acquisition module configured to: acquiring coordinate information of all base stations and the distance from the label to any one base station;
a data pre-processing module configured to: acquiring original laser radar three-dimensional point cloud data, and preprocessing the original laser radar three-dimensional point cloud data;
a quadruped robot and pilot position information calculation module configured to: introducing a weighted compensation factor matrix, and obtaining the position information of the quadruped robot and the pilot according to the coordinate information of all the base stations and the distance from the label to any one base station;
a path search module configured to: mapping the preprocessed laser radar three-dimensional point cloud data to a two-dimensional plane grid map;
and establishing a sliding window based on a two-dimensional plane grid map, the quadruped robot and the position information of the pilot, and searching a path based on an autonomous navigation path planning algorithm to obtain the shortest path after obstacle avoidance.
It will be appreciated by those skilled in the art that the modules or steps of the invention described above may be implemented using general purpose computing apparatus, or alternatively, they may be implemented using program code executable by computing apparatus, whereby the modules or steps may be stored in a memory device and executed by computing apparatus, or separately fabricated into individual integrated circuit modules, or multiple modules or steps thereof may be fabricated into a single integrated circuit module. The present invention is not limited to any specific combination of hardware and software.
Although the embodiments of the present invention have been described with reference to the accompanying drawings, it is not intended to limit the scope of the present invention, and it should be understood by those skilled in the art that various modifications and variations can be made without inventive efforts by those skilled in the art based on the technical solution of the present invention.
Claims (10)
1. A barrier avoiding and pilot following method for a four-legged robot is characterized by comprising the following steps:
acquiring coordinate information of all base stations, the distance from a label to any one base station and original laser radar three-dimensional point cloud data;
preprocessing original laser radar three-dimensional point cloud data;
based on an ultra-wideband trilateral positioning algorithm, obtaining the position information of the quadruped robot and the pilot according to the coordinate information of all the base stations and the distance from the tag to any one of the base stations;
mapping the preprocessed laser radar three-dimensional point cloud data to a two-dimensional plane grid map;
and establishing a sliding window based on a two-dimensional plane grid map, the quadruped robot and the position information of the pilot, and searching a path based on the sliding window and an autonomous navigation path planning algorithm to obtain the shortest path after obstacle avoidance.
2. The quadruped robot obstacle avoidance and pilot following method according to claim 1, wherein the preprocessing of the original lidar three-dimensional point cloud data comprises point cloud filtering and ground point removal.
3. The obstacle avoidance and pilot following method for the quadruped robot as claimed in claim 1, wherein the improved ultra-wideband trilateration algorithm is improved by introducing a weighted compensation factor matrix when calculating the positions of the quadruped robot and pilot.
4. The quadruped robot obstacle avoidance and pilot following method according to claim 1, wherein the two-dimensional plane grid map comprises a plurality of grids with the same size, the grids represent the working space of the robot, the environment is represented by a grid array, the grids comprise a white grid area and a black grid area, the white grid area represents a free space, the black grid represents an obstacle space, and the position of the black grid area in the white grid free space is the position of the obstacle in the environment where the quadruped robot is located.
5. The obstacle avoidance and pilot following method for the quadruped robot as claimed in claim 1, wherein the sliding window construction comprises:
in the two-dimensional plane grid map, a two-dimensional coordinate plane is established by taking the current position of the quadruped robot as an origin, and the area of the sliding window occupies four quadrants of the two-dimensional plane coordinate.
6. The obstacle avoidance and pilot following method for the quadruped robot as claimed in claim 1, wherein the path search based on the sliding window and autonomous navigation path planning algorithm comprises: setting the position of the quadruped robot as a starting point of path planning, and setting the position of a pilot as a target point; when the target point moves in the sliding window, an oblique path finding method is adopted while the path search is carried out based on the autonomous navigation path planning algorithm, and the optimal path is obtained.
7. The obstacle avoidance and pilot following method for the quadruped robot as claimed in claim 1, wherein the autonomous navigation algorithm searches the shortest path for locking the target direction by heuristic information contained in the cost function.
8. The obstacle avoidance and pilot following method for the quadruped robot as claimed in claim 1, wherein in the searching process, two state lists are used: an open list and a close list are used for storing related node information, the open list stores information of nodes which are generated but not accessed, and the close list stores information of nodes which are accessed.
9. The obstacle avoidance and pilot following method for a quadruped robot of claim 8, wherein in searching for a node, the node is selected by traversing the sub-nodes in multiple directions around the current position.
10. The utility model provides a barrier is kept away to quadruped robot and pilot follows system, includes processor module, and with the environment perception module that processor module is connected, environment perception module includes ultra wide band module and 3D laser radar module, processor includes:
a data acquisition module configured to: acquiring coordinate information of all base stations, the distance from a label to any one base station and original laser radar three-dimensional point cloud data;
a data pre-processing module configured to: preprocessing original laser radar three-dimensional point cloud data;
a quadruped robot and pilot position information calculation module configured to: introducing a weighted compensation factor matrix, and obtaining the position information of the quadruped robot and the pilot according to the coordinate information of all the base stations and the distance from the label to any one base station;
a path search module configured to: mapping the preprocessed laser radar three-dimensional point cloud data to a two-dimensional plane grid map; and establishing a sliding window based on a two-dimensional plane grid map, the quadruped robot and the position information of the pilot, and searching a path based on an autonomous navigation path planning algorithm to obtain the shortest path after obstacle avoidance.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111228506.4A CN113885510B (en) | 2021-10-21 | 2021-10-21 | Four-foot robot obstacle avoidance and pilot following method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111228506.4A CN113885510B (en) | 2021-10-21 | 2021-10-21 | Four-foot robot obstacle avoidance and pilot following method and system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113885510A true CN113885510A (en) | 2022-01-04 |
| CN113885510B CN113885510B (en) | 2023-11-10 |
Family
ID=79004258
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111228506.4A Active CN113885510B (en) | 2021-10-21 | 2021-10-21 | Four-foot robot obstacle avoidance and pilot following method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113885510B (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101179075B1 (en) * | 2012-02-28 | 2012-09-03 | 국방과학연구소 | Path planning method for autonomous robot and path planning system thereof |
| CN108051007A (en) * | 2017-10-30 | 2018-05-18 | 上海神添实业有限公司 | AGV navigation locating methods based on ultrasonic wave networking and stereoscopic vision |
| CN111461023A (en) * | 2020-04-02 | 2020-07-28 | 山东大学 | Method for quadruped robot to automatically follow pilot based on three-dimensional laser radar |
| CN112394733A (en) * | 2020-11-26 | 2021-02-23 | 吉林大学 | Intelligent vehicle autonomous following obstacle avoidance method based on UWB and ultrasonic wave |
| CN112525202A (en) * | 2020-12-21 | 2021-03-19 | 北京工商大学 | SLAM positioning and navigation method and system based on multi-sensor fusion |
-
2021
- 2021-10-21 CN CN202111228506.4A patent/CN113885510B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101179075B1 (en) * | 2012-02-28 | 2012-09-03 | 국방과학연구소 | Path planning method for autonomous robot and path planning system thereof |
| CN108051007A (en) * | 2017-10-30 | 2018-05-18 | 上海神添实业有限公司 | AGV navigation locating methods based on ultrasonic wave networking and stereoscopic vision |
| CN111461023A (en) * | 2020-04-02 | 2020-07-28 | 山东大学 | Method for quadruped robot to automatically follow pilot based on three-dimensional laser radar |
| CN112394733A (en) * | 2020-11-26 | 2021-02-23 | 吉林大学 | Intelligent vehicle autonomous following obstacle avoidance method based on UWB and ultrasonic wave |
| CN112525202A (en) * | 2020-12-21 | 2021-03-19 | 北京工商大学 | SLAM positioning and navigation method and system based on multi-sensor fusion |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113885510B (en) | 2023-11-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Xia et al. | Geometric primitives in LiDAR point clouds: A review | |
| CN108152831B (en) | Laser radar obstacle identification method and system | |
| CN106931961B (en) | Automatic navigation method and device | |
| CN111220993B (en) | Target scene positioning method and device, computer equipment and storage medium | |
| Zhu et al. | 3d lidar point cloud based intersection recognition for autonomous driving | |
| EP2849117B1 (en) | Methods, apparatuses and computer program products for automatic, non-parametric, non-iterative three dimensional geographic modeling | |
| CN113345018A (en) | Laser monocular vision fusion positioning mapping method in dynamic scene | |
| US10288425B2 (en) | Generation of map data | |
| CN112305559A (en) | Power transmission line distance measuring method, device and system based on ground fixed-point laser radar scanning and electronic equipment | |
| CN112418245A (en) | Electromagnetic emission point positioning method based on urban environment physical model | |
| CN115900710A (en) | Dynamic environment navigation method based on visual information | |
| Garrote et al. | 3D point cloud downsampling for 2D indoor scene modelling in mobile robotics | |
| CN114459467B (en) | VI-SLAM-based target positioning method in unknown rescue environment | |
| CN117685953A (en) | UWB and vision fusion positioning method and system for multi-unmanned aerial vehicle co-positioning | |
| CN113885510B (en) | Four-foot robot obstacle avoidance and pilot following method and system | |
| Schwertfeger | Robotic mapping in the real world: Performance evaluation and system integration | |
| Zhang et al. | Range-Aided Drift-Free Cooperative Localization and Consistent Reconstruction of Multi-Ground Robots | |
| CN116520302A (en) | Positioning method applied to automatic driving system and method for constructing three-dimensional map | |
| WO2022083529A1 (en) | Data processing method and apparatus | |
| CN113066161B (en) | Modeling method of urban radio wave propagation model | |
| Nabbe et al. | Opportunistic use of vision to push back the path-planning horizon | |
| Li et al. | 3D semantic map construction based on point cloud and image fusion | |
| Zhang et al. | UGV autonomous driving system design for unstructed environment | |
| CN117537803B (en) | Robot inspection semantic-topological map construction method, system, equipment and medium | |
| Liang et al. | Review of three-dimensional environment information perception and reconstruction methods for mobile robot based on multi-sensor fusion |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |