Positioning method, device, medium and electronic equipment of movable mechanism

Abstract

This application provides a positioning method, apparatus, medium, and electronic device for a movable mechanism. The method includes: acquiring a first initial roll angle, a first initial pitch angle, and a first initial heading angle of an inertial measurement unit coordinate system relative to a mechanism coordinate system; performing coordinate transformation on the first initial roll angle, the first initial pitch angle, and the first initial heading angle to obtain a second initial roll angle, a second initial pitch angle, and a second initial heading angle of the mechanism coordinate system relative to the inertial measurement unit coordinate system; solving an error function to obtain a second target pitch angle and a second target heading angle of the mechanism coordinate system relative to the inertial measurement unit coordinate system; and generating an installation angle based on the second target pitch angle and the second target heading angle for positioning. This application embodiment can accurately position a movable mechanism. This application embodiment can be applied to various scenarios such as mapping, autonomous driving, intelligent transportation, and assisted driving.

Description

Technical Field

[0001] This application relates to the field of vehicle navigation technology, and more specifically, to a positioning method, apparatus, computer-readable medium, and electronic device for a movable mechanism. Background Technology

[0002] Currently, vehicle navigation technology increasingly utilizes data collected by IMUs (Inertial Measurement Units) for navigation and positioning. Therefore, determining the installation angle between the IMU and the vehicle's coordinate system has become a pressing issue.

[0003] However, existing methods for estimating the installation angle can only achieve certain results in specific scenarios, have poor versatility, and are inefficient and have low accuracy, which leads to vehicles being unable to accurately navigate and locate. Summary of the Invention

[0004] The embodiments of this application provide a positioning method, apparatus, computer-readable medium, and electronic device for a movable mechanism, which can at least to some extent improve the efficiency and accuracy of estimating the installation angle, further improve the versatility of estimating the installation angle, and improve the accuracy of positioning.

[0005] Other features and advantages of this application will become apparent from the following detailed description, or may be learned in part from practice of this application.

[0006] According to one aspect of the embodiments of this application, a positioning method for a movable mechanism is provided, the method comprising: acquiring a first initial roll angle and a first initial pitch angle of an inertial measurement unit (IMU) coordinate system relative to a mechanism coordinate system of the movable mechanism, wherein the IMU is located on the movable mechanism; determining a first initial heading angle of the IMU coordinate system relative to the mechanism coordinate system, and performing coordinate transformations on the first initial roll angle, the first initial pitch angle, and the first initial heading angle to obtain a second initial roll angle, a second initial pitch angle, and a second initial heading angle of the mechanism coordinate system relative to the IMU coordinate system; and, based on the coordinate transformation of the mechanism coordinate system... An error function is established based on the difference between the velocity transformation result to the inertial measurement unit (IMU) coordinate system and the velocity in the IMU coordinate system. The minimum value of the error function is solved iteratively to obtain the second target pitch angle and the second target heading angle of the mechanism coordinate system relative to the IMU coordinate system. The second initial pitch angle and the second initial heading angle are used to initially transform the velocity of the mechanism coordinate system to the IMU coordinate system. An installation angle between the IMU coordinate system and the mechanism coordinate system is generated based on the second initial roll angle, the second target pitch angle, and the second target heading angle, so as to position the movable mechanism based on the installation angle.

[0007] According to one aspect of the embodiments of this application, a positioning device for a movable mechanism is provided. The device includes: an acquisition unit, configured to acquire a first initial roll angle and a first initial pitch angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system of the movable mechanism, wherein the inertial measurement unit is located on the movable mechanism; a determination and transformation unit, configured to determine a first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system, and perform coordinate transformation on the first initial roll angle, the first initial pitch angle, and the first initial heading angle respectively to obtain a second initial roll angle, a second initial pitch angle, and a second initial heading angle of the mechanism coordinate system relative to the inertial measurement unit coordinate system; and an establishment and solution unit, configured to establish and solve the following... An error function is established based on the difference between the velocity of the mechanism coordinate system converted to the inertial measurement unit (IMU) coordinate system and the velocity of the IMU coordinate system. The minimum value of the error function is solved iteratively to obtain the second target pitch angle and the second target heading angle of the mechanism coordinate system relative to the IMU coordinate system. The second initial pitch angle and the second initial heading angle are used to initially convert the velocity of the mechanism coordinate system to the IMU coordinate system. A generation unit is used to generate an installation angle between the IMU coordinate system and the mechanism coordinate system based on the second initial roll angle, the second target pitch angle, and the second target heading angle, so as to position the movable mechanism based on the installation angle.

[0008] In some embodiments of this application, based on the foregoing scheme, the determination and conversion unit is configured as follows: a plurality of first initial heading angles are set with a preset angle step size; each first initial heading angle is combined with the first initial roll angle and the first initial pitch angle to form an initial mounting angle; for each initial mounting angle, the velocity of the inertial measurement unit coordinate system is converted to the mechanism coordinate system according to the initial mounting angle, and the difference between the converted result and the velocity of the mechanism coordinate system is determined; based on the difference corresponding to each initial mounting angle, the error corresponding to each initial mounting angle is obtained, and the first initial heading angle among the initial mounting angles with the smallest corresponding error is taken as the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system.

[0009] In some embodiments of this application, based on the foregoing scheme, the determination and conversion unit is configured to: if the velocity of the inertial measurement unit coordinate system and the velocity of the mechanism coordinate system corresponding to the target number of times are obtained, then determine the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system.

[0010] In some embodiments of this application, based on the foregoing scheme, the determination and conversion unit is configured to: for each initial installation angle, convert the velocity of the inertial measurement unit coordinate system to the mechanism coordinate system at each moment according to the initial installation angle, and determine the difference between the converted result at each moment and the velocity of the mechanism coordinate system at the same moment; and obtain the error corresponding to each initial installation angle based on the difference between each initial installation angle and the target number of moments respectively.

[0011] In some embodiments of this application, based on the foregoing scheme, the establishment and solution unit is configured as follows: taking the second initial pitch angle as the second current pitch angle and the second initial heading angle as the second current heading angle; iteratively executing the error function solution step until a predetermined convergence condition is met, wherein the error function solution step includes: determining the Jacobian matrix and the error value of the error function on each axis according to the second current pitch angle and the second current heading angle respectively; solving for the increments of the second current pitch angle and the second current heading angle according to the Jacobian matrix and the error value of the error function on each axis; and updating the second current pitch angle and the second current heading angle based on the increments.

[0012] In some embodiments of this application, based on the foregoing scheme, the determining and transforming unit is configured to: determine a first rotation matrix of the inertial measurement unit coordinate system relative to the mechanism coordinate system based on the first initial roll angle, the first initial pitch angle, and the first initial heading angle; determine the transpose matrix of the first rotation matrix to obtain a second rotation matrix; and determine the second initial roll angle, the second initial pitch angle, and the second initial heading angle of the mechanism coordinate system relative to the inertial measurement unit coordinate system based on the second rotation matrix.

[0013] In some embodiments of this application, based on the foregoing scheme, the acquisition unit is configured to: determine the first initial roll angle and the first initial pitch angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system of the movable mechanism based on the output values ​​of the accelerometers in the inertial measurement unit on each axis.

[0014] In some embodiments of this application, based on the foregoing scheme, the acquisition unit is configured to: acquire multiple output values ​​of the accelerometer in the inertial measurement unit on each axis within a predetermined time period; determine the average value of the multiple output values ​​of the accelerometer on each axis; and determine the first initial roll angle and the first initial pitch angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system of the movable mechanism based on the average value of the multiple output values ​​on each axis.

