Aerial work mechanical arm real-time environment perception and autonomous positioning method

Abstract

The application discloses a kind of high-altitude operation mechanical arm real-time environment perception and autonomous positioning method, comprising: obtaining point cloud information by 2D laser radar and 3D ultrasonic sensor, then the point cloud information is handled by inverse sensor model and Bayes filtering, obtains 2D occupancy probability map and 3D occupancy probability map, and is stored in octree data structure;Mapping is obtained by octree data structure, and fused 3D map and 2D map;In the working process of high-altitude operation mechanical arm, the position change of 3D ultrasonic sensor and 2D laser radar at cycle initial and cycle end is obtained by mechanical arm pose inverse solution method, the displacement change and angle change of mechanical arm are used for error reduction, and 3D map is expanded and updated.The application can realize autonomous environment perception, and has higher precision, can realize side operation, side mapping, no longer needs to establish panoramic map in advance, so efficiency is higher, and environmental adaptability, flexibility is better.

Description

Technical Field

[0001] This invention relates to a method for real-time environmental perception and autonomous positioning of a robotic arm for high-altitude operations. Background Technology

[0002] Aerial work platforms mainly consist of a ground-mobile platform, a slewing mechanism, and a platform frame. The platform frame can be lifted, allowing workers to perform high-altitude operations within it. Aerial work robotic arms are an improvement on aerial work platforms, where the platform frame is replaced by a robotic arm to replace manual labor.

[0003] Aerial work robots need to perceive real-time 3D information when operating in high-altitude environments. SLAM technology can perform 3D mapping, but SLAM 3D mapping relies on 3D LiDAR, which is expensive and bulky, making it inconvenient to mount on the relatively small aerial work robot. Currently, aerial work robots are mainly guided by humans and cannot achieve autonomous environmental perception. Specifically, multi-sensor fusion and 3D scanners are used to pre-scan the panoramic scene to obtain a priori map, and then the aerial work robot operates within this map. This pre-mapping process is labor-intensive and requires manual labor, resulting in low efficiency and impacting the environmental adaptability and flexibility of the aerial work robot. Summary of the Invention

[0004] Purpose of the invention: The purpose of this invention is to provide a method for real-time environmental perception and autonomous positioning of a high-altitude work robot, which can achieve autonomous environmental perception and realize simultaneous operation and mapping.

[0005] Technical solution: The real-time environmental perception and autonomous positioning method for high-altitude operation robotic arms described in this invention includes:

[0006] (1) Initial map creation

[0007] A 2D lidar is installed on a ground-based mobile platform, and a 3D ultrasonic sensor is installed on a robotic arm;

[0008] The ground mobile platform moves to the designated work location, and the robotic arm and luffing mechanism maintain a fixed posture. A 2D point cloud is acquired through 2D LiDAR and transmitted to the 2D SLAM system. The position of the center of the ground mobile platform is obtained through particle filtering, and this position information is transmitted into an octree data structure. A 2D map is obtained through 2D mesh mapping.

[0009] 3D point cloud is acquired by a 3D ultrasonic sensor, the 3D point cloud is preprocessed to transform the spherical coordinate system into the Cartesian coordinate system, and the relative distance between the 3D ultrasonic sensor and the 2D LiDAR is obtained by the inverse kinematics method of the robotic arm. This displacement is then incorporated into the 3D point cloud to obtain the relative position coordinates between the 3D point cloud and the 2D LiDAR.

[0010] 2D point clouds are processed into 2D occupancy probability maps through inverse sensor models and Bayesian filtering, and 3D point clouds are processed into 3D occupancy probability maps through inverse sensor models and Bayesian filtering. The 2D and 3D occupancy probability maps are stored in an octree data structure.

[0011] A fused 3D map is obtained by constructing a map using an octree data structure. The 3D occupancy probability map in the octree data structure is then projected onto a two-dimensional plane, and a fused 2D map is obtained through data fusion.

[0012] (2) Map updates during operation

[0013] The ultrasonic cycle timing module records the emission cycle of the 3D ultrasonic sensor for subsequent error processing.

[0014] After the initial map is established, the robotic arm begins to rise or fall for high-altitude positioning; the 3D ultrasonic sensor begins to scan the surrounding objects at the beginning of a cycle, while the gyratory amplitude mechanism and the robotic arm begin to move.

[0015] At the end of one cycle, the 3D ultrasonic sensor transmits the collected 3D point cloud to the 3D coordinate overlay module; the position of the slewing amplitude mechanism and the robotic arm has relative displacement and angle changes at the beginning and end of the cycle. The relative coordinate inverse solution module obtains the position changes of the 3D ultrasonic sensor and the 2D lidar through the robotic arm pose inverse solution method, and then transmits the displacement and angle changes of the robotic arm to the error reduction module.

