High-precision positioning method for stacker based on encoder and vision fusion

Abstract

The application discloses a high-precision positioning method for a stacker based on fusion of an encoder and vision, belongs to the field of stackers, and fuses high-frequency event encoder data and low-frequency event depth camera data, namely, the encoder dominates high-frequency prediction, and vision is used for low-frequency correction, so that optimal estimation values are obtained, which have the advantages of high-frequency characteristics of the encoder and absolute accuracy of vision and fundamentally eliminate cumulative errors. The method mainly comprises the following steps: defining a state vector of a Kalman filter of any walking direction of a walking shaft X shaft or a lifting shaft Z shaft of the stacker, a state transition model and an observation model, and defining the observation model to comprise high-frequency event encoder data and low-frequency event depth camera data; and after the above definition is completed, initialization of the Kalman filter is performed. The application is mainly used for high-precision positioning of the stacker.

Description

Technical Field

[0001] This invention relates to the field of stacker cranes, and more specifically, to a high-precision positioning method for stacker cranes based on encoder and vision fusion. Background Technology

[0002] Stacker cranes are core equipment in automated storage and retrieval systems (AS / RS), and their positioning accuracy directly determines the operational efficiency and reliability of the warehousing system. Currently, the mainstream positioning methods are: 1. Positioning by motor encoder: The position of the stacker crane is indirectly calculated by the rotation angle of the motor. The advantages are continuous data, high sampling frequency and strong anti-interference ability. 2. Motor encoder + optical reader positioning: By laying visual markers on the aisle floor or shelves, the optical reader captures and calculates the absolute position, providing an absolute position without cumulative error.

[0003] Positioning with a motor encoder is essentially a dead reckoning method. It can accumulate errors due to transmission system errors (such as wheel slippage and wheel wear), making it impossible to guarantee high accuracy over a long period. Positioning with a motor encoder and optical reader is susceptible to data loss and jumps caused by damaged markings and lens obstruction. Summary of the Invention

[0004] The purpose of this invention is to provide a high-precision positioning method for stacker cranes based on encoder and vision fusion. It fuses encoder data of high-frequency events and depth camera data of low-frequency events, that is, the encoder dominates high-frequency prediction and vision performs low-frequency correction, and the optimal estimate has the advantages of both encoder high-frequency characteristics and vision absolute accuracy, thus fundamentally eliminating accumulated errors.

[0005] This invention is achieved through the following technical solution: A high-precision positioning method for stacker cranes based on encoder and vision fusion includes the following steps: S1. Define the state vector, state transition model, and observation model of the Kalman filter for any one of the stacker crane's travel axes (X-axis or Z-axis). The defined observation model includes encoder data for high-frequency events. Depth camera data and low-frequency events ; S2. After completing the above definition, initialize the Kalman filter; S3. Based on step S2, predict the prior state estimate and the prior error covariance respectively; S4. Execute the update when data arrives: 1) Encoder data for high-frequency events Upon arrival: S401. Calculate the Kalman gain.

[0006] in, , To estimate the encoder observation noise covariance based on encoder resolution and transmission accuracy. , and The larger the value, the greater the sensor noise, and the more the filter trusts the model predictions. S402, Update posterior state estimation:

[0007] S403, Update the posterior error covariance: ; 2) Depth camera data of low-frequency events Upon arrival: S404. Data validity check: Before updating, first check the data validity... Conduct the inspection: Outlier removal: Judgment If the value is less than a preset threshold, and exceeds it, the visual data is considered an outlier, discarded, and not updated. Confidence check: A comprehensive judgment is made based on the quality of the corner detection of the markers; S405. Calculate the Kalman gain.

[0008] in, , To estimate the encoder observation noise covariance, an accuracy estimate is based on camera calibration and corner point calculation. , and The larger the value, the greater the sensor noise, and the more the filter trusts the model predictions. S406. Update the posterior state estimate.

[0009] S407, Update the posterior error covariance. .

[0010] Furthermore, defining the state vector of the Kalman filter involves the following steps: Since the axis undergoes one-dimensional linear motion, the state vector of the axis is defined as follows:

[0011] Where p is the absolute position of the stacker crane's loading platform in the track direction (unit: meters), and v is the instantaneous running speed of the stacker crane's loading platform in the track direction at the current moment (unit: meters / second).