[0015] In some embodiments of this application, based on the foregoing scheme, the generation unit is configured to: combine the second initial roll angle, the second target pitch angle, and the second target heading angle into an installation angle between the inertial measurement unit coordinate system and the mechanism coordinate system; or convert the second initial roll angle, the second target pitch angle, and the second target heading angle into a first target roll angle, a first target pitch angle, and a first target heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system, and combine the first target roll angle, the first target pitch angle, and the first target heading angle into an installation angle between the inertial measurement unit coordinate system and the mechanism coordinate system.

[0016] According to one aspect of the embodiments of this application, a computer-readable medium is provided having a computer program stored thereon, which, when executed by a processor, implements the positioning method of the movable mechanism as described in the above embodiments.

[0017] According to one aspect of the embodiments of this application, an electronic device is provided, including: one or more processors; and a storage device for storing one or more programs, which, when executed by the one or more processors, cause the one or more processors to implement the positioning method of the movable mechanism as described in the above embodiments.

[0018] According to one aspect of the embodiments of this application, a computer program product is provided, the computer program product including computer instructions stored in a computer-readable storage medium, a processor of a computer device reading the computer instructions from the computer-readable storage medium, and the processor executing the computer instructions, causing the computer device to perform the positioning method of the movable mechanism as described in the above embodiments.

[0019] In some embodiments of this application, the technical solutions are provided by first obtaining the first initial roll angle, the first initial pitch angle, and the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system of the movable mechanism, and then converting the first initial roll angle, the first initial pitch angle, and the first initial heading angle into the second initial roll angle, the second initial pitch angle, and the second initial heading angle of the mechanism coordinate system relative to the inertial measurement unit coordinate system. Based on this, starting from the second initial pitch angle and the second initial heading angle, the minimum value of the error function is solved iteratively until the second target pitch angle and the second target heading angle are found. Finally, the installation angle can be generated based on the second initial roll angle, the second target pitch angle, and the second target heading angle. Therefore, the entire scheme is applicable to various scenarios where inertial measurement units are installed on movable mechanisms, demonstrating strong versatility. Since the error function is established based on the difference between the velocity of the mechanism coordinate system and the velocity of the inertial measurement unit coordinate system, and the second target pitch angle and second target heading angle are obtained by iteratively solving for the minimum value of the error function, the installation angle can be accurately estimated. Furthermore, since the iterative solution of the error function starts from the second initial pitch angle and the second initial heading angle, it does not need to start from 0, improving the efficiency of estimating the installation angle and allowing for rapid estimation. Based on the accurate estimation of the installation angle, the positioning of the movable mechanism can be achieved more precisely.

[0020] It should be understood that the above general description and the following detailed description are exemplary and explanatory only, and do not limit this application. Attached Figure Description

[0021] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application. It is obvious that the drawings described below are merely some embodiments of this application, and those skilled in the art can obtain other drawings based on these drawings without any inventive effort. In the drawings:

[0022] Figure 1 A schematic diagram of an exemplary system architecture that can be used to implement the technical solutions of the embodiments of this application is shown;

[0023] Figure 2 A flowchart illustrating a positioning method for a movable mechanism according to an embodiment of this application is shown;

[0024] Figure 3 A schematic diagram showing the relationship between the IMU coordinate system and the vehicle coordinate system according to an embodiment of this application is shown;

[0025] Figure 4An embodiment according to this application is shown. Figure 2 A flowchart detailing step 210 in the embodiment;

[0026] Figure 5 A flowchart illustrating the determination of a first initial roll angle and a first initial pitch angle based on the output values ​​of the accelerometers in the inertial measurement unit on each axis, according to an embodiment of this application, is shown.

[0027] Figure 6 A flowchart illustrating the determination of a first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system according to an embodiment of this application is shown.

[0028] Figure 7 A flowchart illustrating coordinate transformation of a first initial roll angle, a first initial pitch angle, and a first initial heading angle according to an embodiment of this application is shown.

[0029] Figure 8 A flowchart illustrating an embodiment of this application for iteratively solving for the minimum value of an error function is shown;

[0030] Figure 9 A schematic diagram of the overall process according to an embodiment of this application is shown;

[0031] Figure 10 A block diagram of a positioning device for a movable mechanism according to an embodiment of this application is shown;

[0032] Figure 11 A schematic diagram of the structure of a computer system suitable for implementing the electronic device of the present application is shown. Detailed Implementation

[0033] Exemplary embodiments will now be described more fully with reference to the accompanying drawings. However, these exemplary embodiments can be implemented in many forms and should not be construed as limited to the examples set forth herein; rather, these embodiments are provided to make this application more comprehensive and complete, and to fully convey the concept of the exemplary embodiments to those skilled in the art.

[0034] Furthermore, the described features, structures, or characteristics can be combined in any suitable manner in one or more embodiments. Numerous specific details are provided in the following description to give a thorough understanding of embodiments of this application. However, those skilled in the art will recognize that the technical solutions of this application can be practiced without one or more of the specific details, or other methods, components, apparatuses, steps, etc., can be employed. In other instances, well-known methods, apparatuses, implementations, or operations are not shown or described in detail to avoid obscuring various aspects of this application.

[0035] The block diagrams shown in the accompanying drawings are merely functional entities and do not necessarily correspond to physically independent entities. That is, these functional entities can be implemented in software, in one or more hardware modules or integrated circuits, or in different network and / or processor devices and / or microcontroller devices.

[0036] The flowcharts shown in the accompanying drawings are merely illustrative and do not necessarily include all content and operations / steps, nor do they necessarily need to be performed in the described order. For example, some operations / steps can be broken down, while others can be combined or partially combined; therefore, the actual execution order may change depending on the specific circumstances.

[0037] An IMU (Inertial Measurement Unit), also known as an inertial sensor, contains a three-axis accelerometer and a three-axis gyroscope, which measure the acceleration and angular velocity of the carrier, respectively. After integration, the carrier's position, velocity, and attitude information can be obtained.

[0038] NHC (nonholonomic) refers to the constraint that the lateral and longitudinal velocities are zero in the vehicle coordinate system during normal vehicle operation.

[0039] In the field of vehicle navigation technology, information fusion between IMU (Integrated Measurement Unit) and GNSS (Global Navigation Satellite System), visual sensors, and LiDAR is commonly used to obtain high-precision and continuous positioning results. However, when the latter three sensors fail, the IMU performs self-integration and estimation for positioning. Due to inherent observation errors, the positioning error increases over time. To address this issue, the effective utilization of NHC (Neural Harmony Conversion) and vehicle (wheel) speed information becomes crucial. The application of NHC and wheel speed observation information is based on the vehicle's coordinate system, but navigation coordinate information is typically based on the IMU coordinate system and the navigation coordinate system. Therefore, determining the rotational relationship between the IMU and the vehicle's coordinate system, i.e., the installation angle, becomes a pressing problem. Extensive research has been conducted in academia and industry on the problem of installation angle estimation, resulting in numerous proposed methods.

[0040] Among the relevant technologies, there are three methods that can achieve certain results in specific scenarios.

[0041] Method 1: When designing a combined navigation system, the installation angle is used as the parameter to be estimated. The speed information of NHC and wheel speed is updated and estimated together with the position, velocity and attitude.

