Pose detection method and device based on vision and inertial navigation fusion

Abstract

The invention relates to a pose detection method and device based on vision and inertial navigation fusion, and belongs to the field of pose detection. The method comprises the following steps: constructing a pose detection coordinate system; acquiring working area brightness, and emitting working laser in the working direction of the mechanical equipment according to the working area brightness; a laser point image of the working laser is collected through a camera, and a laser point light spot is obtained; pose detection is carried out through the laser spot light spot, the pose detection comprises visual pose detection and inertial navigation pose detection, high-precision, high-stability and high-efficiency pose detection is realized, and smooth progress of engineering is facilitated.

Description

Technical Field

[0001] The present invention belongs to the technical field of posture detection, and in particular relates to a posture detection method and device based on vision and inertial navigation fusion. Background Art

[0002] Domestic and foreign researchers have conducted a lot of research on posture measurement under complex working conditions, mainly including inertial navigation methods, ultra-wideband positioning methods, total station positioning methods, iGPS positioning methods, visual positioning methods, etc. However, these methods have problems such as increased errors, serious posture divergence, the need for multiple base stations, easy obstruction, and large limitations. In addition, they cannot be adapted to local conditions in dim environments or environments with high brightness, resulting in posture measurement results being greatly affected by environmental factors such as brightness. Summary of the invention

[0003] In order to solve the above problems existing in the prior art, the present invention provides a posture detection method and device based on vision and inertial navigation fusion.

[0004] The purpose of the present invention can be achieved through the following technical solutions:

[0005] A posture detection method based on vision and inertial navigation fusion, the implementation of the posture detection method comprises the following steps:

[0006] Construct a pose detection coordinate system;

[0007] Acquire the brightness of the working area, and emit a working laser in the working direction of the mechanical equipment according to the brightness of the working area;

[0008] Collecting the laser point image of the working laser through a camera and obtaining the laser point spot;

[0009] The laser spot light spot is used to perform posture detection, and the posture detection includes visual posture detection and inertial navigation posture detection.

[0010] Preferably, the constructing of the posture detection coordinate system includes:

[0011] The posture detection coordinate system includes an inertial navigation coordinate system, a camera coordinate system, a workspace coordinate system, and a geographic coordinate system. The inertial navigation coordinate system is constructed with the center of gravity of the mechanical equipment as the origin, the geographic east direction as the horizontal axis, the geographic north direction as the vertical axis, and the vertical upward direction as the vertical axis;

[0012] The camera coordinate system is established with the optical center of the camera as the origin, the right side of the camera as the horizontal axis, the bottom side of the camera as the vertical axis, and the front side of the camera as the vertical axis;

[0013] Divide the work area, establish the work area coordinate system with the center of the work area plane as the origin, the right side of the working direction of the mechanical equipment as the horizontal axis, the working direction of the mechanical equipment as the vertical axis, and the upward direction of the vertical work area plane as the vertical axis;

[0014] The geographic coordinate system is established with the center of the earth as the origin, the direction of the intersection of the prime meridian and the equator pointing to the center of the earth as the horizontal axis, the direction of the earth's rotation axis pointing to the North Pole as the vertical axis, and the upward direction of the plane perpendicular to the horizontal and vertical axes as the vertical axis.

[0015] Preferably, emitting working laser light in the working direction of the mechanical equipment according to the brightness of the working area includes:

[0016] Detecting the brightness of the working area and setting a brightness threshold according to the working conditions, the working laser includes a red working laser and a green working laser, and the number of the working lasers is 3;

[0017] When the brightness of the working area is greater than or equal to the brightness threshold, emitting the green working laser through a laser pointing instrument;

[0018] When the brightness of the working area is less than the brightness threshold, the red working laser is emitted through a laser pointer.

[0019] Preferably, the collecting the laser point image of the working laser by a camera and obtaining the laser point spot comprises:

[0020] Acquire an initial laser point image and detect the number of laser points through a camera, and acquire working laser validity information by judging the number of laser points, wherein the working laser validity information includes working laser valid information and working laser invalid information;

[0021] Determine the number of laser points, and when the number of laser points is 3, output the working laser valid information;

[0022] When the number of laser points is less than 3, the working laser invalid information is output and the angle of the laser emitting device is adjusted, and the cleaning facility is started to automatically clean the laser emitting device until the number of laser points is 3 and the working laser valid information is output;

[0023] When the working laser effective information is output, an effective laser point image is acquired, and the laser point image is acquired by processing the effective laser point image through distortion correction and Gaussian filtering;

[0024] Traversing all pixel information of the laser point image to obtain a light spot set that meets a preset hue range, saturation range, and brightness range;

[0025] The laser point spots are obtained by performing contour extraction and closing operation on the spots in the spot set. The laser point spots include a first laser point spot, a second laser point spot, and a third laser point spot.

[0026] Preferably, the posture detection by the laser spot light spot comprises:

[0027] Performing the visual posture detection by projection method to obtain the posture angle;

[0028] The inertial navigation attitude angle and mechanical equipment speed update equation are obtained through the inertial navigation attitude detection.

[0029] Preferably, the step of performing the visual posture detection by projection method to obtain the posture angle comprises:

[0030] Connect the laser spot to obtain a laser plane triangle, project the laser plane triangle to the camera coordinate system to obtain a laser camera triangle, wherein the three sides of the laser camera triangle are respectively a first camera triangle side, a second camera triangle side, and a third camera triangle side;

[0031] Respectively connect the origin of the camera coordinate system and the laser spot to obtain reference line segments and reference angles, wherein the reference line segments include a first reference line segment, a second reference line segment, and a third reference line segment, and the reference angles include a first reference angle, a second reference angle, and a third reference angle;

[0032] Obtain the laser point spot camera coordinates of the laser point spot in the camera coordinate system by solving the optimal solution of the cosine method equation, wherein the laser point spot camera coordinates include the first laser point spot camera coordinates, the second laser point spot camera coordinates, and the third laser point spot camera coordinates;

[0033] Acquire the laser spot working coordinates of the laser spot in the work area coordinate system by a total station, wherein the laser spot working coordinates include a first laser spot working coordinate, a second laser spot working coordinate, and a third laser spot working coordinate;

[0034] Constructing a posture conversion matrix according to the laser spot camera coordinates and the laser spot working coordinates;

[0035] The attitude angle is calculated according to the posture conversion matrix, and the attitude angle includes a yaw angle, a pitch angle, and a roll angle.

[0036] Preferably, the updating equation for acquiring the inertial navigation attitude angle and mechanical equipment speed through the inertial navigation attitude detection comprises:

[0037] Acquire the inertial navigation attitude angle by quaternion method, wherein the inertial navigation attitude angle includes an inertial navigation yaw angle, an inertial navigation pitch angle, and an inertial navigation roll angle;

[0038] Preset quaternions, the quaternions include the first quaternion, the second quaternion, the third quaternion, and the fourth quaternion, and construct an inertial navigation posture conversion matrix according to the quaternions. The expression of the inertial navigation posture conversion matrix is: Wherein, T is the inertial navigation posture conversion matrix, t1 is the first quaternion, t2 is the second quaternion, t3 is the third quaternion, and t4 is the fourth quaternion; the inertial navigation attitude angle is calculated according to the inertial navigation posture conversion matrix, and the calculation formula is: in, is the yaw angle of the inertial navigation, σ is the pitch angle of the inertial navigation, is the inertial navigation roll angle, T 12 is the first row and second column of the inertial navigation attitude conversion matrix, T 22 is the second row and second column of the inertial navigation attitude conversion matrix, T 32 The third row and second column of the inertial navigation attitude conversion matrix;

[0039] Obtaining the mechanical equipment speed update equation by projecting the mechanical equipment acceleration to the inertial navigation coordinate system;

