THREE-DIMENSIONAL ORIENTATION DETERMINATION SYSTEM AND METHOD FOR MOBILE PLATFORMS

Abstract

The invention relates to an orientation determination system (100) for determining the three-dimensional orientation of a moving platform. System (100) includes at least one gyroscope (120) measuring the angular velocity of the moving platform, at least one accelerometer (130) measuring the acceleration acting on the moving platform, at least one magnetometer (140) measuring the ambient magnetic field, and at least one processing unit (110) connected to the aforementioned sensors. The processing unit (110) generates a state vector containing orientation information, at least one sensor pole value, and a three-dimensional linear acceleration perturbation, and performs nonlinear estimation based on sigma points. The orientation components of the sigma points are processed taking into account the nonlinear geometry of the orientation space. Orientation information, sensor pole value, and linear acceleration perturbation are estimated simultaneously; magnetic perturbation effects are suppressed to determine the compass angle of the moving platform. Figure 1

Description

1 TARIFF THREE-DIMENSIONAL ORIENTATION DETERMINATION SYSTEM FOR MOBILE PLATFORMS AND METHOD TECHNICAL FIELD The invention is an orientation 5 for determining the three-dimensional orientation of a movable platform. It is related to the determination system and method. PREVIOUS TECHNIQUE In the current technology, the aim is to determine the three-dimensional orientation information of mobile platforms. Inertial measurement and orientation where gyroscope, accelerometer, and magnetometer data are processed together. Determination systems are used. In these systems, 10 are used to obtain orientation information. mostly from extended Kalman filter and similar sensor fusion methods It is being used. However, current solutions, especially regarding the compass angle, utilize environmental magnetic fields. Susceptibility to disturbances, changes in sensor polarity and drift values ​​over time. and due to the mixing of the linear acceleration of the moving platform with the gravitational component 15 Errors can occur in orientation estimation. Sufficient magnetic disturbances may be present. The inability to suppress it causes the compass angle to be determined with a fixed or variable offset. It is possible. Similarly, the linear acceleration resulting from motion can be appropriately calculated. inability to distinguish, especially in dynamic motion conditions, roll, pitch and compass This can lead to incorrect calculations of angles. 20 The linear or linearized estimation approaches used in current technology are orientational. It is able to represent the nonlinear structure of the space in a limited way and high frequency This can cause digital stability issues in real-time applications. Additionally... Sensor polarity values ​​and disturbing effects must be detected with sufficient accuracy and dynamically during operation. The inability to predict this leads to a decrease in the accuracy of orientation during long-term use of the system. 25 This is the reason. Patent document number CN103940433B describes measurements from a star sensor and gyroscope. Adaptive square root of gyroscope drift error using error quaternion A satellite attitude determination based on estimation via an odorless Kalman filter. The method is explained. However, this solution is specific to satellite attitude determination. and by evaluating the accelerometer and magnetometer data together, from the motion 2 Decomposition of three-dimensional linear acceleration effects and environmental magnetic fields. It does not present a framework for determining orientation information under disruptive conditions. Patent document number WO2024092876A1 describes a movable platform underwater. initial alignment inertial navigation system, magnetic compass, Doppler accelerometer and 5 Through incremental coarse and fine alignment processes using altitude / depth gauge data. The implementation and use of SRUKF during the rough alignment phase are explained. However, this solution offers many advantages for aligning the underwater navigation system. It is a structure that is progressive and connected to additional navigation sensors, and it adapts to platform movement. sensor pole value and gravity of the resulting three-dimensional linear acceleration disturbance General-purpose orientation determination based on continuous estimation separately from its component 10 Its structure does not reveal this. In the previous technique, state and parameter estimation in nonlinear systems Studies using the square root odorless Kalman filter for the purpose of implementation It is known. In the study conducted by Van Der Merwe and Wan, the situation and parameters For estimation, the square root odorless Kalman filter is considered [1]. In addition, inertial 15 and measurement data obtained from magnetic sensors based on Kalman filter. There are also studies on determining orientation information by combining these elements. In the study conducted by Sabatini, inertial and magnetic sensors were used. The observability of the Kalman filter-based orientation determination approach implemented and its performance is being investigated [2]. However, sensor pole 20 on mobile platforms values, linear acceleration effects resulting from motion, and environmental magnetic disturbances. Therefore, the technique of determining orientation and especially the compass angle accurately and reliably is crucial. It is among the problems that maintain their importance in the field. [1] R. Van Der Merwe, EA Wan, “The square-root unscented Kalman filter for state and parameter-estimation,” 2001 IEEE International Conference on Acoustics, Speech, and 25 Signal Processing, Salt Lake City, UT, USA, 7–11 May 2001, Vol. 6, pp. 3461–3464. [2] AM Sabatini, “Kalman-filter-based orientation determination using inertial / magnetic sensors: Observability analysis and performance evaluation,” Sensors, Vol. 11, No. 10, 2011, pp. 9182–9206. In conclusion, all the problems mentioned above necessitate an innovation in the relevant technical field. 30 It has made it mandatory. A BRIEF DESCRIPTION OF THE INVENTION 3 The present invention aims to eliminate the aforementioned disadvantages and contribute to the relevant technical field. It is related to a system and method of determining an orientation in order to bring new advantages. One aim of the invention is to enable a three-dimensional model of a moving platform under dynamic operating conditions. The aim is to ensure that orientation information is determined more accurately and consistently. 5 Another purpose of the invention is to analyze the polarity and drift values ​​occurring in the sensors and the movement. disturbing effects arising from the linear motion of the platform on orientation estimation The goal is to reduce its impact. Another aim of the invention is to investigate the effect of environmental magnetic disturbances on compass angle. The aim is to reduce the number of people who want to ensure reliable orientation information is obtained in different work environments. 10 All the purposes mentioned above and those that will emerge from the detailed explanation below. The present invention aims to achieve the three-dimensional orientation of a movable platform. It is an orientation determination system aimed at identifying the direction of a trend. Accordingly, the orientation determination system; At least one gyroscope that obtains measurement data regarding the angular velocity of the moving platform, At least one accelerometer that obtains measurement data regarding the acceleration acting on the platform, moving 15 at least one that obtains measurement data regarding the magnetic field in the environment where the platform is located It includes a magnetometer and at least one processing unit connected to the aforementioned sensors. Processing The unit provides orientation information, at least one sensor polarity value, and information from the movement of the mobile platform. It creates a state vector that includes the resulting three-dimensional linear acceleration disturbance, A nonlinear estimation process using sigma points of the state vector 20 It performs and expresses the orientation components of the nonlinear geometry of the orientation space. It processes information taking these factors into account. The processing unit also takes into account orientation information, sensor polarity value, and Simultaneously estimating the linear acceleration disturbance, the linear acceleration disturbance The accelerometer determines this separately from the pole value and the gravitational acceleration component, and It calculates the compass angle by suppressing magnetic interference effects. Thus, the dynamic 25 Accuracy of orientation information under motion, sensor errors and environmental magnetic effects and Its determination is being increased. A feature of a possible construction of the invention is that the nonlinear estimation process is odorless. This is achieved using a Kalman filter. Thus, the nonlinear system... its behavior without the need for Jacobian-based local linearization sigma 30 It is represented through these points. Another possible configuration of the invention features the odorless Kalman filter. Instead of the covariance matrix of the vector, the square root of that covariance matrix. 4 a square root odorless Kalman filter using a square root covariance matrix representing This allows for more stable numerical processing of covariance information. This facilitates the preservation of the positive semi-definite structure. Another possible configuration of the invention features the QR square root covariance matrix. It is parsing and updating using the rank-1 Cholesky update. Thus, exactly 5 The need to reconstruct the covariance matrix in each processing cycle. This reduces the number of computations and increases the numerical stability of real-time calculations. Another possible configuration of the invention features a sigma function relating to the state vector. generating the points using the square root of covariance matrix and nonlinear 10 according to the weight values ​​corresponding to the sigma points of the estimation process This involves determining the mean and uncertainty distributions of the state variables. This is ensured by representation through nonlinear models. Another possible configuration of the invention features sigma related to orientation information. converting the points to axis-angle representation via logarithmic transformation and 15 It is the transformation. Thus, the difference, summation and covariance of the orientation variables are analyzed. This ensures that the operations are performed in a nonlinear orientation space. Another possible configuration of the invention features a quaternion or orientation representation. It must be at least one of the rotation matrix representations. Thus, the three-dimensional orientation by avoiding singularities and being compatible with different control or navigation applications 20 representation is ensured. Another possible configuration of the invention involves the orientation information of the sigma points. the average of the related rotation matrices is calculated using Karcher averaging. This involves determining trends. Thus, instead of using linear arithmetic mean, trends are determined using direction. The manifold is joined using an average that is appropriate to its geometry. 25 Another possible configuration of the invention features angular velocity measurement from the gyroscope. orientation information is obtained by advancing the data through a nonlinear kinematic model. The goal is to create a priori state vector related to the sampling of the moving platform. estimation of the change in orientation between intervals based on angular motion data is provided. 30 Another possible configuration of the invention features three-dimensional acceleration from the accelerometer. through measurement data and three-dimensional magnetic field measurement data obtained from the magnetometer It is the process of updating the prior state vector to create a posterior state vector. Thus... Cumulative orientation errors that may arise from the integration of gyroscope measurements can affect acceleration and... This is corrected using magnetic field measurements. Another possible configuration of the invention involves the comparison of measurement data with prior conditions. a residual representing the difference between the estimated measurement data obtained from the vector the vector is created and the state vector is adaptively determined with the residual vector 5 This involves updating the process model according to a Kalman gain. Thus, the process model and sensor measurements are updated accordingly. The effects on the forecast are balanced according to the current uncertainty values. Another possible configuration of the invention features the state vector's gyroscope pole. It should include the value, the accelerometer pole value and the magnetometer pole value, and the aforementioned It is the dynamic estimation of polarity values ​​during operation. Thus, the temperature is determined during operation 10 the trend of variable sensor errors arising from duration and sensor characteristics Its impact on the forecast is being reduced. Another possible configuration of the invention features a three-dimensional linear acceleration perturbator. using a corresponding process model and process noise covariance value It is the estimation of the dynamic range of the moving platform by the linear deceleration disturbance. In accordance with its characteristics, it is ensured that it is monitored over time. Another possible configuration of the invention is characterized by the movement of the movable platform. the resulting three-dimensional linear acceleration from the accelerometer pole value and orientation information A linear acceleration output is obtained by separating it from the gravitational acceleration component determined accordingly. It is the creation of the acceleration resulting from motion as gravity. Orientation errors that may arise from the evaluation are reduced and the mobile platform A separate output is obtained regarding its linear motion. Another possible configuration of the invention involves measurements taken from the magnetometer. Suppression of environmental magnetic interference effects in the data and unit magnetic field vector This is the creation of a magnetic field. Thus, changes in magnetic field magnitude and environmental magnetic field 25 The impact of these effects on direction estimation is reduced. Another possible configuration of the invention involves using a unit magnetic field vector. The goal is to determine the compass angle of the movable platform. This allows for the circumferential alignment of the compass angle. calculations to be less affected by changes in magnetic field magnitude is provided. 30 Another possible configuration feature of the invention is the orientation information of the movable platform. in the form of at least one of the following representations: quaternion, rotation matrix, and orientation angles. 6 The goal is to create directional information for different control, navigation, localization, and It is possible to provide them in formats suitable for user interface applications. Another possible configuration feature of the invention is that the orientation information operates at 500 Hz. This is determined in real-time at the specified frequency. Thus, rapid changes in orientation are detected. It can be tracked with low latency and used on highly dynamic moving platforms. 5 This enables the acquisition of trend data. Another possible configuration of the invention involves the transfer of the specified orientation information via USB or Data is transmitted to a host computer via at least one of the UART communication interfaces. Thus, orientation information can be used for external control, viewing, recording, navigation, or localization. This ensures that it can be used by the systems. 10 The present invention also relates to determining the three-dimensional orientation of a mobile platform. It is a method. Accordingly, the method involves the angular velocity of the moving platform and the action taken on the moving platform. Obtaining measurement data regarding acceleration and ambient magnetic field, orientation a situation containing information, at least one sensor polarity value and a three-dimensional linear acceleration perturbator. The vector is generated linearly using sigma points related to the state vector. performing a non-existent estimation process and the orientation components of the orientation space The method involves processing steps taking into account its nonlinear geometry. also the simultaneous information of orientation, sensor pole value and linear acceleration disturbance. estimation of the linear acceleration disturbance from the sensor pole value and gravity its determination separately from its component and the suppression of magnetic interference effects resulting in 20 This involves determining the compass angle. Thus, sensor errors, linear movements, and Three-dimensional orientation information is obtained under environmental magnetic field influences. In a possible configuration of the invention, the nonlinear estimation process is odorless Kalman. This is achieved using a filter. Thus, nonlinear processes and measurements are possible. The models are processed using sigma points. 25 In another possible configuration of the invention, the state vector in the odorless Kalman filter would be incorporated. Instead of the covariance matrix, a representation of the square root of that covariance matrix is ​​used. The square root of covariance matrix is ​​used. This ensures the numerical stability of the filtering operations. is being increased. Another possible configuration of the invention involves square root covariance matrix QR decomposition and 30 Rank-1 is updated using the Cholesky update. Thus, the covariance... more stable and computationally efficient approach to addressing uncertainty is provided. 7 In another possible configuration of the invention, the sigma points relating to the state vector are square rooted. It is generated using the covariance matrix and the nonlinear estimation process is performed by Sigma. This is done according to the weight values ​​corresponding to the points. Thus, the situation the mean and covariance information of the distribution after nonlinear transformations This ensures determination. 5 In another possible configuration of the invention, the sigma points relating to orientation information are logarithmic. The axis-angle representation is converted via a transformation tool, and the processed sigma points are displayed. It is converted into an orientation representation through exponential transformation. Thus, the orientation Performing calculations related to the manifold in a nonlinear orientation space It is possible. 10 In another possible configuration of the invention, the orientation representation is quaternion or rotation. It is at least one of the matrix representations. Thus, the orientation information is three-dimensional. It is processed in a way that is suitable for representing rotations. Another possible configuration of the invention involves the orientation information of the sigma points. The average is 15, obtained through Karcher averaging applied to rotation matrices. This is determined. Thus, an average of the orientation samples is found that is suitable for the manifold geometry. It is ensured that they are brought together around it. In another possible configuration of the invention, angular velocity measurement data obtained from the gyroscope A priori regarding orientation information is obtained by advancing through a nonlinear kinematic model. A state vector is created. Thus, current orientation information is combined with angular motion data. It is estimated accordingly. Another possible configuration of the invention involves three-dimensional acceleration measurement obtained from the accelerometer. using the data and three-dimensional magnetic field measurement data obtained from the magnetometer The prior state vector is updated and a posterior state vector is created. Thus... The predicted orientation is corrected by sensor measurements. 25 In another possible configuration of the invention, the measurement data would be derived from the prior state vector. A residual vector is created to represent the difference between the estimated and measured data, and The state vector is now adaptively determined by a Kalman gain that is determined by the vector. It is being updated. Thus, a correction amount appropriate to the measurement and model uncertainties is provided. is being implemented. 30 In another possible configuration of the invention, the gyroscope pole value is in the state vector. The accelerometer pole value and the magnetometer pole value are recorded, and the pole in question is... 8 The values ​​are dynamically estimated during the operation. Thus, the sensor polarity The impact of time-dependent changes in values ​​on orientation output is reduced. In another possible configuration of the invention, the three-dimensional linear deceleration perturbator would be attached to it. using a corresponding process model and process noise covariance value It is estimated that the time-varying behavior of linear motion effects is thus predicted. 5 It is being monitored. In another possible configuration of the invention, the movement of the movable platform would result from... Three-dimensional linear acceleration is determined from the accelerometer pole value and orientation information. A linear acceleration output is generated by isolating it from the gravitational acceleration component. Thus, the gravity information to be used in the orientation calculation and the platform movement will be 10 The resulting accelerations are separate from each other. In another possible configuration of the invention, the measurement is obtained from the magnetometer. Environmental magnetic interference effects in the data are suppressed, resulting in a unit magnetic field vector. This is done in such a way that magnetic field measurements are more useful in determining the compass angle. It is ensured that it is used safely. 15 In another possible configuration of the invention, the moving object could be made using a unit magnetic field vector. The compass angle of the platform is determined. This allows the movable platform to move in the horizontal plane. Its orientation is being calculated with certainty. In another possible configuration of the invention, the orientation information of the movable platform is quaternion. It is constructed in at least one form, either as a rotation matrix or an orientation angle representation. 20 Thus, the orientation output can be adapted to different application and communication requirements. is provided. In another possible configuration of the invention, the orientation information operates at a frequency of 500 Hz. This is determined in real time. Thus, low-volume movements of high dynamic ranges are achieved. Monitoring is provided with a delay. 25 In another possible configuration of the invention, the specified orientation information is via USB or UART. It is transmitted to a host computer via at least one of the communication interfaces. Thus The calculated orientation information can be viewed, recorded, or displayed by external devices. It can be used in the control of the mobile platform. BRIEF DESCRIPTION OF THE FIGURE 30 Figure 1 shows a representative view of the system. 9 DETAILED DESCRIPTION OF THE INVENTION This detailed explanation of the invention is solely for the purpose of providing a better understanding of the subject matter. This is explained with examples that will not create any limiting effects. In the detailed description of the invention, referring to Figure 1, it is stated that a movable platform moves in three-dimensional space. an orientation determination system that enables the real-time determination of orientation (100) is explained. The mentioned orientation determination system (100) is used in robotic systems, autonomous or semi-autonomous vehicles, surface vehicles, underwater vehicles, aircraft It can be used in vehicles, land vehicles, and wearable technologies. Here The term "mobile platform" refers to a vehicle, device, robot, equipment, or person whose orientation needs to be determined. This includes electronic devices or similar movable structures carried by vehicles. The orientation determination system (100) measures the angular velocity data of the moving platform. At least one gyroscope (120) that enables the acquisition of the acceleration acting on the moving platform. At least one accelerometer (130) that enables the acquisition of measurement data, of the moving platform at least 15 that enable the acquisition of measurement data regarding the magnetic field in the environment. a magnetometer (140) and the aforementioned gyroscope (120), accelerometer (130) and magnetometer It contains at least one processing unit (110) connected with (140). In one possible configuration, the processing unit (110) is in the form of a microcontroller unit. It is structured. Gyroscope (120), accelerometer (130) and magnetometer (140), processing unit (110) It is located on the same electronic circuit as the sensors and the processing unit (110) 20 Data transfer between them is preferably carried out via the I²C communication protocol. The orientation determination system (100) is fixed on the movable platform and the sensor axis The tool is associated with the axis tool of the movable platform. Gyroscope (120), accelerometer (130) and magnetometer (140) preferably on three axes perpendicular to each other. It performs the measurement. Thus, the gyroscope (120) measures the three-dimensional angular velocity vector, 25 accelerometer (130) three-dimensional acceleration vector and magnetometer (140) three-dimensional magnetic field It transfers the vector to the processing unit (110). The sensor models are gyroscope (120), accelerometer (130) and magnetometer (140) respectively. It is defined as follows: (1) 30 (2) (3) Here, ωₘ is the angular velocity vector measured by the gyroscope (120), and ωᵣ is the actual angular velocity. n_g represents the gyroscope polarity vector, b_g represents the gyroscope polarity value, and n_g represents the gyroscope measurement noise. aₘ is the acceleration vector measured by the accelerometer (130), aᵣ is the actual acceleration vector, b_a 5 mₘ represents the accelerometer pole value and n_a represents the accelerometer measurement noise. The magnetic field vector measured by the magnetometer (140), mᵣ is the real magnetic field vector, b_m magnetometer polarity value, n_m magnetometer measurement noise, and d It represents the environmental magnetic disturbance component. The mentioned gyroscope (120), accelerometer (130) and magnetometer (140) pole values ​​are orientation 10 During the estimation, the processing unit (110) is dynamically estimating. Accelerometer (130) The measurement of linear acceleration disturbance caused by the movement of the moving platform It is also being modeled and predicted. The processing unit (110), gyroscope (120), accelerometer (130) and magnetometer (140) It processes the measurement data via a sensor fusion algorithm. The sensor in question is 15. The fusion algorithm operates according to a prediction step and an update step. It includes an odorless Kalman filter. Jacobian or in the prediction and update steps. Sigma points obtained with odorless nonlinear transformation instead of the system matrix and The average of these is used. The odorless Kalman filter integrates the data from the gyroscope (120) to form an orientation 20 This forms the hypothesis. Using the covariance values ​​in the process noise matrix... The covariance matrix is ​​advanced. In the update step, from the accelerometer (130) and Using the three-dimensional acceleration and magnetic field vectors from the magnetometer (140) The hypothesis mentioned is confirmed. The Kalman gain is calculated adaptively, and The state vector is now corrected using the vector. The covariance matrix also corrects the measurement noise. 25 The matrix is ​​updated using prior covariances. Odorless Kalman filter, third-order Taylor series for nonlinear systems. It yields results with accuracy comparable to the approach. In a possible configuration, odorless Kalman The filter is applied in the form of a square root odorless Kalman filter. (Square root odorless Kalman) Numerical stability of 30 was achieved using a rank-1 Cholesky update and QR parsing in the filter. It is being improved. 11 The system model used consists of a nonlinear kinematic model. Continuous The time process model and measurement model are used to represent n white noise, as follows: It is defined as follows: ̇ = ⎣⎢⎢⎢ ⎡ ̇ ̇ ̇ ̇ ⎦⎥⎥ ⎥⎤ = ( , ) = ⎣⎢⎢ ⎡ ⊗ − 000 ⎦⎥⎥ | (4) = = ℎ( ) = + + (5) 5 Here, m_g and a_g are the magnetic field vectors with respect to the inertial coordinate system, respectively. preferably representing the magnetic field vector and the gravitational vector in the ENU formulation b_g, b_m and b_a are gyroscope (120), magnetometer (140) and accelerometer (130) respectively. It represents polar values. For the prediction step, the quaternion update involves changing the quaternion orientation vector q and the angular velocity ω to 10. To represent this, it is carried out in the following manner: ̇ ( ) = ( ) ( ) (6) ( ) = −⌊ ⌋× − 0 (7) Within the filter, exponential and logarithmic transformations are used from quaternion representation to axis- The process moves on to angle representation and rotation matrices. When distributing the sigma points, 15 Rotation matrices and the Karcher averaging algorithm are used. Thus, orientation is determined. The nonlinear manifold structure containing the angles is considered. Orientation The sigma points relating to the information are represented in the local axis-angle representation via logarithmic transformation. is being transformed, operations are performed in this representation and through exponential transformation It is being converted back into a directional representation. 20 The dynamics of the sensor polarity values, where n represents the white noise vector, It is defined as follows: ̇ ( ) = ( ) (8) ̇ ( ) = ( ) (9) 12 ̇ ( ) = ( ) (10) Thus, the working polarity values ​​of the gyroscope (120), magnetometer (140) and accelerometer (130) The changes that occur during this process are tracked within the state vector. With the covariance matrix P = SSᵀ, the equations for the odorless transformation are as follows: It is defined as follows: 5 = ²( + ) − (11) ₀ = (12) ᵢ = + √( + ) ᵢ (13) ᵢ₊ₙ = − √( + ) ᵢ (14) Weight values ​​are calculated as follows: 10 ₀ᵐ = ( + )⁄ (15) ₀ᶜ = ( + )⁄ + (1 − ² + ) (16) ᵢ = 1 [2( + )]⁄ (17) Matrix square root extraction, QR parsing, and rank-1 Cholesky update, among others. This is achieved using cholupdate and choldowndate. The covariance matrix is ​​15. Instead, the matrix S, which is the square root, is stored and used. The covariance matrix is ​​Pₖ₋₁|ₖ₋₁. The transformation between Sₖ₋₁ is defined as follows: ∨ = (18) Sigma points are calculated using the following formula: = [ ̂ ̂ + ̂ − ], = 1, … ,∞ (19) 20 The rest of the filter follows the Kalman filter logic with prediction and update steps. It is working on the sensor polarity values, the filter's operating principle, and mathematical models. It is predicted instantly with the help of this. Additionally, the three-dimensional linear acceleration variables in the state vector are polar in the measurement. In addition to its value, it instantaneously models and predicts linear disturbances. 25 A covariance value in the process model relating to the aforementioned linear disturbances. 13 Thus, linear accelerations related to the motion of the system are measured by the accelerometer (130) pole. It is estimated separately from the value of the gravitational acceleration that causes the rotation. This It improves disruptive suppression performance. Continuous analysis of disruptive estimation. The time system model and measurement vector are defined as follows: ̇ = ( , ) = ⎣⎢⎢ ⎢⎢⎡ ̇ ̇ ̇ ̇ ̇ ⎦⎥⎥ ⎥⎥⎤ = ⎣⎢⎢ ⎢⎡ ⊗ − 0000 ⎦⎥⎥ ⎥⎤ (20) 5 = = ℎ( ) = + + + (21) ̇ ( ) = ( ) (22) The output of the prediction step is the prior state vector, and the output of the update step is the posterior state vector. It represents the state vector, in other words, the algorithm output. Angular velocity measurement data obtained by gyroscope (120) in the estimation step 10 It is used. The processing unit (110) estimates the gyroscope pole from the gyroscope measurement data. By separating the value, we obtain the corrected angular velocity vector, and the aforementioned angular velocity by moving the vector through a nonlinear kinematic model, regarding orientation information It forms the prior state vector. In a possible configuration, gyroscope (120) measurement The integration of the data was done using fourth-order Runge-Kutta integration. is being carried out. In the update step, three-dimensional acceleration measurement data obtained from the accelerometer (130) and Three-dimensional magnetic field measurement data obtained from the magnetometer (140) are used. The processing unit (110) generates estimated measurement data from the prior state vector and the actual The residual vector represents the difference between the measured data and the estimated measured data. It determines the state vector, the aforementioned residual vector, and the adaptively determined vector. Updated using Kalman earnings. Estimated measurement in terms of accelerometer (130) according to the estimated orientation of the moving platform. the gravity vector converted to the sensor axis coordinate system, the estimated accelerometer pole value and includes the estimated three-dimensional linear acceleration perturbator. Thus, the accelerometer (130) 25 The entire acceleration vector measured by [the system] is not considered as gravitational acceleration, and The acceleration resulting from the linear motion of the movable platform is estimated separately. 14 The estimated three-dimensional linear acceleration disturbance reduces the disturbing effect on orientation estimation. It is used in suppression and can also be provided as a linear acceleration output. The aforementioned linear acceleration output is used in the motion analysis, control, and navigation of the moving platform. It can be used for localization purposes. Estimated measurement in terms of magnetometer (140) is defined in the inertial axis system 5 The sensor uses the reference magnetic field vector according to the estimated orientation of the moving platform. It is created by converting it into an axis system. The processing unit (110) is formed from the magnetometer. (140) compares the obtained measurement data with the estimated magnetic field measurement and nearby iron-containing objects, electronic circuits, or other magnetic sources It suppresses magnetic interference effects caused by sensor fusion. 10 A unit magnetic field vector is obtained as a result of suppressing magnetic disturbance effects. The processing unit (110) uses the aforementioned unit magnetic field vector. It determines the compass angle of the moving platform. This allows for the reduction of environmental magnetic effects. Its effect on the compass angle is reduced. Orientation information is provided by the processing unit (110) using quaternion, rotation matrix or orientation angles 15 It is formed in at least one of the forms of representations. Orientation in a possible structure. The detection process is performed in real time at an operating frequency of 500 Hz. Thus, it is used in fast-moving robotic systems, aircraft, and other dynamic applications. Trend changes across platforms are tracked with low latency. The orientation determination system (100) is connected to at least one main computer (150). Main 20 computer (150), desktop computer, portable computer, industrial computer, embedded This can be a computer, a robot control unit, or a vehicle control unit. Orientation determination. Communication between the system (100) and the host computer (150) is via USB or UART communication. This is done through at least one of the interfaces. The processing unit (110) takes into account the determined orientation information, the compass angle and the estimated linear acceleration 25 It transmits its output to the main computer (150). The software running on the main computer (150) or the driver is viewing data received from the direction determination system (100), It is used for recording or controlling the mobile platform. In an example use of the orientation determination system (100), the system is placed on a mobile platform. It is fixed and connected to the host computer (150) via USB or UART. Operation 30 measurement data from unit (110), gyroscope (120), accelerometer (130) and magnetometer (140) It receives and subjects the mentioned measurement data to sensor fusion. Gyroscope (120) Orientation is estimated using measurements by accelerometer (130) and magnetometer (140) The trend predicted using the measurements is being updated. Magnetic effects originating from surrounding iron or electronic equipment. is suppressed and a unit magnetic field vector is obtained. The aforementioned unit magnetic field... The compass angle is calculated based on the field vector. 5 from the movement of the movable platform. the resulting linear acceleration disturbance, separate from gravity and accelerometer pole value The estimated direction angles, compass angle, and linear acceleration information are the main factors. is transferred to the computer (150). Orientation determination system (100) measures the environmental pressure value in a possible configuration. It includes at least one barometer. Pressure measurement data obtained from the barometer is displayed on a mobile 10 the platform's altitude above sea level, relative altitude, or altitude change It is used in determining this. Pressure measurement data obtained from the barometer is processed by the gyroscope (120) by the processing unit (110), independent of the measurement data obtained from the accelerometer (130) and magnetometer (140) 15 can be processed or included as additional measurement input to the sensor fusion algorithm. It can be done. In a possible configuration, the barometer measurement data and the accelerometer (130) The measurement data along the vertical axis are evaluated together, and the vertical of the movable platform is considered. The movement in that direction is determined. Barometric altitude information, together with orientation information, can be transmitted to the main computer (150). Thus, the orientation determination system (100) adds 20 to the three-dimensional orientation of the mobile platform. It can provide information regarding its altitude or vertical movement. The steps described here are performed by the computer running on the processing unit (110). This is carried out through applicable instructions. The aforementioned process steps The order can be changed, provided that the technical functionality is preserved, and some process steps can be performed simultaneously. This can be done as follows. 25 Measurement ranges, sensitivities and of the sensors used in the orientation determination system (100) Sampling frequencies are determined according to the dynamic characteristics of the mobile platform on which the system will be implemented. Process noise and measurement noise covariance values ​​are selected. based on the noise characteristics of the sensors and the expected movements of the moving platform is determined. 30 The nonlinear manifold structure of the orientation space of the square root odorless Kalman filter. Used in conjunction with sigma point operations that take into account sensor polarity values. thanks to the simultaneous estimation of the linear acceleration disturbance with orientation information. 16 the effect of sensor errors and dynamic motion effects on orientation calculation is being reduced. Suppression of magnetic interference effects and compass via unit magnetic field vector. By determining the angle, a more stable compass angle is achieved under environmental magnetic influences. is obtained. By storing the square root of the covariance matrix instead of the covariance matrix and 5 Processing the aforementioned matrix using QR decomposition and rank-1 Cholesky update. This improves numerical stability. Orientation determination system (100), where the described components are located within the same body It can be implemented as an integrated sensor module. The described configurations It expresses examples relating to the application of the invention and the scope of protection of the invention. It does not limit it. The technical specifications described in a configuration must be technically compatible. Provided that it is used in conjunction with the technical specifications of other structures. The scope of protection of the invention is set out in the attached claims, and this detailed explanation is provided below. It is not limited to the structures described for illustrative purposes. A technically expert 15 similar individuals, without deviating from the main theme of the invention, in light of what has been explained above. It is clear that these structures can emerge. REFERENCE NUMBERS GIVEN IN THE FIGURE 100 Orientation determination systems 110 Operation units 120 Gyroscope 20 130 Accelerometers 140 Magnetometers 150 Mainframes

