A radar holder self-adaptive super-helix control method based on RMSE speed planning

Abstract

An adaptive superspiral control method for radar gimbals based on RMSE velocity planning is presented, belonging to the field of lidar mapping and motion control technology. This invention solves the problem of unstable dynamic lidar mapping results caused by existing radar gimbal control methods that do not consider the stitching effect of point cloud data and have poor robustness. First, coarse matching is performed on feature points in two adjacent frames of point cloud data. Then, fine matching is performed based on generalized ICP, and RMSE is calculated based on the fine matching results. The rotational speed adjustment rate is designed according to the fitting equation between the radar gimbal rotation speed and RMSE, and a superspiral expansion state observer is used to observe the disturbances experienced by the radar gimbal. Finally, an adaptive superspiral controller is designed based on the disturbance observation results and the rotational speed adjustment rate based on RMSE. The controller's output drives the radar gimbal rotation and continues scanning to obtain the next frame of point cloud data until the entire scanning process is completed. This method can be applied to dynamic lidar mapping tasks in complex large-scale scenes.

Description

Technical Field

[0001] This invention belongs to the field of lidar mapping and motion control technology, specifically relating to a radar gimbal adaptive superspiral control method based on RMSE velocity planning. Background Technology

[0002] With the development of automation and intelligent technologies, lidar is increasingly widely used in industrial production and scientific research. In particular, lidar-based dynamic mapping technology has become a key means for intelligent management and automated measurement in large-scale scenarios. In complex and large-scale application scenarios such as open-air coal bunkers, large farmlands, and construction facilities, the assistance of mobile platforms is indispensable to ensure that lidar can comprehensively scan terrain, materials, and other scene information. By mounting lidar on mobile platforms (such as turntables, drones, and robots), the mobile platform can drive the lidar to perform dynamic scanning, acquiring real-time 3D point cloud data of the work site, thereby achieving accurate measurement and 3D reconstruction of complex and large-scale scenes. However, in actual large-scale scene dynamic mapping, the lack of speed planning in the lidar gimbal motion control process and the mismatch between the point cloud stitching process can easily lead to instability in lidar dynamic mapping. Furthermore, external environmental disturbances and uncertainties also require the lidar gimbal control system to have good robustness and stability to ensure the safe and stable operation of the lidar gimbal in dynamically changing environments. Therefore, the stitching effect of point cloud data from lidar directly affects the accuracy and completeness of dynamic mapping. It is necessary to adaptively adjust the control process of the lidar gimbal based on the stitching effect of the point cloud data, which places higher demands on the motion control strategy of the lidar gimbal.

[0003] Therefore, how to design a radar gimbal motion control strategy that considers both the point cloud data stitching effect and high robustness has become an urgent problem to be solved in large-scene lidar dynamic mapping tasks. Summary of the Invention

[0004] The purpose of this invention is to solve the problem of unstable dynamic mapping effect of lidar caused by the lack of consideration for the stitching effect of point cloud data and poor robustness of existing radar gimbal control methods. Therefore, an adaptive superspiral control method for radar gimbal based on RMSE velocity planning is proposed.

[0005] The technical solution adopted by this invention to solve the above-mentioned technical problems is: an adaptive super-helical control method for radar gimbals based on RMSE velocity planning, the method specifically including the following steps:

[0006] Step 1: Preprocess the point cloud data of the kth frame obtained by the lidar scan to obtain the edge feature points and planar feature points in the kth frame;

[0007] Step 2: After the radar pan-tilt unit rotates, the lidar continues to scan and obtain the point cloud data of the (k+1)th frame. After preprocessing the point cloud data of the (k+1)th frame, the edge feature points and planar feature points in the (k+1)th frame are obtained.

[0008] Step 3: Perform coarse matching on the feature points extracted in Step 1 and Step 2, then perform fine matching based on generalized ICP, and calculate RMSE based on the fine matching results.

[0009] Step 4: Establish a fitting equation based on the relationship between radar gimbal rotation speed and RMSE, and design the radar gimbal rotation speed regulation rate based on the fitting equation and the RMSE calculated in Step 3.

[0010] Step 5: Use a superspiral expansion state observer to observe the disturbances experienced by the radar gimbal during rotation, and design an adaptive superspiral controller based on the disturbance observation results and the rotational speed regulation rate based on RMSE.

[0011] Use the output of the adaptive superspiral controller to drive the radar gimbal to rotate. Let k = k + 1, and return to step two.

[0012] Furthermore, the specific process of step one is as follows:

[0013] Calculate the smoothness c of point i in the kth frame:

[0014]

[0015] Among them, X (k,i) Let X be the three-dimensional coordinates of point i in the lidar coordinate system of the k-th frame. (k,j) Let J be the three-dimensional coordinates of point J in the lidar coordinate system of the k-th frame, ||·||2 is the L2 norm, S is the set of neighborhood points of point i, and |S| is the total number of points in set S.

