Pepper field navigation control method, device, equipment and medium

Abstract

The present application relates to a chili field navigation control method, apparatus, equipment, and medium. The method includes: responding to navigation control instructions of a picking robot in a chili field, obtaining global navigation satellite system data, inertial measurement unit data, and lidar data from the picking robot; using an extended Kalman filter and a Monte Carlo positioning algorithm to calculate and determine the current position of the picking robot based on the global navigation satellite system data, inertial measurement unit data, and lidar data; determining the picking starting point and picking end point of the picking robot, and using an A* algorithm to generate an initial picking path for the picking robot based on the current position, picking starting point, and picking end point of the picking robot; and optimizing the initial picking path of the picking robot based on the TEB algorithm to determine the optimal picking path for the picking robot. The present application can effectively improve the operating efficiency and safety of the robot.

Description

Technical Field

[0001] The present application relates to the field of agricultural production, and in particular to a pepper field navigation control method, corresponding device, electronic equipment and computer-readable storage medium. Background Art

[0002] The ground in the pepper field is relatively bumpy, and the harvesting robot is prone to inaccurate positioning and positioning drift when conducting autonomous navigation in the complex field environment. The traditional autonomous navigation system that only uses a single sensor can no longer meet the needs of high-precision positioning in complex field environments.

[0003] Existing technologies rely on single-chip microcomputers, which have low computing power and poor storage capacity, making them unsuitable for long-term development. Traditional path planning algorithms each have their own limitations and are difficult to achieve the desired results in complex field terrain. Using a single sensor to perceive the outside world often results in large errors, and in areas without GNSS or with weak GNSS signals, it is prone to defects such as inaccurate positioning.

[0004] To sum up, in the existing technology, the ground of the pepper field is relatively bumpy, and the picking robot is prone to inaccurate positioning and positioning drift when performing autonomous navigation in the complex field environment. The applicant has made corresponding explorations to solve this problem. Summary of the Invention

[0005] The purpose of this application is to solve the above problems and provide a pepper field navigation control method, corresponding device, electronic device and computer-readable storage medium.

[0006] In order to meet the various objectives of this application, this application adopts the following technical solutions:

[0007] A pepper field navigation control method proposed to meet one of the purposes of this application includes:

[0008] Responding to navigation control instructions of a harvesting robot in a pepper field, obtaining global navigation satellite system data, inertial measurement unit data, and lidar data in the harvesting robot;

[0009] Using an extended Kalman filter and a Monte Carlo positioning algorithm to calculate and determine the current position of the picking robot based on the global navigation satellite system data, the inertial measurement unit data, and the lidar data;

[0010] Determine the picking starting point and the picking end point of the picking robot, and use the A* algorithm to generate an initial picking path of the picking robot according to the current posture of the picking robot, the picking starting point and the picking end point;

[0011] The initial picking path of the picking robot is optimized based on the TEB algorithm to determine the optimal picking path of the picking robot, so as to complete the navigation control of the picking robot in the pepper field.

[0012] Optionally, the step of using an extended Kalman filter and a Monte Carlo positioning algorithm to calculate and determine the current position of the picking robot based on the global navigation satellite system data, the inertial measurement unit data, and the lidar data includes:

[0013] The expression of the nonlinear system model of the extended Kalman filter is:

[0014] x(k+1)=f(X(k))+ω(k),

[0015] Z(k)=h(X(k))+v(k),

[0016] The process noise of the nonlinear system model is expressed as:

[0017] ω(k)~N(0,Q(k)),

[0018] The observation noise of the nonlinear system model is expressed as:

[0019] v(k)~N(0,R(k)),

[0020] In the prediction step, the extended Kalman filter uses a nonlinear state transition model to predict the next state variable and predicts the covariance, which is expressed as:

[0021] X(k+1|k)=f(X(k)),

[0022] (k+1|k)=h(X(k+1|k)),

[0023] P(k+1|k)=F(k+1)P(k)F(k+1)′+Q(k+1), where,

[0024] is the Jacobian matrix of the nonlinear state transition model;

[0025] is the linearized observation matrix;

[0026] In the update step, the Kalman gain is estimated by the linearized observation model and used to update the state estimate and its covariance. Its expression includes:

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

[0028] X(k+1)=X(k+1|k)+K(k+1)[Z(k+1)-Z(k+1|k)],

[0029] P(k+1)=[IK(k+1)H(k+1)]P(k+1|k),

[0030] The extended Kalman filter provides an approximately optimal estimate of the state in a nonlinear dynamic system through a prediction step and an update step;

[0031] Where F is the Jacobian matrix of the state transfer model, Q is the covariance matrix of the process noise, H is the Jacobian matrix of the observation model, R is the covariance matrix of the measurement noise, and Z is the state vector of the system at time k; f is the nonlinear state transfer function, u(k) is the control input vector at time k; w(k) is the process noise, which is assumed to be Gaussian distributed, z(k) is the measurement vector at time k, H is the nonlinear measurement function, and v(k) is the measurement noise, which is assumed to be Gaussian distributed.

