Indoor positioning method and device for agricultural robot, electronic device and storage medium

Abstract

The application relates to an indoor positioning method and device of an agricultural robot, electronic equipment and a storage medium, wherein the method comprises the following steps: acquiring encoder data, inertial measurement unit (IMU) data, radar data and video data of the robot; obtaining mileage measurement data according to the encoder data and the IMU data; obtaining prior pose data of each particle at a current time according to the mileage measurement data and a preset robot kinematics model; calculating an actual estimation value of the weight of each particle according to the radar data and the video data; taking the average pose of a particle family with the highest weight obtained according to the prior pose data and the actual estimation value of the weight of each particle as a pose estimation value of the robot; and obtaining indoor positioning of the robot according to the pose estimation value of the robot. Thus, the problem that a robot is difficult to globally position in an indoor environment with similar scenes is solved, and fast repositioning of the robot in an indoor environment with a map is realized.

Description

Technical Field

[0001] This application relates to the field of robot positioning technology, and in particular to an indoor positioning method, device, electronic device and storage medium for agricultural robots. Background Technology

[0002] With the development of agricultural automation, agricultural robots have become important productivity tools in agricultural production. In order to improve the automation level of agricultural robots, users need to solve the problem of autonomous driving of robots in agricultural machinery depots, and a basic condition for autonomous driving is the robot's autonomous localization.

[0003] Common positioning systems in related technologies include: inertial navigation systems, radio beacons, QR code beacons, visual odometry, GPS (Global Positioning System), and multi-sensor fusion technology.

[0004] However, the aforementioned systems often suffer from problems such as susceptibility to interference during operation, leading to inaccurate positioning, or insufficient positioning capability when using a single system, which urgently need to be addressed. Summary of the Invention

[0005] This application provides an indoor positioning method, device, electronic device, and storage medium for agricultural robots to solve the problem of global positioning difficulties for robots in indoor environments with similar scenarios, and to achieve rapid repositioning of robots indoors under map conditions.

[0006] To achieve the above objectives, the first aspect of this application proposes an indoor positioning method for an agricultural robot, comprising the following steps:

[0007] Acquire encoder data, inertial measurement unit (IMU) data, radar data, and video data from the robot;

[0008] Odometry data is obtained based on the encoder data and the IMU data, and prior pose data for each particle at the current moment is obtained based on the odometry data and a preset robot kinematics model; and

[0009] The actual estimated value of the weight of each particle is calculated based on the radar data and the video data. The average pose of the group of particles with the highest weight is obtained based on the prior pose data and the actual estimated value of the weight of each particle. The average pose of the group of particles with the highest weight is used as the pose estimate of the robot. The indoor positioning of the robot is obtained based on the pose estimate of the robot.

[0010] According to one embodiment of this application, the average pose of the group of particles with the highest weights is obtained based on the prior pose data and the actual estimated value of the weights of each particle, including:

[0011] Based on the actual estimated value of the weight of each particle, identify particles to be removed whose actual estimated weight is less than a first preset threshold.

[0012] The particles to be removed are eliminated, and target particles that meet the preset dispersion conditions are dispersed within the preset range of the remaining particles. After a preset time, the average pose of the group of particles with the highest weight is obtained based on the prior pose data, the dispersion results of the target particles, and the actual estimated value of the weights of the remaining particles.

[0013] According to one embodiment of this application, calculating the actual estimated value of the weight of each particle based on the radar data and the video data includes:

[0014] Based on a preset weight estimation formula, the actual estimated weight of each particle is calculated according to the radar data and the video data, wherein the preset weight estimation formula is:

[0015] W = W L ×W C ;

[0016] Where W is the actual estimated value of the particle weight; W L The particle weights calculated using the radar data; W C The particle weights are calculated using the video data.

[0017] According to one embodiment of this application, the particle weights estimated from the video data are:

[0018] W C =W Cd (d)×W Cα (α);

[0019]

[0020] According to one embodiment of this application, the encoder data includes first encoder read data and second encoder read data, and before obtaining the mileage measurement data based on the encoder data and the IMU data, it further includes:

[0021] The encoder data is obtained by averaging the data read from the first encoder and the data read from the second encoder.

[0022] According to one embodiment of this application, the above-described indoor positioning method for agricultural robots further includes:

[0023] Based on the QR code beacon corresponding to the video data, the location of the video data in the map coordinate system is obtained from the beacon number table.

[0024] The indoor localization method for agricultural robots proposed in this application involves obtaining odometer data based on the robot's encoder and IMU data. This odometer data is then used to estimate the prior pose data of each particle at its current moment using a preset robot kinematic model. The actual estimated weight of each particle is calculated based on radar and video data. The average pose of the group of particles with the highest weight, obtained from the prior pose data and the actual estimated weight of each particle, is used as the robot's pose estimate. The indoor localization of the robot is then obtained based on this pose estimate. This solves the problem of difficult global localization of robots in indoor environments with similar scenes, enabling rapid relocalization of the robot indoors under map-based conditions.

[0025] To achieve the above objectives, a second aspect of this application provides an indoor positioning device for an agricultural robot, comprising:

[0026] The acquisition module is used to acquire encoder data, inertial measurement unit (IMU) data, radar data, and video data of the robot.

[0027] The results module is used to obtain odometer data based on the encoder data and the IMU data, and to obtain the prior pose data of each particle at the current moment based on the odometer data and a preset robot kinematics model; and

[0028] The positioning module is used to calculate the actual estimated value of the weight of each particle based on the radar data and the video data, and to obtain the average pose of the group of particles with the highest weight based on the prior pose data and the actual estimated value of the weight of each particle, and to use the average pose of the group of particles with the highest weight as the pose estimate of the robot, and to obtain the indoor positioning of the robot based on the pose estimate of the robot.

[0029] According to one embodiment of this application, the positioning module is specifically used for:

[0030] Based on the actual estimated value of the weight of each particle, identify particles to be removed whose actual estimated weight is less than a first preset threshold.

