A pose deviation estimation method for photoelectric sensing equipment based on particle swarm optimization
By setting feature points around the photoelectric sensing device and using particle swarm optimization algorithm, the problem of position deviation during installation of photoelectric sensing devices is solved, high-precision position deviation estimation is achieved, the calibration process is simplified, and the measurement accuracy is improved .
Patent Information
- Application Number
- CN202211135635.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-09-19
- Publication Date
- 2024-11-12
- Estimated Expiration
- 2042-09-19
AI Technical Summary
Position deviations often occur during the installation process of photoelectric sensing equipment, which affects the measurement accuracy. It is difficult for the prior art to achieve high-precision and high-efficiency pose deviation estimation.
Using a method based on particle swarm optimization algorithm, by setting feature points around the photoelectric sensing device, measuring their azimuth and pitch angles, designing an objective function, using the particle swarm algorithm to update the particle position, iteratively solve the displacement and angle deviation, and realizing the position pose Efficient estimation of deviation.
It realizes high-precision estimation of position deviation of photoelectric sensing equipment, improves measurement accuracy, simplifies the calibration process, and is suitable for the installation and use of photoelectric sensing equipment.
Smart Images

Figure CN115479599B_ABST
Abstract
Description
Technical Field
[0001] The invention relates to a posture deviation estimation method in the technical field of photoelectric sensor equipment calibration, and in particular to a posture deviation estimation method for photoelectric sensor equipment based on particle swarm optimization (PSO). Background Art
[0002] Visible light and infrared imaging equipment can be used to obtain video and image information of targets within the field of view, thereby achieving tasks such as identification and tracking. Common application scenarios include drone countermeasure stations. However, during the installation of photoelectric sensor equipment, deviations from the set installation posture are inevitable, and this deviation is a system error. This error will affect the sensor's measurement accuracy of the target. Therefore, how to use limited resources to achieve high-precision and high-efficiency estimation of this error is an important topic in the deployment and use of photoelectric sensor equipment. At present, there is no technology in the prior art that can achieve this goal. Summary of the invention
[0003] In view of this, the present invention provides a method for estimating the posture deviation of a photoelectric sensor device based on a particle swarm optimization algorithm, which can solve the problem that the photoelectric sensor device deviates from the preset posture during installation.
[0004] In order to achieve the above object, the technical solution adopted by the present invention is:
[0005] A method for estimating the posture deviation of a photoelectric sensor device based on particle swarm optimization comprises the following steps:
[0006] Step 1, establish an ideal northeast celestial coordinate system with the measured position of the photoelectric sensor device as the origin; the deviation of the actual position and posture of the photoelectric sensor device relative to the ideal northeast celestial coordinate system includes displacement deviation and angle deviation, wherein the displacement deviation is (Δx, Δy, Δz), Δx, Δy, Δz represent the displacement deviation of the X, Y, and Z axes respectively, and the angle deviation is (Δθx, Δθy, Δθz), Δθx, Δθy, Δθz represent the rotation angles around the X, Y, and Z axes respectively;
[0007] Step 2, setting a plurality of feature points in the surrounding space of the photoelectric sensing device, and measuring the measured position of each feature point; observing each feature point through the photoelectric sensing device to obtain the azimuth angle and pitch angle of each feature point observed by the photoelectric sensing device;
[0008] Step 3, design the following objective function:
[0009]
[0010] Where N is the number of feature points; for feature point i, is the azimuth of the photoelectric sensor device relative to the feature point i calculated from a particle in the particle swarm, θ i1 is the azimuth angle observed by the photoelectric sensing device to the feature point i, is the pitch angle of the photoelectric sensor device relative to the feature point i calculated from a particle in the particle swarm, θ i2 is the pitch angle observed by the photoelectric sensing device for feature point i; δ azi is the inherent error of the position measurement device in the longitude and latitude directions, δ pit The error of height measurement equipment in measuring height; r i is the distance between feature point i and the origin of the ideal northeast celestial coordinate system; δ θ1 and δ θ2 They are the inherent errors of the photoelectric sensor equipment in measuring the azimuth and elevation angles respectively;
[0011] Step 4, set the population size N, particle dimension D, number of iterations T, inertia weight w, learning factors c1, c2, and the displacement offset boundary range, angle offset boundary range, and particle velocity boundary range in the particle position boundary;
[0012] Step 5: Initialize the particle position X using a random function according to the particle position boundary size j (x i ,y i ,z i ,θ x ,θ y ,θ z ), j = 1, 2, ..., N, x i ,y i ,z i ,θ x ,θ y ,θ z Respectively represent the displacement offset and angle offset of the x, y, and z axes of the photoelectric sensor device coordinates; initialize the particle velocity V (v1, v2, v3, v4, v5, v6); initialize the optimal position p of each particle j is the initial position of each particle, and the optimal objective function value pbest of each particle j is the initial objective function value of each particle; the optimal position g of the initial entire population is the particle with the smallest objective function value in the initial population, and the optimal objective function value gbest of the entire population is the minimum value of the objective function in the initial population;
[0013] Step 6: Update V and X according to the following formula, and update p j 、pbest j ,g,gbest:
[0014] V(k+1)=
[0015] w*V(k)+c1*rand(0,1)*(pbest(k)-X(k))+c2*rand(0,1)*(gbest(k)-X(k))
[0016] X(k+1)=X(k)+V(k+1)
[0017] In the formula, rand(0,1) represents a random number uniformly distributed between 0 and 1, and k and k+1 represent the number of iterations;
[0018] Step 7, determine whether the number of iterations is greater than T. If not, return to step 5, otherwise terminate the iteration and output g, which is the final estimated value of the pose deviation.
[0019] The beneficial effects of the present invention are:
[0020] 1. In the method of the present invention, the sensor base installation position is preset to a point p1 (x1, y1, z1) in three-dimensional space, and an ideal three-dimensional coordinate system is established with this point as the origin, thereby defining its ideal posture. However, in the actual installation process, it is inevitable that the sensor will be installed at a certain point p2 (x2, y2, z2), and the displacement deviation is denoted as (Δx, Δy, Δz), and its posture will also deviate from the ideal coordinate system defined above, and the angle deviation is denoted as (Δθx, Δθy, Δθz). The present invention collectively refers to these two deviations as posture deviations, and how to estimate these six variables as accurately as possible is the main purpose of this method. To achieve this purpose, the present invention selects several feature points in a certain way in the space around the photoelectric sensor, so that the pitch angle and azimuth angle measurement values (referred to as measurement values) of the photoelectric sensor for these feature points can be obtained. If it is assumed that the posture deviation of the photoelectric sensor is known, the azimuth and pitch angle of the feature point relative to the photoelectric sensor can be calculated through geometric relationships (referred to as calculated values). In theory, the calculated value and the measured value are approximately equal. According to this principle, the idea of optimization can be used to construct an objective function, and the six quantities describing the posture deviation are set as the values to be calculated. The above problem is converted into how to find a set of optimal solutions to minimize the value of the objective function.
[0021] 2. The present invention adopts the particle swarm algorithm when calibrating the posture of the photoelectric sensor device, and uses computer calculations to convert the solution process of the photoelectric device posture deviation estimation into cooperation and competition between individuals in the foraging behavior of bird flocks. It is also based on the concepts of "population" and "evolution" to achieve the search for the optimal solution in a complex space. BRIEF DESCRIPTION OF THE DRAWINGS
[0022] Figure 1It is a flow chart of realizing the method of the present invention through simulation.
[0023] Figure 2 It is the simulation experiment result of the method of the present invention. DETAILED DESCRIPTION
[0024] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments.
[0025] A method for estimating the posture deviation of a photoelectric sensor device based on particle swarm optimization comprises the following steps:
[0026] Step 1, establish an ideal northeast celestial coordinate system with the measured position of the photoelectric sensor device as the origin; the deviation of the actual position and posture of the photoelectric sensor device relative to the ideal northeast celestial coordinate system includes displacement deviation and angle deviation, wherein the displacement deviation is (Δx, Δy, Δz), Δx, Δy, Δz represent the displacement deviation of the X, Y, and Z axes respectively, and the angle deviation is (Δθx, Δθy, Δθz), Δθx, Δθy, Δθz represent the rotation angles around the X, Y, and Z axes respectively;
[0027] Step 2, setting a plurality of feature points in the surrounding space of the photoelectric sensing device, and measuring the measured position of each feature point; observing each feature point through the photoelectric sensing device to obtain the azimuth angle and pitch angle of each feature point observed by the photoelectric sensing device;
[0028] Step 3, design the following objective function:
[0029]
[0030] Where N is the number of feature points; for feature point i, is the azimuth of the photoelectric sensor device relative to the feature point i calculated from a particle in the particle swarm, θ i1 is the azimuth angle observed by the photoelectric sensing device to the feature point i, is the pitch angle of the photoelectric sensor device relative to the feature point i calculated from a particle in the particle swarm, θ i2 is the pitch angle observed by the photoelectric sensing device for feature point i; δ azi is the inherent error of the position measurement device in the longitude and latitude directions, δ pit The error of height measurement equipment in measuring height; r i is the distance between feature point i and the origin of the ideal northeast celestial coordinate system; δ θ1 and δ θ2 They are the inherent errors of the photoelectric sensor equipment in measuring the azimuth and elevation angles respectively;
[0031] Step 4, set the population size N, particle dimension D, number of iterations T, inertia weight w, learning factors c1, c2, and the displacement offset boundary range, angle offset boundary range, and particle velocity boundary range in the particle position boundary;
[0032] Step 5: Initialize the particle position X using a random function according to the particle position boundary size j (x i ,y i ,z i ,θ x ,θ y ,θ z ), j = 1, 2, ..., N, x i ,y i ,z i ,θ x ,θ y ,θ z Respectively represent the displacement offset and angle offset of the x, y, and z axes of the photoelectric sensor device coordinates; initialize the particle velocity V (v1, v2, v3, v4, v5, v6); initialize the optimal position p of each particle j is the initial position of each particle, and the optimal objective function value pbest of each particle j is the initial objective function value of each particle; the optimal position g of the initial entire population is the particle with the smallest objective function value in the initial population, and the optimal objective function value gbest of the entire population is the minimum value of the objective function in the initial population;
[0033] Step 6: Update V and X according to the following formula, and update p j 、pbest j ,g,gbest:
[0034] V(k+1)=w*V(k)+c1*rand(0,1)*(pbest(k)-X(k))+c2*rand(0,1)*(gbest(k)-X(k))X(k+1)=X(k)+V(k+1)
[0035] In the formula, rand(0,1) represents a random number uniformly distributed between 0 and 1, and k and k+1 represent the number of iterations;
[0036] Step 7, determine whether the number of iterations is greater than T. If not, return to step 5, otherwise terminate the iteration and output g, which is the final estimated value of the pose deviation.
[0037] The method of the present invention is verified by simulation as follows:
[0038] like Figure 1, a method for estimating the posture deviation of photoelectric sensor equipment based on particle swarm optimization, which uses the particle swarm algorithm in the heuristic algorithm to estimate the posture deviation of the photoelectric sensor. The specific steps are as follows:
[0039] Step 1, generate simulation data. Establish an ideal northeast sky (ENU) coordinate system with the location where the photoelectric sensor is observed as the origin. In this ideal coordinate system, generate a number of feature points according to a certain distribution method, and take the position of the feature points at this time as the real position of the feature points. Suppose the deviation of the real posture of the photoelectric sensor relative to the above ideal coordinate system is:
[0040] The displacement deviation is: (Δx, Δy, Δz), Δx, Δy, Δz represent the displacement deviation of X, Y, and Z axes respectively;
[0041] The angular deviation is (Δθx, Δθy, Δθz), where Δθx, Δθy, Δθz represent the rotation angles around the X, Y, and Z axes, respectively, namely the pitch angle, roll angle, and yaw angle, respectively.
[0042] Step 2, assume that the error range of the latitude and longitude measured by GPS for the feature point (converted to the X and Y axes in the plane coordinate system) is GPSError_xy meters, and the error range of the elevation measurement device measuring the elevation (converted to the Z axis in the plane coordinate system) is GPSError_z meters. The XYZ coordinates of the real position of the feature point are respectively added with the measurement errors of the measuring device in each dimension as the observed value of the feature point (corresponding to the value of the three-dimensional coordinates of the feature point obtained by GPS and the elevation device in the actual scene). In this example, GPSError_xy is set to 5 meters, GPSError_z is set to 10 meters, and each error is randomly generated within the corresponding range. Then, according to the preset photoelectric sensor posture deviation, the feature point is converted to the real coordinate system of the photoelectric sensor to obtain the coordinates of the feature point in the photoelectric real coordinate system. According to this new coordinate, the azimuth and pitch angles between each feature point and the photoelectric sensor are calculated using the geometric relationship, and the inherent error of the photoelectric sensor angle measurement (set to 0.2 degrees in this example) is added to the azimuth and pitch angles as the azimuth and pitch angles of each feature point observed by the photoelectric sensor in the actual scene.
[0043] Step 3, design the following objective function:
[0044]
[0045] Where N is the number of feature points. For a certain feature point, is the azimuth angle calculated from a particle in the particle swarm, θ i1 is the azimuth angle measured by the photoelectric sensor to the feature point. and θi2 are the calculated pitch angle and the pitch angle measured by the photoelectric sensor respectively. azi is the inherent error of GPS equipment in measuring longitude and latitude, δ pit The error in the height direction measured by the height measurement equipment; i is the distance between the feature point and the origin; δ θ1 and δ θ2 The inherent errors of the photoelectric sensor in measuring azimuth and elevation angles respectively.
[0046] Step 4, set the population size to N = 100, particle dimension D = 6, number of iterations T = 200, inertia weight w = 0.729, learning factor c1 = c2 = 1.494, the displacement offset boundary range in the particle position boundary is (-10, 10), the angle offset boundary range is (-2, 2), and the particle velocity boundary range is (-5, 5).
[0047] Step 5: Initialize the particle position X using a random function according to the particle position boundary size j (x i ,y i ,z i ,θ x ,θ y ,θ z ), j = 1, 2, ..., N; x i ,y i ,z i ,θ x ,θ y ,θ z Respectively represent the displacement offset and angle offset of the x, y, and z axes of the photoelectric sensor coordinates; Similarly, initialize the particle velocity V (v1, v2, v3, v4, v5, v6). Initialize the individual optimal position p j And the optimal objective function value pbest j , let p j =X j , pbest j = The objective function value of particle j in the initial population. Initialize the optimal position g and the optimal objective function value gbest of the population, g = the one with the smallest objective function value in the initial population, gbest = the minimum objective function value in the initial population.
[0048] Step 6: Update V and X according to the following formula, and update p j 、pbest j ,g,gbest:
[0049] V(k+1)=w*V(k)+c1*rand(0,1)*(pbest(k)-X(k))+c2*rand(0,1)*(gbest(k)-X(k))X(k+1)=X(k)+V(k+1)
[0050] In the formula, rand(0,1) represents a random number uniformly distributed between 0 and 1, and k is the number of iterations.
[0051] Step 7, determine whether the number of iterations is greater than T. If not, return to step 5, otherwise terminate the algorithm and output g, which is the optimal prediction value.
[0052] The simulation results are as follows Figure 2 As shown, the displacement deviation is set to [3, 6, 9] and the angle deviation is set to [0.5, 0.9, 1.7]. The estimation effects of the following four different feature point distribution methods are experimentally counted:
[0053] 1. Draw a circle with a radius of 1000 meters on the XY plane, and evenly select 10 points on the circle. The Z axis range is (-100,100). This is called circular uniform distribution.
[0054] 2. Draw a circle with a radius of 1000 meters on the XY plane, evenly select 10 points on the circle, then add random numbers in the range of (0,2000) to both the X and Y values, and finally set the Z axis range to (-100,100). This is called surrounding random distribution.
[0055] 3. Arrange the points in an approximately straight line on the XY plane, and take 10 points at approximately equal intervals. The x range is (-1500, 1500), the y value is about 500, and the z value range is (-100, 100). This is called a straight line distribution.
[0056] 4. Randomly arrange 5 points in the first and second quadrants respectively, for a total of 10 feature points. The x value range of the first quadrant is (500, 1000), the x value range of the second quadrant is (-1000, -500), the y value range is (500, 1000), and the z value range is (-100, 100).
[0057] The following two points can be concluded from the experimental results:
[0058] 1. The experimental results verify the effectiveness of this method. Circular uniform distribution, surrounding random distribution, straight line distribution, and one- and two-quadrant random distribution all show good estimation effects.
[0059] 2. The best effect is achieved when the feature points are evenly distributed in a circle or randomly distributed in a circle, followed by random distribution in the first and second quadrants. The most unfavorable estimation effect is achieved when the feature points are distributed in a straight line.
[0060] In summary, the present invention selects several feature points and obtains the azimuth and pitch angles of these feature points relative to the sensor; designs the objective function; sets hyperparameters such as population size, particle dimension, number of iterations, inertia weight, learning factor, boundary range of particle position and velocity; initializes the population, particle velocity, individual optimal position and optimal objective function value, global optimal position and optimal objective function value; iteratively updates particle velocity and position, and updates the global optimal position and optimal objective function value, individual optimal position and optimal objective function value; determines whether the termination condition of the iteration is met, and outputs the individual corresponding to the optimal objective function value, which is the optimal estimate of the posture deviation of the photoelectric sensing device.
[0061] The invention is simple and easy to implement, can be used for calibrating the position and posture of photoelectric sensing equipment, and has good application effect.
Claims
1. A method for estimating the posture deviation of an optoelectronic sensor device based on particle swarm optimization, characterized in that: The following steps are involved: Step 1, establish an ideal northeast celestial coordinate system with the measured position of the photoelectric sensor device as the origin; the deviation of the actual position and posture of the photoelectric sensor device relative to the ideal northeast celestial coordinate system includes displacement deviation and angle deviation, wherein the displacement deviation is (Δx, Δy, Δz), Δx, Δy, Δz represent the displacement deviation of the X, Y, and Z axes respectively, and the angle deviation is (Δθx, Δθy, Δθz), Δθx, Δθy, Δθz represent the rotation angles around the X, Y, and Z axes respectively; Step 2, setting a plurality of feature points in the surrounding space of the photoelectric sensing device, and measuring the measured position of each feature point; observing each feature point through the photoelectric sensing device to obtain the azimuth angle and pitch angle of each feature point observed by the photoelectric sensing device; Step 3, design the following objective function: Where N is the number of feature points; for feature point i, is the azimuth of the photoelectric sensor device relative to the feature point i calculated from a particle in the particle swarm, θ i1 is the azimuth angle observed by the photoelectric sensing device to the feature point i, is the pitch angle of the photoelectric sensor device relative to the feature point i calculated from a particle in the particle swarm, θ i2 is the pitch angle observed by the photoelectric sensing device for feature point i; δ azi is the inherent error of the position measurement device in the longitude and latitude directions, δ pit The error of height measurement equipment in measuring height; r i is the distance between feature point i and the origin of the ideal northeast celestial coordinate system; δ θ1 and δ θ2 They are the inherent errors of the photoelectric sensor equipment in measuring the azimuth and elevation angles respectively; Step 4, set the population size N, particle dimension D, number of iterations T, inertia weight w, learning factors c1, c2, and the displacement offset boundary range, angle offset boundary range, and particle velocity boundary range in the particle position boundary; Step 5: Initialize the particle position X using a random function according to the particle position boundary size j (x i ,y i ,z i ,θ x ,θ y ,θ z ), j = 1, 2, ..., N, x i ,y i ,z i ,θ x ,θ y ,θ z Respectively represent the displacement offset and angle offset of the x, y, and z axes of the photoelectric sensor device coordinates; initialize the particle velocity V (v1, v2, v3, v4, v5, v6); initialize the optimal position p of each particle j is the initial position of each particle, and the optimal objective function value pbest of each particle j is the initial objective function value of each particle; the optimal position g of the initial entire population is the particle with the smallest objective function value in the initial population, and the optimal objective function value gbest of the entire population is the minimum value of the objective function in the initial population; Step 6: Update V and X according to the following formula, and update p j 、pbest j ,g,gbest: V(k+1)= w*V(k)+c1*rand(0,1)*(pbest(k)-X(k))+c2*rand(0,1)*(gbest(k)-X(k)) X(k+1)=X(k)+V(k+1) In the formula, rand(0,1) represents a random number uniformly distributed between 0 and 1, and k and k+1 represent the number of iterations; Step 7, determine whether the number of iterations is greater than T. If not, return to step 5, otherwise terminate the iteration and output g, which is the final estimated value of the pose deviation.
