Method and device for unifying coordinate system of wharf ship loader

Abstract

The invention discloses a method and device for unifying a coordinate system of a wharf ship loader, and the method comprises the steps: firstly collecting point cloud data generated by a radar sensor of the ship loader, then collecting data of a positioning encoder of the ship loader and related mechanical size information, and then defining a radar coordinate system for the point cloud data, the method comprises the following steps: acquiring point cloud data, converting the point cloud data from a radar coordinate system to other coordinate systems to adapt to subsequent processing requirements, splicing a plurality of radar point cloud data sets together according to a coordinate conversion result to form a complete point cloud data set, and finally, combining positioning encoder data, mechanical size information and the spliced point cloud data to obtain a complete point cloud data set. A unified coordinate system is established, and it is ensured that all data are subjected to follow-up processing under the same reference system. According to the scheme, the position data of the sensors are fused together, the point cloud coordinate system scanned by the laser radar system is converted and calculated, and the spatial position of each point cloud is unified to the cart coordinate system, so that the full-scene three-dimensional modeling of the ship loader is realized, and the position positioning under the unified coordinate system is realized.

Description

Technical Field

[0001] The invention relates to the field of coordinate conversion of a dock ship loader, and in particular to a method and a device for unifying coordinate systems of a dock ship loader. Background Art

[0002] Ship loader is a large bulk material machine used for loading bulk material at bulk material docks. Generally, ship loader consists of arm belt conveyor, transition belt conveyor, telescopic chute, tail car, running device, gantry, tower, pitch device, slewing device, etc.

[0003] Currently, ship loaders load cargo by manually moving carts, chutes and arms on the operating table. This method relies heavily on the operator's experience and attention. In order to achieve automatic loading, it is necessary to simulate the operator's loading process through the position data of multiple sensors.

[0004] At present, the ship loader positioning system includes three laser radars, boom rotation angle, movable boom pitch angle, boom telescopic distance and chute height. Through high-precision multi-turn absolute encoders, inclinometers and other positioning devices, the real-time three-dimensional coordinates and spatial posture of the ship loader equipment are accurately obtained. The three-dimensional position information detected by the positioning device is synchronized to the system server and meets the requirements for real-time PLC control and safe position judgment. However, these sensor data do not interact with each other, and the loading work cannot be completed using only a single sensor. The single data does not belong to the same coordinate system, which makes it impossible to compare and judge the position between the data, and cannot provide accurate and reliable coordinate position information for automated control. Summary of the invention

[0005] The purpose of the present invention is to overcome the shortcomings of the prior art and provide a method and device for a unified coordinate system for a dock loader, which fuses the position data of the sensors together, converts and calculates the point cloud coordinate system scanned by the lidar system, and then unifies the spatial position of each point cloud into a large vehicle coordinate system, thereby realizing full-scene three-dimensional modeling of the ship loader and achieving position positioning under a unified coordinate system.

[0006] In order to achieve the above object, the technical solution adopted by the present invention is: a method for unifying the coordinate system of a dock loader, comprising the following steps:

[0007] S1. Use multiple radar sensors to obtain multiple point cloud data of the ship loader;

[0008] S2. Establish a radar coordinate system for each point cloud data and define an intermediate coordinate system;

[0009] S3, converting each point cloud data from the corresponding radar coordinate system to the intermediate coordinate system and splicing multiple point cloud data to form complete point cloud data of the ship loader;

[0010] S4. Establish a unified coordinate system, integrate the spliced ​​point cloud data of the complete ship loader with the real size data and position data to establish a unified coordinate system, and convert the point cloud data of the complete ship loader into point cloud data under the unified coordinate system.

[0011] Three radar sensors are used to obtain radar point cloud data of the head, tail and midship of the ship loader.

[0012] In step S2, the establishment of the radar coordinate system of the point cloud data acquired by each radar sensor includes: taking each radar installation position as the origin of the radar coordinate system, that is, the starting point of the radar coordinate system, defining the direction of the coordinate axis, and defining the direction of the coordinate axis according to the right-hand rule. The positive direction of the X, Y, and Z axes is determined by the right-hand rule. According to the right-hand rule, the entire ship loader selects the stern as the positive direction of the X axis, the right side of the ship perpendicular to the outside as the positive direction of the Y axis, and the downward direction of the ship perpendicular to the ship as the positive direction of the Z axis.

[0013] The intermediate coordinate system uses the radar coordinate system corresponding to one of the radar sensors.

[0014] In step S3, the radar coordinate system corresponding to one of the radar sensors is selected as the intermediate coordinate system, that is, the point cloud image scanned by one of the radar sensors is used as a reference point, and the point cloud images scanned by other radar sensors are rotated and translated to obtain a complete point cloud image of the ship loader, and then the complete point cloud image of the ship loader is rotated and translated as a whole to obtain a point cloud image coordinate system with the stern pointing to the positive direction of the X axis, the right side of the ship pointing outward to the positive direction of the Y axis, the downward direction of the ship pointing vertically to the positive direction of the Z axis, and the XOY plane of the radar coordinate system parallel to the shore.

[0015] When rotating and translating the point cloud, the rotation operation includes using the X-axis rotation matrix R x , Y-axis rotation matrix R y , Z-axis rotation matrix R z The coordinates of each point in the point cloud are rotated, and after the rotation transformation, the translation operation is performed through the translation matrix T.