[0012] Furthermore, defining the state transition model of the Kalman filter includes the following steps: This model describes how the state evolves over time. It assumes that the stacker crane undergoes approximately uniform acceleration in a short time interval, with the acceleration uncertainty represented by process noise w. The discrete-time state transition equation is expressed as follows:

[0013] in, , Let k and k-1 be the state vectors. Here is the state transition matrix. The filtering period is 0.001s (encoder sampling period). To control the input items.

[0014] Furthermore, defining the observation model of the Kalman filter specifically includes the following steps: High-frequency encoder data (observation model 1): The encoder provides incremental displacement. This can be considered as a change in system speed. Indirect observation, its observation equation is:

[0015] Observation noise This mainly includes slippage, quantization error, etc. Low-frequency depth camera data (observation model 2): ​​When the camera successfully identifies a visual marker, an absolute position can be provided through position conversion. The observation equation is as follows:

[0016] Observation noise This mainly includes camera calibration error, image calculation error, etc.

[0017] Furthermore, The observation method is as follows: After the depth camera acquires the latest frame image, the three-dimensional coordinates can be calculated based on the depth camera's intrinsic parameters and the depth map acquired by the depth camera, using the following formula:

[0018] Where Xs, Ys, and Zs are the three-dimensional coordinates in the depth camera coordinate system, (u0, v0) represents the intersection of the optical axis and the imaging plane (the coordinates of the origin of the image coordinate system in the pixel coordinate system); (fu, fv) are the focal lengths along the X and Y axes, (u0, v0) and (fu, fv) are fixed parameters of the depth camera, which can be obtained through depth camera calibration, and Z represents the distance from a certain corner point to the depth camera.

[0019] Furthermore, step S2 specifically includes the following steps: Define the initial state of the stacker crane loading platform. ,in The results were obtained from the initial visual positioning after the system was powered on. , Based on the initial state, the error covariance matrix can be estimated as follows: .

[0020] Furthermore, step S3 specifically includes the following steps: Based on the state from the previous step, predict the prior state estimate.

[0021] Based on the error covariance prediction in the previous step, the prior error covariance is predicted. .

[0022] Furthermore, the position conversion includes the following steps: Tracking feature points in the previous frame image using feature point matching to obtain the correspondence between feature points; for the matched point set... and Construct the following least squares problem: find the rotation matrix that minimizes the sum of squared errors J. Translation matrix , The rotation matrix is ​​obtained. Translation matrix Then, according to the following formula,

[0023] Obtain the latest position to get the absolute position of the loading platform.

[0024] Compared with the prior art, the beneficial effects of the present invention are: 1. This application features high-frequency output without accumulated error: the final output is the state estimate optimized by Kalman filtering. An optimal estimate that combines the advantages of encoder high-frequency characteristics and visual absolute accuracy fundamentally eliminates accumulated errors.

[0025] 2. Through the innovation detection mechanism, the system can effectively resist the instantaneous misidentification and outliers of the vision system, ensuring the stability of the fusion system. Even when the visual information is not accurate for a long time, the system can still rely on the encoder and high-precision model to perform degradation operations, and the performance will decline smoothly.

[0026] 3. Tightly Coupled Fusion Framework: Unlike a simple logical switch of "visual data overwriting encoder data," this method performs deep fusion at the state estimation level, increasing Kalman gain. Able to determine the current uncertainty of the system Adaptive assignment of trust weights to encoder and camera data. Attached Figure Description

[0027] Figure 1 This is a flowchart of the present invention; Figure 2 This is a schematic diagram of the implementation environment of the device of the present invention.

[0028] In the image: 1. Stacker crane; 2. Support frame; 3. Loading platform; 4. Depth camera; 5. Ring light source. Detailed Implementation

[0029] The present invention will now be further described in conjunction with the accompanying drawings.

[0030] like Figure 1 As shown in the embodiment, a high-precision positioning method for a stacker crane based on encoder and vision fusion includes the following steps: S1. Define the state vector, state transition model, and observation model of the Kalman filter for any one of the stacker crane's travel axes (X-axis or Z-axis). The defined observation model includes encoder data for high-frequency events. Depth camera data and low-frequency events ; S2. After completing the above definition, initialize the Kalman filter; S3. Based on step S2, predict the prior state estimate and the prior error covariance respectively; S4. Execute the update when data arrives: 1) Encoder data for high-frequency events Upon arrival: S401. Calculate the Kalman gain.