Claims

17 REQUESTS 1. An orientation for determining the three-dimensional orientation of a moving platform. The determination system is (100) and its feature is; obtaining measurement data regarding the angular velocity of the aforementioned mobile platform at least one gyroscope (120) providing; 5 relating to the acceleration acting on the mentioned moving platform at least one accelerometer (130) that enables the acquisition of measurement data; the aforementioned moving obtaining measurement data regarding the magnetic field in the environment where the platform is located providing at least one magnetometer (140) and the aforementioned gyroscope (120), accelerometer (130) and It must contain at least one processing unit (110) connected to the magnetometer (140), the mentioned processing unit (110); 10 - the orientation information of the mentioned mobile platform, the mentioned gyroscope (120), accelerometer (130) and magnetometer (140) with at least one sensor pole its value and the three-dimensional effect resulting from the movement of the aforementioned movable platform. Constructing a state vector that includes a linear acceleration perturbator, - using the sigma points of the mentioned state vector, a nonlinear 15 performing an estimation process, - the orientational components of the aforementioned sigma points, orientation a local vector (axis-angle) that considers the nonlinear geometry of the space between a representation and an orientation representation (quaternion or rotation matrix) processing by transforming 20 - measurements taken from the mentioned gyroscope (120), accelerometer (130) and magnetometer (140) According to the data, the aforementioned orientation information, sensor polarity value, and three-dimensional Simultaneous estimation of linear acceleration disturbance, - the three-dimensional linear accelerometer mentioned is connected to the accelerometer mentioned (130) The gravity is 25, depending on the sensor polarity value and the aforementioned orientation information. determination of the acceleration component separately - the aforementioned orientation information, the aforementioned nonlinear estimation process as a result, to represent the three-dimensional orientation of the moving platform determination, - magnetic interference in the measurement data taken from the mentioned magnetometer (140) 30 According to the magnetic field information obtained by suppressing the effects, the aforementioned moving object Determining the compass angle of the platform It is configured to carry out its steps.

