Position and attitude determination method for movable platform, related device and system
阅读说明：本技术 用于可移动平台的位置姿态确定方法、相关装置和系统 (Position and attitude determination method for movable platform, related device and system ) 是由 刘虎成 于 2020-10-14 设计创作，主要内容包括：本公开涉及一种用于可移动平台的位置姿态确定方法、位置姿态确定装置系统和位置姿态确定系统,该位置姿态确定方法包括：在可移动平台上设置惯性测量单元和位置速度测量单元,将惯性测量单元和位置速度测量单元设置成能够相对运动的形式；该方法还包括：获取由位置速度测量单元测得的第一位置数据和第一运动速度数据；获取对惯性测量单元的位置补偿量和运动速度补偿量；根据第一位置数据和位置补偿量计算惯性测量单元的第二位置数据,并根据第一运动速度数据和运动速度补偿量计算惯性测量单元的第二运动速度数据；以及根据第二位置数据和第二运动速度数据确定可移动平台的位置姿态。本公开通过将惯性测量单元和位置速度测量单元设置成非刚性连接的形式,为可移动平台的设计提供了便利,同时扩大了可移动平台的应用场景。(The present disclosure relates to a position and orientation determination method, a position and orientation determination apparatus system, and a position and orientation determination system for a movable platform, the position and orientation determination method including: arranging an inertia measuring unit and a position and speed measuring unit on the movable platform, and arranging the inertia measuring unit and the position and speed measuring unit in a form capable of moving relatively; the method further comprises the following steps: acquiring first position data and first movement speed data measured by a position and speed measuring unit; acquiring a position compensation quantity and a motion speed compensation quantity of an inertial measurement unit; calculating second position data of the inertial measurement unit according to the first position data and the position compensation amount, and calculating second movement speed data of the inertial measurement unit according to the first movement speed data and the movement speed compensation amount; and determining the position and attitude of the movable platform according to the second position data and the second movement speed data. The inertia measurement unit and the position and speed measurement unit are arranged in a non-rigid connection mode, so that convenience is brought to the design of the movable platform, and meanwhile the application scene of the movable platform is expanded.)
1. A position and orientation determination method for a movable platform, characterized in that an inertial measurement unit and a position and velocity measurement unit are provided on the movable platform, the inertial measurement unit and the position and velocity measurement unit being provided in a form capable of relative movement; the method comprises the following steps:
acquiring first position data and first movement speed data measured by the position and speed measurement unit;
acquiring a position compensation quantity and a motion speed compensation quantity of the inertial measurement unit;
calculating second position data of the inertial measurement unit according to the first position data and the position compensation amount, and calculating second movement speed data of the inertial measurement unit according to the first movement speed data and the movement speed compensation amount; and
determining a position attitude of the movable platform from the second position data and the second motion velocity data.
2. The position posture determination method for a movable platform according to claim 1,
acquiring the position compensation amount and the motion velocity compensation amount for the inertial measurement unit comprises acquiring by a computer vision system.
3. The position posture determination method for a movable platform according to claim 1,
arranging the inertial measurement unit and the position and velocity measurement unit in a relatively movable fashion includes pivotally connecting the inertial measurement unit and the position and velocity measurement unit with a pivot.
4. The position posture determination method for a movable platform according to claim 3,
acquiring the position compensation amount for the inertial measurement unit includes acquiring an attitude rotation matrix measured by the inertial measurement unit, and calculating an equivalent distance between the position velocity measurement unit and the inertial measurement unit.
5. The position posture determination method for a movable platform according to claim 4,
calculating an equivalent distance between the position and velocity measurement unit and the inertial measurement unit by the following formula (1):
δl＝δl0+R1(t)δl1 (1)
wherein δ l is an equivalent distance between the position and velocity measurement unit and the inertial measurement unit; delta l0Is the distance between the inertial measurement unit and the axis of rotation of the pivot; r1(t) is a relative attitude rotation matrix of the position and velocity measurement unit with respect to the inertial measurement unit at time t; delta l1Is the distance between the position and velocity measurement unit and the axis of rotation of the pivot.
6. The position posture determination method for a movable platform according to claim 5,
calculating a position compensation amount for the inertial measurement unit by the following equation (2):
PCOMP＝R(t)×δl (2)
wherein, PCOMPIs a position compensation amount for the inertial measurement unit; r (t) is an attitude rotation matrix at time t measured by the inertial measurement unit.
7. The position posture determination method for a movable platform according to claim 6,
calculating second position data of the inertial measurement unit from the first position data and the position compensation amount includes calculating the second position data by the following equation (3):
PIMU＝PRTK-PCOMP (3)
wherein, PIMUSecond position data for the inertial measurement unit; pRTKIs the first position data measured by the position velocity measurement unit.
8. The position posture determination method for a movable platform according to claim 3,
acquiring the motion velocity compensation amount of the inertial measurement unit comprises calculating an equivalent rotation angular rate of the inertial measurement unit and calculating an equivalent distance between the position velocity measurement unit and the inertial measurement unit.
9. The position posture determination method for a movable platform according to claim 8,
calculating an equivalent rotational angular rate of the inertial measurement unit by the following equation (4):
ω(t)＝ω0(t)+R1(t)×ω1(t) (4)
wherein ω (t) is an equivalent rotational angular rate of the inertial measurement unit; omega0(t) is the absolute angular rate of rotation measured by the inertial measurement unit; r1(t) is a relative attitude rotation matrix of the position velocity measurement unit with respect to the inertial measurement unit; omega1(t) is the relative rotational angular rate of the position and velocity measurement unit with respect to the inertial measurement unit.
10. The position posture determination method for a movable platform according to claim 9,
calculating an equivalent distance between the position and velocity measurement unit and the inertial measurement unit by the following equation (5):
δl＝δl0+R1(t)×δl1 (5)
wherein δ l is an equivalent distance between the position and velocity measurement unit and the inertial measurement unit; delta l0Is the distance between the inertial measurement unit and the axis of rotation of the pivot; delta l1Is the distance between the position and velocity measurement unit and the axis of rotation of the pivot.
11. The position posture determination method for a movable platform according to claim 10,
calculating a motion velocity compensation amount for the inertial measurement unit by the following equation (6):
VCOMP＝ω(t)×δl (6)
wherein, VCOMPTo compensate for the amount of motion velocity of the inertial measurement unit.
12. The position posture determination method for a movable platform according to claim 11,
calculating second movement velocity data of the inertial measurement unit from the first movement velocity data and the movement velocity compensation amount includes calculating the second movement velocity data by the following equation (7):
VIMU＝VRTK-VCOMP (7)
wherein, VIMUSecond motion speed data of the inertial measurement unit; vRTKIs the first movement speed data measured by the position speed measuring unit.
13. The position posture determination method for a movable platform according to claim 1,
the method further comprises the step of obtaining the absolute rotation angle and the absolute rotation angular rate measured by the inertial measurement unit.
14. The position posture determination method for a movable platform according to claim 1,
the movable platform comprises an unmanned aerial vehicle.
15. The position posture determination method for a movable platform according to claim 14,
the unmanned aerial vehicle comprises a vehicle body and a cradle head connected to the vehicle body, wherein the inertia measuring unit is arranged on the cradle head, and the position and speed measuring unit is arranged on the vehicle body.
16. A computer-readable storage medium having executable instructions stored thereon, wherein,
executing the executable instructions to implement the method of any of claims 1 to 15.
17. A position and orientation determination apparatus for a movable platform, comprising:
an inertial measurement unit disposed on the movable platform;
a position and velocity measurement unit disposed on the movable platform for acquiring first position data and first movement velocity data, the inertial measurement unit and the position and velocity measurement unit being disposed to be capable of relative movement;
a position compensation amount acquisition unit for acquiring a position compensation amount for the inertial measurement unit;
a motion velocity compensation amount acquisition unit for acquiring a motion velocity compensation amount for the inertial measurement unit;
a second position data calculation unit that calculates second position data of the inertial measurement unit based on the first position data and the position compensation amount;
a second movement velocity data calculation unit that calculates second movement velocity data of the inertial measurement unit based on the first movement velocity data and the movement velocity compensation amount; and
a position and orientation determination unit that determines a position and orientation of the movable platform based on the second position data and the second movement speed data.
18. The position and orientation determination apparatus for a movable platform according to claim 17,
the position compensation amount acquisition unit and/or the motion velocity compensation amount acquisition unit include a computer vision system.
19. The position and orientation determination apparatus for a movable platform according to claim 17,
the inertial measurement unit and the position and velocity measurement unit are pivotally connected with a pivot.
20. The position and orientation determination apparatus for a movable platform according to claim 19,
acquiring the position compensation amount for the inertial measurement unit includes acquiring an attitude rotation matrix measured by the inertial measurement unit, and calculating an equivalent distance between the position velocity measurement unit and the inertial measurement unit.
21. The position and orientation determination apparatus for a movable platform according to claim 20,
calculating an equivalent distance between the position and velocity measurement unit and the inertial measurement unit by the following formula (1):
δl＝δl0+R1(t)δl1 (1)
wherein δ l is an equivalent distance between the position and velocity measurement unit and the inertial measurement unit; delta l0Is the distance between the inertial measurement unit and the axis of rotation of the pivot; r1(t) is a relative attitude rotation matrix of the position and velocity measurement unit with respect to the inertial measurement unit at time t; delta l1Is the distance between the position and velocity measurement unit and the axis of rotation of the pivot.
22. The position and orientation determination apparatus for a movable platform according to claim 21,
calculating a position compensation amount for the inertial measurement unit by the following equation (2):
PCOMP＝R(t)×δl (2)
wherein，PCOMPIs a position compensation amount for the inertial measurement unit; r (t) is an attitude rotation matrix at time t measured by the inertial measurement unit.
23. The position and orientation determination apparatus for a movable platform according to claim 22,
the second position data calculation unit calculates the second position data according to the following equation (3):
PIMU＝PRTK-PCOMP (3)
wherein, PIMUSecond position data for the inertial measurement unit; pRTKIs the first position data measured by the position velocity measurement unit.
24. The position and orientation determination apparatus for a movable platform according to claim 19,
acquiring the motion velocity compensation amount of the inertial measurement unit comprises calculating an equivalent rotation angular rate of the inertial measurement unit and calculating an equivalent distance between the position velocity measurement unit and the inertial measurement unit.
25. The position and orientation determination apparatus for a movable platform according to claim 24,
calculating an equivalent rotational angular rate of the inertial measurement unit by the following equation (4):
ω(t)＝ω0(t)+R1(t)×ω1(t) (4)
wherein ω (t) is an equivalent rotational angular rate of the inertial measurement unit; omega0(t) is the absolute angular rate of rotation measured by the inertial measurement unit; r1(t) is a relative attitude rotation matrix of the position velocity measurement unit with respect to the inertial measurement unit; omega1(t) is the relative rotational angular rate of the position and velocity measurement unit with respect to the inertial measurement unit.
26. The position and orientation determination apparatus for a movable platform according to claim 25,
calculating an equivalent distance between the position and velocity measurement unit and the inertial measurement unit by the following equation (5):
δl＝δl0+R1(t)×δl1 (5)
wherein δ l is an equivalent distance between the position and velocity measurement unit and the inertial measurement unit; delta l0Is the distance between the inertial measurement unit and the axis of rotation of the pivot; delta l1Is the distance between the position and velocity measurement unit and the axis of rotation of the pivot.
27. The position and orientation determination apparatus for a movable platform according to claim 26,
calculating a motion velocity compensation amount for the inertial measurement unit by the following equation (6):
VCOMP＝ω(t)×δl (6)
wherein, VCOMPTo compensate for the amount of motion velocity of the inertial measurement unit.
28. The position and orientation determination apparatus for a movable platform according to claim 27,
the second moving speed data calculation unit calculates the second moving speed data according to the following equation (7):
VIMU＝VRTK-VCOMP (7)
wherein, VIMUSecond motion speed data of the inertial measurement unit; vRTKIs the first movement speed data measured by the position speed measuring unit.
29. The position and orientation determination apparatus for the movable platform according to claim 17, further comprising:
an absolute rotation angle acquisition unit for acquiring an absolute rotation angle measured by the inertia measurement unit, an
And the absolute rotation angular rate acquisition unit is used for acquiring the absolute rotation angular rate measured by the inertia measurement unit.
30. The position and orientation determination apparatus for a movable platform according to claim 17,
the movable platform comprises an unmanned aerial vehicle.
31. The position and orientation determination apparatus for a movable platform according to claim 30,
the unmanned aerial vehicle comprises a vehicle body and a cradle head connected to the vehicle body, wherein the inertia measuring unit is arranged on the cradle head, and the position and speed measuring unit is arranged on the vehicle body.
32. A position and orientation determination system for a movable platform, characterized by comprising an inertial measurement unit and a position and velocity measurement unit provided on the movable platform, the inertial measurement unit and the position and velocity measurement unit being provided so as to be relatively movable; the position and orientation determination system further includes a processor and a memory, the memory to store executable instructions, the processor to invoke the executable instructions stored in the memory to perform the following:
acquiring first position data and first movement speed data measured by the position and speed measurement unit;
acquiring a position compensation quantity and a motion speed compensation quantity of the inertial measurement unit;
calculating second position data of the inertial measurement unit according to the first position data and the position compensation amount, and calculating second movement speed data of the inertial measurement unit according to the first movement speed data and the movement speed compensation amount; and
determining a position attitude of the movable platform from the second position data and the second motion velocity data.
33. The position and orientation determination system for a movable platform of claim 32,
and the computer vision system is used for acquiring a position compensation quantity and a motion speed compensation quantity of the inertial measurement unit.
34. The position and orientation determination system for a movable platform of claim 32,
the inertial measurement unit and the position and velocity measurement unit are pivotally connected with a pivot.
35. The position pose determination system for a movable platform of claim 34,
acquiring the position compensation amount for the inertial measurement unit includes acquiring an attitude rotation matrix measured by the inertial measurement unit, and calculating an equivalent distance between the position velocity measurement unit and the inertial measurement unit.
36. The position pose determination system for a movable platform of claim 35,
calculating an equivalent distance between the position and velocity measurement unit and the inertial measurement unit by the following formula (1):
8l＝δl0+R1(t)δl1 (1)
wherein δ l is an equivalent distance between the position and velocity measurement unit and the inertial measurement unit; delta l0Is the distance between the inertial measurement unit and the axis of rotation of the pivot; r1(t) is a relative attitude rotation matrix of the position and velocity measurement unit with respect to the inertial measurement unit at time t; delta l1Is the distance between the position and velocity measurement unit and the axis of rotation of the pivot.
37. The position and orientation determination system for a movable platform of claim 36,
calculating a position compensation amount for the inertial measurement unit by the following equation (2):
PCOMP＝R(t)×δl (2)
wherein, PCOMPIs a position compensation amount for the inertial measurement unit; r (t) is an attitude rotation matrix at time t measured by the inertial measurement unit.
38. The position pose determination system for a movable platform of claim 37,
calculating second position data of the inertial measurement unit from the first position data and the position compensation amount includes calculating the second position data by the following equation (3):
PIMU＝PRTK-PCOMP (3)
wherein, PIMUSecond position data for the inertial measurement unit; pRTKIs the first position data measured by the position velocity measurement unit.
39. The position pose determination system for a movable platform of claim 34,
acquiring the motion velocity compensation amount of the inertial measurement unit comprises calculating an equivalent rotation angular rate of the inertial measurement unit and calculating an equivalent distance between the position velocity measurement unit and the inertial measurement unit.
40. The position pose determination system for a movable platform of claim 39,
calculating an equivalent rotational angular rate of the inertial measurement unit by the following equation (4):
ω(t)＝ω0(t)+R1(t)×ω1(t) (4)
wherein ω (t) is the inertia measurement unitThe equivalent angular rate of rotation of the element; omega0(t) is the absolute angular rate of rotation measured by the inertial measurement unit; r1(t) is a relative attitude rotation matrix of the position velocity measurement unit with respect to the inertial measurement unit; omega1(t) is the relative rotational angular rate of the position and velocity measurement unit with respect to the inertial measurement unit.
41. The position and orientation determination system for a movable platform of claim 40,
calculating an equivalent distance between the position and velocity measurement unit and the inertial measurement unit by the following equation (5):
δl＝δl0+R1(t)×δl1 (5)
wherein δ l is an equivalent distance between the position and velocity measurement unit and the inertial measurement unit; delta l0Is the distance between the inertial measurement unit and the axis of rotation of the pivot; delta l1Is the distance between the position and velocity measurement unit and the axis of rotation of the pivot.
42. The position and orientation determination system for a movable platform of claim 41,
calculating a motion velocity compensation amount for the inertial measurement unit by the following equation (6):
VCOMP＝ω(t)×δl (6)
wherein, VCOMPTo compensate for the amount of motion velocity of the inertial measurement unit.
43. The position and orientation determination system for a movable platform of claim 42,
calculating second movement velocity data of the inertial measurement unit from the first movement velocity data and the movement velocity compensation amount includes calculating the second movement velocity data by the following equation (7):
VIMU＝VRTK-VCOMP (7)
wherein, VIMUSecond motion speed data of the inertial measurement unit; vRTKIs the first movement speed data measured by the position speed measuring unit.
44. The position and orientation determination system for a movable platform of claim 32,
the method further comprises the step of obtaining the absolute rotation angle and the absolute rotation angular rate measured by the inertial measurement unit.
45. The position and orientation determination system for a movable platform of claim 32,
the movable platform comprises an unmanned aerial vehicle.
46. The position pose determination system for a movable platform of claim 45,
the unmanned aerial vehicle comprises a vehicle body and a cradle head connected to the vehicle body, wherein the inertia measuring unit is arranged on the cradle head, and the position and speed measuring unit is arranged on the vehicle body.
Technical Field
The present disclosure relates to the field of parameter measurement of moving objects, and more particularly, to a method for measuring a position and an attitude of a movable platform, and a related apparatus and system.
Background
For mobile platforms such as unmanned vehicles, automatic flying unmanned aerial vehicles, mobile measurement carriers and other mobile engineering platforms, position information and attitude information of the mobile platforms need to be continuously acquired in real time, which is a core step for realizing feedback control and safety monitoring. In the related art, a position and attitude system is used to measure the position and attitude of a movable platform, so as to obtain required relevant data information according to the measured relevant position data and attitude data. The position and attitude system in the related art generally includes two sensors, namely, a position and velocity measurement module for acquiring high-precision position and velocity information in real time by using a dynamic carrier-phase differential technique, and an inertial measurement module for measuring triaxial angular rate and acceleration information of a movable platform at a high frequency.
Generally, the position and velocity measurement module and the inertial measurement module are connected in a relatively fixed manner, that is, a rigid connection manner is adopted between the two modules, and the two modules respectively acquire related dynamic data, and combine the data acquired by the two modules so as to obtain continuous and complete position, velocity and attitude information of the mobile platform. Due to the rigid connection requirement between the position and speed measuring module and the inertial measuring module, certain constraints are brought to the installation of the position and speed measuring module and the inertial measuring module, and for some specific applications, the requirement brings much inconvenience to the actual system design.
Disclosure of Invention
The embodiment of the disclosure provides a position and attitude determination method, device and system for a movable platform, which can enable an inertia measurement unit and a position and speed measurement unit to be arranged on the same component without any need, have more flexible relative installation positions and provide convenience for the design of the movable platform.
In a first aspect, the disclosed embodiments provide a position and attitude determination method for a movable platform, wherein an inertial measurement unit and a position and velocity measurement unit are provided on the movable platform, and the inertial measurement unit and the position and velocity measurement unit are provided in a form capable of relative movement; the method comprises the following steps: acquiring first position data and first movement speed data measured by the position and speed measurement unit; acquiring a position compensation quantity and a motion speed compensation quantity of the inertial measurement unit; calculating second position data of the inertial measurement unit according to the first position data and the position compensation amount, and calculating second movement speed data of the inertial measurement unit according to the first movement speed data and the movement speed compensation amount; and determining a position attitude of the movable platform from the second position data and the second motion speed data.
In a second aspect, embodiments of the present disclosure provide a computer-readable storage medium having executable instructions stored thereon, wherein the executable instructions are executed to implement the position and attitude determination method for a movable platform as described above.
In a third aspect, an embodiment of the present disclosure provides a position and orientation determining apparatus for a movable platform, including: an inertial measurement unit disposed on the movable platform; a position and velocity measurement unit disposed on the movable platform for acquiring first position data and first movement velocity data, the inertial measurement unit and the position and velocity measurement unit being disposed to be capable of relative movement; a position compensation amount acquisition unit for acquiring a position compensation amount for the inertial measurement unit; a motion velocity compensation amount acquisition unit for acquiring a motion velocity compensation amount for the inertial measurement unit; a second position data calculation unit that calculates second position data of the inertial measurement unit based on the first position data and the position compensation amount; a second movement velocity data calculation unit that calculates second movement velocity data of the inertial measurement unit based on the first movement velocity data and the movement velocity compensation amount; and a position and orientation determination unit that determines a position and orientation of the movable platform based on the second position data and the second movement speed data.
In a fourth aspect, embodiments of the present disclosure provide a position and orientation determination system for a movable platform, wherein the position and orientation determination system includes an inertial measurement unit and a position and velocity measurement unit provided on the movable platform, the inertial measurement unit and the position and velocity measurement unit being provided so as to be relatively movable; the position and orientation determination system further includes a processor and a memory, the memory to store executable instructions, the processor to invoke the executable instructions stored in the memory to perform the following: acquiring first position data and first movement speed data measured by the position and speed measurement unit; acquiring a position compensation quantity and a motion speed compensation quantity of the inertial measurement unit; calculating second position data of the inertial measurement unit according to the first position data and the position compensation amount, and calculating second movement speed data of the inertial measurement unit according to the first movement speed data and the movement speed compensation amount; and determining a position attitude of the movable platform from the second position data and the second motion speed data.
According to the position and posture determining method for the movable platform, the inertial measurement unit and the position and speed measurement unit on the movable platform are arranged in a mode capable of moving relatively, so that the inertial measurement unit and the position and speed measurement unit are not required to be arranged on the same component, the relative installation position is flexible, the compensation value of the relative movement data between the inertial measurement unit and the position and speed measurement unit can be obtained only by acquiring the relative movement relation between the inertial measurement unit and the position and speed measurement unit, and the relevant data of the inertial measurement unit can be obtained after relevant compensation is carried out by taking the data measured by the position and speed measurement unit as a basis. Therefore, convenience is provided for the design of the movable platform, and the position and posture determining method disclosed by the invention has wider application scenes.
Drawings
Fig. 1 is a flow chart of a position and attitude determination method for a movable platform according to the present disclosure.
Fig. 2 is a schematic structural diagram of a movable platform according to the present disclosure.
Fig. 3 is a block diagram of a structure of a computer-readable storage medium according to the present disclosure.
Fig. 4 is a block diagram of a position and orientation determining apparatus for a movable platform according to the present disclosure.
Fig. 5 is a block diagram of a position and attitude determination system for a movable platform according to the present disclosure.
Detailed Description
Reference will now be made in detail to embodiments of the present disclosure, examples of which are illustrated in the accompanying drawings, wherein like or similar reference numerals refer to the same or similar elements or elements having the same or similar function throughout. The embodiments described below with reference to the accompanying drawings are illustrative only for the purpose of explaining the present disclosure, and are not to be construed as limiting the present disclosure.
In the description of the present disclosure, it is to be understood that the terms "first", "second" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance or implying any number of technical features indicated. Thus, features defined as "first", "second", may explicitly or implicitly include one or more of the described features. In the description of the present disclosure, "a plurality" means two or more unless specifically limited otherwise.
In the description of the present disclosure, it is to be noted that, unless otherwise explicitly specified or limited, the terms "mounted," "connected," and "connected" are to be construed broadly, e.g., as meaning either a fixed connection, a removable connection, or an integral connection; may be mechanically connected, may be electrically connected or may be in communication with each other; either directly or indirectly through intervening media, either internally or in any other relationship. The specific meaning of the above terms in the present disclosure can be understood by those of ordinary skill in the art as appropriate.
The following disclosure provides many different embodiments or examples for implementing different features of the disclosure. In order to simplify the disclosure of the present disclosure, specific example components and arrangements are described below. Of course, they are merely examples and are not intended to limit the present disclosure. Moreover, the present disclosure may repeat reference numerals and/or reference letters in the various examples, which have been repeated for purposes of simplicity and clarity and do not in themselves dictate a relationship between the various embodiments and/or arrangements discussed. In addition, the present disclosure provides examples of various specific processes and materials, but one of ordinary skill in the art may recognize applications of other processes and/or use of other materials.
The present disclosure provides a position and orientation determining method for a movable platform, which can solve the problem in the related art that two sensors can only be relatively fixedly installed, so that a position and velocity measuring unit and an inertial measuring unit for constituting a position and orientation system are assembled together in a relatively movable manner, thereby providing a more convenient design solution for a movable platform carrying such a position and orientation system. That is, the position and orientation determination method for a movable platform according to the present disclosure may allow a position and velocity measurement unit and an inertial measurement unit to be arbitrarily provided on the movable platform, and may be capable of obtaining relative motion data for the movable platform from the relative position and velocity data measured by the position and velocity measurement unit and the relative rotation data measured by the inertial measurement unit to determine the position and orientation of the movable platform.
In the position and orientation determination method for a movable platform according to the present disclosure, S1 may be first performed, as shown in fig. 1, the position-velocity measurement unit 20 and the inertial measurement unit 40 may be provided on the movable platform 100, and the position-velocity measurement unit 20 and the inertial measurement unit 40 may be provided in a form capable of relative movement, as shown in fig. 2, which shows a schematic structural view of the movable platform 100. Here, the position and velocity Measurement unit 20 may be an RTK (Real Time Kinematic) antenna, and the inertial Measurement unit 40 may be an imu (inertial Measurement unit) component. The relative-motion-enabled arrangement of the position-velocity measurement unit 20 and the inertial measurement unit 40 may include relative-translational-motion-enabled arrangement, relative-rotational-motion-enabled arrangement, and relative-translational-rotational-motion-enabled arrangement, where only the relative motion relationship between the two needs to be determined, and the relative motion relationship between the two is determined, so that the relative motion parameters between the two can be mutually converted. The position and orientation determination method according to the present disclosure further includes (as shown in fig. 2): s2, the first position data and the first movement velocity data measured by the position velocity measuring unit 20 are acquired, that is, the position data and the movement velocity data with the center of the sensor of the position velocity measuring unit 20 as the measuring point can be directly acquired by the position velocity measuring unit 20 as the first position data and the first movement velocity data. Then, S3 is executed to acquire a position compensation amount and a movement velocity compensation amount for the inertial measurement unit 40, where the position data and the movement velocity data of the inertial measurement unit 40 are adjusted in relation to the first position data and the first movement velocity data acquired by the position velocity measurement unit 20 with the inertial measurement unit 40 as a measurement center, so that the compensated movement data of the inertial measurement unit 40 can be acquired. That is, S4 is executed to calculate second position data of the inertial measurement unit 40 based on the first position data and the position compensation amount, and to calculate second movement velocity data of the inertial measurement unit 40 based on the first movement velocity data and the movement velocity compensation amount. Finally, S5 is executed, and the position and the posture of the movable platform are determined according to the second position data and the second movement speed data. It should be noted that, by performing the position compensation and the motion velocity compensation on the inertial measurement unit 40, the position data and the motion velocity data of the inertial measurement unit 40 are obtained and used as the relevant data of the movable platform.
Here, any point on the movable platform may be selected as the measurement point at which measurement is performed, as long as the relative motion relationship of the position velocity measurement unit 20 and the inertial measurement unit 40 with the measurement point can be determined, the correlation data compensation amount performed on the measurement point is determined by determining the relative motion relationship of the position velocity measurement unit 20 and the inertial measurement unit 40 with the measurement point, the correlation data measured by the position velocity measurement unit 20 and the inertial measurement unit 40 is combined into a measurement point parameter group, the position and orientation of the measurement point is determined, and the position and orientation data of the measurement point may be used as the position and orientation data of the movable platform. Advantageously, the amount of position compensation and the amount of motion velocity compensation for the inertial measurement unit 40 may be obtained by a computer vision system. That is, the relative motion relationship between the inertial measurement unit 40 and the position and velocity measurement unit 20 may be recognized by the computer vision system, and the position data and the motion velocity data measured by the position and velocity measurement unit 20 are compensated accordingly according to the relative motion relationship to obtain the second position data and the second motion velocity data of the inertial measurement unit 40, thereby obtaining the position and attitude data of the movable platform. Therefore, the relative motion relationship between the inertial measurement unit 40 and the position and velocity measurement unit 20 can be identified by the computer vision system, and the position data and the motion velocity data of the inertial measurement unit 40 can be obtained according to the position and the motion velocity data measured by the position and velocity measurement unit 20, and any relative motion can be generated between the inertial measurement unit 40 and the position and velocity measurement unit 20 without arranging the inertial measurement unit 40 and the position and velocity measurement unit 20 relatively fixedly, i.e. without rigidly connecting the two in the design process of the movable platform.
Advantageously, providing the inertial measurement unit 40 and the position and velocity measurement unit 20 in a relatively movable fashion may include pivotally connecting the inertial measurement unit 40 and the position and velocity measurement unit 20 with a pivot shaft 60. For example, the position and velocity measurement unit 20 may be disposed on a first carrier 120 of the movable platform 100, and correspondingly, the inertial measurement unit 40 may be disposed on a second carrier 140 of the movable platform 100, the first carrier 120 and the second carrier 140 may be connected together by a pivot 60, thereby enabling a pivotal connection between the position and velocity measurement unit 20 and the inertial measurement unit 40. This arrangement of the inertial measurement unit 40 and the position and velocity measurement unit 20 is particularly advantageous for an unmanned aerial vehicle on which the imaging unit is mounted. For example, the position and velocity measurement unit 20 may be disposed on the airframe of the unmanned aerial vehicle, that is, the first carrier 120 corresponds to the airframe of the unmanned aerial vehicle, while the inertia measurement unit 40 is disposed on the pan/tilt head connected to the airframe of the unmanned aerial vehicle, that is, the second carrier 140 corresponds to the pan/tilt head of the unmanned aerial vehicle. The cradle head may be pivotally connected to the airframe of the unmanned aerial vehicle.
Further, in the method according to the present disclosure, acquiring the position compensation amount for the inertial measurement unit 40 may include acquiring an attitude rotation matrix measured by the inertial measurement unit 40, and calculating an equivalent distance between the position velocity measurement unit 20 and the inertial measurement unit 40. The amount of position compensation of the inertial measurement unit 40 with respect to the positional velocity measurement unit 20 can be obtained by the attitude rotation matrix directly acquired by the inertial measurement unit 40 in combination with the above equivalent distance. When the position and velocity measuring unit 20 and the inertial measuring unit 40 are rigidly connected, the distance between the position and velocity measuring unit 20 and the inertial measuring unit 40 is a fixed value and can be directly obtained through measurement, and when the position and velocity measuring unit 20 and the inertial measuring unit 40 are in relative motion connection, the equivalent distance between the two needs to be determined according to the relative motion relationship between the two. The process of acquiring the position compensation amount will be described in detail below.
The equivalent distance between the position and velocity measurement unit 20 and the inertial measurement unit 40 can be calculated by the following equation (1):
δl＝δl0+R1(t)δl1 (1)
wherein δ l is an equivalent distance between the position and velocity measuring unit 20 and the inertia measuring unit 40, and the equivalent distance δ l is a three-dimensional parameter; delta l0The distance between the inertial measurement unit 40 and the axis of rotation of the pivot 60 may be determined during the design of the movable platform; r1(t) is a relative attitude rotation matrix of the position and velocity measurement unit 20 at time t with respect to the inertial measurement unit 40, which can be obtained by the inertial measurement unit 40, which is typically a third-order square matrix; delta l1For position and speed measuring sheetThe distance between the element 20 and the axis of rotation of the pivot 60, which can be determined during the design of the movable platform.
Next, after calculating the equivalent distance between the position velocity measurement unit 20 and the inertial measurement unit 40, the position compensation amount for the inertial measurement unit 40 can be calculated by the following equation (2):
PCOMP＝R(t)×δl (2)
wherein, PCOMPTo compensate for the position of the inertial measurement unit 40; r (t) is an attitude rotation matrix at time t measured by the inertial measurement unit 40, which is an attitude rotation matrix of the inertial measurement unit 40 itself sensed by the inertial measurement unit 40.
Further, calculating the second position data of the inertial measurement unit 40 from the first position data and the position compensation amount may include calculating the second position data by the following equation (3):
PIMU＝PRTK-PCOMP (3)
wherein, PIMUSecond position data for the inertial measurement unit 40; pRTKFor the first position data measured by the position-velocity measuring unit 20, the position-velocity measuring unit 20 may directly acquire its own position data for use as a basis for calculating the second position data of the inertial measuring unit 40.
While the position compensation amount for the inertial measurement unit 40 is acquired to calculate the second position data thereof, the motion velocity compensation amount for the inertial measurement unit 40 is acquired, which may include calculating an equivalent rotational angular rate of the inertial measurement unit 40, and calculating an equivalent distance between the position velocity measurement unit 20 and the inertial measurement unit 40. Here, the equivalent rotational angular rate of the inertial measurement unit 40 can be calculated by the following equation (4):
ω(t)＝ω0(t)+R1(t)×ω1(t) (4)
where ω (t) is the equivalent rotational angular rate of the inertial measurement unit 40; omega0(t) is the absolute rotational angular rate measured by the inertial measurement unit 40, i.e., the rotational angular rate of the inertial measurement unit 40 itself; r1(t) position velocity measurementA relative attitude rotation matrix of the unit 20 with respect to the inertial measurement unit 40, which can be acquired by the inertial measurement unit 40, such as a relative attitude rotation matrix of the position velocity measurement unit 20 with respect to the inertial measurement unit 40, which can be obtained by controlling the relative movement of the two relative to each other; omega1(t) is the relative rotational angular rate of the position and velocity measurement unit 20 with respect to the inertial measurement unit 40, which can also be obtained by controlling the data of the relative movement between the position and velocity measurement unit 20 and the inertial measurement unit 40.
Further, the equivalent distance between the position and velocity measurement unit 20 and the inertial measurement unit 40 can be calculated by the following equation (5):
δ1＝δl0+R1(t)×δl1 (5)
here, similarly to the equivalent distance calculated by the equation (1), where δ l is the equivalent distance between the position velocity measurement unit 20 and the inertia measurement unit 40; delta l0Is the distance between the inertial measurement unit 40 and the axis of rotation of the pivot shaft 60; delta l1Is the distance between the position and velocity measuring unit 20 and the axis of rotation of the pivot shaft 60.
According to the position and orientation determination method for the movable platform of the present disclosure, the amount of motion velocity compensation for the inertial measurement unit 40 can be calculated by the following equation (6):
VCOMP＝ω(t)×δl (6)
wherein, VCOMPTo compensate for the amount of motion velocity of the inertial measurement unit 40.
Finally, the movement velocity of the inertial measurement unit 40 may be compensated by the calculated movement velocity compensation amount. That is, calculating the second movement velocity data of the inertial measurement unit from the first movement velocity data and the movement velocity compensation amount may calculate the second movement velocity data by the following equation (7):
VIMU＝VRTK-VCOMP (7)
wherein, VIMUSecond movement velocity data of the inertial measurement unit 40; vRTKIs the first movement speed data measured by the position speed measuring unit 20, i.e. the position speedThe degree measurement unit 20 measures its own movement speed data.
It should be noted here that the relative attitude rotation matrix R of the position and velocity measurement unit 20 with respect to the inertial measurement unit 401(t) is the relative rotation angle θ of the position and velocity measurement unit 20 with respect to the inertial measurement unit 401(t) function, i.e. R1(t)＝R{θ1(t) }, in which θ1(t) is a relative rotation angle of the position and velocity measurement unit 20 with respect to the inertial measurement unit 40 at time t. Assuming that the rotation angles of the position-velocity measurement unit 20 with respect to the inertial measurement unit 40 along the X-axis, the Y-axis, and the Z-axis at time t are α, β, and γ, respectively, there are:
then
Then, the equivalent distance δ l between the position and velocity measurement unit 20 and the inertial measurement unit 40 is calculated as follows: assume the distance δ l between the inertial measurement unit 40 and the axis of rotation of the pivot shaft 600Comprises the following steps:
and, the distance δ l between the position and velocity measuring unit 20 and the rotation axis of the pivot shaft 601Comprises the following steps:
δ l is δ l0+R1(t)×δl1The calculation process of (a) is as follows:
the component l of delta l on X, Y and Z axis can be calculated by the above formulax、lyAnd lz. In the above calculations, the parameter with index X represents the distance along the X axis, the parameter with index Y represents the distance along the Y axis, and the parameter with index Z represents the distance along the Z axis.
The position and orientation determination method for the movable platform according to the present disclosure may further include acquiring an absolute rotation angle and an absolute rotation angular rate measured by the inertial measurement unit 40. Here, the position and velocity measuring unit 20 acquires its own position data and motion velocity data, and compensates the data to acquire the second position data and second motion velocity data of the inertial measuring unit 40, and the inertial measuring unit 40 measures its own rotation angle and rotation angular rate data, so that the position and orientation data of the inertial measuring unit 40 can be acquired from the four sets of data, and the data can be used as the position and orientation data of the movable platform carrying the position and velocity measuring unit 20 and the inertial measuring unit 40, and the position and orientation information of the movable platform can be acquired.
In the method for determining the position and orientation of a movable platform according to the present disclosure, the movable platform may include an unmanned aerial vehicle, and advantageously, the unmanned aerial vehicle includes a body and a cradle head connected to the body, that is, the movable platform 100 in fig. 2 may be regarded as an unmanned aerial vehicle, the first carrier 120 as the body of the unmanned aerial vehicle, and the second carrier 140 as the cradle head of the unmanned aerial vehicle, wherein the inertial measurement unit 40 is disposed on the cradle head, and the position and velocity measurement unit 20 is disposed on the body of the unmanned aerial vehicle. Therefore, the position and velocity measuring unit 20 and the inertia measuring unit 40 do not need to be arranged on the body of the unmanned aerial vehicle at the same time, that is, the position and velocity measuring unit 20 and the inertia measuring unit 40 do not need to be rigidly connected, a connection mode capable of moving relatively between the two is realized, that is, a non-rigid connection mode is realized, and thus the application scene of the position and attitude determination method is expanded. Of course, it is also possible here to arrange the position-velocity measuring unit 20 on the pan/tilt head, while the inertial measuring unit 40 is arranged on the airframe of the unmanned aerial vehicle.
In the above-described embodiment, in the case where both the position velocity measurement unit 20 and the inertial measurement unit 40 are single, the above-described method according to the present disclosure is equally applicable to the case where there are a plurality of position velocity measurement units 20 and/or a plurality of inertial measurement units 40. In addition, the method according to the present disclosure is also applicable to the following cases, such as the related applications of the Global Navigation Satellite System (GNSS) base station-less single-point positioning, pseudo-range differential positioning, precise single-point positioning, and pure position compensation and velocity compensation. The same applies to the embodiments of the present disclosure described below.
The present disclosure also relates to a computer-readable storage medium 200, as shown in fig. 3, the computer-readable storage medium 200 storing executable instructions 220, wherein the executable instructions 220 are executed to implement any of the position and orientation determination methods for a movable platform as described above.
Further, the present disclosure also provides a position and orientation determining apparatus 400 for a movable platform, the position and orientation determining apparatus 400 including: an inertial measurement unit 440, the inertial measurement unit 440 being disposed on the movable platform; a position and velocity measuring unit 420, the position and velocity measuring unit 420 being provided on the movable platform for acquiring first position data and first movement velocity data, the inertia measuring unit 440 and the position and velocity measuring unit 420 being provided to be capable of relative movement; a position compensation amount acquisition unit 460 for acquiring a position compensation amount for the inertial measurement unit 440; a motion velocity compensation amount acquisition unit 480 for acquiring a motion velocity compensation amount for the inertia measurement unit 440; a second position data calculation unit 500, the second position data calculation unit 500 calculating second position data of the inertial measurement unit 440 according to the first position data and the position compensation amount; a second movement velocity data calculation unit 520, the second movement velocity data calculation unit 520 calculating second movement velocity data of the inertia measurement unit 440 according to the first movement velocity data and the movement velocity compensation amount; and a position and orientation determination unit 540, the position and orientation determination unit 540 determining the position and orientation of the movable platform based on the second position data and the second movement speed data. Here, the inertia measurement unit 440 and the position and velocity measurement unit 420 may be connected in any manner, for example, the inertia measurement unit 440 and the position and velocity measurement unit 420 may be respectively disposed on different parts of the movable platform to achieve a movable connection relationship therebetween. The mutual motion between the two can be limited arbitrarily, for example, the two can be in relative translation, relative rotation or both relative translation and relative rotation, and the position compensation amount and the motion speed compensation amount can be determined only by determining the relative motion relationship between the two.
The position and orientation determination apparatus 400 for the movable platform according to the present disclosure determines the second position data and the second movement velocity data of the inertial measurement unit 440 as the position data and the movement velocity data of the movable platform by determining the position compensation amount and the velocity compensation amount for the inertial measurement unit 440 according to the position data and the movement velocity data measured by the position and velocity measurement unit 420 and according to the relative movement relationship between the position and velocity measurement unit 420 and the inertial measurement unit 440. It is thus not necessary to ensure a relatively fixed connection relationship between the position velocity measurement unit 420 and the inertial measurement unit 440, that is, a non-rigid connection relationship can be achieved therebetween, thereby facilitating the hardware design of the position and orientation determining apparatus 400 for a movable platform, which makes it more flexible in design.
The position compensation amount acquisition unit 460 and/or the motion velocity compensation amount acquisition unit 480 of the position posture determination apparatus 400 for a movable platform according to the present disclosure may include a computer vision system. That is, the position compensation data and the motion velocity compensation data for the inertial measurement unit 440 relative to the position and velocity measurement unit 420 can be obtained directly by the computer vision system, and the computer vision system can obtain the compensation data by identifying the relative motion relationship between the two.
Advantageously, the position and attitude determination apparatus 400 for a movable platform according to the present disclosure pivotally connects the inertial measurement unit 440 and the position and velocity measurement unit 420 together with a pivot. In the case where the inertial measurement unit 440 and the position-velocity measurement unit 420 are pivotally connected together by a pivot, the acquisition of the position compensation amount and the movement-velocity compensation amount of the inertial measurement unit 440 can be performed in the manner described above with reference to the position-posture determining method for the movable platform, and the second position data and the second movement-velocity data of the inertial measurement unit 440 can be acquired based on the first position data and the first movement-velocity data acquired by the position-velocity measurement unit 420, which will not be described in detail herein.
Further, the position and orientation determination apparatus 400 for a movable platform according to the present disclosure may further include an absolute rotation angle acquisition unit for acquiring an absolute rotation angle measured by the inertia measurement unit 440, and an absolute rotation angular rate acquisition unit for acquiring an absolute rotation angular rate measured by the inertia measurement unit 440. Thus, the position and attitude information of the inertial measurement unit 440 can be obtained by the obtained second position data and second movement velocity data of the inertial measurement unit 440, and the absolute rotation angle and absolute rotation angular rate.
Advantageously, the above-mentioned movable platform may comprise an unmanned aerial vehicle, i.e. the position and attitude determination means 400 may be used to determine position and attitude information of the unmanned aerial vehicle. The unmanned aerial vehicle may include a body and a pan/tilt head connected to the body, wherein the inertia measurement unit 440 is disposed on the pan/tilt head, and the position and velocity measurement unit 420 is disposed on the body of the unmanned aerial vehicle.
The present disclosure also relates to a position and orientation determination system 600 for a movable platform, as shown in fig. 5, wherein the position and orientation determination system 600 comprises an inertial measurement unit 640 and a position and velocity measurement unit 620 provided on the movable platform, the inertial measurement unit 640 and the position and velocity measurement unit 640 being provided to be capable of relative movement; the position and orientation determination system 600 also includes a processor 660 and a memory 680, the memory 680 to store executable instructions 682, the processor 660 to invoke the executable instructions 682 stored in the memory 680 to perform the following: acquiring first position data and first movement speed data measured by the position and speed measurement unit 620; acquiring a position compensation amount and a motion velocity compensation amount for the inertial measurement unit 640; calculating second position data of the inertia measurement unit 640 according to the first position data and the position compensation amount, and calculating second movement velocity data of the inertia measurement unit 640 according to the first movement velocity data and the movement velocity compensation amount; and determining the position and attitude of the movable platform according to the second position data and the second movement speed data.
The position and attitude determination system 600 for a movable platform according to the present disclosure may further include a computer vision system for obtaining a position compensation amount and a motion velocity compensation amount for the inertial measurement unit 640. That is, the relative motion relationship between the position and velocity measurement unit 620 and the inertial measurement unit 640 is obtained by the computer vision system, and the above-mentioned position compensation amount and motion velocity compensation amount can be obtained.
Advantageously, inertial measurement unit 640 and position-velocity measurement unit 620 may be pivotally connected with a pivot. Of course, other connection means may be used to realize the non-rigid connection between the inertial measurement unit 640 and the position and velocity measurement unit 620, for example, the non-rigid connection between the two units may be realized by other servos such as a multi-stage serial mechanism.
The processor 660 of the position and attitude determination system 600 for a movable platform according to the present disclosure may also obtain the amount of position compensation and the amount of motion velocity compensation for the inertial measurement unit 640 by invoking the executable instructions 682 stored in the memory 680. The manner of acquiring the position compensation amount and the motion velocity compensation amount for the inertial measurement unit 640 in this embodiment is the same as that in the position and orientation determination method for a movable platform of the present disclosure, and is not described herein again.
The processor 660 of the position and attitude determination system 600 for a movable platform according to the present disclosure, invoking the executable instructions 682 stored in the memory 680, is further configured to obtain the absolute angle of rotation and absolute angular rate of rotation measured by the inertial measurement unit 640. The position and orientation determining system 600 can determine the position information and the orientation information of the inertial measurement unit 640 from the obtained second position data and second movement velocity data of the inertial measurement unit 640 and the correlation data of the absolute rotation angle and the absolute rotation angular rate measured by the inertial measurement unit 640, and can use this as the position and orientation information of the movable platform.
Further, the movable platform may include an unmanned aerial vehicle, which may include an airframe and a pan-tilt head connected to the airframe, wherein the inertial measurement unit 640 is disposed on the pan-tilt head, and the position and velocity measurement unit 620 is disposed on the airframe. The tripod head and the machine body are connected in a mutually movable manner, so that the mutual movement connection between the position and speed measuring unit 620 and the inertia measuring unit 640 is realized. Without the need of simultaneously arranging the position and velocity measuring unit 620 and the inertial measuring unit 640 on the machine body as in the related art, so as to ensure a relatively fixed connection manner between the two units. The position and attitude determination system 600 for a movable platform according to the present disclosure is more flexible in design, and can dispose the position and velocity measurement unit 620 and the inertial measurement unit 640 on any relevant components of the unmanned aerial vehicle so as to realize the combination of the motion data of the two, thereby obtaining the required position data and attitude data.
The position and attitude determination method for a movable platform according to the present disclosure may be used in combination, for example, more than one position and velocity measurement unit and/or more than one inertial measurement unit may be disposed on the movable platform, the position and velocity measurement unit and the inertial measurement unit may be connected to move relatively, and the position and velocity measurement unit may acquire required relevant position data and motion velocity data, and the inertial measurement unit may acquire required relevant rotation angle data and rotation angle rate data, and may convert the data into relevant motion data at a measurement point on the movable platform through compensation of the data, for example, the measurement point may be a center of gravity of the movable platform, and of course, the measurement point may be any position of interest related to the movable platform. The design also realizes a relatively movable connection mode of the position and speed measuring unit and the inertial measuring unit on the movable platform.
In the above embodiments, the implementation may be wholly or partially realized by software, hardware, firmware, or any combination thereof. When implemented in software, may be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions. The procedures or functions according to the embodiments of the present application are all or partially generated when the computer program instructions are loaded and executed on a computer. The computer may be a general purpose computer, a special purpose computer, a network of computers, or other programmable device. The computer instructions may be stored on or transmitted over a computer-readable storage medium. The computer instructions may be transmitted from one website site, computer, server, or data center to another website site, computer, server, or data center by wire (e.g., coaxial cable, fiber optic, Digital Subscriber Line (DSL)) or wirelessly (e.g., infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that can be accessed by a computer or a data storage device, such as a server, a data center, etc., that incorporates one or more of the available media. The usable medium may be a magnetic medium (e.g., a floppy disk, a hard disk, a magnetic tape), an optical medium (e.g., a DVD), or a semiconductor medium (e.g., a Solid State Disk (SSD)), among others.
The above description is only a preferred embodiment of the present disclosure and is not intended to limit the present disclosure, and various modifications and changes may be made to the present disclosure by those skilled in the art. Any modification, equivalent replacement, improvement and the like made within the spirit and principle of the present disclosure should be included in the protection scope of the present disclosure.