Step scanning control method based on path planning and forward compensation

Abstract

The invention particularly relates to a step scanning control method based on path planning and forward compensation. The step scanning control method comprises the following steps: calculating an observation distance from an optical axis to a current pointing direction by using a height difference between a photoelectric turret and an observation target and a platform coordinate system attitude matrix; then, calculating an attitude angle, an azimuth angle and a pitch angle of the equivalent observation ray under a flight path horizontal coordinate system by using the observation distance, an aircraft center geographic position matrix and a platform coordinate system attitude matrix; then, obtaining a scanning track speed according to the path planning information, and then calculating an optical axis scanning angular speed instruction corresponding to the scanning track; and finally, performing feedforward compensation on the flight speed of the aircraft and the earth rotation angular velocity, and calculating to obtain an optical axis scanning inertia angular velocity instruction. According to the method, on the basis of an optical axis scanning angular velocity instruction generated by a path planning trajectory velocity angular velocity, forward compensation is performed on an optical axis scanning angular velocity instruction generated by the flight speed of an aircraft and the earth rotation angular velocity, and the method is visual and effective.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of photoelectric control, in particular to a step scanning control method based on path planning and forward compensation. BACKGROUND

[0002] In order to realize stable and clear imaging in an airborne environment, as well as rapid target aiming, capturing and tracking, the stable platform of the photoelectric pod plays a crucial role, and is developing towards high precision, no delay, reliability, high intelligence, etc. Unmanned aerial vehicles or helicopters can perform various reconnaissance and search tasks, have the advantages of rapid deployment, wide detection range, strong maneuverability, good flexibility and adaptability to complex environments. Unmanned aerial vehicles or helicopters have the above advantages, and need to carry photoelectric pods, which are generally installed under the fuselage and work with the ground console to detect ground targets.

[0003] As an important component of an aerial aircraft, the photoelectric pod realizes the important functions of detecting, scanning, target tracking and identifying a target area in various complex scenes. Due to the need for rapid search of a large target area by the aircraft during flight, the photoelectric turret needs to rotate quickly, and the boresight needs to deflect rapidly. Due to the rapid rotation, the imaging effect of the photoelectric pod is affected by the large boresight deflection, and high-quality images cannot be taken, which affects the detection and identification of the target area.

[0004] It should be noted that the information disclosed in the above background section is only used to strengthen the understanding of the background of the present application, and therefore can include information that does not constitute prior art known to those of ordinary skill in the art. SUMMARY

[0005] In view of the problem of wide-range and rapid scanning of the photoelectric pod, the present application provides a step scanning control method based on path planning and forward compensation, which can overcome the defects in the prior art to some extent.

[0006] Other characteristics and advantages of the present application will become apparent from the following detailed description, or will be learned by practice of the present application.

[0007] According to a first aspect of the present application, a step scanning control method based on path planning and forward compensation is provided, the method comprising: Step 1: calculating the observation distance of the optical axis pointing to the current target point by using the height difference between the photoelectric turret and the observed target, and the platform coordinate system attitude matrix ; Step 2: calculating the attitude angle azimuth angle and the pitch angle of the equivalent observation ray in the track horizontal coordinate system based on the observation distance and the aircraft center geographic position matrix and the platform coordinate system attitude matrix ; Step 3: Calculate the scan trajectory velocity in the track horizontal coordinate system using the equivalent observation ray in the azimuth angle and the pitch angle of the attitude angle of the track horizontal coordinate system ; Step 4: Convert the scan trajectory velocity in the track horizontal coordinate system into the first optical axis scan angular velocity command using the attitude matrix of the platform coordinate system ; Step 5: Calculate the forward motion velocity of the forward compensation aircraft through the second optical axis scan angular velocity generated by the rotation matrix projection ; Step 6: Superimpose the first optical axis scan angular velocity command and the second optical axis scan angular velocity to obtain the solution optical axis forward sector strip scan inertial angular velocity command , and use the scan inertial angular velocity command to control the optical axis scan of the photoelectric turret. In some example embodiments, the calculation of the observation distance of the optical axis pointing to the current target point , specifically: The pitch angle of the platform coordinate system when the photoelectric system observes the target is calculated as

[0008] : , ,

[0009] wherein, is a vector value of the attitude matrix of the platform coordinate system; The observation distance is calculated based on the pitch angle of the platform coordinate system according to the spherical earth model:

[0010] wherein, is the ground height of the center point of the photoelectric detection device , and is the ground height of the region to be scanned.