2. According to claim 1, an orientation determination system (100) has the characteristic of; the aforementioned linear 35 The non-predictive process is carried out using an odorless Kalman filter. 18 3. According to claim 2, it is an orientation determination system (100) and its feature is; the mentioned odorless The Kalman filter uses the covariance matrix for the aforementioned state vector instead of the relevant matrix. using a square root of covariance matrix that represents the square root of the covariance matrix The square root of the problem is that it has an odorless Kalman filter.

4. According to claim 3, it is an orientation determination system (100) and its feature is that the mentioned processing unit (110), QR decomposition of the mentioned square root covariance matrix and rank-1 Cholesky It is configured to update using the update function.

5. A system for determining orientation according to claim 3 or 4 (100) and its feature is; the mentioned operation 10 The square root covariance of the sigma points of the mentioned state vector of the unit (110). using the matrix to create it and the weights corresponding to the aforementioned sigma points. configured to perform nonlinear estimation based on the values It is the fact that it has been done.

6. It is a system for determining orientation according to any of the previous requests (100) and its feature is; The logarithmic sigma points of the mentioned processing unit (110) relating to the orientation information It converts the axis-angle representation through a transformation tool and renders the processed sigma points as exponential. configured to transform into an orientation representation through transformation that is. 20 7. According to claim 6, it is an orientation determination system (100) and its feature is; the mentioned orientation The representation must be at least one of the quaternion or rotation matrix representations.