[0032] Optionally, the step of determining a picking starting point and a picking end point of the picking robot, and using an A* algorithm to generate an initial picking path of the picking robot according to the current posture, picking starting point, and picking end point of the picking robot includes:

[0033] Initialize the open list, closed list, and node attributes, where the open list is used to store nodes to be evaluated, and the closed list is used to store evaluated nodes. The node attributes include g(n), h(n), and f(n), where g(n) represents the actual path cost from the picking starting point to the current node, h(n) represents the estimated cost from the current node to the target node, f(n) represents the total estimated cost, and n represents the current node;

[0034] Select the node with the smallest f(n) value from the open list and record it as the current node. If the current node is the target node, the path planning is completed; otherwise, move it to the closed list;

[0035] Traverse all adjacent nodes of the current node. If the adjacent node is in the closed list, skip it. Calculate the g(n) value, h(n) value and f(n) value of the adjacent node.

[0036] If the adjacent node is not in the open list, add the adjacent node and set its parent node to the current node; if the adjacent node is already in the open list and the new g(n) value is smaller, update its g(n) value and f(n) value and set a new parent node;

[0037] Trace backward from the target node to find the complete path from the picking start point to the picking end point, and use it as the initial picking path of the picking robot.

[0038] Optionally, the step of optimizing the initial picking path of the picking robot based on the TEB algorithm to determine the optimal picking path of the picking robot includes:

[0039] Obtaining path optimization objectives and cost functions, wherein the path optimization objectives include ensuring the smoothness of the picking robot's trajectory, ensuring that the path avoids collisions with obstacles, and optimizing the time cost of the path so that the robot can quickly reach the picking destination. The cost function includes the number of collisions, path length, time consumption, and target position offset;

[0040] The preset TEB algorithm is called to determine the optimal picking path of the picking robot according to the path optimization target and the cost function.

[0041] Optionally, the number of collisions represents the number of times the picking robot has obvious contact with obstacles in the entire navigation process; the path length represents the length of the navigation planned path between the picking starting point and the picking end point; the time consumption represents the average time taken by the picking robot to reach the picking end point from the picking starting point; the target position offset represents the distance between the actual position reached by the picking robot and the picking end point.

[0042] Optionally, the step of optimizing the initial picking path of the picking robot based on the TEB algorithm to determine the optimal picking path of the picking robot includes:

[0043] Obtaining an optimal picking path for the picking robot, and discretizing the optimal picking path into various picking key points;

[0044] The picking robot is controlled to pick peppers according to each key picking point in the optimal picking path to complete the navigation control of the picking robot in the pepper field.

[0045] Optionally, the master node of the pepper field navigation control system subscribes to the filtered odometer topic published by the extended Kalman filter node to obtain the initial position of the picking robot, wherein the master node and the Kalman filter node subscribe to each other.

[0046] A chili field navigation control device provided for another purpose of the present application includes:

[0047] a data acquisition module configured to respond to navigation control instructions of the harvesting robot in the pepper field and acquire global navigation satellite system data, inertial measurement unit data, and lidar data from the harvesting robot;

[0048] a current posture determination module configured to calculate and determine the current posture of the picking robot based on the global navigation satellite system data, the inertial measurement unit data, and the lidar data using an extended Kalman filter and a Monte Carlo positioning algorithm;

[0049] an initial path determination module, configured to determine a picking starting point and a picking end point of the picking robot, and to generate an initial picking path of the picking robot using an A* algorithm according to the current position, picking starting point, and picking end point of the picking robot;

[0050] The optimal path determination module is configured to optimize the initial picking path of the picking robot based on the TEB algorithm to determine the optimal picking path of the picking robot to complete the navigation control of the picking robot in the pepper field.

[0051] An electronic device provided to meet another purpose of the present application includes a central processing unit and a memory, wherein the central processing unit is used to call and run a computer program stored in the memory to execute the steps of the pepper field navigation control method described in the present application.

[0052] A computer-readable storage medium is provided to meet another purpose of the present application, which stores a computer program implemented according to the pepper field navigation control method in the form of computer-readable instructions. When the computer program is called and executed by a computer, the steps included in the corresponding method are executed.

[0053] Compared with the existing technology, this application addresses the problems in the existing technology where the ground in the pepper field is relatively bumpy, and the picking robot is prone to inaccurate positioning and positioning drift when autonomously navigating in the complex field environment. This application includes but is not limited to the following beneficial effects:

[0054] By combining the extended Kalman filter and the Monte Carlo positioning algorithm, this application can effectively fuse GNSS, IMU data, and LIDAR data to accurately calculate the current position of the harvesting robot, enabling robust positioning and navigation in complex pepper field environments.

[0055] By integrating the A* algorithm with the TEB algorithm, the path can be adjusted and optimized in real time in a dynamic environment to meet the requirements of various picking tasks. By determining the initial picking path through the A* algorithm and optimizing the initial picking path through the TEB algorithm, the robot's operating efficiency and safety can be effectively improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] The above and / or additional aspects and advantages of the present application will become apparent and easily understood from the following description of the embodiments in conjunction with the accompanying drawings, in which:

[0057] Figure 1Schematic diagram of the flow of the pepper field navigation control method in the embodiment of the present application;

[0058] Figure 2 This is a schematic diagram of the pepper field environment in the embodiment of this application;

[0059] Figure 3 This is a schematic diagram of a picking robot in an embodiment of the present application;

[0060] Figure 4 This is a flowchart of the algorithm of the extended Kalman filter in the embodiment of the present application;

[0061] Figure 5 This is a flowchart of a navigation control experiment for a harvesting robot in an embodiment of the present application;

[0062] Figure 6 Schematic diagram of a bar chart showing the number of collisions in each experimental scheme in the examples of this application;

[0063] Figure 7 Schematic diagram showing the comparison of the path length and average path length of each experimental scheme in the examples of this application;

[0064] Figure 8 A schematic diagram showing the comparison of the path time and average path time of each experimental scheme in the embodiments of the present application;

[0065] Figure 9 Schematic diagram showing the comparison of the path deviation and average path deviation of each experimental scheme in the embodiment of the present application;

[0066] Figure 10 This is a functional block diagram of the pepper field navigation control device in an embodiment of the present application;

[0067] Figure 11 Schematic diagram of the structure of the computer device in the embodiment of the present application. DETAILED DESCRIPTION

[0068] The following describes in detail embodiments of the present application, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present application, and are not to be construed as limiting the present application.

[0069] It will be understood by those skilled in the art that, unless expressly stated otherwise, the singular forms "a", "an", "said" and "the" used herein may also include the plural forms. It should be further understood that the term "comprising" used in the specification of the present application refers to the presence of the features, integers, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or groups thereof. It should be understood that when we refer to an element as being "connected" or "coupled" to another element, it may be directly connected or coupled to the other element, or there may be intermediate elements. In addition, "connected" or "coupled" as used herein may include wireless connections or wireless couplings. The term "and / or" used herein includes all or any units and all combinations of one or more associated listed items.

[0070] It will be understood by those skilled in the art that, unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as commonly understood by those skilled in the art to which this application belongs. It should also be understood that terms such as those defined in common dictionaries should be understood to have meanings consistent with their meanings in the context of the prior art and will not be interpreted in an idealized or overly formal sense unless specifically defined as herein.

[0071] It will be understood by those skilled in the art that the terms "client," "terminal," and "terminal device" as used herein include both devices that are wireless signal receivers, i.e., devices that only have wireless signal receivers without transmission capabilities, and devices that have receiving and transmitting hardware capable of two-way communication over a two-way communication link. Such devices may include: cellular or other communication devices such as personal computers and tablet computers, which have single-line displays, multi-line displays, or cellular or other communication devices without multi-line displays; PCS (Personal Communications Service), which may combine voice, data processing, fax, and / or data communication capabilities; PDA (Personal Digital Assistant), which may include a radio frequency receiver, a pager, Internet / Intranet access, a web browser, a notepad, a calendar, and / or a GPS (Global Positioning System) receiver; and conventional laptop and / or palmtop computers or other devices, which have and / or include a radio frequency receiver. As used herein, the terms "client," "terminal," or "terminal device" may be portable, transportable, or installed in a vehicle (air, sea, and / or land), or may be adapted and / or configured to operate locally and / or in a distributed manner at any other location on Earth and / or in space. As used herein, the terms "client," "terminal," or "terminal device" may also refer to a communication terminal, an Internet terminal, or a music / video playback terminal, such as a PDA, an MID (Mobile Internet Device), and / or a mobile phone with music / video playback capabilities, or may include a smart TV, a set-top box, or other device.

[0072] The hardware referred to by names such as "server", "client", and "service node" in this application is essentially an electronic device with capabilities equivalent to those of a personal computer. It is a hardware device that has the necessary components revealed by the von Neumann principle, such as a central processing unit (including an arithmetic unit and a controller), a memory, an input device, and an output device. Computer programs are stored in its memory, and the central processing unit loads the program stored in the external memory into the internal memory for execution, executes the instructions in the program, and interacts with the input and output devices to complete specific functions.

[0073] It should be noted that the concept of "server" referred to in this application can also be extended to server clusters. Based on the network deployment principles understood by those skilled in the art, the servers described should be logically divided. In physical space, these servers can be independent of each other but callable through interfaces, or integrated into a single physical computer or a computer cluster. Those skilled in the art should understand this flexibility and should not use it to constrain the implementation of the network deployment method of this application.

[0074] Unless expressly specified, one or more technical features of the present application can be deployed on a server for implementation and accessed by a client through a remote call to obtain an online service interface provided by the server, or can be directly deployed and run on a client for implementation.