[0016] The coordinates of each point in the complete point cloud image of the ship loader in the radar coordinate system are converted into coordinates in the unified coordinate system through the coordinate conversion matrix S, and then the point cloud image data in the unified coordinate system is obtained, completing the establishment of the unified coordinate system of the terminal ship loader.

[0017] The coordinate transformation matrix S is:

[0018]

[0019] in

[0020]

[0021]

[0022] l′ adz is the real-time distance from the trolley to the center of the chute and β is the angle between the trolley arm and the vertical shore direction; the initial length of the chute is s0, and the distance from the radar to the center of the chute is set to l z , the distance from the lifting point of the trolley arm to the center of the chute is l cd And the trolley arm lifting angle θ.

[0023] The point cloud in the radar coordinate system is rotated around the three coordinate axes of X, Y, and Z respectively;

[0024] Rotate the result around the X axis using a 3×3 rotation matrix R x It is expressed as:

[0025]

[0026] Rotate the result around the Y axis using a 3×3 matrix R y It is expressed as:

[0027]

[0028] Rotate the result around the Z axis using a 3×3 matrix R z It is expressed as:

[0029]

[0030] A device for unifying the coordinate system of a dock ship loader, the device comprising a data acquisition module, a radar coordinate system establishment module, a point cloud coordinate transformation and splicing module, and a unified coordinate system module; wherein the data acquisition module is used to collect multiple point cloud data of the ship loader obtained by multiple radar sensors;

[0031] The radar coordinate system establishment module establishes a radar coordinate system for each point cloud data and defines an intermediate coordinate system;

[0032] The point cloud coordinate transformation and splicing module converts each point cloud data from the corresponding radar coordinate system to the intermediate coordinate system and splices multiple point cloud data to form complete point cloud data of the ship loader;

[0033] The unified coordinate system module is used to propose a unified coordinate system, fuse the spliced ​​complete point cloud data of the ship loader with the real size data and position data to establish a unified coordinate system, and convert the complete point cloud data of the ship loader into point cloud data under the unified coordinate system.

[0034] The advantages of the present invention are: 1. By establishing a unified coordinate system, the position of an object in space can be expressed as a set of numerical values, which makes it easier to compare the positions of other objects;

[0035] 2. By establishing a unified coordinate system, the ship loader can be easily operated and controlled. The operator can quickly adjust the position and posture of the ship loader according to the data in the coordinate system to improve the accuracy and efficiency of the operation;

[0036] 3. Through the unified coordinate system, accurate positioning information can be provided to ensure that the loader can accurately determine the position of the cargo on the ship when loading the cargo. This helps to reduce loading errors, improve loading efficiency, and ensure the safe transportation of cargo;

[0037] 4. Through the unified coordinate system, accurate positioning and monitoring functions can be provided, which helps to ensure the safety of the ship loader during operation. Operators can promptly discover potential safety hazards based on the data in the coordinate system and take corresponding measures to prevent and deal with them. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] The following is a brief description of the contents expressed in the drawings of the present invention and the symbols in the drawings:

[0039] Figure 1 It is a schematic diagram of the composition of the device for the unified coordinate system of the dock ship loader of the present invention;

[0040] Figure 2 A flow chart of the method for unifying the coordinate system of the present invention; DETAILED DESCRIPTION

[0041] The specific implementation of the present invention will be further explained in detail below by describing the optimal embodiment with reference to the accompanying drawings.

[0042] This solution provides a method for unifying the coordinate system of the dock loader. Under this method, the coordinates of the loader are unified. It is recommended to use a unified coordinate system to model the coordinates of each component on the loader, and to represent the position of an object in space as a set of values, which is more convenient for comparison with the positions of other objects. Of course, it can also provide unified position information for subsequent automatic control and manual operation. The method includes the following steps:

[0043] S1. Use multiple radar sensors to obtain multiple point cloud data of the ship loader;

[0044] S2. Establish a radar coordinate system for each point cloud data and define an intermediate coordinate system;

[0045] S3, converting each point cloud data from the corresponding radar coordinate system to the intermediate coordinate system and splicing multiple point cloud data to form complete point cloud data of the ship loader;

[0046] S4. Establish a unified coordinate system, integrate the spliced ​​point cloud data of the complete ship loader with the real size data and position data to establish a unified coordinate system, and convert the point cloud data of the complete ship loader into point cloud data under the unified coordinate system.

[0047] In this solution, multiple radar sensors are used to collect point cloud data of the ship loader. Preferably, three radar sensors are used to obtain radar point cloud data of the head, tail and midship of the ship loader respectively. The three radar sensors can cover the entire ship loader.

[0048] In step S2, the establishment of the radar coordinate system of the point cloud data acquired by each radar sensor includes: taking each radar installation position as the origin of the radar coordinate system, that is, the starting point of the radar coordinate system, defining the direction of the coordinate axis, and defining the direction of the coordinate axis according to the right-hand rule, and using the right-hand rule to determine the positive direction of the X, Y, and Z axes. According to the right-hand rule, the entire ship loader selects the stern as the positive direction of the X axis, the right side of the ship is perpendicular to the outside as the positive direction of the Y axis, and the downward direction of the ship is perpendicular to the positive direction of the Z axis. Since three radar sensors are set, there are three radar coordinate systems corresponding to each radar sensor and its point cloud.