8. A system for determining orientation according to claim 6 or 7 (100) and its feature is; the mentioned operation 25 The unit (110) averages the orientation information of the sigma points of the rotation matrices. configured to determine via the Karcher average applied to it. It is the fact that.

9. A system for determining orientation according to any of the previous requests (100) and its feature is; 30 The mentioned processing unit (110) linearly processes the angular velocity measurement data received from the gyroscope (120). a priori condition regarding orientation information by proceeding through a non-existent kinematic model It is configured to create the vector.

10. According to claim 9, it is an orientation determination system (100) and its feature is; the mentioned operation 35 three-dimensional acceleration measurement data from the accelerometer (130) and unit (110) through three-dimensional magnetic field measurement data obtained from the magnetometer (140) 19 This will create a posterior state vector by updating the aforementioned prior state vector. It is configured in this way.

11. According to claim 10, there is an orientation determination system (100) and its feature is the mentioned operation. Estimated measurement data obtained from the prior state vector with measurement data of unit (110) 5 It creates a residual vector representing the difference between them and the state vector mentioned. It will now update according to a Kalman gain determined adaptively by the vector. It is configured in this way.

12. A system for determining orientation according to any of the previous requests (100) and its feature is; 10 the mentioned state vector's gyroscope (120) pole value, accelerometer (130) pole value and the magnetometer (140) includes the pole value and the mentioned processing unit (110) The subject is configured to dynamically estimate polarity values ​​during the study. It is the fact that it has been done.