[0042] Method 2: Instead of observing velocity, estimate the installation angle by analyzing the correlation of trajectories.

[0043] Method 3: Estimate the installation angle by the correlation between the IMU angular velocity and the vehicle body angular velocity.

[0044] However, these methods all have certain drawbacks:

[0045] For Method 1, it is generally assumed that the installation angle is a small angle. This assumption leads to relatively fast convergence when the IMU is fixedly installed on the vehicle and the initial installation angle is small. However, for mobile phone IMUs, which can be freely installed and have a larger installation angle, the algorithm takes too long to converge and is inefficient.

[0046] Method 2 requires the IMU to perform a self-derivation process, and its algorithm has high requirements for IMU accuracy. Therefore, it may be difficult to apply to consumer-grade IMUs.

[0047] Method three requires more stringent estimation conditions and necessitates a series of turning maneuvers by the vehicle, which is rather unreasonable in practical applications.

[0048] Therefore, this application first provides a method for locating a movable mechanism. The method for locating a movable mechanism provided in this application overcomes the aforementioned shortcomings and is applicable to installation angle estimation in mobile phone IMU scenarios. It does not require high precision for the IMU, nor does it require setting stringent estimation conditions. Even for mobile phone IMUs, which can be freely installed and have a large installation angle, the installation angle can be estimated quickly and accurately. Therefore, it improves the versatility and convenience of installation angle estimation, thereby enabling more precise positioning of the movable mechanism.

[0049] Figure 1 A schematic diagram of an exemplary system architecture that can be used to implement the technical solutions of the embodiments of this application is shown. Figure 1As shown, the system architecture 100 includes a vehicle 110 and a cloud 120, which are capable of communication. Specifically, the vehicle 110 includes a lidar 111, a smartphone 112, and a GNSS (Global Navigation Satellite System) sensor 113. The smartphone 112 has a built-in IMU (Inertial Measurement Unit). The vehicle 110 can transmit signals collected by the lidar 111, the IMU in the smartphone 112, and the GNSS sensor 113 to the cloud 120 to obtain high-precision positioning results returned by the cloud 120. When the lidar 111 and GNSS sensor 113 fail, the vehicle 110 estimates the installation angle of the IMU coordinate system in the smartphone 112 relative to the vehicle body coordinate system through the following process: First, the vehicle 110 determines the first initial roll angle, the first initial pitch angle, and the first initial heading angle of the IMU coordinate system relative to the vehicle body coordinate system, and converts the first initial roll angle, the first initial pitch angle, and the first initial heading angle into the second initial roll angle, the second initial pitch angle, and the second initial heading angle of the vehicle body coordinate system relative to the IMU coordinate system; then, the vehicle 110 obtains the second target pitch angle and the second target heading angle of the vehicle body coordinate system relative to the IMU coordinate system by finding the minimum value of the error function established based on the second initial pitch angle and the second initial heading angle; finally, the vehicle 110 generates the installation angle of the vehicle body coordinate system relative to the IMU coordinate system based on the second initial roll angle, the second target pitch angle, and the second target heading angle. After obtaining the installation angle, vehicle 110 will send the signal collected by IMU in smartphone 112, non-integrity constraints, vehicle speed observation information, and installation angle of vehicle coordinate system relative to IMU coordinate system to cloud 120, thereby obtaining the positioning result returned by cloud 120.

[0050] In some embodiments of this application, the IMU in the smartphone 112 includes an accelerometer and a gyroscope, and the vehicle 110 determines the first initial roll angle and the first initial pitch angle of the IMU coordinate system relative to the vehicle coordinate system based on the output values ​​of the accelerometer in the IMU on each axis.

[0051] In some embodiments of this application, after determining the first initial roll angle and the first initial pitch angle of the IMU coordinate system relative to the vehicle coordinate system, the vehicle 110 obtains the first initial heading angle by keeping the first initial roll angle and the first initial pitch angle unchanged and searching in preset angle steps each time.

[0052] In some embodiments of this application, the vehicle 110 estimates the first initial heading angle, the second target pitch angle, and the second target heading angle based on the velocity in the vehicle coordinate system and the velocity in the IMU coordinate system.

[0053] It should be understood that Figure 1 The number of lidar, GNSS sensors, IMUs, and smartphones shown is merely illustrative. Depending on implementation needs, any number of lidar, GNSS sensors, IMUs, and smartphones can be used; for example, there can be multiple lidar, GNSS sensors, IMUs, and smartphones.

[0054] It should be noted that, Figure 1 The example shown is only one embodiment of this application. Although Figure 1 The solution in this embodiment is used in the field of vehicle navigation, but in other embodiments of this application, the solution can also be applied to various other fields, such as robot navigation; although in Figure 1 In the embodiment of the scheme, the IMU is located on the smartphone, but in other embodiments of this application, the IMU can be located on various devices, or the IMU can be installed as a separate device in a car, or even directly embedded in the vehicle; although in Figure 1 In the embodiment, the installation angle estimation is performed on the vehicle, while vehicle positioning is implemented in the cloud. However, in other embodiments of this application, both installation angle estimation and vehicle positioning can be implemented on the vehicle or both in the cloud. Figure 1 In the embodiments described, the vehicle is located based on LiDAR, smartphones, and GNSS sensors. However, in other embodiments of this application, the vehicle can also be located based on various other types of sensors, such as visual sensors and ultrasonic sensors. This application does not limit the scope of protection of this application in any way.

[0055] It is easy to understand that the positioning method for the movable mechanism provided in this application embodiment is generally executed by an in-vehicle terminal, and correspondingly, the positioning device for the movable mechanism is generally installed in the in-vehicle terminal. However, in other embodiments of this application, the server may also have similar functions to the in-vehicle terminal, thereby executing the positioning scheme for the movable mechanism provided in this application embodiment.

[0056] Therefore, the embodiments of this application can be applied to terminals or servers. The server can be a standalone physical server, a server cluster or distributed system composed of multiple physical servers, or a cloud server providing basic cloud computing services such as cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network services, cloud communication, middleware services, domain name services, security services, CDN, and big data and artificial intelligence platforms. The terminal can be a smartphone, tablet, laptop, desktop computer, smart speaker, smartwatch, etc., but is not limited to these. The terminal and server can be directly or indirectly connected via wired or wireless communication, and this application does not impose any restrictions.

[0057] The implementation details of the technical solutions in the embodiments of this application are described in detail below:

[0058] Figure 2 A flowchart illustrating a positioning method for a movable mechanism according to an embodiment of this application is shown. This positioning method can be executed by various computing and processing devices, such as in-vehicle terminals or cloud servers. In-vehicle terminals include, but are not limited to, mobile phones, computers, intelligent voice interaction devices, and portable devices. Embodiments of this application can be applied to various scenarios, including but not limited to cloud technology, artificial intelligence, smart transportation, and assisted driving. Please refer to... Figure 2 As shown, the positioning method of this movable mechanism includes at least the following steps:

[0059] In step 210, the first initial roll angle and the first initial pitch angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system of the movable mechanism are obtained.

[0060] The inertial measurement unit is located on the movable mechanism.

[0061] An inertial measurement unit (IMU) can be a standalone device or an embedded module within a device such as a smartphone. The movable mechanism can be any type of carrier capable of housing the IMU, such as a vehicle, autonomous vehicle, or robot. When the movable mechanism is a vehicle, its mechanism coordinate system is the same as the vehicle's body coordinate system. The following will use a vehicle as an example to describe the solution of this application's embodiments.