[0016] After obtaining the inverse solution of the relative position change within one cycle, the ultrasonic cycle timing module increments by one cycle and begins the inverse solution of the position change and the acquisition of the 3D point cloud for the next cycle. At the same time, the error reduction module obtains the relative displacement and angle change of each point cloud by acquiring the time of each point cloud acquisition, according to the proportional relationship between displacement and angle change within one cycle, and assuming that the displacement and rotation of the robotic arm within a very short cycle are uniform, and then transmits it to the 3D coordinate overlay module. The 3D coordinate overlay module adds the displacement change to the 3D point cloud, and then rotates the 3D point cloud according to the angle change to make the direction of the new 3D point cloud consistent with the direction of the 3D map obtained from the initial positioning.

[0017] The new 3D point cloud is fed into an octree data structure for mapping. A wider 3D map is then built on the initial 3D map, thus obtaining a 3D map within a cycle of a 3D ultrasonic sensor. The above process is repeated to update the map during operation.

[0018] Furthermore, the strategy for generating integrated 3D maps is as follows:

[0019] First, set a threshold for the change of map cell state during fusion. The threshold ranges from 0 to 1. The cell state has three types: occupied, idle, and unknown. Then, perform mathematical operations on the two probability values ​​of cells or voxels at the same location on the new map from the 2D and 3D maps to obtain the fusion probability of the voxel at that location on the new map. If the probability of the voxel reaches the threshold, the voxel state is changed to generate different states of different cells on the new map. The final new map is the fused 3D map.

[0020] Furthermore, the cell occupancy threshold is set to 0.7, the idle threshold is set to 0.4, a cell probability P≥0.7 is considered occupied, P≤0.4 is considered idle, and 0.4<P<0.7 is considered unknown; P1 is set as the probability value of a 3D map voxel, P2 is the probability value of a 2D map cell, and P is set as the value of P1 and P2 after mathematical operation; if P1≥0.7, P=P1 regardless of the value of P2; if P1<0.4, then P=P1+P2; if 0.4≤P1<0.7, then P=P2.

[0021] Furthermore, the generation strategy for fused 2D maps is as follows: similar to the generation strategy for fused 3D maps, a threshold for the change of map cell state during fusion is first set. Then, 3D map voxels that are closer to the 2D map in height are projected onto a new 2D map. The two probability values ​​of cells at the same location in the new 2D map and the 2D map generated by LiDAR are mathematically calculated, and the resulting fusion probability result covers the probability of cells in the new 2D map. If the cell probability reaches the threshold, the cell state is changed. After all cells are fused, the new map obtained is the fused 2D map.

[0022] Furthermore, the process of processing a 2D point cloud into a 2D occupancy probability map includes:

[0023] p(m i |s j ) is defined as the occupancy probability value of a cell in the inverse sensor model, m i s represents the i-th cell. j This represents the measurement data from the j-th sensor;

[0024] The sector is the area measured by the sensor in one measurement, including the white area A1 and the gray area A2. The sector angle is Δα. j represents a specific point obtained by the sensor in one measurement, located at the center of the gray area A2, and the distance from the arc edge of the gray area A2 is l. The line connecting point j and the origin O lies on the angle bisector of the sector. j It is the angle between the line connecting point j and the origin O in polar coordinates and the polar axis; r j It is the distance from point j to the origin O in polar coordinates;

[0025] Point i is the center point of a certain cell, and its distance from the origin O is d. i ,β i It is the angle between the line connecting point i and the origin O in the polar coordinate system and the polar axis;

[0026] Calculate the probability of cell i occupying the gray area A2, d. i Larger than the radius of the white area A1:

[0027] First calculate the distance probability O r and angular probability O α :

[0028]

[0029]

[0030] Where k r With k α The value ranges from 0 to 1;

[0031] Then, according to p(m) i |s j ) = O r O α Find the probability of cell i occupying the gray area A2. The closer the cell is to point j, the greater the probability value.

[0032] When point i is a cell in the white area A1, d i Smaller than the radius of the white area A1:

[0033] First calculate the distance probability E r and angular probability E α :

[0034]

[0035]

[0036] Then, according to p(m) i |s j ) = 1 - E r E αFind the probability that point i occupies a cell in the white area A1.

[0037] Furthermore, when p(m) i |s j When the cell probability exceeds the occupied probability threshold or falls below the idle probability threshold, update the cell occupancy probability:

[0038] The obtained inverse sensor model value p(m) i |s j Substituting into the Bayesian filter formula below, we can iteratively calculate and predict the probability value of this cell appearing in the next data point:

[0039]