[0031] The particles to be removed are eliminated, and target particles that meet the preset dispersion conditions are dispersed within the preset range of the remaining particles. After a preset time, the average pose of the group of particles with the highest weight is obtained based on the prior pose data, the dispersion results of the target particles, and the actual estimated value of the weights of the remaining particles.

[0032] According to one embodiment of this application, the positioning module is specifically used for:

[0033] Based on a preset weight estimation formula, the actual estimated weight of each particle is calculated according to the radar data and the video data, wherein the preset weight estimation formula is:

[0034] W = W L ×W C ;

[0035] Where W is the actual estimated value of the particle weight; W L The particle weights calculated using the radar data; W C The particle weights are calculated using the video data.

[0036] According to one embodiment of this application, the particle weights estimated from the video data are:

[0037] W C =W Cd (d)×W Cα (α);

[0038] in,

[0039] According to one embodiment of this application, the encoder data includes first encoder read data and second encoder read data. Before obtaining the mileage measurement data based on the encoder data and the IMU data, the result module is further configured to:

[0040] The encoder data is obtained by averaging the data read from the first encoder and the data read from the second encoder.

[0041] According to one embodiment of this application, the above-mentioned indoor positioning device for agricultural robots further includes:

[0042] The lookup module is used to find the location of the video data in the map coordinate system from the beacon number table based on the QR code beacon corresponding to the video data.

[0043] The indoor positioning device for agricultural robots proposed in this application obtains odometer data based on the robot's encoder data and IMU data. This odometer data is then used to estimate the prior pose data of each particle at the current moment using a preset robot kinematic model. The actual estimated weight of each particle is calculated based on radar data and video data. The average pose of the group of particles with the highest weight, obtained from the prior pose data and the actual estimated weight of each particle, is used as the robot's pose estimate. The indoor positioning of the robot is then obtained based on this pose estimate. This solves the problem of difficult global positioning of robots in indoor environments with similar scenes, enabling rapid relocalization of the robot indoors under map-based conditions.

[0044] To achieve the above objectives, a third aspect of this application provides an electronic device, including: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the indoor positioning method for an agricultural robot as described in the above embodiments.

[0045] To achieve the above objectives, a fourth aspect of this application provides a computer-readable storage medium having a computer program stored thereon, which is executed by a processor to implement the indoor positioning method for an agricultural robot as described in the above embodiments.

[0046] Additional aspects and advantages of this application will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of this application. Attached Figure Description

[0047] The above and / or additional aspects and advantages of this application will become apparent and readily understood from the following description of the embodiments taken in conjunction with the accompanying drawings, wherein:

[0048] Figure 1 This is a schematic diagram illustrating the operating results of ORB-SLAM in a normal working environment according to an embodiment of this application;

[0049] Figure 2 This is a schematic diagram of a similar corridor environment simulation experiment according to an embodiment of this application;

[0050] Figure 3 This is a schematic diagram of particle distribution during the operation of an indoor robot using only radar data according to an embodiment of this application;

[0051] Figure 4 This is a flowchart of an indoor positioning method for an agricultural robot according to an embodiment of this application;

[0052] Figure 5This is a schematic diagram of the composition of an indoor positioning system for a robot according to an embodiment of this application;

[0053] Figure 6 This is a schematic diagram of a steering model according to an embodiment of this application;

[0054] Figure 7 This is a schematic diagram illustrating the location distribution estimated from video data according to one embodiment of this application;

[0055] Figure 8 This is a schematic diagram of the yaw angle distribution estimated from video data according to one embodiment of this application;

[0056] Figure 9 This is a schematic diagram of the particle weight as a function of distance according to an embodiment of this application;

[0057] Figure 10 This is a schematic diagram of a QR code beacon according to one embodiment of this application;

[0058] Figure 11 This is a schematic diagram illustrating the spatial relationship between a map, a camera, and a body coordinate system according to an embodiment of this application;

[0059] Figure 12 This is a schematic diagram showing the pose distribution of a robot in a map according to an embodiment of this application;

[0060] Figure 13 This is a schematic diagram of the estimated pose change curve when using a camera according to an embodiment of this application;

[0061] Figure 14 This is a schematic diagram of an estimated pose change curve when using a camera, according to one embodiment of this application;

[0062] Figure 15 This is a block diagram of an indoor positioning device for an agricultural robot according to an embodiment of this application;

[0063] Figure 16 This is a schematic diagram of the structure of an electronic device according to an embodiment of this application. Detailed Implementation

[0064] The embodiments of this application are described in detail below. Examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and intended to explain this application, and should not be construed as limiting this application.

[0065] The following describes, with reference to the accompanying drawings, an indoor positioning method, apparatus, electronic device, and storage medium for agricultural robots according to embodiments of this application. First, the indoor positioning method for agricultural robots according to embodiments of this application will be described with reference to the accompanying drawings.

[0066] Before introducing the indoor positioning method for agricultural robots proposed in the embodiments of this application, let's briefly introduce some common positioning systems in related technologies:

[0067] (1) An inertial navigation system is a system that uses inertial measurement elements such as accelerometers, gyroscopes, encoders or velocity measurement elements to measure the displacement of a robot relative to its initial point.

[0068] (2) RFID (Radio Frequency Identification, wireless communication technology), WIFI (Wireless Fidelity, mobile hotspot), Bluetooth, UWB (Ultra Wide Band, ultra-wideband) and other radio technologies are widely used for positioning. These technologies usually require the placement of multiple beacons that transmit or receive radio signals in space. The robot determines the distance between itself and the beacons based on the strength of the beacon signal and the communication time with the beacons. Then, the robot's position is calculated based on the distance between the robot and multiple beacons and the distribution of the beacons on the map.

[0069] (3) QR code beacons have the advantages of being free from electromagnetic interference and low cost compared to beacons that use electromagnetic signals. Only one beacon is needed for positioning. They can be sprayed or pasted on the location that needs to be marked, making them easy to deploy. The robot can estimate the positional relationship between itself and the beacon based on the QR code beacon image captured by the camera, and then determine its own position on the map based on the beacon's posting location.