[0062] Figure 3 A schematic diagram showing the relationship between the IMU coordinate system and the vehicle body coordinate system according to an embodiment of this application is provided. Please refer to... Figure 3 As shown, it displays a top view of two coordinate systems: the b-frame and the v-frame. The b-frame is short for IMU coordinate system (Body Frame), and the v-frame is short for vehicle coordinate system (Vehicle Frame). The relationship between the two coordinate systems is... Figure 3 As shown, a certain rotation angle exists between the two. Both the b-frame and the v-frame include X-axis, Y-axis, and Z-axis. In the v-frame, the vehicle's forward direction is taken as the X-axis, the Z-axis is perpendicular to the vehicle body and pointing downwards, and the Y-axis is to the right, forming a right-handed coordinate system with the X-axis and Z-axis. The velocity in the v-frame is the vehicle's velocity. According to non-holonomic constraints, the velocity in the v-frame is special and can be expressed as v v =[v vx 0 0] T , where v vx The velocity in the v-frame is the velocity along the x-axis; the velocity in the b-frame is determined based on the IMU output value, and can be expressed as v. b =[v bx v by v bz ] T , where v bx Let v be the velocity of system b in the x-axis direction. by Let v be the velocity of system b in the y-axis direction. bz Let be the velocity of system b in the z-axis direction.

[0063] The rotational relationship between the two coordinate systems is represented by two sets of Euler angles: the Euler angles for rotation from the b-frame to the v-frame are respectively... (roll angle) (pitch angle) (Heading angle), corresponding rotation matrix The Euler angles of rotation from the v-frame to the b-frame are respectively (roll angle) (pitch angle) (Heading angle), corresponding rotation matrix

[0064] The conversion relationship between Euler angles and rotation matrices is shown in the following two formulas:

[0065]

[0066]

[0067] Where C is the rotation matrix, φ is the roll angle, θ is the pitch angle, and ψ is the yaw angle. 32 This represents the third row and second column of the rotation matrix, where cθ = cosθ, sθ = sinθ, and so on.

[0068] Figure 4 An embodiment according to this application is shown. Figure 2 A flowchart detailing step 210 in the embodiment. Please refer to [link / reference]. Figure 4 As shown, Figure 2 Step 210 in the embodiment may specifically include the following steps:

[0069] In step 210', the first initial roll angle and the first initial pitch angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system of the movable mechanism are determined based on the output values ​​of the accelerometers in the inertial measurement unit on each axis.

[0070] The IMU includes a three-axis accelerometer, which can output values ​​corresponding to each axis.

[0071] The inventors of this application discovered that, due to the existence of gravity, the projection of the direction of gravity sensed by the accelerometer onto each axis can reflect the current roll and pitch angles of the IMU relative to the geographic coordinate system. When a vehicle is driving normally on the road, it can be assumed that the vehicle coordinate system is approximately consistent with the geographic coordinate system in the two dimensions of roll and pitch. Therefore, the first initial roll angle and the first initial pitch angle of the IMU coordinate system relative to the vehicle coordinate system can be determined based on the output values ​​of the accelerometer on each axis.

[0072] Figure 5 A flowchart illustrating the determination of a first initial roll angle and a first initial pitch angle based on the output values ​​of the accelerometers in the inertial measurement unit along each axis, according to an embodiment of this application, is shown. See also... Figure 5 As shown, the following steps may be included:

[0073] In step 510, multiple output values ​​of the accelerometer in the inertial measurement unit for each axis are acquired within a predetermined time period.

[0074] An accelerometer can generate output values ​​corresponding to each axis at a certain frequency, thus obtaining multiple output values ​​corresponding to each axis within a predetermined time period. For example, if the predetermined time period is 10 seconds, and one output value corresponding to each axis can be obtained every second, then 10 output values ​​corresponding to each axis can be obtained within 10 seconds.

[0075] In step 520, the average value of multiple output values ​​of the accelerometer on each axis is determined.

[0076] Since each axis corresponds to multiple output values, the average of the multiple output values ​​corresponding to each axis can be determined.

[0077] In step 530, the first initial roll angle and the first initial pitch angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system of the movable mechanism are determined based on the average of multiple output values ​​for each axis.

[0078] Specifically, the first initial roll angle and the first initial pitch angle can be determined based on the average of multiple output values ​​for each axis and using the following formula:

[0079]

[0080] Among them, F x F is the average value of the accelerometer's output value along the x-axis. y F is the average value of the accelerometer's output value on the y-axis. z This represents the average value of the accelerometer's output along the z-axis. This is the initial roll angle of the B-series relative to the V-series. Let be the first initial pitch angle of the b-frame relative to the v-frame.

[0081] In this embodiment of the application, by determining the first initial roll angle and the first initial pitch angle based on the average of multiple output values ​​of each axis, the influence of random factors can be eliminated, and the accuracy of the determined first initial roll angle and the first initial pitch angle can be improved, thereby improving the efficiency of installation angle estimation.

[0082] In one embodiment of this application, the variance of multiple output values ​​of the accelerometer in the inertial measurement unit on each axis is less than a predetermined variance threshold within a predetermined time period.

[0083] In one embodiment of this application, within a predetermined time period, the maximum difference between multiple output values ​​of the accelerometer in the inertial measurement unit on each axis is less than a predetermined difference threshold.

[0084] The variance of multiple output values, or the maximum difference among multiple output values, reflects the stability of the multiple output values ​​for each axis. Therefore, by determining the first initial roll angle and the first initial pitch angle only under these conditions, the accuracy of determining the first initial roll angle and the first initial pitch angle is improved.

[0085] In step 220, the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system is determined, and the first initial roll angle, the first initial pitch angle and the first initial heading angle are transformed to obtain the second initial roll angle, the second initial pitch angle and the second initial heading angle of the mechanism coordinate system relative to the inertial measurement unit coordinate system.

[0086] After obtaining the first initial roll angle of the b-system relative to the v-system. and the first initial pitch angle Next, it is necessary to further determine the first initial heading angle of the B-series relative to the V-series.

[0087] In one embodiment of this application, determining the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system includes: randomly generating the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system.

[0088] Figure 6A flowchart illustrating the determination of the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system according to one embodiment of this application is shown. Figure 6 In the embodiment shown, a fixed first initial roll angle is used. and the first initial pitch angle The initial heading angle remains unchanged; the search calculates the first initial heading angle. Please see Figure 6 As shown, the following steps may be included:

[0089] In step 610, multiple first initial heading angles are set with a preset angle step size, and each first initial heading angle is combined with a first initial roll angle and a first initial pitch angle to form an initial installation angle.

[0090] Specifically, the preset angle step size can be freely set, for example, it can be set to 10 degrees. By setting the preset angle step size in 10-degree increments from 0 degrees to 360 degrees, 36 first initial heading angles can be obtained. Each first initial heading angle With the first initial roll angle and the first initial pitch angle By combining the components, an initial installation angle is obtained.

[0091] In step 620, for each initial installation angle, the velocity of the inertial measurement unit coordinate system is converted to the mechanism coordinate system based on the initial installation angle, and the difference between the converted result and the velocity of the mechanism coordinate system is determined.

[0092] The velocity v of the inertial measurement unit coordinate system b velocity v relative to the mechanism coordinate system v It can be the speed collected at the same time.