[0031] in, , To estimate the encoder observation noise covariance based on encoder resolution and transmission accuracy. , and The larger the value, the greater the sensor noise, and the more the filter trusts the model predictions. S402, Update posterior state estimation:

[0032] S403, Update the posterior error covariance: ; 2) Depth camera data of low-frequency events Upon arrival: S404. Data validity check: Before updating, first check the data validity... Conduct the inspection: Outlier removal: Judgment If the value is less than a preset threshold, and exceeds it, the visual data is considered an outlier, discarded, and not updated. Confidence check: A comprehensive judgment is made based on the quality of the corner detection of the markers; S405. Calculate the Kalman gain.

[0033] in, , To estimate the encoder observation noise covariance, an accuracy estimate is based on camera calibration and corner point calculation. , and The larger the value, the greater the sensor noise, and the more the filter trusts the model predictions. S406. Update the posterior state estimate.

[0034] S407, Update the posterior error covariance.

[0035] Defining the state vector of a Kalman filter involves the following steps: Since the axis undergoes one-dimensional linear motion, the state vector of the axis is defined as follows:

[0036] Where p is the absolute position of the stacker crane's loading platform in the track direction (unit: meters), and v is the instantaneous running speed of the stacker crane's loading platform in the track direction at the current moment (unit: meters / second).

[0037] Defining the state transition model of a Kalman filter involves the following steps: This model describes how the state evolves over time. It assumes that the stacker crane undergoes approximately uniform acceleration in a short time interval, with the acceleration uncertainty represented by process noise w. The discrete-time state transition equation is expressed as follows:

[0038] in, , Let k and k-1 be the state vectors. Here is the state transition matrix. The filtering period is 0.001s (encoder sampling period). To avoid using control inputs, this model chooses not to use control inputs and instead incorporates all unmodeled dynamic characteristics, such as the motor's acceleration uncertainty, into the process noise. This greatly simplifies the model and eliminates the need for precise input of parameters such as motor torque, thus reducing process noise. The filter follows a Gaussian distribution with a mean of 0 and a covariance matrix of Q. The Q matrix is ​​crucial for parameter tuning, representing the trade-off between the level of confidence in the model. A larger Q indicates greater uncertainty, meaning the filter places more trust in the observed data. The Q matrix can be adjusted experimentally. .

[0039] Defining the observation model of a Kalman filter involves the following steps: This model describes the relationship between sensor measurements and system state. In this invention, two types of sensors provide observations. High-frequency encoder data (observation model 1): The encoder provides incremental displacement. This can be considered as a change in system speed. Indirect observation, its observation equation is:

[0040] Observation noise This mainly includes slippage, quantization error, etc. Low-frequency depth camera data (observation model 2): ​​When the camera successfully identifies a visual marker, an absolute position can be provided through position conversion. The observation equation is as follows:

[0041] Observation noise This mainly includes camera calibration error, image calculation error, etc.

[0042] The observation method is as follows: After the depth camera acquires the latest frame image, the three-dimensional coordinates can be calculated based on the depth camera's intrinsic parameters and the depth map acquired by the depth camera, using the following formula:

[0043] Where Xs, Ys, and Zs are the three-dimensional coordinates in the depth camera coordinate system, (u0, v0) represents the intersection of the optical axis and the imaging plane (the coordinates of the origin of the image coordinate system in the pixel coordinate system); (fu, fv) are the focal lengths along the X and Y axes, (u0, v0) and (fu, fv) are fixed parameters of the depth camera, which can be obtained through depth camera calibration, and Z represents the distance from a certain corner point to the depth camera.

[0044] Step S2 specifically includes the following steps: Define the initial state of the stacker crane loading platform. ,in The results were obtained from the initial visual positioning after the system was powered on. , Based on the initial state, the error covariance matrix can be estimated as follows: .

[0045] Step S3 specifically includes the following steps: Based on the state from the previous step, predict the prior state estimate.

[0046] Based on the error covariance prediction in the previous step, the prior error covariance is predicted. .