[0040] l i,N The logarithmic odd representation of the probability that cell i occupies the Nth data point. i,0 =0;

[0041] The obtained l i,N Substituting the value into the following formula, we can update the cell occupancy probability to obtain the occupancy probability of the i-th grid cell in the Nth data:

[0042]

[0043] When p(m) i |s1,...,s N When the value is greater than or equal to the occupancy probability threshold, the cell is considered to exist as an object, and the cell's occupancy probability is reset to the occupancy probability threshold. When the value is less than or equal to the idle probability threshold, the cell is considered to be empty, and the cell's occupancy probability is reset to the idle probability threshold.

[0044] Furthermore, the occupancy probabilities of each raster cell are multiplied together to obtain the local raster map occupancy probability:

[0045]

[0046] M represents the number of cells that make up the large cell.

[0047] Furthermore, the process of processing 3D point clouds into a 3D occupancy probability map includes:

[0048] The same method is used to calculate the occupancy probability of the 3D cells, dividing the 3D point cloud into 3D cells. Then, a height parameter, p(m), is added to the inverse sensor model value formula. i |s j ) = O r O α O h With p(m) i |s j) = 1 - E r E α E h The calculation method is then consistent with the occupancy probability calculation of 2D cells. Finally, the occupancy probability of 3D cells remains unchanged. That is, even if the final result exceeds the occupancy probability threshold or is lower than the idle probability threshold, the occupancy probability threshold and the idle probability threshold will not be used to cover the final result.

[0049] Furthermore, the 3D ultrasonic sensor is denoted as T at the beginning of the i-th cycle. i a The 3D ultrasonic sensor at the end of the i-th cycle is denoted as T. i b ; ΔXi, ΔYi, ΔZi, Δαi, Δβi, Δγi are T i a Time to T i b The displacement and angular relative position change of the 3D ultrasonic sensor within one cycle is given by ΔXi, where Δαi is the displacement change along the X-axis, ΔYi is the displacement change along the Y-axis, Δβi is the angular change around the Y-axis, ΔZi is the displacement change along the Z-axis, and Δγi is the angular change around the Z-axis.

[0050] Furthermore, the error reduction module obtains the distance L between the voxel and the origin of the 3D ultrasonic sensor coordinate system through the three-dimensional point cloud. Based on the principle that the ultrasonic wave speed is constant in the same medium, the velocity V is obtained. The time t = 2L / V, where t < T, is obtained by the ratio of 2L to V.

[0051] Due to T i a To T i b Let the time be T, and the distance the ultrasonic sensor moves relative to its previous position be ΔXi. Assuming the robotic arm moves at a constant speed within a very short period, then the actual distance ΔXi moves within t seconds is:

[0052]

[0053] The actual relative displacement of the point cloud after processing by the error reduction module, i.e., Δxi, is compared with the displacement at the end of one period T. i b The relative displacement at any given moment must be small; according to the proportion. The values ​​of the other five relative variables are calculated, and then error reduction processing is performed on all the 3D point clouds.

[0054] Beneficial effects: Compared with the prior art, the present invention has the following significant advantages:

[0055] This invention acquires point cloud information based on 2D LiDAR and 3D ultrasonic sensors, then converts the 2D and 3D point clouds into a probabilistic map and fuses them to obtain an initial map. The robotic arm achieves autonomous localization (position in the environment) during the mapping process. The map is then updated and errors are reduced during operation. This invention enables the robotic arm to autonomously perceive the environment at high altitudes with high accuracy. It can perform mapping while working, eliminating the need to pre-build a panoramic map, thus resulting in higher efficiency and better environmental adaptability and flexibility. Attached Figure Description

[0056] Figure 1 This is a structural diagram of a robotic arm for aerial work.

[0057] Figure 2 This is a schematic diagram of a multi-sensor measurement method;

[0058] Figure 3 This is a schematic diagram of the inverse sensor model of a 3D ultrasonic sensor in the XY components.

[0059] Figure 4 This is a schematic diagram of the calculation method for the inverse sensor model;

[0060] Figure 5 This is a flowchart of the initial map creation method;

[0061] Figure 6 This is a schematic diagram of point cloud data collected by a 3D ultrasonic sensor and a 2D lidar.

[0062] Figure 7 This is a diagram illustrating the map update method during the work process;

[0063] Figure 8 This is a simplified diagram of the two-state transformation structure of a high-altitude work robot arm;

[0064] Figure 9 This is a schematic diagram of the change in the surface projection of the relative position variable within a period;

[0065] Figure 10 This is a schematic diagram showing the projection of a relative position variable onto the ZY plane within a period.

[0066] Figure 11 This is a schematic diagram illustrating the reduction of errors in point cloud positioning. Detailed Implementation