[0016] If the smoothness c is greater than or equal to α1, then point i is an edge feature point;

[0017] If α2 < c < α1, then point i is a feature point of the plane;

[0018] Similarly, the smoothness of each point in the k-th frame is calculated to obtain the edge feature points and planar feature points in the k-th frame.

[0019] Furthermore, the total number of points |S| in the set S is:

[0020]

[0021] Where L is the arc length, h′ is the horizontal scanning resolution, and r is the scanning distance.

[0022] Furthermore, in step three, coarse matching is performed on the feature points extracted in steps one and two, specifically as follows:

[0023] Based on the point-edge distance function, an edge feature constraint error function model f is established. ε A planar feature constraint error function model f is established based on the point-plane distance function. h :

[0024]

[0025]

[0026] Where, d ε It is the point-edge distance function, d h It is a point-to-plane distance function. Let be the pose transformation matrix between frame (k+1) and frame (k).

[0027] For the m-th edge feature point in the (k+1)-th frame, according to Obtain the edge line corresponding to the m-th edge feature point in the k-th frame, and denote the two endpoints of the edge line as point n and point q, respectively. Then calculate the distance from the m-th edge feature point in the (k+1)-th frame to the edge line in the k-th frame. That is, express the point-edge feature function as:

[0028]

[0029] Among them, X (k+1,m) X is the three-dimensional coordinate of point m in the lidar coordinate system of the (k+1)th frame. (k,n) X is the three-dimensional coordinate of edge feature point n in the lidar coordinate system of the k-th frame. (k,q) It is the three-dimensional coordinate of the edge feature point q in the lidar coordinate system of the k-th frame, |X (k,n) -X (k,q) | represents X (k,n) With X (k,q) Euclidean distance;

[0030] For the m′-th planar feature point in the (k+1)-th frame, according to Obtain the feature plane corresponding to the m′-th planar feature point in the k-th frame. Label any three planar feature points on the feature plane as n′, q′, and r′, respectively. Then calculate the distance from the m′-th planar feature point in the (k+1)-th frame to the feature plane in the k-th frame. This can be expressed as the point-plane feature function:

[0031]

[0032] Among them, X (k,n′) X is the three-dimensional coordinate of the planar feature point n′ in the lidar coordinate system of the k-th frame.(k,m′) X is the three-dimensional coordinate of the planar feature point m′ in the lidar coordinate system of the k-th frame. (k,q′) X is the three-dimensional coordinate of the planar feature point q′ in the lidar coordinate system of the k-th frame. (k,r′) X is the three-dimensional coordinate of the planar feature point r′ in the lidar coordinate system of the k-th frame. (k+1,m′) It is the three-dimensional coordinate of point m′ in the lidar coordinate system of the (k+1)th frame;

[0033] To obtain the coarse matching matrix The optimization calculation is performed using the following formula:

[0034]

[0035] Among them, f ε f is the set of all edge feature points in the k-th frame. h It is the set of all planar feature points in the k-th frame.

[0036] Furthermore, the fine matching based on the coarse matching result specifically includes:

[0037] The matrix obtained from coarse matching As the initial pose transformation matrix for the generalized ICP algorithm, a fine matching is then performed based on the generalized ICP algorithm to calculate the pose transformation matrix obtained from the fine matching:

[0038]

[0039] Where, p i It is the i-th point in the k-th frame, q j For the j-th point in the (k+1)-th frame, n j For q j The unit normal vector at point w ij To match point pairs (p i ,q j The weights of ) are given by the superscript T, which represents transpose.

[0040] Furthermore, the calculation of the root mean square error based on the fine matching result is specifically as follows:

[0041]

[0042] Where, d i It is a matching point pair (p) i ,q j Point p in ) i and point q j The distance between them, where N is the total number of matched point pairs.

[0043] Furthermore, the specific process of step four is as follows:

[0044] Step 41: Establish the fitting equation from radar gimbal rotation speed ω to RMSE:

[0045]

[0046] Where R(ω) is the fitting equation from angular velocity ω to RMSE, R t ω t η1 and η2 are the parameters of the fitted equation;

[0047] Step 4.2: Set the fitting equation from rotational speed ω to RMSE as follows:

[0048] R r (ω)=R rc (ω)+d R (11)

[0049] Among them, R r (ω) represents the RMSE after considering the perturbation, and R rc The form of (ω) is the same as that of R(ω), d R The disturbance component;

[0050] Step 43: Set the expected RMSE t With preset speed ω s , let R t For RMSE t ω s For ω t Substitute the RMSE calculated in step three into equation (11), and then use R... -1 (ω) Inverse solution yields the rotational speed

[0051] Step 44, according to Design speed regulation rate ω r :

[0052]

[0053] Where f(·) is the adjustment function and ω is the actual rotational speed;

[0054] If the regulation function is taken as a PI controller, then the speed regulation rate is:

[0055]

