Chili field navigation control method, device, equipment and medium

Abstract

The invention relates to a chili field navigation control method, device and equipment and a medium, and the method comprises the steps: responding to a navigation control instruction of a picking robot in a chili field, and obtaining global navigation satellite system data, inertial measurement unit data and laser radar data in the picking robot; adopting an extended Kalman filter and a Monte Carlo positioning algorithm to calculate and determine the current pose of the picking robot according to the global navigation satellite system data, the inertial measurement unit data and the laser radar data; determining a picking start point and a picking end point of the picking robot, and generating an initial picking path of the picking robot by adopting an A * algorithm according to the current pose, the picking start point and the picking end point of the picking robot; and optimizing the initial picking path of the picking robot based on a TEB algorithm to determine the optimal picking path of the picking robot. The operation efficiency and safety of the robot can be effectively improved.

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 devices, 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 performing 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 are limited to single-chip microcomputers, which have low computing power and poor storage capacity, which is not conducive to long-term development; traditional path planning algorithms each have their limitations and it is difficult to achieve the expected results in complex field terrains; using a single sensor to perceive the outside world often causes large errors, and in places where there is no GNSS or the GNSS signal is weak, it is prone to defects such as inaccurate positioning.

[0004] To sum up, in the prior art, 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 a 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-mentioned problems and to 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] In response to navigation control instructions of the harvesting robot in the 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 and posture of the picking robot according to 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 position, picking starting point and picking end point of the picking robot;