[0093] Specifically, the first initial heading angle in each initial installation angle can be... First initial roll angle and the first initial pitch angle Substituting these values ​​into the formula for calculating the rotation matrix C, we obtain... Then according to The velocity v of the b system b Transform to the v-frame and determine the velocity v of the transformed system relative to the b-frame. b The difference between them.

[0094] In step 630, the error corresponding to each initial installation angle is obtained based on the difference corresponding to each initial installation angle, and the first initial heading angle of the initial installation angle with the smallest corresponding error is taken as the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system.

[0095] In this embodiment of the application, by first determining the first initial heading angle based on the error, and then performing the subsequent installation angle estimation steps, the estimation efficiency of the installation angle can be improved.

[0096] In one embodiment of this application, determining the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system includes: if the velocities of the inertial measurement unit coordinate system and the mechanism coordinate system corresponding to the target number of times are obtained, then determining the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system.

[0097] The velocities of the inertial measurement unit coordinate system and the mechanism coordinate system are collected at each moment. When the velocities of the inertial measurement unit coordinate system and the mechanism coordinate system corresponding to the target number of moments are collected, the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system is determined.

[0098] In this embodiment of the application, after collecting the velocities corresponding to the target number of times, the first initial heading angle can be determined based on these velocities.

[0099] In one embodiment of this application, for each initial installation angle, the velocity of the inertial measurement unit coordinate system is transformed to the mechanism coordinate system based on the initial installation angle, and the difference between the transformed result and the velocity of the mechanism coordinate system is determined, including:

[0100] For each initial installation angle, the velocity of the inertial measurement unit coordinate system at each moment is transformed to the mechanism coordinate system based on the initial installation angle, and the difference between the transformed result at each moment and the velocity of the mechanism coordinate system at the same moment is determined.

[0101] Based on the difference corresponding to each initial installation angle, the error corresponding to each initial installation angle is obtained, including:

[0102] The error corresponding to each initial installation angle is obtained by calculating the difference between each initial installation angle and the target number of time points.

[0103] In one embodiment of this application, the error corresponding to each initial installation angle is obtained based on the difference between each initial installation angle and the target number of times, including: determining the sum of the absolute values ​​of the differences between each initial installation angle and the target number of times, as the error corresponding to each initial installation angle.

[0104] Specifically, the error corresponding to each initial installation angle can be calculated using the following formula:

[0105]

[0106] in, The rotation matrix is ​​obtained by substituting each initial installation angle into the formula for calculating the rotation matrix C described above. Let b be the velocity of system b at time i. Let v be the velocity of the v system at the i-th time.

[0107] In this embodiment of the application, by determining the error based on the velocities of the two coordinate systems at multiple times, and then determining the first initial heading angle, the influence of random factors can be eliminated and the accuracy of determining the first initial heading angle can be improved.

[0108] Figure 7 A flowchart illustrating coordinate transformations of the first initial roll angle, first initial pitch angle, and first initial heading angle according to an embodiment of this application is shown. Please refer to... Figure 7 As shown, the specific steps may include:

[0109] In step 710, the first rotation matrix of the inertial measurement unit coordinate system relative to the mechanism coordinate system is determined based on the first initial roll angle, the first initial pitch angle, and the first initial heading angle.

[0110] Specifically, the determined first initial heading angle First initial roll angle and the first initial pitch angle Substituting into the formula for calculating the rotation matrix C above, we can obtain the first rotation matrix of frame b relative to frame v.

[0111] In step 720, the transpose of the first rotation matrix is ​​determined to obtain the second rotation matrix.

[0112] pass This formula calculates the second rotation matrix of the v-frame relative to the b-frame.

[0113] In step 730, the second initial roll angle, the second initial pitch angle, and the second initial heading angle of the mechanism coordinate system relative to the inertial measurement unit coordinate system are determined based on the second rotation matrix.

[0114] Based on the second rotation matrix Based on the formulas for calculating φ, θ, and ψ mentioned above, the second initial roll angle of the v-frame relative to the b-frame can be calculated. Second initial pitch angle Second initial heading angle

[0115] Please continue reading Figure 2As shown, in step 230, an error function is established based on the difference between the velocity of the mechanism coordinate system and the velocity of the inertial measurement unit coordinate system, and the minimum value of the error function is solved iteratively to obtain the second target pitch angle and the second target heading angle of the mechanism coordinate system relative to the inertial measurement unit coordinate system.

[0116] The second initial pitch angle and the second initial heading angle are used to initially convert the velocity of the mechanism coordinate system to the inertial measurement unit coordinate system.

[0117] An error function can be established based on the velocities in the b-frame and v-frame at multiple moments. Specifically, the error function can be expressed by the following formula:

[0118]

[0119] in, This is the second rotation matrix used to convert the velocity of the v-frame to the b-frame. Corresponding to each second pitch angle and second heading angle, It can be calculated using the formula for the rotation matrix C mentioned above. Let b be the velocity of system b at time i. Let v be the velocity of the v system at the i-th time.

[0120] For the velocities of the b-frame and v-frame at a certain moment, the error function can be expanded into the following form according to the formula for calculating the rotation matrix C:

[0121]

[0122] Among them, e x Let e ​​be the component of the error function in the x-axis direction. y Let e ​​be the component of the error function in the y-axis direction. z Let v be the component of the error function in the z-axis direction. vx Let v be the component of the velocity of the v system along the x-axis. bx Let v be the x-component of the velocity of system b along the x-axis. by Let v be the y-component of the velocity of system b. bz Let be the z-component of the velocity of system b.

[0123] From the expansion of the above error function, it can be seen that the error function is related to the second initial roll angle. Irrelevant. This is because of v v The value only exists on the X-axis, therefore the installation angle estimation process... It is not observable.

[0124] The process of solving the error function iteratively is actually a process of minimizing the error step by step, with the variable to be optimized being...

[0125] Figure 8 A flowchart illustrating the iterative solution for minimizing an error function according to an embodiment of this application is shown. See also... Figure 8 As shown, the specific steps may include:

[0126] In step 810, the second initial pitch angle is used as the second current pitch angle, and the second initial heading angle is used as the second current heading angle.

[0127] Assign the second initial pitch angle and the second initial heading angle to the variables to be optimized.

[0128] In step 820, the Jacobian matrix and the error value of the error function on each axis are determined based on the second current pitch angle and the second current heading angle, respectively.

[0129] Specifically, the error values ​​of the error function on each axis can be represented by the expansion form shown above, and the Jacobian matrix can be obtained by solving as follows:

[0130]

[0131] Where J is the Jacobian matrix, v vx Let v be the component of the velocity of the v-system along the x-axis. This is the second current pitch angle. f is the second current heading angle. x f y f z The value of can only be 1 or -1, and its calculation method is as follows:

[0132]

[0133] Where J is the Jacobian matrix, v vx Let v be the component of the velocity of the v system along the x-axis. bx Let v be the x-component of the velocity of system b along the x-axis. by Let v be the y-component of the velocity of system b. bz Let be the z-component of the velocity of system b.

[0134] In step 830, the increments of the second current pitch angle and the second current heading angle are obtained by solving the error values ​​of the Jacobian matrix and the error function on each axis.

[0135] The Jacobian matrix mentioned above is the Jacobian matrix corresponding to a certain time step. By concatenating the Jacobian matrices of all time steps row-wise, a larger Jacobian matrix is ​​formed. In each iteration, it is obtained by solving the following formula. Increment:

[0136]