[0075] Unless expressly specified otherwise, the neural network models referenced or may be referenced in this application may be deployed on a remote server and remotely called on the client, or may be deployed and directly called on a client with sufficient device capabilities. In some embodiments, when it runs on the client, its corresponding intelligence may be obtained through transfer learning to reduce the requirements for the client's hardware operating resources and avoid excessive occupation of the client's hardware operating resources.

[0076] Unless explicitly specified, the various data involved in this application can be stored remotely on a server or on a local terminal device, as long as they are suitable for being called by the technical solution of this application.

[0077] Those skilled in the art should be aware that although the various methods of this application are described based on the same concept and thus exhibit commonality, unless otherwise specified, these methods can be independently executed. Similarly, the various embodiments disclosed in this application are all based on the same inventive concept. Therefore, concepts with the same expression, as well as concepts that are appropriately transformed for convenience despite different expression, should be understood as equivalent.

[0078] Unless expressly stated to be mutually exclusive, the various embodiments disclosed in this application may be cross-combined with the relevant technical features of the various embodiments to flexibly construct new embodiments, as long as such combination does not deviate from the creative spirit of this application and can meet the needs of the prior art or resolve certain deficiencies in the prior art. Those skilled in the art should be aware of such flexibility.

[0079] See also Figure 1 、 Figure 2 as well as Figure 3 In one embodiment, the pepper field navigation control method of the present application includes:

[0080] Step S10, responding to navigation control instructions of the picking robot in the pepper field, obtaining global navigation satellite system data, inertial measurement unit data, and lidar data in the picking robot;

[0081] The pepper field navigation control system can respond to the navigation control instructions of the picking robot in the pepper field and obtain the global navigation satellite system data, inertial measurement unit data and lidar data of the picking robot. Specifically, since the ground of the pepper field is relatively bumpy, it is easy to cause positioning drift. In order to solve the limitation problem of a single sensor and comprehensively utilize different sensor data to achieve the complementary advantages between them, this application integrates lidar, inertial measurement unit (IMU) and global navigation satellite system (GNSS) to accurately position the picking robot.

[0082] In the specific embodiment, the Silan S2 lidar, WHEELTEC N200 IMU and WHEELTEC DETA100 GNSS were selected, and the extended Kalman filter (EKF) algorithm running in the ROS unmanned system architecture was used to perform a series of fusion operations on global navigation satellite system data, inertial measurement unit data and lidar data.

[0083] Step S20: using an extended Kalman filter and a Monte Carlo positioning algorithm to calculate and determine the current position of the picking robot based on the global navigation satellite system data, the inertial measurement unit data, and the lidar data;

[0084] After obtaining global navigation satellite system data, inertial measurement unit data, and lidar data from the picking robot, an extended Kalman filter and a Monte Carlo positioning algorithm are used to calculate and determine the current position of the picking robot based on the global navigation satellite system data, inertial measurement unit data, and lidar data;

[0085] In the multi-sensor fusion process, lidar is primarily responsible for capturing the structural features of the external environment and accurately matching them with the real-world map. The inertial measurement unit (IMU) provides attitude data, providing continuous data on the robot's velocity and tilt angles in all directions. The global navigation satellite system (GNSS) primarily assists in providing macroscopic position information. By fusing the data from these three sensors using an extended Kalman filter and combining it with the adaptive Monte Carlo localization (AMCL) algorithm, this method leverages the high precision of lidar, the global positioning capabilities of GNSS, and the short-term stability of the IMU to achieve more accurate and reliable positioning results. Accurate pose estimation and updates not only improve positioning accuracy but also maintain positioning continuity and accuracy even when the GPS signal is weak or lost, leveraging information from the inertial measurement unit (IMU) and lidar.

[0086] When performing multi-sensor fusion, each sensor operates independently and provides its own solution results respectively, and then these results are fused, that is, a loosely coupled fusion method is adopted to provide overall navigation information. Each sensor module is relatively independent and easy to expand and maintain. In this way, errors introduced by sensor noise and environmental factors can be corrected in complex field environments. At the same time, this application reflects the uncertainty of the system model by adjusting the process noise covariance, performs smooth state estimation, and adjusts the response speed of the filter, optimizes the fusion effect of different sensors, improves the stability and robustness of the filter, and makes the fused data more in line with the needs of pepper field environment positioning.

[0087] At the algorithm level, this application takes optimization measures. In order to solve the problem of publishing erroneous information because the fusion node cannot determine the robot's position in the map when there is no GNSS signal and GNSS is received, the master node also needs to subscribe to the filtered odometry topic published by the extended Kalman filter (EKF) node to obtain the initial position of the picking robot. By forming a data loop through mutual subscription, the accuracy is improved and errors are reduced.

[0088] Specifically, in order to achieve accurate positioning even in the absence of GNSS signals, some optimization measures were taken in the study. The master node needs to subscribe to the filtered odometry topic published by the EKF (Extended Kalman Filter) node. This means that the master node will receive the odometry data processed by the EKF node to obtain the initial position of the robot. Through this subscription relationship, a data loop is formed between the master node and the EKF node. This means that information is constantly exchanged between the two, and the position data obtained by the master node can be used to correct or update the EKF estimate, and vice versa. By subscribing to each other, the master node can obtain filtered odometry information in real time, which can improve the robot's position accuracy. In the absence of GNSS signals, the robot can still rely on odometry data for relative positioning, reducing the release of erroneous information due to position uncertainty.