13. It is a system for determining orientation according to any of the previous requests (100) and its feature is; The mentioned processing unit (110) corresponds to the three-dimensional linear acceleration perturbator to itself. by using an incoming process model and the process noise covariance value to estimate It is configured.

14. According to claim 13, it is an orientation determination system (100) and its feature is the mentioned operation. unit (110) three-dimensional linear acceleration resulting from the movement of the moving platform Accelerometer (130) Gravity acceleration determined according to pole value and orientation information configured to generate a linear acceleration output by isolating it from its component that is. 25 15. It is a system for determining orientation according to any of the previous requests (100) and its feature is; environmental measurement data from the magnetometer (140) of the mentioned processing unit (110). by suppressing magnetic disturbance effects and generating a unit magnetic field vector It is configured. 30 16. According to claim 15, it is an orientation determination system (100) and its feature is the mentioned operation. unit (110), using the aforementioned unit magnetic field vector, the movable platform It is configured to determine the compass angle. 35 17. It is a system for determining orientation according to any of the previous requests (100) and its feature is; The mentioned processing unit (110) receives the orientation information of the moving platform quaternion, rotation Configure it to generate at least one of the matrix and orientation angle representations. It is the fact that it has been done.

18. It is a system for determining orientation according to any of the previous requests (100) and its feature is; The mentioned processing unit (110) transmits the mentioned orientation information at a working frequency of 500 Hz. It is configured to determine this in real time.