[0049] In step S3, the radar coordinate system corresponding to one of the radar sensors is selected as the intermediate coordinate system, that is, the point cloud image scanned by one of the radar sensors is used as a reference point, and the point cloud images scanned by other radar sensors are rotated and translated to obtain a complete point cloud image of the ship loader, and then the complete point cloud image of the ship loader is rotated and translated as a whole to obtain a point cloud image coordinate system with the stern pointing to the positive direction of the X axis, the right side of the ship pointing outward to the positive direction of the Y axis, the downward direction of the ship pointing vertically to the positive direction of the Z axis, and the XOY plane of the radar coordinate system parallel to the shore.

[0050] When rotating and translating the point cloud, the rotation operation includes using the X-axis rotation matrix R x , Y-axis rotation matrix R y , Z-axis rotation matrix R z The coordinates of each point in the point cloud are rotated, and after the rotation transformation, the translation operation is performed through the translation matrix T.

[0051] The coordinates of each point in the complete point cloud image of the ship loader in the radar coordinate system are converted into coordinates in the unified coordinate system through the coordinate conversion matrix S, and then the point cloud image data in the unified coordinate system is obtained, completing the establishment of the unified coordinate system of the terminal ship loader.

[0052] The position data of the sensors are fused together, and the point cloud coordinate system scanned by the lidar system is converted and calculated. The spatial position of each point cloud is then unified to the truck coordinate system to achieve full-scene three-dimensional modeling of the ship loader and position positioning under a unified coordinate system.

[0053] A unified coordinate system method for a dock ship loader, the method comprising the following contents: firstly, collecting point cloud data generated by a radar sensor of the ship loader, then collecting data of a positioning encoder of the ship loader and related mechanical size information, secondly defining a radar coordinate system for the point cloud data, and converting the point cloud data from the radar coordinate system to other coordinate systems to meet subsequent processing requirements, splicing multiple radar point cloud data sets together according to the result of the coordinate transformation to form a complete point cloud data set, and finally establishing a unified coordinate system by combining the positioning encoder data, mechanical size information and the spliced ​​point cloud data to ensure that all data are subsequently processed in the same reference system.

[0054] like Figure 1 As shown, this solution mainly consists of the following four modules, namely, data acquisition module, radar coordinate system establishment module, point cloud coordinate transformation and splicing module, and unified coordinate system module.

[0055] 1. Data acquisition module

[0056] A set of 3D laser scanning devices are respectively installed at the left, right and rear positions of the maintenance platform on the upper part of the chute. The device has a large scanning angle and a high scanning frequency. It can scan the entire hull and the chute, and can obtain point cloud data of the bow position, cabin position and stern position. Two sets of Beidou satellite positioning mobile stations are installed on the loader boom, and a set of Beidou satellite positioning base station is installed in an unobstructed position on the roof. The RTK real-time differential positioning technology is used to accurately locate the rotation angle of the boom and the telescopic distance of the movable boom; a set of pitch boom lifting detection device has been configured on the movable pitch boom lifting drive drum mechanism. The device has a built-in absolute value multi-turn encoder. The encoder supports DP network. The detection data is directly transmitted to the PLC controller of the ship loader to accurately locate the pitch angle on the pitch boom; a trolley position detection device has been configured on the right side walking driven wheel of the trolley. It has a built-in absolute multi-turn encoder, which supports DP network. The detection data is directly transmitted to the ship loader's built-in PLC controller to accurately locate the trolley's travel position. On the right side of the movable boom's driven wheel, a boom telescopic distance detection device has been configured, which has a built-in absolute multi-turn encoder, which supports DP network. The detection data is directly transmitted to the ship loader's built-in PLC controller to accurately locate the boom's telescopic distance. The system automatically reads an absolute encoder that has been configured on the chute lifting and driving drum mechanism to accurately locate the chute's lifting and lowering distance. In addition, the mechanical dimensions of the ship loader are obtained through manual measurement.

[0057] 2. Establish radar coordinate system module

[0058] The entire ship loader is scanned simultaneously by three radars, each installed in a different position but fixed relative to the chute.

[0059] The three radars are numbered. Radar No. 1 scans the middle of the ship, radar No. 2 scans the stern, and radar No. 3 scans the bow. The three radars can scan the entire ship. Determine the radar positions, and use the positions where the three radars are installed as the origin of the coordinate system, which is the starting point of the radar coordinate system. Define the direction of the coordinate axis. Define the direction of the coordinate axis according to the right-hand rule. Use the right-hand rule to determine the positive direction of the X, Y, and Z axes. According to the right-hand rule, the entire ship loader chooses to point to the stern as the positive direction of the X axis, the right side of the ship perpendicular to the outside as the positive direction of the Y axis, and the vertical ship downward as the positive direction of the Z axis.

[0060] According to the above definitions, the coordinate systems of radar No. 1, radar No. 2 and radar No. 3 are established respectively.

[0061] 3. Point cloud coordinate transformation and splicing module