[0047] Position conversion includes the following steps: Tracking feature points in the previous frame image using feature point matching to obtain the correspondence between feature points; for the matched point set... and Construct the following least squares problem: find the rotation matrix that minimizes the sum of squared errors J. Translation matrix , The rotation matrix is ​​obtained. Translation matrix Then, according to the following formula,

[0048] By obtaining the latest position, the absolute position of the loading platform can be obtained. In the actual filter, we treat these two sensors as two independent observation sources and update the system at different times.

[0049] The system normally uses an encoder for positioning. When the camera detects a marker, it directly overwrites the current encoder position value with the absolute position value read by the camera.

[0050] like Figure 2As shown, this application is also implemented based on the following device: it includes a stacker crane 1 and a loading platform 3. A bracket 2 and a loading platform 3 are respectively installed on the stacker crane 1. The bracket 2 is fixed on the loading platform 3. A depth camera 4 and a ring light source 5 are set on the left and right sides of the loading platform 3. The loading platform 3 is located in the center of the stacker crane 1 and is used to place goods. The edge computer and the stacker crane control system (except for the method described in this application, the rest of the edge computer and the stacker crane control system are all prior art) are placed in the electrical control cabinet. The edge computer is used to process RGB image data and the algorithm proposed in this invention in real time, and can transmit the data to the stacker crane control system. The depth camera 4 and the ring light source 5 are located above the loading platform. The depth camera 4 is used to collect RGB images of the goods in real time and transmit them to the edge computer. The light source 5 is a supplementary lighting element to ensure that the depth camera obtains clear images.

[0051] The filter execution strategy is as follows: the filter runs the prediction step and the update step for the encoder at high speed with the encoder's sampling period, thereby providing a high-frequency, continuous optimal velocity / position estimate. When visual data arrives, as an asynchronous event, an update step for the camera is inserted, the main function of which is to correct the position state P and reset the accumulated error, while also indirectly correcting the velocity state v.

[0052] Firstly, the stacker crane operates in three dimensions, and each dimension is a linear motion. In the actual control process, the X-axis of travel and the Z-axis of lifting are also independently positioned and there is no coupling phenomenon. Therefore, the following explanation will take the X-axis of travel as an example.

[0053] Specifically: 1. The loading platform 3 needs to move to a specified position on the X-axis. The edge computing system extracts the corner information of the visual identifier from the RGB image and determines the absolute position of the loading platform 3 based on the corner information, which is then assigned in step S2. ,in The vector represents the position along the X-axis of the walking axis. The second digit represents the velocity along the X-axis of the travel axis. Since it is initially stationary, it is set to 0. Based on the initial state, the error covariance matrix is ​​estimated and can be set to... .

[0054] The walking motor starts rotating. When the encoder data arrives, the observed values ​​can be obtained from the Kalman filter's observation model and combined and assigned to... Then, the prior state estimate is obtained from step S3. and prior error covariance The Kalman gain is calculated using steps S401, S402, and S403 respectively. Obtain the posterior state estimate (Actual state) and posterior error covariance .

[0055] At the same time, if depth camera data Upon arrival, an asynchronous event is triggered, inserting an update step for the camera. This involves obtaining the observation values ​​from the Kalman filter's observation model and combining them to assign them to... Then, the posterior error covariance of the encoder in step S3 is used. Obtain the prior error covariance Then, steps S401, S402, and S403 are executed to calculate the Kalman gain. Obtain the posterior state estimate (Actual state) and posterior error covariance This corrects the actual position of the stacker crane's loading platform 3, thereby correcting the position state P and resetting the accumulated error. It also indirectly corrects the speed state v.

[0056] Specifically, step two involves the loading platform 3 moving to a designated position in space that includes the X-axis (travel axis) and Z-axis (lift axis). The edge computer extracts the corner information of the visual identifier from the RGB image and determines the absolute position of the loading platform based on the corner information, which is then assigned in step S2. ,in This represents the position of the travel axis (X-axis). Represents the position of the lifting axis (Z-axis), vector The third and fourth digits represent the velocities of the travel axis (X-axis) and the lift axis (Z-axis), respectively. Since they are initially stationary, they are set to 0. Based on the initial state, the error covariance matrix is ​​estimated and can be set to... .