[0067] The invention will now be further described with reference to the accompanying drawings.

[0068] Figure 1The diagram shows the structure of a high-altitude work robot, which includes a ground-moving platform 1, a luffing mechanism 2, and a robot arm 3. In this embodiment, the robot arm 3 is a six-degree-of-freedom robot arm, fixed to the front end of the luffing mechanism 2.

[0069] This application provides a method for real-time environmental perception and autonomous positioning of a high-altitude work robotic arm, which specifically includes the following steps:

[0070] (1) Initial map creation

[0071] A 2D LiDAR 5 is installed above the front end of the ground mobile platform 1, and a 3D ultrasonic sensor 4 is installed at the end of the robotic arm 3. It should be noted that the installation positions of the 2D LiDAR 5 and the 3D ultrasonic sensor 4 are not fixed. As long as the 2D LiDAR 5 is installed on the ground mobile platform 1 and the 3D ultrasonic sensor 4 is installed on the robotic arm 3, the detection purpose can be achieved.

[0072] The ground mobile platform moves to the designated work location, and the robotic arm and luffing mechanism maintain a fixed posture, as shown in the initial posture. Figure 1 As shown. The 3D ultrasonic sensor and 2D LiDAR simultaneously begin scanning the surrounding environment to collect information. The initial map creation process is as follows: Figure 5 As shown.

[0073] 2D point clouds are acquired using 2D LiDAR and then fed into a 2D SLAM system. The position of the center of the ground mobile platform is obtained using a particle filtering method, and this position information is fed into an octree data structure. A 2D map is then generated by building a 2D mesh.

[0074] 3D point clouds are acquired using a 3D ultrasonic sensor. The 3D point clouds are preprocessed (transforming the spherical coordinate system into the Cartesian coordinate system). The relative distance between the 3D ultrasonic sensor and the 2D LiDAR is then obtained using the inverse kinematics method of a robotic arm. This displacement is incorporated into the 3D point cloud to obtain the relative position coordinates between the 3D point cloud and the 2D LiDAR. Finally, the point cloud information collected by the two sensors is fed into the inverse sensor models of the two sensors respectively.

[0075] The 2D point cloud is processed into a 2D occupancy probability map (the probability of each cell in the 2D grid map) through an inverse sensor model and Bayesian filtering. The 3D point cloud is processed into a 3D occupancy probability map (the probability of each voxel in the 3D map) through an inverse sensor model and Bayesian filtering. The 2D occupancy probability map and the 3D occupancy probability map (i.e. the Bayesian filtering result) are stored in an octree data structure.

[0076] like Figure 2The diagram illustrates a multi-sensor measurement method, where P represents probability and m represents a grid cell. For a given object, each sensor obtains a different measurement value. These values ​​are converted into probability values ​​using an inverse sensor model and Bayesian filtering. The probability values ​​from different sensors are then fused to infer the probability of an obstacle's presence. This invention fuses probability values ​​obtained from a 3D ultrasonic sensor and a 2D LiDAR.

[0077] Figure 4 The diagram shows a schematic of the inverse sensor model calculation method (processing point cloud data into a probability map), p(m i |s j ) is defined as the occupancy probability value of a cell in the inverse sensor model, m i s represents the i-th cell. j p(m) represents the measurement data of the j-th sensor. i |s j ) represents the probability of the i-th cell under the measurement data of the j-th sensor.

[0078] The sector-shaped region is the area measured by the sensor in one measurement, including the white region A1 and the gray region A2. The sector angle is Δα. j represents a specific point obtained by the sensor in one measurement, located on the angle bisector of the sector (the line connecting point j and the origin O), and located at the center of the gray region A2, with a distance l from the arc edge of the gray region A2; α j It is the angle between the line connecting point j and the origin O in polar coordinates and the polar axis; r j It is the distance from point j to the origin O in polar coordinates.

[0079] Point i is the center point of a certain cell, and its distance from the origin O is d. i ,β i It is the angle between the line connecting point i and the origin O in the polar coordinate system and the polar axis;

[0080] Calculate the probability of cell i occupying point i in the gray area A2 (d i (Greater than the radius of the white area A1):

[0081] First calculate the distance probability O r and angular probability O α :

[0082]

[0083]

[0084] Where k r With k α The value ranges from 0 to 1;

[0085] Then, according to p(m) i |sj ) = O r O α Find the probability of cell i occupying the gray area A2. The closer the cell is to point j, the greater the probability value.

[0086] When point i is a cell in the white area A1 (d i (smaller than the radius of the white area A1):

[0087] First calculate the distance probability E r and angular probability E α :

[0088]

[0089]

[0090] Then, according to p(m) i |s j ) = 1 - E r E α Find the probability that point i occupies a cell in the white area A1.