[0011] In some example embodiments, the calculation of the equivalent observation ray in the azimuth angle and the pitch angle of the attitude angle of the track horizontal coordinate system , specifically: The coordinates of the observation ray from the center A of the photoelectric system to the target M in the earth rectangular coordinate system are: ​​​​

[0012] wherein, is the aircraft center geographic position matrix, is the platform coordinate system attitude matrix; the unit vector coordinates of the equivalent observation ray from the photoelectric system center to the target at the beginning of the strip are which are calculated as follows:

[0013] wherein, is the photoelectric detection device center point to the target distance value; Let the unit vector coordinates of the equivalent observation ray in the track horizontal coordinate system be:

[0014] wherein, is the track horizontal coordinate system attitude matrix; From the coordinate transformation relationship, we have: .

[0015] In some example embodiments, the calculation of the scanning track speed in the track horizontal coordinate system is specifically:

[0016]

[0017] wherein, is the height difference between the carrier and the target, and ω is the rotation angular velocity of the equivalent observation ray.

[0018] In some example embodiments, the first optical axis scanning angular velocity instruction is converted by a conversion method, specifically

[0019]

[0020]

[0021] wherein, is the platform coordinate system attitude matrix, is the equivalent aircraft forward speed in the navigation coordinate.

[0022] In some example embodiments, the second optical axis scanning angular velocity is generated by a generation method, specifically:

[0023]

[0024]

[0025] wherein, is a platform coordinate system attitude matrix, is an eastward velocity, is a northward velocity, is a skyward velocity.

[0026] In some example embodiments, the optical axis forward fan-shaped strip scanning inertial angular velocity instruction is solved by a method, specifically:

[0027]

[0028] wherein, is an earth inertial velocity.

[0029] According to a second aspect of the present application, there is provided a storage medium having stored thereon a computer program which, when executed by a processor, implements the path planning and forward compensation based step scanning control method of the first aspect.

[0030] According to a third aspect of the present application, there is provided a computer program product having stored thereon a computer program which, when executed by a processor, implements the path planning and forward compensation based step scanning control method of the first aspect.

[0031] According to a fourth aspect of the present application, there is provided an electronic device comprising: a processor; and a memory for storing executable instructions of the processor; wherein the processor is configured to implement the path planning and forward compensation based step scanning control method of the first aspect via execution of the executable instructions.

[0032] The path planning and forward compensation based step scanning control method provided by the embodiments of the present application adopts a path planning trajectory velocity of a track coordinate system at a planning time, and projects the path planning trajectory velocity to an angular velocity under a platform coordinate system. The path at the track coordinate system at the planning time is stationary in a geographic coordinate system, which is conducive to forming a stable and high-quality image. The method forward compensates an optical axis scanning angular velocity instruction generated by a flight velocity of an aircraft and an earth rotation angular velocity on the basis of an optical axis scanning angular velocity instruction generated by a path planning trajectory velocity angular velocity, to obtain an optical axis scanning inertial angular velocity instruction, which is intuitive and effective.

[0033] It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the application, as claimed. BRIEF DESCRIPTION OF DRAWINGS

[0034] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the application and serve to explain the principles of the application. It is readily apparent to one skilled in the art that the following described embodiments are only exemplary and that many other embodiments can be implemented based on these drawings without paying creative labor.

[0035] Figure 1 is a flow chart of the method of the present application; Figure 2 is a plot of the aircraft scan trajectory in the ground plane. DETAILED DESCRIPTION

[0036] Example implementations are now described with reference to the drawings; it being understood that the example implementations can be implemented in various forms and should not be limited to the examples set forth herein; instead, the example implementations are provided so that this disclosure will be thorough and complete, and will fully convey the concept of the example implementations to those skilled in the art. The described features, structures, or characteristics can be combined in one or more implementations.

[0037] Furthermore, the drawings are not necessarily drawn to scale. Like reference numerals in different drawings denote like or similar parts other than for repeated description. Some of the block components, which have the same function, are denoted by the same reference numerals, and thus repeated description thereof will be omitted. Some of the block components shown in the drawings are functional entities that do not necessarily have to correspond to physically or logically independent entities. The functional entities can be implemented in software, or in one or more hardware modules or integrated circuits, or in different network and / or processor devices and / or microcontroller devices.

[0038] In order to meet the requirement of the aircraft to scan a large area quickly during flight, the application designs a stepping scanning control method based on path planning and feedforward compensation. The optoelectronic turret scans from one side to the other side according to the preset path planning track under the guidance of the control instruction, and forms a large area of continuous area search strip image. The application converts the angular velocity information corresponding to the scanning track of the unmanned aerial vehicle into the angular velocity instruction of the optical axis scanning of the optoelectronic turret, superimposes the optical axis scanning angular velocity generated by the flight speed and the earth rotation angular velocity of the aircraft to obtain the inertial angular velocity instruction of the optical axis scanning. The application can obtain the inertial angular velocity instruction of the optical axis according to the fan-shaped strip scanning by combining the heading, attitude, speed and position data of the aircraft output by the aircraft inertial navigation system, and the installation angle of the optoelectronic system, the frame observation angle of the target by the optoelectronic system, the target height information and other data.

