An AGV positioning method based on improved unscented Kalman filtering

Abstract

An AGV positioning method based on improved unscented Kalman filtering includes the following steps: Step 1: First, the forward speed v of the AGV is detected using a motor incremental encoder and an angle encoder. k and steering speed ω k Step 2: Then, receive the information scanned by the LiDAR and generate the reflector coordinate sequence; Step 3: Establish an AGV kinematic model to estimate the AGV pose; Step 4: Transform the reflector coordinates scanned by the LiDAR to the world coordinate system and generate the AGV observation equation; Step 5: Perform an improved unscented Kalman filter based on the state-space equation to obtain the AGV vehicle pose after filtering. This invention, based on the proposed improved unscented Kalman filter algorithm, utilizes the reflector coordinate information to locate the vehicle's position; it boasts fast computation speed, effectively reduces hardware and labor costs, and better overcomes the impact of vehicle position shifts during driving, enhancing subsequent vehicle control.

Description

Technical Field

[0001] This invention belongs to the field of AGV positioning and relates to a positioning method, specifically a method for positioning Automated Guided Vehicles (AGVs). This method uses a lidar to transmit the coordinate information of reflectors within a fixed area to the system, and uses the coordinate information of the reflectors to locate the vehicle's position based on an unscented Kalman filter algorithm. Background Technology

[0002] Most existing AGVs use inertial navigation for positioning, followed by Kalman filtering or extended Kalman filtering. However, inertial navigation is prone to accumulating errors in complex working environments, leading to reduced positioning accuracy. Furthermore, both of these algorithms have limited capabilities in estimating AGV coordinates, and the errors often fall short of the specified range.

[0003] To reduce errors, some researchers have proposed using unscented Kalman filtering to estimate the coordinates of AGVs, as exemplified by Chinese Patent Application No. 201910326780, entitled "A High-Precision Positioning Method for Magnetic Navigation AGVs." However, the extended Kalman filter (EPF) method requires linearizing the nonlinear system equations when estimating AGV coordinates, which reduces estimation accuracy, retaining only first-order precision. Unscented Kalman filtering, on the other hand, uses unscented transformation to select sigma points, approximating the nonlinear system with mean and covariance, minimizing the loss of accuracy in the system state equations and achieving second-order computational precision. However, when there are only n state information points, a typical unscented Kalman filter requires selecting 2n or 2n+1 sigma points for the unscented transformation, undoubtedly increasing computational complexity and reducing estimation speed, which does not meet the expectations of practical applications. Summary of the Invention

[0004] To overcome the shortcomings of existing technologies, this invention provides an AGV positioning method based on an improved unscented Kalman filter. This method improves existing AGV positioning methods, is simple in process, highly reliable, and has good stability, and can be applied to various positioning fields.

[0005] The technical solution adopted by this invention to solve its technical problem is:

[0006] An AGV positioning method based on improved unscented Kalman filtering includes the following steps:

[0007] Step 1: Use an incremental encoder for the motor and an angle encoder to detect the forward speed v of the AGV. k and steering speed ω k ;

[0008] Step 2: Receive the information scanned by the lidar and generate a reflector coordinate sequence;

[0009] Step 3: Establish a motion model of the AGV to estimate its pose, X k It is the pose (x) of the AGV in the world coordinate system at time k. k ,y k ,θ k ), θ k It is the yaw angle of the vehicle at time k, v k ω is the forward speed of the vehicle at time k. k W is the vehicle's steering angular velocity at time k. k It is system noise with a mean of 0 and a covariance of Q;

[0010]

[0011] Where t is the time at each moment, and X k This represents the state at each time step, with the index k indicating the k-th time step.

[0012] Step 4: Transform the coordinates of the reflector scanned by the lidar to the world coordinate system and establish the following observation equations:

[0013]

[0014] These are coordinates in the lidar coordinate system. This refers to the coordinates of the lidar in the vehicle's coordinate system, where lθ is the lidar's mounting deflection angle. The coordinates of the vehicle body at time k are given by x. k The x-coordinate of the vehicle body at time k is represented by y. k LMX represents the ordinate at time k. k V is the world coordinate of the reflector. k It is measurement noise with a mean of 0 and a covariance of R;