[0070] (4) Visual odometry mainly relies on vision to determine the robot's pose in the environment. The core idea of ​​visual odometry is to obtain the camera's displacement in the interval by comparing the differences between two adjacent frames, thereby obtaining the camera's pose.

[0071] (5) GPS is the most commonly used positioning technology for outdoor positioning. It determines the location of the receiver by the distance information between the receiver and multiple satellites.

[0072] (6) LiDAR (Light Detection and Ranging) uses lasers to scan the surrounding environment, determines the position of the radar and the surrounding environment by measuring the laser reflection time, and uses particle filtering and other methods to determine its own position on the map. LiDAR is widely used in robots such as robotic vacuum cleaners, and its measurement accuracy is usually high.

[0073] (7) Multi-sensor fusion technology integrates and applies measurement data from multiple sensors through algorithms to leverage the advantages of multiple sensors. For example, the fusion of LiDAR and GPS can help robots achieve high-precision positioning in locations with limited GPS signals, which is common in JD.com's outdoor logistics vehicles and various autonomous driving systems; the fusion of cameras and LiDAR can allow robots to obtain data with both depth and texture information, which is widely used in some special scenarios such as exploration and intersection driving.

[0074] However, the existing problems with the aforementioned common positioning systems are as follows:

[0075] (1) Due to the existence of cumulative error, the inertial navigation system is greatly affected by measurement error, and the measured value is the position relative to the initial point rather than the absolute position on the map.

[0076] (2) Radio beacons are usually used for indoor positioning. They are easily interfered with by electromagnetic waves, obstacles, etc., and can only be located within the range of the beacon signal coverage.

[0077] (3) QR code beacons can only be used for positioning when the beacon is in the field of view and the image is clear, so they cannot usually be used alone.

[0078] (4) Visual odometry will fail when there is little texture information in the environment. For example, in an office building corridor, the field of vision may be full of white walls, which will cause the visual odometry to malfunction.

[0079] This application conducted experiments on two commonly used visual odometry methods: SVO (Fast Semi-Direct Monocular Visual Odometry) and ORB-SLAM (Oriented FAST and Rotated BRIEF-Simultaneous Localization and Mapping). The SVO algorithm determines the camera's displacement by identifying feature points in the image and comparing the displacement of these feature points. ORB-SLAM, on the other hand, creates a local point cloud map based on the identified feature points.

[0080] Specifically, simulations and physical experiments with SVO revealed that SVO is more suitable for positioning when an aircraft's camera is capturing images of the ground. By comparing two adjacent frames, SVO can obtain the camera's translation on a plane parallel to the camera's image plane, its translation along the normal vector of this plane, and its rotation about the normal vector of this plane—that is, the translation and yaw of the aircraft in three directions when the camera is pointing downwards. However, for a robot moving with its camera facing the direction of the vehicle's front, its rotation axis is a vector parallel to the camera's image plane. Feature points are easily lost during the robot's turning process. Since SVO only compares adjacent images, it loses its positioning capability after the robot rotates.

[0081] ORB-SLAM creates a point cloud map while simulating localization, preserving historical feature points from the robot's movement. This allows it to maintain good localization even during robot rotation. However, research shows that ORB-SLAM's successful localization requires a sufficient number of feature points in the environment, such as an office environment with many desks and chairs. In a real-world corridor environment, images captured by cameras on the robot's chassis mostly show walls and the ground, with very few feature points, insufficient for ORB-SLAM to complete localization and build a point cloud map. Figure 1 As shown in (a), the office environment images captured during the experiment where ORB-SLAM functioned normally are listed. Figure 1 (b) shows a real corridor environment image. The dots and lines shown in the image are the feature points identified by ORB-SLAM. When there are enough feature points, the algorithm will change from lines to dots. It can be seen that in the real corridor environment, apart from the edge features generated by shadows, there are few feature points, which are insufficient for the algorithm to execute normally.

[0082] (5) GPS cannot be used indoors, is easily affected by obstructions, and has low measurement accuracy.

[0083] (6) LiDAR is one of the most commonly used sensors for positioning in robots and autonomous driving. Since LiDAR is an active sensor with high accuracy, it can be used in most scenarios. However, LiDAR can only scan the surrounding obstacles and needs to be combined with prior estimates of its own position to achieve global positioning. Therefore, using only LiDAR data will result in insufficient global positioning capability. The insufficient global positioning capability is mainly manifested in two ways: First, when the robot is hijacked, that is, when the robot's prior pose deviates far from the true pose, it is difficult for the robot's estimated pose to converge to the vicinity of the true pose based solely on LiDAR data; Second, the tracking of the robot's estimated pose to the true pose is not ideal during the robot's movement.

[0084] Here, the convergence criterion is defined as the majority of particles concentrating within a circular region with a diameter of 1m. Aggregation refers to particles being scattered within a radius of approximately 1.5m. The specific reasons for the aforementioned problems differ in indoor and outdoor scenarios, as analyzed below:

[0085] In indoor scenes, there may be many similar environments, such as similar corridors and corners. When the prior pose is far from the true pose, the matching degree between the LiDAR data and the environment surrounding the current prior pose may be high, and particles may converge at incorrect positions. This application conducts experiments and verifications on a mobile robot platform, selecting, for example... Figure 2 The corridor environment shown was used in a simulation experiment. The particle distribution during the robot's operation is as follows: Figure 3 As shown, Figure 3 (a), (b), and (c) represent the particle distribution diagrams of the robot in its initial state, after 20 seconds of operation, and after 2 minutes and 30 seconds of operation, respectively. The center of the region where the particles in pose 1 (used for localization) are concentrated represents the robot's estimate of its own position. The robot's actual pose in the experiment was... Figure 3 The particle initial aggregation region shown in the 2-pose diagram is near the 1-pose. The robot performs back-and-forth movements near the 2-pose to acquire more map information. The actual environment detected by the radar is a "T"-shaped corridor, which is somewhat similar to the long straight corridor in the 1-pose diagram. Moreover, the initial pose of most particles is in the long straight corridor, so the particles converge to the 1-pose. It can be seen that the particles do not tend to relocate to the 2-pose. Repeating the above experiment 5 times yielded the same result.

