Photovoltaic robot positioning and mapping method based on coarse and fine granularity state estimation

Abstract

The invention discloses a photovoltaic robot positioning and mapping method based on coarse and fine granularity state estimation, and aims to solve the problem that a laser radar odometer is difficult to restrain wrong matching caused by dynamic targets such as operators due to insufficient specular reflection and drifting constraint in strong regular array environments such as a photovoltaic power station and the like. And the problem that a constructed photovoltaic scene map is inaccurate due to low state estimation precision caused by sensitivity to a rough initial predicted value under a constant-speed assumption is solved. Coarse-grained state estimation and fine-grained state estimation are utilized to process the preprocessed point cloud, and the prior state of the photovoltaic robot at the next moment is predicted based on the state of the photovoltaic robot at the current moment in the coarse-grained state estimation; and according to the fine-granularity state estimation, calculating a corresponding posterior state according to a prior state obtained by the coarse-granularity state estimation, comparing the posterior state with a posterior state of a point cloud of a previous key frame, and if a difference value is greater than a threshold value, determining that the point cloud at the next moment is a key frame so as to update a global map of a photovoltaic scene.

Description

Technical Field

[0001] This invention relates to the field of mobile robot mapping, specifically to a method for localization and mapping of photovoltaic robots based on coarse and fine granularity state estimation. Background Technology

[0002] Continuous and reliable state estimation and map building are key capabilities for mobile robots. In large-scale, highly regular array environments such as photovoltaic power plants, traditional lidar odometry methods face severe challenges. LiDAR data exhibits characteristics such as sparseness, repetition, and periodic geometric structures, and robots are prone to sudden accelerations or sharp turns during operation, all of which significantly increase the difficulty of registration and localization.

[0003] Existing lidar odometry frameworks can be divided into iterative nearest point (RIB) frameworks and iterative extended Kalman filter (EPF) frameworks. The RIB framework has a clear mathematical foundation and is simple to implement, but it is sensitive to initial conditions. In the absence of auxiliary sensors, mobile robots typically rely on the assumption of uniform velocity for prediction, but this cannot accurately reflect the complex motion in reality. Inaccurate initial predictions lead to inaccurate point cloud distortion correction, causing the mobile robot to get trapped in local minima and affecting the accuracy of the final state estimation.

[0004] Iterative extended Kalman filtering (EBSD) methods can naturally perform multi-sensor fusion and output state covariance, employing iterative and robust processing for nonlinear and outlier observations. However, they struggle to explicitly distinguish between stable structures and dynamic disturbances within a scene. While these methods perform well in general road or indoor environments, in strongly regular array scenarios like photovoltaic power plants, the numerous parallel structures formed by the photovoltaic panel's metal frame and support beams, along with specular reflection effects, result in insufficient constraints on lateral and heading drift. Furthermore, they are unable to suppress erroneous matching caused by dynamic targets such as operators. Therefore, a robust mapping method is urgently needed that can overcome the aforementioned problems of motion distortion, initial value sensitivity, and structural degradation, while effectively suppressing drift. Summary of the Invention

[0005] To address the problems of inaccurate photovoltaic scene maps caused by insufficient state estimation in existing lidar odometers in strongly regular array environments such as photovoltaic power plants due to specular reflection, drift constraints, difficulty in suppressing erroneous matching caused by dynamic targets such as operators, and sensitivity to coarse initial predictions under the assumption of uniform velocity, this invention proposes a photovoltaic robot localization and mapping method based on coarse and fine granular state estimation.

[0006] The technical solution adopted in this invention is:

[0007] It includes the following steps:

[0008] S1. The photovoltaic robot receives the point cloud of the photovoltaic scene in real time and preprocesses the point cloud at each moment.

[0009] S2, based on the assumption of uniform motion and Coarse-grained state estimation is performed on the photovoltaic robot's state in the point cloud after time-mapping preprocessing to obtain the photovoltaic robot's state at time. Prior states and prior covariance in point clouds at different times. Take any integer;