[0015] Step 5: Improve the motion model obtained in Step 3 and the observation model obtained in Step 4 by performing unscented Kalman filtering to achieve AGV positioning.

[0016] Furthermore, the process of step 5 is as follows:

[0017] Initialize the state vector and estimate the covariance:

[0018]

[0019]

[0020] in, E(X) is the estimated value at time 0, which is equal to the expected value of the state at time 0. E(X) is the expected value of X, which is the mean obtained by multiplying each element of the vector by its corresponding probability and then summing them up.

[0021] Define the weight M corresponding to the sigma point. (i) and related vectors

[0022]

[0023]

[0024] Among them, M (i) This represents the weight value at the sigma point i, where the superscript i indicates the number of weights, and the relevant variables... It is a vector calculated from the weight values. Its superscript indicates which weight value it was calculated from, and its subscript indicates the vector index.

[0025] For j = 2, ..., n, the n-dimensional vector is obtained recursively according to the following equation (7). Among them 0 j-1 It is a column vector containing j-1 zeros;

[0026]

[0027] Generate sigma points:

[0028]

[0029] The sigma point is the state estimate at time k-1. Square root of the covariance with the state estimate sum vector The sum of the products is calculated, and the data generated for the next step of calculation is used. The subscript i represents the i-th time.

[0030] Based on the system kinematics model, the sigma point is transformed into...

[0031]

[0032] Using M (i) weighted average constitute and in It is the estimated value at time k. Here is the covariance estimate at time k:

[0033]

[0034]

[0035] By selecting the sigma point again, we obtain the estimated measurement value:

[0036]

[0037] in k is a sigma point, using observation equation (2) and the generated The first two dimensions of the point, i.e. Perform measurement updates; combine vehicle body coordinates with reflector coordinates observed by lidar (lmx). k ,lmy k Substituting this into (2), we obtain the sigma point of the reflector's coordinates in the world coordinate system.

[0038] In step 5, the state update is based on the measurement:

[0039] Using M (i) weighted average Obtain the measurement prediction value at time k, where These are the estimated coordinates of the reflector:

[0040]

[0041] Estimate the covariance P of the measurement prediction yy The subscript represents the measurement, taking noise R into account. k :

[0042]

[0043] estimate and Covariance P between xy The subscripts represent the state and observation. It is the sigma point at time k. It is the estimated value at time k. It is the sigma point of the reflector coordinates. These are estimated values ​​for the reflector coordinates:

[0044]

[0045] Use the following equation to update state measurements:

[0046]

[0047]

[0048]

[0049] Where K k It is the Kalman gain at time k. The inverse of the estimated value measurement prediction, This represents the true covariance at time k. It is the covariance estimated at time k. It is the true state value at time k. It is the estimated state value at time k, LMX k These are the actual coordinates of the reflector. Estimated coordinates of the reflector.

[0050] The beneficial effects of this invention are mainly reflected in the following aspects: Unlike traditional positioning methods, inertial navigation is easily affected and interfered with by external factors. Therefore, a lidar sensor is used to collect data and correct the positioning results of the AGV. The calculation process requires handling nonlinear processes. Compared with other algorithms such as extended Kalman filtering, unscented Kalman filtering has higher accuracy. Moreover, the improved unscented Kalman filtering algorithm of this invention has similar accuracy to that of general unscented Kalman filtering, but with less computation and faster calculation speed, which can effectively reduce measurement errors. Compared with other positioning methods, the improved unscented Kalman filtering used in this invention has a simple process, high reliability, and good stability, and can be applied to various positioning fields. Attached Figure Description

[0051] Figure 1 This is a schematic diagram of the hardware structure of the present invention;

[0052] Figure 2 It is the kinematic model of the vehicle;

[0053] Figure 3 This is an overall flowchart of the present invention;

[0054] Figure 4 This is a diagram showing the relative coordinates of the reflector to the lidar, converted to world coordinates. Detailed Implementation