[0091] When p(m) i |s j When the value exceeds 0.7 or falls below 0.4, update the cell occupancy probability: The obtained inverse sensor model value p(m) i |s j Substituting into the Bayesian filter formula below, we can iteratively calculate and predict the probability value of this cell appearing in the next data point:

[0092]

[0093] l i,N The logarithmic odd representation of the probability that cell i occupies the Nth data point. i,0 =0;

[0094] The obtained l i,N Substituting the value into the following formula, we can update the cell occupancy probability to obtain the occupancy probability of the i-th grid cell in the Nth data:

[0095]

[0096] When p(m) i |s1,...,s N When the value is greater than or equal to 0.7, the cell is considered to exist as an object, and the cell's occupancy probability is reset to 0.7. When the value is less than or equal to 0.4, the cell is considered to not exist as an object, and the cell's occupancy probability is reset to 0.4.

[0097] Then multiply the occupancy probabilities of each raster cell to obtain the local raster map occupancy probability (since the raster map cells are too fine, multiple cells can be merged into one large cell to reduce computational complexity):

[0098]

[0099] Where M represents the number of cells that make up the large cell.

[0100] The final raster map obtained from the measurement data is as follows: Figure 3 As shown.

[0101] The same method is used to calculate the occupancy probability of 3D cells (or voxels). The 3D point cloud obtained by the sensor is divided into 3D cells (at this point, the number of 3D cells is huge; storing the resulting 3D raster occupancy map after Bayesian filtering in an octree data structure can optimize storage space). Then, a height parameter, p(m), is added to the inverse sensor model value formula. i |s j ) = O r O α O h With p(m) i |s j ) = 1 - E r E α E h The calculation method is then consistent with the occupancy probability calculation of 2D cells. Finally, the occupancy probability of 3D cells remains unchanged. That is, even if the final result exceeds 0.7 or is lower than 0.4, 0.7 and 0.4 will not be used to cover the final result.

[0102] A fused 3D map is obtained by constructing a map using an octree data structure. The 3D occupancy probability map in the octree data structure is then projected onto a two-dimensional plane, and a fused 2D map is obtained through data fusion. The fusion strategy is as follows.

[0103] The strategy for generating integrated 3D maps is as follows:

[0104] First, set the threshold for the change of map cell state during fusion (the threshold value ranges from 0 to 1, and the cell state has three types: occupied, idle, and unknown). In this embodiment, the cell occupation threshold is set to 0.7, the idle threshold is set to 0.4, the cell probability P≥0.7 is regarded as occupied, P≤0.4 is regarded as idle, and 0.4<P<0.7 is regarded as unknown.

[0105] Then, the probability values ​​of the cells or voxels at the same location (same X, Y coordinates) on the new map from the 2D and 3D maps are mathematically calculated to obtain the fusion probability of the voxel at that location on the new map. If the probability of the voxel reaches the threshold, the voxel state is changed to generate different states of different cells on the new map. The final new map is the fused 3D map.

[0106] Let P1 be the probability value of a 3D map voxel, and P2 be the probability value of a 2D map cell. Let P be the value of P1 and P2 after mathematical operations. If P1 ≥ 0.7, then P = P1 regardless of the value of P2. If P1 < 0.4, then P = P1 + P2. If 0.4 ≤ P1 < 0.7, then P = P2.

[0107] like Figure 6 As shown, when the 3D ultrasonic sensor obtains point cloud information of an object, it can acquire all point cloud data within the range of the angle between lines 1 and 3, while the 2D LiDAR can only scan the data at point 3. When both the voxel and the cell are generated from the data at point 3, the above fusion strategy is adopted to keep the 3D map generated by the 3D ultrasonic sensor outside of point 3 unchanged. The final new map is the fused 3D map.

[0108] The strategy for generating integrated 2D maps is as follows:

[0109] Similar to the strategy for generating fused 3D maps, a threshold for changing the state of map cells during fusion is first set. Then, 3D map voxels that are closer to the 2D map in height are projected onto a new 2D map. The two probability values ​​of cells at the same location in the new 2D map and the 2D map generated by LiDAR are mathematically calculated, and the resulting fusion probability result is used to overwrite the probability of cells in the new 2D map. If the cell probability reaches the threshold, the cell state is changed. After all cells are fused, the resulting new map is the fused 2D map.

[0110] (2) Map updates during operation

[0111] During the process, map updates are based on the established initial map. The continuously updated 3D point cloud is fed into a Bayesian filter for processing and then stored in an octree data structure to continuously expand the initial 3D map. Specifically, this includes:

[0112] like Figure 7 As shown, the ultrasonic cycle timing module records the emission cycle of the 3D ultrasonic sensor for subsequent error processing.