[0039] The application obtains the inertial angular velocity instruction of the forward optical axis according to the fan-shaped strip scanning by combining the heading, attitude, speed and position data of the aircraft output by the aircraft inertial navigation system, and the installation angle of the optoelectronic system, the frame observation angle of the target by the optoelectronic system, the target height information and other data. Firstly, the observation distance of the optical axis to the current pointing direction is calculated by using the height difference between the optoelectronic turret and the observed target and the platform coordinate system attitude matrix; then, the attitude angle, azimuth angle and pitch angle of the equivalent observation ray in the track horizontal coordinate system are calculated by using the observation distance, the geographical position matrix of the aircraft center and the platform coordinate system attitude matrix; then, the scanning track speed is obtained according to the path planning information, and then the optical axis scanning angular velocity instruction corresponding to the scanning track is calculated; finally, the inertial angular velocity instruction of the optical axis scanning is calculated by feedforward compensation of the flight speed and the earth rotation angular velocity of the aircraft.

[0040] The coordinate system used in the application is as follows: Geocentric inertial coordinate system : the center of the earth is taken as the origin, the x-axis points to the vernal equinox, the y-axis is along the earth rotation axis, and the z-axis forms a Cartesian coordinate system with the other two axes and obeys the right-hand screw rule. Since the vernal equinox is at infinity, no matter where the geocenter moves on the orbit, the vector from the geocenter to the vernal equinox is always parallel. The intersection of the celestial equator and the celestial ecliptic is the vernal equinox and the autumnal equinox, respectively. The celestial sphere is a sphere with the geocenter as the center and the radius as infinity. The circle obtained by extending the earth equatorial plane to cut the celestial sphere is called the celestial equator; the circle obtained by extending the earth orbit plane to cut the celestial sphere is called the celestial ecliptic.

[0041] Geographical coordinate system : since the distance between the center of the optoelectronic system and the center of the aircraft is very small, the difference of the geographical coordinate system between the two points can be ignored. The geographical coordinate system takes the center of the optoelectronic system as the origin, and the point When the local east direction is the axis, the north direction is the axis, and the sky direction is the axis.

[0042] Track horizontal coordinate system : The track horizontal coordinate system can be obtained by rotating the geographic coordinate system around the axis, and the track angle of the aircraft is the center of the photoelectric system, and the point When the local aircraft track direction is the axis, the sky direction is the axis, the axis and the other two axes form a Cartesian coordinate system and obey the right-hand screw rule.

[0043] Platform coordinate system : The platform coordinate system has the center of the photoelectric system as the origin, and the positive direction of the axis is along the upward direction of the azimuth motor inside the photoelectric system, the positive direction of the axis is the optical axis of the photoelectric system, the axis and the other two axes form a Cartesian coordinate system and obey the right-hand screw rule.

[0044] Related coordinate change matrix calculation Aircraft center geographic position matrix Let the aircraft center geographic position matrix be , and the calculation is as follows: ; Where: the longitude and the latitude of the aircraft center are given by the known conditions.

[0045] Let the platform coordinate system attitude matrix be , .

[0046] The required known input conditions derived by the present application are as follows: 1) The flight speed of the aircraft inertial navigation center point at the current time: 2) The eastward speed , the northward speed , and the skyward speed ; 3) The distance value of the photoelectric detection device center point to the target ; 4) The ground surface height of the region to be scanned is ; 5) The ground surface height of the region to be scanned is ; 5) the center point of the photoelectric detection device The surface height is .

[0047] Referring to Figure 1 , the method can specifically include the following steps: Step 1: calculating the observation distance of the optical axis pointing to the current target point ; Step 2: calculating the azimuth angle and the pitch angle of the equivalent observation ray in the track horizontal coordinate system according to the observation distance , the geographic position matrix of the aircraft center, and the attitude matrix of the platform coordinate system ; Step 3: calculating the scanning track speed in the track horizontal coordinate system by using the azimuth angle and the pitch angle of the equivalent observation ray in the track horizontal coordinate system ; Step 4: converting the scanning track speed in the track horizontal coordinate system into the optical axis scanning angular velocity instruction by using the attitude matrix of the platform coordinate system ; Step 5: calculating the forward motion speed of the forward compensation aircraft after the optical axis scanning angular velocity generated by the rotation matrix projection ; Step 6: superimposing the scanning track angular velocity , the forward motion speed of the forward compensation aircraft , and the optical axis scanning angular velocity of the earth rotation speed to obtain the optical axis forward sector strip scanning inertial angular velocity instruction .