[0062] Since the three radars are located in different positions, it is necessary to unify the three different radar coordinate systems. Taking the point cloud image scanned by radar No. 1 as the reference point, the point cloud images scanned by radar No. 2 and radar No. 3 are rotated and translated to obtain a complete point cloud image of the ship loader. Finally, the whole image is rotated and translated to obtain the positive direction of the X-axis pointing to the stern, the positive direction of the Y-axis perpendicular to the right side of the ship, and the positive direction of the Z-axis perpendicular to the ship downward. The XOY plane of the radar coordinate system is parallel to the shore.

[0063] Since the acquired point cloud data is relative to the laser radar coordinate system, in order to unify the coordinates of each radar into the coordinate system of radar No. 1, it is necessary to perform coordinate transformation on the coordinates of each point in the point cloud.

[0064] Coordinate transformation includes rotation transformation and translation transformation. First, the radar coordinate system is rotated around the three coordinate axes of X, Y, and Z respectively.

[0065] The result of rotating around the X axis can be expressed as a 3×3 rotation matrix R x It means:

[0066]

[0067] The result of rotating around the Y axis can be expressed as a 3×3 matrix R y It means:

[0068]

[0069] The result of rotating around the Z axis can be expressed as a 3×3 matrix R z It means:

[0070]

[0071] After the radar is rotated, the radar coordinate system is translated. Given the three-dimensional coordinates (x, y, z) of the laser radar, the translation matrix T of the laser radar can be expressed as:

[0072]

[0073] R x , R y , R z Represents the rotation matrix obtained by rotating the point cloud coordinates around the X-axis, Y-axis, and Z-axis in the coordinate system, and the coordinate vector of each point in the coordinate point cloud in the initial radar coordinate system (where x, y, and z are the coordinates of a point in the radar coordinate system), then rotate around the X axis The point cloud obtained by rotating μ degrees around the Y axis and ∈ degrees around the Z axis is:

[0074] Then, after point cloud translation, we get the complete point cloud of the ship loader with the stern pointing to the positive direction of the X axis, the right side of the ship pointing outward to the positive direction of the Y axis, the vertical ship downward to the positive direction of the Z axis, and the XOY plane of the radar coordinate system parallel to the shore. This point cloud is the point cloud in the radar coordinate system, that is:

[0075]

[0076] Degree, μ degree, ∈ degree are the angles of rotation of the point cloud around the X, Y, and Z axes respectively. The x, y, and z values ​​in T represent the direction (positive and negative) and length (absolute value) of the translation of the point cloud data along the x, y, and z axes. V″ is the complete point cloud of the ship loader in the radar coordinate system after rotation and translation.

[0077] 4. Unified coordinate system module

[0078] After point cloud coordinate transformation and point cloud data splicing in the point cloud coordinate transformation and splicing module, a complete point cloud map of the ship loader is obtained, which is spliced ​​by three different radars and based on radar No. 1. Next, the point cloud data in the radar coordinate system needs to be unified to the truck coordinate system. The following conditions need to be met to establish a unified coordinate system for the truck:

[0079] 1) Simultaneity: When acquiring location data from multiple sensors, it is necessary to ensure that the data from each sensor is acquired at the same time;

[0080] 2) Real-time: Since the loading process is a dynamic process, data needs to be obtained in a short time;

[0081] 3) Accuracy: The unified coordinate system is established by fusing multiple sensors, so the data accuracy of each sensor directly affects the accuracy of the unified coordinate system;

[0082] 4) Stability: The unified coordinate system should be stable and able to maintain its accuracy and stability over a long period of time. This requires ensuring that the reference points and measuring equipment of the coordinate system will not be affected by the external environment;

[0083] 5) Reliability: The unified coordinate system should be reliable and able to work properly under various working conditions. This requires the selection of reliable measurement equipment and technology, and sufficient testing and verification.

[0084] By satisfying the above conditions, a unified coordinate system for the trolley is established, and the data of each encoder is obtained by the Snap7 communication protocol. The telescopic length of the trolley arm can be set as l′ bc , the elevation angle of the trolley arm is θ, and the horizontal movement angle of the radar is Combined with the mechanical dimensions of the ship loader in its initial state, the trolley height can be set to h, and the distance from the trolley to the starting point of the trolley arm extension can be set to l. ab The distance from the telescopic starting point of the trolley arm to the lifting point of the trolley arm is l bc The distance from the lifting point of the trolley arm to the center of the chute is l cd , the distance from the trolley to the trolley arm lifting point is l ac , the distance from the cart to the center of the chute is l ad , the vertical distance from the radar to the center of the slide is set to l x , the horizontal distance from the radar to the center of the chute is set to l y , the distance from the radar to the center vertex of the slide is set to l z , the angle between the trolley arm and the radar is α, and the length of the chute in the initial state is s0. The mechanical dimensions of the ship loader in the non-initial state can be set as the real-time distance from the trolley to the trolley arm elevation point as l′ ac , the real-time distance from the trolley to the center of the chute is l′ ad The real-time distance from the trolley to the center vertex of the chute is l′ adz ,

[0085] According to the initial state of the trolley, the distance from the trolley to the starting point of the trolley arm extension is l ab , and the distance l from the starting point of the trolley arm extension to the trolley arm lifting point bc , get the distance l from the trolley to the trolley arm elevation point ac for:

[0086] l ac = l ab +l bc

[0087] According to the initial state of the trolley, the distance l from the trolley to the starting point of the trolley arm extension ab, the distance from the starting point of the trolley arm extension to the trolley arm lifting point l bc And the distance from the lifting point of the trolley arm to the center of the chute l cd , the distance from the cart to the center of the chute is l ad for:

[0088] l ad = l ab +l bc +l cd

[0089] According to the initial state of the trolley, the distance from the trolley to the center of the chute is l ad , the vertical distance l from the radar to the center of the chute x , the horizontal distance from the radar to the center of the chute is l y , the angle α between the vehicle arm and the radar is obtained as:

[0090]

[0091] According to the encoder data, the telescopic length of the trolley arm is l' bc The distance l from the initial state trolley to the trolley arm lifting point ac , we can get the real-time distance l′ from the trolley to the trolley arm raising point ac for:

[0092] l′ ac = l ac +l′ bc

[0093] According to the trolley arm lifting angle θ, the real-time distance l′ from the trolley to the trolley arm lifting point ac And the distance from the lifting point of the trolley arm to the center of the chute l cd , get the real-time distance l′ from the center of the trolley to the chute ad for:

[0094]

[0095] According to the real-time distance l' from the trolley to the center of the chute ad And the horizontal distance l from the radar to the center of the chute y , get the real-time distance l′ from the center vertex of the chute to the trolley adz for:

[0096] l′ adz = l′ ad +l y

[0097] The radar horizontal movement angle obtained by the encoder The angle formed between the trolley arm and the radar is α, and the supplementary angle τ, and the angle β between the trolley arm and the vertical shore direction is: (The angle between the two objects in this scheme is equivalent to the angle formed by the line connecting a certain object and the coordinate origin and the line connecting another object and the coordinate origin. For example, the trolley arm is approximately a line segment starting from the origin of the coordinate system, and the line segment where the radar is located also starts from the origin of the coordinate system. An angle is formed between the trolley and the radar (the approximate angle ignores the volume, and both are approximate line segments))

[0098]

[0099] According to the initial length of the chute s0, the distance from the radar to the center vertex of the chute is set to l z , the distance from the lifting point of the trolley arm to the center of the chute is l cd And the trolley arm lifting angle θ, get the real-time length of the chute s′ z for:

[0100]

[0101] According to the real-time distance l′ from the trolley to the center vertex of the chute adz As well as the angle β between the trolley arm and the vertical shore direction (since the wharf shore is parallel to the river bank, the vertical shore direction is the direction perpendicular to the river bank), the real-time coordinates of the center vertex of the chute in the horizontal plane (s′) are obtained. x , s′ y )for:

[0102]

[0103] Through the above calculation, we can get the translation transformation matrix T corresponding to the vehicle coordinate system by transforming the entire radar coordinate system through the coordinate transformation S:

[0104]

[0105] The complete point cloud image of the ship loader is defined as P (that is, after point cloud coordinate transformation and point cloud data splicing, the complete point cloud data V″ of the ship loader is obtained), and the coordinates under the point cloud P are defined as P(x, y, z).

[0106] First, the point cloud data is processed so that the ship is parallel to the shore. The coordinates of any point cloud of the complete ship loader in the radar coordinate system P (x, y, z) are rotated while keeping the X axis unchanged to obtain the rotation matrix R1 x ,Right now:

[0107]

[0108] First rotate π degrees around the Y axis, corresponding to μ = π, and obtain the rotation matrix R1 of μ degrees around the Y axis y :

[0109]

[0110] Then rotate P around the Z axis to get the horizontal movement angle of the radar. Right now Get the rotation matrix R1 around the Z axis z :

[0111]

[0112] After the rotation transformation, point P(x, y, z) is obtained as P′(x′, y′, z′), that is:

[0113]

[0114] Translate P'(x', y', z') using the translation matrix

[0115] The final coordinate system P″(x″, y″, z″) is obtained, that is:

[0116]

[0117] Convert each coordinate in the ship loader point cloud in the complete radar coordinate system into the coordinate P″(x″, y″, z″) in the truck coordinate system according to the above conversion formula, and you can get the coordinate values ​​of all point clouds in the unified coordinate system of the truck, thus completing the unification of coordinates.

[0118] According to the attached Figure 2 , this plan should carry out the following implementation steps:

[0119] a. Obtain radar point cloud data of ship loader;

[0120] b. Obtain the positioning encoder data and the mechanical dimensions of the ship loader;

[0121] c. Establish radar coordinate system for point cloud data;

[0122] d. Perform coordinate transformation on point cloud data;

[0123] e. According to step d, stitching the ship loader point cloud data;

[0124] f. According to step b and step e, a unified coordinate system is established;

[0125] In one of the embodiments, a unified coordinate system method for a dock loader is provided, and a method for transforming a radar coordinate system into a truck coordinate system is provided, so that the loader can be easily operated and controlled, ensuring that the loader can accurately determine the position of the cargo on the ship when loading the cargo, which helps to reduce loading errors, improve loading efficiency, and ensure that the cargo can be transported safely. It also helps to ensure the safety of the loader during operation. The operator can promptly identify potential safety hazards based on the data in the coordinate system and take corresponding measures to prevent and deal with them.

[0126] Step 1:

[0127] A set of 3D laser scanning devices is installed on the left, right and rear of the upper maintenance platform of the chute. The device has a large scanning angle and high scanning frequency. It can scan the entire hull and chute, and obtain point cloud data of the bow position, cabin position and stern position. According to a complete ship loader operation process, a total of 350 continuous point cloud data and corresponding encoder data are obtained, as well as 7 mechanical dimensions measured by the ship loader.