[0089] Inaccurate positioning is the main problem that causes the inability to reach the target point in the implementation of pepper field navigation. This application uses the extended Kalman filter (EKF) to realize the data fusion of multiple sensors including: lidar, GNSS global satellite positioning module, and IMU inertial navigation unit to obtain the accurate position and posture of the picking robot in real time.

[0090] Specifically, see Figure 4 The Extended Kalman Filter (EKF) is a nonlinear extension of the standard Kalman filter. It introduces nonlinear state transition and observation models to represent the state space model of the system, where the state variables are presented in vector form. Due to the nonlinear relationships in these models, the standard Kalman filter cannot be directly applied. Therefore, the EKF uses Taylor series expansion to linearize the nonlinear model and uses the Jacobian matrix to approximate these nonlinear relationships, thereby treating it as a linear model locally.

[0091] The expression of the nonlinear system model of the extended Kalman filter is:

[0092] x(k+1)=f(X(k))+ω(k),

[0093] Z(k)=h(X(k))+v(k),

[0094] The process noise of the nonlinear system model is expressed as:

[0095] ω(k)~N(0,Q(k)),

[0096] The observation noise of the nonlinear system model is expressed as:

[0097] v(k)~N(0,R(k)),

[0098] In the prediction step, the extended Kalman filter uses a nonlinear state transition model to predict the next state variable and predicts the covariance, which is expressed as:

[0099] X(k+1|k)=f(X(k)),

[0100] (k+1|k)=h(X(k+1|k)),

[0101] P(k+1|k)=F(k+1)P(k)F(k+1)′+Q(k+1), where,

[0102] is the Jacobian matrix of the nonlinear state transition model;

[0103] is the linearized observation matrix;

[0104] In the update step, the Kalman gain is estimated by the linearized observation model and used to update the state estimate and its covariance. Its expression includes:

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

[0106] X(k+1)=X(k+1|k)+K(k+1)[Z(k+1)-Z(k+1|k)],

[0107] P(k+1)=[IK(k+1)H(k+1)]P(k+1|k),

[0108] Where F is the Jacobian matrix of the state transfer model, Q is the covariance matrix of the process noise, H is the Jacobian matrix of the observation model, R is the covariance matrix of the measurement noise, and Z is the state vector of the system at time k; f is the nonlinear state transfer function, u(k) is the control input vector at time k; w(k) is the process noise, which is assumed to be Gaussian distributed, z(k) is the measurement vector at time k, H is the nonlinear measurement function, and v(k) is the measurement noise, which is assumed to be Gaussian distributed.

[0109] Through a prediction step and an update step, the extended Kalman filter provides a near-optimal estimate of the state in a nonlinear dynamic system.

[0110] Step S30: determining the picking starting point and the picking end point of the picking robot, and using the A* algorithm to generate an initial picking path of the picking robot according to the current posture, the picking starting point and the picking end point of the picking robot;

[0111] An extended Kalman filter and a Monte Carlo positioning algorithm are used to calculate and determine the current posture of the picking robot based on the global navigation satellite system data, the inertial measurement unit data, and the lidar data. Then, the picking starting point and the picking end point of the picking robot are determined. An A* algorithm is used to generate an initial picking path for the picking robot based on the current posture, the picking starting point, and the picking end point of the picking robot.

[0112] In a specific embodiment, the steps of determining the picking starting point and the picking end point of the picking robot, and using the A* algorithm to generate the initial picking path of the picking robot according to the current posture, the picking starting point, and the picking end point of the picking robot include:

[0113] Step S301: Initialize the open list, closed list, and node attributes. The open list is used to store nodes to be evaluated, and the closed list is used to store evaluated nodes. The node attributes include g(n), h(n), and f(n). G(n) represents the actual path cost from the picking starting point to the current node, h(n) represents the estimated cost from the current node to the target node, f(n) represents the total estimated cost, and n represents the current node.

[0114] Step S302: Select the node with the smallest f(n) value from the open list and record it as the current node. If the current node is the target node, the path planning is completed; otherwise, move it to the closed list.

[0115] Step S303: traverse all adjacent nodes of the current node. If the adjacent node is in the closed list, skip it; calculate the g(n) value, h(n) value and f(n) value of the adjacent node;

[0116] Step S304: If the adjacent node is not in the open list, add the adjacent node and set its parent node as the current node; if the adjacent node is already in the open list and the new g(n) value is smaller, update its g(n) value and f(n) value and set a new parent node;

[0117] Step S305: Trace backward from the target node to find a complete path from the picking start point to the picking end point, and use it as the initial picking path of the picking robot.

[0118] In some embodiments, the formula and calculation process for initializing the path mainly involve two key functions: a heuristic function h(n) and a path cost function g(n). These two functions are used together to calculate the comprehensive evaluation function f(n) of node n:

[0119] f(n)=g(n)+h(n),

[0120] Where: g(n) is the actual path cost from the starting node to the current node n, h(n) is the heuristic function, which is the estimated cost from the current node n to the target node.

[0121] Set the function requirements:

[0122] 1) Do not overestimate the cost of traveling from a node to a target node;

[0123] 2) For each node n and its successor node n′, satisfy:

[0124] h(n)≤c(n,n′)+h(n′),

[0125] Among them, c(n,n′) is the actual cost from n to n′.

[0126] Step S40: Optimize the initial picking path of the picking robot based on the TEB algorithm to determine the optimal picking path of the picking robot, so as to complete the navigation control of the picking robot in the pepper field.

[0127] Determine the picking starting point and picking end point of the picking robot, use the A* algorithm to generate the initial picking path of the picking robot according to the current posture, picking starting point and picking end point of the picking robot, and then optimize the initial picking path of the picking robot based on the TEB algorithm to determine the optimal picking path of the picking robot, so as to complete the navigation control of the picking robot in the pepper field.

[0128] In a specific embodiment, the step of optimizing the initial picking path of the picking robot based on the TEB algorithm to determine the optimal picking path of the picking robot includes:

[0129] Step S401: Obtaining a path optimization objective and a cost function, wherein the path optimization objective includes ensuring the smoothness of the picking robot's trajectory, ensuring that the path avoids collisions with obstacles, and optimizing the time cost of the path so that the robot can quickly reach the picking destination. The cost function includes the number of collisions, path length, time consumption, and target position offset;

[0130] The number of collisions represents the number of times the picking robot has obvious contact with obstacles during the entire navigation process; the path length represents the length of the navigation planning path between the picking starting point and the picking end point; the time consumption represents the average time taken by the picking robot to reach the picking end point from the picking starting point; the target position offset represents the distance between the actual position reached by the picking robot and the picking end point.

[0131] Step S402: calling a preset TEB algorithm to determine the optimal picking path of the picking robot according to the path optimization target and the cost function.

[0132] In some embodiments, the step of optimizing the initial picking path of the picking robot based on the TEB algorithm to determine the optimal picking path of the picking robot includes:

[0133] Step S4001: obtaining an optimal picking path of the picking robot, and discretizing the optimal picking path into various picking key points;

[0134] Step S4002: Control the picking robot to pick peppers according to each key picking point in the optimal picking path to complete the navigation control of the picking robot in the pepper field.

[0135] In some embodiments, the following experimental schemes were used in pepper fields with the same slope:

[0136] Experimental plan 1: standard TEB algorithm + radar + IMU;

[0137] Experimental solution 2: standard TEB algorithm + radar + IMU + GNSS;

[0138] Experimental plan three: Optimize TEB algorithm + radar + IMU;

[0139] Experimental Plan 4: Optimize the positioning and navigation effects of the TEB algorithm + radar + IMU + GNSS. The experimental area is set between the ridges of a typical pepper field.

[0140] Among them, the number of collisions represents the number of times the picking robot has obvious contact with obstacles in the entire navigation process; the path length represents the length of the navigation planning path between the picking starting point and the picking end point; the time consumption represents the average time taken by the picking robot to reach the picking end point from the picking starting point; the target position offset represents the distance between the actual position reached by the picking robot and the picking end point.

[0141] In some embodiments, Figure 6 Schematic diagram of a bar chart showing the number of collisions in each experimental scheme in the examples of this application; Figure 7 Schematic diagram showing the comparison of the path length and average path length of each experimental scheme in the examples of this application; Figure 8 A schematic diagram showing the comparison of the path time and average path time of each experimental scheme in the embodiments of the present application; Figure 9 Schematic diagram comparing the path deviation and average path deviation of various experimental schemes in the embodiments of the present application.

[0142] See also Figure 5 and Figure 6 , 10 sets of data for each scheme were obtained from the experiment. The experimental results after sorting and analysis are as follows:

[0143] From the bar chart, we can see that in the experiment, the number of collisions between narrow ridges in Experimental Scheme 1 was as high as 8 times, and multiple collisions occurred during the experiment; the number of collisions in Experimental Scheme 2 decreased after integrating GNSS positioning; Experimental Scheme 3 also partially optimized the number of collisions based on path smoothing and optimal fusion solution. The collision locations of the above three schemes all occurred at the turning points; the number of collisions in Experimental Scheme 4 was only 1, which greatly reduced the probability of ridge collision in the pepper field.

[0144] See also Figures 7 to 9 Data comparison shows that the navigation system based on multi-sensor data fusion and the optimized TEB algorithm reduces the average planned path length by 0.58m, shortens the average time from the starting point to the target location by 2.55s, and reduces the average target position offset by 4.3cm. The above line graph shows that compared to the first three solutions, the fluctuations in the line for Solution 4 are smaller, indicating that the data fluctuations over multiple experiments are smaller, improving the overall navigation stability.