[0113] After the initial map is established, the robotic arm begins to rise or fall for high-altitude positioning; the 3D ultrasonic sensor starts from the beginning of the i-th cycle (denoted as T). i aIt begins scanning the surrounding objects, and at the same time, the gyratory amplitude mechanism and the robotic arm begin to move.

[0114] For ease of understanding, such as Figure 8 As shown, the initial pose and working pose of the aerial work robot are transformed into simplified diagrams. At the end of the i-th cycle (denoted as T...), the 3D ultrasonic sensor... i b The acquired 3D point cloud is then transmitted to the 3D coordinate overlay module. Simultaneously, the relative displacement and angle of the slewing mechanism and the robotic arm change at the beginning and end of the cycle. The relative coordinate inverse kinematics module uses the robotic arm pose inverse kinematics method to obtain the positional changes of the 3D ultrasonic sensor and the 2D LiDAR. For example... Figure 9 and Figure 10 As shown, ΔXi, ΔYi, ΔZi, Δαi, Δβi, Δγi are T i a Time to T i b The relative position changes of the displacement and angle of the 3D ultrasonic sensor within one cycle are recorded, where ΔXi is the displacement change along the X-axis, Δαi is the angle change around the X-axis, ΔYi is the displacement change along the Y-axis, Δβi is the angle change around the Y-axis, ΔZi is the displacement change along the Z-axis, and Δγi is the angle change around the Z-axis (in the figure, ΔXi', ΔYi', and ΔZi' represent the relative displacements caused by the movement of the robotic arm alone). Then, the three displacement changes and three angle changes are transmitted to the error reduction module.

[0115] like Figure 11 As shown, the error reduction module obtains the distance L between the voxel and the origin of the 3D ultrasonic sensor coordinate system through the three-dimensional point cloud. Based on the principle that the ultrasonic wave speed is constant in the same medium, the velocity V is obtained. The time t = 2L / V, where t < T, is obtained by the ratio of 2L to V.

[0116] Taking a displacement as an example: due to T i a To T i b The time taken is T, and the distance the ultrasonic sensor moves relative to its previous position is ΔXi (e.g., ...). Figure 9 and Figure 10 As shown in the figure, assuming the robotic arm moves at a constant speed within an extremely short period, the actual distance Δxi traveled in t seconds is:

[0117]

[0118] Figure 11 In the process, the actual relative displacement of the point cloud after processing by the error reduction module, i.e., Δxi, is compared with the displacement at the end of one cycle, T. ib The relative displacement at any given moment must be small; according to the proportion. The values ​​of the other five relative variables are calculated, and then error reduction processing is performed on all the 3D point clouds.

[0119] The process then proceeds in two parts. First, after obtaining the inverse kinematics of the relative position change within one cycle, the ultrasonic cycle timing module increments by one cycle (i++) to begin the inverse kinematics of the position change and the acquisition of the 3D point cloud for the next cycle. Second, the error reduction module obtains the relative displacement and angle changes of each point cloud by acquiring the time of each point cloud acquisition, according to the proportional relationship between displacement and angle changes within one cycle, and assuming that the displacement and rotation of the robotic arm within a very short cycle are uniform. This is then passed to the 3D coordinate overlay module, which adds the displacement changes to the 3D point cloud and rotates the 3D point cloud according to the angle changes to make the direction of the new 3D point cloud consistent with the direction of the 3D map obtained from the initial positioning, thus obtaining the absolute 3D point cloud at time Ti.

[0120] The new 3D point cloud is fed into an octree data structure for mapping. A wider 3D map is then built on the initial 3D map, thus obtaining a 3D map within a cycle of a 3D ultrasonic sensor. The above process is repeated to update the map during operation.

Claims

1. A method for real-time environmental perception and autonomous positioning of a high-altitude work robotic arm, characterized in that, include: (1) Initial map creation A 2D lidar is installed on a ground-based mobile platform, and a 3D ultrasonic sensor is installed on a robotic arm; The ground mobile platform moves to the designated work location, and the robotic arm and luffing mechanism maintain a fixed posture. A 2D point cloud is acquired through 2D LiDAR and transmitted to the 2D SLAM system. The position of the center of the ground mobile platform is obtained through particle filtering, and this position information is transmitted into an octree data structure. A 2D map is obtained through 2D mesh mapping. 3D point cloud is acquired by a 3D ultrasonic sensor, the 3D point cloud is preprocessed to transform the spherical coordinate system into the Cartesian coordinate system, and the relative distance between the 3D ultrasonic sensor and the 2D LiDAR is obtained by the inverse kinematics method of the robotic arm. This displacement is then incorporated into the 3D point cloud to obtain the relative position coordinates between the 3D point cloud and the 2D LiDAR. 2D point clouds are processed into 2D occupancy probability maps through inverse sensor models and Bayesian filtering, and 3D point clouds are processed into 3D occupancy probability maps through inverse sensor models and Bayesian filtering. The 2D and 3D occupancy probability maps are stored in an octree data structure. A fused 3D map is obtained by constructing a map using an octree data structure. The 3D occupancy probability map in the octree data structure is then projected onto a two-dimensional plane, and a fused 2D map is obtained through data fusion. (2) Map updates during operation The ultrasonic cycle timing module records the emission cycle of the 3D ultrasonic sensor for subsequent error processing. After the initial map is established, the robotic arm begins to rise or fall for high-altitude positioning; the 3D ultrasonic sensor begins to scan the surrounding objects at the beginning of a cycle, while the gyratory amplitude mechanism and the robotic arm begin to move. At the end of one cycle, the 3D ultrasonic sensor transmits the collected 3D point cloud to the 3D coordinate overlay module; the position of the slewing amplitude mechanism and the robotic arm has relative displacement and angle changes at the beginning and end of the cycle. The relative coordinate inverse solution module obtains the position changes of the 3D ultrasonic sensor and the 2D lidar through the robotic arm pose inverse solution method, and then transmits the displacement and angle changes of the robotic arm to the error reduction module. After obtaining the inverse solution of the relative position change within one cycle, the ultrasonic cycle timing module increments by one cycle and begins the inverse solution of the position change and the acquisition of the 3D point cloud for the next cycle. At the same time, the error reduction module obtains the relative displacement and angle change of each point cloud by acquiring the time of each point cloud acquisition, according to the proportional relationship between displacement and angle change within one cycle, and assuming that the displacement and rotation of the robotic arm within a very short cycle are uniform, and then transmits it to the 3D coordinate overlay module. The 3D coordinate overlay module adds the displacement change to the 3D point cloud, and then rotates the 3D point cloud according to the angle change to make the direction of the new 3D point cloud consistent with the direction of the 3D map obtained from the initial positioning. The new 3D point cloud is fed into an octree data structure for mapping. A wider 3D map is then built on the initial 3D map, thus obtaining a 3D map within a cycle of a 3D ultrasonic sensor. The above process is repeated to update the map during operation.

2. The method for real-time environmental perception and autonomous positioning of a high-altitude work robot according to claim 1, characterized in that, The strategy for generating integrated 3D maps is as follows: First, set a threshold for the change of map cell state during fusion. The threshold ranges from 0 to 1. The cell state has three types: occupied, idle, and unknown. Then, perform mathematical operations on the two probability values ​​of cells or voxels at the same location on the new map from the 2D and 3D maps to obtain the fusion probability of the voxel at that location on the new map. If the probability of the voxel reaches the threshold, the voxel state is changed to generate different states of different cells on the new map. The final new map is the fused 3D map.

3. The method for real-time environmental perception and autonomous positioning of a high-altitude work robot according to claim 2, characterized in that, The cell occupancy threshold is set to 0.7, and the idle threshold is set to 0.

4. A cell probability P ≥ 0.7 is considered occupied, P ≤ 0.4 is considered idle, and 0.4 < P < 0.7 is considered unknown. P1 is set as the probability value of a 3D map voxel, and P2 is set as the probability value of a 2D map cell. Let P be the value of P1 and P2 after mathematical operations. If P1 ≥ 0.7, P = P1 regardless of the value of P2. If P1 < 0.4, then P = P1 + P2. If 0.4 ≤ P1 < 0.7, then P = P2.

4. The method for real-time environmental perception and autonomous positioning of a high-altitude work robot according to claim 3, characterized in that, The strategy for generating fused 2D maps is similar to that for generating fused 3D maps. First, a threshold for changing the state of map cells during fusion is set. Then, 3D map voxels that are closer to the 2D map in height are projected onto a new 2D map. The two probability values ​​of cells at the same location in the new 2D map and the 2D map generated by LiDAR are mathematically calculated, and the resulting fusion probability result is used to overwrite the probability of cells in the new 2D map. If the cell probability reaches the threshold, the cell state is changed. After all cells are fused, the resulting new map is the fused 2D map.

5. The method for real-time environmental perception and autonomous positioning of a high-altitude work robot according to any one of claims 2 to 4, characterized in that, The process of processing a 2D point cloud into a 2D occupancy probability map includes: p(m i |s j ) is defined as the occupancy probability value of a cell in the inverse sensor model, m i s represents the i-th cell. j This represents the measurement data from the j-th sensor; The sector is the area measured by the sensor in one measurement, including the white area A1 and the gray area A2. The sector angle is Δα. j represents a specific point obtained by the sensor in one measurement, located at the center of the gray area A2, and the distance from the arc edge of the gray area A2 is l. The line connecting point j and the origin O lies on the angle bisector of the sector. j It is the angle between the line connecting point j and the origin O in polar coordinates and the polar axis; r j It is the distance from point j to the origin O in polar coordinates; Point i is the center point of a certain cell, and its distance from the origin O is d. i ,β i It is the angle between the line connecting point i and the origin O in the polar coordinate system and the polar axis; Calculate the probability of cell i occupying the gray area A2, d. i Larger than the radius of the white area A1: First calculate the distance probability O r and angular probability O α : Where k r With k α The value ranges from 0 to 1; Then, according to p(m) i |s j ) = O r O α Find the probability of cell i occupying the gray area A2. The closer the cell is to point j, the greater the probability value. When point i is a cell in the white area A1, d i Smaller than the radius of the white area A1: First calculate the distance probability E r and angular probability E α : Then, according to p(m) i |s j ) = 1 - E r E α Find the probability that point i occupies a cell in the white area A1.