[0040] Obtain the rotation angular velocity, which includes the geographic rotation angular velocity and the inertial navigation rotation angular velocity. Obtain the mechanical equipment speed update equation according to the rotation angular velocity. The expression of the mechanical equipment speed update equation is E=Tf-(2ω1+ω2)×V+g, wherein E is the mechanical equipment speed update equation, T is the inertial navigation posture conversion matrix, f is the mechanical equipment acceleration, ω1 is the geographic rotation angular velocity, ω2 is the inertial navigation rotation angular velocity, V is the mechanical equipment speed, and g is the gravity acceleration, which is 9.8.

[0041] A posture detection device based on vision and inertial navigation fusion, used to perform the posture detection method described above, characterized in that it includes a coordinate system construction module, a laser emission module, a light spot capture module, and a posture detection module;

[0042] The coordinate system construction module is used to construct a posture detection coordinate system;

[0043] The laser emission module is used to obtain the brightness of the working area and emit a working laser in the working direction of the mechanical equipment according to the brightness of the working area;

[0044] The light spot capturing module is used to collect the laser point image of the working laser through a camera and obtain the laser point light spot;

[0045] The posture detection module is used to perform posture detection through the laser spot, and the posture detection includes visual posture detection and inertial navigation posture detection.

[0046] An electronic device comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the above-mentioned posture detection method is implemented when the processor executes the program.

[0047] A storage medium containing computer executable instructions, which are used to perform the above-mentioned posture detection method when executed by a computer processor.

[0048] The beneficial effects of the present invention are:

[0049] (1) Through the fusion of vision and inertial navigation for posture detection, the problems of increased error, serious posture divergence, the need for cooperation of multiple base stations, easy obstruction, and large limitations that are prone to occur in single posture detection methods are overcome, which is conducive to the smooth implementation of the project;

[0050] (2) By emitting three beams of working laser in the working direction of the mechanical equipment according to the brightness of the working area, it is possible to avoid situations where the laser is difficult to be captured by the camera when the brightness is too high or the laser damages the human eye when the brightness is too low, thereby improving the efficiency of posture detection and protecting relevant personnel;

[0051] (3) When the number of laser points is less than 3, the working laser is invalid and the angle of the laser emitting device is adjusted. At the same time, the cleaning facility is started to automatically clean the laser emitting device until the number of laser points is 3 and the working laser is valid, so as to avoid the laser not being captured by the camera, resulting in a series of subsequent detection failures or large errors;

[0052] (4) The camera coordinates of the laser spot in the camera coordinate system are obtained by solving the optimal solution of the cosine method equation. The solution is simple and can significantly improve work efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] In order to facilitate understanding by those skilled in the art, the present invention is further described below with reference to the accompanying drawings.

[0054] Figure 1 The present invention is a flowchart of the steps of a posture detection method based on vision and inertial navigation fusion. DETAILED DESCRIPTION

[0055] In order to further explain the technical means and effects adopted by the present invention to achieve the predetermined invention purpose, the specific implementation methods, structures, features and effects of the present invention are described in detail below in conjunction with the accompanying drawings and preferred embodiments.

[0056] The working principle and use process of the present invention:

[0057] See also Figure 1 , a posture detection method based on vision and inertial navigation fusion, including:

[0058] S1: constructing a posture detection coordinate system, wherein the posture detection coordinate system includes an inertial navigation coordinate system, a camera coordinate system, a workspace coordinate system, and a geographic coordinate system;

[0059] S2: emitting three working laser beams in the working direction of the mechanical equipment according to the brightness of the working area, wherein the working laser beams include a green working laser beam and a red working laser beam;

[0060] S3: collecting the laser point image of the working laser and obtaining the laser point spot through a camera installed above the mechanical equipment, wherein the laser point spot includes a first laser point spot, a second laser point spot, and a third laser point spot;

[0061] S4: Performing posture detection through the laser spot, wherein the posture detection includes visual posture detection and inertial navigation posture detection.

