WO2022042184A1 - 一种跟踪目标的位置估计方法、装置及无人飞行器 - Google Patents

一种跟踪目标的位置估计方法、装置及无人飞行器 Download PDF

Info

Publication number
WO2022042184A1
WO2022042184A1 PCT/CN2021/108888 CN2021108888W WO2022042184A1 WO 2022042184 A1 WO2022042184 A1 WO 2022042184A1 CN 2021108888 W CN2021108888 W CN 2021108888W WO 2022042184 A1 WO2022042184 A1 WO 2022042184A1
Authority
WO
WIPO (PCT)
Prior art keywords
target
tracking target
position information
estimated
tracking
Prior art date
Application number
PCT/CN2021/108888
Other languages
English (en)
French (fr)
Inventor
黄金鑫
Original Assignee
深圳市道通智能航空技术股份有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Priority claimed from CN202010896152.XA external-priority patent/CN112037260B/zh
Application filed by 深圳市道通智能航空技术股份有限公司 filed Critical 深圳市道通智能航空技术股份有限公司
Publication of WO2022042184A1 publication Critical patent/WO2022042184A1/zh
Priority to US18/173,935 priority Critical patent/US20230215024A1/en

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64U—UNMANNED AERIAL VEHICLES [UAV]; EQUIPMENT THEREFOR
    • B64U20/00—Constructional aspects of UAVs
    • B64U20/80—Arrangement of on-board electronics, e.g. avionics systems or wiring
    • B64U20/87—Mounting of imaging devices, e.g. mounting of gimbals
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/12—Target-seeking control
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/60—Analysis of geometric attributes
    • G06T7/62—Analysis of geometric attributes of area, perimeter, diameter or volume
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64U—UNMANNED AERIAL VEHICLES [UAV]; EQUIPMENT THEREFOR
    • B64U2101/00—UAVs specially adapted for particular uses or applications
    • B64U2101/30—UAVs specially adapted for particular uses or applications for imaging, photography or videography
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64U—UNMANNED AERIAL VEHICLES [UAV]; EQUIPMENT THEREFOR
    • B64U2201/00—UAVs characterised by their flight controls
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G5/00—Traffic control systems for aircraft, e.g. air-traffic control [ATC]
    • G08G5/003—Flight plan management
    • G08G5/0034—Assembly of a flight plan