[0048] In the following, each step in the example embodiment will be described in more detail with reference to the accompanying drawings and examples.

[0049] Step 1: calculating the observation distance of the optical axis pointing to the current target point ; When the photoelectric system observes a target, the pitch angle of the platform coordinate system is , and the pitch angle of the platform coordinate system is obtained .

[0050] , .

[0051] The observation distance is calculated according to the spherical earth model

[0052] Step 2: According to the observed distance and the aircraft center geographic position matrix and the platform coordinate system attitude matrix, the azimuth angle and the pitch angle of the equivalent observation ray in the track horizontal coordinate system are calculated and ; Let the coordinates of the observation ray from the center of the photoelectric system to the target M in the earth rectangular coordinate system be :

[0053] The unit vector coordinates of the equivalent observation ray from the center of the photoelectric system to the target at the beginning of the strip are , which are calculated as follows: ; Let the unit vector coordinates of the equivalent observation ray in the track horizontal coordinate system be: ; The track horizontal coordinate system attitude matrix , the aircraft center geographic position matrix .

[0054] From the coordinate transformation relationship, we have: ; is the azimuth angle of the equivalent observation ray in the track horizontal system, is the pitch angle of the equivalent observation ray in the track horizontal system.

[0055] Step 3: Using the azimuth angle and the pitch angle of the equivalent observation ray in the track horizontal coordinate system and , the scanning trajectory speed in the track horizontal coordinate system is calculated ; as shown in Figure 2 ;

[0056] : the height difference between the carrier and the target, ; ω: the rotational angular velocity of the equivalent observation ray, defined as positive from left to right; Let the scanning trajectory speed in the track horizontal coordinate system be , then we have:

[0057] Step 4: Using the platform coordinate system attitude matrix, the scanning trajectory speed in the track horizontal coordinate system is converted into the optical axis scanning angular velocity command ; Let the scanning speed of the trajectory in the navigation coordinate system be , then we have:

[0058] Let the equivalent forward speed of the aircraft in the navigation coordinate system be , then we have:

[0059] For convenience of description, let

[0060] ; wherein: is the induced speed caused by the equivalent forward speed of the aircraft in the navigation coordinate system; is the induced speed caused by the rotation of the relative reference system.

[0061]

[0062] Let the angular velocity of the rotation of the relative reference system relative to the absolute reference system be , and for convenience of derivation, let ; Therefore, the induced speed caused by the rotation is: ; Calculate the optical axis scanning angular velocity command generated by the scanning trajectory speed of the flight path horizontal coordinate system ; ; ; ; The present application is two-axis, without the need to be calculated.

[0063] Step 5: Calculate the forward movement speed of the forward compensation aircraft The optical axis scanning angular velocity generated by the rotation matrix projection ; ; wherein: is the forward movement speed induced by the translation; is the induced speed caused by the rotation of the relative reference system, i.e. the speed term caused by the Coriolis acceleration.

[0064] ​

[0065] Let the angular velocity of the relative reference frame relative to the absolute reference frame be... For ease of derivation, we can set: ; Therefore, the entrainment velocity caused by rotation can be derived as: ; Calculate the forward flight speed of the aircraft The generated optical axis scanning angular velocity command ; ; ; ; This invention is a two-axis invention. There is no need to ask for it.

[0066] Step 6: Scan trajectory angular velocity Forward compensating for the forward motion speed of the aircraft Earth's rotation speed Optical axis scanning angular velocity The commands for solving the forward sector scan inertial angular velocity of the optical axis are obtained by superimposing the commands. .

[0067]

[0068] Based on the dynamic compensation control mechanism of the gyroscope-stabilized platform, the compensation command for the optical axis coordinate system can be calculated. as follows: ; in: It is Earth's inertial velocity; .

[0069] It should be noted that, as another aspect, this application also provides a storage medium, which may be included in an electronic device or may exist independently without being assembled into the electronic device. The storage medium carries one or more programs, which, when executed by an electronic device, cause the electronic device to perform the methods described in the following embodiments.

[0070] In one embodiment, this application provides a computer program product including a computer program that, when executed by a processor, implements the steps in the above-described method embodiments.