[0128] Step 2:

[0129] In establishing the radar coordinate system module, the entire ship loader is scanned simultaneously by three radars. Each radar is installed in a different position, but its relative position to the chute is fixed.

[0130] The three radars are numbered, with radar No. 1 scanning the middle of the ship, radar No. 2 scanning the stern, and radar No. 3 scanning the bow. The three radars can scan the entire ship. Determine the radar positions, and take the positions where the three radars are installed as the origin of the coordinate system, which is the starting point of the radar coordinate system.

[0131] Define the direction of the coordinate axis. Define the direction of the coordinate axis according to the right-hand rule. Use the right-hand rule to determine the positive direction of the X, Y, and Z axes. According to the right-hand rule, the entire ship loader selects the positive direction of the X axis pointing to the stern, the positive direction of the Y axis perpendicular to the right side of the ship, and the positive direction of the Z axis perpendicular to the ship downward. Each radar has its own coordinate system. For the point cloud image scanned by radar No. 1, radar No. 1 uses the location of the radar as the coordinate origin, points to the stern as the positive direction of the X axis, perpendicular to the right side of the ship as the positive direction of the Y axis, and perpendicular to the ship downward as the positive direction of the Z axis to establish a radar coordinate system. Scan the middle of the ship to obtain a point cloud image. For radar No. 2 scanning the point cloud, radar No. 2 uses the location of the radar as the coordinate origin, points to the stern as the positive direction of the X axis, perpendicular to the right side of the ship as the positive direction of the Y axis, and perpendicular to the ship downward as the positive direction of the Z axis to establish a radar coordinate system. Scan the stern to obtain a point cloud image. For the point cloud image scanned by the No. 3 radar, the No. 3 radar establishes a radar coordinate system with the location of the radar as the coordinate origin, pointing to the stern as the positive direction of the X-axis, perpendicular to the right side of the ship outward as the positive direction of the Y-axis, and perpendicular to the ship downward as the positive direction of the Z-axis, and scans the bow of the ship to obtain a point cloud image.

[0132] Step 3:

[0133] In the point cloud data coordinate transformation and stitching module, since the coordinates of the point cloud data obtained by the lidar are relative to the radar coordinate system, the three different radar coordinate systems need to be unified. The point cloud image of the midship scanned by radar No. 1 is used as the reference point. In order to unify the coordinates of each radar to the coordinate system of radar No. 1, the coordinates of each point in the point cloud need to be converted.

[0134] Coordinate transformation includes rotation transformation and translation transformation. First, the radar coordinate system is rotated around the three coordinate axes of X, Y, and Z respectively.

[0135] The result of rotating around the X axis can be expressed as a 3×3 rotation matrix R x It means:

[0136]

[0137] Corresponding to the No. 1 radar Get the rotation matrix around the X axis:

[0138]

[0139] Corresponding to the No. 2 radar Get the rotation matrix around the X axis:

[0140]

[0141] Corresponding to the No. 3 radar Get the rotation matrix around the X axis:

[0142]

[0143] The result of rotating around the Y axis can be expressed as a 3×3 rotation matrix R y It means:

[0144]

[0145] For μ=0 corresponding to radar No. 1, the rotation matrix around the Y axis is obtained:

[0146]

[0147] For the μ=(0.311π) corresponding to the No. 2 radar, the rotation matrix around the Y axis is obtained:

[0148]

[0149] For the μ=(-0.896π) corresponding to the No. 3 radar, the rotation matrix around the Y axis is obtained:

[0150]

[0151] The result of rotating around the Z axis can be expressed as a 3×3 rotation matrix R z It means:

[0152]

[0153] For ∈=0 corresponding to radar No. 1, the rotation matrix around the Z axis is obtained:

[0154]

[0155] For the ∈=(-0.188π) corresponding to the No. 2 radar, the rotation matrix around the Z axis is obtained:

[0156]

[0157] For the ∈=(0.536π) corresponding to the No. 3 radar, the rotation matrix around the Z axis is obtained:

[0158]

[0159] After the point cloud data is rotated and transformed, it is then translated. The three-dimensional coordinates of a point in the radar coordinate system are known to be (x, y, z). The translation matrix T of the laser radar can be expressed as:

[0160]

[0161] The translation matrix T1 for radar No. 1 is:

[0162]

[0163] The translation matrix T2 for radar No. 2 is:

[0164]

[0165] The translation matrix T3 for radar No. 3 is:

[0166]

[0167] The above coordinate transformation is to use the point cloud image of the midship scanned by radar No. 1 as the reference point, rotate and translate the point cloud images scanned by radar No. 2 and radar No. 3, splice a complete point cloud image of the ship loader, and then transform the whole through the coordinate transformation matrix R′ x , R′ y , R′ z , T′ is as follows:

[0168]

[0169]

[0170]

[0171]

[0172] Finally, the radar coordinate system based on the No. 1 radar is obtained, with the positive direction of the X-axis pointing to the stern, the positive direction of the Y-axis perpendicular to the right side of the ship, the positive direction of the Z-axis perpendicular to the ship downward, and the XOY plane of the radar coordinate system is parallel to the shore.