[0145] As can be seen from the above embodiments, compared with the prior art, the present application addresses the problems in the prior art where the ground of pepper fields is relatively bumpy, and the picking robots are prone to inaccurate positioning and positioning drift when autonomously navigating in complex field environments. The present application has, but is not limited to, the following beneficial effects:

[0146] By combining the extended Kalman filter and the Monte Carlo positioning algorithm, this application can effectively fuse GNSS, IMU data, and LIDAR data to accurately calculate the current position of the harvesting robot, enabling robust positioning and navigation in complex pepper field environments.

[0147] By integrating the A* algorithm with the TEB algorithm, the path can be adjusted and optimized in real time in a dynamic environment to meet the requirements of various picking tasks. By determining the initial picking path through the A* algorithm and optimizing the initial picking path through the TEB algorithm, the robot's operating efficiency and safety can be effectively improved.

[0148] See also Figure 10A pepper field navigation control device provided to meet one of the purposes of this application includes a data acquisition module 1100, a current posture determination module 1200, an initial path determination module 1300 and an optimal path determination module 1400. Among them, the data acquisition module 1100 is configured to respond to the navigation control instructions of the picking robot in the pepper field, and obtain the global navigation satellite system data, inertial measurement unit data and lidar data in the picking robot; the current posture determination module 1200 is configured to use the extended Kalman filter and Monte Carlo positioning algorithm to calculate and determine the current posture of the picking robot based on the global navigation satellite system data, inertial measurement unit data and lidar data; the initial path determination module 1300 is configured to determine the picking starting point and picking end point of the picking robot, and use the A* algorithm to generate the initial picking path of the picking robot according to the current posture, picking starting point and picking end point of the picking robot; the optimal path determination module 1400 is configured to optimize the initial picking path of the picking robot based on the TEB algorithm to determine the optimal picking path of the picking robot to complete the navigation control of the picking robot in the pepper field.

[0149] Based on any embodiment of this application, please refer to Figure 11 Another embodiment of the present application further provides an electronic device, which can be implemented by a computer device, such as Figure 11 As shown, a schematic diagram of the internal structure of a computer device. The computer device includes a processor, a computer-readable storage medium, a memory, and a network interface connected via a system bus. Among them, the computer-readable storage medium of the computer device stores an operating system, a database, and computer-readable instructions. The database may store a control information sequence. When the computer-readable instructions are executed by the processor, the processor may implement a pepper field navigation control method. The processor of the computer device is used to provide computing and control capabilities to support the operation of the entire computer device. The memory of the computer device may store computer-readable instructions. When the computer-readable instructions are executed by the processor, the processor may execute the pepper field navigation control method of the present application. The network interface of the computer device is used to connect and communicate with the terminal. Those skilled in the art will understand that Figure 11 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than shown in the figure, or combine certain components, or have a different component arrangement.

[0150] In this embodiment, the processor is used to execute Figure 10The memory stores the program code and various data required to execute the specific functions of each module in the field navigation control device. The network interface is used to transmit data between user terminals or servers. The memory in this embodiment stores the program code and data required to execute all modules / submodules in the chili field navigation control device of this application. The server can call the server's program code and data to execute the functions of all submodules.

[0151] The present application also provides a storage medium storing computer-readable instructions. When the computer-readable instructions are executed by one or more processors, the one or more processors execute the steps of the pepper field navigation control method described in any embodiment of the present application.

[0152] The present application also provides a computer program product, including a computer program / instruction, which, when executed by one or more processors, implements the steps of the pepper field navigation control method described in any embodiment of the present application.

[0153] Those skilled in the art will appreciate that all or part of the processes in the above-mentioned embodiments of the present application can be implemented by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium. When the program is executed, it can include the processes of the above-mentioned embodiments of the method. The aforementioned storage medium can be a computer-readable storage medium such as a magnetic disk, an optical disk, a read-only memory (ROM), or a random access memory (RAM).

[0154] The above description is only part of the implementation methods of the present application. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present application. These improvements and modifications should also be regarded as the scope of protection of the present application.

Claims