[0056] Where, k ωp and k ωi These are the parameters for the PI controller.

[0057] Furthermore, the specific process of step five is as follows:

[0058] Step 51: The system model of the radar pan-tilt unit is as follows:

[0059]

[0060] Where θ is the actual rotation angle of the radar pan-tilt unit. It is the first derivative of θ. ω is the first derivative of ω, d is the disturbance experienced by the radar gimbal, and h is the rate of change of the disturbance experienced by the radar gimbal. It is the first derivative of d, where K and T are constants;

[0061] The superspiral expansion state observer of the system model is designed as follows:

[0062]

[0063] Where z1 is the observation value of θ by the superspiral expansion state observer. Z1 is the first derivative of z1, and z2 is the observation of ω by the superspiral expansion state observer. z2 is the first derivative of z2, and z3 is the observation of d by the superspiral expansion state observer. It is the first derivative of z3, α1, α2, α3, α4 and α5 are the coefficients of the superspiral expansion state observer, e1 = z1 - θ m θ m For the measured rotation angle of the radar pan-tilt unit, sgn(·) is the sign function, ω m The measured rotational speed includes noise.

[0064] The observed value z2 is compared with the ω measured by the radar gimbal measuring element. m A weighted average filter is applied to obtain a smoothed rotational speed ω. a :

[0065] ω a =wω m +(1-w)z2 (16)

[0066] Where w is the fusion coefficient;

[0067] Step 52: Based on the RMSE speed planning results and the smoothed rotational speed, the rotational speed error e of the radar gimbal is obtained as follows:

[0068] e = ω a -ω r (17)

[0069] Subsequently, the output of the radar gimbal adaptive superspiral controller was designed as follows:

[0070]

[0071] Where u is the output of the controller. It is ωr The first derivatives, ε1, ε2, ε3, ε4, and ε5 are the adjustable parameters of the controller. is u a The first derivative, k c For adaptive parameters;

[0072]

[0073]

[0074] Furthermore, the coefficients α1, α2, and α3 satisfy:

[0075]

[0076] Among them, L h It is the upper bound of the rate of change h of the disturbance experienced by the radar pan-tilt unit.

[0077] The beneficial effects of this invention are:

[0078] (1) A fitting equation between RMSE and gimbal rotation speed ω was established, realizing adaptive rotation speed planning for the radar gimbal.

[0079] This invention quantifies and fits the relationship between radar gimbal rotation speed ω and mapping evaluation RMSE in practice. A piecewise, adjustable-parameter fitting function is designed to convert between radar gimbal rotation speed ω and RMSE. Based on this, a speed planning scheme is designed to adjust the radar gimbal rotation speed based on a specified RMSE index, thereby optimizing the point cloud stitching quality and ensuring effective mapping.

[0080] (2) A novel superspiral expansion state observer was designed, which enabled rapid and accurate estimation of the rotational speed and disturbance of the radar gimbal.

[0081] This invention designs a novel superspiral expansion state observer that can overcome the adverse effects of complex dynamic environments on the measurement and control of control systems. The designed observer can quickly and accurately observe the current state variables of the system. In complex and noisy environments, radar gimbal rotation speed measurement data are generally severely interfered with, while the superspiral expansion state observer can generate a smooth observation value of the system rotation speed. It can also perform weighted data fusion on the actual system rotation speed observation value to produce a filtering effect, thereby facilitating the use of more accurate and smooth angular velocities in the controller.

[0082] (3) A novel adaptive superspiral controller was designed to achieve smooth and robust motion control of the radar gimbal.

[0083] This invention designs a novel adaptive superspiral controller. Compared with the traditional superspiral controller, the adaptive superspiral controller of this invention adds a linear term, which improves the convergence speed of the error. The controller has an adaptive coefficient related to the rotational speed error, which can achieve a larger convergence speed when the error is large, and can also reduce the output jitter of the system when the error is small, making the controller output smoother, thereby improving the stitching quality of the lidar point cloud. Attached Figure Description

[0084] Figure 1 Is when ω s =0.01rad / s, RMSE t When the speed is 5, a comparison chart of the speed of the control scheme of the present invention and the traditional control scheme is shown.

[0085] Figure 2 Is when ω s =0.01rad / s, RMSE t When RMSE = 5, a comparison chart of the control scheme of the present invention and the traditional control scheme is shown.

[0086] Figure 3 Is when ω s =1 rad / s, RMSE t When the speed is 5, a comparison chart of the speed of the control scheme of the present invention and the traditional control scheme is shown.

[0087] Figure 4 Is when ω s =1 rad / s, RMSE t When RMSE = 5, a comparison chart of the control scheme of the present invention and the traditional control scheme is shown.

[0088] Figure 5 It is in ω s =0.01rad / s, RMSE t =5, RMSE result image after radar point cloud fine matching;

[0089] Figure 6 It is in ω s =0.01rad / s, RMSE t =5, the rotational speed of the radar gimbal;