[0057] Once the walking motor starts rotating, observations can be obtained from the Kalman filter observation model and combined and assigned to... Then, the prior state estimate is obtained from step S3. and prior error covariance Next, when encoder data arrives, steps S401, S402, and S403 are executed to calculate the Kalman gain. Obtain the posterior state estimate (Actual state) and posterior error covariance .

[0058] At the same time, if depth camera data Upon arrival, an asynchronous event is triggered, inserting an update step for the camera. This involves obtaining the observation values ​​from the Kalman filter's observation model and combining them to assign them to... Then, the posterior error covariance of the encoder in step S3 is used. Obtain the prior error covariance Then, steps S401, S402, and S403 are executed to calculate the Kalman gain. Obtain the posterior state estimate (Actual state) and posterior error covariance The second embodiment recreates the actual positioning scenario of the stacker crane. By fusing the motor encoder and the camera, the actual position of the stacker crane's loading platform is corrected, thus correcting the position state P and resetting the accumulated error. At the same time, it also indirectly corrects the speed state v.

[0059] The specific steps by which the edge computer extracts corner information of visual identifiers from RGB images and determines the absolute position of the loading platform based on this corner information can be, but are not limited to, the following: After the depth camera acquires the latest frame, the 3D coordinates can be calculated based on the depth camera's intrinsic parameters and the depth map it captured. The formula is as follows:

[0060] Where Xs, Ys, and Zs are the three-dimensional coordinates in the depth camera coordinate system, (u0, v0) represents the intersection of the optical axis and the imaging plane, that is, the coordinates of the origin of the image coordinate system in the pixel coordinate system; (fu, fv) are the focal lengths along the X and Y axes, (u0, v0) and (fu, fv) are the fixed parameters of the depth camera, which can be obtained through the calibration of the depth camera, and Z represents the distance from a certain corner point to the depth camera; By using feature point matching to track feature points in the previous frame, the correspondence between feature points can be obtained. For the matched point set and The following least squares problem needs to be constructed: find the rotation matrix that minimizes the sum of squared errors J. Translation matrix ,Right now .

[0061] Obtain the rotation matrix Translation matrix Then, according to the following formula

[0062] This allows us to obtain the latest location and thus the absolute position of the loading platform.

[0063] This invention achieves deep fusion at the state estimation level, treating the encoder and camera as different observation sources of the same system state (position, velocity). Through the optimal estimation algorithm of Kalman filtering (KF), the Kalman gain K is adaptively calculated based on the current system uncertainty (covariance matrix P), thereby scientifically allocating trust weights to the data from the two sensors. This is the theoretical basis for achieving a performance improvement of "1+1>2".

[0064] Based on the Kalman filter framework, even in the event of prolonged visual sensor failure (such as when all markers are obscured), the system can automatically degrade to a pure encoder-driven "dead reckoning" mode. Although the accuracy will slowly decrease over time, it ensures the continuity of function, prevents sudden shutdowns, and greatly improves system availability.

[0065] The data arrival times of the encoder (high frequency) and the camera (low frequency) are asynchronous. The KF implementation of this invention elegantly handles this multi-rate, asynchronous sensing problem. The filter runs at high speed with the encoder's rhythm for prediction and updates, while visual data is treated as an asynchronous event, triggering an independent update step upon its arrival. This design ensures that the system can simultaneously enjoy the benefits of high frequency and high accuracy.

[0066] The values ​​of the Q (process noise) and R (observation noise) matrices directly determine the performance of the filter. Advanced implementations of this invention may include online tuning or adaptive estimation of these parameters. For example, when stacker crane acceleration is detected, Q can be appropriately increased to represent increased model uncertainty; when the confidence level of camera image processing is low, it can be increased accordingly. This adaptive capability can further improve the system's performance in dynamically changing environments.

[0067] This invention accurately captures the core characteristics of the one-dimensional motion of a stacker crane and establishes a concise and effective Kalman filter model, which ensures estimation accuracy while controlling computational complexity, making it suitable for real-time operation in embedded controllers.

Claims