6. The method for real-time environmental perception and autonomous positioning of a high-altitude work robot according to claim 5, characterized in that, When p(m) i |s j When the cell probability exceeds the occupied probability threshold or falls below the idle probability threshold, update the cell occupancy probability: The obtained inverse sensor model value p(m) i |s j Substituting into the Bayesian filter formula below, we can iteratively calculate and predict the probability value of this cell appearing in the next data point: l i,N The logarithmic odd representation of the probability that cell i occupies the Nth data point. i,0 =0; The obtained l i,N Substituting the value into the following formula, we can update the cell occupancy probability to obtain the occupancy probability of the i-th grid cell in the Nth data: When p(m) i |s1,...,s N When the value is greater than or equal to the occupancy probability threshold, the cell is considered to exist as an object, and the cell's occupancy probability is reset to the occupancy probability threshold. When the value is less than or equal to the idle probability threshold, the cell is considered to be empty, and the cell's occupancy probability is reset to the idle probability threshold.

7. The method for real-time environmental perception and autonomous positioning of a high-altitude work robot according to claim 6, characterized in that, Then multiply the occupancy probabilities of each raster cell to obtain the local raster map occupancy probability: M represents the number of cells that make up the large cell.

8. The method for real-time environmental perception and autonomous positioning of a high-altitude work robot according to claim 7, characterized in that, The process of processing 3D point clouds into a 3D occupancy probability map includes: The same method is used to calculate the occupancy probability of the 3D cells, dividing the 3D point cloud into 3D cells. Then, a height parameter, p(m), is added to the inverse sensor model value formula. i |s j ) = O r O α O h With p(m) i |s j ) = 1 - E r E α E h The calculation method is then consistent with the occupancy probability calculation of 2D cells. Finally, the occupancy probability of 3D cells remains unchanged. That is, even if the final result exceeds the occupancy probability threshold or is lower than the idle probability threshold, the occupancy probability threshold and the idle probability threshold will not be used to cover the final result.

