Robot positioning method and system based on trunk clustering tracking
阅读说明：本技术 一种基于树干聚类跟踪的机器人定位方法及系统 (Robot positioning method and system based on trunk clustering tracking ) 是由 张伟民 孙尧 聂富宇 王洋 石永亮 郭子源 于 2019-10-16 设计创作，主要内容包括：本发明公开了一种基于树干聚类跟踪的机器人定位方法及系统。所述方法包括：获取割胶机器人所处林间环境的激光雷达信息；根据割胶机器人预设距离范围和林间环境中物体的体积,对激光雷达信息进行过滤；采用密度聚类算法对过滤后的激光雷达信息进行聚类,得到激光雷达聚类信息；根据激光雷达聚类信息,采用圆弧拟合算法确定林间环境中每棵树木的树干圆柱面圆心二维坐标和树干位置信息；由树干圆柱面圆心二维坐标和树干位置信息,计算割胶机器人的定位信息；根据树干圆柱面圆心二维坐标和定位信息,采用迪杰斯特拉算法确定割胶机器人的行进路径,以实现割胶机器人的定位。本发明能够提高机器人的定位精度。(The invention discloses a robot positioning method and system based on trunk clustering tracking. The method comprises the following steps: acquiring laser radar information of a forest environment where a tapping robot is located; filtering the laser radar information according to a preset distance range of the rubber tapping robot and the volume of an object in a forest environment; clustering the filtered laser radar information by adopting a density clustering algorithm to obtain laser radar clustering information; determining two-dimensional coordinates of the circle center of the trunk cylindrical surface of each tree in the forest environment and trunk position information by adopting an arc fitting algorithm according to the laser radar clustering information; calculating the positioning information of the tapping robot according to the two-dimensional coordinates of the circle center of the cylindrical surface of the trunk and the position information of the trunk; and determining the advancing path of the tapping robot by adopting a Dijkstra algorithm according to the two-dimensional coordinates and the positioning information of the circle center of the cylindrical surface of the trunk so as to realize the positioning of the tapping robot. The invention can improve the positioning precision of the robot.)
1. A robot positioning method based on trunk clustering tracking is characterized by comprising the following steps:
acquiring laser radar information of a forest environment where a tapping robot is located; the laser radar information is obtained by scanning a forest environment by a laser radar positioned on the tapping robot; the laser radar information comprises distance information and angle information;
filtering the laser radar information according to a preset distance range of the rubber tapping robot and the volume of an object in the forest environment to obtain filtered laser radar information;
clustering the filtered laser radar information by adopting a density clustering algorithm to obtain laser radar clustering information; the laser radar clustering information represents the outline of each tree in the forest environment;
determining two-dimensional coordinates of the circle center of the trunk cylindrical surface of each tree in the inter-forest environment and trunk position information by adopting an arc fitting algorithm according to the laser radar clustering information; the trunk position information comprises position coordinates of the trunk, the angle of the trunk relative to the tapping robot and the radius of a trunk cylindrical surface;
calculating the positioning information of the tapping robot according to the two-dimensional coordinates of the circle center of the cylindrical surface of the trunk and the position information of the trunk; the positioning information is the relative position information of the tapping robot relative to the trunk; the relative position information comprises a position coordinate of the tapping robot and an angle of the tapping robot relative to the trunk;
and determining the advancing path of the tapping robot by adopting a Dijkstra algorithm according to the two-dimensional coordinates of the circle center of the cylindrical surface of the trunk and the positioning information so as to realize the positioning of the tapping robot.
2. The robot positioning method based on trunk clustering tracking according to claim 1, wherein the laser radar information is filtered according to a preset distance range of the tapping robot and a volume of an object in the forest environment to obtain filtered laser radar information, and specifically comprises:
deleting information which is out of a preset distance range of the tapping robot in the laser radar information to obtain preliminarily filtered laser radar information; the preset distance range of the rubber tapping robot is an area defined by a preset rectangular frame; the preset rectangular frame is a rectangle with coordinate points (-1m,0), (2m,0), (-1m,8m) and (2m,8m) as vertexes in a planar rectangular coordinate system constructed by taking the tapping robot as an origin, the right side of the tapping robot as the x-axis forward direction and the right front of the tapping robot as the y-axis forward direction;
calculating the volume of each object in the forest environment; the volume is calculated by the formula
and deleting the information of which the volume is smaller than a preset volume threshold value in the preliminarily filtered laser radar information to obtain the filtered laser radar information.
3. The robot positioning method based on trunk clustering tracking according to claim 1, wherein the clustering the filtered lidar information by using a density clustering algorithm to obtain lidar clustering information specifically comprises:
and clustering the filtered laser radar information by adopting a DBSCAN clustering algorithm according to the density of the filtered laser radar information to obtain laser radar clustering information.
4. The method according to claim 1, wherein the determining, according to the lidar clustering information, the two-dimensional coordinates of the center of the trunk cylindrical surface of each tree and the trunk position information in the inter-forest environment by using an arc fitting algorithm specifically comprises:
calculating the distance from each point in the laser radar clustering information to a preset arc;
determining a trunk curve corresponding to each tree according to the distance; the trunk curve is formed by points with equal distances from contour points of the tree to the preset circular arc;
and determining two-dimensional coordinates of the circle center of the trunk cylindrical surface of the corresponding tree and trunk position information by the trunk curve.
5. The tree trunk clustering tracking-based robot positioning method according to claim 1, wherein a Dijkstra algorithm is used to determine a traveling path of the tapping robot according to the two-dimensional coordinates of the center of the cylindrical surface of the tree trunk and the positioning information, so as to realize positioning of the tapping robot, and specifically comprises:
determining the current position of the tapping robot according to the positioning information;
judging whether the tapping robot located at the current position finishes tapping operation on the current tree or not;
if not, continuing to execute the rubber tapping operation;
if so, taking a point which is positioned right in front of the rubber tapping robot and is 0.5m away from the two-dimensional coordinate of the circle center of the trunk cylindrical surface of the current tree as the next position of the rubber tapping robot, taking the tree corresponding to the next position as the current tree, and returning to judge whether the rubber tapping robot positioned at the current position finishes rubber tapping operation on the current tree or not until rubber tapping on all trees in the forest environment is finished.
6. A robot positioning system based on trunk clustering tracking, comprising:
the radar information acquisition module is used for acquiring laser radar information of the forest environment where the tapping robot is located; the laser radar information is obtained by scanning a forest environment by a laser radar positioned on the tapping robot; the laser radar information comprises distance information and angle information;
the filtering module is used for filtering the laser radar information according to a preset distance range of the tapping robot and the volume of an object in the forest environment to obtain filtered laser radar information;
the clustering module is used for clustering the filtered laser radar information by adopting a density clustering algorithm to obtain laser radar clustering information; the laser radar clustering information represents the outline of each tree in the forest environment;
the circular arc fitting module is used for determining two-dimensional coordinates of the circle center of the trunk cylindrical surface of each tree in the inter-forest environment and trunk position information by adopting a circular arc fitting algorithm according to the laser radar clustering information; the trunk position information comprises position coordinates of the trunk, the angle of the trunk relative to the tapping robot and the radius of a trunk cylindrical surface;
the positioning information calculation module is used for calculating the positioning information of the tapping robot according to the two-dimensional coordinate of the circle center of the cylindrical surface of the trunk and the position information of the trunk; the positioning information is the relative position information of the tapping robot relative to the trunk; the relative position information comprises a position coordinate of the tapping robot and an angle of the tapping robot relative to the trunk;
and the advancing path determining module is used for determining the advancing path of the tapping robot by adopting a Dijkstra algorithm according to the two-dimensional coordinates of the circle center of the cylindrical surface of the trunk and the positioning information so as to realize the positioning of the tapping robot.
7. The robot positioning system based on trunk cluster tracking according to claim 6, wherein the filtering module specifically comprises:
the first deleting unit is used for deleting information, which is positioned outside a preset distance range of the tapping robot, in the laser radar information to obtain preliminarily filtered laser radar information; the preset distance range of the rubber tapping robot is an area defined by a preset rectangular frame; the preset rectangular frame is a rectangle with coordinate points (-1m,0), (2m,0), (-1m,8m) and (2m,8m) as vertexes in a planar rectangular coordinate system constructed by taking the tapping robot as an origin, the right side of the tapping robot as the x-axis forward direction and the right front of the tapping robot as the y-axis forward direction;
the volume calculation unit is used for calculating the volume of each object in the forest environment; the volume is calculated by the formula
and the second deleting unit is used for deleting the information of which the volume is smaller than a preset volume threshold value in the preliminarily filtered laser radar information to obtain the filtered laser radar information.
8. The tree trunk cluster tracking based robot positioning system according to claim 6, wherein the clustering module specifically comprises:
and the clustering unit is used for clustering the filtered laser radar information by adopting a DBSCAN clustering algorithm according to the density of the filtered laser radar information to obtain laser radar clustering information.
9. The robot positioning system based on trunk cluster tracking according to claim 6, wherein the arc fitting module specifically comprises:
the distance calculation unit is used for calculating the distance from each point in the laser radar clustering information to a preset arc;
a trunk curve determining unit, configured to determine a trunk curve corresponding to each tree from the distance; the trunk curve is formed by points with equal distances from contour points of the tree to the preset circular arc;
and the position information determining unit is used for determining the two-dimensional coordinates of the circle center of the trunk cylindrical surface of the corresponding tree and the trunk position information according to the trunk curve.
10. The tree trunk cluster tracking-based robot positioning system according to claim 6, wherein the travel path determining module specifically comprises:
the current position determining unit is used for determining the current position of the tapping robot according to the positioning information;
the judging unit is used for judging whether the tapping robot located at the current position finishes tapping operation on the current tree or not; if not, continuing to execute the rubber tapping operation; if so, taking a point which is positioned right in front of the rubber tapping robot and is 0.5m away from the two-dimensional coordinate of the circle center of the trunk cylindrical surface of the current tree as the next position of the rubber tapping robot, taking the tree corresponding to the next position as the current tree, and returning to judge whether the rubber tapping robot positioned at the current position finishes rubber tapping operation on the current tree or not until rubber tapping on all trees in the forest environment is finished.
Technical Field
The invention relates to the technical field of robot positioning, in particular to a robot positioning method and system based on trunk clustering tracking.
Background
Rubber tapping of rubber trees is as long as 30-40 years, and the labor investment of rubber tapping accounts for more than 60% of the total labor investment of the whole rubber production, so that the rubber tapping is the most important link in the rubber production. The quality of the tapping technology and the tapping system not only influences the yield of the rubber trees, but also influences the rubber production life of the rubber trees; meanwhile, the yield of the rubber trees is closely related to the temperature, the humidity and the illumination in the local environment, rubber tapping is usually performed in the early morning in order to ensure the rubber yield, and the heavy physical strength and the severe working environment cause the shortage of rubber workers to become a new normal state for the development of the whole natural rubber industry and also severely restrict the development of the natural rubber. Therefore, how to reduce the labor intensity of manual tapping and improve the tapping labor productivity is the bottleneck of the development of the current natural rubber industry. Therefore, the replacement of manual tapping by mechanization, automation and intelligence (namely tapping robots) is a milestone promoting the revolution of natural rubber production.
Most of the existing rubber tapping robots are of a one-machine-one-tree type, namely, one rubber tapping robot is installed on each rubber tree, the rubber tapping operation can be well realized by the aid of the rubber tapping robots, but the rubber tapping robots are high in cost and are easily damaged in severe weather. Therefore, the mobile tapping robot is provided, the robot can realize autonomous movement in a rubber forest and finish tapping actions, one robot can tap hundreds of trees, and the defects of the robot with one machine and one tree can be overcome.
In the research and development process of the mobile tapping robot, accurate tree positioning is the basic premise of normal robot positioning navigation, accurate identification of a cutter start position and cutter mark track control. The traditional tree positioning method can generate large position uncertainty under the condition that road marking points are sparse, and the uncertainty can cause the result of positioning failure of the system under the condition of long-time operation. Traditional tree positioning methods include yaw angle calculation and triangulation. The situation that the terrain is complex in the forest environment, the odometer slips and the like can occur, the traditional yaw angle calculation method adopts the reading of the odometer to calculate, and the problem of 'robot kidnapping' which cannot be recovered can occur under certain situations; the triangulation positioning method is complicated to arrange, the non-line-of-sight effect is obvious in the forest environment, and the positioning is greatly influenced. Therefore, the existing tree positioning method has the problem of inaccurate positioning, which causes the defect of low positioning precision of the robot positioning method.
Disclosure of Invention
Therefore, it is necessary to provide a robot positioning method and system based on trunk clustering tracking to improve the positioning accuracy of the robot.
In order to achieve the purpose, the invention provides the following scheme:
a robot positioning method based on trunk clustering tracking comprises the following steps:
acquiring laser radar information of a forest environment where a tapping robot is located; the laser radar information is obtained by scanning a forest environment by a laser radar positioned on the tapping robot; the laser radar information comprises distance information and angle information;
filtering the laser radar information according to a preset distance range of the rubber tapping robot and the volume of an object in the forest environment to obtain filtered laser radar information;
clustering the filtered laser radar information by adopting a density clustering algorithm to obtain laser radar clustering information; the laser radar clustering information represents the outline of each tree in the forest environment;
determining two-dimensional coordinates of the circle center of the trunk cylindrical surface of each tree in the inter-forest environment and trunk position information by adopting an arc fitting algorithm according to the laser radar clustering information; the trunk position information comprises position coordinates of the trunk, the angle of the trunk relative to the tapping robot and the radius of a trunk cylindrical surface;
calculating the positioning information of the tapping robot according to the two-dimensional coordinates of the circle center of the cylindrical surface of the trunk and the position information of the trunk; the positioning information is the relative position information of the tapping robot relative to the trunk; the relative position information comprises a position coordinate of the tapping robot and an angle of the tapping robot relative to the trunk;
and determining the advancing path of the tapping robot by adopting a Dijkstra algorithm according to the two-dimensional coordinates of the circle center of the cylindrical surface of the trunk and the positioning information so as to realize the positioning of the tapping robot.
Optionally, preset the distance range according to the tapping robot and the volume of object in the forest environment, it is right laser radar information filters, obtains laser radar information after filtering, specifically includes:
deleting information which is out of a preset distance range of the tapping robot in the laser radar information to obtain preliminarily filtered laser radar information; the preset distance range of the rubber tapping robot is an area defined by a preset rectangular frame; the preset rectangular frame is a rectangle with coordinate points (-1m,0), (2m,0), (-1m,8m) and (2m,8m) as vertexes in a planar rectangular coordinate system constructed by taking the tapping robot as an origin, the right side of the tapping robot as the x-axis forward direction and the right front of the tapping robot as the y-axis forward direction;
calculating the volume of each object in the forest environment; the volume is calculated by the formula
and deleting the information of which the volume is smaller than a preset volume threshold value in the preliminarily filtered laser radar information to obtain the filtered laser radar information.
Optionally, the clustering the filtered laser radar information by using a density clustering algorithm to obtain laser radar clustering information specifically includes:
and clustering the filtered laser radar information by adopting a DBSCAN clustering algorithm according to the density of the filtered laser radar information to obtain laser radar clustering information.
Optionally, the determining, according to the laser radar clustering information, a two-dimensional coordinate of a circle center of a trunk cylindrical surface of each tree and trunk position information by using an arc fitting algorithm specifically includes:
calculating the distance from each point in the laser radar clustering information to a preset arc;
determining a trunk curve corresponding to each tree according to the distance; the trunk curve is formed by points with equal distances from contour points of the tree to the preset circular arc;
and determining two-dimensional coordinates of the circle center of the trunk cylindrical surface of the corresponding tree and trunk position information by the trunk curve.
Optionally, according to the two-dimensional coordinate of the circle center of the trunk cylindrical surface and the positioning information, determining a traveling path of the tapping robot by using a dijkstra algorithm to realize positioning of the tapping robot, specifically including:
determining the current position of the tapping robot according to the positioning information;
judging whether the tapping robot located at the current position finishes tapping operation on the current tree or not;
if not, continuing to execute the rubber tapping operation;
if so, taking a point which is positioned right in front of the rubber tapping robot and is 0.5m away from the two-dimensional coordinate of the circle center of the trunk cylindrical surface of the current tree as the next position of the rubber tapping robot, taking the tree corresponding to the next position as the current tree, and returning to judge whether the rubber tapping robot positioned at the current position finishes rubber tapping operation on the current tree or not until rubber tapping on all trees in the forest environment is finished.
The invention also provides a robot positioning system based on trunk clustering tracking, which comprises:
the radar information acquisition module is used for acquiring laser radar information of the forest environment where the tapping robot is located; the laser radar information is obtained by scanning a forest environment by a laser radar positioned on the tapping robot; the laser radar information comprises distance information and angle information;
the filtering module is used for filtering the laser radar information according to a preset distance range of the tapping robot and the volume of an object in the forest environment to obtain filtered laser radar information;
the clustering module is used for clustering the filtered laser radar information by adopting a density clustering algorithm to obtain laser radar clustering information; the laser radar clustering information represents the outline of each tree in the forest environment;
the circular arc fitting module is used for determining two-dimensional coordinates of the circle center of the trunk cylindrical surface of each tree in the inter-forest environment and trunk position information by adopting a circular arc fitting algorithm according to the laser radar clustering information; the trunk position information comprises position coordinates of the trunk, the angle of the trunk relative to the tapping robot and the radius of a trunk cylindrical surface;
the positioning information calculation module is used for calculating the positioning information of the tapping robot according to the two-dimensional coordinate of the circle center of the cylindrical surface of the trunk and the position information of the trunk; the positioning information is the relative position information of the tapping robot relative to the trunk; the relative position information comprises a position coordinate of the tapping robot and an angle of the tapping robot relative to the trunk;
and the advancing path determining module is used for determining the advancing path of the tapping robot by adopting a Dijkstra algorithm according to the two-dimensional coordinates of the circle center of the cylindrical surface of the trunk and the positioning information so as to realize the positioning of the tapping robot.
Optionally, the filtering module specifically includes:
the first deleting unit is used for deleting information, which is positioned outside a preset distance range of the tapping robot, in the laser radar information to obtain preliminarily filtered laser radar information; the preset distance range of the rubber tapping robot is an area defined by a preset rectangular frame; the preset rectangular frame is a rectangle with coordinate points (-1m,0), (2m,0), (-1m,8m) and (2m,8m) as vertexes in a planar rectangular coordinate system constructed by taking the tapping robot as an origin, the right side of the tapping robot as the x-axis forward direction and the right front of the tapping robot as the y-axis forward direction;
the volume calculation unit is used for calculating the volume of each object in the forest environment; the volume is calculated by the formula
and the second deleting unit is used for deleting the information of which the volume is smaller than a preset volume threshold value in the preliminarily filtered laser radar information to obtain the filtered laser radar information.
Optionally, the clustering module specifically includes:
and the clustering unit is used for clustering the filtered laser radar information by adopting a DBSCAN clustering algorithm according to the density of the filtered laser radar information to obtain laser radar clustering information.
Optionally, the arc fitting module specifically includes:
the distance calculation unit is used for calculating the distance from each point in the laser radar clustering information to a preset arc;
a trunk curve determining unit, configured to determine a trunk curve corresponding to each tree from the distance; the trunk curve is formed by points with equal distances from contour points of the tree to the preset circular arc;
and the position information determining unit is used for determining the two-dimensional coordinates of the circle center of the trunk cylindrical surface of the corresponding tree and the trunk position information according to the trunk curve.
Optionally, the travel path determining module specifically includes:
the current position determining unit is used for determining the current position of the tapping robot according to the positioning information;
the judging unit is used for judging whether the tapping robot located at the current position finishes tapping operation on the current tree or not; if not, continuing to execute the rubber tapping operation; if so, taking a point which is positioned right in front of the rubber tapping robot and is 0.5m away from the two-dimensional coordinate of the circle center of the trunk cylindrical surface of the current tree as the next position of the rubber tapping robot, taking the tree corresponding to the next position as the current tree, and returning to judge whether the rubber tapping robot positioned at the current position finishes rubber tapping operation on the current tree or not until rubber tapping on all trees in the forest environment is finished.
Compared with the prior art, the invention has the beneficial effects that:
the invention provides a robot positioning method and system based on trunk clustering tracking. The method comprises the following steps: acquiring laser radar information of a forest environment where a tapping robot is located; filtering the laser radar information according to a preset distance range of the rubber tapping robot and the volume of an object in a forest environment; clustering the filtered laser radar information by adopting a density clustering algorithm to obtain laser radar clustering information; determining two-dimensional coordinates of the circle center of the trunk cylindrical surface of each tree in the forest environment and trunk position information by adopting an arc fitting algorithm according to the laser radar clustering information; calculating the positioning information of the tapping robot according to the two-dimensional coordinates of the circle center of the cylindrical surface of the trunk and the position information of the trunk; and determining the advancing path of the tapping robot by adopting a Dijkstra algorithm according to the two-dimensional coordinates and the positioning information of the circle center of the cylindrical surface of the trunk so as to realize the positioning of the tapping robot. Compared with the traditional yaw angle calculation method and the traditional triangulation method, the method can fuse the position information of a plurality of trees, utilize the information quantity obtained by the laser radar as much as possible, has high positioning precision and has high efficiency and stability in calculation.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings needed to be used in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings without inventive exercise.
Fig. 1 is a flowchart of a robot positioning method based on trunk clustering tracking according to embodiment 1 of the present invention;
FIG. 2 is a schematic view of a scanning range of a laser radar according to embodiment 2 of the present invention;
fig. 3 is a schematic diagram of a process of calculating positioning information according to embodiment 2 of the present invention;
fig. 4 is a schematic structural diagram of a robot positioning system based on trunk clustering tracking in embodiment 3 of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
In order to make the aforementioned objects, features and advantages of the present invention comprehensible, embodiments accompanied with figures are described in further detail below.