Multi-agent collaborative target localization method based on variable-focus and variable-pose optical module
Through the multi-agent, the zoom and change posture optical module is equipped with a multi-agent to calculate the rotation matrix and the translation vector, combined with the least squares method, the problem of low long-distance target positioning accuracy in the existing technology is solved, and high-precision three-dimensional positioning and multi-objectives are achieved Positioning ability.
Patent Information
- Application Number
- CN202210403694.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-04-18
- Publication Date
- 2024-11-29
- Estimated Expiration
- 2042-04-18
Smart Images

Figure CN114777786B_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field of visual perception positioning, and in particular relates to a multi-agent collaborative target positioning method based on a variable-focus and variable-pose optical module. Background Art
[0002] With the continuous development of navigation and positioning technology, intelligent agents can obtain their own three-dimensional position information, as well as their own yaw, pitch and roll angles; vision-based perception modules are inexpensive and can obtain rich texture information, but monocular cameras cannot directly obtain the depth information of the target, and binocular cameras are limited by the baseline length between the left and right cameras, so the positioning accuracy of long-distance targets is not high. The perception range of laser radar is limited. The farther the distance, the lower the resolution of the radar. Although laser ranging has high accuracy, the farther the laser is emitted, the greater the diffusion of its cone surface, and it is easily interfered by objects between the target and the laser source, and it is impossible to determine whether the ranging point accurately hits the target. Summary of the invention
[0003] The purpose of the present invention is to overcome the shortcomings of the above-mentioned technology and to provide a multi-agent collaborative target positioning method based on a variable-focus and variable-pose optical module. Multiple agents are equipped with variable-focus and variable-pose optical modules to accurately position the target in three dimensions according to their own positions and postures and the postures of the optical modules relative to the agents, and in combination with the positions of the positioning targets in the images of each optical module.
[0004] In order to achieve the above-mentioned purpose, the present invention adopts the following technical scheme: a multi-agent collaborative target positioning method based on a zoom-variable and posture-variable optical module, characterized in that: using more than two agents capable of self-positioning, equipped with zoom-variable and posture-variable optical modules, according to their own positions and postures, and the positions and postures of the optical modules relative to the agents, and in combination with the positions of the positioning targets in the images of each optical module, the three-dimensional position of the target is precisely positioned collaboratively, and the specific steps are as follows:
[0005] Step 1: The agent obtains its current position in the world coordinate system O w -x w y w z w Position and posture of the lower body;
[0006] Step 2: The optical module obtains its own posture relative to the intelligent body;
[0007] Step 3: Calculate the optical center coordinate system o of each optical module i -x c y c z c With the agent coordinate system o v -x v y v zv The rotation matrix between
[0008] Step 4: Calculate the coordinate system o of each agent v -x v y v z v With the world coordinate system O w -x w y w z w The rotation matrix between
[0009] Step 5: Calculate the optical center coordinate system o of each optical module i -x c y c z c With the world coordinate system O w -x w y w z w The rotation matrix R i and the translation vector T i ;
[0010] Step 6: Calculate the internal parameter matrix K of each optical module at the current zoom value zoom i ;
[0011] Step 7: Calculate the world coordinate system O of the located target w -x w y w z w The coordinates under are converted to the pixel coordinate system of each optical module o i -Transformation matrix H of the coordinates under uv i ;
[0012] Step 8: Based on the multiple transformation matrices in step 7, the simultaneous equations are used to calculate the three-dimensional position P(x w ,y w ,z w ).
[0013] Furthermore, the current position obtained in step 1 is the three-dimensional position coordinates of each agent in the world coordinate system t i (a i ,b i ,c i ), obtain its own posture including the Euler angle of the intelligent body coordinate system relative to the world coordinate system; Euler angle includes yaw angle, pitch angle and roll angle.
[0014] Furthermore, the position of the optical module is set to coincide with the positioning center of the intelligent body, and the optical module obtains its own posture relative to the intelligent body as the optical center coordinate system o i -x c y c z c Relative to the agent coordinate system o i -x v y v z v The horizontal angle is pan, and the pitch angle is tilt.
[0015] Furthermore, in step 3, the optical center coordinate system o of the optical module i -x c y c z c With the agent coordinate system o v -x v y v z v The rotation matrix between The solution is: let the rotation vector of horizontal rotation in the intelligent body coordinate system be v v , the pitch rotation vector in the intelligent body coordinate system is h v , then the rotation matrix between the optical center coordinate system and the intelligent body coordinate system is:
[0016]
[0017] in, Represents the rotation vector v v The rotation matrix of the pan angle can be obtained according to the Rodriguez formula: The meaning and same.
[0018] Further step 4: the agent coordinate system o v -x v y v z v With the world coordinate system O w -x w y w z w The rotation matrix between The solution is: Assume that in the world coordinate system, the rotation vector of the yaw angle yaw is v w , the rotation vector of the pitch angle is h w , the rotation vector of the roll angle roll is r w , then the rotation matrix between the optical center coordinate system and the intelligent body coordinate system is:
[0019]
[0020] in The meaning of same.
[0021] Furthermore, in step 5, the optical center coordinate system o of each optical module i -x c y c z c With the world coordinate system O w -x w y w z w The rotation matrix R i and the translation vector T i , and its solution is:
[0022]
[0023] Furthermore, the internal parameter matrix K of each optical module at the current zoom value described in step 6 is i The solution is: according to the camera zoom characteristics, establish the function model between the zoom values in the camera, and then calculate the internal parameter matrix K according to the current zoom value i .
[0024] Furthermore, the coordinate transformation matrix H of the located target in step 7 is i , which is calculated as:
[0025]
[0026] That is, the target P(x w ,y w ,z w ) is converted to the pixel coordinate system i (u i ,v i ) is:
[0027] (u i ,v i ,1) T =H i (x w ,y w ,z w ,1) T (6).
[0028] Furthermore, the process of calculating the three-dimensional position of the target by using the least square method in step 8 is as follows: expand the transformation relationship of each group of equations (6) to obtain 2i groups of equations, and combine them into a system of simultaneous equations to obtain:
[0029] A (2i)×3 (x w ,y w,z w ) T =B (2i)×1 (7)
[0030] The least square method is used to solve equation (7) to obtain the three-dimensional position coordinates P(x w ,y w ,z w ).
[0031] Beneficial effects: Compared with the prior art, the invention can form a cluster of intelligent agents equipped with variable focus and variable posture optical modules, and use the above method to achieve reconnaissance and positioning of targets. Even if a certain intelligent agent is destroyed, as long as the number of intelligent agents in the cluster is greater than or equal to 2, the above function can still be achieved. When the number of single intelligent agents is large, the intelligent agents can be divided into multiple groups to achieve positioning of multiple targets. BRIEF DESCRIPTION OF THE DRAWINGS
[0032] Figure 1 It is a schematic diagram of multi-agent collaborative target positioning;
[0033] Figure 2 It is a top view of a single intelligent body equipped with a variable-focus and variable-pose optical module;
[0034] Figure 3 It is a side view of a single intelligent body equipped with a variable-focus and variable-pose optical module;
[0035] Figure 4 This is a front view of a single intelligent entity equipped with a zoom and posture-adjustable optical module.
[0036] Figure 2 Middle: yaw is the yaw angle of the agent relative to the world coordinate system, and pan is the horizontal rotation angle of the optical module relative to the agent coordinate system;
[0037] Figure 3 Middle: pitch is the pitch angle of the agent relative to the world coordinate system, and tilt is the pitch angle of the optical module relative to the agent coordinate system;
[0038] Figure 4 Middle: roll is the roll angle of the agent relative to the world coordinate system. DETAILED DESCRIPTION
[0039] In order to be able to more clearly understand the above-mentioned objects, features and advantages of the present invention, the present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be noted that, in the absence of conflict, the embodiments of the present application and the features in the embodiments can be combined with each other. In the following description, many specific details are set forth in order to fully understand the present invention, and the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in the art without making creative work are within the scope of protection of the present invention. Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those generally understood by technicians in the technical field of the present invention. The terms used in the specification of the present invention herein are only for the purpose of describing specific embodiments and are not intended to limit the present invention.
[0040] As shown in the accompanying drawings, this embodiment provides a multi-agent collaborative target positioning method based on a variable-focus, variable-pose optical module, using two or more agents that can position themselves, equipped with a variable-focus, variable-pose optical module, according to their own position and posture, as well as the position and posture of the optical module relative to the agent, and combined with the position of the positioning target in the image of each optical module, the three-dimensional position of the target is accurately positioned collaboratively, and the specific steps are as follows:
[0041] Step 1: The agent obtains its current position in the world coordinate system O w -x w y w z w Position and posture of the lower body;
[0042] Step 2: The optical module obtains its own posture relative to the intelligent body;
[0043] Step 3: Calculate the optical center coordinate system o of each optical module i -x c y c z c With the agent coordinate system o v -x v y v z v The rotation matrix between
[0044] Step 4: Calculate the coordinate system o of each agent v -x v y v z v With the world coordinate system O w -x w y w z w The rotation matrix between
[0045] Step 5: Calculate the optical center coordinate system o of each optical module i -x c y c z c With the world coordinate system O w -x w y w z w The rotation matrix R i and the translation vector T i ;
[0046] Step 6: Calculate the internal parameter matrix K of each optical module at the current zoom value zoom i ;
[0047] Step 7: Calculate the world coordinate system O of the located target w -x w y w z w The coordinates under are converted to the pixel coordinate system of each optical module o i -Transformation matrix H of the coordinates under uv i ;
[0048] Step 8: Based on the multiple transformation relationships in step 7, the three-dimensional position P(x w ,y w ,z w ).
[0049] Furthermore, the current position obtained in step 1 is the three-dimensional position coordinates of each agent in the world coordinate system t i (a i ,b i ,c i ), obtain its own posture including the Euler angle of the intelligent body coordinate system relative to the world coordinate system; Euler angle includes yaw angle, pitch angle and roll angle.
[0050] Furthermore, the position of the optical module is set to coincide with the positioning center of the intelligent body, and the optical module obtains its own posture relative to the intelligent body as the optical center coordinate system o i -x c y c z c Relative to the agent coordinate system o i -x v y v z v The horizontal angle is pan, and the pitch angle is tilt.
[0051] Furthermore, the optical center coordinate system of the optical module described in step 3 is i -xc y c z c With the agent coordinate system o v -x v y v z v The rotation matrix between The solution is: let the rotation vector of the horizontal angle pan in the intelligent body coordinate system be v v , the pitch rotation tilt vector is h v , then the rotation matrix between the optical center coordinate system and the intelligent body coordinate system is:
[0052]
[0053] in, Represents the rotation vector v v The rotation matrix of the pan angle can be obtained according to the Rodriguez formula: The meaning and same.
[0054] Furthermore, the agent coordinate system o described in step 4 v -x v y v z v With the world coordinate system O w -x w y w z w The rotation matrix R i v The solution is: Assume that in the world coordinate system, the rotation vector of the yaw angle yaw is v w , the rotation vector of the pitch angle is h w , the rotation vector of the roll angle roll is r w , then the rotation matrix between the optical center coordinate system and the intelligent body coordinate system is:
[0055]
[0056] in The meaning of same.
[0057] Furthermore, the optical center coordinate system o of each optical module is calculated as described in step 5. i -x c y c z c With the world coordinate system O w -x w y w z w The rotation matrix R i and the translation vector Ti , and its solution is:
[0058]
[0059] Furthermore, the intrinsic parameter matrix K is calculated at the current focal length in step 6. The solution method is to establish a relationship model between the internal parameters of the camera and the zoom value according to the camera zoom characteristics, and then estimate the intrinsic parameter matrix K according to the current zoom value. i .
[0060] Furthermore, the coordinate transformation matrix H of the located target in step 7 is i , which is calculated as:
[0061]
[0062] That is, the target P(x w ,y w ,z w ) is converted to the pixel coordinate system i (u i ,v i ) is:
[0063] (u i ,v i ,1) T =H i (x w ,y w ,z w ,1) T (6).
[0064] Furthermore, the least squares method is used to calculate the three-dimensional position information of the target. The process is as follows: expand the transformation relationship of each group of equations (6) to obtain 2i groups of equations:
[0065]
[0066] Write the simultaneous equations in matrix form:
[0067]
[0068] Right now:
[0069] A (2i)×3 (x w ,y w ,z w ) T =B (2i)×1 (9)
[0070] Solve equation (9) using the least squares method and get P(x w ,yw ,z w )’s three-dimensional coordinates.
[0071] How it works
[0072] The key feature of the present invention is: the collaborative positioning of multiple intelligent agents equipped with optical modules with variable focus and variable posture. Emphasis on "multiple" and "collaborative linkage". 1. Each intelligent agent can share the two-dimensional position information of the target in the image, the current rotation angle of the camera, the zoom value, the intelligent agent's own position and the Euler angle of the intelligent agent's coordinate system relative to the world coordinate system with each other through wireless communication, or transmit it back to the fixed base station terminal; 2. The computing unit of each intelligent agent or the computing platform of the base station terminal can calculate the three-dimensional position of the target based on the above information.
[0073] The detailed description of the multi-agent collaborative target positioning method based on a variable-focus and variable-pose optical module in the above reference embodiment is illustrative rather than restrictive, and several embodiments can be listed according to the limited scope. Therefore, changes and modifications without departing from the overall concept of the present invention should fall within the scope of protection of the present invention.
Claims
1. A multi-agent collaborative target positioning method based on a variable-focus, variable-pose optical module, characterized by: Use two or more intelligent bodies that can locate themselves, equipped with optical modules that can change focus and posture, and coordinately locate the three-dimensional position of the target based on their own position and posture, as well as the position of the optical module relative to the intelligent body, and the position of the positioning target in the image of each optical module. The specific steps are as follows: Step 1: The agent obtains its current position in the world coordinate system O w -x w y w z w Position and posture of the lower body; Step 2: The optical module obtains its own posture relative to the intelligent body; Step 3: Calculate the optical center coordinate system o of each optical module i -x c y c z c With the agent coordinate system o v -x v y v z v The rotation matrix between Step 4: Calculate the coordinate system o of each agent v -x v y v z v With the world coordinate system O w -x w y w z w The rotation matrix between Step 5: Calculate the optical center coordinate system o of each optical module i -x c y c z c With the world coordinate system O w -x w y w z w The rotation matrix R i and the translation vector T i ; Step 6: Calculate the internal parameter matrix K of each optical module at the current zoom value zoom i ; Step 7: Calculate the world coordinate system O of the located target w -x w y w z w The coordinates under are converted to the pixel coordinate system of each optical module o i -Transformation matrix H of the coordinates under uv i ; Step 8: Based on the multiple transformation matrices in step 7, the simultaneous equations are used to calculate the three-dimensional position P(x w ,y w ,z w ).
2. The multi-agent collaborative target positioning method based on a variable-focus, variable-pose optical module according to claim 1 is characterized in that: The current position obtained in step 1 is the three-dimensional position coordinates of each agent in the world coordinate system t i (a i ,b i ,c i ), obtain its own posture including the Euler angle of the intelligent body coordinate system relative to the world coordinate system; the Euler angle includes yaw, pitch and roll.
3. The multi-agent collaborative target positioning method based on a variable-focus, variable-pose optical module according to claim 1 is characterized in that: The position of the optical module is set to coincide with the positioning center of the intelligent body itself, and the optical module obtains its own posture relative to the intelligent body as the optical center coordinate system o i -x c y c z c Relative to the agent coordinate system o i -x v y v z v The horizontal angle is pan, and the pitch angle is tilt.
4. The multi-agent collaborative target positioning method based on a variable-focus, variable-pose optical module according to claim 1 is characterized in that: Step 3: The optical center coordinate system of the optical module i -x c y c z c With the agent coordinate system o v -x v y v z v The rotation matrix between The solution is: let the rotation vector of horizontal rotation in the intelligent body coordinate system be v v , the pitch rotation vector in the intelligent body coordinate system is h v , then the rotation matrix between the optical center coordinate system and the intelligent body coordinate system is: in, Represents the rotation vector v v The rotation matrix of the pan angle can be obtained according to the Rodriguez formula: The meaning and same.
5. The multi-agent collaborative target positioning method based on a variable-focus, variable-pose optical module according to claim 2 is characterized by:
4. Intelligent body coordinate system v -x v y v z v With the world coordinate system O w -x w y w z w The rotation matrix between The solution is: Assume that in the world coordinate system, the rotation vector of the yaw angle yaw is v w , the rotation vector of the pitch angle is h w , the rotation vector of the roll angle roll is r w , then the rotation matrix between the optical center coordinate system and the intelligent body coordinate system is: 。 6. The multi-agent collaborative target positioning method based on a variable-focus, variable-pose optical module according to claim 1 is characterized by: The optical center coordinate system of each optical module described in step 5 is o i -x c y c z c With the world coordinate system O w -x w y w z w The rotation matrix R i and the translation vector T i , and its solution is:
7. The multi-agent collaborative target positioning method based on a variable-focus, variable-pose optical module according to claim 1 is characterized by: Step 6: The internal parameter matrix K of each optical module at the current zoom value i The solution is: according to the camera zoom characteristics, establish the function model between the zoom values in the camera, and then calculate the internal parameter matrix K according to the current zoom value i .
8. The multi-agent collaborative target positioning method based on a variable-focus, variable-pose optical module according to claim 6 is characterized by: Step 7: The coordinate transformation matrix H of the located target i , which is calculated as: That is, the target P(x w ,y w ,z w ) is converted to the pixel coordinate system i (u i ,v i ) is: (u i ,v i ,1) T =H i (x w ,y w ,z w ,1) T (6)。
Citation Information
Patent Citations
Monocular camera-based planar moving target visual navigation method and system
CN112179357A
Passive target real-time positioning method based on multi-rotor unmanned aerial vehicle cooperation
CN112229380A