9. The method for real-time environmental perception and autonomous positioning of a high-altitude work robot according to claim 1, characterized in that, The 3D ultrasonic sensor starts from the beginning of the i-th cycle, denoted as T. i a The 3D ultrasonic sensor at the end of the i-th cycle is denoted as T. i b ; ΔXi, ΔYi, ΔZi, Δαi, Δβi, Δγi are T i a Time to T i b The displacement and angular relative position change of the 3D ultrasonic sensor within one cycle is given by ΔXi, where Δαi is the displacement change along the X-axis, ΔYi is the displacement change along the Y-axis, Δβi is the angular change around the Y-axis, ΔZi is the displacement change along the Z-axis, and Δγi is the angular change around the Z-axis.

10. The method for real-time environmental perception and autonomous positioning of a high-altitude work robot according to claim 9, characterized in that, The error reduction module obtains the distance L between the voxel and the origin of the 3D ultrasonic sensor coordinate system through the 3D point cloud. Based on the principle that the ultrasonic wave speed is constant in the same medium, the velocity V is obtained. The time t = 2L / V, where t < T, is obtained by the ratio of 2L to V. Due to T i a To T i b Let the time be T, and the distance the ultrasonic sensor moves relative to its previous position be ΔXi. Assuming the robotic arm moves at a constant speed within a very short period, then the actual distance ΔXi moves within t seconds is: The actual relative displacement of the point cloud after processing by the error reduction module, i.e., Δxi, is compared with the displacement at the end of one period T. i b The relative displacement at any given moment must be small; according to the proportion. The values ​​of the other five relative variables are calculated, and then error reduction processing is performed on all the 3D point clouds.

Images