Integrated navigation system and method of intelligent pusher robot based on multi-sensor fusion
Through the use of multi-sensor fusion system and Kalman filter, the problems of insufficient navigation flexibility and noise interference of material pushing robots are solved, high-precision navigation and stability are achieved, and navigation performance is improved.
Patent Information
- Application Number
- CN202111516593.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-12-07
- Publication Date
- 2024-12-06
- Estimated Expiration
- 2041-12-07
AI Technical Summary
The existing material pushing robots have insufficient flexibility, measurement of noise and interference problems, and the lack of effective heading angle measurement methods, resulting in low navigation accuracy and stability.
Using a multi-sensor fusion system, including ultrasonic sensors, inertial sensors (IMU), wheeled odometers and magnetic sensors, combined with human-computer interactive interface, combines a path and uses a Kalman filter and a software filter to achieve combined navigation and heading. Angle reset correction, eliminates noise and interference, and improves navigation accuracy.
It realizes flexible path settings in different ranch environments, improves navigation accuracy and stability, effectively eliminates noise interference and heading angle drift problems of ultrasonic distance measurement, and improves the navigation performance of the material pushing robot.
Smart Images

Figure CN114216458B_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the field of animal husbandry automation and is a technology and method of an intelligent material-pushing robot combined navigation system based on multi-sensor fusion. Background Art
[0002] Animal husbandry has become a supporting industry for herders in my country and is their main source of income. So far, most cattle farms in China have adopted manual feeding, which is time-consuming and labor-intensive, and cannot guarantee operation accuracy and frequency. In order to solve the problems caused by manual feeding and break through the bottleneck of cattle farming automation, the industry has developed different types of feed-pushing robots, but there is still much room for improvement in navigation and control performance.
[0003] At present, the navigation methods of robots mainly include track navigation, ultrasonic navigation, magnetic strip navigation, magnetic nail navigation and marker line navigation. Track navigation requires laying of hanging tracks in advance, which greatly changes the ranch environment. Magnetic navigation requires laying of a large number of magnetic nails or magnetic strips in advance, which is costly and relatively troublesome to construct.
[0004] At present, the navigation problems of pusher robots mainly include the following points:
[0005] First, different ranch environments require different robot working routes. Therefore, the system needs a flexible path setting method;
[0006] Second, cattle fence ranging based on ultrasonic sensors is the current mainstream navigation control solution. However, it is difficult to avoid measurement noise with a single sensor, and cattle grazing will cause very serious dynamic interference to ultrasonic ranging;
[0007] Third, there is a lack of effective measurement methods for the heading angle of the pusher robot relative to the cattle pen.
[0008] In response to the above problems, this paper invents an intelligent pusher robot combined navigation system based on multi-sensor fusion, which proposes an effective solution to the above problems. Summary of the invention
[0009] The object of the present invention is to provide a device and method for a pusher robot integrated navigation system based on multi-sensor fusion.
[0010] The main devices of the present invention include a human-machine interface (touch screen DOP-107BV), an ultrasonic sensor, an inertial sensor (IMU), a motor, and an encoder. The steps of the robot combined navigation method are as follows:
[0011] Preparation: Install the corresponding motors, encoders, ultrasonic sensors, IMU, etc. on the robot.
[0012] Step 1: Set the navigation path of the pusher robot through the human-computer interaction interface.
[0013] Step 2: Use the magnetic strip navigation to drive out of the charging station and drive to the cattle pen.
[0014] Step 3: After the robot drives to the cattle pen, it starts the ultrasonic wave, inertial sensor (IMU), and wheel odometer for combined positioning and navigation.
[0015] Step 4: Drive to the end of the cattle pen, detect the magnetic strip, determine the direction of the robot according to step 1, and navigate to the other side of the cattle pen.
[0016] Step 5: After step 4 is completed, repeat step 3 until the magnetic stripe is detected again. Determine the direction of the robot according to step 1 and navigate back to the charging station according to the magnetic stripe.
[0017] According to the present invention of the above scheme, the human-machine interaction interface adopts a touch screen, and the direction and route of the robot can be flexibly set according to the actual environment of the ranch.
[0018] According to the above-mentioned scheme of the present invention, the step two is mainly used to guide the robot to drive to the cattle pen from the charging pile outside the cattle pen.
[0019] According to the present invention of the above scheme, in step three, after the robot drives to the cattle pen, the core problem of navigation is how to accurately obtain the lateral distance of the robot relative to the cattle pen, and ensure that the cow head and other external interferences are filtered out, how to obtain the heading angle of the robot relative to the cattle pen, and eliminate the navigation offset problem caused by the heading angle drift. Regarding the acquisition of the lateral distance, after entering the cattle pen, ultrasonic waves are used to measure the lateral distance of the robot relative to the cattle pen, and the data collected by the ultrasonic waves are pre-processed to filter out the distance outside the set distance and the distance that has mutated. At the same time, encoder data is collected for wheel odometer calculation, and the processed ultrasonic data and wheel odometer data are filtered using a Kalman filter to obtain the lateral distance of the robot relative to the cattle pen, effectively filtering out the accidental errors of ultrasonic waves and the interference of cow heads and other external factors. In the process of wheel odometer calculation, we use the heading angle measured by IMU instead of the heading angle calculated by the wheel encoder for calculation, which greatly improves the position accuracy of the wheel odometer calculation. The wheel odometer calculation formula is as follows:
[0020]
[0021] Where x(k+1), y(k+1), θ(k+1) are the robot coordinates and heading angle at time k+1, respectively; x(k), y(k), θ(k) are the robot coordinates and heading angle at time k, respectively. Δs is the distance traveled by the robot at the sampling time, Δθ represents the increment of the robot heading angle during the sampling time, cosθ represents the cosine value of the angle θ, and sinθ represents the sine value of the angle θ. Here θ is the heading angle measured by the IMU.
[0022] The Kalman filter filtering process is as follows:
[0023] The state equation is:
[0024] x k =x k-1 +Δs k,k-1 ×sin(θ k-1 )+w k-1 (1.2)
[0025] The observation equation is:
[0026] z k =x k +v k (1.3)
[0027] where x k represents the state value at time k, x k-1 represents the state value at time k-1, z k represents the observed value at time k, w k-1 and v k The k-1 moment state process noise and the k moment observation process noise are represented, and both obey the normal distribution. k,k-1 represents the robot's walking distance from time k-1 to time k, sinθ k-1 Represents the sine value of the angle θ at time k-1. The filter is mainly divided into the following two parts: prediction and update. The prediction formula is:
[0028]
[0029] The update phase formula is:
[0030]
[0031] Where: represents the prior estimate of the lateral distance at time k, represents the posterior estimate of the lateral distance at time k-1, represents the covariance of the prior distance estimate at time k, P k represents the posterior covariance of the horizontal distance at time k, P k-1 represents the posterior covariance of the horizontal distance at time k-1, Kk represents the Kalman filter gain at time k, Q is the covariance of the state noise, and R is the covariance of the observation noise.
[0032] By processing the data measured by ultrasonic waves and wheel odometers through the Kalman filter, a relatively smooth and stable lateral distance can be obtained, which provides a guarantee for the control of the robot.
[0033] To solve the problem of IMU heading angle drift, software filtering is used for reset correction. The main idea is: continuously collect the heading angle of the robot relative to the cattle pen and the lateral distance from the cattle pen. If the heading angle and the lateral distance from the cattle pen change within the threshold we set, we think that the robot and the cattle pen are in a relatively parallel position. At this moment, we compare the heading angle at this time with the initial heading angle. If the current heading angle has a large deviation from the initial heading angle, the heading angle is reset. The so-called reset operation is to clear all previous measurement data of the heading angle. After reset, the heading angle is 0, which eliminates the heading angle error and effectively eliminates the heading angle drift problem caused by the robot's long-distance travel. It provides a stable angle for the robot's heading control.
[0034] According to the present invention of the above scheme, in step four, when the pushing robot drives to the end of one side of the cattle pen, the magnetic sensor will detect a horizontal magnetic strip pre-laid at the end of the cattle pen, and determine the current direction according to the path set in step 1, and guide it to the other side of the cattle pen according to the magnetic strip.
[0035] According to the present invention of the above scheme, in step five, after entering the cattle pen on the other side, repeat the contents of step three until reaching the end of the cattle pen on this side, detecting the pre-laid magnetic strip, determining the current turn according to the pre-selected path in step 1, and navigating to the charging pile according to the magnetic strip to stop, waiting for the start of the next pushing task. BRIEF DESCRIPTION OF THE DRAWINGS
[0036] Figure 1 It is a flow chart of the present invention.
[0037] Figure 2 This is a picture of the robot entity.
[0038] Figure 3 This is a picture of the experimental site environment.
[0039] Figure 4 Layout picture for cattle farm.
[0040] Figure 5 Path setting diagram for the human-computer interaction interface.
[0041] Figure 6 This is a schematic diagram of the heading angle.
[0042] Figure 7 This is the Kalman filter flow chart.
[0043] Figure 8 The pictures are the effect before and after the experiment. DETAILED DESCRIPTION
[0044] As described in the background technology, there are deficiencies in the prior art. To solve the above technical problems, a typical method of the present invention is as follows: Figure 1 As shown, the main steps are as follows:
[0045] The robots used in the experiment are Figure 2 As shown, the experimental environment is shown in 3, and the layout of the cattle farm is shown in Figure 4 As shown in the figure, the robot runs a circle from the charging station and then returns to the cattle pen. Figure 5 The human-computer interaction interface sets the corresponding form route, which is:
[0046] Charging pile—>U-turn—>Magnetic stripe navigation—>Ultrasonic combined navigation—>Right turn—>Magnetic stripe navigation—>Ultrasonic combined navigation—>Right turn—>Magnetic stripe navigation—>Charging pile.
[0047] After the path is set, the robot starts working. It first turns around and drives out of the charging station, then navigates along the magnetic strip to the cattle pen.
[0048] After the robot runs from the charging station to the cattle pen, it starts the combined navigation mode of ultrasonic wave, IMU and wheel odometer. First, the initial lateral distance of the robot from the cattle pen at the current moment is recorded, which is recorded as x(0). The lateral distance is shown as follows: Figure 6 As shown in the figure, the robot linear velocity v is obtained by extrapolating the data measured by the encoder. The sampling time is Δt, and the heading angle collected by the IMU is θ, where the heading angle is shown as Figure 6 As shown in the figure, according to the calculation formula of the odometer in step 3, the lateral distance of the robot relative to the cattle pen can be obtained as:
[0049]
[0050] Among them, x is the lateral distance between the robot and the cattle pen at the current moment, x_last is the lateral coordinate of the robot from the cattle pen at the last sampling moment, and its initial value is x(0), v represents the linear velocity of the robot, and Δt represents the sampling time. First, the ultrasonic data is filtered and preprocessed, and the wheel odometer is calculated at the same time. The Kalman filter is used to fuse the ultrasonic and wheel odometer data. The filter is mainly divided into the following two parts: prediction and update. The prediction part formula is:
[0051]
[0052] The update phase formula is:
[0053]
[0054] The process of Kalman filter fusion data is as follows Figure 7 shown.
[0055] While driving in the cattle pen, the IMU heading angle drift problem was addressed by using software filtering to reset and correct the IMU.
[0056] The specific method is as follows: Assume that the robot is set to push materials at a distance of 0.8m from the cattle pen, the effective range of ultrasonic data collection is 0.6-1.0m, the initial heading angle is recorded as θ0, and the angle change threshold is set as T θ , the distance threshold change is set to T d , the heading angle deviation is set to θ e During the movement, the current robot's heading angle and lateral distance from the cattle pen are collected every 30 seconds for 5 consecutive times. The heading angle is recorded as: {θ1, θ2, θ3, θ4, θ5}, and the distance is recorded as: {d1, d2, d3, d4, d5}. If the collected heading angle and lateral distance values change relatively smoothly, that is, the absolute value of the difference between the heading angles of each two consecutive times from θ1 to θ5 is less than T θ , the absolute value of the difference between each two adjacent lateral distances between d1 and d5 is less than T d , and the measured values from d1 to d5 are all within the valid range, we believe that the robot's walking direction is parallel to the cattle pen. Then find the average of the currently collected heading angles θ1 to θ5 and record it as Now calculate The difference with θ0, if the absolute value of this difference is greater than the heading angle deviation θ we set e At this time, we believe that the robot has a heading angle deviation, and perform the IMU reset operation to eliminate the current accumulated heading angle error. This effectively eliminates the heading angle drift problem caused by the robot's long-distance travel or other interference factors.
[0057] After the navigation of one side of the cattle pen is completed, a horizontal magnetic strip is pre-laid at the end of the cattle pen. When the magnetic sensor detects the magnetic strip, it determines the current direction according to the path rule set in step 1, and moves to the other side of the cattle pen according to the magnetic navigation.
[0058] After entering the cattle pen on the other side, repeat step 3 until the magnetic navigation sensor detects the magnetic stripe at the end of the cattle pen. At the same time, according to the route rules set in step 1, the current robot turns and navigates to the charging station according to the magnetic stripe, waiting for the start of the next round of navigation tasks.
[0059] After experiments, the combined navigation system method of the pusher robot based on multi-sensor fusion invented in this paper is good, the navigation is accurate, and the experimental effect is good. Figure 8 shown.
Claims
1. An intelligent pusher robot combined navigation method based on multi-sensor fusion, characterized in that: The following steps are involved: 1) Set the navigation path of the pusher robot through the human-computer interaction interface; 2) Use the magnetic strip navigation to drive out of the charging station and drive to the cattle pen; 3) After the robot reaches the cattle pen, it activates the ultrasonic sensor, inertial sensor IMU, and wheel odometer for combined positioning and navigation; 4) When the robot reaches the end of the cattle pen, it detects the magnetic strip, determines the direction of the robot according to step 1, and navigates to the other side of the cattle pen; 5) After step 4 is completed, repeat step 3 until the magnetic stripe is detected again, determine the direction of the robot according to step 1, and navigate back to the charging station according to the magnetic stripe; Step 3, after the robot drives to the cattle pen, the core problem of navigation is how to accurately obtain the lateral distance of the robot relative to the cattle pen, and ensure that the cow head and other external interference are filtered out, how to obtain the heading angle of the robot relative to the cattle pen, and eliminate the navigation offset problem caused by heading angle drift; for the acquisition of the lateral distance, after entering the cattle pen, use ultrasound to measure the lateral distance of the robot relative to the cattle pen, and pre-process the data collected by ultrasound to filter out the distance outside the set distance and the distance that has mutated, and at the same time collect encoder data for wheel odometer calculation, use Kalman filter to filter the processed ultrasonic data and wheel odometer data to obtain the lateral distance of the robot relative to the cattle pen, and effectively filter out the accidental error of ultrasound and the interference of cow heads and other external factors; in the process of wheel odometer calculation, the heading angle measured by IMU is used instead of the heading angle calculated by the wheel encoder for calculation; the wheel odometer calculation formula is as follows: Where x(k+1), y(k+1), θ(k+1) are the robot coordinates and heading angle at time k+1, respectively; x(k), y(k), θ(k) are the robot coordinates and heading angle at time k, respectively; Δs is the distance traveled by the robot during the sampling time, Δθ represents the increment of the robot heading angle during the sampling time, cosθ represents the cosine value of the angle θ, and sinθ represents the sine value of the angle θ; the θ here is the heading angle measured by the IMU; The Kalman filter filtering process is as follows, and the state equation is: x k =x k-1 +Δs k,k-1 ×sin(θ k-1 )+w k-1 (1.2) The observation equation is: z k =x k +v k (1.3) where x k represents the state value at time k, x k-1 represents the state value at time k-1, zk represents the observation value at time k, and w k-1 and v k The k-1 moment state process noise and the k moment observation process noise are represented, and both obey the normal distribution. k,k-1 represents the distance traveled by the robot from time k-1 to time k, sin θ k-1 Represents the sine value of the angle θ at time k-1; the filter is mainly divided into the following two parts: prediction and update, where the prediction part formula is: The update phase formula is: Where: represents the prior estimate of the lateral distance at time k, represents the posterior estimate of the lateral distance at time k-1, represents the covariance of the prior distance estimate at time k, P k represents the posterior covariance of the horizontal distance at time k, P k-1 represents the posterior covariance of the horizontal distance at time k-1, K k represents the Kalman filter gain at time k, Q is the covariance of the state noise, and R is the covariance of the observation noise; By processing the data measured by ultrasonic waves and wheel odometers through Kalman filters, a relatively smooth and stable lateral distance can be obtained, which provides a guarantee for the control of the robot; To solve the problem of IMU heading angle drift, software filtering is used for reset correction. The robot's heading angle relative to the cattle pen and the lateral distance from the cattle pen are continuously collected. If the heading angle and the lateral distance from the cattle pen change within the set threshold, the robot and the cattle pen are in a relatively parallel position and move forward. At this moment, the heading angle at this moment is compared with the initial heading angle. If the current heading angle has a large deviation from the initial heading angle, the heading angle is reset. The so-called reset operation is to clear all previous measurement data of the heading angle. After reset, the heading angle is 0, thus eliminating the heading angle error and effectively eliminating the heading angle drift problem caused by the robot's long-distance travel, providing a stable angle for the robot's heading control. In the process of driving in the cattle pen, the IMU heading angle drift problem was addressed by using software filtering to reset and correct the IMU. Specifically, the robot is set to push materials at a distance of 0.8m from the cattle pen, the effective range of ultrasonic data collection is 0.6-1.0m, the initial heading angle is recorded as θ0, and the angle change threshold is set as T θ , the distance threshold change is set to T d , the heading angle deviation is set to θ e During the movement, the heading angle and the lateral distance from the cattle pen of the current robot are collected every 30 seconds for 5 consecutive times. The heading angle is recorded as: {θ1, θ2, θ3, θ4, θ5}, and the distance is recorded as: {d1, d2, d3, d4, d5}; if the collected heading angle and lateral distance values change relatively smoothly, that is, the absolute value of the difference between the heading angles of each two adjacent times from θ1 to θ5 is less than T θ , the absolute value of the difference between each two adjacent lateral distances between d1 and d5 is less than T d , and the measured values from d1 to d5 are all within the valid range. At this time, the robot's walking direction is parallel to the cattle pen; then the average value of the currently collected heading angles θ1 to θ5 is calculated and recorded as Now calculate The difference with θ0, if the absolute value of this difference is greater than the set heading angle deviation θ e At this time, the robot has a heading angle deviation, and the IMU reset operation is performed to eliminate the current accumulated heading angle error. This effectively eliminates the heading angle drift problem caused by the robot's long-distance travel or other interference factors.
2. The intelligent pusher robot combined navigation method based on multi-sensor fusion according to claim 1 is characterized in that: Observe the cattle shed environment on site and set the navigation path through the human-computer interaction interface designed with a touch screen.
3. The intelligent pusher robot combined navigation method based on multi-sensor fusion according to claim 1 is characterized in that: The robot charging station is placed outside the cattle pen. Before the robot runs from the charging station to the cattle pen, it uses a magnetic strip for positioning and navigation until it runs into the cattle pen.
4. The intelligent pusher robot combined navigation method based on multi-sensor fusion according to claim 1 is characterized in that: After driving to the cattle pen, the IMU is used to measure the robot's heading angle relative to the cattle pen, and the heading angle measured by the IMU is used instead of the heading angle calculated by the wheels for odometer calculation. The ultrasonic measurement data is preprocessed, and then the Kalman filter is used to fuse the ultrasonic and wheel odometer data to obtain the lateral distance of the robot relative to the cattle pen.
5. The intelligent pusher robot combined navigation method based on multi-sensor fusion according to claim 1 is characterized in that: Drive along one side of the cattle pen to the end, detect the pre-laid magnetic strip, determine the robot's direction based on the path information pre-set in step 1, and then navigate along the magnetic strip to the other side of the cattle pen.
6. The intelligent pusher robot combined navigation method based on multi-sensor fusion according to claim 1 is characterized in that: After driving to the other side of the cattle pen, repeat step 4. When navigating to the end of the cattle pen, detect the pre-laid magnetic strip, determine the direction of the robot according to the path information pre-set in step 1, and then navigate along the magnetic strip to the charging pile. Navigation ends and waits for the next round of navigation tasks.
7. The intelligent pusher robot combined navigation method based on multi-sensor fusion according to claim 1 is characterized in that: The ultrasonic sensor is installed on the side of the robot, and the IMU is rigidly connected to the robot; the encoder used by the wheel odometer to collect data is 2500 lines.