Definitions

  • Embodiments of the present invention relate to the field of unmanned aerial vehicles, and in particular, to a position estimation method and device for tracking a target, and an unmanned aerial vehicle.
  • the target following system of unmanned aerial vehicle it is usually desirable to achieve real-time following of the target, and the size of the target frame in the image remains basically unchanged during the following process, that is, to maintain a "fixed height and constant distance" between the unmanned aerial vehicle and the target. Therefore, it is particularly important to estimate the position of the target stably.
  • the more accurate target estimation method is the back-projection method, which uses the binocular camera on the UAV to back-project the obtained depth data into the image, and determines the three-dimensional position of the target according to whether the projected point falls in the target frame.
  • this type of method has high accuracy, but requires the target to appear within the field of view of the binocular camera and within the measurement range of the binocular camera. For long-distance tracking, the target is often outside the field of view or measurement range of the binocular camera, which directly leads to the failure of this type of method.
  • Another method uses the altitude information of the aircraft and the pitch angle information of the gimbal to roughly calculate the position of the target. This type of method relies heavily on the ground height information fused by the UAV. Accumulation of the flight time, the accuracy is getting worse and worse, and when the target distance is too far, the pitch angle changes small, can not accurately reflect the moving distance of the target.
  • the inventor found that there are at least the following problems in the related art: when the target distance is relatively far, the position estimation accuracy of the target is insufficient, and the target position cannot be estimated stably.
  • the purpose of the embodiments of the present invention is to provide a position estimation method, device and unmanned aerial vehicle for tracking a target, so as to solve the problem that the position estimation accuracy of the tracking target is insufficient and the position of the tracking target cannot be estimated stably when the target distance is long. , to achieve stable estimation of the position of the tracking target.
  • an embodiment of the present invention provides a method for estimating a position of a tracking target, which is applied to an unmanned aerial vehicle, where the unmanned aerial vehicle includes a gimbal camera, and the gimbal camera is used to acquire images, and the method includes:
  • the estimated target position information determine the estimated width and estimated height of the tracking target in the image
  • the estimated width and estimated height and the actual height and actual width determine an error term, and the error term includes a height difference and a width difference;
  • the target position information of the tracking target at the next moment is updated.
  • determining the estimated width and estimated height of the tracking target in the image according to the estimated target position information includes:
  • the tracking target is modeled, a model corresponding to the tracking target is determined, and a target frame of the tracking target in the image is determined according to the model corresponding to the tracking target.
  • the modeling of the tracking target, determining a model corresponding to the tracking target, and determining a target frame of the tracking target in the image according to the model corresponding to the tracking target includes:
  • determining the estimated width and estimated height of the tracking target in the image according to the estimated target position information includes:
  • the estimated target position information determine the camera position information of the tracking target in the camera coordinate system, wherein the estimated target position information is the world position information in the world coordinate system;
  • the pixel position information of the tracking target in the pixel coordinate system is determined.
  • determining the camera position information of the tracking target in the camera coordinate system according to the estimated target position information includes:
  • determining the pixel position information of the tracking target in the pixel coordinate system according to the camera position information of the tracking target in the camera coordinate system includes:
  • K intr is the internal parameter matrix of the gimbal camera
  • P cam is the three-dimensional coordinate of the tracking target in the camera coordinate system
  • p z is the three-dimensional coordinate of the tracking target in the world coordinate system.
  • the internal parameter matrix of the gimbal camera is the focal length of the gimbal camera, and (c x , c y ) is the coordinate of the main point.
  • the determining the estimated width and estimated height of the tracking target in the image includes:
  • the estimated height and estimated width of the target frame in the image are respectively determined.
  • the determining the estimated height of the target frame in the image includes:
  • the highest point of the target frame of the tracking target in the image in the direction of the vertical axis is a
  • the lowest point is b
  • the highest point a corresponds to the tangent point of the sphere
  • the lowest point b corresponds to the
  • the tangent point of the sphere is B
  • the pixel coordinates of the tangent point A in the pixel coordinate system are (u a , v a )
  • the pixel coordinates of the tangent point B in the pixel coordinate system are (u b , v b )
  • the height difference between the highest point a and the lowest point b is height
  • K intr is the internal parameter matrix of the gimbal camera
  • R is the radius of the sphere
  • z is the Z-axis coordinate of the tracking target in the camera coordinate system.
  • the determining the estimated width of the target frame in the image includes:
  • the leftmost point of the target frame of the tracking target in the image on the horizontal axis is c
  • the rightmost point is d
  • the tangent point of the leftmost point c corresponding to the sphere is C
  • the rightmost point The tangent point of d corresponding to the sphere is D
  • the pixel coordinates of the tangent point C in the pixel coordinate system are (u c , v c )
  • the pixel coordinates of the tangent point B in the pixel coordinate system are (u d ,v d )
  • the width difference between the leftmost point c and the rightmost point d is width
  • the estimated width of the target frame in the image is Among them, K intr is the internal parameter matrix of the gimbal camera, R is the radius of the sphere, and z is the Z-axis coordinate of the tracking target in the camera coordinate system.
  • updating the target position information of the tracking target at the next moment according to the error term includes:
  • the target position information of the tracking target at the next moment is updated based on the extended Kalman filter algorithm.
  • updating the target position information of the tracking target at the next moment including:
  • a first Jacobian matrix of the estimated height to the state vector of the tracking target and a second Jacobian matrix of the estimated width to the state vector of the tracking target are determined, wherein, the state vector is the transpose matrix of the estimated target position information;
  • the target position information of the tracking target at the next moment is updated.
  • the first Jacobian matrix is The second Jacobian matrix is Among them, K intr is the internal parameter matrix of the gimbal camera, R is the radius of the sphere, and z is the Z-axis coordinate of the tracking target in the camera coordinate system.
  • the extended Kalman filter algorithm includes a prediction stage and an update stage, and according to the first Jacobian matrix and the second Jacobian matrix, based on the extended Kalman filter algorithm, the tracking target is updated in the following The target location information at a moment, including:
  • the state vector corresponding to the initial position information of the tracking target at the current moment is used as the estimated value of the extended Kalman filter, and according to the prediction equation, the predicted value of the target position information of the tracking target at the next moment is estimated,
  • the prediction equation includes a state vector equation and a covariance equation
  • the prediction equation is updated, the updated prediction equation is determined, and the updated value is obtained;
  • the target position information of the tracking target at the next moment is updated.
  • the prediction equation is:
  • x(k+1) is the predicted value of the state vector corresponding to the position of the tracking target at the next moment
  • x(k) is the estimated value of the state vector corresponding to the position of the tracking target at the current moment
  • F is the system state transition matrix
  • P(k+1) is the covariance prediction value
  • P(k) is the covariance estimation value
  • F T is the transpose matrix of the system state transition matrix
  • Q is the noise matrix.
  • the calculating the Kalman gain according to the first Jacobian matrix and the second Jacobian matrix includes:
  • Kalman gain K P(k+1)J T (HP(k+1)H T +R) -1 , where P(k+1) is the covariance prediction value, and J T is the first Jacobian matrix Or the transpose matrix of the second Jacobian matrix, H is the state-to-observation transition matrix, P(k+1) is the covariance prediction value, H T is the state-to-observation transition matrix, and R is the measurement noise variance.
  • updating the prediction equation according to the Kalman gain, the measured value and the predicted value, and determining the updated prediction equation includes:
  • the updated prediction equation is: Among them, x(k+1) is the predicted value of the state vector corresponding to the target position information of the tracking target at the next moment, K is the Kalman gain, z measurement is the measurement value of the height or width of the target frame, and z prediction is the first The predicted value of the height of the target frame represented by the Jacobian matrix J h or the predicted value of the width of the target frame represented by the second Jacobian matrix J w , P(k+1) is the predicted value of covariance, and H is the state-to-observation transformation matrix.
  • updating the target position information of the tracking target at the next moment according to the updated prediction equation includes:
  • the update value obtained in the update stage is substituted into the prediction equation of the prediction stage, and the target position information of the tracking target at the next moment is updated.
  • the method further includes:
  • an embodiment of the present invention provides a position estimation device for tracking a target, which is applied to an unmanned aerial vehicle, where the unmanned aerial vehicle includes a gimbal camera, and the gimbal camera is used to acquire images, and the device includes:
  • a target position information estimation unit used for estimating the target position information of the tracking target at the next moment according to the initial position information of the given tracking target at the current moment;
  • an estimation unit for determining the estimated width and estimated height of the tracking target in the image according to the estimated target position information
  • the actual unit is used to obtain the actual width and height of the tracking target in the image
  • an error term unit configured to determine an error term according to the estimated width and estimated height and the actual height and actual width, and the error term includes a height difference and a width difference;
  • the updating unit is configured to update the target position information of the tracking target at the next moment according to the error term.
  • the estimation unit is specifically used for:
  • the tracking target is modeled, a model corresponding to the tracking target is determined, and a target frame of the tracking target in the image is determined according to the model corresponding to the tracking target.
  • the estimation unit is specifically used for:
  • the estimation unit includes:
  • a camera position module configured to determine the camera position information of the tracking target in the camera coordinate system according to the estimated target position information, wherein the estimated target position information is the world position information in the world coordinate system;
  • the pixel position module is configured to determine the pixel position information of the tracking target in the pixel coordinate system according to the camera position information of the tracking target in the camera coordinate system.
  • the camera position module is specifically used for:
  • the pixel location module is specifically used for:
  • K intr is the internal parameter matrix of the gimbal camera
  • P cam is the three-dimensional coordinate of the tracking target in the camera coordinate system
  • p z is the three-dimensional coordinate of the tracking target in the world coordinate system.
  • the internal parameter matrix of the gimbal camera is the focal length of the gimbal camera, and (c x , c y ) is the coordinate of the main point.
  • the estimation unit includes:
  • an estimated height module for determining the estimated height of the target frame in the image according to the highest point and the lowest point of the target frame in the direction of the vertical axis
  • the estimated width unit is configured to determine the estimated width of the target frame in the image according to the leftmost point and the rightmost point of the target frame in the horizontal axis direction.
  • the estimating height module is specifically used for:
  • the highest point of the target frame of the tracking target in the image in the direction of the vertical axis is a
  • the lowest point is b
  • the highest point a corresponds to the tangent point of the sphere
  • the lowest point b corresponds to the
  • the tangent point of the sphere is B
  • the pixel coordinates of the tangent point A in the pixel coordinate system are (u a , v a )
  • the pixel coordinates of the tangent point B in the pixel coordinate system are (u b , v b )
  • the height difference between the highest point a and the lowest point b is height
  • K intr is the internal parameter matrix of the gimbal camera
  • R is the radius of the sphere
  • z is the Z-axis coordinate of the tracking target in the camera coordinate system.
  • the estimated width unit is specifically used for:
  • the leftmost point of the target frame of the tracking target in the image on the horizontal axis is c
  • the rightmost point is d
  • the tangent point of the leftmost point c corresponding to the sphere is C
  • the rightmost point The tangent point of d corresponding to the sphere is D
  • the pixel coordinates of the tangent point C in the pixel coordinate system are (u c , v c )
  • the pixel coordinates of the tangent point B in the pixel coordinate system are (u d ,v d )
  • the width difference between the leftmost point c and the rightmost point d is width
  • the estimated width of the target frame in the image is Among them, K intr is the internal parameter matrix of the gimbal camera, R is the radius of the sphere, and z is the Z-axis coordinate of the tracking target in the camera coordinate system.
  • the updating unit is specifically used for:
  • the target position information of the tracking target at the next moment is updated based on the extended Kalman filter algorithm.
  • the updating unit is specifically used for:
  • a first Jacobian matrix of the estimated height to the state vector of the tracking target and a second Jacobian matrix of the estimated width to the state vector of the tracking target are determined, wherein, the state vector is the transpose matrix of the estimated target position information;
  • the target position information of the tracking target at the next moment is updated.
  • the first Jacobian matrix is The second Jacobian matrix is Among them, K intr is the internal parameter matrix of the gimbal camera, R is the radius of the sphere, and z is the Z-axis coordinate of the tracking target in the camera coordinate system.
  • the extended Kalman filter algorithm includes a prediction stage and an update stage, and the update unit is specifically used for:
  • the state vector corresponding to the initial position information of the tracking target at the current moment is used as the estimated value of the extended Kalman filter, and according to the prediction equation, the predicted value of the target position information of the tracking target at the next moment is estimated,
  • the prediction equation includes a state vector equation and a covariance equation
  • the prediction equation is updated, the updated prediction equation is determined, and the updated value is obtained;
  • the target position information of the tracking target at the next moment is updated.
  • the prediction equation is:
  • x(k+1) is the predicted value of the state vector corresponding to the position of the tracking target at the next moment
  • x(k) is the estimated value of the state vector corresponding to the position of the tracking target at the current moment
  • F is the system state transition matrix
  • P(k+1) is the covariance prediction value
  • P(k) is the covariance estimation value
  • F T is the transpose matrix of the system state transition matrix
  • Q is the noise matrix.
  • the Kalman gain K P(k+1)J T (HP(k+1)H T +R)-1, where Pk+1 is the covariance prediction value, and JT is the first The transpose matrix of the first Jacobian matrix or the second Jacobian matrix, H is the state-to-observation transition matrix, P(k+1) is the covariance prediction value, H T is the state-to-observation transition matrix of the transition matrix, R is the measurement noise variance.
  • the updated prediction equation is: Among them, x(k+1) is the predicted value of the state vector corresponding to the target position information of the tracking target at the next moment, K is the Kalman gain, z measurement is the measurement value of the height or width of the target frame, and z prediction is the first The predicted value of the height of the target frame represented by the Jacobian matrix J h or the predicted value of the width of the target frame represented by the second Jacobian matrix J w , P(k+1) is the predicted value of covariance, and H is the state-to-observation transformation matrix.
  • the updating unit is specifically used for:
  • the update value obtained in the update stage is substituted into the prediction equation of the prediction stage, and the target position information of the tracking target at the next moment is updated.
  • the apparatus further includes:
  • the iterative unit is configured to use the updated position information as estimated target position information, and iteratively update the target position information of the tracking target at the next moment.
  • an embodiment of the present invention provides an unmanned aerial vehicle.
  • the unmanned aerial vehicle includes a fuselage, an arm connected to the fuselage, a power system arranged on the arm, and a fuselage arranged on the fuselage.
  • the tracking system wherein, the tracking system includes a controller, and the controller includes:
  • the memory stores instructions executable by the at least one processor, the instructions being executed by the at least one processor to enable the at least one processor to perform the above-described method for estimating a position of a tracking target.
  • a non-volatile computer-readable storage medium storing computer-executable instructions, when the computer-executable instructions are executed by an unmanned aerial vehicle, causes the unmanned aerial vehicle to The position estimation method of the tracking target as described above is performed.
  • the beneficial effects of the embodiments of the present invention are: by providing a method for estimating a position of a tracking target, which is applied to an unmanned aerial vehicle, the unmanned aerial vehicle includes a gimbal camera, and the gimbal camera is used to acquire images, and the method includes : According to the initial position information of the given tracking target at the current moment, estimate the target position information of the tracking target at the next moment; According to the estimated target position information, determine the estimated width and estimated height of the tracking target in the image; Obtain the actual width and actual height of the tracking target in the image; determine an error term according to the estimated width and estimated height and the actual height and actual width, and the error term includes a height difference and a width difference; according to the error term , and update the target position information of the tracking target at the next moment.
  • the embodiment of the present invention can realize stable estimation of the position of the tracking target.
  • FIG. 1 is a schematic diagram of an application scenario provided by an embodiment of the present invention.
  • FIG. 2 is a schematic structural diagram of an unmanned aerial vehicle provided by an embodiment of the present invention.
  • Figure 3a is a schematic diagram of the correspondence between a model and an image provided by an embodiment of the present invention.
  • 3b is a schematic diagram of a corresponding relationship between another model and an image provided by an embodiment of the present invention.
  • FIG. 4 is a schematic flowchart of a method for estimating a position of a tracking target according to an embodiment of the present invention
  • Fig. 5 is the refinement flow chart of step S20 in Fig. 4;
  • Fig. 6 is the refinement flow chart of step S50 in Fig. 4;
  • Fig. 7 is the refinement flow chart of step S52 in Fig. 6;
  • FIG. 8 is a schematic structural diagram of an apparatus for estimating a position of a tracking target according to an embodiment of the present invention.
  • FIG. 9 is a schematic diagram of a hardware structure of a controller of a tracking system for an unmanned aerial vehicle provided by an embodiment of the present invention.
  • the method for estimating the position of the tracking target provided by the embodiment of the present invention can be applied to the application scenario shown in FIG. 1 .
  • the application scenario shown in FIG. 1 includes the unmanned aerial vehicle 100 and the tracking target 300 .
  • the unmanned aerial vehicle 100 may be used to track the tracking target 300 .
  • an obstacle 400 may be encountered.
  • the UAV 100 needs to avoid the obstacle 400 while tracking the tracking target 300 to achieve normal flight.
  • UAV 100 may be a suitable UAV, including fixed-wing UAVs and rotary-wing UAVs, such as helicopters, quadcopters, and aircraft having other numbers of rotors and/or rotor configurations.
  • the unmanned aerial vehicle 100 may also be other movable objects, such as manned aircraft, model aircraft, unmanned airships, unmanned hot air balloons, and robots.
  • the tracking target 300 can be any suitable movable or immovable object, including vehicles, people, animals, buildings, mountains, rivers, and the like. Obstacles 400 such as buildings, mountains, trees, forests, signal towers, or other movable or immovable objects (only one obstacle is shown in Figure 1, there may be more obstacles or no obstacles in practical applications) .
  • FIG. 2 is a schematic structural diagram of an unmanned aerial vehicle provided by an embodiment of the present invention.
  • the unmanned aerial vehicle 100 includes a fuselage 10, an arm (not shown in the figure) connected to the fuselage 10, a power system (not shown in the figure) provided on the arm, and a device The control system in the fuselage 10 .
  • the power system is used to provide thrust, lift, etc. for the flight of the UAV 100
  • the control system is the central nervous system of the UAV 100, and may include multiple functional units, such as the flight control system 20, the tracking system 30, the path planning system 50, Vision system 40 and other systems with specific functions.
  • Both the tracking system 30 and the vision system 40 include a camera device and a control chip, wherein the camera device is a pan-tilt camera, and the tracking system 30 is used to obtain the status of the tracking target and the tracking distance (that is, the distance between the UAV 100 and the tracking target). ), etc., the vision system 40 is used to provide a map of the environment or a raw point cloud of the surrounding environment of the UAV, etc.
  • the flight control system 20 includes various sensors (eg, gyroscopes, accelerometers, etc.), and the flight control system 20 is used to obtain the real-time position of the UAV and control the flight attitude of the UAV.
  • the path planning system 50 is used for planning the path, and instructing the flight control system 20 to control the flight attitude of the UAV 100 to make the UAV 100 fly according to the designated path.
  • the flight control system 20 and the path planning system 50 may be arranged inside the fuselage 10 , and the tracking system 30 and the vision system 40 may be arranged outside the fuselage 10 and fixed on the fuselage 10 .
  • the camera device of the tracking system 30 and the vision system 40 can form a monocular or binocular vision system, wherein the camera device can be a high-definition digital camera or other camera device, and the camera device can be set in any suitable position that is conducive to shooting, in some embodiments Among them, the camera device of the tracking system 30 is installed on the bottom of the fuselage 10 through the pan/tilt, and the camera device of the vision system 40 is arranged on the front and/or lower part of the fuselage 10 . Wherein, each system may be set up separately, and in some embodiments, some or all of the above-mentioned systems may also be integrated into one or more than one device.
  • the UAV 100 tracks the tracking target according to the target characteristics, wherein, in some embodiments, the target characteristics are stored in the UAV 100 in advance, and in some embodiments, the target characteristics are obtained through other way to obtain.
  • the electronic device 200 is also included, and the target feature can be sent to the unmanned aerial vehicle 100 through the electronic device 200 .
  • the electronic device 200 can display a picture taken by the unmanned aerial vehicle 100, and the user makes a frame selection on the target in the picture. After the target image selected by the user is uploaded to the unmanned aerial vehicle 100, the unmanned aerial vehicle 100 The target image extracts target features. Between the UAV 100 and the electronic device 200 , a communication connection can be established through wireless communication modules (eg, signal receivers, signal transmitters, etc.) respectively provided in the respective interiors, and data/commands can be uploaded or issued.
  • the electronic device 200 is, for example, a smart phone, a tablet computer, a computer, a remote control, and the like.
  • the UAV 100 needs to plan an appropriate path according to the position of the tracking target, and fly according to the planned path to avoid obstacles while maintaining tracking. Therefore, the estimation of the target position is crucial.
  • the embodiment of the present invention uses various methods to obtain the correlation value of the target position, uses the correlation value of the target position as the measurement value of the extended Kalman filter, and uses the extended Kalman filter to perform recursive estimation to obtain the estimated value of the target position.
  • the model corresponding to the tracking target is determined by modeling the tracking target, wherein the embodiment of the present invention is described by taking that the tracking target is modeled as a sphere as an example.
  • FIG. 3a is a schematic diagram of a corresponding relationship between a model and an image provided by an embodiment of the present invention
  • FIG. 3b is a schematic diagram of a corresponding relationship between another model and an image provided by an embodiment of the present invention
  • the radius R of the sphere corresponding to the tracking target modeling is determined.
  • the radius of the sphere is determined as: Among them, a is the length of the tracking target, b is the width of the tracking target, c is the height of the tracking target, and R is the radius of the sphere corresponding to the tracking target.
  • the length, width and height of the tracking target do not need to be very precise, and the shape and size of the object can be roughly expressed.
  • the tracking target is a general car
  • the general size of 4.7*1.5*1.8 can be used.
  • the image is the PTZ image obtained by the PTZ camera
  • the tracking target is modeled as a sphere, as follows: As shown in Figure 3a, it is assumed that the highest point of the tracking target in the longitudinal direction of the image is a, the lowest point is b, and the highest point is a corresponding to the tangent point A of the sphere, and the lowest point b corresponds to the tangent point B of the sphere.
  • connection line OA between the tangent point A and the target center O of the tracking target is perpendicular to the connection line OG between the optical center G (ie the optical center G) of the gimbal camera and the target center O
  • connection line OB between the tangent point B and the target center O of the tracking target is also perpendicular to the connection line OG between the optical center G of the gimbal camera and the target center O, and it is approximately considered that the connection line between the tangent points A and B passes through the target center O.
  • connection line OC between the tangent point C and the target center O of the tracking target is perpendicular to the connection line OG between the optical center G of the gimbal camera and the target center O
  • connection line OD between D and the target center O of the tracking target is perpendicular to the connection line OG between the optical center G of the gimbal camera and the target center O, and it is approximately considered that the connection line between the tangent point C and the tangent point D passes through the target center O.
  • the embodiment of the present invention can estimate the position of the tracking target based on the modeled sphere.
  • FIG. 4 is a schematic flowchart of a method for estimating a position of a tracking target provided by an embodiment of the present invention; wherein, the method for estimating the position of a tracking target is applied to an unmanned aerial vehicle, and the unmanned aerial vehicle includes a gimbal A camera, the pan-tilt camera is used to acquire images.
  • the method for estimating the position of the tracking target includes: Step S10: according to the initial position information of the given tracking target at the current moment, estimate the target position information of the tracking target at the next moment;
  • the current target position of the tracking target is unknown.
  • the initial position information needs to be given as the input of the extended Kalman filter algorithm to converge to the actual position of the tracking target,
  • the initial position information of the tracking target at the current moment is given by the processor of the UAV, and the initial position information is estimated by the UAV according to the actual situation of the tracking target.
  • the initial position of , and the estimated position is used as the initial position information of the tracking target at the current position, which can improve the convergence speed compared with random setting.
  • the initial position information of the tracking target at the current moment is given, the initial position information is used as the current world position information of the tracking target, and then the next position is estimated according to the current position, and the next position becomes the current position again The iterative process until the error converges to less than the preset threshold, and the iterative process is stopped.
  • the initial position information is used as the input of the extended Kalman filter (extended kalman filter, EKF) algorithm
  • the target position information of the tracking target at the next moment is estimated by the extended Kalman filter algorithm
  • the estimated target position information is obtained , wherein the estimated target position information is world coordinate information.
  • the extended Kalman filter algorithm includes a prediction stage and an update stage.
  • the core idea of the extended Kalman filter algorithm is prediction + measurement feedback, which consists of two parts, the first part is the state prediction equation, and the second part is the observation equation.
  • the prediction stage the predicted value calculated by the system state equation is used as a priori information, and then this information is re-updated in the observation part.
  • the prediction equations include the state vector equation and the covariance equation, which are:
  • x(k+1) is the predicted value of the state vector corresponding to the position of the tracking target at the next moment
  • x(k) is the measured value of the state vector corresponding to the position of the tracking target at the current moment
  • F is the system state transition matrix
  • p(k+1) is the covariance prediction value
  • P(k) is the covariance measurement value
  • F T is the transpose matrix of the system state transition matrix
  • Q is the noise matrix.
  • system state transition matrix F may be
  • Step S20 According to the estimated target position information, determine the estimated width and estimated height of the tracking target in the image;
  • the image is a PTZ image obtained by a PTZ camera
  • determining the estimated width and estimated height of the tracking target in the image according to the estimated target position information includes: constructing the tracking target. model, determine the model corresponding to the tracking target, and determine the target frame of the tracking target in the image according to the model corresponding to the tracking target.
  • the modeling of the tracking target, determining the model corresponding to the tracking target, and determining the target frame of the tracking target in the image according to the model corresponding to the tracking target including:
  • the tracking target is modeled as a sphere, the average value of the length, width and height of the tracking target is calculated, and the radius R of the sphere is determined as the average value of the length, width and height, that is, Among them, a is the length of the tracking target, b is the width of the tracking target, c is the height of the tracking target, and R is the radius of the sphere corresponding to the tracking target.
  • a target frame corresponding to the sphere corresponding to the tracking target in the image is determined. Specifically, the sphere corresponding to the tracking target is projected into the image to determine the target frame corresponding to the sphere.
  • the sphere corresponding to the tracking target is projected into the image
  • the sphere corresponding to the tracking target is projected from a three-dimensional shape to a two-dimensional shape in the image
  • the target frame is a circular frame.
  • the position of the target frame in the image can be determined according to the position and radius of the center of the circular frame.
  • the determining the target frame corresponding to the sphere includes: determining the center position and radius of the target frame, and determining the target frame corresponding to the sphere in the image according to the center position and radius of the target frame. location information in .
  • the highest point of the tracking target in the vertical axis direction of the image is a
  • the lowest point is b
  • the highest point a corresponds to the tangent point A of the sphere
  • the lowest point b corresponds to the tangent point B of the sphere.
  • connection line OA between the tangent point A and the target center O of the tracking target is perpendicular to the connection line OG between the optical center G (ie the optical center G) of the gimbal camera and the target center O
  • connection line OB between the tangent point B and the target center O of the tracking target is also perpendicular to the connection line OG between the optical center G of the gimbal camera and the target center O, and it is approximately considered that the connection line between the tangent points A and B passes through the target center O.
  • connection line OC between the tangent point C and the target center O of the tracking target is perpendicular to the connection line OG between the optical center G of the gimbal camera and the target center O
  • connection line OD between D and the target center O of the tracking target is perpendicular to the connection line OG between the optical center G of the gimbal camera and the target center O, and it is approximately considered that the connection line between the tangent point C and the tangent point D passes through the target center O.
  • FIG. 5 is a detailed flowchart of step S20 in FIG. 4 ;
  • the step S20 determining the estimated width and estimated height of the tracking target in the image according to the estimated target position information, including:
  • Step S21 Determine the camera position information of the tracking target in the camera coordinate system according to the estimated target position information, wherein the estimated target position information is the world position information in the world coordinate system;
  • the coordinates of the tangent points A and B in the camera coordinate system are respectively expressed as:
  • P cam is the coordinate of the target center in the camera coordinate system
  • P cam,A is the coordinate of the tangent point A in the camera coordinate system
  • P cam,B is the coordinate of the tangent point B in the camera coordinate system
  • P cam is The coordinates of the target center O in the camera coordinate system
  • R 1 (0,R,0) T
  • R is the radius of the sphere.
  • the tangent point A corresponds to the highest point a, and its pixel coordinates in the image are (u a , v a )
  • the tangent point B corresponds to the lowest point b, and its pixel coordinates in the image are (u b , v b ).
  • P cam,C is the coordinate of the tangent point C in the camera coordinate system
  • P cam,D is the coordinate of the tangent point D in the camera coordinate system
  • P cam is the coordinate of the target center O in the camera coordinate system
  • P 2 [R,0,0]
  • R is the radius of the sphere.
  • the tangent point C corresponds to the leftmost point c, and its pixel coordinates in the image are (u c , v c )
  • the tangent point D corresponds to the rightmost point d, and its pixel coordinates in the image are (u d , v d ) ).
  • Step S22 Determine the pixel position information of the tracking target in the pixel coordinate system according to the camera position information of the tracking target in the camera coordinate system.
  • K intr is the internal parameter matrix of the gimbal camera
  • P cam is the three-dimensional coordinate of the tracking target in the camera coordinate system
  • p z is the three-dimensional coordinate of the tracking target in the world coordinate system. It can be understood that P cam is the tracking target.
  • the three-dimensional coordinates of the target center O in the camera coordinate system, p z is the three-dimensional coordinates of the estimated target position information in the world coordinate system.
  • (f x , f y ) are the focal lengths
  • (c x , c y ) are the coordinates of the principal point, where both the focal length and the principal point coordinates can be obtained through the calibration of the PTZ camera, and the conversion relationship is converted into a matrix form:
  • K intr is the internal parameter matrix of the gimbal camera
  • P cam is the three-dimensional coordinate of the tracking target in the camera coordinate system
  • p z is the three-dimensional coordinate of the tracking target in the world coordinate system.
  • the internal parameter matrix of the PTZ camera Among them, (f x , f y ) is the focal length of the gimbal camera, and (c x , c y ) is the coordinate of the main point.
  • the determining the estimated width and estimated height of the tracking target in the image includes:
  • the estimated height and estimated width of the target frame in the image are respectively determined.
  • the determining the estimated height of the target frame in the image includes:
  • the highest point of the target frame of the tracking target in the image in the direction of the vertical axis is a
  • the lowest point is b
  • the highest point a corresponds to the tangent point of the sphere
  • the lowest point b corresponds to the
  • the tangent point of the sphere is B
  • the pixel coordinates of the tangent point A in the pixel coordinate system are (u a , v a )
  • the pixel coordinates of the tangent point B in the pixel coordinate system are (u b , v b )
  • the height difference between the highest point a and the lowest point b is height
  • K intr is the internal parameter matrix of the gimbal camera
  • R is the radius of the sphere
  • z is the Z-axis coordinate of the tracking target in the camera coordinate system.
  • the tangent points A, B, C, and D in the camera coordinate system determine the highest point and the lowest point of the target frame in the vertical axis direction of the image and the leftmost point and the lowest point in the horizontal axis direction of the image.
  • determining the estimated height of the target frame in the image is equivalent to determining the distance between the highest point a corresponding to the tangent point A and the lowest point b corresponding to the tangent point B in the image.
  • the distance of point B in the image, that is, the estimated height height can be expressed as:
  • K intr is the internal parameter matrix of the gimbal camera
  • P cam,B is the three-dimensional coordinate of the tangent point B in the camera coordinate system
  • p z,B is the three-dimensional coordinate of the tangent point B in the world coordinate system
  • P cam,A is the three-dimensional coordinate of the tangent point A in the camera coordinate system
  • p z,A is the three-dimensional coordinate of the tangent point A in the world coordinate system.
  • the internal parameter matrix K intr of the pan-tilt camera is calibrated in advance by Obtained, for example, by camera calibration techniques.
  • K intr is the internal parameter matrix of the gimbal camera
  • R is the radius of the sphere
  • z is the Z-axis coordinate of the tracking target in the camera coordinate system.
  • the determining the estimated width of the target frame in the image includes:
  • the leftmost point of the target frame of the tracking target in the image on the horizontal axis is c
  • the rightmost point is d
  • the tangent point of the leftmost point c corresponding to the sphere is C
  • the rightmost point The tangent point of d corresponding to the sphere is D
  • the pixel coordinates of the tangent point C in the pixel coordinate system are (u c , v c )
  • the pixel coordinates of the tangent point B in the pixel coordinate system are (u d ,v d )
  • the width difference between the leftmost point c and the rightmost point d is width
  • the estimated width of the target frame in the image is Among them, K intr is the internal parameter matrix of the gimbal camera, R is the radius of the sphere, and z is the Z-axis coordinate of the tracking target in the camera coordinate system.
  • determining the estimated width of the target frame in the image is equivalent to determining the distance between the leftmost point c corresponding to the tangent point C and the rightmost point d corresponding to the tangent point D in the image.
  • the tangent point The distance between C and the tangent point D in the image, that is, the estimated width width can be expressed as:
  • K intr is the internal parameter matrix of the gimbal camera
  • P cam is the three-dimensional coordinate of the tangent point D in the camera coordinate system
  • P cam is the three-dimensional coordinate of the tangent point C in the camera coordinate system
  • z is the tracking target.
  • Step S30 obtaining the actual width and actual height of the tracking target in the image
  • the actual width and actual height of the tracking target in the image are obtained by an external module, for example, the tracking module obtains the target frame of the tracking target in the image, and measures the actual width and actual height of the target frame.
  • Step S40 Determine an error term according to the estimated width and estimated height and the actual height and actual width, and the error term includes a height difference and a width difference;
  • Step S50 According to the error term, based on the extended Kalman filter algorithm, update the target position information of the tracking target at the next moment.
  • the extended Kalman filter (extended kalman filter, EKF) is an algorithm that uses the linear system state equation to optimally estimate the system state through the system input and output observation data.
  • FIG. 6 is a detailed flowchart of step S50 in FIG. 5;
  • step S50 according to the error term, update the target position information of the tracking target at the next moment, including:
  • Step S51 According to the estimated height and estimated width of the tracking target in the image, determine a first Jacobian matrix of the estimated height to the state vector of the tracking target and a second Jacobian matrix of the estimated width to the state vector of the tracking target. ratio matrix, wherein the state vector is the transpose matrix of the estimated target position information;
  • the estimated height of the tracking target in the image is
  • the state vector of the tracking target is the transpose matrix of the world position information of the tracking target, that is, the state vector corresponding to the estimated target position information of the tracking target.
  • the first Jacobian matrix of the estimated height to the state vector of the tracking target is:
  • K intr is the internal parameter matrix of the gimbal camera
  • R is the radius of the sphere
  • z is the Z-axis coordinate of the tracking target in the camera coordinate system
  • the width of the tracking target in the image is
  • the state vector of the tracking target is the transpose matrix of the world position information of the tracking target, that is, the state vector corresponding to the estimated target position information of the tracking target.
  • the second Jacobian matrix of the estimated width to the state vector of the tracking target is:
  • K intr is the internal parameter matrix of the gimbal camera
  • R is the radius of the sphere
  • z is the Z-axis coordinate of the tracking target in the camera coordinate system
  • Step S52 According to the first Jacobian matrix and the second Jacobian matrix, based on the extended Kalman filter algorithm, update the target position information of the tracking target at the next moment.
  • FIG. 7 is a detailed flowchart of step S52 in FIG. 6 ;
  • this step S52 According to the first Jacobian matrix and the second Jacobian matrix, based on the extended Kalman filter algorithm, update the target position information of the tracking target at the next moment, including:
  • Step S521 in the prediction stage, take the state vector corresponding to the initial position information of the tracking target at the current moment as the estimated value of the extended Kalman filter, and estimate the target position information of the tracking target at the next moment according to the prediction equation.
  • a predicted value wherein the prediction equation includes a state vector equation and a covariance equation;
  • the extended Kalman filter algorithm includes a prediction stage and an update stage.
  • the core idea of the extended Kalman filter algorithm is prediction + measurement feedback, which consists of two parts, the first part is the state prediction equation, and the second part is the observation equation .
  • the prediction stage the predicted value calculated by the system state equation is used as a priori information, and then this information is re-updated in the observation part.
  • the prediction equations include the state vector equation and the covariance equation, which are:
  • x(k+1) is the predicted value of the state vector corresponding to the position of the tracking target at the next moment, that is, the estimated target position information of the tracking target at the next moment
  • x(k) is the position corresponding to the tracking target at the current moment.
  • the measured value of the state vector F is the system state transition matrix
  • P(k+1) is the covariance prediction value
  • P(k) is the covariance measurement value
  • F T is the transpose matrix of the system state transition matrix
  • Q noise matrix is the measured value of the state vector.
  • system state transition matrix F may be
  • Step S522 in the update phase, calculate the Kalman gain according to the first Jacobian matrix and the second Jacobian matrix;
  • the Kalman gain K P(k+1)J T (HP(k+1)H T +R) -1 , where P(k+1) is the covariance prediction value, and J T is the The transpose matrix of the first Jacobian matrix and the second Jacobian matrix, H is the state-to-observation transition matrix, P(k+1) is the covariance prediction value, H T is the state-to-observation transition matrix of the transition matrix , R is the measurement noise variance.
  • the J T is the transpose matrix of the first Jacobian matrix or the second Jacobian matrix.
  • the J T is the first Jacobian matrix J h , that is, The Kalman gain calculated at this time is the first Kalman gain.
  • the first Kalman gain is determined according to the first Jacobian matrix, which specifically includes:
  • the first Kalman gain K1 P(k+1)J h T (HP(k+1)H T +R) -1 , where P(k+1) is the covariance prediction value, and J h T is the first A transpose matrix of the Jacobian matrix, H is the state-to-observation transition matrix, H T is the transpose matrix of the state-to-observation transition matrix, and R is the measurement noise variance.
  • the J T is the second Jacobian matrix J w , that is, The Kalman gain calculated at this time is the second Kalman gain.
  • the second Kalman gain is determined according to the second Jacobian matrix, which specifically includes:
  • the second Kalman gain K2 P(k+1)J w T (HP(k+1)H T +R) -1 , where P(k+1) is the covariance prediction value, and J h T is the first A transpose matrix of the Jacobian matrix, H is the state-to-observation transition matrix, H T is the transpose matrix of the state-to-observation transition matrix, and R is the measurement noise variance.
  • Step S523 According to the Kalman gain, the estimated value and the predicted value, update the prediction equation, determine the updated prediction equation, and obtain the updated value;
  • the prediction equation is updated, the updated prediction equation is determined, and the updated value is obtained.
  • the updated prediction equation is: Among them, x(k+1) is the predicted value of the state vector corresponding to the position of the tracking target at the next moment, K is the Kalman gain, and z measurement is the height of the target frame in the image represented by the first Jacobian matrix J h .
  • the Kalman gain K is the first Kalman gain K1
  • z prediction
  • the Kalman gain K is the second Kalman gain K2
  • z prediction
  • x(k+1) is the predicted value of the state vector corresponding to the position of the tracking target at the next moment
  • K1 is the first Kalman gain
  • K2 is the second Kalman gain
  • v b is the ordinate of the highest point b of the target frame in the pixel coordinate system
  • v a is the lowest point a of the target frame in the pixel coordinate system
  • v d is the ordinate of the rightmost point d of the target frame in the pixel coordinate system
  • vc is the ordinate of the leftmost point c of the target frame in the pixel coordinate system.
  • the predicted value of the state vector corresponding to the position of the tracking target at the next moment is determined, which is beneficial to better estimate the position of the tracking target.
  • Step S524 Update the target position information of the tracking target at the next moment according to the updated value.
  • the updated prediction equation is determined, the updated value obtained in the update stage is substituted into the prediction equation of the prediction stage, and the target position information of the tracking target at the next moment is estimated.
  • x(k+1) as x(k), P(k+1) as P(k) into the prediction equation
  • the predicted state quantity of the tracking target is obtained, and the state quantity is used as the target position information of the tracking target at the next moment, so as to obtain the target position information of the tracking target at the next moment.
  • the method further includes:
  • the present invention can better converge the estimated target position information to the actual position information.
  • a method for estimating a position of a tracking target is provided, which is applied to an unmanned aerial vehicle, where the unmanned aerial vehicle includes a gimbal camera, and the gimbal camera is used to acquire images, and the method includes: according to Given the initial position information of the tracking target at the current moment, estimate the target position information of the tracking target at the next moment; According to the estimated target position information, determine the estimated width and estimated height of the tracking target in the image; Obtain the tracking The actual width and actual height of the target in the image; according to the estimated width and estimated height and the actual height and actual width, an error term is determined, and the error term includes a height difference and a width difference; according to the error term, update The target position information of the tracking target at the next moment.
  • the present invention can realize stable estimation of the position of the tracking target.
  • FIG. 8 is a schematic structural diagram of a position estimation device for tracking a target provided by an embodiment of the present invention.
  • the device for estimating the position of the tracking target is applied to an unmanned aerial vehicle, and the unmanned aerial vehicle includes a pan-tilt camera, and the pan-tilt camera is used to acquire images.
  • the device for estimating the position of the tracking target 80 includes:
  • the target position information estimation unit 81 is used to estimate the target position information of the tracking target at the next moment according to the initial position information of the given tracking target at the current moment;
  • An estimation unit 82 configured to determine the estimated width and estimated height of the tracking target in the image according to the estimated target position information
  • the actual unit 83 is used to obtain the actual width and actual height of the tracking target in the image
  • An error term unit 84 configured to determine an error term according to the estimated width and estimated height and the actual height and actual width, and the error term includes a height difference and a width difference;
  • the updating unit 85 is configured to update the target position information of the tracking target at the next moment according to the error term.
  • the estimation unit is specifically used for:
  • the tracking target is modeled, a model corresponding to the tracking target is determined, and a target frame of the tracking target in the image is determined according to the model corresponding to the tracking target.
  • the estimation unit is specifically used for:
  • the estimation unit includes:
  • a camera position module configured to determine the camera position information of the tracking target in the camera coordinate system according to the estimated target position information, wherein the estimated target position information is the world position information in the world coordinate system;
  • the pixel position module is configured to determine the pixel position information of the tracking target in the pixel coordinate system according to the camera position information of the tracking target in the camera coordinate system.
  • the camera position module is specifically used for:
  • the pixel location module is specifically used for:
  • K intr is the internal parameter matrix of the gimbal camera
  • P cam is the three-dimensional coordinate of the tracking target in the camera coordinate system
  • p z is the three-dimensional coordinate of the tracking target in the world coordinate system.
  • the internal parameter matrix of the PTZ camera is the focal length of the gimbal camera, and (c x , c y ) is the coordinate of the main point.
  • the estimation unit includes:
  • an estimated height module for determining the estimated height of the target frame in the image according to the highest point and the lowest point of the target frame in the direction of the vertical axis
  • the estimated width unit is configured to determine the estimated width of the target frame in the image according to the leftmost point and the rightmost point of the target frame in the horizontal axis direction.
  • the estimating height module is specifically used for:
  • the highest point of the target frame of the tracking target in the image in the direction of the vertical axis is a
  • the lowest point is b
  • the highest point a corresponds to the tangent point of the sphere
  • the lowest point b corresponds to the
  • the tangent point of the sphere is B
  • the pixel coordinates of the tangent point A in the pixel coordinate system are (u a , v a )
  • the pixel coordinates of the tangent point B in the pixel coordinate system are (u b , v b )
  • the height difference between the highest point a and the lowest point b is height
  • K intr is the internal parameter matrix of the gimbal camera
  • R is the radius of the sphere
  • z is the Z-axis coordinate of the tracking target in the camera coordinate system.
  • the estimated width unit is specifically used for:
  • the leftmost point of the target frame of the tracking target in the image on the horizontal axis is c
  • the rightmost point is d
  • the tangent point of the leftmost point c corresponding to the sphere is C
  • the rightmost point The tangent point of d corresponding to the sphere is D
  • the pixel coordinates of the tangent point C in the pixel coordinate system are (u c , v c )
  • the pixel coordinates of the tangent point B in the pixel coordinate system are (u d ,v d )
  • the width difference between the leftmost point c and the rightmost point d is width
  • the estimated width of the target frame in the image is Among them, K intr is the internal parameter matrix of the gimbal camera, R is the radius of the sphere, and z is the Z-axis coordinate of the tracking target in the camera coordinate system.
  • the updating unit is specifically used for:
  • a first Jacobian matrix of the estimated height to the state vector of the tracking target and a second Jacobian matrix of the estimated width to the state vector of the tracking target are determined, wherein, the state vector is the transpose matrix of the estimated target position information;
  • the target position information of the tracking target at the next moment is updated.
  • the first Jacobian matrix is The second Jacobian matrix is Among them, K intr is the internal parameter matrix of the gimbal camera, R is the radius of the sphere, and z is the Z-axis coordinate of the tracking target in the camera coordinate system.
  • the extended Kalman filter algorithm includes a prediction stage and an update stage, and the update unit is specifically used for:
  • the state vector corresponding to the initial position information of the tracking target at the current moment is used as the estimated value of the extended Kalman filter, and according to the prediction equation, the predicted value of the target position information of the tracking target at the next moment is estimated,
  • the prediction equation includes a state vector equation and a covariance equation
  • the prediction equation is updated, the updated prediction equation is determined, and the updated value is obtained;
  • the target position information of the tracking target at the next moment is updated.
  • the prediction equation is:
  • x(k+1) is the predicted value of the state vector corresponding to the position of the tracking target at the next moment
  • x(k) is the estimated value of the state vector corresponding to the position of the tracking target at the current moment
  • F is the system state transition matrix
  • P(k+1) is the covariance prediction value
  • P(k) is the covariance estimation value
  • F T is the transpose matrix of the system state transition matrix
  • Q is the noise matrix.
  • the Kalman gain K P(k+1)J T (HP(k+1HT+R)-1, where Pk+1 is the covariance prediction value, and JT is the first Jacques ratio matrix or the transpose matrix of the second Jacobian matrix, H is the state-to-observation transition matrix, P(k+1) is the covariance prediction value, H T is the state-to-observation transition matrix, and R is the transpose matrix of the state-to-observation transition matrix. Measure noise variance.
  • the updated prediction equation is: Among them, x(k+1) is the predicted value of the state vector corresponding to the target position information of the tracking target at the next moment, K is the Kalman gain, z measurement is the measurement value of the height or width of the target frame, and z prediction is the first The predicted value of the height of the target frame represented by the Jacobian matrix J h or the predicted value of the width of the target frame represented by the second Jacobian matrix J w , P(k+1) is the predicted value of the covariance, and H is the state-to-observation transformation matrix.
  • the updating unit is specifically used for:
  • the update value obtained in the update stage is substituted into the prediction equation of the prediction stage, and the target position information of the tracking target at the next moment is updated.
  • the device further includes:
  • the iterative unit is configured to use the updated position information as estimated target position information, and iteratively update the target position information of the tracking target at the next moment.
  • the above-mentioned apparatus can execute the method provided by the embodiments of the present application, and has corresponding functional modules and beneficial effects for executing the method.
  • the above-mentioned apparatus can execute the method provided by the embodiments of the present application, and has corresponding functional modules and beneficial effects for executing the method.
  • the methods provided in the embodiments of the present application can execute the method provided by the embodiments of the present application.
  • a position estimation device for tracking a target is provided, which is applied to an unmanned aerial vehicle, where the unmanned aerial vehicle includes a pan-tilt camera, and the pan-tilt camera is used to acquire images, and the device includes: a target
  • the position information estimation unit is used for estimating the target position information of the tracking target at the next moment according to the initial position information of the given tracking target at the current moment; the estimation unit is used for determining the tracking target according to the estimated target position information
  • the estimated width and estimated height of the target in the image; the actual unit is used to obtain the actual width and actual height of the tracking target in the image;
  • the error term unit is used to obtain the actual height and actual height according to the estimated width and estimated height
  • the width is used to determine an error term, and the error term includes a height difference and a width difference; an update unit is configured to update the target position information of the tracking target at the next moment according to the error term.
  • FIG. 9 is a schematic diagram of a hardware structure of a controller of a tracking system for an unmanned aerial vehicle provided by an embodiment of the present invention.
  • the controller 31 includes: one or more processors 31 a and a memory 31 b.
  • one processor 31 a is used as an example.
  • the processor 31a and the memory 31b may be connected by a bus or in other ways, and the connection by a bus is taken as an example in FIG. 9 .
  • the processor 31a is used to estimate the target position information of the tracking target at the next moment according to the initial position information of the given tracking target at the current moment; according to the estimated target position information, determine the estimation of the tracking target in the image width and estimated height; obtain the actual width and actual height of the tracking target in the image; according to the estimated width and estimated height and the actual height and actual width, determine an error term, and the error term includes a height difference and a width difference; According to the error term, the target position information of the tracking target at the next moment is updated.
  • the memory 31b can be used to store non-volatile software programs, non-volatile computer-executable programs and modules, such as the method for estimating the position of the tracking target in the embodiment of the present application. program instructions/modules.
  • the processor 31a executes various functional applications and data processing of the controller by running the non-volatile software programs, instructions and modules stored in the memory 31b, ie, implements the method for estimating the position of the tracking target in the above method embodiments.
  • the memory 31b may include a storage program area and a storage data area, wherein the storage program area may store an operating system, an application program required for at least one function; the storage data area may store data created according to the use of the controller, and the like.
  • the memory 31b may include high-speed random access memory, and may also include non-volatile memory, such as at least one magnetic disk storage device, flash memory device, or other non-volatile solid-state storage device.
  • memory 31b may optionally include memory located remotely from processor 31a, which may be connected to the controller via a network. Examples of such networks include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.
  • the one or more modules are stored in the memory 31b, and when executed by the one or more processors 31a, execute the method for estimating the position of the tracking target in any of the above method embodiments, for example, execute the above-described method. Steps S10 to S50 of the method in FIG. 4 ; realize the functions of the modules in FIG. 8 .
  • Embodiments of the present application provide a non-volatile computer-readable storage medium, where the computer-readable storage medium stores computer-executable instructions, and the computer-executable instructions are executed by one or more processors, for example, in FIG. 9
  • One processor 31a in the above-mentioned one or more processors can execute the method for estimating the position of the tracking target in any of the above-mentioned method embodiments, and perform the above-described method steps S10 to S50 in FIG. 4 ; function of the module.
  • the device embodiments described above are only illustrative, wherein the units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in One place, or it can be distributed over multiple network elements. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution in this embodiment.
  • each embodiment can be implemented by means of software plus a general hardware platform, and certainly can also be implemented by hardware.
  • Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing the relevant hardware through a computer program, and the program can be stored in a computer-readable storage medium, and the program is During execution, it may include the processes of the embodiments of the above-mentioned methods.
  • the storage medium may be a magnetic disk, an optical disk, a read-only memory (Read-Only Memory, ROM), or a random access memory (Random Access Memory, RAM) or the like.

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Remote Sensing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Geometry (AREA)
  • Microelectronics & Electronic Packaging (AREA)
  • Mechanical Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Automation & Control Theory (AREA)
  • Image Analysis (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

一种跟踪目标的位置估计方法、装置及无人飞行器，该方法包括：根据给定的跟踪目标在当前时刻的初始位置信息，估计所述跟踪目标在下一个时刻的目标位置信息（S10）；根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度（S20）；获取跟踪目标在图像中的实际宽度和实际高度（S30）；根据所述估计宽度和估计高度与所述实际高度和实际宽度，确定误差项，所述误差项包括高度差和宽度差（S40）；根据所述误差项，更新所述跟踪目标在下一个时刻的目标位置信息（S50）。通过确定该估计宽度和估计高度与获取的实际宽度和实际高度，计算跟踪目标在图像中的误差项，更新跟踪目标在下一个时刻的目标位置信息，该方法能够实现稳定地估计跟踪目标的位置。

Description

一种跟踪目标的位置估计方法、装置及无人飞行器
本申请要求于2020年8月31日提交中国专利局、申请号为202010896152X、申请名称为“一种跟踪目标的位置估计方法、装置及无人飞行器”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。
技术领域
本发明实施例涉及无人飞行器领域，特别涉及一种跟踪目标的位置估计方法、装置及无人飞行器。
背景技术
在无人飞行器的目标跟随系统中，通常希望实现对目标的实时跟随，且跟随过程中图像中的目标框大小基本保持不变，即保持无人飞行器与目标间“定高、恒距”。因此，稳定地估计目标的位置则显得尤为重要。
目前，较精确的目标估计方法有反投影法，利用无人飞行器上的双目相机，将获得的深度数据反投影至图像中，根据投影点是否落在目标框中，来确定目标的三维位置，该类方法精度较高，但是要求目标能出现在双目相机的视场角内，且在双目相机的测量范围内。对于远距离跟踪来说，目标物往往在双目相机的视场外或者测量范围外，这直接导致该类方法失效。
另一类方法利用飞机的高度信息和云台的俯仰角信息，大致计算出目标物的位置，该类方法严重依赖了无人飞行器融合的对地高度信息，然而高度信息会随着无人飞行器的飞行时间的累积，精度越来越差，而且当目标距离过远时，俯仰角变化小，无法准确的反映出目标的移动距离。
发明人在实现本发明过程中，发现相关技术中至少存在如下问题：在目标距离较远时，目标的位置估计精度不足，无法稳定地估计目标的位置。
发明内容
本发明实施例的目的在于提供一种跟踪目标的位置估计方法、装置及无人飞行器，以解决在目标距离较远时，跟踪目标的位置估计精度不足，无法稳定地估计跟踪目标的位置的问题，实现稳定地估计跟踪目标的位置。
第一方面，本发明实施例提供一种跟踪目标的位置估计方法，应用于无人飞行器，所述无人飞行器包括云台相机，所述云台相机用于获取图像，所述方法包括：
根据给定的跟踪目标在当前时刻的初始位置信息，估计所述跟踪目标在下一个时刻的目标位置信息；
根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度；
获取跟踪目标在图像中的实际宽度和实际高度；
根据所述估计宽度和估计高度与所述实际高度和实际宽度，确定误差项，所述误差项包括高度差和宽度差；
根据所述误差项，更新所述跟踪目标在下一个时刻的目标位置信息。
在一些实施例中，所述根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度，包括：
对所述跟踪目标进行建模，确定所述跟踪目标对应的模型，根据所述跟踪目标对应的模型，确定所述跟踪目标在图像中的目标框。
在一些实施例中，所述对所述跟踪目标进行建模，确定所述跟踪目标对应的模型，根据所述跟踪目标对应的模型，确定所述跟踪目标在图像中的目标框，包括：
将所述跟踪目标建模为球体，并根据所述跟踪目标的长度、宽度以及高度，确定所述跟踪目标对应的球体的半径R；
确定跟踪目标对应的球体在图像中对应的目标框。
在一些实施例中，所述根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度，包括：
根据估计的目标位置信息，确定所述跟踪目标在相机坐标系下的相机位置信息，其中，所述估计的目标位置信息为世界坐标系下的世界位置信息；
根据所述跟踪目标在相机坐标系下的相机位置信息，确定所述跟踪目标在像素坐标系下的像素位置信息。
在一些实施例中，所述根据估计的目标位置信息，确定所述跟踪目标在相机坐标系下的相机位置信息，包括：
假设所述估计的目标位置信息P world＝(p x,p y,p z)，则所述估计的目标位置信息对应的状态向量
Figure PCTCN2021108888-appb-000001
则所述跟踪目标在相机坐标系下的相机位置信息
Figure PCTCN2021108888-appb-000002
其中，P cam＝(x,y,z) T为跟踪目标在相机坐标系下的三维坐标，
Figure PCTCN2021108888-appb-000003
为世界坐标系到相机坐标系的旋转矩阵，t为世界坐标系到相机坐标系的平移向量。
在一些实施例中，所述根据所述跟踪目标在相机坐标系下的相机位置信息，确定所述跟踪目标在像素坐标系下的像素位置信息，包括：
假设所述跟踪目标在像素坐标系下的像素位置信息p＝(u,v) T，则
Figure PCTCN2021108888-appb-000004
其中，K intr为云台相机的内参矩阵，P cam为跟踪目标在相机坐标系下的三维坐标，p z为跟踪目标在世界坐标系下的三维坐标。
在一些实施例中，所述云台相机的内参矩阵
Figure PCTCN2021108888-appb-000005
其中，(f x,f y)为云台相机的焦距，(c x,c y)为主点坐标。
在一些实施例中，所述确定所述跟踪目标在图像中的估计宽度和估计高度，包括：
根据所述目标框在纵轴方向上的最高点和最低点以及横轴方向上的最左点和最右点，分别确定所述目标框在图像中的估计高度和估计宽度。
在一些实施例中，所述确定所述目标框在图像中的估计高度，包括：
假设所述跟踪目标在图像中的目标框在纵轴方向上的最高点为a，最低点为b，所述最高点a对应所述球体的切点为A，所述最低点b对应所述球体的切点为B，所述切点A在像素坐标系下的像素坐标为(u a,v a)，所述切点B在像素坐标系下的像素坐标为(u b,v b)，所述最高点a和最低点b的高度差为height，则确定所述目标框在图像中的估计高度为
Figure PCTCN2021108888-appb-000006
其中，K intr为云台相机的内参矩阵，R为球体的半径，z为跟踪目标在相机坐标系下的Z轴坐标。
在一些实施例中，所述确定所述目标框在图像中的估计宽度，包括：
假设所述跟踪目标在图像中的目标框在橫轴方向上的最左点为c，最右点为d，所述最左点c对应所述球体的切点为C，所述最右点d对应所述球体的切点为D，所述切点C在像素坐标系下的像素坐标为(u c,v c)，所述切点B在像素坐标系下的像素坐标为(u d,v d)，所述最左点c和最右点d的宽度差为width，则所述目标框在图像中的估计宽度 为
Figure PCTCN2021108888-appb-000007
其中，K intr为云台相机的内参矩阵，R为球体的半径，z为跟踪目标在相机坐标系下的Z轴坐标。
在一些实施例中，所述根据所述误差项，更新所述跟踪目标在下一个时刻的目标位置信息，包括：
根据所述误差项，基于扩展卡尔曼滤波算法，更新所述跟踪目标在下一个时刻的目标位置信息。
在一些实施例中，所述根据所述误差项，基于扩展卡尔曼滤波算法，更新所述跟踪目标在下一个时刻的目标位置信息，包括：
根据所述跟踪目标在图像中的估计高度和估计宽度，确定所述估计高度对跟踪目标的状态向量的第一雅克比矩阵以及所述估计宽度对跟踪目标的状态向量的第二雅克比矩阵，其中，所述状态向量为估计的目标位置信息的转置矩阵；
根据所述第一雅克比矩阵和第二雅克比矩阵，基于扩展卡尔曼滤波算法，更新所述跟踪目标在下一个时刻的目标位置信息。
在一些实施例中，所述第一雅克比矩阵为
Figure PCTCN2021108888-appb-000008
所述第二雅克比矩阵为
Figure PCTCN2021108888-appb-000009
Figure PCTCN2021108888-appb-000010
其中，K intr为云台相机的内参矩阵，R为球体的半径，z为跟踪目标在相机坐标系下的Z轴坐标。
在一些实施例中，所述扩展卡尔曼滤波算法包括预测阶段和更新阶段，所述根据所述第一雅克比矩阵和第二雅克比矩阵，基于扩展卡尔曼滤波算法，更新所述跟踪目标在下一个时刻的目标位置信息，包括：
在预测阶段，将所述跟踪目标在当前时刻的初始位置信息对应的状态向量作为扩展卡尔曼滤波器的估计值，根据预测方程，估计所述跟踪目标在下一个时刻的目标位置信息的预测值，其中，所述预测方程包括状态向量方程和协方差方程；
在更新阶段，根据所述第一雅克比矩阵和第二雅克比矩阵，计算卡尔曼增益；
根据所述卡尔曼增益、估计值以及预测值，更新所述预测方程，确定更新后的预测方程，并获取更新值；
根据所述更新值，更新所述跟踪目标在下一个时刻的目标位置信息。
在一些实施例中，所述预测方程为：
Figure PCTCN2021108888-appb-000011
其中，x(k+1)为跟踪目标在下一个时刻的位置对应的状态向量的预测值，x(k)为跟踪目标在当前时刻的位置对应的状态向量的估计值，F为系统状态转移矩阵，P(k+1)为协方差预测值，P(k)为协方差估计值，F T为系统状态转移矩阵的转置矩阵，Q为噪声矩阵。
在一些实施例中，所述根据所述第一雅克比矩阵和第二雅克比矩阵，计算卡尔曼增益，包括：
卡尔曼增益K＝P(k+1)J T(HP(k+1)H T+R) -1，其中，P(k+1)为协方差预测值，J T为第一雅克比矩阵或第二雅克比矩阵的转置矩阵，H为状态到观测的转换矩阵，P(k+1)为协方差预测值，H T为状态到观测的转换矩阵的转置矩阵，R为测量噪声方差。
在一些实施例中，所述根据所述卡尔曼增益、测量值以及预测值，更新所述预测方程，确定更新后的预测方程，包括：
更新后的预测方程为：
Figure PCTCN2021108888-appb-000012
其中，x(k+1)为跟踪目标在下一个时刻的目标位置信息对应的状态向量的预测值，K为卡尔曼增益，z measurement为目标框的高度或宽度的测量值，z prediction为第一雅克比矩阵J h代表的目标框的高度的预测值或者第二雅克比矩阵J w代表的目标框的宽度的预测值，P(k+1)为协方差预测值，H为状态到观测的转换矩阵。
在一些实施例中，所述根据所述更新后的预测方程，更新所述跟踪目标在下一个时刻的目标位置信息，包括：
将更新阶段获得的更新值代入所述预测阶段的预测方程，更新所述跟踪目标在下一个时刻的目标位置信息。
在一些实施例中，所述方法还包括：
将所述更新的位置信息作为估计的目标位置信息，迭代更新所述跟踪目标在下一个时刻的目标位置信息。
第二方面，本发明实施例提供一种跟踪目标的位置估计装置，应用于无人飞行器，所述无人飞行器包括云台相机，所述云台相机用于获取图像，所述装置包括：
目标位置信息估计单元，用于根据给定的跟踪目标在当前时刻的初始位置信息，估计所述跟踪目标在下一个时刻的目标位置信息；
估计单元，用于根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度；
实际单元，用于获取跟踪目标在图像中的实际宽度和实际高度；
误差项单元，用于根据所述估计宽度和估计高度与所述实际高度和实际宽度，确定误差项，所述误差项包括高度差和宽度差；
更新单元，用于根据所述误差项，更新所述跟踪目标在下一个时刻的目标位置信息。
在一些实施例中，所述估计单元，具体用于：
对所述跟踪目标进行建模，确定所述跟踪目标对应的模型，根据所述跟踪目标对应的模型，确定所述跟踪目标在图像中的目标框。
在一些实施例中，所述估计单元，具体用于：
将所述跟踪目标建模为球体，并根据所述跟踪目标的长度、宽度以及高度，确定所述跟踪目标对应的球体的半径R；
确定跟踪目标对应的球体在图像中对应的目标框。
在一些实施例中，所述估计单元，包括：
相机位置模块，用于根据估计的目标位置信息，确定所述跟踪目标在相机坐标系下的相机位置信息，其中，所述估计的目标位置信息为世界坐标系下的世界位置信息；
像素位置模块，用于根据所述跟踪目标在相机坐标系下的相机位置信息，确定所述跟踪目标在像素坐标系下的像素位置信息。
在一些实施例中，所述相机位置模块，具体用于：
假设所述估计的目标位置信息P world＝(p x,p y,p z)，则所述估计的目标位置信息对应的状态向量
Figure PCTCN2021108888-appb-000013
则所述跟踪目标在相机坐标系下的相机位置信息
Figure PCTCN2021108888-appb-000014
其中，P cam＝(x,y,z) T为跟踪目标在相机坐标系下的三维坐标，
Figure PCTCN2021108888-appb-000015
为世界坐标系到相机坐标系的旋转矩阵，t为世界坐标系到相机坐标系的平移向量。
在一些实施例中，所述像素位置模块，具体用于：
假设所述跟踪目标在像素坐标系下的像素位置信息p＝(u,v) T，则
Figure PCTCN2021108888-appb-000016
其中，K intr为云台相机的内参矩阵，P cam为跟踪目标在相机坐标系下的三维坐标，p z为跟踪目标在世界坐标系下的三维坐标。
在一些实施例中，所述云台相机的内参矩阵
Figure PCTCN2021108888-appb-000017
其中，(f x,f y)为云台相机的焦距，(c x,c y)为主点坐标。
在一些实施例中，所述估计单元，包括：
估计高度模块，用于根据所述目标框在纵轴方向上的最高点和最低点，确定所述目标框在图像中的估计高度；
估计宽度单元，用于根据所述目标框在横轴方向上的最左点和最右点，确定所述目标框在图像中的估计宽度。
在一些实施例中，所述估计高度模块，具体用于：
假设所述跟踪目标在图像中的目标框在纵轴方向上的最高点为a，最低点为b，所述最高点a对应所述球体的切点为A，所述最低点b对应所述球体的切点为B，所述切点A在像素坐标系下的像素坐标为(u a,v a)，所述切点B在像素坐标系下的像素坐标为(u b,v b)，所述最高点a和最低点b的高度差为height，则确定所述目标框在图像中的估计高度为
Figure PCTCN2021108888-appb-000018
其中，K intr为云台相机的内参矩阵，R为球体的半径，z为跟踪目标在相机坐标系下的Z轴坐标。
在一些实施例中，所述估计宽度单元，具体用于：
假设所述跟踪目标在图像中的目标框在橫轴方向上的最左点为c，最右点为d，所述最左点c对应所述球体的切点为C，所述最右点d对应所述球体的切点为D，所述切点C在像素坐标系下的像素坐标为(u c,v c)，所述切点B在像素坐标系下的像素坐标为(u d,v d)，所述最左点c和最右点d的宽度差为width，则所述目标框在图像中的估计宽度为
Figure PCTCN2021108888-appb-000019
其中，K intr为云台相机的内参矩阵，R为球体的半径，z为跟踪目标在相机坐标系下的Z轴坐标。
在一些实施例中，所述更新单元，具体用于：
根据所述误差项，基于扩展卡尔曼滤波算法，更新所述跟踪目标在下一个时刻的目标位置信息。
在一些实施例中，所述更新单元，具体用于：
根据所述跟踪目标在图像中的估计高度和估计宽度，确定所述估计高度对跟踪目标的状态向量的第一雅克比矩阵以及所述估计宽度对跟踪目标的状态向量的第二雅克比矩阵，其中，所述状态向量为估计的目标位置信息的转置矩阵；
根据所述第一雅克比矩阵和第二雅克比矩阵，基于扩展卡尔曼滤波算法，更新所述跟踪目标在下一个时刻的目标位置信息。
在一些实施例中，所述第一雅克比矩阵为
Figure PCTCN2021108888-appb-000020
所述第二雅克比矩阵为
Figure PCTCN2021108888-appb-000021
Figure PCTCN2021108888-appb-000022
其中，K intr为云台相机的内参矩阵，R为球体的半径，z为跟踪目标在相机坐标系下的Z轴坐标。
在一些实施例中，所述扩展卡尔曼滤波算法包括预测阶段和更新阶段，所述更新单元，具体用于：
在预测阶段，将所述跟踪目标在当前时刻的初始位置信息对应的状态向量作为扩展卡尔曼滤波器的估计值，根据预测方程，估计所述跟踪目标在下一个时刻的目标位置信息的预测值，其中，所述预测方程包括状态向量方程和协方差方程；
在更新阶段，根据所述第一雅克比矩阵和第二雅克比矩阵，计算卡尔曼增益；
根据所述卡尔曼增益、估计值以及预测值，更新所述预测方程，确定更新后的预测方程，并获取更新值；
根据所述更新值，更新所述跟踪目标在下一个时刻的目标位置信息。
在一些实施例中，所述预测方程为：
Figure PCTCN2021108888-appb-000023
其中，x(k+1)为跟踪目标在下一个时刻的位置对应的状态向量的预测值，x(k)为跟踪目标在当前时刻的位置对应的状态向量的估计值，F为系统状态转移矩阵，P(k+1)为协方差预测值，P(k)为协方差估计值，F T为系统状态转移矩阵的转置矩阵，Q为噪声矩阵。
在一些实施例中，所述卡尔曼增益K＝P(k+1)J T(HP(k+1)H T+R)-1，其中，Pk+1为协方差预测值，JT为第一雅克比矩阵或第二雅克比矩阵的转置矩阵，H为状态到观测的转换矩阵，P(k+1)为协方差预测值，H T为状态到观测的转换矩阵的转置矩阵，R为测量噪声方差。
在一些实施例中，所述更新后的预测方程为：
Figure PCTCN2021108888-appb-000024
其中，x(k+1)为跟踪目标在下一个时刻的目标位置信息对应的状态向量的预测值，K为卡尔曼增益，z measurement为目标框的高度或宽度的测量值，z prediction为第一雅克比矩阵J h代表的目标框的高度的预测值或者第二雅克比矩阵J w代表的目标框的宽度的预测值，P(k+1)为协方差预测值，H为状态到观测的转换矩阵。
在一些实施例中，所述更新单元，具体用于：
将更新阶段获得的更新值代入所述预测阶段的预测方程，更新所述跟踪目标在下一个时刻的目标位置信息。
在一些实施例中，所述装置还包括：
迭代单元，用于将所述更新的位置信息作为估计的目标位置信息，迭代更新所述跟踪目标在下一个时刻的目标位置信息。
第三方面，本发明实施例提供一种无人飞行器，所述无人飞行器包括机身、与所述机身相连的机臂、设于所述机臂的动力系统、设置于所述机身的跟踪系统；其中，所述跟踪系统包括控制器，所述控制器包括：
至少一个处理器；以及，
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行上述的跟踪目标的位置估计方法。
第四方面，一种非易失性计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，当所述计算机可执行指令被无人飞行器执行时，使所述无人飞行器执行如上所述的跟踪目标的位置估计方法。
本发明实施例的有益效果在于：通过提供一种跟踪目标的位置估计方法，应用于无人飞行器，所述无人飞行器包括云台相机，所述云台相机用于获取图像，所述方法包括：根据给定的跟踪目标在当前时刻的初始位置信息，估计所述跟踪目标在下一个时刻的目标位置信息；根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度；获取跟踪目标在图像中的实际宽度和实际高度；根据所述估计宽度和估计高度与所述实际高度和实际宽度，确定误差项，所述误差项包括高度差和宽度差；根据所述误差项，更新所述跟踪目标在下一个时刻的目标位置信息。通过计算跟踪目标在图像中的误差项，更新跟踪目标在下一个时刻的目标位置信息，本发明实施例能够实现稳定地估计跟踪目标的位置。
附图说明
一个或多个实施例通过与之对应的附图中的图片进行示例性说明，这些示例性说明并不构成对实施例的限定，附图中具有相同参考数字标号的元件表示为类似的元件，除非有特别申明，附图中的图不构成比例限制。
图1是本发明实施例提供的一种应用场景的示意图；
图2是本发明实施例提供的一种无人飞行器的结构示意图；
图3a是本发明实施例提供的一种模型与图像的对应关系示意图；
图3b是本发明实施例提供的另一种模型与图像的对应关系示意图；
图4是本发明实施例提供的一种跟踪目标的位置估计方法的流程示意图；
图5是图4中的步骤S20的细化流程图；
图6是图4中的步骤S50的细化流程图；
图7是图6中的步骤S52的细化流程图；
图8是本发明实施例提供的一种跟踪目标的位置估计装置的结构示意图；
图9是本发明实施例提供的一种无人飞行器的跟踪系统的控制器的硬件结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明实施例提供的跟踪目标的位置估计方法可以应用于如图1所示的应用场景，在图1所示的应用场景中，包括无人飞行器100和跟踪目标300。无人飞行器100可以用于对所述跟踪目标300进行跟踪，在无人飞行器100对跟踪目标300进行跟踪的过程中，有可能会遇到障碍物400。无人飞行器100需要在对跟踪目标300进行跟踪的同时躲避障碍物400，以实现正常飞行。
其中，无人飞行器100可以为合适的无人飞行器，包括固定翼无人飞行器和旋转翼无人飞行器，例如直升机、四旋翼机和具有其它数量的旋翼和/或旋翼配置的飞行器。无人飞行器100还可以是其他可移动物体，例如载人飞行器、航模、无人飞艇、无人热气球和机器人等。
其中，跟踪目标300可以为任何合适的可移动或不可移动物体，包括交通工具、人、动物、建筑物、山川河流等。障碍物400例如建筑物、山体、树木、森林、信号塔或其他可移动或不可移动物体(图1中只示出了一个障碍物，实际应用中可能会有更多障碍物或者没有障碍物)。
其中，在一些实施例中，请参照图2，图2是本发明实施例提供的一种无人飞行器的结构示意图；
如图2所示，该无人飞行器100包括机身10、与所述机身10相连的机臂(图中未示出)、设于机臂的动力系统(图中未示出)和设于机身10的控制系统。动力系统用于提供无人飞行器100飞行的推力、升力等，控制系统是无人飞行器100的中枢神经，可以包括多个功能性单元，例如飞控系统20、跟踪系统30、路径规划系统50、视觉系统40以及其他具有特定功能的系统。
跟踪系统30和视觉系统40均包括摄像装置和控制芯片，其中，所述摄像装置为云台相机，跟踪系统30用于获得跟踪目标的状态、跟踪距离(即无人飞行器100距跟踪目标的距离)等，视觉系统40用于提供环境地图或无人飞行器周围环境的原始点云等。
飞控系统20包括各类传感器(例如陀螺仪、加速计等)，飞控系统20用于获得实时的无人飞行器位置以及控制无人飞行器飞行姿态等。路径规划系统50用于对路径进行规划，并指示飞控系统20控制无人飞行器100的飞行姿态以使无人飞行器100按指定路径飞行。
在实际应用时，飞控系统20、路径规划系统50可以设置于机身10内部，跟踪系统30和视觉系统40可以设置于机身10外部并固定于机身10上。跟踪系统30和视觉系统40的摄像装置可以组成单目或双目视觉系统，其中，摄像装置可以为高清数码相机或其他摄像装置，摄像装置可以设置于任何利于拍摄的合适位置，在一些实施例中，跟踪系统30的摄像装置通过云台安装于机身10的底部，视觉系统40的摄像装置设置于机身10的前部和/或下部。其中，各个系统可以分别设置，在一些实施例中，上述系统中的部分或全部也可以集成在一个或多于一个的装置中。
在一些实施例中，无人飞行器100根据目标特征对所述跟踪目标进行跟踪，其中，在部分实施例中，目标特征事先存储于无人飞行器100中，在部分实施例中，目标特征通过其他途径获得。在无人飞行器100的一些应用场景中还包括电子设备200，目标特征可以通过电子设备200发送给无人飞行器100。
具体的，电子设备200可以显示无人飞行器100拍摄的图片，由用户对图片中的目标进行框选，用户框选的目标图片上传无人飞行器100后，无人飞行器100可以根据该框选的目标图片提取目标特征。无人飞行器100和电子设备200之间，可以通过分别设置在各自内部的无线通信模块(例如信号接收器、信号发送器等)建立通信连接，上传或者下发数据/指令。其中，电子设备200例如智能手机、平板电脑、电脑、遥控器等。
无人飞行器100在对跟踪目标进行跟踪和躲避障碍物飞行的过程中，需要根据跟踪目标的位置规划合适的路径，按规划的路径飞行以在保持跟踪的前提下躲避障碍物。因此，目标位置的估计至关重要。本发明实施例利用多种方法获得目标位置的相关值，并将该目标位置的相关值作为扩展卡尔曼滤波器的测量值，利用扩展卡尔曼滤波器进行递推估计获得目标位置的估计值。
在本发明实施例中，通过对跟踪目标进行建模，确定所述跟踪目标对应的模型，其中，本发明实施例以跟踪目标被建模为球体为例进行说明。
请一并参阅图3a和图3b，图3a是本发明实施例提供的一种模型与图像的对应关系示意图；图3b是本发明实施例提供的另一种模型与图像的对应关系示意图；
具体的，根据跟踪目标的长度、宽度、高度，确定与跟踪目标建模对应的球体的半径R，具体的，根据跟踪目标的长a，宽b，高c，确定球体半径为：
Figure PCTCN2021108888-appb-000025
其中，a为跟踪目标的长度，b为跟踪目标的宽度，c为跟踪目标的高度，R为跟踪目标对应的球体的半径。
可以理解的是，跟踪目标的长宽高不需要十分精确，大致表达物体的形状和尺寸即可，例如：跟踪目标为一般小轿车，可使用通用尺寸4.7*1.5*1.8。
其中，在确定跟踪目标对应的模型之后，需要建立模型与图像之间的投影关系，其中，所述图像为云台相机获取到的云台图像，所述跟踪目标被建模为球体，则如图3a所示，假设所述跟踪目标在图像的纵轴方向的最高点为a，最低点为b，而最高点为a对应球体的切点A，最低点b对应球体的切点B。其中，在远距离跟踪时，近似地认为切点A与跟踪目标的目标中心O的连线OA垂直于云台相机光心G(即光学中心G)与目标中心O的连线OG，并且，切点B与跟踪目标的目标中心O的连线OB也垂直于云台相机光心G与目标中心O的连线OG，并且近似地认为切点A、B两点的连线经过目标中心O；
并且，如图3b所示，假设所述跟踪目标在图像的横轴方向的最左点为c，最右点为d，而最左点c对应球体的切点C，最右点对应球体的切点D，其中，在远距离跟踪时，近似地认为切点C与跟踪目标的目标中心O的连线OC垂直于云台相机光心G与目标中心O的连线OG，并且，切点D与跟踪目标的目标中心O的连线OD垂直于云台相机光心G与目标中心O的连线OG，并且近似地认为切点C和切点D两点的连线经过目标中心O。
通过对跟踪目标进行建模，将跟踪目标建模为球体，本发明实施例能够基于建模的球体，对跟踪目标的位置进行估计。
请再参阅图4，图4是本发明实施例提供的一种跟踪目标的位置估计方法的流程示意图；其中，该跟踪目标的位置估计方法应用于无人飞行器，所述无人飞行器包括云台相机，所述云台相机用于获取图像。
如图4所示，该跟踪目标的位置估计方法，包括：步骤S10：根据给定的跟踪目标在当前时刻的初始位置信息，估计所述跟踪目标在下一个时刻的目标位置信息；
可以理解的是，跟踪目标的当前目标位置未知，为了估计跟踪目标的当前目标位置，需要给定所述初始位置信息作为扩展卡尔曼滤波算法的输入，以收敛到所述跟踪目标的实际位置，其中，所述跟踪目标在当前时刻的初始位置信息由无人飞行器的处理器给出，所述初始位置信息由无人飞行器根据所述跟踪目标的实际情况估计得出，通过估计所述跟踪目标的初始位置，将估计得到的位置作为跟踪目标在当前位置的初始位置信息，相比随机给定，其能够提高收敛速度。当给定所述跟踪目标在当前时刻的初始位置信息之后，将所述初始位置信息作为所述跟踪目标的当前世界位置信息，然后根据当前位置估算下一位置，下一位置又变为当前位置的迭代过程，直至收敛到误差小于预设阈值，停止迭代过程。
具体的，将所述初始位置信息作为扩展卡尔曼滤波(extended kalman filter，EKF)算法的输入，通过扩展卡尔曼滤波算法估计所述跟踪目标在下一个时刻的目标位置信息，得到估计的目标位置信息，其中，所述估计的目标位置信息为世界坐标信息。其中，所述扩展卡尔曼滤波算法包括预测阶段和更新阶段，扩展卡尔曼滤波算法的核心思想在于预测+测量反馈，它由两部分组成，第一部分是状态预测方程，第二部分是观测方程。在预测阶段，要通过系统状态方程计算的预测值作为先验信息，之后在观测部分再重新更新这个信息。
在预测阶段，预测方程包括状态向量方程和协方差方程，分别为：
Figure PCTCN2021108888-appb-000026
其中，x(k+1)为跟踪目标在下一个时刻的位置对应的状态向量的预测值，x(k)为跟踪目标在当前时刻的位置对应的状态向量的测量值，F为系统状态转移矩阵，p(k+1)为协方差预测值，P(k)为协方差测量值，F T为系统状态转移矩阵的转置矩阵，Q为噪声矩阵。
在本发明实施例中，系统状态转移矩阵F可以为
Figure PCTCN2021108888-appb-000027
步骤S20：根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度；
具体的，所述图像为云台相机获取到的云台图像，所述根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度，包括：对所述跟踪目标进行建模，确定所述跟踪目标对应的模型，根据所述跟踪目标对应的模型，确定所述跟踪目标在图像中的目标框。
具体的，所述对所述跟踪目标进行建模，确定所述跟踪目标对应的模型，根据所述跟踪目标对应的模型，确定所述跟踪目标在图像中的目标框，包括：
将所述跟踪目标建模为球体，并根据所述跟踪目标的长度、宽度以及高度，确定所述跟踪目标对应的球体的半径R；
确定跟踪目标对应的球体在图像中对应的目标框。
具体的，将所述跟踪目标建模为球体，计算所述跟踪目标的长度、宽度以及高度的平均值，将所述球体的半径R确定为长度、宽度以及高度的平均值，即
Figure PCTCN2021108888-appb-000028
其中，a为跟踪目标的长度，b为跟踪目标的宽度，c为跟踪目标的高度，R为跟踪目标对应的球体的半径。
确定跟踪目标对应的球体在图像中对应的目标框，具体的，将与所述跟踪目标对应的球体投影到图像中，确定与所述球体对应的目标框。
具体的，将所述跟踪目标对应的球体投影到图像中，所述跟踪目标对应的球体从三维形态投影到图像中的二维形态，在所述跟踪目标对应的球体被投影到图像中之后，在图像中形成一目标框，在本发明实施例中，所述目标框为圆形框。可以理解的是，由于与所述球体对应的目标框为圆形框，则根据圆形框的圆心所在的位置以及半径，即可确定目标框在图像中的位置。具体的，所述确定与所述球体对应的目标框，包括：确定所述目标框的圆心位置以及半径，根据所述目标框的圆心位置以及半径，确定与所述球体对应的目标框在图像中的位置信息。
如图3a所示，假设所述跟踪目标在图像的纵轴方向的最高点为a，最低点为b，而最高点为a对应球体的切点A，最低点b对应球体的切点B。其中，在远距离跟踪时，近似地认为切点A与跟踪目标的目标中心O的连线OA垂直于云台相机光心G(即光学中心G)与目标中心O的连线OG，并且，切点B与跟踪目标的目标中心O的连线OB也垂直于云台相机光心G与目标中心O的连线OG，并且近似地认为切点A、B两点的连线经过目标中心O；
并且，如图3b所示，假设所述跟踪目标在图像的横轴方向的最左点为c，最右点为d，而最左点c对应球体的切点C，最右点对应球体的切点D，其中，在远距离跟踪时，近似地认为切点C与跟踪目标的目标中心O的连线OC垂直于云台相机光心G与目标中心O的连线OG，并且，切点D与跟踪目标的目标中心O的连线OD垂直于云台相机光心G与目标中心O的连线OG，并且近似地认为切点C和切点D两点的连线经过目标中心O。
具体的，请再参阅图5，图5是图4中的步骤S20的细化流程图；
如图5所示，所述步骤S20：根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度，包括：
步骤S21：根据估计的目标位置信息，确定所述跟踪目标在相机坐标系下的相机位置信息，其中，所述估计的目标位置信息为世界坐标系下的世界位置信息；
其中，所述估计的目标位置信息为世界坐标系下的世界位置信息，具体的，假设所述跟踪目标在下一个时刻的世界位置信息P world＝(p x,p y,p z)，即估计的目标位置信息为P world＝(p x,p y,p z)，则所述跟踪目标的目标位置信息对应的状态向量
Figure PCTCN2021108888-appb-000029
则所述跟踪目标在相机坐标系下的相机位置信息
Figure PCTCN2021108888-appb-000030
即估计的相机位置信息为
Figure PCTCN2021108888-appb-000031
可以理解的是，所述估计的相机位置信息为所述目标中心O对应的相机位置信息，其中，P cam＝(x,y,z) T为跟踪目标在相机坐标系下的三维坐标，即P cam＝(x,y,z) T为跟踪目标的目标中心O在相机坐标系下的三维坐标，
Figure PCTCN2021108888-appb-000032
为世界坐标系到相机坐标系的旋转矩阵，t为世界坐标系到相机坐标系的平移向量。
其中，切点A、B在相机坐标系下的坐标分别表示为：
Figure PCTCN2021108888-appb-000033
其中，P cam为目标中心在相机坐标系下的坐标，P cam,A为切点A在相机坐标系下的坐标，P cam,B为切点B在相机坐标系下的坐标，P cam为目标中心O在相机坐标系下的坐标，R 1＝(0,R,0) T，R为球体半径。其中，切点A对应最高点a，其在图像中的像素坐标为(u a,v a)，切点B对应最低点b，其在图像中的像素坐标为(u b,v b)。
其中，切点C、D在相机坐标系下的坐标分别表示为：
Figure PCTCN2021108888-appb-000034
其中，P cam,C为切点C在相机坐标系下的坐标，P cam,D为切点D在相机坐标系下的坐标，P cam为目标中心O在相机坐标系下的坐标，P 2＝[R,0,0]，R为球体半径。其中，切点C对应最左点c，其在图像中的像素坐标为(u c,v c)，切点D对应最右点d，其在图像中的像素坐标为(u d,v d)。
步骤S22：根据所述跟踪目标在相机坐标系下的相机位置信息，确定所述跟踪目标在像素坐标系下的像素位置信息。
具体的，假设所述跟踪目标在像素坐标系下的像素位置信息p＝(u,v) T，则
Figure PCTCN2021108888-appb-000035
其中，K intr为云台相机的内参矩阵，P cam为跟踪目标在相机坐标系下的三维坐标，p z为跟踪目标在世界坐标系下的三维坐标，可以理解的是，P cam为跟踪目标的目标中心O在 相机坐标系下的三维坐标，p z为估计的目标位置信息在世界坐标系下的三维坐标。
具体的，根据相机坐标系到像素坐标系的转换关系：
Figure PCTCN2021108888-appb-000036
其中，(f x,f y)为焦距，(c x,c y)为主点坐标，其中，焦距和主点坐标均可以通过云台相机标定获得，将所述转换关系转化为矩阵形式：
Figure PCTCN2021108888-appb-000037
其中，K intr为云台相机的内参矩阵，P cam为跟踪目标在相机坐标系下的三维坐标，p z为跟踪目标在世界坐标系下的三维坐标。在本发明实施例中，云台相机的内参矩阵
Figure PCTCN2021108888-appb-000038
其中，(f x,f y)为云台相机的焦距，(c x,c y)为主点坐标。
在本发明实施例中，通过将跟踪目标在当前时刻的世界位置信息转化为所述跟踪目标在图像中的像素位置信息，有利于确定跟踪目标在图像中的位置变化，以方便对跟踪目标进行位置估计。
具体的，所述确定所述跟踪目标在图像中的估计宽度和估计高度，包括：
根据所述目标框在纵轴方向上的最高点和最低点以及横轴方向上的最左点和最右点，分别确定所述目标框在图像中的估计高度和估计宽度。
具体的，所述确定所述目标框在图像中的估计高度，包括：
假设所述跟踪目标在图像中的目标框在纵轴方向上的最高点为a，最低点为b，所述最高点a对应所述球体的切点为A，所述最低点b对应所述球体的切点为B，所述切点A在像素坐标系下的像素坐标为(u a,v a)，所述切点B在像素坐标系下的像素坐标为(u b,v b)，所述最高点a和最低点b的高度差为height，则确定所述目标框在图像中的估计高度为
Figure PCTCN2021108888-appb-000039
其中，K intr为云台相机的内参矩阵，R为球体的半径，z为跟踪目标在相机坐标系下的Z轴坐标。
具体的，根据切点A、B、C、D在相机坐标系下的坐标，确定所述目标框在图像的纵轴方向上的最高点和最低点以及横轴方向上的最左点和最右点，参阅图3a和图3b，其中，切点A对应最高点a，其在图像中的像素坐标为(u a,v a)，切点B对应最低点b，其在图像中的像素 坐标为(u b,v b)，切点C对应最左点c，其在图像中的像素坐标为(u c,v c)，切点D对应最右点d，其在图像中的像素坐标为(u d,v d)；
其中，确定所述目标框在图像中的估计高度，相当于确定所述切点A对应的最高点a与切点B对应的最低点b在图像中的距离，具体的，切点A和切点B在图像中的距离，即估计高度height可以表示为：
Figure PCTCN2021108888-appb-000040
其中，K intr为云台相机的内参矩阵，P cam,B为切点B在相机坐标系下的三维坐标，p z,B为切点B在世界坐标系下的三维坐标，P cam,A为切点A在相机坐标系下的三维坐标，p z,A为切点A在世界坐标系下的三维坐标，在本发明实施例中，所述云台相机的内参矩阵K intr通过事先标定获得，例如通过相机标定技术获取。
由于切点A和切点B两者之间只有y轴坐标不一致，因此，上式
Figure PCTCN2021108888-appb-000041
可以被简化为：
Figure PCTCN2021108888-appb-000042
相当于
Figure PCTCN2021108888-appb-000043
其中，K intr为云台相机的内参矩阵，R为球体的半径，z为跟踪目标在相机坐标系下的Z轴坐标。
具体的，所述确定所述目标框在图像中的估计宽度，包括：
假设所述跟踪目标在图像中的目标框在橫轴方向上的最左点为c，最右点为d，所述最左点c对应所述球体的切点为C，所述最右点d对应所述球体的切点为D，所述切点C在像素坐标系下的像素坐标为(u c,v c)，所述切点B在像素坐标系下的像素坐标为(u d,v d)，所述最左点c和最右点d的宽度差为width，则所述目标框在图像中的估计宽度为
Figure PCTCN2021108888-appb-000044
其中，K intr为云台相机的内参矩阵，R为球体的半径，z为跟踪目标在相机坐标系下的Z轴坐标。
具体的，确定所述目标框在图像中的估计宽度，相当于确定所述切点C对应的最左点c与切点D对应的最右点d在图像中的距离，同理，切点C和切点D在图像中的距离，即估计宽度width可以表示为：
Figure PCTCN2021108888-appb-000045
其中，K intr为云台相机的内参矩阵，P cam,D为切点D在相机坐标系下的三维坐标，P cam,C为切点C在相机坐标系下的三维坐标，z为跟踪 目标在相机坐标系下的Z轴坐标，上式
Figure PCTCN2021108888-appb-000046
可以被简化为：
Figure PCTCN2021108888-appb-000047
通过根据所述跟踪目标在图像中的像素位置信息，确定所述跟踪目标在图像中的高度差和宽度差，有利于确定跟踪目标的在图像中的位置，为估计图像在下一时刻的位置提供有利条件。
步骤S30：获取跟踪目标在图像中的实际宽度和实际高度；
具体的，所述跟踪目标在图像中的实际宽度和实际高度由外部模块获取，例如通过跟踪模块获取所述跟踪目标在图像中的目标框，并测量所述目标框的实际宽度和实际高度。
步骤S40：根据所述估计宽度和估计高度与所述实际高度和实际宽度，确定误差项，所述误差项包括高度差和宽度差；
具体的，所述高度差为所述估计高度和实际高度之差的绝对值，即高度差＝|估计高度-实际高度|，所述宽度差为所述估计宽度与实际宽度之差，即宽度差＝|估计宽度-实际宽度|。
步骤S50：根据所述误差项，基于扩展卡尔曼滤波算法，更新所述跟踪目标在下一个时刻的目标位置信息。
其中，扩展卡尔曼滤波(extended kalman filter，EKF)，是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。
请再参阅图6，图6是图5中的步骤S50的细化流程图；
如图6所示，所述步骤S50：根据所述误差项，更新所述跟踪目标在下一个时刻的目标位置信息，包括：
根据所述误差项，基于扩展卡尔曼滤波算法，更新所述跟踪目标在下一个时刻的目标位置信息，具体的，包括如下步骤：
步骤S51：根据所述跟踪目标在图像中的估计高度和估计宽度，确定所述估计高度对跟踪目标的状态向量的第一雅克比矩阵以及所述估计宽度对跟踪目标的状态向量的第二雅克比矩阵，其中，所述状态向量为估计的目标位置信息的转置矩阵；
具体的，所述跟踪目标在图像中的估计高度为
Figure PCTCN2021108888-appb-000048
Figure PCTCN2021108888-appb-000049
所述跟踪目标的状态向量为所述跟踪目标的世界位置信息的转置矩阵，即所述跟踪目标的估计的目标位置信息对应的状态向量
Figure PCTCN2021108888-appb-000050
则所述估计高度对跟踪目标的状态向量的第一雅克比矩阵为：
Figure PCTCN2021108888-appb-000051
简化后，可得：
Figure PCTCN2021108888-appb-000052
其中，K intr为云台相机的内参矩阵，R为球体的半径，z为跟踪目标在相机坐标系下的Z轴坐标，
Figure PCTCN2021108888-appb-000053
为世界坐标系到相机坐标系的旋转矩阵。
具体的，所述跟踪目标在图像中的宽度为
Figure PCTCN2021108888-appb-000054
所述跟踪目标的状态向量为所述跟踪目标的世界位置信息的转置矩阵，即所述跟踪目标的估计的目标位置信息对应的状态向量
Figure PCTCN2021108888-appb-000055
则所述估计宽度对跟踪目标的状态向量的第二雅克比矩阵为：
Figure PCTCN2021108888-appb-000056
简化后，可得：
Figure PCTCN2021108888-appb-000057
其中，K intr为云台相机的内参矩阵，R为球体的半径，z为跟踪目标在相机坐标系下的Z轴坐标，
Figure PCTCN2021108888-appb-000058
为世界坐标系到相机坐标系的旋转矩阵。
步骤S52：根据所述第一雅克比矩阵和第二雅克比矩阵，基于扩展卡尔曼滤波算法，更新所述跟踪目标在下一个时刻的目标位置信息。
具体的，请再参阅图7，图7是图6中的步骤S52的细化流程图；
如图7所示，该步骤S52：根据所述第一雅克比矩阵和第二雅克比矩阵，基于扩展卡尔曼滤波算法，更新所述跟踪目标在下一个时刻的目标位置信息，包括：
步骤S521：在预测阶段，将所述跟踪目标在当前时刻的初始位置信息对应的状态向量作为扩展卡尔曼滤波器的估计值，根据预测方程，估计所述跟踪目标在下一个时刻的目标位置信息的预测值，其中，所述预测方程包括状态向量方程和协方差方程；
具体的，所述扩展卡尔曼滤波算法包括预测阶段和更新阶段，扩展卡尔曼滤波算法的核心思想在于预测+测量反馈，它由两部分组成，第一部分是状态预测方程，第二部分是观测方程。在预测阶段，要通 过系统状态方程计算的预测值作为先验信息，之后在观测部分再重新更新这个信息。
在预测阶段，预测方程包括状态向量方程和协方差方程，分别为：
Figure PCTCN2021108888-appb-000059
其中，x(k+1)为跟踪目标在下一个时刻的位置对应的状态向量的预测值，即估计的跟踪目标在下一个时刻的目标位置信息，x(k)为跟踪目标在当前时刻的位置对应的状态向量的测量值，F为系统状态转移矩阵，P(k+1)为协方差预测值，P(k)为协方差测量值，F T为系统状态转移矩阵的转置矩阵，Q为噪声矩阵。
在本发明实施例中，系统状态转移矩阵F可以为
Figure PCTCN2021108888-appb-000060
步骤S522：在更新阶段，根据所述第一雅克比矩阵和第二雅克比矩阵，计算卡尔曼增益；
具体的，所述卡尔曼增益K＝P(k+1)J T(HP(k+1)H T+R) -1，其中，P(k+1)为协方差预测值，J T为第一雅克比矩阵和第二雅克比矩阵的转置矩阵，H为状态到观测的转换矩阵，P(k+1)为协方差预测值，H T为状态到观测的转换矩阵的转置矩阵，R为测量噪声方差。
其中，所述J T为第一雅克比矩阵或第二雅克比矩阵的转置矩阵，具体的，针对目标框的高度，所述J T为第一雅克比矩阵J h，即
Figure PCTCN2021108888-appb-000061
Figure PCTCN2021108888-appb-000062
此时计算得到的卡尔曼增益为第一卡尔曼增益，具体的，根据所述第一雅克比矩阵，确定第一卡尔曼增益，具体包括：
第一卡尔曼增益K1＝P(k+1)J h T(HP(k+1)H T+R) -1，其中，P(k+1)为协方差预测值，J h T为第一雅克比矩阵的转置矩阵，H为状态到观测的转换矩阵，H T为状态到观测的转换矩阵的转置矩阵，R为测量噪声方差。
具体的，针对目标框的宽度，所述J T为第二雅克比矩阵J w，即
Figure PCTCN2021108888-appb-000063
此时计算得到的卡尔曼增益为第二卡尔曼增益，具体的，根据所述第二雅克比矩阵，确定第二卡尔曼增益，具体包括：
第二卡尔曼增益K2＝P(k+1)J w T(HP(k+1)H T+R) -1，其中，P(k+1)为协方差预测值，J h T为第一雅克比矩阵的转置矩阵，H为状态到观测的转换矩阵，H T为状态到观测的转换矩阵的转置矩阵，R为测量噪声方差。
步骤S523：根据所述卡尔曼增益、估计值以及预测值，更新所述预测方程，确定更新后的预测方程，并获取更新值；
具体的，根据所述第一卡尔曼增益、第二卡尔曼增益、测量值以及预测值，更新所述预测方程，确定更新后的预测方程，并获得更新值。
具体的，更新后的预测方程为：
Figure PCTCN2021108888-appb-000064
其中，x(k+1)为跟踪目标在下一个时刻的位置对应的状态向量的预测值，K为卡尔曼增益，z measurement为第一雅克比矩阵J h代表的图像中的目标框的高的测量值或者第二雅克比矩阵J w代表的图像中的目标框的宽的测量值，z prediction为估计出的跟踪目标在图像中的高和宽的预测值，P(k+1)为协方差预测值，H为状态到观测的转换矩阵。
其中，当所述z measurement为第一雅克比矩阵J h代表的图像中的目标框的高度的测量值时，所述卡尔曼增益K为第一卡尔曼增益K1，此时z prediction＝|v b-v a|，其中，v b为目标框的最高点b在像素坐标系下的纵坐标，v a为目标框的最低点a在像素坐标系下的纵坐标。
其中，当所述z measurement为第二雅克比矩阵J w代表的图像中的目标框的宽度的测量值时，所述卡尔曼增益K为第二卡尔曼增益K2，此时z prediction＝|v d-v c|，其中，v d为目标框的最右点d在像素坐标系下的纵坐标，v c为目标框的最左点c在像素坐标系下的纵坐标。
通过分别确定目标框的高的测量值、目标框的宽的测量值，分别利用第一卡尔曼增益K1和第二卡尔曼增益K2，从而确定跟踪目标在下一个时刻的位置对应的状态向量的预测值x(k+1)，即
Figure PCTCN2021108888-appb-000065
Figure PCTCN2021108888-appb-000066
Figure PCTCN2021108888-appb-000067
其中，x(k+1)为跟踪目标在下一个时刻的位置对应的状态向量的预测值，K1为第一卡尔曼增益，K2为第二卡尔曼增益，
Figure PCTCN2021108888-appb-000068
为第一雅克比矩阵J h代表的图像中的目标框的高的测量值，
Figure PCTCN2021108888-appb-000069
为第二雅克比矩阵J w代表的图像中的目标框的宽的测量值，v b为目标框的最高点b在像素坐标系下的纵坐标，v a为目标框的最低点a在像素坐标系下的纵坐标，v d为目标框的最右点d在像素坐标系下的纵坐标，v c为目标框的最左点c在像素坐标系下的纵坐标。
在本发明实施例中，通过分别计算目标框的长、宽的更新值，从而确定跟踪目标在下一个时刻的位置对应的状态向量的预测值，有利于更好地估计跟踪目标的位置。
步骤S524：根据所述更新值，更新所述跟踪目标在下一个时刻的目标位置信息。
具体的，在确定更新后的预测方程之后，将所述更新阶段获得的更新值代入所述预测阶段的预测方程，估计所述跟踪目标在下一个时刻的目标位置信息，具体的，将更新后的x(k+1)作为x(k)，P(k+1)作为P(k)，代入所述预测方程
Figure PCTCN2021108888-appb-000070
从而得到预测的跟踪目标的状态量，将所述状态量作为所述跟踪目标在下一个时刻的目标位置信息，从而得到所述跟踪目标在下一个时刻的目标位置信息。
在本发明实施例中，所述方法还包括：
将所述更新的位置信息作为估计的目标位置信息，迭代更新所述跟踪目标在下一个时刻的目标位置信息。
具体的，将所述更新的位置信息作为估计的目标位置信息，重复根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度及其之后的步骤，迭代更新所述跟踪目标在下一个时刻的目标位置信息，直至收敛到误差小于预设阈值，则停止迭代过程，通过迭代的方式，本发明能够更好地使估计的目标位置信息收敛到实际位置信息。
在本发明实施例中，通过提供一种跟踪目标的位置估计方法，应用于无人飞行器，所述无人飞行器包括云台相机，所述云台相机用于获取图像，所述方法包括：根据给定的跟踪目标在当前时刻的初始位置信息，估计所述跟踪目标在下一个时刻的目标位置信息；根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度；获取跟踪目标在图像中的实际宽度和实际高度；根据所述估计宽度和估计高度与所述实际高度和实际宽度，确定误差项，所述误差项包括高度差和宽度差；根据所述误差项，更新所述跟踪目标在下一个时刻的目标位置信息。通过计算跟踪目标在图像中的误差项，更新跟踪目标在下一个时刻的目标位置信息，本发明能够实现稳定地估计跟踪目标的位置。
请参阅图8，图8是本发明实施例提供的一种跟踪目标的位置估计装置的结构示意图；
其中，该跟踪目标的位置估计装置应用于无人飞行器，所述无人飞行器包括云台相机，所述云台相机用于获取图像，如图8所示，所述跟踪目标的位置估计装置80，包括：
目标位置信息估计单元81，用于根据给定的跟踪目标在当前时刻的初始位置信息，估计所述跟踪目标在下一个时刻的目标位置信息；
估计单元82，用于根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度；
实际单元83，用于获取跟踪目标在图像中的实际宽度和实际高度；
误差项单元84，用于根据所述估计宽度和估计高度与所述实际高度和实际宽度，确定误差项，所述误差项包括高度差和宽度差；
更新单元85，用于根据所述误差项，更新所述跟踪目标在下一个时刻的目标位置信息。
在本发明实施例中，所述估计单元，具体用于：
对所述跟踪目标进行建模，确定所述跟踪目标对应的模型，根据所述跟踪目标对应的模型，确定所述跟踪目标在图像中的目标框。
在本发明实施例中，所述估计单元，具体用于：
将所述跟踪目标建模为球体，并根据所述跟踪目标的长度、宽度以及高度，确定所述跟踪目标对应的球体的半径R；
确定跟踪目标对应的球体在图像中对应的目标框。
在本发明实施例中，所述估计单元，包括：
相机位置模块，用于根据估计的目标位置信息，确定所述跟踪目标在相机坐标系下的相机位置信息，其中，所述估计的目标位置信息为世界坐标系下的世界位置信息；
像素位置模块，用于根据所述跟踪目标在相机坐标系下的相机位置信息，确定所述跟踪目标在像素坐标系下的像素位置信息。
在本发明实施例中，所述相机位置模块，具体用于：
假设所述估计的目标位置信息P world＝(p x,p y,p z)，则所述估计的目标位置信息对应的状态向量
Figure PCTCN2021108888-appb-000071
则所述跟踪目标在相机坐标系下的相机位置信息
Figure PCTCN2021108888-appb-000072
其中，P cam＝(x,y,z) T为跟踪目标在相机坐标系下的三维坐标，
Figure PCTCN2021108888-appb-000073
为世界坐标系到相机坐标系的旋转矩阵，t为世界坐标系到相机坐标系的平移向量。
在本发明实施例中，所述像素位置模块，具体用于：
假设所述跟踪目标在像素坐标系下的像素位置信息p＝(u,v) T，则
Figure PCTCN2021108888-appb-000074
其中，K intr为云台相机的内参矩阵，P cam为跟踪目标在相机坐标系下的三维坐标，p z为跟踪目标在世界坐标系下的三维坐标。
在本发明实施例中，所述云台相机的内参矩阵
Figure PCTCN2021108888-appb-000075
其中，(f x,f y)为云台相机的焦距，(c x,c y)为主点坐标。
在一些实施例中，所述估计单元，包括：
估计高度模块，用于根据所述目标框在纵轴方向上的最高点和最低点，确定所述目标框在图像中的估计高度；
估计宽度单元，用于根据所述目标框在横轴方向上的最左点和最右点，确定所述目标框在图像中的估计宽度。
在本发明实施例中，所述估计高度模块，具体用于：
假设所述跟踪目标在图像中的目标框在纵轴方向上的最高点为a，最低点为b，所述最高点a对应所述球体的切点为A，所述最低点b对应所述球体的切点为B，所述切点A在像素坐标系下的像素坐标为(u a,v a)，所述切点B在像素坐标系下的像素坐标为(u b,v b)，所述最高点a和最低点b的高度差为height，则确定所述目标框在图像中的估计高度为
Figure PCTCN2021108888-appb-000076
其中，K intr为云台相机的内参矩阵，R为球体的半径，z为跟踪目标在相机坐标系下的Z轴坐标。
在本发明实施例中，所述估计宽度单元，具体用于：
假设所述跟踪目标在图像中的目标框在橫轴方向上的最左点为c，最右点为d，所述最左点c对应所述球体的切点为C，所述最右点d对应所述球体的切点为D，所述切点C在像素坐标系下的像素坐标为(u c,v c)，所述切点B在像素坐标系下的像素坐标为(u d,v d)，所述最左点c和最右点d的宽度差为width，则所述目标框在图像中的估计宽度为
Figure PCTCN2021108888-appb-000077
其中，K intr为云台相机的内参矩阵，R为球体的半径，z为跟踪目标在相机坐标系下的Z轴坐标。
在本发明实施例中，所述更新单元，具体用于：
根据所述跟踪目标在图像中的估计高度和估计宽度，确定所述估计高度对跟踪目标的状态向量的第一雅克比矩阵以及所述估计宽度对跟踪目标的状态向量的第二雅克比矩阵，其中，所述状态向量为估计的目标位置信息的转置矩阵；
根据所述第一雅克比矩阵和第二雅克比矩阵，基于扩展卡尔曼滤波算法，更新所述跟踪目标在下一个时刻的目标位置信息。
在本发明实施例中，所述第一雅克比矩阵为
Figure PCTCN2021108888-appb-000078
Figure PCTCN2021108888-appb-000079
所述第二雅克比矩阵为
Figure PCTCN2021108888-appb-000080
Figure PCTCN2021108888-appb-000081
其中，K intr为云台相机的内参矩阵，R为球体的半径，z为跟踪目标在相机坐标系下的Z轴坐标。
在本发明实施例中，所述扩展卡尔曼滤波算法包括预测阶段和更新阶段，所述更新单元，具体用于：
在预测阶段，将所述跟踪目标在当前时刻的初始位置信息对应的状态向量作为扩展卡尔曼滤波器的估计值，根据预测方程，估计所述跟踪目标在下一个时刻的目标位置信息的预测值，其中，所述预测方程包括状态向量方程和协方差方程；
在更新阶段，根据所述第一雅克比矩阵和第二雅克比矩阵，计算卡尔曼增益；
根据所述卡尔曼增益、估计值以及预测值，更新所述预测方程，确定更新后的预测方程，并获取更新值；
根据所述更新值，更新所述跟踪目标在下一个时刻的目标位置信息。
在本发明实施例中，所述预测方程为：
Figure PCTCN2021108888-appb-000082
其中，x(k+1)为跟踪目标在下一个时刻的位置对应的状态向量的预测值，x(k)为跟踪目标在当前时刻的位置对应的状态向量的估计值，F为系统状态转移矩阵，P(k+1)为协方差预测值，P(k)为协方差估计值，F T为系统状态转移矩阵的转置矩阵，Q为噪声矩阵。
在本发明实施例中，所述卡尔曼增益K＝P(k+1)J T(HP(k+1HT+R)-1，其中，Pk+1为协方差预测值，JT为第一雅克比矩阵或第二雅克比矩阵的转置矩阵，H为状态到观测的转换矩阵，P(k+1)为协方差预测值，H T为状态到观测的转换矩阵的转置矩阵，R为测量噪声方差。
在本发明实施例中，所述更新后的预测方程为：
Figure PCTCN2021108888-appb-000083
其中，x(k+1)为跟踪目标在下一个时刻的目标位置信息对应的状态向量的预测值，K为卡尔曼增益，z measurement为目标框的高度或宽度的测量值，z prediction为第一雅克比矩阵J h代表的目标框的高度的预测值或者第二 雅克比矩阵J w代表的目标框的宽度的预测值，P(k+1)为协方差预测值，H为状态到观测的转换矩阵。
在本发明实施例中，所述更新单元，具体用于：
将更新阶段获得的更新值代入所述预测阶段的预测方程，更新所述跟踪目标在下一个时刻的目标位置信息。
在本发明实施例中，所述装置还包括：
迭代单元，用于将所述更新的位置信息作为估计的目标位置信息，迭代更新所述跟踪目标在下一个时刻的目标位置信息。
需要说明的是，上述装置可执行本申请实施例所提供的方法，具备执行方法相应的功能模块和有益效果。未在装置实施例中详尽描述的技术细节，可参见本申请实施例所提供的方法。
在本发明实施例中，通过提供一种跟踪目标的位置估计装置，应用于无人飞行器，所述无人飞行器包括云台相机，所述云台相机用于获取图像，所述装置包括：目标位置信息估计单元，用于根据给定的跟踪目标在当前时刻的初始位置信息，估计所述跟踪目标在下一个时刻的目标位置信息；估计单元，用于根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度；实际单元，用于获取跟踪目标在图像中的实际宽度和实际高度；误差项单元，用于根据所述估计宽度和估计高度与所述实际高度和实际宽度，确定误差项，所述误差项包括高度差和宽度差；更新单元，用于根据所述误差项，更新所述跟踪目标在下一个时刻的目标位置信息。通过计算跟踪目标在图像中的误差项，更新跟踪目标在下一个时刻的目标位置信息，本发明能够实现稳定地估计跟踪目标的位置。
请再参阅图9，图9是本发明实施例提供的一种无人飞行器的跟踪系统的控制器的硬件结构示意图；
如图9所示，该控制器31包括：一个或多个处理器31a以及存储器31b，图9中以一个处理器31a为例。
处理器31a和存储器31b可以通过总线或者其他方式连接，图9中以通过总线连接为例。
处理器31a，用于根据给定的跟踪目标在当前时刻的初始位置信息，估计所述跟踪目标在下一个时刻的目标位置信息；根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度；获取跟踪目标在图像中的实际宽度和实际高度；根据所述估计宽度和估计高度与所述实际高度和实际宽度，确定误差项，所述误差项包括高度差和宽度差；根据所述误差项，更新所述跟踪目标在下一个时刻的目标位置信息。
存储器31b作为一种非易失性计算机可读存储介质，可用于存储非易失性软件程序、非易失性计算机可执行程序以及模块，如本申请实施例中的跟踪目标的位置估计方法对应的程序指令/模块。处理器31a通过运行存储在存储器31b中的非易失性软件程序、指令以及模块，从而执行控制器的各种功能应用以及数据处理，即实现上述方法实施例的跟踪目标的位置估计方法。
存储器31b可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储根据控制器的使用所创建的数据等。此外，存储器31b可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实施例中，存储器31b可选包括相对于处理器31a远程设置的存储器，这些远程存储器可以通过网络连接至控制器。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
所述一个或者多个模块存储在所述存储器31b中，当被所述一个或者多个处理器31a执行时，执行上述任意方法实施例中的跟踪目标的位置估计方法，例如，执行以上描述的图4中的方法步骤S10至步骤S50；实现图8中的模块的功能。
需要说明的是，上述产品可执行本申请实施例所提供的方法，具备执行方法相应的功能模块和有益效果。未在本实施例中详尽描述的技术细节，可参见本申请实施例所提供的方法。
本申请实施例提供了一种非易失性计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，该计算机可执行指令被一个或多个处理器执行，例如图9中的一个处理器31a，可使得上述一个或多个处理器可执行上述任意方法实施例中的跟踪目标的位置估计方法，执行以上描述的图4中的方法步骤S10至步骤S50；实现图8中的模块的功能。
以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。
通过以上的实施例的描述，本领域普通技术人员可以清楚地了解到各实施例可借助软件加通用硬件平台的方式来实现，当然也可以通过硬件。本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括 如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory,ROM)或随机存储记忆体(RandomAccessMemory,RAM)等。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；在本发明的思路下，以上实施例或者不同实施例中的技术特征之间也可以进行组合，步骤可以以任意顺序实现，并存在如上所述的本发明的不同方面的许多其它变化，为了简明，它们没有在细节中提供；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (22)

  1. 一种跟踪目标的位置估计方法，应用于无人飞行器，所述无人飞行器包括云台相机，所述云台相机用于获取图像，其特征在于，所述方法包括：
    根据给定的跟踪目标在当前时刻的初始位置信息，估计所述跟踪目标在下一个时刻的目标位置信息；
    根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度；
    获取跟踪目标在图像中的实际宽度和实际高度；
    根据所述估计宽度和估计高度与所述实际高度和实际宽度，确定误差项，所述误差项包括高度差和宽度差；
    根据所述误差项，更新所述跟踪目标在下一个时刻的目标位置信息。
  2. 根据权利要求1所述的方法，其特征在于，所述根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度，包括：
    对所述跟踪目标进行建模，确定所述跟踪目标对应的模型，根据所述跟踪目标对应的模型，确定所述跟踪目标在图像中的目标框。
  3. 根据权利要求2所述的方法，其特征在于，所述对所述跟踪目标进行建模，确定所述跟踪目标对应的模型，根据所述跟踪目标对应的模型，确定所述跟踪目标在图像中的目标框，包括：
    将所述跟踪目标建模为球体，并根据所述跟踪目标的长度、宽度以及高度，确定所述跟踪目标对应的球体的半径R；
    确定跟踪目标对应的球体在图像中对应的目标框。
  4. 根据权利要求3所述的方法，其特征在于，所述根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度，包括：
    根据估计的目标位置信息，确定所述跟踪目标在相机坐标系下的相机位置信息，其中，所述估计的目标位置信息为世界坐标系下的世界位置信息；
    根据所述跟踪目标在相机坐标系下的相机位置信息，确定所述跟踪目标在像素坐标系下的像素位置信息。
  5. 根据权利要求4所述的方法，其特征在于，所述根据估计的目标位置信息，确定所述跟踪目标在相机坐标系下的相机位置信息，包括：
    假设所述估计的目标位置信息P world＝(p x,p y,p z)，则所述估计的目标位置信息对应的状态向量
    Figure PCTCN2021108888-appb-100001
    则所述跟踪目标在相机坐标系下的相机位置信息
    Figure PCTCN2021108888-appb-100002
    其中，P cam＝(x,y,z) T为跟踪目标在相机坐标系下的三维坐标，
    Figure PCTCN2021108888-appb-100003
    为世界坐标系到相机坐标系的旋转矩阵，t为世界坐标系到相机坐标系的平移向量。
  6. 根据权利要求5所述的方法，其特征在于，所述根据所述跟踪目标在相机坐标系下的相机位置信息，确定所述跟踪目标在像素坐标系下的像素位置信息，包括：
    假设所述跟踪目标在像素坐标系下的像素位置信息p＝(u,v) T，则
    Figure PCTCN2021108888-appb-100004
    其中，K intr为云台相机的内参矩阵，P cam为跟踪目标在相机坐标系下的三维坐标，p z为跟踪目标在世界坐标系下的三维坐标。
  7. 根据权利要求6所述的方法，其特征在于，所述云台相机的内参矩阵
    Figure PCTCN2021108888-appb-100005
    其中，(f x,f y)为云台相机的焦距，(c x,c y)为主点坐标。
  8. 根据权利要求7所述的方法，其特征在于，所述确定所述跟踪目标在图像中的估计宽度和估计高度，包括：
    根据所述目标框在纵轴方向上的最高点和最低点以及横轴方向上的最左点和最右点，分别确定所述目标框在图像中的估计高度和估计宽度。
  9. 根据权利要求8所述的方法，其特征在于，所述确定所述目标框在图像中的估计高度，包括：
    假设所述跟踪目标在图像中的目标框在纵轴方向上的最高点为a，最低点为b，所述最高点a对应所述球体的切点为A，所述最低点b对应所述球体的切点为B，所述切点A在像素坐标系下的像素坐标为(u a,v a)，所述切点B在像素坐标系下的像素坐标为(u b,v b)，所述最高点a和最低点b的高度差为height，则确定所述目标框在图像中的估计 高度为
    Figure PCTCN2021108888-appb-100006
    其中，K intr为云台相机的内参矩阵，R为球体的半径，z为跟踪目标在相机坐标系下的Z轴坐标。
  10. 根据权利要求8所述的方法，其特征在于，所述确定所述目标框在图像中的估计宽度，包括：
    假设所述跟踪目标在图像中的目标框在橫轴方向上的最左点为c，最右点为d，所述最左点c对应所述球体的切点为C，所述最右点d对应所述球体的切点为D，所述切点C在像素坐标系下的像素坐标为(u c,v c)，所述切点B在像素坐标系下的像素坐标为(u d,v d)，所述最左点c和最右点d的宽度差为width，则所述目标框在图像中的估计宽度
    Figure PCTCN2021108888-appb-100007
    其中，K intr为云台相机的内参矩阵，R为球体的半径，z为跟踪目标在相机坐标系下的Z轴坐标。
  11. 根据权利要求1所述的方法，其特征在于，所述根据所述误差项，更新所述跟踪目标在下一个时刻的目标位置信息，包括：
    根据所述误差项，基于扩展卡尔曼滤波算法，更新所述跟踪目标在下一个时刻的目标位置信息。
  12. 根据权利要求11所述的方法，其特征在于，所述根据所述误差项，基于扩展卡尔曼滤波算法，更新所述跟踪目标在下一个时刻的目标位置信息，包括：
    根据所述跟踪目标在图像中的估计高度和估计宽度，确定所述估计高度对跟踪目标的状态向量的第一雅克比矩阵以及所述估计宽度对跟踪目标的状态向量的第二雅克比矩阵，其中，所述状态向量为估计的目标位置信息的转置矩阵；
    根据所述第一雅克比矩阵和第二雅克比矩阵，基于扩展卡尔曼滤波算法，更新所述跟踪目标在下一个时刻的目标位置信息。
  13. 根据权利要求12所述的方法，其特征在于，所述第一雅克比矩阵为
    Figure PCTCN2021108888-appb-100008
    所述第二雅克比矩阵为
    Figure PCTCN2021108888-appb-100009
    其中，K intr为云台相机的内参矩阵，R为球体的半径，z为跟踪目标在相机坐标系下的Z轴坐标。
  14. 根据权利要求13所述的方法，其特征在于，所述扩展卡尔曼滤波算法包括预测阶段和更新阶段，所述根据所述第一雅克比矩阵和第二雅克比矩阵，基于扩展卡尔曼滤波算法，更新所述跟踪目标在下一个时刻的目标位置信息，包括：
    在预测阶段，将所述跟踪目标在当前时刻的初始位置信息对应的状态向量作为扩展卡尔曼滤波器的估计值，根据预测方程，估计所述跟踪目标在下一个时刻的目标位置信息的预测值，其中，所述预测方程包括状态向量方程和协方差方程；
    在更新阶段，根据所述第一雅克比矩阵和第二雅克比矩阵，计算卡尔曼增益；
    根据所述卡尔曼增益、估计值以及预测值，更新所述预测方程，确定更新后的预测方程，并获取更新值；
    根据所述更新值，更新所述跟踪目标在下一个时刻的目标位置信息。
  15. 根据权利要求14所述的方法，其特征在于，所述预测方程为：
    Figure PCTCN2021108888-appb-100010
    其中，x(k+1)为跟踪目标在下一个时刻的位置对应的状态向量的预测值，x(k)为跟踪目标在当前时刻的位置对应的状态向量的估计值，F为系统状态转移矩阵，P(k+1)为协方差预测值，P(k)为协方差估计值，F T为系统状态转移矩阵的转置矩阵，Q为噪声矩阵。
  16. 根据权利要求15所述的方法，其特征在于，所述根据所述第一雅克比矩阵和第二雅克比矩阵，计算卡尔曼增益，包括：
    卡尔曼增益K＝P(k+1)J T(HP(k+1)H T+R) -1，其中，P(k+1)为协方差预测值，J T为第一雅克比矩阵或第二雅克比矩阵的转置矩阵，H为状态到观测的转换矩阵，P(k+1)为协方差预测值，H T为状态到观测的转换矩阵的转置矩阵，R为测量噪声方差。
  17. 根据权利要求16所述的方法，其特征在于，所述根据所述卡尔曼增益、测量值以及预测值，更新所述预测方程，确定更新后的预测方程，包括：
    更新后的预测方程为：
    Figure PCTCN2021108888-appb-100011
    其中，x(k+1)为跟踪目标在下一个时刻的目标位置信息对应的状态向量的预测值，K为卡尔曼增益，z measurement为目标框的高度或宽度的测量值，z prediction为第一雅克比矩阵J h代表的目标框的高度的预测值或者第二雅克比矩阵J w代表的目标框的宽度的预测值，P(k+1)为协方差预测值，H为状态到观测的转换矩阵。
  18. 根据权利要求17所述的方法，其特征在于，所述根据所述更新后的预测方程，更新所述跟踪目标在下一个时刻的目标位置信息，包括：
    将更新阶段获得的更新值代入所述预测阶段的预测方程，更新所述跟踪目标在下一个时刻的目标位置信息。
  19. 根据权利要求18所述的方法，其特征在于，所述方法还包括：
    将所述更新的位置信息作为估计的目标位置信息，迭代更新所述跟踪目标在下一个时刻的目标位置信息。
  20. 一种跟踪目标的位置估计装置，应用于无人飞行器，所述无人飞行器包括云台相机，所述云台相机用于获取图像，其特征在于，所述装置包括：
    目标位置信息估计单元，用于根据给定的跟踪目标在当前时刻的初始位置信息，估计所述跟踪目标在下一个时刻的目标位置信息；
    估计单元，用于根据估计的目标位置信息，确定所述跟踪目标在图像中的估计宽度和估计高度；
    实际单元，用于获取跟踪目标在图像中的实际宽度和实际高度；
    误差项单元，用于根据所述估计宽度和估计高度与所述实际高度和实际宽度，确定误差项，所述误差项包括高度差和宽度差；
    更新单元，用于根据所述误差项，更新所述跟踪目标在下一个时刻的目标位置信息。
  21. 一种无人飞行器，其特征在于，所述无人飞行器包括机身、与所述机身相连的机臂、设于所述机臂的动力系统、设置于所述机身 的跟踪系统；其中，所述跟踪系统包括控制器，所述控制器包括：
    至少一个处理器；以及，
    与所述至少一个处理器通信连接的存储器；其中，
    所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1-19任一项所述的方法。
  22. 一种非易失性计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机可执行指令，当所述计算机可执行指令被无人飞行器执行时，使所述无人飞行器执行如权利要求1-19任一项所述的方法。
PCT/CN2021/108888 2020-08-31 2021-07-28 一种跟踪目标的位置估计方法、装置及无人飞行器 WO2022042184A1 (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
US18/173,935 US20230215024A1 (en) 2020-08-31 2023-02-24 Position estimation method and apparatus for tracking target, and unmanned aerial vehicle

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202010896152.XA CN112037260B (zh) 2020-08-31 一种跟踪目标的位置估计方法、装置及无人飞行器
CN202010896152.X 2020-08-31

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US18/173,935 Continuation US20230215024A1 (en) 2020-08-31 2023-02-24 Position estimation method and apparatus for tracking target, and unmanned aerial vehicle

Publications (1)

Publication Number Publication Date
WO2022042184A1 true WO2022042184A1 (zh) 2022-03-03

Family

ID=73587202

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2021/108888 WO2022042184A1 (zh) 2020-08-31 2021-07-28 一种跟踪目标的位置估计方法、装置及无人飞行器

Country Status (2)

Country Link
US (1) US20230215024A1 (zh)
WO (1) WO2022042184A1 (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115511955A (zh) * 2022-11-18 2022-12-23 西湖大学 分布式地对空协同对未知无人机状态估计方法及其应用
CN115713565A (zh) * 2022-12-16 2023-02-24 盐城睿算电子科技有限公司 一种双目伺服相机目标定位方法
CN116188533A (zh) * 2023-04-23 2023-05-30 深圳时识科技有限公司 特征点跟踪方法与装置、电子设备

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116952081B (zh) * 2023-07-26 2024-04-16 武汉巨合科技有限公司 灭火弹落点参数图像空中监测系统及监测方法
CN117495900A (zh) * 2023-11-06 2024-02-02 长沙理工大学 基于相机运动趋势估计的多目标视觉跟踪方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20120048958A (ko) * 2010-11-08 2012-05-16 재단법인대구경북과학기술원 객체 추적 및 위치 추정 방법
CN108802707A (zh) * 2018-08-31 2018-11-13 中国科学院电子学研究所 改进的用于目标跟踪的卡尔曼滤波方法
CN110222581A (zh) * 2019-05-13 2019-09-10 电子科技大学 一种基于双目相机的四旋翼无人机视觉目标跟踪方法
CN112037260A (zh) * 2020-08-31 2020-12-04 深圳市道通智能航空技术有限公司 一种跟踪目标的位置估计方法、装置及无人飞行器

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20120048958A (ko) * 2010-11-08 2012-05-16 재단법인대구경북과학기술원 객체 추적 및 위치 추정 방법
CN108802707A (zh) * 2018-08-31 2018-11-13 中国科学院电子学研究所 改进的用于目标跟踪的卡尔曼滤波方法
CN110222581A (zh) * 2019-05-13 2019-09-10 电子科技大学 一种基于双目相机的四旋翼无人机视觉目标跟踪方法
CN112037260A (zh) * 2020-08-31 2020-12-04 深圳市道通智能航空技术有限公司 一种跟踪目标的位置估计方法、装置及无人飞行器

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
YANG RUNFENG, LUO CHUNBO; ZHANG ZHICONG; LI MINGZHAO: "UAV Positioning Based on Extended Kalman Filter Algorithm", TELECOMMUNICATION ENGINEERING, DIANXUN JISHU ZAZHISHE, CN, vol. 56, no. 1, 1 January 2016 (2016-01-01), CN , pages 60 - 66, XP055904880, ISSN: 1001-893X, DOI: 10.3969/j.issn.1001-893X.2013.01.011 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115511955A (zh) * 2022-11-18 2022-12-23 西湖大学 分布式地对空协同对未知无人机状态估计方法及其应用
CN115511955B (zh) * 2022-11-18 2023-03-10 西湖大学 分布式地对空协同对未知无人机状态估计方法及其应用
CN115713565A (zh) * 2022-12-16 2023-02-24 盐城睿算电子科技有限公司 一种双目伺服相机目标定位方法
CN116188533A (zh) * 2023-04-23 2023-05-30 深圳时识科技有限公司 特征点跟踪方法与装置、电子设备
CN116188533B (zh) * 2023-04-23 2023-08-08 深圳时识科技有限公司 特征点跟踪方法与装置、电子设备

Also Published As

Publication number Publication date
US20230215024A1 (en) 2023-07-06
CN112037260A (zh) 2020-12-04

Similar Documents

Publication Publication Date Title
WO2022042184A1 (zh) 一种跟踪目标的位置估计方法、装置及无人飞行器
US10565732B2 (en) Sensor fusion using inertial and image sensors
CN110068335B (zh) 一种gps拒止环境下无人机集群实时定位方法及系统
CN112525202A (zh) 一种基于多传感器融合的slam定位导航方法及系统
EP3158293B1 (en) Sensor fusion using inertial and image sensors
Heng et al. Self-calibration and visual slam with a multi-camera system on a micro aerial vehicle
US20200378760A1 (en) Hover control
EP3158417B1 (en) Sensor fusion using inertial and image sensors
WO2021047502A1 (zh) 一种目标状态估计方法、装置和无人机
EP3158411B1 (en) Sensor fusion using inertial and image sensors
CN108303099A (zh) 基于三维视觉slam的无人机室内自主导航方法
US10322819B2 (en) Autonomous system for taking moving images from a drone, with target tracking and improved target location
WO2020103034A1 (zh) 一种无人机路径规划方法、装置及无人机
CN111338383B (zh) 基于gaas的自主飞行方法及系统、存储介质
WO2018045538A1 (zh) 无人机及其避障方法和避障系统
WO2021052334A1 (zh) 一种无人飞行器的返航方法、装置及无人飞行器
US10740986B2 (en) Systems and methods for reconstructing a moving three-dimensional object
WO2020135449A1 (zh) 一种中继点生成方法、装置和无人机
CN112184812A (zh) 提升无人机相机对AprilTag识别定位精度方法及定位方法和系统
WO2021056139A1 (zh) 获取降落位置的方法、设备、无人机、系统及存储介质
CN112037260B (zh) 一种跟踪目标的位置估计方法、装置及无人飞行器
CN112991400B (zh) 一种无人艇的多传感器辅助定位方法
WO2018133074A1 (zh) 一种基于大数据及人工智能的智能轮椅系统
Ready et al. Inertially aided visual odometry for miniature air vehicles in gps-denied environments
WO2020107487A1 (zh) 图像处理方法和无人机

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 21860024

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 21860024

Country of ref document: EP

Kind code of ref document: A1