[0055] The present invention will now be further described with reference to the accompanying drawings.

[0056] Reference Figures 1-4 An AGV positioning method based on improved unscented Kalman filtering includes the following steps:

[0057] Step 1: Use an incremental encoder for the motor and an angle encoder to detect the forward speed v of the AGV. k and steering speed ω k ;

[0058] Step 2: Receive the information scanned by the lidar and generate a reflector coordinate sequence;

[0059] Step 3: Establish a motion model of the AGV to estimate its pose, X k It is the pose (x) of the AGV in the world coordinate system at time k. k,y k ,θ k ), θ k It is the yaw angle of the vehicle at time k, v k ω is the forward speed of the vehicle at time k. k W is the vehicle's steering angular velocity at time k. k It is system noise with a mean of 0 and a covariance of Q;

[0060]

[0061] Where t is the time at each moment, and X k This represents the state at each time step, with the index k indicating the k-th time step.

[0062] Step 4: Transform the coordinates of the reflector scanned by the lidar to the world coordinate system and establish the following observation equations:

[0063]

[0064] These are coordinates in the lidar coordinate system. This refers to the coordinates of the lidar in the vehicle's coordinate system, where lθ is the lidar's mounting deflection angle. The coordinates of the vehicle body at time k are given by x. k The x-coordinate of the vehicle body at time k is represented by y. k LMX represents the ordinate at time k. k V is the world coordinate of the reflector. k It is measurement noise with a mean of 0 and a covariance of R;

[0065] Step 5: Improve the motion model obtained in Step 3 and the observation model obtained in Step 4 using unscented Kalman filtering to achieve AGV positioning. The process is as follows:

[0066] Initialize the state vector and estimate the covariance:

[0067]

[0068]

[0069] in, E(X) is the estimated value at time 0, which is equal to the expected value of the state at time 0. E(X) is the expected value of X, which is the mean obtained by multiplying each element of the vector by its corresponding probability and then summing them up.

[0070] Define the weight M corresponding to the sigma point. (i) and related vectors

[0071]

[0072]

[0073] Among them, M (i) This represents the weight value at the sigma point i, where the superscript i indicates the number of weights, and the relevant variables... It is a vector calculated from the weight values, where the superscript indicates which weight value it was calculated from, and the subscript indicates the vector index; for j = 2, ..., n, the n-dimensional vector is obtained recursively according to the following formula (7). Among them 0 j-1 It is a column vector containing j-1 zeros;

[0074]

[0075] Generate sigma points:

[0076]

[0077] The sigma point is the state estimate at time k-1. Square root of the covariance with the state estimate sum vector The sum of the products is calculated, and the data generated for the next step of calculation is used. The subscript i represents the i-th time.

[0078] Based on the system kinematics model, the sigma point is transformed into... in The sigma point at time k:

[0079]

[0080] Using M (i) weighted average constitute and

[0081]

[0082]

[0083] Using M (i) weighted average constitute and in It is the estimated value at time k. Here is the covariance estimate at time k:

[0084]

[0085] in k is a sigma point, using observation equation (2) and the generated The first two dimensions of the point, i.e. Perform measurement updates; combine vehicle body coordinates with reflector coordinates observed by lidar (lmx). k ,lmy k Substituting this into (2), we obtain the sigma point of the reflector's coordinates in the world coordinate system.

[0086]

[0087] Using M (i) weighted average Obtain the measurement prediction value at time k, where These are the estimated coordinates of the reflector:

[0088]

[0089] Estimate the covariance P of the measurement prediction yy The subscript represents the measurement, taking noise R into account. k :

[0090]

[0091] estimate and Covariance P between xy The subscripts represent the state and observation. It is the sigma point at time k. It is the estimated value at time k. It is the sigma point of the reflector coordinates. These are estimated values ​​for the reflector coordinates:

[0092]

[0093] Use the following equation to update state measurements:

[0094]

[0095]

[0096]