[0062] In this embodiment, a posture detection coordinate system is constructed, and the posture detection coordinate system includes an inertial navigation coordinate system, a camera coordinate system, a workspace coordinate system, and a geographic coordinate system, which can be implemented by the following steps:

[0063] S101: constructing the inertial navigation coordinate system with the center of gravity of the mechanical equipment as the origin, the geographic east direction as the horizontal axis, the geographic north direction as the vertical axis, and the vertical upward direction as the vertical axis. The mechanical equipment includes but is not limited to excavators, bulldozers, etc.;

[0064] S102: Establishing the camera coordinate system with the optical center of the camera as the origin, the right side of the camera as the horizontal axis, the bottom side of the camera as the vertical axis, and the front side of the camera as the vertical axis;

[0065] S103: Divide the work area, establish the work area coordinate system with the center of the work area plane as the origin, the right side of the working direction of the mechanical equipment as the horizontal axis, the working direction of the mechanical equipment as the vertical axis, and the upward direction of the vertical work area plane as the vertical axis;

[0066] S104: Establish the geographic coordinate system with the center of the earth as the origin, the direction of the intersection of the prime meridian and the equator pointing to the center of the earth as the horizontal axis, the direction of the earth's rotation axis pointing to the North Pole as the vertical axis, and the upward direction of the plane perpendicular to the horizontal and vertical axes as the vertical axis.

[0067] In this embodiment, three working laser beams are emitted in the working direction of the mechanical equipment according to the brightness of the working area, and the working lasers include a green working laser and a red working laser. The specific implementation can be as follows:

[0068] The brightness of the working area is detected, and a brightness threshold is set according to the working conditions. When the brightness of the working area is greater than or equal to the brightness threshold, the green working laser is emitted through a laser pointer. When the brightness of the working area is less than the brightness threshold, the red working laser is emitted through a laser pointer. This avoids situations where the emitted laser is difficult to be captured by the camera when the brightness is too high or the laser hurts the human eye when the brightness is too low, thereby improving the efficiency of posture detection and protecting relevant personnel.

[0069] In this embodiment, a laser point image of the working laser is collected by a camera mounted above the mechanical equipment and a laser point spot is obtained. The laser point spot includes a first laser point spot, a second laser point spot, and a third laser point spot. The steps can be specifically implemented as follows:

[0070] S301: acquiring an initial laser point image through a camera and detecting the number of laser points, and acquiring working laser validity information by determining the number of laser points, wherein the working laser validity information includes working laser valid information and working laser invalid information;

[0071] Determine the number of laser points, and when the number of laser points is 3, output the working laser valid information;

[0072] When the number of laser points is less than 3, the invalid information of the working laser is output and the angle of the laser emitting device is adjusted. At the same time, the cleaning facility is started to automatically clean the laser emitting device until the number of laser points is 3 and the valid information of the working laser is output, so as to avoid a series of subsequent detection failures or excessive errors caused by the laser not being captured by the camera.

[0073] S302: when the working laser effective information is output, obtaining an effective laser point image, and obtaining the laser point image by processing the effective laser point image through distortion correction and Gaussian filtering;

[0074] S303: traverse all pixel information of the laser point image to obtain a light spot set that meets a preset hue range, saturation range, and brightness range;

[0075] S304: performing contour extraction and closing operation on the light spots in the light spot set to obtain the laser point light spots.

[0076] In this embodiment, the posture detection is performed by the laser spot, and the posture detection includes visual posture detection and inertial navigation posture detection, which can be implemented by the following steps:

[0077] S401: Performing the visual posture detection by projection method to obtain the posture angle;

[0078] S402: Obtaining the inertial navigation attitude angle and mechanical equipment speed update equation through the inertial navigation attitude detection.

[0079] In this embodiment, the visual posture detection is performed by a projection method to obtain the posture angle, which can be specifically implemented by the following steps:

[0080] S401-1: Connect the laser spot to obtain a laser plane triangle, project the laser plane triangle to the camera coordinate system to obtain a laser camera triangle, where three sides of the laser camera triangle are respectively a first camera triangle side, a second camera triangle side, and a third camera triangle side;

[0081] S401-2: respectively connecting the origin of the camera coordinate system and the laser spot to obtain reference line segments and reference angles, wherein the reference line segments include a first reference line segment, a second reference line segment, and a third reference line segment, and the reference angles include a first reference angle, a second reference angle, and a third reference angle, wherein the first reference angle is the angle between the first reference line segment and the second reference line segment, the second reference angle is the angle between the first reference line segment and the third reference line segment, and the third reference angle is the angle between the second reference line segment and the third reference line segment;

[0082] S401-3: Obtain the laser spot camera coordinates of the laser spot in the camera coordinate system by solving the optimal solution of the cosine method equation. The solution method is simple and can significantly improve work efficiency. The laser spot camera coordinates include the first laser spot camera coordinates, the second laser spot camera coordinates, and the third laser spot camera coordinates. The cosine method equation is Where a is the ratio of the square length of the second reference line segment to the square length of the first reference line segment, b is the ratio of the square length of the third reference line segment to the square length of the first reference line segment, α is the first reference angle, β is the second reference angle, γ is the third reference angle, and x i is the horizontal coordinate of the camera coordinate of the i-th laser spot, y i The ordinate of the camera coordinate of the i-th laser spot;

[0083] S401-4: obtaining, by a total station, the laser point spot working coordinates of the laser point spot in the work area coordinate system, wherein the laser point spot working coordinates include a first laser point spot working coordinate, a second laser point spot working coordinate, and a third laser point spot working coordinate;

[0084] S401-5: Construct a posture conversion matrix according to the laser spot camera coordinates and the laser spot working coordinates. The expression of the posture conversion matrix is: Wherein, R is the pose conversion matrix, (x1, y1, z1) is the camera coordinate of the first laser spot, (x2, y2, z2) is the camera coordinate of the second laser spot, (x3, y3, z3) is the camera coordinate of the third laser spot, (m1, m1, v1) is the working coordinate of the first laser spot, (m2, m2, v2) is the working coordinate of the second laser spot, (m3, m3, v3) is the working coordinate of the third laser spot;

[0085] S401-6: Calculate the attitude angle according to the posture conversion matrix, the attitude angle includes yaw angle, pitch angle, and roll angle, and the calculation formula is: Among them, η is the yaw angle, θ is the pitch angle, θ is the roll angle, R 12is the first row and second column of the pose transformation matrix, R 22 is the second row and second column of the pose transformation matrix, R 32 It is the third row and second column of the pose transformation matrix.

[0086] In this embodiment, the inertial navigation attitude angle and the mechanical equipment speed update equation are obtained through the inertial navigation attitude detection, which can be implemented by the following steps:

[0087] S402-1: Obtain the inertial navigation attitude angle by using a quaternion method, where the inertial navigation attitude angle includes an inertial navigation yaw angle, an inertial navigation pitch angle, and an inertial navigation roll angle;

[0088] Preset quaternions, the quaternions include the first quaternion, the second quaternion, the third quaternion, and the fourth quaternion, and construct an inertial navigation posture conversion matrix according to the quaternions. The expression of the inertial navigation posture conversion matrix is: Wherein, T is the inertial navigation posture conversion matrix, t1 is the first quaternion, t2 is the second quaternion, t3 is the third quaternion, and t4 is the fourth quaternion; the inertial navigation attitude angle is calculated according to the inertial navigation posture conversion matrix, and the calculation formula is: in, is the yaw angle of the inertial navigation, σ is the pitch angle of the inertial navigation, is the inertial navigation roll angle, T 12 is the first row and second column of the inertial navigation attitude conversion matrix, T 22 is the second row and second column of the inertial navigation attitude conversion matrix, T 32 The third row and second column of the inertial navigation attitude conversion matrix;