[0010] S3, Regarding photovoltaic robots Fine-grained state estimation is performed on the prior states and prior covariance in the time-matter point cloud to obtain the state of the photovoltaic robot. Posterior state and posterior covariance in time-matter point clouds;

[0011] S4, Calculation The difference between the posterior state of the photovoltaic robot in the point cloud of the previous time frame and the posterior state of the photovoltaic robot in the point cloud of the previous keyframe is used. If the difference is greater than a threshold, then the photovoltaic robot receives... The point cloud at each moment is determined as a keyframe. The global map of the photovoltaic scene is updated based on the keyframes, and S2-S4 are executed repeatedly until the global map of the photovoltaic scene is obtained.

[0012] The beneficial effects of this invention are as follows:

[0013] This invention proposes coarse-grained and fine-grained state estimation. Coarse-grained state estimation predicts the current state and covariance of the photovoltaic robot based on the previous moment's state and the assumption of uniform motion. A robust point cloud frame to local map matching process is then introduced to correct the predicted state and covariance, resulting in a more accurate current state and covariance. Fine-grained state estimation, building upon coarse-grained estimation, utilizes a robust hierarchical iterative extended Kalman filter to further accurately estimate the photovoltaic robot's state, mitigating geometric degradation and dynamic interference problems in strongly regular array environments. It comprises upper-level motion compensation and lower-level state update, these two modules working collaboratively in an iterative loop. Upper-level motion compensation transforms each point in the current point cloud frame to the coordinate system at the end of the point cloud frame acquisition. Lower-level state update extracts features from the transformed point cloud frame, constructs residuals for each point in the frame based on these features, and uses these residuals to construct a maximum a posteriori probability estimation problem, thus deriving the formula for lower-level state update. The predicted state and covariance of the current frame are refined using an "upper-layer + lower-layer iterative state update" approach. This iteratively corrects point cloud distortion while simultaneously optimizing the state estimation using the corrected point cloud, ultimately achieving effective suppression of lateral and directional drift of the photovoltaic panel and obtaining high-precision state estimation results. The posterior state of the photovoltaic robot in the current frame point cloud is subtracted from the posterior state of the photovoltaic robot in the previous keyframe point cloud. If the difference is greater than a threshold, the current frame is determined as a keyframe. The global map of the photovoltaic scene is updated based on the keyframes until the global map of the photovoltaic scene is obtained.

[0014] This invention proposes a coarse-to-fine lidar odometry framework. This framework utilizes observations from the previous frame, local map, and global map at each level, and fuses multi-level features through a robust hierarchical iterative extended Kalman filter. It is specifically designed around the geometric characteristics and interference modes of photovoltaic power plants, and finally completes the task of accurate localization and mapping of photovoltaic robots in photovoltaic scenarios. Attached Figure Description

[0015] Figure 1 This is a flowchart of the present invention;

[0016] Figure 2 This is a schematic diagram of point-to-surface residuals; Detailed Implementation

[0017] Specific implementation method one: Combining Figures 1-2 This embodiment describes a photovoltaic robot localization and mapping method based on coarse-grained state estimation. The main objective of this invention is to solve for the global pose of the photovoltaic robot and construct a scene map based on continuously input LiDAR point clouds in a photovoltaic scene. It includes the following steps:

[0018] S1. LiDAR point cloud preprocessing:

[0019] Photovoltaic robots receive real-time lidar point clouds of photovoltaic scenes. , the initial time The corresponding lidar coordinate system is used as the global coordinate system. For lidar, although laser emission and reception are very fast, the points constituting the point cloud are not generated at the same time. Therefore, this invention groups the accumulated point cloud data within 0.1 seconds into a single frame of point cloud output, defining the photovoltaic robot's position within this frame. The lidar point cloud received at each moment is the first of all point clouds. Frame. The received LiDAR point cloud at each time step is preprocessed sequentially using the nearest neighbor filtering algorithm and the voxel downsampling algorithm to obtain the preprocessed LiDAR point cloud at each time step. The nearest neighbor filtering algorithm is used to eliminate points within a 1-meter radius of the LiDAR sensor, reducing interference from operators and the photovoltaic robot carrying the LiDAR. The voxel downsampling algorithm is set to a voxel resolution of 0.5m in outdoor environments.