[0086] In addition, indoor scenes have a small environment and many reference objects in the map, so the radar obtains more information. Therefore, after the radar converges to the vicinity of the true pose, the robot can follow the true pose during its movement. However, the radar only obtains two-dimensional information. When the true pose is in the particle aggregation area, the estimated pose can converge to the true pose, but the speed is relatively slow.

[0087] Based on the aforementioned problems, this application proposes an indoor localization method for agricultural robots. This method obtains odometer data from the robot's encoder and IMU data, and estimates the prior pose data of each particle at the current moment using the odometer data and a preset robot kinematics model. The actual estimated weight of each particle is calculated based on radar and video data. The average pose of the group of particles with the highest weight, obtained from the prior pose data and the actual estimated weight of each particle, is used as the robot's pose estimate. The indoor localization of the robot is then obtained based on this pose estimate. This solves the problem of difficult global localization of robots in indoor environments with similar scenes, and enables rapid relocalization of the robot indoors under map-based conditions.

[0088] Figure 4 This is a flowchart of an indoor positioning method for an agricultural robot according to an embodiment of this application.

[0089] In this embodiment, four sensors are selected: encoder, inertial measurement unit (IMU), lidar, and camera. Sensor data fusion is performed based on the ROS (Robot Operating System), Apriltags QR code beacons, extended Kalman filter algorithm, and AMCL (Adaptive Monte Carlo Localization) algorithm. The system composition is as follows: Figure 5 As shown.

[0090] Specifically, Figure 5 The indoor positioning system for the robot in this embodiment mainly includes a particle filter and various sensors. An extended Kalman filter (EKF) is used to fuse encoder data and IMU data to obtain odometer data, which is used to estimate the robot's pose (position and attitude, i.e., two-dimensional position and heading) relative to the initial point. The particle filter uses an adaptive Monte Carlo algorithm to implement particle filtering. Its core idea is to scatter some particles with pose information in the map coordinate system. Based on the odometer data and the preset robot kinematic model, the prior pose data of each particle at the current moment is estimated. Then, based on the data of other sensors and the model, the weight of each particle is calculated, particles with low weights are deleted, and more particles are scattered near particles with high weights. In this way, after a period of time, all particles will gather near the actual pose.

[0091] The adaptive nature of the AMCL algorithm is reflected in the following aspects: when the particle dispersion range is wide, more target particles can be dispersed, while after the particles begin to concentrate, only fewer target particles need to be dispersed to improve the running speed; on the other hand, when most of the particles have gathered near a certain location, AMCL will still place some target particles in other locations on the map, so that relocation can be achieved when the robot suddenly changes position for some reason (robot kidnapping).

[0092] It should be noted that the basis for determining whether relocation is needed is the long-term likelihood and short-term likelihood functions. The long-term likelihood function is the average value of the particle weights over a longer period of time, while the short-term likelihood is the average value of the particle weights over a shorter period of time. When the robot is running normally, the long-term likelihood and the short-term likelihood are roughly equal. However, when the robot is abducted to another location, the short-term likelihood suddenly decreases and becomes much smaller than the long-term likelihood, indicating that the robot needs to be relocated.

[0093] The algorithm uses the average pose of the group of particles with the highest weight as the robot's pose estimate.

[0094] Furthermore, embodiments of this application also establish a map coordinate system, a body coordinate system, an odometer coordinate system, and a sensor coordinate system.

[0095] Among them, the map coordinate system (also known as the map coordinate system) is the coordinate system of the environment, z m The axis is perpendicular to the ground and pointing upwards.

[0096] The body coordinate system (i.e., the base_link coordinate system) is a non-inertial system fixed to the robot chassis, with its origin O. b Let x be the projection of the geometric center of the robot chassis onto the ground. b The axis points towards the front of the car, z b The axis is perpendicular to the chassis and points upwards, y b The axis is perpendicular to x b -O b -z b The plane points to the left. Robot localization essentially involves processing data from various sensors to determine the robot's relative position within the map's coordinate system.

[0097] The odometry coordinate system (ODOM) was introduced to determine the robot's pose on the map, serving as a bridge between the body coordinate system and the map coordinate system. The origin O of the ODOM coordinate system is... o This is the position x when the mobile robot begins to move. o The axis is the same as the body coordinate system in the initial state. b y-axis coincides o The axis and the body coordinate system y in the initial state b Axis coincidence.

[0098] The sensor coordinate system is the coordinate system of each sensor itself. The raw position and attitude information returned by the sensor is in the sensor's own coordinate system. The sensor coordinate systems involved in this application include the IMU coordinate system, radar coordinate system, GPS coordinate system, and camera coordinate system. The raw data obtained by the encoder is only the distance the wheel has traveled, and it is bound to the body coordinate system. Therefore, there is no separate encoder coordinate system.

[0099] like Figure 4 As shown, the indoor positioning method for this agricultural robot includes the following steps:

[0100] In step S401, the robot's encoder data, inertial measurement unit (IMU) data, radar data, and video data are acquired.

[0101] The encoder data, inertial measurement unit (IMU) data, radar data, and video data are obtained from four types of sensors: encoder, IMU, lidar, and camera, respectively. This application embodiment can use relevant technical means to obtain the data from these four sensors. To avoid redundancy, it will not be described in detail here.