[0089] S402-2: Obtaining a speed update equation of the mechanical device by projecting the acceleration of the mechanical device to the inertial navigation coordinate system;

[0090] Obtain the rotation angular velocity, wherein the rotation angular velocity includes the geographic rotation angular velocity and the inertial navigation rotation angular velocity, wherein the geographic rotation angular velocity is the rotation angular velocity of the geographic coordinate system relative to the center of the earth, and the inertial navigation angular velocity is the rotation angular velocity of the inertial navigation coordinate system relative to the geographic coordinate system. Obtain the mechanical equipment speed update equation according to the rotation angular velocity, and the expression of the mechanical equipment speed update equation is E=Tf-(2ω1+ω2)×V+g, wherein E is the mechanical equipment speed update equation, T is the inertial navigation posture conversion matrix, f is the mechanical equipment acceleration, ω1 is the geographic rotation angular velocity, ω2 is the inertial navigation rotation angular velocity, V is the mechanical equipment speed, and g is the gravitational acceleration, which is 9.8.

[0091] A posture detection device based on vision and inertial navigation fusion comprises a coordinate system construction module, a laser emission module, a light spot capture module and a posture detection module.

[0092] The coordinate system construction module is used to construct a posture detection coordinate system, and the posture detection coordinate system includes an inertial navigation coordinate system, a camera coordinate system, a workspace coordinate system, and a geographic coordinate system;

[0093] The laser emission module is used to emit three working laser beams in the working direction of the mechanical equipment according to the brightness of the working area, and the working lasers include a green working laser and a red working laser;

[0094] The light spot capture module is used to collect the laser point image of the working laser and obtain the laser point light spot through a camera set up above the mechanical equipment, and the laser point light spot includes a first laser point light spot, a second laser point light spot, and a third laser point light spot;

[0095] The posture detection module is used to perform posture detection through the laser spot, and the posture detection includes visual posture detection and inertial navigation posture detection.

[0096] The computer storage medium of the embodiment of the present invention may adopt any combination of one or more computer-readable media. The computer-readable medium may be a computer-readable signal medium or a computer-readable storage medium. The computer-readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device or device, or any combination of the above. More specific examples (non-exhaustive list) of computer-readable storage media include: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In this document, a computer-readable storage medium may be any tangible medium containing or storing a program that may be used by or in combination with an instruction execution system, device or device.

[0097] Computer-readable signal media may include data signals propagated in baseband or as part of a carrier wave, which carry computer-readable program code. Such propagated data signals may take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. Computer-readable signal media may also be any computer-readable medium other than a computer-readable storage medium, which may send, propagate, or transmit a program for use by or in conjunction with an instruction execution system, apparatus, or device.

[0098] The program code included on the computer readable medium can be transmitted with any appropriate medium, including but not limited to wireless, electric wire, optical cable, RF, etc., or any suitable combination of the above. The computer program code for performing the operation of the present invention can be written in one or more programming languages ​​or their combinations, and the programming language includes object-oriented programming languages-such as Java, Smalltalk, C++, and also includes conventional procedural programming languages-such as "C" language or similar programming languages. The program code can be executed completely on the user's computer, partially on the user's computer, as an independent software package, partially on the user's computer and partially on the remote computer, or completely on the remote computer or server. In the case of a remote computer, the remote computer can be connected to the user's computer through any type of network, including a local area network (LAN) or a wide area network (WAN), or it can be connected to an external computer (for example, using an Internet service provider to connect through the Internet).

[0099] The above description is only a preferred embodiment of the present invention and does not limit the present invention in any form. Although the present invention has been disclosed as a preferred embodiment as above, it is not used to limit the present invention. Any technical personnel in this field can make some changes or modify the technical contents disclosed above into equivalent embodiments without departing from the scope of the technical solution of the present invention. However, any simple modification, equivalent change and modification made to the above embodiments according to the technical essence of the present invention without departing from the content of the technical solution of the present invention still fall within the scope of the technical solution of the present invention.