[0020] S2. Based on the assumption of uniform motion and the state of the photovoltaic robot in the preprocessed lidar point cloud at each time step, coarse-grained state estimation is performed to obtain the prior state and prior covariance of the photovoltaic robot in the lidar point cloud at the next time step. The specific process is as follows:

[0021] To address the coarsness caused by the uniform velocity assumption and optimize the initial value, this invention proposes a coarse-grained state estimation, which is divided into two stages: the first stage is forward propagation, and the second stage is prediction correction.

[0022] The main objective of Phase One is to predict the pose of the photovoltaic robot at the current moment based on its state and the assumption of uniform motion from the previous moment. First, the photovoltaic robot... ( The state of the point cloud after preprocessing at any integer time (take any integer). Includes rotation matrix Translation vector Linear velocity vector and angular velocity vector ,Right now:

[0023] (1)

[0024] In equation (1), This indicates transpose.

[0025] Secondly, the forward propagation is defined based on the nonlinear state function. Using photovoltaic robots Posterior state at time step and posterior covariance Predicting photovoltaic robots Prior state at time and prior covariance The forward propagation formula is as follows:

[0026] (2)

[0027] (3)

[0028] In equations (2) and (3), For noise vectors, , This is the linear velocity noise vector. This is the angular velocity noise vector. It is a nonlinear state function. For time intervals, . for Time error state Compared to Time error state The Jacobian matrix. for The transpose of . for Time error state Compared to Time-of-flight noise vector The Jacobian matrix. for The covariance matrix. for The transpose of . This is a custom operator used to perform incremental computation on a manifold. The introduction of the manifold allows the 3D rigid body attitude transformation to be converted into matrix multiplication, thus simplifying the expression and calculation of rotations. The specific calculation formula is shown below:

[0029] (4)

[0030] In equation (4), Denotes a special orthogonal group. Represents a vector. Represents an exponential mapping. , It is an identity matrix.

[0031] In the motion scenario of the photovoltaic robot, the state of the photovoltaic robot under the assumption of uniform motion. rotation matrix and translation vector If there are strong coupling constraints, then a nonlinear state function is defined. for:

[0032] (5)

[0033] In equation (5), posterior state Includes angular velocity vectors, posterior state Includes rotation matrices, posterior state Includes linear velocity vectors, for The reverse.

[0034] (6)

[0035] (7)

[0036] Photovoltaic robots The error state at time t is defined as follows:

[0037] (8)

[0038] In equation (8), for Rotation error at time, for Translation error at time, for Linear velocity error at time t, for Angular velocity error at time t, This is a custom operator used to perform "subtraction" calculations on a manifold, defined as follows:

[0039] (9)

[0040] In equation (9), express The inverse mapping of .

[0041] Phase Two: Predictive Correction

[0042] Considering that the uniform motion assumption in Stage 1 is not entirely consistent with the actual motion, in order to correct the coarse prediction in Stage 1, this invention introduces a matching process between point cloud frames and known local maps that is robust to the initial values. This process aims to obtain the pose increment of each frame of point cloud relative to the corresponding local map, which is expressed as minimizing the following loss function:

[0043] (10)

[0044] In equation (10), For the first Frame point cloud midpoint and Corresponding points on the local map The distance between them , for Position at any given moment , for The covariance matrix of the distribution of surrounding neighboring points. for The covariance matrix of the distribution of surrounding neighboring points. for The reverse, for The transpose of .

[0045] By minimizing equation (10), the photovoltaic robot is finally obtained. optimal pose at time Let the independent pose observation be denoted as , , for The transpose. Photovoltaic robots use pose observation. The results obtained in the first correction stage Prior state at time and prior covariance This allows for a more robust and accurate state estimate without relying on additional sensors. The correction process is as follows:

[0046] (11)

[0047] (12)

[0048] (13)

[0049] (14)

[0050] in, for transpose, To observe the covariance, it is approximated by the Hessian matrix of the frame-to-local map matching results. For the corrected The prior state at any given moment. For the corrected Prior covariance at time, Indicates reversal.

[0051] The final output of the above coarse-grained state estimation is the corrected value. Prior state at time and prior covariance , Compared to Phase One It is a more accurate and robust state estimate. This will be used as prior input to the next step of fine-grained state estimation for the final state update.

[0052] S3, to Fine-grained state estimation is performed on the prior state and prior covariance of the photovoltaic robot at each moment, resulting in... The specific process for determining the posterior state and posterior covariance of the photovoltaic robot at any given time is as follows:

[0053] Fine-grained state estimation builds upon coarse-grained state estimation by utilizing a robust hierarchical iterative extended Kalman filter to further refine the state of the photovoltaic robot, mitigating geometric degradation and dynamic disturbances in strongly regular array environments. Unlike methods based solely on point-to-surface residuals, the robust hierarchical iterative extended Kalman filter in this invention explicitly incorporates the stability line characteristics of the photovoltaic scene. It comprises two key modules: upper-level motion compensation and lower-level state update. These two modules work collaboratively in an iterative loop until the state estimation converges. The detailed execution stages are as follows:

[0054] Phase 1: Upper-level motion compensation

[0055] During the scanning of a point cloud frame by the lidar (e.g., 0.1 seconds), the photovoltaic robot itself is in motion, which causes motion distortion in the acquired point cloud. To improve the accuracy of state estimation within the hierarchical iterative extended Kalman filter, this invention iteratively utilizes the updated state in the lower-level processing to correct distortion in the upper-level iteration.

[0056] Since this invention defines a point cloud frame to be acquired at a given moment, therefore The point cloud collected at each moment can be regarded as the first Frame point cloud. For the first frame... any point in a frame cloud The photovoltaic robot can calculate the point through linear interpolation. corresponding Precise pose of the real-time lidar :

[0057] (15)

[0058] in, , , from From which, This indicates the iteration number. This interpolation process is performed by the photovoltaic robot on the [number]th iteration. Posterior pose in frame point cloud (from (obtained from) and the first The latest estimated pose of the frame point cloud obtained in the lower-level state update It was carried out between them.

[0059] Finally, the photovoltaic robot uses formula (16) to place the point Transform to the End of frame point cloud In the corresponding lidar coordinate system, it is represented as :

[0060] (16)

[0061] According to equations (15) and (16), the first... Transformation of all points in the frame point cloud to the end time In the corresponding lidar coordinate system, the new first... Frame point cloud. In this step, the accuracy of distortion correction depends on the accuracy of the underlying state update; therefore, as iterations proceed, the state estimation... It's becoming more and more accurate, and the effect of point cloud distortion removal is getting better and better.

[0062] Phase Two: Lower-Level State Update

[0063] First, photovoltaic robots for the new In the point cloud, each point performs a nearest neighbor search in its corresponding local map, followed by local plane fitting to obtain the plane corresponding to each point. Combining the planes corresponding to all points yields a new first plane. Planar features of frame point clouds.

[0064] The photovoltaic robot can also robustly extract stable line features from the metal frames of the photovoltaic panels and the support beams in strongly regular array environments such as photovoltaic power plants. These line features have stable geometry and orientation across multiple frame point clouds, and their main direction is highly correlated with the arrangement height of the photovoltaic array, providing strong constraints for subsequent suppression of lateral and heading drift. The support beams are the structures that provide support for the photovoltaic panels. The specific process for extracting line features is as follows:

[0065] Photovoltaic robots for the new In the point cloud, for each point, a nearest neighbor search is performed in its corresponding local map to obtain a neighborhood point set. The neighborhood covariance matrix is ​​calculated based on the neighborhood point set, and eigenvalues ​​are obtained by eigenvalue decomposition of the neighborhood covariance matrix. , and And the eigenvector corresponding to each eigenvalue, Select the eigenvector with the largest sum. Line segments with the same direction are selected as candidate line segments. Considering the structure of the photovoltaic panel's metal frame and support beam, candidate line segments with an angle of less than 5° between their direction and the photovoltaic array's arrangement direction are removed. This yields new candidate line segments for each point. These new candidate line segments are then combined to obtain the new... Linear features of frame point clouds.

[0066] Based on the extracted planar and line features, the photovoltaic robot is calculated and updated in the new... The state in the frame point cloud, specifically the process is as follows:

[0067] The photovoltaic robot is the new first in the local map. For each point in the frame point cloud, the following two types of residuals are simultaneously constructed and jointly optimized:

[0068] (1) Constructing point-to-surface residuals based on planar features. Specifically, based on planar features, for the new... For each point in the frame point cloud, the vector from that point to any point on the corresponding plane is projected onto the normal direction of that plane, and the resulting projection length (distance from the point to the plane) is used as the point-to-plane residual. For example... Figure 2 As shown.

[0069] (2) Constructing point-line residuals based on line features. Specifically, based on S332, for the new... For each point in the frame point cloud, the vector from that point to any endpoint of the corresponding line segment is projected onto a plane perpendicular to the direction of the line segment. The resulting projection length (distance from the point to the line segment) is used as the point-line residual. For example, assuming P is a point in the point cloud, the point-line residual is the distance from point P to line segment AB in space. First, starting from endpoint A of the line segment, vectors AP and AB are constructed; then, the magnitude of the cross product of vectors AP and AB is calculated and divided by the magnitude of vector AB to obtain the final point-line residual. The point-line residual provides stronger geometric constraints in the lateral (perpendicular to the direction of robot movement) and lateral (direction of robot movement) directions of a single photovoltaic panel, effectively suppressing drift during long-term operation, especially maintaining stable state estimation when there is a lack of ground height differences or significant corner points. The point-plane residual and point-line residual corresponding to each point are weighted and fitted, with the weights taken from the reciprocal of their respective observation covariances, to obtain the jointly optimized residual for each point. Next, based on the residuals obtained from joint optimization at each point, the photovoltaic robot state estimation problem is constructed into a maximum a posteriori probability estimation problem, as shown below:

[0070] (17)

[0071] In equation (17), Indicates photovoltaic robot The true state at any given moment Indicates the first The residual after joint optimization at each point in the next iteration Indicates the first In the next iteration, the residual after joint optimization at each point relates to the error state. Jacobian matrix, Indicates the first The covariance matrix of the residuals after joint optimization at each point in the next iteration is used to characterize the reliability of the residuals.

[0072] Based on equation (17), the lower-level state update formula is obtained as follows:

[0073] (18)

[0074] (19)

[0075] (20)

[0076] in, , Representing photovoltaic robots Time of the first The estimated states before and after each iteration. Formulas (18)-(20) directly incorporate scene structure information into the filtering process to achieve the final update of the state.

[0077] Subsequently, it is determined whether the lower-level state update iteration has ended. If the update amount of the state estimate obtained in the current iteration is less than the update amount of the state estimate obtained in the previous complete "upper-level + lower-level iteration state update", the lower-level state iteration ends, and the photovoltaic robot outputs... The final posterior state at time [time] and the final posterior covariance Conversely, the photovoltaic robot will update the lower layer to obtain a new state. The process is then passed back to the upper layer, and the upper-layer compensation and lower-layer iterative update are repeated until the lower-layer state iteration termination condition is met.

[0078] When the iteration of the next layer state ends, the final covariance is obtained:

[0079] (twenty one)

[0080] In summary, fine-grained state estimation is an iterative optimization process. Through close cooperation between the upper and lower layers, it iteratively corrects point cloud distortion while using the corrected point cloud to iteratively optimize the state estimation. Ultimately, it effectively suppresses the lateral and directional drift of the photovoltaic panel and obtains high-precision state estimation results.

[0081] S4, Calculation The difference between the posterior state of the photovoltaic robot at a given time and the posterior state of the photovoltaic robot in the point cloud of the previous keyframe is used. If the difference is greater than a threshold, then the photovoltaic robot receives... The point cloud at each moment is identified as a keyframe. The global map of the photovoltaic scene is updated based on these keyframes. S2-S4 are then repeated to obtain the global map of the photovoltaic scene. The specific process is as follows:

[0082] After fine-grained state estimation, the photovoltaic robot will base its state on the posterior state of the photovoltaic robot in the point cloud of the previous keyframe and... The difference between the posterior states of the photovoltaic robot at any given time is used to determine whether the current two frames of point cloud meet the conditions of a keyframe.

[0083] like At any given moment, the translation of the photovoltaic robot in the point cloud compared to the previous keyframe is greater than the translation threshold or If, at a given moment, the rotation of the photovoltaic robot in the point cloud compared to the previous keyframe is greater than a rotation threshold, then the photovoltaic robot receives the [number]th [keyframe]. Frame point clouds are identified as keyframes, and the photovoltaic robot updates the local map based on these keyframes. Conversely, the first frame... Frame point clouds are not keyframes.

[0084] To address the regular arrangement of photovoltaic panels in photovoltaic power plant scenarios, translation and rotation thresholds are set separately based on the array arrangement direction: the translation threshold along the photovoltaic array arrangement direction is set to 0.5m, and the translation threshold in the lateral direction of the photovoltaic array arrangement is set to 0.05m to enhance the constraint on lateral drift. For the rotation threshold, the deflection angle of the photovoltaic robot's forward direction between two frames is calculated using a rotation matrix, and a 1... The rotation angle threshold is set to further improve map consistency in array environments.

[0085] The local map of the photovoltaic scene is updated based on the keyframes. After the local map is updated as a whole, it is used for the prediction and correction stage in the subsequent coarse-grained state estimation. The updated local map is then incorporated into the global map managed by ikd-Tree. The above process is repeated until the global map of the photovoltaic scene is obtained.

[0086] The above examples of this invention are merely illustrative of the computational model and process of this invention, and are not intended to limit the implementation of this invention. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is impossible to exhaustively list all possible implementations here. Any obvious variations or modifications derived from the technical solutions of this invention are still within the scope of protection of this invention.

Claims

1. A photovoltaic robot localization and mapping method based on coarse-grained and fine-grained state estimation, characterized in that: It includes the following steps: S1. The photovoltaic robot receives the point cloud of the photovoltaic scene in real time and preprocesses the point cloud at each moment. S2, based on the assumption of uniform motion and Coarse-grained state estimation is performed on the photovoltaic robot's state in the point cloud after time-mapping preprocessing to obtain the photovoltaic robot's state at time. Prior states and prior covariance in point clouds at different times. Take any integer; S3, Regarding photovoltaic robots Fine-grained state estimation is performed on the prior states and prior covariance in the time-matter point cloud to obtain the state of the photovoltaic robot. Posterior state and posterior covariance in time-matter point clouds; S4, Calculation The difference between the posterior state of the photovoltaic robot in the point cloud of the previous time frame and the posterior state of the photovoltaic robot in the point cloud of the previous keyframe is used. If the difference is greater than a threshold, then the photovoltaic robot receives... The point cloud at each moment is determined as a keyframe. The global map of the photovoltaic scene is updated based on the keyframes, and S2-S4 are executed repeatedly until the global map of the photovoltaic scene is obtained.

2. The photovoltaic robot localization and mapping method based on coarse-grained state estimation according to claim 1, characterized in that: The preprocessing methods in S1 include a nearest neighbor filtering algorithm and a voxel downsampling algorithm.

3. The photovoltaic robot localization and mapping method based on coarse-grained state estimation according to claim 2, characterized in that: The specific process of S2 is as follows: S21. Define coarse-grained state estimation as including forward propagation and prediction correction; S22, The forward propagation formula is: （1） （2） in, for The prior state of the photovoltaic robot in the point cloud at any given moment. for The posterior state of the photovoltaic robot in the point cloud after time-mapping, wherein the state includes rotation matrix, translation vector, linear velocity vector, and angular velocity vector. These are custom operators used to perform addition calculations on manifolds. It is a nonlinear state function. For noise vectors, , This is the linear velocity noise vector. This is the angular velocity noise vector. Indicates transpose. , for Prior covariance of photovoltaic robots in point cloud, for The posterior covariance of the photovoltaic robot in the point cloud after time-mapping. for Time error state Compared to Time error state Jacobian matrix, for transpose, for Time error state Compared to Time-of-flight noise vector Jacobian matrix, for The covariance matrix, for transpose; S23, Predictive Correction: （3） in, For the first Frame point cloud midpoint and Corresponding points on the local map The distance between, the first Frame point cloud Point clouds of time, , for Position at any given moment , for Status of photovoltaic robot at all times Included rotation matrix, for Status of photovoltaic robot at all times Included translation vectors, for The covariance matrix of the distribution of surrounding neighboring points. for The covariance matrix of the distribution of surrounding neighboring points. for The reverse, for transpose; The photovoltaic robot is obtained by minimizing equation (3). optimal pose at time ,remember , for Transpose, using Correction and The specific process is as follows: （4） （5） （6） （7） in, for transpose, To observe the covariance, it is approximated by the Hessian matrix of the frame-to-local map matching results. For the corrected The prior state at any given moment. For the corrected Prior covariance at time, for The reverse.

4. The photovoltaic robot localization and mapping method based on coarse-grained state estimation according to claim 3, characterized in that: The custom operator in S22 The specific calculation formula is as follows: （8） in, Denotes a special orthogonal group. Represents a vector. Indicates an exponential mapping. , It is an identity matrix.

5. The photovoltaic robot localization and mapping method based on coarse-grained state estimation according to claim 4, characterized in that: The nonlinear state function in S22 for: （9） in, posterior state Includes angular velocity vectors, posterior state Includes rotation matrices, posterior state Includes linear velocity vectors, for The reverse.

6. The photovoltaic robot localization and mapping method based on coarse-grained state estimation according to claim 5, characterized in that: The Jacobian matrix in S22 The calculation formula is: （10） The Jacobian matrix The calculation formula is: （11） The error state The calculation formula is: （12） in, for Rotation error at any moment, for Translation error at time, for Linear velocity error at time t, for Angular velocity error at time t, This is a custom operator used to perform subtraction on a manifold, expressed as: （13） in, express The inverse mapping of .

7. The photovoltaic robot localization and mapping method based on coarse-grained state estimation according to claim 6, characterized in that: The specific process of S3 is as follows: S31. Define fine-grained state estimation as including upper-level motion compensation and lower-level state update, and let the number of iterations be... ; S32, Upper-layer motion compensation: For the any point in a frame cloud The photovoltaic robot obtains the point through linear interpolation calculation. corresponding Precise pose of the real-time lidar : （14） in, For the first The posterior pose of the photovoltaic robot in the frame point cloud, the first Frame point cloud Point clouds of time, , , For the first The latest estimated pose of the frame point cloud obtained in the lower-level state update is obtained by get, This represents the number of iterations. Use formula (15) to point Transform to the End of frame point cloud In the corresponding lidar coordinate system, it is represented as : （15） in, for The reverse; According to equations (14) and (15), the first Transformation of all points in the frame point cloud to the end time In the corresponding lidar coordinate system, the new first... Frame point cloud; S33, Lower-level state update: S331, Photovoltaic Robots for New Generation In the point cloud of a frame, for each point, a nearest neighbor search is performed in the local map corresponding to that point, followed by local plane fitting to obtain the plane corresponding to each point. The planes corresponding to all points are then combined to obtain a new [frame name missing]. Planar features of frame point clouds; S332. Define the linear feature formed between the metal frame of the photovoltaic panel and the support beam. The specific process for obtaining the linear feature is as follows: Photovoltaic robots for the new For each point in the frame point cloud, a nearest neighbor search is performed on the local map corresponding to that point to obtain a neighborhood point set. The neighborhood covariance matrix is ​​calculated based on the neighborhood point set, and eigenvalues ​​are obtained by eigenvalue decomposition of the neighborhood covariance matrix. , and And the eigenvector corresponding to each eigenvalue, Select the eigenvector with the largest sum. Line segments with the same direction are selected as candidate line segments. Candidate line segments whose direction makes an angle less than 5° with the photovoltaic array arrangement direction are removed. New candidate line segments are obtained for each point. All new candidate line segments are combined to obtain the new [number] line segment. Line features of frame point clouds; S333. Based on the planar features and the line features, calculate and update the photovoltaic robot in the new... The state in the frame point cloud, specifically the process is as follows: New First For each point in the frame point cloud, the local map corresponding to that point simultaneously constructs and jointly optimizes the following two types of residuals: (1) Constructing point-to-surface residuals based on planar features: Based on S331, the new first The projection length of the vector from each point in the frame point cloud to any point on the plane corresponding to that point onto the normal direction of the plane is used as the point-to-plane residual. (2) Constructing point-line residuals based on line features: Based on S332, the new first The vector from each point in the frame point cloud to any endpoint of the corresponding line segment is projected onto a plane perpendicular to the direction of the line segment, and the resulting projection length is used as the point-line residual. The point-to-surface residual and point-to-line residual corresponding to each point are weighted and fitted, with the weights being the reciprocal of the observation covariance of each residual, to obtain the joint optimized residual for each point; Based on the correction Prior state at time Prior covariance By combining the residuals from joint optimization at each point, the photovoltaic robot state estimation problem is constructed as a maximum a posteriori probability estimation problem, yielding the following expression: （16） in, Indicates photovoltaic robot The true state at any given moment Indicates the first The residual after joint optimization at each point in the next iteration Indicates the first In the next iteration, the residual after joint optimization at each point relates to the error state. Jacobian matrix, Indicates the first The covariance matrix of the residuals after joint optimization at each point in the next iteration. for The reverse; Based on equation (16), the lower-level state update formula is obtained: （17） （18） （19） in, and The photovoltaic robot is the first The estimated states before and after the next iteration. Indicates transpose. Indicates reversal; S34, if the first If the update amount of the state obtained in the next iteration is less than the threshold compared to the state obtained in the previous upper and lower iterations, the lower iteration update ends, and the photovoltaic robot outputs. Posterior state at time step and posterior covariance ; Conversely, make , using the Repeat steps S32 to S33 to obtain the state obtained in the second iteration. The state of the next iteration is used to repeat S34 until the condition of the next iteration is met. If the update amount of the state obtained in the next iteration is less than the threshold compared to the state obtained in the previous upper and lower iterations, the lower iteration update ends, and the photovoltaic robot outputs. Posterior state at time step and posterior covariance .

8. The photovoltaic robot localization and mapping method based on coarse-grained state estimation according to claim 7, characterized in that: The thresholds in S4 include translation thresholds and rotation thresholds.

9. The photovoltaic robot localization and mapping method based on coarse-grained state estimation according to claim 8, characterized in that: The translation threshold along the photovoltaic array arrangement direction is set to 0.5m, the translation threshold in the lateral direction of the photovoltaic array arrangement is set to 0.05m, and the rotation threshold is set to 1°.

10. The photovoltaic robot localization and mapping method based on coarse-grained state estimation according to claim 9, characterized in that: The specific process of S4 is as follows: Based on the posterior state of the photovoltaic robot in the point cloud of the previous keyframe and The posterior state of the photovoltaic robot at any given time, if At any given moment, the translation of the photovoltaic robot in the point cloud compared to the previous keyframe is greater than the translation threshold or If, at a given moment, the rotation of the photovoltaic robot in the point cloud compared to the previous keyframe is greater than a rotation threshold, then the photovoltaic robot receives the [number]th [keyframe]. Frame point clouds are identified as keyframes. The local map of the photovoltaic scene is updated based on the keyframes. After the local map is updated, the global map is updated with the updated local map to obtain the global map of the photovoltaic scene.

Images