[0097] Where K k It is the Kalman gain at time k. The inverse of the estimated value measurement prediction, This represents the true covariance at time k. It is the covariance estimated at time k. It is the true state value at time k. It is the estimated state value at time k, LMX k These are the actual coordinates of the reflector. Estimated coordinates of the reflector.

[0098] exist Figure 4 In the diagram, the leftmost XOY coordinate system is the world coordinate system. (x, y) represents the vehicle's coordinates in the world coordinate system, and θ is the vehicle's yaw angle. (lmX, lmY) represents the reflector's coordinates in the world coordinate system.

[0099] The LXO'LY coordinate system in the middle is a coordinate system established with the vehicle body coordinates (x, y) as the origin O'. (lx, ly) are the radar coordinates in the vehicle body coordinate system, and lθ is the fixed installation deviation angle of the radar on the vehicle body, which can be obtained through calibration.

[0100] The coordinate system of LMXO"LMY on the far right is the radar coordinate system. The system's observation input is the reflector scanned by the lidar, (lmx,lmy) are the coordinates of the reflector in the lidar coordinate system, and (lmX,lmY) are the coordinates of the reflector in the world coordinate system.

[0101] The purpose of this design is to transform the relative coordinates of the reflector on the radar to the world coordinate system. The vehicle body coordinates (x, y) are calibrated in the world coordinate system and do not need to be changed.

[0102] The radar coordinates (lx, ly) are determined based on the vehicle coordinate system. Since there is a yaw angle θ between the vehicle coordinate system and the world coordinate system, the radar coordinates need to be projected into the world coordinate system through coordinate transformation. The projection of the horizontal coordinate is lx·cosθ-ly·sinθ, and the projection of the vertical coordinate is lx·sinθ+ly·cosθ.

[0103] The relative coordinates (lmx, lmy) of the reflector with respect to the radar in the radar coordinate system are based on the radar coordinate system. The radar coordinate system has a yaw angle θ and an installation deviation angle lθ relative to the world coordinate system. Therefore, the projection of its horizontal coordinate is lmx·cos(θ+lθ)-lmy·sin(θ+lθ), and the projection of its vertical coordinate is lmx·sin(θ+lθ)+lmy·cos(θ+lθ).

[0104] Finally, the coordinate transformation formula is derived:

[0105]

[0106] In this embodiment, an unscented Kalman filter method is used to estimate the data collected by the lidar, which is then used to correct the AGV positioning method. The overall process of this invention is as follows: Figure 3 As shown, the implementation process is as follows:

[0107] Step 1: Install a reflector in the working environment and preset the coordinates of the reflector in the working environment, which is called world coordinates.

[0108] Step 2: The lidar installed on the AGV starts working, performs laser scanning on the working environment, receives the laser signal reflected by the reflector, and generates the relative coordinates of the reflector with respect to the laser sensor.

[0109] Step 3: Collect the forward speed and turning speed of the AGV from the motor incremental encoder and angle encoder.

[0110] Step 4: Establish a coordinate system with the vehicle body as the origin and a coordinate system with the LiDAR as the origin. Transform the relative coordinates of the reflector scanned by the LiDAR to the world coordinate system, generating a set of measured reflector world coordinates. See details... Figure 4 .

[0111]

[0112] Step 5: Initialize using (3) and (4) to obtain the initial state and covariance. Use equation (5) to generate the weights of the sigma points. Then use (6) and (7) to generate the vector γ. Substitute the information obtained from (3), (4), and (7) into (8) to generate the sigma points. Initialization is complete. Then substitute the sigma points generated in (8) into (9) to complete the time update from time k-1 to time k.

[0113] The time update process has been completed. It is necessary to prepare for the measurement update. The sigma points generated in (9) and the weights generated in (5) are substituted into (10) to generate the posterior estimate of the measurement update. Then, the posterior estimate of the state is substituted into (12) to obtain the sigma point of the state at time k. The sigma point of the state at time k is substituted into (13) to generate the sigma point of the measurement at time k. Then, the measurement sigma points in (13) are weighted by (14) to complete the measurement update.

[0114] Finally, the Kalman gain (17) is calculated using (15) and (16), and then the state estimation is completed using (18) and (19).