19. It is a system for determining orientation according to any of the previous requests (100) and its feature is; The mentioned processing unit (110) transmits the specified orientation information via USB or UART communication. 10 to be configured to transfer to a host computer (150) via at least one of its interfaces It is the fact that it has been done.

20. A method for determining the three-dimensional orientation of a moving platform. feature; - Measurement data regarding the angular velocity of the mentioned mobile platform must be obtained from at least one gyroscope (120) 15 obtaining by means of, - Measurement data regarding the acceleration acting on the aforementioned moving platform must be obtained from at least one accelerometer. (130) by means of, - measurement of the magnetic field in the environment where the aforementioned mobile platform is located. data must be obtained by means of at least one magnetometer (140), 20 - the orientation information of the mentioned mobile platform, the mentioned gyroscope (120), accelerometer (130) and at least one sensor polarity value relating to at least one of the magnetometers (140) and the mentioned including a three-dimensional linear acceleration disturbance resulting from the movement of the moving platform Creating a state vector, - a nonlinear 25 using sigma points related to the mentioned state vector performing the estimation process, - the orientation information components of the aforementioned sigma points, the orientation space in a nonlinear orientation space, considering its nonlinear geometry Transformation between a representation and an orientation representation, - Measurements obtained from the mentioned gyroscope (120), accelerometer (130) and magnetometer (140) 30 According to the data, the aforementioned orientation information, sensor polarity value, and three-dimensional linear Simultaneous estimation of the accelerometer disturbance, - the three-dimensional linear accelerometer mentioned, relating to the accelerometer (130) gravitational acceleration based on sensor polarity and the aforementioned orientation information. Determining it separately from its component, 35 - magnetic interference in the measurement data obtained from the mentioned magnetometer (140) suppression of effects and 21 - based on magnetic field information obtained as a result of suppressing magnetic interference effects Determining the compass angle of the aforementioned mobile platform. It is characterized by including its steps.