1. A high-precision positioning method for stacker cranes based on encoder and vision fusion, characterized in that: Includes the following steps: S1. Define the state vector, state transition model, and observation model of the Kalman filter for any one of the stacker crane's travel axes (X-axis or Z-axis). The defined observation model includes encoder data for high-frequency events. Depth camera data and low-frequency events ; S2. After completing the above definition, initialize the Kalman filter; S3. Based on step S2, predict the prior state estimate and the prior error covariance respectively; S4. Execute the update when data arrives: 1) Encoder data for high-frequency events Upon arrival: S401. Calculate the Kalman gain. in, , To estimate the encoder observation noise covariance based on encoder resolution and transmission accuracy. , and The larger the value, the greater the sensor noise; S402, Update posterior state estimation: S403, Update the posterior error covariance: ； 2) Depth camera data of low-frequency events Upon arrival: S404. Data validity check: Before updating, first check the data validity... Conduct the inspection: Outlier removal: Judgment If the value is less than a preset threshold, and exceeds it, the visual data is considered an outlier, discarded, and not updated. Confidence check: A comprehensive judgment is made based on the quality of the corner detection of the markers; S405. Calculate the Kalman gain. in, , To estimate the encoder observation noise covariance, an accuracy estimate is based on camera calibration and corner point calculation. , and The larger the value, the greater the sensor noise; S406. Update the posterior state estimate. S407, Update the posterior error covariance. 。 2. The high-precision positioning method for stacker cranes based on encoder and vision fusion according to claim 1, characterized in that: Defining the state vector of a Kalman filter involves the following steps: Since the axis undergoes one-dimensional linear motion, the state vector of the axis is defined as follows: Where p is the absolute position of the stacker crane's loading platform in the track direction, in meters, and v is the instantaneous running speed of the stacker crane's loading platform in the track direction at the current moment, in meters per second.

3. The high-precision positioning method for stacker cranes based on encoder and vision fusion according to claim 1, characterized in that: Defining the state transition model of a Kalman filter involves the following steps: Assuming the stacker crane undergoes approximately uniform acceleration over a short time, with the acceleration uncertainty represented by process noise w, the discrete-time state transition equation is expressed as follows: in, , Let k and k-1 be the state vectors. Here is the state transition matrix. For the filter period, To control the input items.

4. The high-precision positioning method for stacker cranes based on encoder and vision fusion according to claim 2, characterized in that: Defining the observation model of a Kalman filter involves the following steps: High-frequency encoder data: The encoder provides incremental displacement. This can be considered as a change in system speed. Indirect observation, its observation equation is: Observation noise Including slippage and quantization error; Low-frequency depth camera data: When the camera successfully identifies a visual marker, it can provide an absolute position through position calculation. The observation equation is as follows: Observation noise This includes camera calibration error and image calculation error.

5. The high-precision positioning method for stacker cranes based on encoder and vision fusion according to claim 4, characterized in that: The observation method is as follows: After the depth camera acquires the latest frame image, the three-dimensional coordinates can be calculated based on the depth camera's intrinsic parameters and the depth map acquired by the depth camera, using the following formula: in, , , Let (u0, v0) be the three-dimensional coordinates in the depth camera coordinate system, where (u0, v0) represents the intersection of the optical axis and the imaging plane; (f... u f v Let (u0, v0) and (f) be the focal lengths along the X and Y axes, respectively. u f v ) are fixed parameters of the depth camera, which can be obtained through depth camera calibration, and Z represents the distance from a certain corner point to the depth camera.

6. The high-precision positioning method for stacker cranes based on encoder and vision fusion according to claim 3, characterized in that: Step S2 specifically includes the following steps: Define the initial state of the stacker crane loading platform. ,in The results were obtained from the initial visual positioning after the system was powered on. , Based on the initial state, the error covariance matrix can be estimated as follows: .

7. The high-precision positioning method for stacker cranes based on encoder and vision fusion according to claim 6, characterized in that: Step S3 specifically includes the following steps: Based on the state from the previous step, predict the prior state estimate. Based on the error covariance prediction in the previous step, the prior error covariance is predicted. 。 8. The high-precision positioning method for stacker cranes based on encoder and vision fusion according to claim 4, characterized in that: Position conversion includes the following steps: Tracking feature points in the previous frame image using feature point matching to obtain the correspondence between feature points; for the matched point set... and Construct the following least squares problem: find the rotation matrix that minimizes the sum of squared errors J. Translation matrix , The rotation matrix is ​​obtained. Translation matrix Then, according to the following formula, Obtain the latest position to get the absolute position of the loading platform.

Images