[0102] Specifically, the inertial measurement unit integrates an accelerometer and a gyroscope to measure three-axis attitude angles (or angular rates) and acceleration; the lidar is an active sensor that measures the depth information of the surrounding environment by emitting lasers and receiving reflected signals. When working, the lidar rotates 360 degrees and returns a set of polar coordinate data according to the set rotation speed and angular resolution, representing the depth information in several directions in a two-dimensional plane under the lidar coordinate system. The lidar rotates continuously, collecting information about the surrounding environment in real time, and can send one frame of data to the embedded computing platform every time it rotates once; the monocular camera sensor is mainly used to capture two-dimensional images and use video data to assist the robot in positioning.

[0103] In step S402, odometer data is obtained based on encoder data and IMU data, and prior pose data of each particle at the current moment is obtained based on odometer data and a preset robot kinematics model.

[0104] Furthermore, in some embodiments, the encoder data includes first encoder read data and second encoder read data, and before obtaining odometer measurement data based on the encoder data and IMU data, the method further includes obtaining encoder data based on the average of the first encoder read data and the second encoder read data.

[0105] Specifically, this embodiment uses dual encoders, meaning encoders are installed at both rear wheels of the robot to measure the mileage per unit time for each rear wheel. Therefore, the average value of the data read by the two encoders is taken as the observed value of the actual mileage (i.e., the final encoder data). Let the mileage data read by the left encoder be d1 and the mileage data read by the right encoder be d2, then the increase in robot mileage is:

[0106]

[0107] Furthermore, in the embodiments of this application, the robot can be considered to have the same center of rotation for all four wheels when turning. Let the turning radius of the outer rear wheel be r2, the odometer data be d2, the turning radius of the inner rear wheel be r1, the odometer data be d1, and the angle the robot turns be ψ. Figure 6 As shown, we have:

[0108]

[0109] We can solve for:

[0110]

[0111] Let the sampling period be Δt, and the measured values ​​of the robot's velocity and angular velocity be:

[0112]

[0113]

[0114] In step S403, the actual estimated value of the weight of each particle is calculated based on radar data and video data, and the average pose of the group of particles with the highest weight is obtained based on the prior pose data and the actual estimated value of the weight of each particle. The average pose of the group of particles with the highest weight is used as the pose estimate of the robot, and the indoor positioning of the robot is obtained based on the pose estimate of the robot.

[0115] It is understood that, in the embodiments of this application, the actual estimated value of the weight of each particle can be calculated based on radar data and video data, and then the average pose of the group of particles with the highest weight obtained based on prior pose data and the actual estimated value of the weight of each particle can be used as the pose estimate of the robot, thereby obtaining the indoor positioning of the robot.

[0116] Further, in some embodiments, the average pose of the group of particles with the highest weight is obtained based on prior pose data and the actual estimated value of the weight of each particle, including: identifying particles to be removed whose actual estimated weight is less than a first preset threshold based on the actual estimated value of the weight of each particle; removing the particles to be removed, and dispersing target particles that meet preset dispersion conditions within a preset range of the remaining particles, and obtaining the average pose of the group of particles with the highest weight after a preset time period based on prior pose data, the dispersion result of the target particles and the actual estimated value of the weight of the remaining particles.

[0117] Specifically, in this embodiment, the actual estimated value of the weight of each particle can be calculated based on radar data and video data. Particles whose actual estimated weight is less than a first preset threshold are identified as particles to be eliminated. Particles with lower weights are eliminated, and target particles that meet preset dispersion conditions are dispersed within a preset range of the remaining particles with higher weights. After a period of time, all particles will gather near the actual pose, thereby obtaining the average pose of the group of particles with the highest weight based on the prior pose data, the dispersion result of the target particles, and the actual estimated value of the weight of the remaining particles.

[0118] Furthermore, in some embodiments, calculating the actual estimated value of the weight of each particle based on radar data and video data includes: calculating the actual estimated value of the weight of each particle based on radar data and video data according to a preset weight estimation formula, wherein the preset weight estimation formula is:

[0119] W = W L ×W C ;

[0120] Where W is the actual estimated value of the particle weight, W L To calculate the particle weights using radar data, W C The particle weights are calculated using video data.

[0121] Specifically, this application embodiment uses the likelihood domain model of an existing LiDAR sensor model. Based on the distance between the nearest obstacle to the laser beam endpoint on the map and the laser beam endpoint, the probability of the laser beam is estimated. Finally, the probabilities of all laser beams are summed as the weight of the corresponding particle. In essence, it compares the contour of the surrounding environment scanned by the laser with the contour of the surrounding environment of the particle's prior pose on the map. If the matching degree is high, the particle weight is high.

[0122] In some embodiments, the particle weights estimated from the video data are:

[0123] W C =W Cd (d)×W Cα (α);

[0124] in,

[0125] Specifically, the task of the camera sensor model is to determine W. C The functional relationship between the estimated pose from video data and the prior particle pose. In this embodiment, W... C It is divided into two parts, one of which is the function W of the positional deviation. Cd W is a function of angular deviation. Cα Since the robot's position is homogeneous in different directions on the two-dimensional plane, the position deviation function takes the distance d between the estimated position and the prior position as the independent variable, and the angle deviation function takes the angle difference α between the estimated position and the prior position as the independent variable.

[0126] This application embodiment records the robot's pose estimated from video data at a fixed point, and the results are as follows: Figure 7 and Figure 8 As shown, where, Figure 7 (a) is a data distribution diagram used to estimate the distribution of pose in a two-dimensional plane. Figure 7 (b) is a frequency distribution plot, used to show the frequency of a certain estimated pose in all data. Figure 8 This is a frequency-yaw angle distribution diagram.

[0127] From this, it can be seen that the pose estimated from the video data basically follows a uniform distribution within a distance deviation of 0.03 m and an angular deviation of 0.02 radians from the true pose.

[0128] If we take:

[0129] W Cd (d) = 1 when d ≤ 0.03 m;