[0090] Figure 7 It is in ω s =1 rad / s, RMSE t =5, RMSE result image after radar point cloud fine matching;

[0091] Figure 8 It is in ω s =1 rad / s, RMSE t =5, the rotational speed of the radar gimbal. Detailed Implementation

[0092] Specific Implementation Method 1: The radar gimbal adaptive superspiral control method based on RMSE velocity planning described in this implementation method specifically includes the following steps:

[0093] Step 1: Preprocess the k-th frame point cloud data obtained by lidar scanning to obtain the edge feature points and planar feature points in the k-th frame (that is, divide the point cloud data into two parts: edge feature points and planar feature points).

[0094] Step 2: After the radar pan-tilt unit rotates, the lidar continues to scan and obtain the point cloud data of the (k+1)th frame. After preprocessing the point cloud data of the (k+1)th frame, the edge feature points and planar feature points in the (k+1)th frame are obtained.

[0095] Step 3: Perform coarse matching on the feature points extracted in Step 1 and Step 2, then perform fine matching based on the generalized ICP (generalized iterative nearest point), and calculate RMSE (root mean square error) based on the fine matching results.

[0096] Step 4: Establish a fitting equation based on the relationship between radar gimbal rotation speed and RMSE, and design the radar gimbal rotation speed regulation rate based on the fitting equation and the RMSE calculated in Step 3.

[0097] Step 5: Use a superspiral expansion state observer to observe the disturbances experienced by the radar gimbal during rotation, and design an adaptive superspiral controller based on the disturbance observation results and the rotational speed regulation rate based on RMSE.

[0098] Use the output of the adaptive superspiral controller to drive the radar gimbal to rotate. Let k = k + 1, and return to step two.

[0099] After controlling the radar gimbal using the control force output by the adaptive superspiral controller, the radar gimbal rotates and continues to acquire the next frame of point cloud data, i.e., the k+2 frame of point cloud data. Then, the method in step one is used to preprocess the k+1 frame of point cloud data and calculate the RMSE. Then, the rotation speed regulation rate is designed based on the RMSE. The output force of the controller is obtained based on the observation results of the disturbance by the superspiral expansion state observer and the designed rotation speed regulation rate. Finally, the radar gimbal is controlled based on the output force until the entire scanning process is completed.

[0100] Specific Implementation Method Two: This implementation method differs from Specific Implementation Method One in that the specific process of step one is as follows:

[0101] Calculate the smoothness c of point i in the kth frame:

[0102]

[0103] Among them, X (k,i) Let X be the three-dimensional coordinates of point i in the lidar coordinate system of the k-th frame. (k,j) Let J be the three-dimensional coordinates of point j in the lidar coordinate system of the k-th frame, ||·||2 is the L2 norm, S is the set of neighborhood points of point i (the number of neighborhood points is taken as |S|, that is, the |S| points closest to point i), and |S| is the total number of points in set S.

[0104] If the smoothness c is greater than or equal to α1, then point i is an edge feature point;

[0105] If α2 < c < α1, then point i is a feature point of the plane;

[0106] Similarly, the smoothness of each point in the k-th frame is calculated to obtain the edge feature points and planar feature points in the k-th frame.

[0107] The other steps and parameters are the same as in Specific Implementation Method 1.

[0108] This embodiment uses a smoothness index to extract edge feature points and planar feature points, which can reduce the amount of point cloud and improve the running speed. Preferably, α1 is 1 and α2 is 0.

[0109] Specific Implementation Method Three: This implementation method differs from Specific Implementation Method One or Two in that the total number of points |S| in the set S is:

[0110]

[0111] Where L is the arc length, h′ is the horizontal scanning resolution, and r is the scanning distance.

[0112] Other steps and parameters are the same as in specific implementation method one or two.

[0113] Specific Implementation Method Four: This implementation method differs from Specific Implementation Methods One to Three in that, in step three, coarse matching is performed on the feature points extracted in steps one and two, specifically as follows:

[0114] Based on the point-edge distance function, an edge feature constraint error function model f is established. ε A planar feature constraint error function model f is established based on the point-plane distance function. h :

[0115]

[0116]

[0117] Where, d ε It is the point-edge distance function, d h It is a point-to-plane distance function. Let be the pose transformation matrix between frame (k+1) and frame (k).

[0118] For the m-th edge feature point in the (k+1)-th frame, according to Obtain the edge line corresponding to the m-th edge feature point in the k-th frame, and denote the two endpoints of the edge line as point n and point q, respectively. Then calculate the distance from the m-th edge feature point in the (k+1)-th frame to the edge line in the k-th frame. That is, express the point-edge feature function as:

[0119]

[0120] Among them, X (k+1,m) X is the three-dimensional coordinate of point m in the lidar coordinate system of the (k+1)th frame. (k,n) X is the three-dimensional coordinate of edge feature point n in the lidar coordinate system of the k-th frame. (k,q) It is the three-dimensional coordinate of the edge feature point q in the lidar coordinate system of the k-th frame, |X (k,n) -X (k,q) | represents X (k,n) With X (k,q) Euclidean distance;