[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 and posture of the picking robot according to 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 transfer 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 transfer model;

[0025] is the linearized observation matrix;

[0026] In the update step, the Kalman gain is estimated through the linearized observation model, and the Kalman gain is 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, wherein the open list is used to store the nodes to be evaluated, the closed list is used to store the evaluated nodes, and the node attributes include g(n), h(n) and f(n), wherein 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 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;

[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 a path optimization target and a cost function, wherein the path optimization target includes ensuring that the trajectory of the picking robot is smooth, 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 end point, and 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; and 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] Acquire the optimal picking path of the picking robot, and discretize into various picking key points according to the optimal picking path;

[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 adapted to another purpose of the present application comprises:

[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 in the harvesting robot;

[0048] A current posture determination module is configured to use an extended Kalman filter and a Monte Carlo positioning algorithm to calculate and determine the current posture of the picking robot according to the global navigation satellite system data, the inertial measurement unit data and the laser radar data;

[0049] An initial path determination module is 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 according to the current position, picking starting point and picking end point of the picking robot using an A* algorithm;

[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 prior art, the present application aims at the problem that the pepper field in the prior art is relatively bumpy, and the picking robot is prone to inaccurate positioning and positioning drift when autonomously navigating in a complex field environment. The present 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, accurately calculate the current position of the harvesting robot, and achieve 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 needs of various picking tasks. The initial picking path is determined by the A* algorithm, and the initial picking path is optimized by the TEB algorithm, which can effectively improve the robot's operating efficiency and safety. 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 1This is a flow chart of a pepper field navigation control method in an embodiment of the present application;

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

[0059] Figure 3 This is a schematic diagram of a harvesting 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 A schematic diagram of a bar chart of the number of collisions in each experimental scheme in the embodiments of the present application;

[0063] Figure 7 A schematic diagram showing the comparison of the path length and average path length of each experimental scheme in the embodiments of the present application;

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

[0065] Fig. 9 A schematic diagram showing the comparison of the path deviation and the average path deviation of each experimental scheme in the embodiment of the present application;

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

[0067] Fig.11 It is a schematic diagram of the structure of the computer device in the embodiment of the present application. DETAILED DESCRIPTION

[0068] The embodiments of the present application are described in detail below, and examples of the embodiments 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 cannot be interpreted as limiting the present application.

[0069] It will be understood by those skilled in the art that, unless expressly stated, the singular forms "one", "said", and "the" used herein may also include 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 an intermediate element. In addition, the "connection" or "coupling" used herein may include wireless connection or wireless coupling. The term "and / or" used herein includes all or any unit 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 those generally understood by those skilled in the art to which this application belongs. It should also be understood that terms such as those defined in general dictionaries should be understood to have meanings consistent with those in the context of the prior art, and will not be interpreted with idealized or overly formal meanings unless specifically defined as here.

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

[0072] The hardware referred to by the names such as "server", "client", and "service node" in this application is essentially an electronic device with the equivalent capabilities of a personal computer. It is a hardware device with 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. The computer program is stored in its memory, and the central processing unit calls 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 pointed out that the concept of "server" referred to in this application can also be extended to the case of server clusters. According to the network deployment principle understood by those skilled in the art, the servers should be logically divided. In physical space, these servers can be independent of each other but can be called through interfaces, or integrated into a physical computer or a set of computer clusters. Those skilled in the art should understand this flexibility, and should not use it to restrict 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 access.

[0075] The neural network models referenced or may be referenced in this application, unless expressly specified, can be deployed on a remote server and remotely called on the client, or can be deployed and directly called on a client with sufficient device capabilities. In some embodiments, when it runs on the client, its corresponding intelligence can 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 it is 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 the present application are described based on the same concept and thus present commonality to each other, unless otherwise specified, these methods can be independently executed. Similarly, for each embodiment disclosed in the present application, they are all proposed based on the same inventive concept, therefore, concepts with the same expression, and concepts that are appropriately changed for convenience despite different expressions, should be understood as equivalent.

[0078] Unless the mutually exclusive relationship between the embodiments to be disclosed in this application is explicitly stated, the relevant technical features involved in each embodiment can be cross-combined to flexibly construct a new embodiment, 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 solve certain deficiencies in the prior art. Those skilled in the art should be aware of this 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 the navigation control instruction of the picking robot in the pepper field, acquiring 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 in 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 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, Silan S2 LiDAR, WHEELTEC N200 IMU and WHEELTEC DETA100 GNSS are selected, and the Extended Kalman Filter (EKF) algorithm running in the ROS unmanned system architecture is used to perform a series of fusions on the 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 and posture of the picking robot according to the global navigation satellite system data, the inertial measurement unit data, and the lidar data;

[0084] After acquiring the global navigation satellite system data, the inertial measurement unit data and the laser radar data in the picking robot, an extended Kalman filter and a Monte Carlo positioning algorithm are used to calculate and determine the current position and posture of the picking robot according to the global navigation satellite system data, the inertial measurement unit data and the laser radar data;

[0085] In the multi-sensor fusion process, LiDAR is mainly responsible for capturing the structural features of the external environment and accurately matching it with the real map, while the Inertial Measurement Unit (IMU) provides attitude data and continuous data on the robot's speed and tilt angle in all directions. The Global Navigation Satellite System (GNSS) is mainly responsible for assisting in providing macroscopic location information. The data of these three sensors are fused through the extended Kalman filter, combined with the Adaptive Monte Carlo Localization (AMCL) algorithm, and the high precision of LiDAR, the global positioning capability of GNSS and the short-term stability of IMU are comprehensively utilized to obtain more accurate and reliable positioning results. Accurate pose estimation and updating can not only improve the accuracy of positioning, but also maintain the continuity and accuracy of positioning through the information of the Inertial Measurement Unit (IMU) and LiDAR when the GPS signal is weak or lost.

[0086] When performing multi-sensor fusion, each sensor runs 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 position of the robot in the map when there is no GNSS signal and GNSS is received, the master node also needs to subscribe to the filtered odometer topic published by the extended Kalman filter (EKF) node to obtain the initial position of the picking robot, and form a data loop through mutual subscription to improve accuracy and reduce errors.

[0088] Specifically, in order to accurately locate the robot in the absence of GNSS signals, some optimization measures were taken in the study. The master node needs to subscribe to the filtered odometer topic published by the EKF (Extended Kalman Filter) node. This means that the master node will receive the odometer 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 odometer information in real time, which can improve the robot's position accuracy. In the absence of GNSS signals, the robot can still rely on odometer 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 implement multi-sensor data including: lidar, GNSS global satellite positioning module, IMU inertial navigation unit, and obtain the accurate position and posture of the picking robot in real time by fusing the data of these three.

[0090] Specifically, see Figure 4 The extended Kalman filter (EKF) is a nonlinear extension model 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 locally treating it as a linear model.

[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 transfer 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 transfer model;

[0103] is the linearized observation matrix;

[0104] In the update step, the Kalman gain is estimated through the linearized observation model, and the Kalman gain is 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 position, picking starting point and picking end point of the picking robot;

[0111] After calculating and determining the current posture of the picking robot according to the global navigation satellite system data, the inertial measurement unit data and the lidar data, the extended Kalman filter and the Monte Carlo positioning algorithm are used, and the picking starting point and the picking end point of the picking robot are determined. The A* algorithm is used 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;

[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, the closed list and the node attributes, wherein the open list is used to store the nodes to be evaluated, the closed list is used to store the evaluated nodes, and the node attributes include g(n), h(n) and f(n), wherein 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, and 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 of initializing the path mainly involve two key functions: a heuristic function h(n) and a path cost function g(n), which 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) The cost from the node to the target node will not be overestimated;

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

[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 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 target and a cost function, wherein the path optimization target includes ensuring that the trajectory of the picking robot is smooth, 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 end point, and 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 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.

[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 the 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 are 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 effect of 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 A schematic diagram of a bar chart of the number of collisions in each experimental scheme in the embodiments of the present application; Figure 7 A schematic diagram showing the comparison of the path length and average path length of each experimental scheme in the embodiments of the present application; Figure 8 A schematic diagram showing the comparison of the path time and the average path time of each experimental scheme in the embodiments of the present application; Fig. 9 Schematic diagram of the comparison of the path deviation and average path deviation of each experimental scheme 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, and the collision positions 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 collisions in the pepper field.

[0144] See also Figures 7 to 9 Through data comparison, it can be seen that the navigation system based on multi-sensor data fusion and optimized TEB algorithm in this application shortens the average length of the planned path by 0.58m, shortens the average time from the starting point to the target position by 2.55s, and reduces the average offset of the target position by 4.3cm. From the above line graph, it can be concluded that compared with the first three schemes, the ups and downs of the line of Scheme 4 are smaller, indicating that the data fluctuations of multiple experiments are smaller, which improves the stability of the overall navigation.

[0145] As can be seen from the above embodiments, compared with the prior art, the present application aims at the problem that the pepper field in the prior art is relatively bumpy, and the picking robot is prone to inaccurate positioning and positioning drift when autonomously navigating in a complex field environment. The present application includes 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, accurately calculate the current position of the harvesting robot, and achieve 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 needs of various picking tasks. The initial picking path is determined by the A* algorithm, and the initial picking path is optimized by the TEB algorithm, which can effectively improve the robot's operating efficiency and safety.

[0148] See also Fig.10A pepper field navigation control device provided to meet one of the purposes of the present 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 the Monte Carlo positioning algorithm to calculate and determine the current posture of the picking robot according to 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 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; 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 Fig.11 Another embodiment of the present application further provides an electronic device, which can be implemented by a computer device, such as Fig.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 can 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 can 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 a terminal. Those skilled in the art can understand that Fig.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 those shown in the figure, or combine certain components, or have a different arrangement of components.

[0150] In this embodiment, the processor is used to execute Fig.10The memory stores the program code and various data required to execute the above modules or submodules. 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 pepper field navigation control device of the present application, and the server can call the program code and data of the server 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 can understand 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, and the computer program can be stored in a computer-readable storage medium. When the program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, the aforementioned storage medium can be a computer-readable storage medium such as a disk, an optical disk, a read-only memory (ROM), or a random access memory (RAM).

[0154] The above description is only a partial implementation method 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: In response to navigation control instructions of the harvesting robot in the 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 and posture of the picking robot according to 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 an initial picking path of the picking robot according to the current position, picking starting point and picking end point 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, 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 step of using an extended Kalman filter and a Monte Carlo positioning algorithm to calculate and determine the current position and posture of the picking robot according to the global navigation satellite system data, the inertial measurement unit data and the laser radar data includes: The expression of the nonlinear system model of the extended Kalman filter is: x(k+1)=f(X(k))+ω(k), Z(k)=h(X(k))+v(k), The process noise of the nonlinear system model is expressed as: ω(k)~N(0,Q(k)), The observation noise of the nonlinear system model is expressed as: v(k)~N(0,R(k)), In the prediction step, the extended Kalman filter uses a nonlinear state transfer model to predict the next state variable and predicts the covariance, which is expressed as: X(k+1|k)=f(X(k)), (k+1|k)=h(X(k+1|k)), P(k+1|k)=F(k+1)P(k)F(k+1)′+Q(k+1), where, is the Jacobian matrix of the nonlinear state transfer model; is the linearized observation matrix; In the update step, the Kalman gain is estimated through the linearized observation model, and the Kalman gain is used to update the state estimate and its covariance. Its expression includes: 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 , X(k+1)=X(k+1|k)+K(k+1)[Z(k+1)-Z(k+1|k)], P(k+1)=[IK(k+1)H(k+1)]P(k+1|k), The extended Kalman filter provides a near-optimal estimate of the state in a nonlinear dynamic system through a prediction step and an update step; 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.

3. The pepper field navigation control method according to claim 1, characterized in that: The steps of 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 include: Initialize the open list, closed list and node attributes, wherein the open list is used to store the nodes to be evaluated, the closed list is used to store the evaluated nodes, and the node attributes include g(n), h(n) and f(n), wherein 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 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; 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.

4. The pepper field navigation control method according to claim 1, 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 a path optimization target and a cost function, wherein the path optimization target includes ensuring that the trajectory of the picking robot is smooth, 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 end point, and 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.

5. The pepper field navigation control method according to claim 4, 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.

6. The pepper field navigation control method according to any one of claims 1 to 5, 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: Acquire the optimal picking path of the picking robot, and discretize into various picking key points according to the optimal picking path; 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.

7. The pepper field navigation control method according to any one of claims 1 to 5, 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.

8. A chili 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 in the harvesting robot; A current posture determination module is configured to use an extended Kalman filter and a Monte Carlo positioning algorithm to calculate and determine the current posture of the picking robot according to the global navigation satellite system data, the inertial measurement unit data and the laser radar data; An initial path determination module is 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 according to the current position, picking starting point and picking end point of the picking robot using an A* algorithm; 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.

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

10. 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 7 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