[0130] It can be found that in order to ensure the normal operation of the particle filter algorithm, the weights of particles whose prior positions are more than 1 m away from the position estimated by the camera cannot be too small. Otherwise, due to factors such as calculation accuracy, when the initial aggregation range of particles does not include the true position, the weights of all particles in the algorithm are near zero, and the algorithm cannot calculate the true pose based on the particle weights. According to experimental measurements, increasing the weight by 0.05 within a range of more than 0.03 m can ensure the normal operation of the algorithm and has no significant impact on the convergence of particles. In addition, there are certain errors in the estimation of the QR code beacon in the map during the experiment, so the distance value of the function breakpoint is taken as 0.1 m.

[0131] At this time:

[0132]

[0133] However, since there is no difference in the weights between particles with d > 0.1 m, that is, it is impossible to distinguish the distance of the particle prior position from the camera estimate. When the initial aggregation area of particles does not contain the position estimated by the camera, the particle weights are only multiplied by the same coefficient and still cannot converge to the true position estimated using the camera data. Therefore, a monotonically decreasing linear region is added within the range of 0.1 m < d ≤ d1, that is:

[0134]

[0135] Among them, if d1 is too small, the above problem cannot be solved. If d1 is too large, the linear decreasing segment is too gentle and cannot significantly distinguish the particle weights. According to experimental observations, when d1 = 3 m, the particles can converge effectively. At this time, the curve of the particle weight changing with distance is as Figure 9 shown.

[0136] The above problem also exists in the estimation of the angular deviation. Therefore, a normal distribution is selected to represent the angular deviation, and take:

[0137] W Cα (α) = 0.2 × f N (α) + 0.1N ∼ N(0, 0.2);

[0138] Among them, f N (α) represents the probability density of particles with an angular deviation of α. Taking the angular division value as 0.2 radians and multiplying gives the probability.

[0139] In this embodiment of the application, it was found in experiments that since the range of angle deviation α is -π to π, and the range of distance deviation d is 0 to positive infinity, which can reach more than 10m in the experimental environment, if the angle and distance deviations are to be estimated simultaneously, particles with larger position deviations will converge to the correct direction first. At this time, the algorithm will delete particles with correct positions but incorrect directions. Finally, all particles converge to the pose with correct directions but incorrect positions. Therefore, this embodiment of the application adopts a strategy of prioritizing position convergence. When the particle's prior pose distance d estimated from the camera data is ≤ 2m, the deviation is calculated based on the angle. Thus, the final value is:

[0140]

[0141] Furthermore, in some embodiments, the above-described indoor positioning method for agricultural robots further includes: finding the position of the video data in the map coordinate system from a beacon number table based on the QR code beacon corresponding to the video data.

[0142] Specifically, the embodiments of this application use camera recognition such as... Figure 10 The April tags QR code beacon method shown uses camera information to obtain the robot's pose. The number at the end of the text below the image is the information contained in the QR code, indicating the beacon's number. The pose of each beacon in the map coordinate system can be obtained by looking up the beacon number in a table. In a farm machinery hangar, the QR codes can be posted at the hangar entrance and on the walls of each parking space to assist the robot in positioning.

[0143] Furthermore, in this embodiment of the application, by calling the apriltag_ros algorithm package, a QR code can be identified from the image captured by the camera. Based on the beacon number and the camera intrinsic parameter matrix, the pose of the QR code fixed to the map relative to the camera is obtained using the PNP method. Based on the pose of the QR code in the map, the transformation relationship between the camera coordinate system and the map coordinate system can be determined. Then, based on the pose relationship between the camera and the robot, the pose of the robot in the map can be determined.

[0144] The spatial relationship between the map, camera, and aircraft coordinate system is as follows: Figure 11 As shown, the camera coordinate system is rotated relative to the body coordinate system. The x-axis and y-axis of the camera coordinate system represent the coordinates of the camera image. Let R1 be the 3D rotation matrix from the map coordinate system to the camera coordinate system, and t1 be the displacement vector. Let R0 be the rotation matrix from the camera coordinate system to the body coordinate system, and t0 be the displacement vector of the body coordinate system in the camera coordinate system. Let P be the position of a point in space in the camera coordinate system. c Its position in the map coordinate system is P. m Its position in the body coordinate system is P.b Then we have:

[0145] P c =R1P m +t1;

[0146] P b =R0(P c -t0);

[0147] Therefore, the position of the machine's coordinate system on the map is:

[0148]

[0149] The rotation matrix of the body coordinate system in the map coordinate system is:

[0150]

[0151] To help those skilled in the art further understand the indoor positioning method for agricultural robots proposed in the embodiments of this application, further explanation is provided below with reference to examples.

[0152] like Figure 12 As shown, Figure 12 The diagram shows the distribution of robot poses in a map according to an embodiment of this application. The initial particle aggregation pose is selected as pose 1. The robot is placed at three similar poses with different orientations (2, 3, and 4) and the indoor positioning experiment of the robot kidnapping problem is repeated. The experimental results of the actual pose not being within the particle aggregation range after fusing camera data are shown in Table 1.

[0153] Table 1

[0154]

[0155] In the table, “——” indicates that the particle did not converge and showed no tendency to converge in the experiment.

[0156] Figure 13 and Figure 14 The figures show the estimated pose changes when using a camera and when not using a camera in a certain experiment. The darker curve represents the pose estimated based on camera data, and the lighter curve represents the pose represented by particle clouds. Since the camera determines the pose by recognizing a QR code, it has high global positioning accuracy; therefore, the pose estimated based on camera data is taken as the true pose. Thus, after using camera data, the robot has relocalization capability in corridor scenes with many similar environments. The angle convergence capability is slightly weaker than the position convergence capability. The results also show that the robot's relocalization capability varies depending on the distance and orientation, with the worst case being that angle convergence is very difficult.

[0157] In addition, this application embodiment also conducted an experiment on the case where the robot's true pose is within the particle aggregation range during the robot's movement. The positioning results were compared between using only LiDAR data and using both LiDAR and camera data. In this experiment, the angle and position converged almost simultaneously, so they are not listed separately. The experimental results of the true pose within the particle aggregation range are shown in Table 2.

[0158] Table 2

[0159]