21. A method according to claim 20, whose characteristic is; the aforementioned nonlinear estimation 5 The process is carried out using an odorless Kalman filter.

22. A method according to claim 21, characterized by the use of the aforementioned odorless Kalman filter, instead of the covariance matrix relating to the aforementioned state vector, the covariance in question This involves using a square root covariance matrix that represents the square root of the matrix. 10 23. A method according to claim 22, whose characteristic is; the QR of the aforementioned square root covariance matrix. This involves parsing and updating using the rank-1 Cholesky update.

24. A method according to claim 22 or 23, and its characteristic is; relating to the aforementioned state vector 15 Generating sigma points using the square root of covariance matrix and linearly the weight values ​​corresponding to the aforementioned sigma points of the non-existent estimation process It is carried out according to [the regulations].

25. A method according to any of the previous method requirements, and its characteristic is; orientation 20 axis-angle representation of sigma points relating to information via logarithmic transformation Transformation and orientation of processed sigma points via exponential transformation. It is the transformation of its representation.

26. It is a method according to claim 25, and its characteristic is that the mentioned orientation representation is quaternion 25. It must be at least one of the following: a rotation matrix representation.

27. A method according to claim 25 or 26, characterized by its reliance on the orientation information of the sigma points. the average of the related rotation matrices is calculated using Karcher averaging. It is the determination of. 30 28. A method that, according to any of the previous method requirements, has the characteristic of being from a gyroscope. (120) A nonlinear kinematic model of the obtained angular velocity measurement data The goal is to create a priori state vector regarding orientation information by progressing through this process. 35 29. It is a method according to claim 28, and its feature is the three-dimensional acceleration obtained from the accelerometer (130). measurement data and three-dimensional magnetic field measurement data obtained from the magnetometer (140) 22 By using the aforementioned prior state vector to update and generate a posterior state... It is the creation of the vector.