[0137] Where J is the Jacobian matrix, e x Let e ​​be the error value of the error function in the x-axis direction. y Let e ​​be the error value of the error function in the y-axis direction. z Let δx be the error value of the error function in the z-axis direction, and let δx be the increment.

[0138] In step 840, the second current pitch angle and the second current heading angle are updated based on the increment.

[0139] The following formula can be used to... Update:

[0140] x = x + δx,

[0141] Where δx is the increment, x on the right side of the equal sign is the second current pitch angle and the second current heading angle before the update, and x on the left side of the equal sign is the second current pitch angle and the second current heading angle after the update.

[0142] In step 850, is the predetermined convergence condition satisfied?

[0143] If the predetermined convergence condition is not met, then step 820 and the subsequent error function solution steps are iterated again based on the updated second current pitch angle and second current heading angle.

[0144] In one embodiment of this application, the predetermined convergence condition is that the number of iterations of the error function solution step reaches a predetermined number or the increment is less than a predetermined increment threshold.

[0145] When δx is less than a certain threshold or the number of iterations exceeds a certain threshold, the iteration is terminated, and x at this time is taken as the pitch angle and heading angle of the second target.

[0146] Although the above embodiments are based on solving for the minimum value of the error function using the Jacobian matrix, in other embodiments of this application, the minimum value of the error function can also be solved based on iterative optimization algorithms such as the Levenberg-Marquardt algorithm, the Doglg algorithm, and the gradient descent method.

[0147] In step 240, an installation angle between the inertial measurement unit coordinate system and the mechanism coordinate system is generated based on the second initial roll angle, the second target pitch angle, and the second target heading angle, so as to position the movable mechanism based on the installation angle.

[0148] The installation angle is the angle of difference between the inertial measurement unit coordinate system and the mechanism coordinate system, or between the mechanism coordinate system and the inertial measurement unit coordinate system.

[0149] In one embodiment of this application, generating the installation angle between the inertial measurement unit coordinate system and the mechanism coordinate system based on the second initial roll angle, the second target pitch angle, and the second target heading angle includes:

[0150] The second initial roll angle, the second target pitch angle, and the second target heading angle are combined to form the installation angle between the inertial measurement unit coordinate system and the mechanism coordinate system; or

[0151] The second initial roll angle, the second target pitch angle, and the second target heading angle are converted into the first target roll angle, the first target pitch angle, and the first target heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system. The first target roll angle, the first target pitch angle, and the first target heading angle are then combined into the installation angle between the inertial measurement unit coordinate system and the mechanism coordinate system.

[0152] By combining the second initial roll angle, the second target pitch angle, and the second target heading angle, the installation angle of the v-frame relative to the b-frame can be obtained; the installation angle of the b-frame relative to the v-frame can be obtained according to the above formula for calculating the rotation matrix C and the formulas for calculating φ, θ, and ψ.

[0153] In one embodiment of this application, the movable mechanism is a vehicle, and positioning the movable mechanism based on the installation angle includes: navigating and positioning the vehicle based on the installation angle, non-integrity constraints, vehicle speed information, and the output value of the inertial measurement unit.

[0154] After obtaining the installation angle, the vehicle can be navigated and positioned with higher precision based on the installation angle.

[0155] Figure 9 A schematic flowchart of an embodiment according to this application is shown. Below, in conjunction with… Figure 9 The embodiments of this application are further described below. Please refer to [link to relevant documentation]. Figure 9 As shown, firstly, the average value of the accelerometer over a period of time is obtained, and the initial value is calculated based on the average value. and Then, based on the triaxial velocities in the b-frame and the forward velocity in the v-frame, the initial velocity is calculated. Next, the second initial roll angle is obtained by transforming the Euler angles in coordinate system representation. Second initial pitch angle Second initial heading angle Then, fix the second initial roll angle. Iterative computation and The pitch angle and heading angle of the second target are obtained. During the iteration process, the three-axis velocity in the b-frame and the forward velocity in the v-frame are required. Finally, the installation angle is generated based on the pitch angle and heading angle of the second target and then output.

[0156] In summary, the positioning method for the movable mechanism provided according to the embodiments of this application can achieve at least the following technical effects: it can overcome the shortcomings of existing methods, is applicable to any installation angle, has strong versatility, has almost no requirements on the accuracy of the IMU, and can quickly, accurately and conveniently estimate the installation angle, thereby effectively improving the navigation and positioning performance of the vehicle.

[0157] The following describes an embodiment of the apparatus described in this application, which can be used to execute the positioning method of the movable mechanism in the above embodiments of this application. For details not disclosed in the apparatus embodiments of this application, please refer to the embodiments of the positioning method of the movable mechanism described above in this application.

[0158] Figure 10 A block diagram of a positioning device for a movable mechanism according to an embodiment of this application is shown.

[0159] Reference Figure 10As shown, a positioning device 1000 for a movable mechanism according to an embodiment of this application includes: an acquisition unit 1010, a determination and transformation unit 1020, an establishment and solution unit 1030, and a generation unit 1040. The acquisition unit 1010 is used to acquire the first initial roll angle and the first initial pitch angle of the inertial measurement unit's coordinate system relative to the mechanism coordinate system of the movable mechanism, wherein the inertial measurement unit is located on the movable mechanism; the determination and transformation unit 1020 is used to determine the first initial heading angle of the inertial measurement unit's coordinate system relative to the mechanism coordinate system, and to perform coordinate transformation on the first initial roll angle, the first initial pitch angle, and the first initial heading angle respectively to obtain the second initial roll angle, the second initial pitch angle, and the second initial heading angle of the mechanism coordinate system relative to the inertial measurement unit's coordinate system; the establishment and solution unit 1030 is used to determine the positioning angle of the mechanism coordinate system relative to the inertial measurement unit's coordinate system based on the velocity of the mechanism coordinate system. An error function is established based on the difference between the transformation result of the inertial measurement unit coordinate system and the velocity of the inertial measurement unit coordinate system. The minimum value of the error function is solved iteratively to obtain the second target pitch angle and the second target heading angle of the mechanism coordinate system relative to the inertial measurement unit coordinate system. The second initial pitch angle and the second initial heading angle are used to initially transform the velocity of the mechanism coordinate system to the inertial measurement unit coordinate system. The generation unit 1040 is used to generate the installation angle between the inertial measurement unit coordinate system and the mechanism coordinate system based on the second initial roll angle, the second target pitch angle and the second target heading angle, so as to position the movable mechanism based on the installation angle.

[0160] In some embodiments of this application, based on the aforementioned scheme, the determination and conversion unit 1020 is configured as follows: a plurality of first initial heading angles are set with a preset angle step size; each first initial heading angle is combined with the first initial roll angle and the first initial pitch angle to form an initial mounting angle; for each initial mounting angle, the velocity of the inertial measurement unit coordinate system is converted to the mechanism coordinate system according to the initial mounting angle, and the difference between the converted result and the velocity of the mechanism coordinate system is determined; based on the difference corresponding to each initial mounting angle, the error corresponding to each initial mounting angle is obtained, and the first initial heading angle among the initial mounting angles with the smallest corresponding error is taken as the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system.

[0161] In some embodiments of this application, based on the foregoing scheme, the determination and conversion unit 1020 is configured to: if the velocity of the inertial measurement unit coordinate system and the velocity of the mechanism coordinate system corresponding to the target number of times are obtained, then determine the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system.