[0121] For the m′-th planar feature point in the (k+1)-th frame, according to Obtain the feature plane corresponding to the m′-th planar feature point in the k-th frame. Label any three planar feature points on the feature plane as n′, q′, and r′, respectively. Then calculate the distance from the m′-th planar feature point in the (k+1)-th frame to the feature plane in the k-th frame. This can be expressed as the point-plane feature function:

[0122]

[0123] Among them, X (k,n′) X is the three-dimensional coordinate of the planar feature point n′ in the lidar coordinate system of the k-th frame. (k,m′) X is the three-dimensional coordinate of the planar feature point m′ in the lidar coordinate system of the k-th frame. (k,q′) X is the three-dimensional coordinate of the planar feature point q′ in the lidar coordinate system of the k-th frame. (k,r′) X is the three-dimensional coordinate of the planar feature point r′ in the lidar coordinate system of the k-th frame. (k+1,m′) It is the three-dimensional coordinate of point m′ in the lidar coordinate system of the (k+1)th frame;

[0124] To obtain the coarse matching matrix The optimization calculation is performed using the following formula:

[0125]

[0126] Among them, f εf is the set of all edge feature points in the k-th frame. h It is the set of all planar feature points in the k-th frame.

[0127] The other steps and parameters are the same as those in one of the specific implementation methods one to three.

[0128] It should be noted that when a certain edge feature point does not have a corresponding edge line in the k-th frame (i.e., there is no matching point in the k-th frame), then... The value is 0. Similarly, when a planar feature point has no corresponding feature plane in the k-th frame (i.e., there is no matching point in the k-th frame), then... It is 0.

[0129] Specific Implementation Method Five: This implementation method differs from Specific Implementation Methods One to Four in that the fine matching based on generalized ICP is specifically as follows:

[0130] The matrix obtained from coarse matching As the initial pose transformation matrix for the generalized ICP algorithm, a fine matching is then performed based on the generalized ICP algorithm to calculate the pose transformation matrix obtained from the fine matching:

[0131]

[0132] Where, p i It is the i-th point in the k-th frame, q j For the j-th point in the (k+1)-th frame, n j For q j The unit normal vector at point w ij To match point pairs (p i ,q j The weights of ) are given by the superscript T, which represents transpose.

[0133] The other steps and parameters are the same as those in one of the specific implementation methods one to four.

[0134] Specific Implementation Method Six: This implementation method differs from Specific Implementation Methods One to Five in that the calculation of RMSE based on the fine matching result is specifically as follows:

[0135]

[0136] Where, d i It is a matching point pair (p) i ,q j Point p in ) i and point q j The distance between them, where N is the total number of matched point pairs.

[0137] The other steps and parameters are the same as those in one of the specific implementation methods one to five.

[0138] Specific Implementation Method Seven: This implementation method differs from Specific Implementation Methods One to Six in that the specific process of step four is as follows:

[0139] Step 41: Establish the fitting equation from radar gimbal rotation speed ω to RMSE:

[0140]

[0141] Where R(ω) is the fitting equation from angular velocity ω to RMSE, R t ω t η1 and η2 are adjustable parameters of the fitting equation;

[0142] In actual operation, the parameters of the fitting equation need to be adjusted appropriately according to the working characteristics of the radar gimbal. Based on the fitting equation, the fitting relationship between the radar gimbal rotation speed and the RMSE index can be obtained, which facilitates the speed adjustment.

[0143] Step 4.2: Set the fitting equation from the rotational speed ω to the RMSE as follows:

[0144] R r (ω)=R rc (ω)+d R (11)

[0145] Among them, R r (ω) represents the RMSE after considering the perturbation, is the modelable part, and R rc The form of (ω) is the same as that of R(ω), but the specific parameter selection is slightly different, d R The perturbation component is the part that cannot be modeled, and the combination of the two produces the actual RMSE metric.

[0146] Step 43: Set the expected RMSE t With preset speed ω s , let R t For RMSE t ω s For ω t This allows for faster rotation speed and better point cloud stitching quality. Substituting the RMSE calculated in step three into equation (11), and then using R... -1 (ω) inversely solves for the rotational speed

[0147] Step 44, according to Design speed regulation rate ω r :

[0148]

[0149] Where f(·) is the adjustment function and ω is the actual rotational speed;

[0150] If the regulation function is taken as a PI controller, then the speed regulation rate is:

[0151]

[0152] Where, k ωp and k ωi These are the adjustable parameters of the PI controller.

[0153] The other steps and parameters are the same as those in one of the specific implementation methods one to six.

[0154] This invention uses a superspiral expansion state observer to observe the angle, rotation speed and disturbances experienced by the radar gimbal during its rotation, and compensates for and eliminates the observed disturbances. This can reduce the negative impact of dust and other factors on angle measurement and improve control robustness and tracking performance.