[0160]

[0161] As can be seen from the table, when the true pose is within the particle aggregation range, radar data alone can converge to the true pose in the corridor environment, but the speed is slower than the experimental results after fusing camera data, thus proving the advantages of data fusion.

[0162] In summary, the embodiments of this application extend the existing method of positioning using LiDAR by using a camera to recognize QR codes. For the problem that robots need to operate in both indoor and outdoor environments, a sensor fusion framework based on particle filtering is used, which facilitates the expansion of other sensors such as GPS.

[0163] It should be noted that the odometer part involved in the embodiments of this application can be replaced by any inertial component, the April tags QR code beacon can also be replaced by other QR codes or texture features, and the lidar can also be replaced by various types such as single-line and multi-line, and no unique limitation is made here.

[0164] The indoor localization method for agricultural robots proposed in this application involves obtaining odometer data based on the robot's encoder and IMU data. This odometer data is then used to estimate the prior pose data of each particle at its current moment using a preset robot kinematic model. The actual estimated weight of each particle is calculated based on radar and video data. The average pose of the group of particles with the highest weight, obtained from the prior pose data and the actual estimated weight of each particle, is used as the robot's pose estimate. The indoor localization of the robot is then obtained based on this pose estimate. This solves the problem of difficult global localization of robots in indoor environments with similar scenes, enabling rapid relocalization of the robot indoors under map-based conditions.

[0165] Next, referring to the accompanying drawings, an indoor positioning device for an agricultural robot according to an embodiment of this application is described.

[0166] Figure 15 This is a block diagram of an indoor positioning device for an agricultural robot according to an embodiment of this application.

[0167] like Figure 15 As shown, the indoor positioning device 10 of the agricultural robot includes: an acquisition module 100, a result module 200, and a positioning module 300.

[0168] The acquisition module 100 is used to acquire encoder data, inertial measurement unit (IMU) data, radar data, and video data of the robot.

[0169] The results module 200 is used to obtain odometer data based on encoder data and IMU data, and to obtain the prior pose data of each particle at the current moment based on the odometer data and a preset robot kinematics model; and

[0170] The positioning module 300 is used to calculate the actual estimated value of the weight of each particle based on radar data and video data, and to obtain the average pose of the group of particles with the highest weight based on the prior pose data and the actual estimated value of the weight of each particle. The average pose of the group of particles with the highest weight is used as the pose estimate of the robot, and the indoor positioning of the robot is obtained based on the pose estimate of the robot.

[0171] Furthermore, in some embodiments, the positioning module 300 is specifically used for:

[0172] Based on the actual estimated value of the weight of each particle, identify particles to be removed whose actual estimated weight is less than a first preset threshold.

[0173] Remove the particles to be removed, and distribute target particles that meet the preset distribution conditions within the preset range of the remaining particles. After a preset time, obtain the average pose of the group of particles with the highest weight based on the prior pose data, the distribution results of the target particles, and the actual estimated value of the weights of the remaining particles.

[0174] Furthermore, in some embodiments, the positioning module 300 is specifically used for:

[0175] Based on a preset weight estimation formula, the actual estimated weight of each particle is calculated using radar data and video data. The preset weight estimation formula is as follows:

[0176] W = W L ×W C ;

[0177] Where W is the actual estimated value of the particle weight; W L The particle weights calculated using radar data; W C The particle weights are calculated using video data.

[0178] Furthermore, in some embodiments, the particle weights estimated from the video data are:

[0179] WC =W Cd (d)×W Cα (α);

[0180] in,

[0181] Furthermore, in some embodiments, the encoder data includes first encoder read data and second encoder read data. Before obtaining odometer measurement data based on the encoder data and IMU data, the result module 200 is further configured to:

[0182] Encoder data is obtained by averaging the data read from the first encoder and the data read from the second encoder.

[0183] Furthermore, in some embodiments, the indoor positioning device 10 for the agricultural robot described above further includes:

[0184] The lookup module is used to find the location of the video data in the map coordinate system based on the QR code beacon corresponding to the video data from the beacon number table.

[0185] It should be noted that the foregoing explanation of the indoor positioning method embodiment for agricultural robots also applies to the indoor positioning device for agricultural robots in this embodiment, and will not be repeated here.

[0186] The indoor positioning device for agricultural robots proposed in this application obtains odometer data based on the robot's encoder data and IMU data. This odometer data is then used to estimate the prior pose data of each particle at the current moment using a preset robot kinematic model. The actual estimated weight of each particle is calculated based on radar data and video data. The average pose of the group of particles with the highest weight, obtained from the prior pose data and the actual estimated weight of each particle, is used as the robot's pose estimate. The robot's indoor positioning is then obtained based on this pose estimate. This solves the problem of difficult global positioning of robots in indoor environments with similar scenes, enabling rapid relocalization of the robot indoors under map-based conditions.

[0187] Figure 16 A schematic diagram of the structure of an electronic device provided in an embodiment of this application. The electronic device may include:

[0188] The memory 1601, the processor 1602, and the computer program stored on the memory 1601 and executable on the processor 1602.

[0189] When the processor 1602 executes the program, it implements the indoor positioning method for agricultural robots provided in the above embodiments.

[0190] Furthermore, electronic devices also include:

[0191] Communication interface 1603 is used for communication between memory 1601 and processor 1602.

[0192] The memory 1601 is used to store computer programs that can run on the processor 1602.

[0193] The memory 1601 may include high-speed RAM (Random Access Memory) memory, and may also include non-volatile memory, such as at least one disk storage.

[0194] If the memory 1601, processor 1602, and communication interface 1603 are implemented independently, then the communication interface 1603, memory 1601, and processor 1602 can be interconnected via a bus to complete communication between them. The bus can be an ISA (Industry Standard Architecture) bus, a PCI (Peripheral Component Interconnect) bus, or an EISA (Extended Industry Standard Architecture) bus, etc. The bus can be divided into address bus, data bus, control bus, etc. For ease of representation, Figure 16 The bus is represented by a single thick line, but this does not mean that there is only one bus or one type of bus.