[0071] Furthermore, the above figures are merely illustrative of the processes included in the method according to exemplary embodiments of the present invention, and are not intended to be limiting. It is readily understood that the processes shown in the above figures do not indicate or limit the temporal order of these processes. Additionally, it is readily understood that these processes may be executed synchronously or asynchronously, for example, in multiple modules.

[0072] Other embodiments of the invention will readily occur to those skilled in the art upon consideration of the specification and practice of the invention herein. This application is intended to cover any variations, uses, or adaptations of the invention that follow the general principles of the invention and include common knowledge or customary techniques in the art not disclosed herein. The specification and embodiments are to be considered exemplary only, and the true scope and spirit of the invention are indicated by the claims.

[0073] It should be understood that the present invention is not limited to the precise structure described above and shown in the accompanying drawings, and various modifications and changes can be made without departing from its scope. The scope of the invention is defined only by the appended claims.

Claims

1. A step-scan control method based on path planning and forward compensation, characterized in that, The method includes: Step 1: Calculate the observation distance from the optical axis to the current target point using the height difference between the photoelectric turret and the observed target, as well as the attitude matrix of the platform coordinate system. ; Step 2: Based on observation distance In addition to the aircraft center geographic location matrix and the platform coordinate system attitude matrix, the attitude angle and azimuth of the equivalent observation ray in the track horizon coordinate system are calculated. And a pitch angle ; Step 3: Utilize the attitude angle and azimuth of the equivalent observation ray in the horizontal coordinate system of the flight track. And a pitch angle Calculate the scanning trajectory velocity in the horizontal coordinate system of the flight path. ; Step 4: Using the platform coordinate system attitude matrix, determine the scanning trajectory velocity in the horizontal coordinate system. Converted into first optical axis scanning angular velocity command ; Step 5: Calculate the forward velocity of the forward-compensated aircraft. The second optical axis scanning angular velocity generated by the rotation matrix projection ; Step 6: Send the first optical axis scanning angular velocity command Second optical axis scanning angular velocity The commands for solving the forward sector scan inertial angular velocity of the optical axis are obtained by superimposing the commands. The command to scan inertial angular velocity will be executed. Used to control the optical axis scanning of the photoelectric turret.

2. The method according to claim 1, characterized in that, The calculation of the observation distance from the optical axis to the current target point Specifically: The elevation angle of the platform coordinate system when calculating the target observation by the photoelectric system is: : ， in, This is a vector value of the attitude matrix in the platform coordinate system; Pitch angle based on platform coordinate system Calculate the observation distance using the spherical Earth model: in, Center point of photoelectric detection equipment The ground elevation, This represents the elevation of the area to be scanned above the ground.

3. The method according to claim 2, characterized in that, The calculation of the equivalent observation ray's attitude angle and azimuth in the track horizon coordinate system... And a pitch angle Specifically: Let the coordinates of the observation ray from the center A of the photoelectric system to the target M in the Earth's rectangular coordinate system be... for: in, For the geographical location matrix of the aircraft center, The attitude matrix is ​​the platform coordinate system. The equivalent observation ray unit vector coordinates from the center of the photoelectric system to the target at the start of the strip are: The calculation is as follows: in, Center point of photoelectric detection equipment For the target Distance value; Let the unit vector coordinates of the equivalent observation ray in the horizontal coordinate system of the track be: in, The attitude matrix in the horizontal coordinate system of the flight track; From the coordinate transformation relationship, we can know that: 。 4. The method according to claim 3, characterized in that, The scanning trajectory velocity in the horizontal coordinate system of the calculated track Specifically: in, Let ω be the altitude difference between the aircraft and the target, and ω be the angular velocity of the equivalent observation ray.

5. The method according to claim 4, characterized in that, First optical axis scanning angular velocity command The conversion method is as follows: in, The attitude matrix is ​​the platform coordinate system. This is the equivalent forward velocity of the aircraft in navigation coordinates.

6. The method according to claim 5, characterized in that, The second optical axis scanning angular velocity The method for generating it is as follows: in, The attitude matrix is ​​the platform coordinate system. For eastward speed, For northbound speed, The speed is oriented upwards.

7. The method according to claim 6, characterized in that, The optical axis forward fan-shaped strip scanning inertial angular velocity command The solution method is as follows: in, It is Earth's inertial velocity.

8. A storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the step scan control method based on path planning and forward compensation as described in any one of claims 1 to 7.

9. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the step scan control method based on path planning and forward compensation as described in any one of claims 1 to 7.

10. An electronic device, characterized in that, include: processor; as well as Memory for storing the executable instructions of the processor; The processor is configured to execute the step scan control method based on path planning and forward compensation as described in any one of claims 1 to 7 by executing the executable instructions.

Images