[0115] The embodiments described in this specification are merely examples of implementations of the inventive concept and are for illustrative purposes only. The scope of protection of this invention should not be considered limited to the specific forms described in these embodiments; rather, it extends to equivalent technical means conceived by those skilled in the art based on the inventive concept.

Claims

1. An AGV positioning method based on improved unscented Kalman filtering, characterized in that, The method comprises the following steps: Step 1: Detect the forward speed v of the AGV using a motor incremental encoder and an angle encoder k and the steering speed ω k ; Step 2: receiving the information scanned by the laser radar to generate a retroreflective plate coordinate sequence; Step 3: Establish the motion model of AGV to estimate the pose of AGV, X k is the pose of AGV in the world coordinate system at time k (x k ,y k ,θ k ), θ k is the yaw angle of the vehicle body at time k, v k is the forward speed of the vehicle body at time k, ω k is the steering angular velocity of the vehicle body at time k, W k is the system noise with mean 0 and covariance Q; where t is the time at each time instant, X k is the state at each time instant, and subscript k is the kth time instant; Step 4: converting the retroreflective plate coordinates scanned by the laser radar into a world coordinate system to establish the following observation equation: is the coordinate in the laser radar coordinate system, is the coordinate of the laser radar in the vehicle body coordinate system, and lQ is the radar installation deflection angle, is the coordinate of the vehicle body at time k, wherein x k represents the horizontal coordinate of the vehicle body at time k, y k represents the longitudinal coordinate at time k, LMX k is the world coordinate of the reflector, V k is the measurement noise with a mean of 0 and a covariance of R; Step 5: improving the motion model obtained based on step 3 and the observation model obtained based on step 4 by using the unscented Kalman filter to realize AGV positioning; The process of step 5 is as follows: Initialize the state vector and the estimated covariance: wherein, is the estimate at time 0, equal to the expectation of the state value at time 0, E(X) is the expectation of X, i.e. the mean value obtained by multiplying each element of the vector by its corresponding probability and then summing them up. Define the weight M corresponding to the sigma point (i) and the related vector wherein, wherein M (i) is a weight value of the sigma point at i, the superscript i indicates the number of weight values, and the relevant variables is a vector calculated from the weight values, the superscript indicates which weight value is calculated, and the subscript indicates the vector number; For j = 2,..., n, recursively obtain the n-dimensional vector Y according to the following equation (7) i (n) where 0 j-1 is a column vector containing j - 1 zeros; Generate sigma points: The sigma point is the state estimate at time k-1. Square root of the covariance with the state estimate sum vector The sum of the products is calculated, and the data generated for the next step of calculation is used. The subscript i represents the i-th time. According to the system kinematic model, the sigma points are transformed into where sigma points at time k M (i) weighted average comprise and wherein is an estimate at time k, is a covariance estimate at time k: Select the sigma points again to calculate the estimated value of the measurement: where is the sigma point for k is generated using the observation equation (2) and the first two dimensions in the sigma point, i.e. the body frame coordinates and the retro-reflective panel coordinates (lmx k ,lmy k ) observed by the lidar are substituted into (2) to obtain the sigma point of the retro-reflective panel coordinates in the world frame 2. The AGV positioning method based on improved unscented Kalman filter according to claim 1, wherein, In step 5, update the state based on the measurement: Utilizing M (i) Weighted average Obtain the measurement prediction value at time k, wherein is the coordinate estimate of the retro-reflective panel: Estimate of the covariance P of the measurement prediction yy The subscript represents the measurement, taking into account the noise R k : estimate with covariance P xy , the subscripts represent state and observation, is the sigma point at time k, is the estimate at time k, is the sigma point of the retroreflector coordinates, is the estimate of the retroreflector coordinates: The state measurement update is performed with the following equation: where K k is the Kalman gain at time k, represents the inverse of the estimated value measurement prediction, represents the true covariance at time k, is the estimated covariance at time k, is the true state value at time k, is the estimated state value at time k, LMX k is the true retroreflector coordinate, is the coordinate estimate of the retroreflector.

Images