[0155] Specific Implementation Method Eight: This implementation method differs from Specific Implementation Methods One to Seven in that the specific process of step five is as follows:

[0156] Step 51: The system model of the radar pan-tilt unit is as follows:

[0157]

[0158] Where θ is the actual rotation angle of the radar pan-tilt unit. It is the first derivative of θ. ω is the first derivative of ω, d is the disturbance experienced by the radar gimbal, and h is the rate of change of the disturbance experienced by the radar gimbal. It is the first derivative of d, where K and T are constants;

[0159] Generally, |h| < L h The superspiral expansion state observer of the system model is designed as follows:

[0160]

[0161] Where z1 is the observation value of θ by the superspiral expansion state observer. Z1 is the first derivative of z1, and z2 is the observation of ω by the superspiral expansion state observer. z2 is the first derivative of z2, and z3 is the observation of d by the superspiral expansion state observer. It is the first derivative of z3, α1, α2, α3, α4 and α5 are the coefficients of the superspiral expansion state observer, e1 = z1 - θ m θ m For the measured rotation angle of the radar pan-tilt unit, sgn(·) is the sign function, ω m The measured rotational speed includes noise.

[0162] The observed value z2 is compared with the ω measured by the radar gimbal measuring element. m A weighted average filter is applied to obtain a smoothed rotational speed ω. a :

[0163] ω a =wω m +(1-w)z2 (16)

[0164] Where w is the fusion coefficient;

[0165] Step 52: Based on the RMSE speed planning results and the smoothed rotational speed, the rotational speed error e of the radar gimbal is obtained as follows:

[0166] e = ω a -ω r (17)

[0167] Subsequently, the output of the radar gimbal adaptive superspiral controller was designed as follows:

[0168]

[0169] Where u is the output of the controller. It is ω r The first derivatives, ε1, ε2, ε3, ε4, and ε5 are the adjustable parameters of the controller. is u a The first derivative, k c For adaptive parameters;

[0170]

[0171]

[0172] The other steps and parameters are the same as those in any of the specific implementation methods one to seven.

[0173] Specific Implementation Method Nine: This implementation method differs from Specific Implementation Methods One through Eight in that the coefficients α1, α2, and α3 satisfy:

[0174]

[0175] Among them, L h It is the upper bound of the rate of change h of the disturbance experienced by the radar pan-tilt unit.

[0176] The other steps and parameters are the same as those in one of the specific implementation methods one to eight.

[0177] By selecting the values ​​of coefficients α1, α2, and α3 according to this implementation method, observer convergence can be achieved.

[0178] Experimental Section

[0179] To verify the effectiveness of the radar gimbal rotation control scheme based on the evaluation index RMSE of this invention, a radar gimbal control simulation experiment was carried out on a simulation platform with radar gimbal scanning mapping as the background, demonstrating the effectiveness of the control scheme in maintaining good mapping and the robustness of control performance.

[0180] Against this backdrop, the system parameters of the radar pan-tilt unit are set as K = 3.2, T = 0.53, the initial rotation angle of the radar pan-tilt unit is set as θ = 0, the initial rotation speed is set as ω = 0, and the preset rotation speeds are set as ω. s =0.1 rad / s and ω s =1 rad / s, representing the preset rotational speeds as too fast and too slow, respectively. The adaptive superspiral controller parameters are selected as ε1 = 10, ε2 = 10, ε3 = 0.5, ε4 = 1.5, ε5 = 1. The superspiral expansion state observer parameters are selected as α1 = 8.8, α2 = 45.2, α3 = 110, α4 = 100, α5 = 5. The data fusion parameter is selected as w = 0.2.

[0181] The parameters of the fitting function are chosen as follows: R t =1, v t =0.05, η1=20, η2=200. The radar rotation speed and point cloud stitching index RMSE are subject to many external interferences. In actual operation, R(ω) cannot accurately reflect this correspondence. To verify the robustness of the radar gimbal motion control scheme of this invention, a corresponding relationship R for actual operation is designed. r (ω)=R rc (ω)+d R , where R rc (ω) parameter is selected as R t =1, v t =0.1, eta1=10, eta2=100. d R The selected signal is white noise superimposed with a sinusoidal signal disturbance, and d R It is directly proportional to the speed. f(·) selects a PI controller, and the parameter is selected as k. ωp =0.25, k ωi =1.