Claims

1. A posture detection method based on vision and inertial navigation fusion, characterized in that: The implementation of the posture detection method comprises the following steps: Construct a pose detection coordinate system; Acquire the brightness of the working area, and emit a working laser in the working direction of the mechanical equipment according to the brightness of the working area; Collecting the laser point image of the working laser through a camera and obtaining the laser point spot; The laser spot light spot is used to perform posture detection, and the posture detection includes visual posture detection and inertial navigation posture detection.

2. The posture detection method according to claim 1, characterized in that: The constructing of the posture detection coordinate system comprises: The posture detection coordinate system includes an inertial navigation coordinate system, a camera coordinate system, a workspace coordinate system, and a geographic coordinate system. The inertial navigation coordinate system is constructed with the center of gravity of the mechanical equipment as the origin, the geographic east direction as the horizontal axis, the geographic north direction as the vertical axis, and the vertical upward direction as the vertical axis; The camera coordinate system is established with the optical center of the camera as the origin, the right side of the camera as the horizontal axis, the bottom side of the camera as the vertical axis, and the front side of the camera as the vertical axis; Divide the work area, establish the work area coordinate system with the center of the work area plane as the origin, the right side of the working direction of the mechanical equipment as the horizontal axis, the working direction of the mechanical equipment as the vertical axis, and the upward direction of the vertical work area plane as the vertical axis; The geographic coordinate system is established with the center of the earth as the origin, the direction of the intersection of the prime meridian and the equator pointing to the center of the earth as the horizontal axis, the direction of the earth's rotation axis pointing to the North Pole as the vertical axis, and the upward direction of the plane perpendicular to the horizontal and vertical axes as the vertical axis.

3. The posture detection method according to claim 1, characterized in that: The method of emitting working laser light in the working direction of the mechanical equipment according to the brightness of the working area comprises: Detecting the brightness of the working area and setting a brightness threshold according to the working conditions, the working laser includes a red working laser and a green working laser, and the number of the working lasers is 3; When the brightness of the working area is greater than or equal to the brightness threshold, emitting the green working laser through a laser pointing instrument; When the brightness of the working area is less than the brightness threshold, the red working laser is emitted through a laser pointer.

4. The posture detection method according to claim 1, characterized in that: The step of collecting the laser point image of the working laser by using a camera and obtaining the laser point spot comprises: Acquire an initial laser point image and detect the number of laser points through a camera, and acquire working laser validity information by judging the number of laser points, wherein the working laser validity information includes working laser valid information and working laser invalid information; Determine the number of laser points, and when the number of laser points is 3, output the working laser valid information; When the number of laser points is less than 3, the working laser invalid information is output and the angle of the laser emitting device is adjusted, and the cleaning facility is started to automatically clean the laser emitting device until the number of laser points is 3 and the working laser valid information is output; When the working laser effective information is output, an effective laser point image is acquired, and the laser point image is acquired by processing the effective laser point image through distortion correction and Gaussian filtering; Traversing all pixel information of the laser point image to obtain a light spot set that meets a preset hue range, saturation range, and brightness range; The laser point spots are obtained by performing contour extraction and closing operation on the spots in the spot set. The laser point spots include a first laser point spot, a second laser point spot, and a third laser point spot.

5. The posture detection method according to claim 1, characterized in that: The posture detection by the laser spot light spot comprises: Performing the visual posture detection by projection method to obtain the posture angle; The inertial navigation attitude angle and mechanical equipment speed update equation are obtained through the inertial navigation attitude detection.