[0195] Optionally, in a specific implementation, if the memory 1601, processor 1602, and communication interface 1603 are integrated on a single chip, then the memory 1601, processor 1602, and communication interface 1603 can communicate with each other through an internal interface.

[0196] The processor 1602 may be a CPU (Central Processing Unit), an ASIC (Application Specific Integrated Circuit), or one or more integrated circuits configured to implement the embodiments of this application.

[0197] This application also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the above-described indoor positioning method for agricultural robots.

[0198] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this application, "multiple" means at least two, such as two, three, etc., unless otherwise explicitly specified.

[0199] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of this application. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0200] Although embodiments of this application have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting this application. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of this application.

Claims

1. An indoor positioning method for an agricultural robot, characterized in that, Includes the following steps: Acquire encoder data, inertial measurement unit (IMU) data, radar data, and video data from the robot; The odometer data is obtained based on the encoder data and the IMU data, and the prior pose data of each particle at the current moment is obtained based on the odometer data and the preset robot kinematics model. as well as The actual estimated value of the weight of each particle is calculated based on the radar data and the video data. The average pose of the group of particles with the highest weight is obtained based on the prior pose data and the actual estimated value of the weight of each particle. The average pose of the group of particles with the highest weight is used as the pose estimate of the robot. The indoor positioning of the robot is obtained based on the pose estimate of the robot. The step of calculating the actual estimated value of the weight of each particle based on the radar data and the video data includes: calculating the actual estimated value of the weight of each particle based on the radar data and the video data according to a preset weight estimation formula, wherein the preset weight estimation formula is: W=W L ×W C ; Where W is the actual estimated value of the particle weight, W L To calculate the particle weights using the radar data, W C The particle weights are calculated using the video data; The particle weights estimated from the video data are as follows: W C =W Cd (d)×W Cα (α); in, Among them, W Cd (d) is a function of positional deviation, W Cα (α) is a function of angular deviation, d is the distance between the estimated position and the prior position, α is the angular deviation between the estimated position and the prior position, and f N (α) is the probability density of particles with an angle deviation of α, f N (π) represents the probability density of a particle with an angular deviation of π radians.

2. The method according to claim 1, characterized in that, The average pose of the group of particles with the highest weights is obtained based on the prior pose data and the actual estimated weights of each particle, including: Based on the actual estimated value of the weight of each particle, identify particles to be removed whose actual estimated weight is less than a first preset threshold. The particles to be removed are eliminated, and target particles that meet the preset dispersion conditions are dispersed within the preset range of the remaining particles. After a preset time, the average pose of the group of particles with the highest weight is obtained based on the prior pose data, the dispersion results of the target particles, and the actual estimated value of the weights of the remaining particles.

3. The method according to claim 1, characterized in that, The encoder data includes first encoder read data and second encoder read data. Before obtaining the mileage measurement data based on the encoder data and the IMU data, the data further includes: The encoder data is obtained by averaging the data read from the first encoder and the data read from the second encoder.

4. The method according to claim 1, characterized in that, Also includes: Based on the QR code beacon corresponding to the video data, the location of the video data in the map coordinate system is obtained from the beacon number table.

5. An indoor positioning device for an agricultural robot, characterized in that, include: The acquisition module is used to acquire encoder data, inertial measurement unit (IMU) data, radar data, and video data of the robot. The results module is used to obtain odometer measurement data based on the encoder data and the IMU data, and to obtain the prior pose data of each particle at the current moment based on the odometer measurement data and the preset robot kinematics model. as well as The positioning module is used to calculate the actual estimated value of the weight of each particle based on the radar data and the video data, and to obtain the average pose of the group of particles with the highest weight based on the prior pose data and the actual estimated value of the weight of each particle, and to use the average pose of the group of particles with the highest weight as the pose estimate of the robot, and to obtain the indoor positioning of the robot based on the pose estimate of the robot. Specifically, the positioning module is used to: calculate the actual estimated value of the weight of each particle based on the radar data and the video data according to a preset weight estimation formula, wherein the preset weight estimation formula is: w=w L ×w C ; Where W is the actual estimated value of the particle weight; W L The particle weights calculated using the radar data; W C The particle weights are calculated using the video data; The particle weights estimated from the video data are as follows: W C =W Cd (d)×W Cα (α); in, Among them, W Cd (d) is a function of positional deviation, W Cα (α) is a function of angular deviation, d is the distance between the estimated position and the prior position, α is the angular deviation between the estimated position and the prior position, and f N (α) is the probability density of particles with an angle deviation of α, f N (π) represents the probability density of a particle with an angular deviation of π radians.

6. The apparatus according to claim 5, characterized in that, The positioning module is specifically used for: Based on the actual estimated value of the weight of each particle, identify particles to be removed whose actual estimated weight is less than a first preset threshold. The particles to be removed are eliminated, and target particles that meet the preset dispersion conditions are dispersed within the preset range of the remaining particles. After a preset time, the average pose of the group of particles with the highest weight is obtained based on the prior pose data, the dispersion results of the target particles, and the actual estimated value of the weights of the remaining particles.

7. The apparatus according to claim 5, characterized in that, The encoder data includes first encoder read data and second encoder read data. Before obtaining the mileage measurement data based on the encoder data and the IMU data, the result module is further configured to: The encoder data is obtained by averaging the data read from the first encoder and the data read from the second encoder.

8. The apparatus according to claim 5, characterized in that, Also includes: The lookup module is used to find the location of the video data in the map coordinate system from the beacon number table based on the QR code beacon corresponding to the video data.

9. An electronic device, characterized in that, include: A memory, a processor, and a computer program stored in the memory and executable on the processor, the processor executing the program to implement the indoor positioning method for an agricultural robot as described in any one of claims 1-4.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, The program is executed by the processor to implement the indoor positioning method for agricultural robots as described in any one of claims 1-4.

Images