[0182] After completing the parameter selection, the preset rotation speed of the radar pan-tilt unit is set to ω. s =0.01rad / s and ω s The experiment was conducted at a speed of 1 rad / s, and the expected RMSE was set. t =5. The control scheme of this invention adopts an adaptive superspiral control strategy based on a superspiral expanding state observer, and selects a PID control strategy based on a linear expanding state observer as the traditional control scheme. The PID parameter is selected as k. p =10,k i=10, the disturbance observation bandwidth of the linear extended state observer is set to 100 rad / s. The motion control and velocity planning effects of the radar gimbal under different control schemes are as follows: Figures 1 to 8 As shown. By Figures 1 to 4 It can be seen that, compared with traditional control schemes, the control scheme of this invention has a smoother control process and stronger anti-interference capability, and can robustly and smoothly realize the rotational speed control process of the radar gimbal. From Figures 5 to 8 It can be seen that the speed planning scheme of the present invention can be based on the expected RMSE. t The rotation speed of the radar gimbal is adjusted to achieve adaptive, high-quality dynamic point cloud stitching for LiDAR. Specifically, when the initial rotation speed of the radar gimbal is low, this solution can adaptively increase the rotation speed and mapping efficiency of the radar gimbal while ensuring the mapping effect of LiDAR; while when the initial rotation speed of the radar gimbal is too high, this solution can adaptively reduce the rotation speed of the radar gimbal to ensure the dynamic stitching quality of the LiDAR point cloud.

[0183] The above examples of the present invention are merely illustrative of the computational model and process of the present invention, and are not intended to limit the implementation of the present 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 the present invention are still within the scope of protection of the present invention.

Claims

1. A radar gimbal adaptive superspiral control method based on RMSE velocity programming, characterized in that, The method specifically includes the following steps: Step 1: Preprocess the point cloud data of the kth frame obtained by lidar scanning to obtain the edge feature points and planar feature points in the kth frame; Step 2: After the radar platform rotates, the lidar continues to scan and obtain the point cloud data of the (k+1)th frame. After preprocessing the point cloud data of the (k+1)th frame, the edge feature points and planar feature points in the (k+1)th frame are obtained. Step 3: Perform coarse matching on the feature points extracted in Step 1 and Step 2, then perform fine matching based on generalized ICP, and calculate RMSE based on the fine matching results. Step 4: Establish a fitting equation based on the relationship between radar gimbal rotation speed and RMSE, and design the radar gimbal rotation speed regulation rate based on the fitting equation and the RMSE calculated in Step 3. Step 5: Use a superspiral expansion state observer to observe the disturbances experienced by the radar gimbal during rotation, and design an adaptive superspiral controller based on the disturbance observation results and the rotational speed regulation rate based on RMSE. Use the output of the adaptive superspiral controller to drive the radar gimbal to rotate. Let k = k + 1, and return to step two.

2. The radar gimbal adaptive superspiral control method based on RMSE velocity planning according to claim 1, characterized in that, The specific process of step one is as follows: Calculate the smoothness c of point i in the kth frame: Among them, X (k,i) Let X be the three-dimensional coordinates of point i in the lidar coordinate system of the k-th frame. (k,j) Let J be the three-dimensional coordinates of point J in the lidar coordinate system of the k-th frame, ||·||2 is the L2 norm, S is the set of neighborhood points of point i, and |S| is the total number of points in set S. If the smoothness c is greater than or equal to α1, then point i is an edge feature point; If α2 < c < α1, then point i is a feature point of the plane; Similarly, the smoothness of each point in the k-th frame is calculated to obtain the edge feature points and planar feature points in the k-th frame.

3. The radar gimbal adaptive superspiral control method based on RMSE velocity planning according to claim 2, characterized in that, The total number of points |S| in the set S is: Where L is the arc length, h′ is the horizontal scanning resolution, and r is the scanning distance.

4. The radar gimbal adaptive superspiral control method based on RMSE velocity planning according to claim 3, characterized in that, In step three, a coarse matching is performed on the feature points extracted in steps one and two, specifically as follows: Based on the point-edge distance function, an edge feature constraint error function model f is established. ε A planar feature constraint error function model f is established based on the point-plane distance function. h : Where, d ε It is the point-edge distance function, d h It is a point-to-plane distance function. Let be the pose transformation matrix between frame (k+1) and frame (k). For the m-th edge feature point in the (k+1)-th frame, according to Obtain the edge line corresponding to the m-th edge feature point in the k-th frame, and denote the two endpoints of the edge line as point n and point q, respectively. Then calculate the distance from the m-th edge feature point in the (k+1)-th frame to the edge line in the k-th frame. That is, express the point-edge feature function as: Among them, X (k+1,m) X is the three-dimensional coordinate of point m in the lidar coordinate system of the (k+1)th frame. (k,n) X is the three-dimensional coordinate of edge feature point n in the lidar coordinate system of the k-th frame. (k,q) It is the three-dimensional coordinate of the edge feature point q in the lidar coordinate system of the k-th frame, |X (k,n) -X (k,q) | represents X (k,n) With X (k,q) Euclidean distance; For the m′-th planar feature point in the (k+1)-th frame, according to Obtain the feature plane corresponding to the m′-th planar feature point in the k-th frame. Label any three planar feature points on the feature plane as n′, q′, and r′, respectively. Then calculate the distance from the m′-th planar feature point in the (k+1)-th frame to the feature plane in the k-th frame. This can be expressed as the point-plane feature function: Among them, X (k,n′) X is the three-dimensional coordinate of the planar feature point n′ in the lidar coordinate system of the k-th frame. (k,m′) X is the three-dimensional coordinate of the planar feature point m′ in the lidar coordinate system of the k-th frame. (k,q′) X is the three-dimensional coordinate of the planar feature point q′ in the lidar coordinate system of the k-th frame. (k,r′) X is the three-dimensional coordinate of the planar feature point r′ in the lidar coordinate system of the k-th frame. (k+1,m′) It is the three-dimensional coordinate of point m′ in the lidar coordinate system of the (k+1)th frame; To obtain the coarse matching matrix The optimization calculation is performed using the following formula: Among them, f ε f is the set of all edge feature points in the k-th frame. h It is the set of all planar feature points in the k-th frame.