[0173] Step 4:

[0174] In the unified coordinate system module, after point cloud coordinate transformation and point cloud data splicing, a complete point cloud map of the ship loader can be obtained, which is spliced ​​together by three different radars and based on radar No. 1.

[0175] Convert the point cloud data in the radar coordinate system to the vehicle coordinate system and establish a unified coordinate system. The following conditions must be met to establish a unified coordinate system:

[0176] 1. Simultaneity: When acquiring the location data of multiple sensors, it is necessary to ensure that the data of each sensor is acquired at the same time;

[0177] 2. Real-time: Since the loading process is a dynamic process, data needs to be obtained in a short time;

[0178] 3. Accuracy: The establishment of a unified coordinate system is achieved through the fusion of multiple sensors, so the data accuracy of each sensor directly affects the accuracy of the unified coordinate system;

[0179] 4. Stability: The unified coordinate system should be stable and able to maintain its accuracy and stability over a long period of time. This requires ensuring that the reference points and measuring equipment of the coordinate system will not be affected by the external environment;

[0180] 5. Reliability: The unified coordinate system should be reliable and able to work properly under various working conditions. This requires the selection of reliable measurement equipment and technology, and sufficient testing and verification.

[0181] By satisfying the above conditions, a unified coordinate system for the trolley is established. The data of each encoder is obtained by the Snap7 communication protocol. The mechanical dimensions in the initial state, the mechanical dimensions in the non-initial state, and the encoder data can be obtained. Taking the data obtained at a certain moment as an example, the mechanical dimensions of the trolley in the initial state, the elevation angle of the trolley arm raised θ=2.3531, and the horizontal movement angle of the radar The initial length of the chute is s0 = 10.355, and the distance from the trolley to the starting point of the trolley arm extension is l ab =12, distance from the starting point of the trolley arm extension to the trolley arm lifting point l bc=13, distance from the lifting point of the trolley arm to the center of the chute l cd =17.9, the distance from the trolley to the trolley arm elevation point is l ac =25, the distance from the trolley to the center of the chute is l ad =42.9, the vertical distance from the radar to the center of the chute is l x =3.5, the horizontal distance from the radar to the center of the chute is l y = 0.5, the distance from the radar to the center of the chute is set to l z =3, the angle between the trolley arm and the radar is α. The mechanical dimensions of the ship loader in the non-initial state, the real-time distance from the trolley to the trolley arm elevation point is l′ ac , telescopic length of trolley arm l′ bc , the real-time distance from the trolley to the center of the chute is l′ ad The real-time distance from the trolley to the center vertex of the chute is l′ adz ,The initial state is using the radar as the coordinate system, and the point cloud map of the loader operation in a continuous period of time is obtained.

[0182] According to the initial state of the trolley, the distance from the trolley to the starting point of the trolley arm extension is l ab , and the distance l from the starting point of the trolley arm extension to the trolley arm lifting point bc , get the distance l from the trolley to the trolley arm elevation point ac for:

[0183] l ac =12+13=25

[0184] According to the initial state of the trolley, the distance from the trolley to the starting point of the trolley arm extension is l ab , the distance from the starting point of the trolley arm extension to the trolley arm lifting point l bc And the distance from the lifting point of the trolley arm to the center of the chute l cd , the distance from the cart to the center of the chute is l ad for:

[0185] l ad =12+13+17.9=42.9

[0186] According to the initial state of the trolley, the distance from the trolley to the center of the chute is l ad , the vertical distance l from the radar to the center of the chute x , the horizontal distance from the radar to the center of the chute is l y , the angle α between the vehicle arm and the radar is obtained as:

[0187]

[0188] According to the encoder data, the telescopic length of the trolley arm is l' bc The distance l from the initial state trolley to the trolley arm lifting point ac, the real-time distance from the trolley to the trolley arm raising point is l′ ac for:

[0189] l′ ac =25+5.1165=30.1165

[0190] According to the trolley arm lifting angle θ, the real-time distance l′ from the trolley to the trolley arm lifting point ac And the distance from the lifting point of the trolley arm to the center of the chute l cd , get the real-time distance l′ from the center of the trolley to the chute ad for:

[0191]

[0192] According to the real-time distance l′ from the trolley to the center of the chute ad And the horizontal distance l from the radar to the center of the chute y , get the real-time distance l′ from the center vertex of the chute to the trolley adz is: l′ adz =48.0014+0.5=48.5014

[0193] The radar horizontal movement angle obtained by the encoder The included angle between the trolley arm and the radar is α and the supplementary angle τ, and the included angle β between the trolley arm and the vertical shore direction is obtained as:

[0194] β=-0.9353-4.6106+1.9=-3.6459

[0195] According to the initial length of the chute s0, the distance from the radar to the center vertex of the chute is set to l z , the distance from the lifting point of the trolley arm to the center of the chute is l cd And the trolley arm lifting angle θ, get the real-time length of the chute s′ z for:

[0196]

[0197] According to the real-time distance l′ from the trolley to the center vertex of the chute adz As well as the angle β between the trolley arm and the vertical shore direction, the real-time coordinates of the center vertex of the chute in the horizontal plane (s′ x , s′ y )for:

[0198]

[0199] Finally, the entire radar coordinate system is transformed into a coordinate system based on the vehicle, that is:

[0200]