30. It is a method according to claim 29, and its characteristic is that it is obtained from measurement data and a priori state vector.

5. Creating a residual vector that represents the difference between the estimated and measured data. and a Kalman vector that is adaptively determined by the aforementioned residual vector of the state vector. It is updated according to your earnings.

31. A method according to any of the previous method requirements, and its characteristic is; mentioned In the state vector, the pole value of the gyroscope (120), the pole value of the accelerometer (130) and 10 holding the pole value of the magnetometer (140) and working with the pole values ​​in question. It is the dynamic estimation during the process.

32. A method according to any of the previous method requirements, and its characteristic is; mentioned a three-dimensional linear acceleration disturbance, a corresponding process model and process 15 Noise estimation is achieved using the covariance value.

33. A method according to claim 32, characterized by its movement from the movable platform. The resulting three-dimensional linear acceleration is derived from the pole value and orientation of the accelerometer (130). By separating the gravitational acceleration component determined according to the information, a linear acceleration 20 It is the creation of the output.

34. A method that conforms to any of the previous method requirements and whose characteristic is; environmental magnetic interference effects in the measurement data obtained from the magnetometer (140) It is the creation of a unit magnetic field vector by suppression. 25 35. A method according to claim 34, whose characteristic is; the aforementioned unit magnetic field vector. This involves determining the compass angle of the moving platform using a device.

36. A method according to any of the previous method requirements, and its characteristic is; mobile 30 platform orientation information from quaternion, rotation matrix and orientation angle representations It is created in at least one form.

37. A method according to any of the previous method requirements, and its characteristic is; mentioned It is the real-time determination of orientation information at an operating frequency of 500 Hz. 35 23 38. A method according to any of the previous method requirements, and its characteristic is; specified Orientation information is transmitted to a host via at least one of the USB or UART communication interfaces. transfer to the computer (150).

Images