5. The radar gimbal adaptive superspiral control method based on RMSE velocity planning according to claim 4, characterized in that, Fine matching is performed based on the coarse matching results, specifically as follows: The matrix obtained from coarse matching As the initial pose transformation matrix for the GICP algorithm, a fine matching is then performed based on the GICP algorithm to calculate the pose transformation matrix obtained from the fine matching: Where, p i It is the i-th point in the k-th frame, q j For the j-th point in the (k+1)-th frame, n j For q j The unit normal vector at point w ij To match point pairs (p i ,q j The weights of ) are given by the superscript T, which represents transpose.

6. The radar gimbal adaptive superspiral control method based on RMSE velocity planning according to claim 5, characterized in that, The calculation of the root mean square error based on the fine matching result is as follows: Where, d i It is a matching point pair (p) i ,q j Point p in ) i and point q j The distance between them, where N is the total number of matched point pairs.

7. The radar gimbal adaptive superspiral control method based on RMSE velocity planning according to claim 6, characterized in that, The specific process of step four is as follows: Step 41: Establish the fitting equation from radar gimbal rotation speed ω to RMSE: Where R(ω) is the fitting equation from angular velocity ω to RMSE, R t ω t η1 and η2 are the parameters of the fitted equation; Step 4.2: Set the fitting equation from the rotational speed ω to the RMSE as follows: R r (ω)=R rc (ω)+d R (11) Among them, R r (ω) represents the RMSE after considering the perturbation, and R rc The form of (ω) is the same as that of R(ω), d R The disturbance component; Step 43: Set the expected RMSE t With preset speed ω s , let R t For RMSE t ω s For ω t Substitute the RMSE calculated in step three into equation (11), and then use R... -1 (ω) solves for the rotational speed value Step 44, according to Design speed regulation rate ω r : Where f(·) is the adjustment function and ω is the actual rotational speed; If the regulation function is taken as a PI controller, then the speed regulation rate is: Where, k ωp and k ωi These are the parameters for the PI controller.

8. The radar gimbal adaptive superspiral control method based on RMSE velocity planning according to claim 7, characterized in that, The specific process of step five is as follows: Step 51: The system model of the radar platform is as follows: Where θ is the actual rotation angle of the radar platform. It is the first derivative of θ. ω is the first derivative of ω, d is the disturbance experienced by the radar gimbal, and h is the rate of change of the disturbance experienced by the radar gimbal. It is the first derivative of d, where K and T are constants; The superspiral expansion state observer of the system model is designed as follows: Where z1 is the observation value of θ by the superspiral expansion state observer. Z1 is the first derivative of z1, and z2 is the observation of ω by the superspiral expansion state observer. z2 is the first derivative of z2, and z3 is the observation of d by the superspiral expansion state observer. It is the first derivative of z3, α1, α2, α3, α4 and α5 are the coefficients of the superspiral expansion state observer, e1 = z1 - θ m θ m For the measured rotation angle of the radar pan-tilt unit, sgn(·) is the sign function, ω m This is a measurement of rotational speed including noise. The observed value z2 is compared with the ω measured by the radar gimbal measuring element. m A weighted average filter is applied to obtain a smoothed rotational speed ω. a : oh a =wω m +(1-w)z2 (16) Where w is the fusion coefficient; Step 52: Based on the RMSE speed planning results and the smoothed rotational speed, the rotational speed error e of the radar gimbal is obtained as follows: e=ω a -oh r (17) Subsequently, the output of the radar gimbal adaptive superspiral controller was designed as follows: Where u is the output of the controller. It is ω r The first derivatives, ε1, ε2, ε3, ε4, and ε5 are the adjustable parameters of the controller. is u a The first derivative, k c For adaptive parameters; 9. The radar gimbal adaptive superspiral control method based on RMSE velocity planning according to claim 8, characterized in that, The coefficients α1, α2, and α3 satisfy: Among them, L h It is the upper bound of the rate of change h of the disturbance experienced by the radar pan-tilt unit.

Images