6. The posture detection method according to claim 5, characterized in that: The obtaining of the posture angle by performing the visual posture detection by projection method comprises: Connect the laser spot to obtain a laser plane triangle, project the laser plane triangle to the camera coordinate system to obtain a laser camera triangle, wherein the three sides of the laser camera triangle are respectively a first camera triangle side, a second camera triangle side, and a third camera triangle side; Respectively connect the origin of the camera coordinate system and the laser spot to obtain reference line segments and reference angles, wherein the reference line segments include a first reference line segment, a second reference line segment, and a third reference line segment, and the reference angles include a first reference angle, a second reference angle, and a third reference angle; Obtain the laser point spot camera coordinates of the laser point spot in the camera coordinate system by solving the optimal solution of the cosine method equation, wherein the laser point spot camera coordinates include the first laser point spot camera coordinates, the second laser point spot camera coordinates, and the third laser point spot camera coordinates; Acquire the laser spot working coordinates of the laser spot in the work area coordinate system by a total station, wherein the laser spot working coordinates include a first laser spot working coordinate, a second laser spot working coordinate, and a third laser spot working coordinate; Constructing a posture conversion matrix according to the laser spot camera coordinates and the laser spot working coordinates; The attitude angle is calculated according to the posture conversion matrix, and the attitude angle includes a yaw angle, a pitch angle, and a roll angle.

7. The posture detection method according to claim 5, characterized in that: The updating equation for obtaining the inertial navigation attitude angle and the mechanical equipment speed through the inertial navigation attitude detection includes: The inertial navigation attitude angle is obtained by using the quaternion method, wherein the inertial navigation attitude angle includes an inertial navigation yaw angle, an inertial navigation pitch angle, and an inertial navigation roll angle; Preset quaternions, the quaternions include the first quaternion, the second quaternion, the third quaternion, and the fourth quaternion, and construct an inertial navigation posture conversion matrix according to the quaternions. The expression of the inertial navigation posture conversion matrix is: Wherein, T is the inertial navigation posture conversion matrix, t1 is the first quaternion, t2 is the second quaternion, t3 is the third quaternion, and t4 is the fourth quaternion; the inertial navigation attitude angle is calculated according to the inertial navigation posture conversion matrix, and the calculation formula is: in, is the yaw angle of the inertial navigation, σ is the pitch angle of the inertial navigation, is the inertial navigation roll angle, T 12 is the first row and second column of the inertial navigation attitude conversion matrix, T 22 is the second row and second column of the inertial navigation attitude conversion matrix, T 32 The third row and second column of the inertial navigation attitude conversion matrix; Obtaining the mechanical equipment speed update equation by projecting the mechanical equipment acceleration to the inertial navigation coordinate system; Obtain the rotational angular velocity, the rotational angular velocity includes the geographic rotational angular velocity and the inertial navigation rotational angular velocity, and obtain the mechanical equipment speed update equation according to the rotational angular velocity. The expression of the mechanical equipment speed update equation is E=Tf-(2ω1+ω 2) ×V+g, where E is the mechanical equipment speed update equation, T is the inertial navigation posture conversion matrix, f is the mechanical equipment acceleration, ω1 is the geographic rotation angular velocity, ω2 is the inertial navigation rotation angular velocity, V is the mechanical equipment speed, and g is the gravitational acceleration, which is taken as 9.

8.

8. A posture detection device based on vision and inertial navigation fusion using the posture detection method based on vision and inertial navigation fusion according to claim 1, characterized in that: It includes coordinate system construction module, laser emission module, light spot capture module and posture detection module; The coordinate system construction module is used to construct a posture detection coordinate system; The laser emission module is used to obtain the brightness of the working area and emit a working laser in the working direction of the mechanical equipment according to the brightness of the working area; The light spot capturing module is used to collect the laser point image of the working laser through a camera and obtain the laser point light spot; The posture detection module is used to perform posture detection through the laser spot, and the posture detection includes visual posture detection and inertial navigation posture detection.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, the posture detection method as described in any one of claims 1-7 is implemented.

10. A storage medium containing computer executable instructions, characterized in that: The computer executable instructions are used to execute the posture detection method as described in any one of claims 1-7 when executed by a computer processor.

Images