[0201] By transforming the radar coordinate system into the truck coordinate system, we can finally obtain a point cloud map of the ship loader operation within a continuous period of time in the truck coordinate system.

[0202] Obviously, the specific implementation of the present invention is not limited to the above-mentioned methods. As long as various non-substantial improvements are made using the method concept and technical solution of the present invention, they are all within the protection scope of the present invention.

Claims

1. A method for unifying the coordinate system of a dock loader, characterized by: The steps include: S1. Use multiple radar sensors to obtain multiple point cloud data of the ship loader; S2. Establish a radar coordinate system for each point cloud data and define the intermediate coordinate system; S3. Convert each point cloud data from the corresponding radar coordinate system to the intermediate coordinate system and splice multiple point cloud data to form complete point cloud data of the ship loader; S4. It is recommended to unify the coordinate system, integrate the spliced ​​complete point cloud data of the ship loader with the real size data and position data to establish a unified coordinate system, and convert the complete point cloud data of the ship loader into point cloud data under the unified coordinate system.

2. The method for unifying the coordinate system of a dock loader according to claim 1, characterized in that: Three radar sensors are used to obtain radar point cloud data of the head, tail and midship of the ship loader.

3. A method for unifying coordinate systems for dock ship loaders according to claim 1 or 2, characterized in that: In step S2, the establishment of the radar coordinate system for the point cloud data acquired by each radar sensor includes: taking each radar installation position as the origin of the radar coordinate system, that is, the starting point of the radar coordinate system, defining the coordinate axis direction, and using the right-hand rule to define the coordinate axis direction. The right-hand rule is used to determine the positive direction of the X-axis, Y-axis, and Z-axis. According to the right-hand rule, the entire ship loader selects the direction pointing to the stern as the positive direction of the X-axis, the direction perpendicular to the right side of the ship and outward as the positive direction of the Y-axis, and the direction perpendicular to the ship and downward as the positive direction of the Z-axis.

4. A method for unifying coordinate systems for dock ship loaders according to claim 1 or 2, characterized in that: The intermediate coordinate system uses the radar coordinate system corresponding to one of the radar sensors.

5. A method for unifying coordinate systems for dock ship loaders according to any one of claims 1 to 3, characterized in that: In step S3, the radar coordinate system corresponding to one of the radar sensors is selected as the intermediate coordinate system, that is, the point cloud image scanned by one of the radar sensors is used as a reference point, and the point cloud images scanned by other radar sensors are rotated and translated to obtain a complete point cloud image of the ship loader. Then, the complete point cloud image of the ship loader is rotated and translated as a whole to obtain a point cloud image coordinate system with the stern pointing to the positive direction of the X axis, the right side of the ship pointing outward as the positive direction of the Y axis, and the downward direction of the ship as the positive direction of the Z axis, and the XOY plane of the radar coordinate system parallel to the shore.

6. The method for unifying the coordinate system of a dock loader according to claim 5, characterized in that: When rotating and translating the point cloud, the rotation operation includes using the X-axis rotation matrix R x , Y-axis rotation matrix R y , Z-axis rotation matrix R z The coordinates of each point in the point cloud are rotated, and after the rotation transformation, the translation operation is performed through the translation matrix T.

7. A method for unifying coordinate systems for dock ship loaders according to any one of claims 1 to 6, characterized in that: The coordinates of each point in the complete point cloud image of the ship loader in the radar coordinate system are converted into coordinates in the unified coordinate system through the coordinate transformation matrix S, and then the point cloud image data in the unified coordinate system is obtained, completing the establishment of the unified coordinate system of the terminal ship loader.

8. The method for unifying coordinate systems of dock ship loaders according to claim 7, characterized in that: The coordinate transformation matrix S is: in l′ adz is the real-time distance from the trolley to the center of the chute, and β is the angle between the trolley arm and the vertical bank direction; the initial length of the chute is s0, and the distance from the radar to the center of the chute is l z , the distance from the lifting point of the trolley arm to the center of the chute is l cd And the trolley arm lifting angle θ.

9. The method for unifying coordinate systems of dock ship loaders according to claim 6, characterized in that: Rotate the point cloud in the radar coordinate system around the three coordinate axes of X, Y, and Z respectively; in Rotate the result around the X axis using a 3×3 rotation matrix R x Expressed as: Rotate the result around the Y axis using a 3×3 matrix R y Expressed as: Rotate the result around the Z axis using a 3×3 matrix R z Expressed as:

10. A device for unifying the coordinate system of a dock loader, characterized by: The device includes a data acquisition module, a radar coordinate system establishment module, a point cloud coordinate transformation and splicing module, and a unified coordinate system module; wherein the data acquisition module is used to collect multiple point cloud data of the ship loader obtained by multiple radar sensors; The radar coordinate system establishment module establishes a radar coordinate system for each point cloud data and defines an intermediate coordinate system; The point cloud coordinate transformation and splicing module converts each point cloud data from the corresponding radar coordinate system to the intermediate coordinate system and splices multiple point cloud data to form complete point cloud data of the ship loader; The unified coordinate system module is used to propose a unified coordinate system, fuse the spliced ​​complete point cloud data of the ship loader with the real size data and position data to establish a unified coordinate system, and convert the complete point cloud data of the ship loader into point cloud data under the unified coordinate system.

Images