[0162] In some embodiments of this application, based on the aforementioned scheme, the determination and conversion unit 1020 is configured to: for each initial installation angle, convert the velocity of the inertial measurement unit coordinate system to the mechanism coordinate system at each moment according to the initial installation angle, and determine the difference between the converted result at each moment and the velocity of the mechanism coordinate system at the same moment; and obtain the error corresponding to each initial installation angle based on the difference between each initial installation angle and the target number of moments respectively.

[0163] In some embodiments of this application, based on the aforementioned scheme, the establishment and solution unit 1030 is configured as follows: taking the second initial pitch angle as the second current pitch angle and the second initial heading angle as the second current heading angle; iteratively executing the error function solution step until a predetermined convergence condition is met, wherein the error function solution step includes: determining the Jacobian matrix and the error value of the error function on each axis according to the second current pitch angle and the second current heading angle respectively; solving for the increments of the second current pitch angle and the second current heading angle according to the Jacobian matrix and the error value of the error function on each axis; and updating the second current pitch angle and the second current heading angle based on the increments.

[0164] In some embodiments of this application, based on the foregoing scheme, the determination and conversion unit 1020 is configured to: determine a first rotation matrix of the inertial measurement unit coordinate system relative to the mechanism coordinate system based on the first initial roll angle, the first initial pitch angle and the first initial heading angle; determine the transpose matrix of the first rotation matrix to obtain a second rotation matrix; and determine the second initial roll angle, the second initial pitch angle and the second initial heading angle of the mechanism coordinate system relative to the inertial measurement unit coordinate system based on the second rotation matrix.

[0165] In some embodiments of this application, based on the foregoing scheme, the acquisition unit 1010 is configured to: determine the first initial roll angle and the first initial pitch angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system of the movable mechanism based on the output values ​​of the accelerometers in the inertial measurement unit on each axis.

[0166] In some embodiments of this application, based on the foregoing scheme, the acquisition unit 1010 is configured to: acquire multiple output values ​​of the accelerometer in the inertial measurement unit on each axis within a predetermined time period; determine the average value of the multiple output values ​​of the accelerometer on each axis; and determine the first initial roll angle and the first initial pitch angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system of the movable mechanism based on the average value of the multiple output values ​​on each axis.

[0167] In some embodiments of this application, based on the foregoing scheme, the generation unit 1040 is configured to: combine the second initial roll angle, the second target pitch angle, and the second target heading angle into an installation angle between the inertial measurement unit coordinate system and the mechanism coordinate system; or convert the second initial roll angle, the second target pitch angle, and the second target heading angle into a first target roll angle, a first target pitch angle, and a first target heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system, and combine the first target roll angle, the first target pitch angle, and the first target heading angle into an installation angle between the inertial measurement unit coordinate system and the mechanism coordinate system.

[0168] Figure 11 A schematic diagram of the structure of a computer system suitable for implementing the electronic device of the present application is shown.

[0169] It should be noted that, Figure 11 The computer system 1100 of the electronic device shown is merely an example and should not impose any limitation on the functionality and scope of use of the embodiments of this application.

[0170] like Figure 11 As shown, the computer system 1100 includes a Central Processing Unit (CPU) 1101, which can perform various appropriate actions and processes based on programs stored in Read-Only Memory (ROM) 1102 or programs loaded from storage portion 1108 into Random Access Memory (RAM) 1103, such as performing the methods described in the above embodiments. Various programs and data required for system operation are also stored in RAM 1103. The CPU 1101, ROM 1102, and RAM 1103 are interconnected via bus 1104. An Input / Output (I / O) interface 1105 is also connected to bus 1104.

[0171] The following components are connected to I / O interface 1105: an input section 1106 including a keyboard, mouse, etc.; an output section 1107 including a cathode ray tube (CRT), liquid crystal display (LCD), etc., and speakers, etc.; a storage section 1108 including a hard disk, etc.; and a communication section 1109 including a network interface card such as a LAN (Local Area Network) card, modem, etc. The communication section 1109 performs communication processing via a network such as the Internet. A drive 1110 is also connected to I / O interface 1105 as needed. Removable media 1111, such as a disk, optical disk, magneto-optical disk, semiconductor memory, etc., are installed on drive 1110 as needed so that computer programs read from them can be installed into storage section 1108 as needed.

[0172] Specifically, according to embodiments of this application, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments of this application include a computer program product comprising a computer program carried on a computer-readable medium, the computer program containing program code for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via communication section 1109, and / or installed from removable medium 1111. When the computer program is executed by central processing unit (CPU) 1101, it performs various functions defined in the system of this application.

[0173] It should be noted that the computer-readable medium shown in the embodiments of this application can be a computer-readable signal medium or a computer-readable storage medium, or any combination of the two. A computer-readable storage medium can be, for example,—but not limited to—an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of a computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM), flash memory, optical fiber, portable compact disc read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In this application, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device. In this application, a computer-readable signal medium can include a data signal propagated in baseband or as part of a carrier wave, carrying computer-readable program code. Such transmitted data signals can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. The computer-readable signal medium can also be any computer-readable medium other than a computer-readable storage medium, which can send, propagate, or transmit a program for use by or in connection with an instruction execution system, apparatus, or device. The program code contained on the computer-readable medium can be transmitted using any suitable medium, including but not limited to wireless, wired, etc., or any suitable combination thereof.

[0174] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of this application. Each block in a flowchart or block diagram may represent a module, segment, or portion of code, which contains one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in a block diagram or flowchart, and combinations of blocks in a block diagram or flowchart, can be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions.

[0175] The units described in the embodiments of this application can be implemented in software or hardware, and the described units can also be located in a processor. The names of these units do not necessarily limit the specific unit itself.

[0176] In one aspect, this application also provides a computer-readable medium, which may be included in the electronic device described in the above embodiments; or it may exist independently and not assembled into the electronic device. The computer-readable medium carries one or more programs, which, when executed by the electronic device, cause the electronic device to perform the methods described in the above embodiments.

[0177] It should be noted that although several modules or units for the device used to perform actions have been mentioned in the detailed description above, this division is not mandatory. In fact, according to the embodiments of this application, the features and functions of two or more modules or units described above can be embodied in one module or unit. Conversely, the features and functions of one module or unit described above can be further divided and embodied by multiple modules or units.

[0178] Through the above description of the embodiments, those skilled in the art will readily understand that the exemplary embodiments described herein can be implemented by software or by combining software with necessary hardware. Therefore, the technical solutions according to the embodiments of this application can be embodied in the form of a software product, which can be stored in a non-volatile storage medium (such as a CD-ROM, USB flash drive, external hard drive, etc.) or on a network, including several instructions to cause a computing device (such as a personal computer, server, touch terminal, or network device, etc.) to execute the method according to the embodiments of this application.

[0179] It is understood that in the specific implementation of this application, vehicle-related data is involved. When the above embodiments of this application are applied to specific products or technologies, user permission or consent is required, and the collection, use and processing of related data must comply with the relevant laws, regulations and standards of the relevant countries and regions.

[0180] Other embodiments of this application will readily occur to those skilled in the art upon consideration of the specification and practice of the embodiments disclosed herein. This application is intended to cover any variations, uses, or adaptations of this application that follow the general principles of this application and include common knowledge or customary techniques in the art not disclosed herein.

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

Claims