1. A pepper field navigation control method, characterized in that: include: Responding to navigation control instructions of a harvesting robot in a pepper field, obtaining global navigation satellite system data, inertial measurement unit data, and lidar data in the harvesting robot; Using an extended Kalman filter and a Monte Carlo positioning algorithm to calculate and determine the current position of the picking robot based on the global navigation satellite system data, the inertial measurement unit data, and the lidar data; Determine the picking starting point and the picking end point of the picking robot, and use the A* algorithm to generate the initial picking path of the picking robot according to the current posture, picking starting point and picking end point of the picking robot, which includes: Initialize the open list, closed list, and node attributes, where the open list is used to store nodes to be evaluated, and the closed list is used to store evaluated nodes. The node attributes include g(n), h(n), and f(n), where g(n) represents the actual path cost from the picking starting point to the current node, h(n) represents the estimated cost from the current node to the target node, f(n) represents the total estimated cost, and n represents the current node; Select the node with the smallest f(n) value from the open list and record it as the current node. If the current node is the target node, the path planning is completed; otherwise, move it to the closed list; Traverse all adjacent nodes of the current node. If the adjacent node is in the closed list, skip it. Calculate the g(n) value, h(n) value and f(n) value of the adjacent node. If the adjacent node is not in the open list, add the adjacent node and set its parent node to the current node; if the adjacent node is already in the open list and the new g(n) value is smaller, update its g(n) value and f(n) value and set a new parent node; Tracing backward from the target node to find the complete path from the picking start point to the picking end point, and using it as the initial picking path of the picking robot; The initial picking path of the picking robot is optimized based on the TEB algorithm to determine the optimal picking path of the picking robot, which includes: Obtaining path optimization objectives and cost functions, wherein the path optimization objectives include ensuring the smoothness of the picking robot's trajectory, ensuring that the path avoids collisions with obstacles, and optimizing the time cost of the path so that the robot can quickly reach the picking destination. The cost function includes the number of collisions, path length, time consumption, and target position offset; The preset TEB algorithm is called to determine the optimal picking path of the picking robot according to the path optimization target and the cost function, so as to complete the navigation control of the picking robot in the pepper field.

2. The pepper field navigation control method according to claim 1, characterized in that: The number of collisions represents the number of times the picking robot makes obvious contact with obstacles during the entire navigation process; The path length represents the length of the navigation planning path between the picking starting point and the picking end point; the time consumption represents the average time taken by the picking robot to reach the picking end point from the picking starting point; The target position offset represents the distance between the actual position reached by the picking robot and the picking end point.

3. The pepper field navigation control method according to any one of claims 1 to 2, characterized in that: The step of optimizing the initial picking path of the picking robot based on the TEB algorithm to determine the optimal picking path of the picking robot includes: Obtaining an optimal picking path for the picking robot, and discretizing the optimal picking path into various picking key points; The picking robot is controlled to pick peppers according to each key picking point in the optimal picking path to complete the navigation control of the picking robot in the pepper field.

4. The pepper field navigation control method according to any one of claims 1 to 2, characterized in that: The master node of the pepper field navigation control system subscribes to the filtered odometer topic published by the extended Kalman filter node to obtain the initial position of the picking robot, wherein the master node and the Kalman filter node subscribe to each other.

5. A pepper field navigation control device, characterized in that: include: a data acquisition module configured to respond to navigation control instructions of the harvesting robot in the pepper field and acquire global navigation satellite system data, inertial measurement unit data, and lidar data from the harvesting robot; a current posture determination module configured to calculate and determine the current posture of the picking robot based on the global navigation satellite system data, the inertial measurement unit data, and the lidar data using an extended Kalman filter and a Monte Carlo positioning algorithm; The initial path determination module is configured to determine the picking starting point and the picking end point of the picking robot, and uses the A* algorithm to generate the initial picking path of the picking robot according to the current posture, picking starting point and picking end point of the picking robot, which includes: Initialize the open list, closed list, and node attributes, where the open list is used to store nodes to be evaluated, and the closed list is used to store evaluated nodes. The node attributes include g(n), h(n), and f(n), where g(n) represents the actual path cost from the picking starting point to the current node, h(n) represents the estimated cost from the current node to the target node, f(n) represents the total estimated cost, and n represents the current node; Select the node with the smallest f(n) value from the open list and record it as the current node. If the current node is the target node, the path planning is completed; otherwise, move it to the closed list; Traverse all adjacent nodes of the current node. If the adjacent node is in the closed list, skip it. Calculate the g(n) value, h(n) value and f(n) value of the adjacent node. If the adjacent node is not in the open list, add the adjacent node and set its parent node to the current node; if the adjacent node is already in the open list and the new g(n) value is smaller, update its g(n) value and f(n) value and set a new parent node; Tracing backward from the target node to find the complete path from the picking start point to the picking end point, and using it as the initial picking path of the picking robot; The optimal path determination module is configured to optimize the initial picking path of the picking robot based on the TEB algorithm to determine the optimal picking path of the picking robot, and includes: Obtaining path optimization objectives and cost functions, wherein the path optimization objectives include ensuring the smoothness of the picking robot's trajectory, ensuring that the path avoids collisions with obstacles, and optimizing the time cost of the path so that the robot can quickly reach the picking destination. The cost function includes the number of collisions, path length, time consumption, and target position offset; The preset TEB algorithm is called to determine the optimal picking path of the picking robot according to the path optimization target and the cost function, so as to complete the navigation control of the picking robot in the pepper field.

6. An electronic device comprising a central processing unit and a memory, characterized in that: The central processing unit is configured to call and run a computer program stored in the memory to execute the steps of the method according to any one of claims 1 to 4.

7. A computer-readable storage medium, characterized in that It stores a computer program implemented according to the method described in any one of claims 1 to 4 in the form of computer-readable instructions, and when the computer program is called and executed by a computer, the steps included in the corresponding method are executed.

Images