1. A positioning method for a movable mechanism, characterized in that, The method includes: The inertial measurement unit coordinate system is obtained relative to the mechanism coordinate system of the movable mechanism, with the inertial measurement unit located on the movable mechanism. A first initial heading angle is determined relative to the coordinate system of the inertial measurement unit (IMU) and the coordinate system of the mechanism is transformed to obtain a second initial roll angle, a second initial pitch angle, and a second initial heading angle relative to the coordinate system of the IMU. An error function is established based on the difference between the velocity of the mechanism coordinate system and the velocity of the inertial measurement unit coordinate system, and the minimum value of the error function is solved iteratively to obtain the second target pitch angle and the second target heading angle of the mechanism coordinate system relative to the inertial measurement unit coordinate system. The second initial pitch angle and the second initial heading angle are used to initially convert the velocity of the mechanism coordinate system to the inertial measurement unit coordinate system. The installation angle between the inertial measurement unit coordinate system and the mechanism coordinate system is generated based on the second initial roll angle, the second target pitch angle, and the second target heading angle, so as to position the movable mechanism based on the installation angle.

2. The positioning method for a movable mechanism according to claim 1, characterized in that, Determining the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system includes: Multiple first initial heading angles are set with preset angle steps, and each first initial heading angle is combined with the first initial roll angle and the first initial pitch angle to form an initial installation angle. For each initial installation angle, the velocity of the inertial measurement unit coordinate system is transformed to the mechanism coordinate system according to the initial installation angle, and the difference between the transformed result and the velocity of the mechanism coordinate system is determined. Based on the difference corresponding to each initial installation angle, the error corresponding to each initial installation angle is obtained, and the first initial heading angle of the initial installation angle with the smallest corresponding error is taken as the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system.

3. The positioning method for a movable mechanism according to claim 2, characterized in that, Determining the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system includes: If the velocities of the inertial measurement unit coordinate system and the mechanism coordinate system corresponding to the target number of times are obtained, then the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system is determined.

4. The positioning method for a movable mechanism according to claim 3, characterized in that, For each initial installation angle, the process of transforming the velocity of the inertial measurement unit coordinate system to the mechanism coordinate system based on the initial installation angle, and determining the difference between the transformed result and the velocity of the mechanism coordinate system, includes: For each initial installation angle, the velocity of the inertial measurement unit coordinate system at each moment is transformed to the mechanism coordinate system according to the initial installation angle, and the difference between the transformed result at each moment and the velocity of the mechanism coordinate system at the same moment is determined. The step of obtaining the error corresponding to each initial installation angle based on the difference corresponding to each initial installation angle includes: The error corresponding to each initial installation angle is obtained by calculating the difference between each initial installation angle and the target number of time points.

5. The positioning method for a movable mechanism according to claim 1, characterized in that, The step of iteratively finding the minimum value of the error function includes: The second initial pitch angle is used as the second current pitch angle, and the second initial heading angle is used as the second current heading angle; The error function solution process is iteratively executed until a predetermined convergence condition is met. The error function solution process includes: The Jacobian matrix and the error value of the error function on each axis are determined based on the second current pitch angle and the second current heading angle, respectively. The increments of the second current pitch angle and the second current heading angle are obtained by solving the error values ​​of the error function on each axis based on the Jacobian matrix and the error function. The second current pitch angle and the second current heading angle are updated based on the increment.

6. The positioning method for a movable mechanism according to claim 1, characterized in that, The step of performing coordinate transformations on the first initial roll angle, the first initial pitch angle, and the first initial heading angle to obtain the second initial roll angle, the second initial pitch angle, and the second initial heading angle of the mechanism coordinate system relative to the inertial measurement unit coordinate system includes: The first rotation matrix of the inertial measurement unit coordinate system relative to the mechanism coordinate system is determined based on the first initial roll angle, the first initial pitch angle, and the first initial heading angle. Determine the transpose of the first rotation matrix to obtain the second rotation matrix; Based on the second rotation matrix, the second initial roll angle, the second initial pitch angle, and the second initial heading angle of the mechanism coordinate system relative to the inertial measurement unit coordinate system are determined.

7. The positioning method for a movable mechanism according to claim 1, characterized in that, The acquisition of the first initial roll angle and the first initial pitch angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system of the movable mechanism includes: The first initial roll angle and the first initial pitch angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system of the movable mechanism are determined based on the output values ​​of the accelerometers in the inertial measurement unit on each axis.

8. The positioning method for a movable mechanism according to claim 7, characterized in that, The step of determining the first initial roll angle and the first initial pitch angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system of the movable mechanism based on the output values ​​of the accelerometers in the inertial measurement unit on each axis includes: Acquire multiple output values ​​of the accelerometer in the inertial measurement unit for each axis within a predetermined time period; Determine the average of multiple output values ​​of the accelerometer on each axis; The first initial roll angle and the first initial pitch angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system of the movable mechanism are determined based on the average of multiple output values ​​for each axis.

9. The positioning method for a movable mechanism according to claim 1, characterized in that, The step of generating the installation angle between the inertial measurement unit coordinate system and the mechanism coordinate system based on the second initial roll angle, the second target pitch angle, and the second target heading angle includes: The second initial roll angle, the second target pitch angle, and the second target heading angle are combined to form the installation angle between the inertial measurement unit coordinate system and the mechanism coordinate system; or The second initial roll angle, the second target pitch angle, and the second target heading angle are converted into the first target roll angle, the first target pitch angle, and the first target heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system, and the first target roll angle, the first target pitch angle, and the first target heading angle are combined to form the installation angle between the inertial measurement unit coordinate system and the mechanism coordinate system.

10. A positioning device for a movable mechanism, characterized in that, The device includes: An acquisition unit is used to acquire the first initial roll angle and the first initial pitch angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system of the movable mechanism, wherein the inertial measurement unit is located on the movable mechanism; The determination and transformation unit is used to determine the first initial heading angle of the inertial measurement unit coordinate system relative to the mechanism coordinate system, and to perform coordinate transformation on the first initial roll angle, the first initial pitch angle and the first initial heading angle respectively to obtain the second initial roll angle, the second initial pitch angle and the second initial heading angle of the mechanism coordinate system relative to the inertial measurement unit coordinate system. An establishment and solution unit is used to establish an error function based on the difference between the velocity of the mechanism coordinate system to the inertial measurement unit coordinate system and the velocity of the inertial measurement unit coordinate system, and to solve for the minimum value of the error function in an iterative manner to obtain the second target pitch angle and the second target heading angle of the mechanism coordinate system relative to the inertial measurement unit coordinate system, wherein the second initial pitch angle and the second initial heading angle are used to initially transform the velocity of the mechanism coordinate system to the inertial measurement unit coordinate system; The generation unit is used to generate an installation angle between the inertial measurement unit coordinate system and the mechanism coordinate system based on the second initial roll angle, the second target pitch angle and the second target heading angle, so as to position the movable mechanism based on the installation angle.

11. A computer-readable medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the positioning method of the movable mechanism as described in any one of claims 1 to 9.

12. An electronic device, characterized in that, include: One or more processors; A storage device for storing one or more programs, which, when executed by one or more processors, cause the one or more processors to implement the positioning method of the movable mechanism as described in any one of claims 1 to 9.

13. A computer program product, characterized in that, The computer program product includes computer instructions stored in a computer-readable storage medium, a processor of a computer device reading the computer instructions from the computer-readable storage medium, and the processor executing the computer instructions to cause the computer device to perform the positioning method of the movable mechanism as described in any one of claims 1 to 9.

Images