A surface robot control device based on multi-sensor fusion of UKF algorithm
Through the integration of UKF algorithm and multi-sensor data, the stability and obstacle avoidance efficiency of the existing technology of small and medium-sized surface robot control devices is solved, and the smooth navigation and efficient obstacle avoidance of large surface robots are realized, and the adaptability of the system is improved. and energy efficiency.
Patent Information
- Application Number
- CN202210160436.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-02-22
- Publication Date
- 2024-11-05
- Estimated Expiration
- 2042-02-22
AI Technical Summary
The existing multi-sensor control device design is mainly aimed at small surface robots, and the lack of data fusion technology has led to low control stability and obstacle avoidance efficiency of large surface robots, poor adaptability, and unsmooth automatic navigation and high energy consumption.
The UKF algorithm is used to fusion of multi-sensor data, combining GNSS positioning, accelerometers, gyroscopes, electronic compass, lidar, millimeter wave radar and visual radar data, and data processing is carried out through the Kalman filtering algorithm to realize the precise control and intelligence of surface robots Avoid obstacles.
It improves the data prediction accuracy and system stability of the surface robot, achieves smooth autonomous navigation, high obstacle avoidance efficiency, and low energy consumption, and enhances the adaptability and reliability of the control device.
Smart Images

Figure CN114545941B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of ship control, and in particular to a surface robot control device based on multi-sensor fusion of UKF algorithm. Background Art
[0002] Most of the existing multi-sensor control devices are designed for small surface robots, and most of the multi-sensors do not perform data fusion. The small surface robot receives the signal of the GPS positioning sensor through the central controller to obtain the absolute coordinates; receives the signal of the inertial navigation module to obtain the acceleration and angular velocity of the small surface robot, and integrates the obtained acceleration or angular velocity to obtain the real-time speed and angle; receives the laser ranging sensor or radar data to determine the distance to the obstacle in front; receives the data of the electronic compass to obtain the direction of travel; the central processor performs Kalman filtering on the signal of the GPS positioning sensor and the electronic compass data to reduce Gaussian noise, and directly drives the drive device and steering device to control the speed and heading of the small surface robot after processing with the above sensor data and the filtered data.
[0003] For the existing obstacle avoidance design of small surface robots, the CPU obtains obstacle avoidance radar data at regular intervals, and determines whether the obstacle is stationary through the obstacle avoidance radar data. If it is a stationary obstacle, the robot will re-plan the path to bypass the obstacle; if it is a moving obstacle, the robot will slow down or stop to wait for the obstacle to leave. Since multi-sensor data fusion technology is not adopted, most of the existing ship control technologies can only be customized for small surface robots. The hull and power unit of the surface robot have changed, and this control technology needs to be redesigned, with poor redundancy and adaptability, and poor stability. In addition, the existing obstacle avoidance design for small surface robots has poor real-time performance, low efficiency, and the small surface robot is not smooth when it automatically navigates, and has high energy consumption and other disadvantages.
[0004] In view of the above shortcomings of the existing surface robot control technology, the present invention uses the UKF algorithm to perform multi-sensor data fusion. It integrates GNSS positioning data, accelerometer, gyroscope, electronic compass, laser radar, millimeter wave radar and visual radar and other sensor data, and proposes a surface robot control device based on the UKF algorithm and multi-sensor fusion. Summary of the invention
[0005] In view of the above-mentioned deficiencies in the prior art, the present invention provides a surface robot control device based on a multi-sensor fusion UKF algorithm. The present invention makes data prediction more accurate, the system more stable and reliable, the autonomous navigation smoother, and the obstacle avoidance more efficient and intelligent.
[0006] The present invention provides the following technical solutions: A multi-sensor fusion water surface robot control device based on UKF algorithm, comprising a controller, and the controller has an onboard accelerometer, a gyroscope, an electronic compass, a GNSS module, a laser radar, a millimeter wave radar, a communication interface and a motor interface;
[0007] The gyroscope is an onboard sensor that provides angular acceleration information, which has a significant impact on the fused attitude data and also has an impact on the fused absolute position data;
[0008] The accelerometer is an onboard sensor that provides linear acceleration information, which has a greater impact on the fused posture data and also has a greater impact on the fused absolute position data;
[0009] The electronic compass adopts a redundant design. When the controller is installed on the surface robot and the surrounding electromagnetic environment has little interference, the onboard electronic compass can be used. When the controller is installed on the surface robot and the surrounding electromagnetic environment has large interference, an external electronic compass can be used to adapt to different application environments. The electronic compass provides global heading information and has a greater impact on the heading data after fusion of the surface robot;
[0010] The GNSS module supports global satellite positioning systems such as GPS and Beidou, and also supports high-precision global satellite positioning systems such as DGPS and differential GPS to obtain absolute position information such as latitude and longitude data.
[0011] The controller uses the Kalman filter (UKF) algorithm for data fusion. The UKF time and process update equations are obtained according to the algorithm. The controller updates the time according to the data rate received from the gyroscope. At this time, the posture, speed, absolute position, heading and other data will be updated;
[0012] The laser radar provides obstacle detection information, and can provide information such as the distance, shape, and size of obstacles;
[0013] The millimeter wave radar provides obstacle detection information, which can provide information such as the distance, size and speed of the obstacle;
[0014] The communication interface is mainly used for the surface robot to communicate with the ground control system, and the motor interface is mainly used to output the control instructions of the motor after the fused data is processed, so as to achieve precise control of the surface robot.
[0015] Preferably, the UKF algorithm comprises the following steps:
[0016] S1. Establish a mathematical model for nonlinear systems:
[0017] x k+1 =f(x k )+w k
[0018] y k =h(x k )+v k
[0019] Assuming the state is a Gaussian random vector, the statistical characteristics of process noise and measurement noise are:
[0020] w k ~N(0,Q k )v k ~N(0,R k )
[0021] S2. State initialization
[0022]
[0023]
[0024] S3. Calculate Sigma Points
[0025]
[0026]
[0027]
[0028] S4. Time update equation
[0029]
[0030]
[0031]
[0032]
[0033]
[0034] S5. Measurement update equation
[0035]
[0036]
[0037] K=P xy,k P y,k -1
[0038]
[0039] Preferably, the steps of calibrating the initial value of the UKF state or the network parameters are as follows:
[0040] S1. The controller device is designed with SD card storage function. The calibration method must be equipped with a refrigerator at -15° and an environment away from vibration interference.
[0041] S2. Make a container for placing the controller. The container is covered with a sponge material that absorbs moisture. The upper cover can be easily opened. When the upper cover is closed, ensure that the interior is basically in a closed state.
[0042] S3. After the above two conditions are met, start the calibration operation:
[0043] The first step is to place the controller in a container, open the top cover, and place it in a -15° refrigerator for 60 minutes;
[0044] The second step is to open the refrigerator, quickly close the upper cover in the refrigerator, take out the container, make sure the X-axis of the controller points to the center of the earth, put it in place, power on the controller, and let it stand for 60 minutes. During the standing period, make sure there is no vibration interference around, and the SD card records the operating status data at this time;
[0045] Step 3: Repeat steps 1 and 2, but make sure the Y axis points to the center of the earth before powering on the controller. The SD card records the running status data at this time.
[0046] Step 4: Repeat steps 1 and 2, but make sure the controller is placed flat before powering on, and the SD card will record the running status data at this time;
[0047] Step 5: Using the three recorded data, the PC calibration program is used to calibrate the initial state value, the temperature drift coefficient of the accelerometer and gyroscope, the inter-axis coupling coefficient and other parameters.
[0048] Compared with the prior art, the present invention has the following beneficial effects:
[0049] The multi-sensor fusion surface robot control device based on the UKF algorithm performs multi-sensor data fusion processing on the control of the surface robot, making information such as position and posture more accurate and effective, avoiding system operation risks caused by inaccurate or faulty data of a single sensor, and improving the operation reliability and stability of the surface robot; the calibration of the UKF algorithm and structural parameters in the system network makes the controller more adaptable and more stable; the new obstacle avoidance strategy of the surface robot makes the operation of the surface robot more efficient and energy-saving, effectively improving the operation efficiency and operation time of the surface robot. BRIEF DESCRIPTION OF THE DRAWINGS
[0050] Figure 1 It is a schematic diagram of the structure of the present invention. DETAILED DESCRIPTION
[0051] In order to make the objectives, technical solutions and advantages of the embodiments of the present disclosure clearer, the technical solutions of the embodiments of the present disclosure will be clearly and completely described below in conjunction with the drawings of the embodiments of the present disclosure. In order to keep the following description of the embodiments of the present disclosure clear and concise, the present disclosure omits detailed descriptions of known functions and known components to avoid unnecessary confusion of the concepts of the present invention.
[0052] See also Figure 1 A multi-sensor fusion water surface robot control device based on UKF algorithm includes a controller, and the controller has an onboard accelerometer, a gyroscope, an electronic compass, a GNSS module, a laser radar, a millimeter wave radar, a communication interface and a motor interface;
[0053] The gyroscope is an onboard sensor that provides angular acceleration information, which has a significant impact on the fused attitude data and also has an impact on the fused absolute position data;
[0054] The accelerometer is an onboard sensor that provides linear acceleration information, which has a greater impact on the fused posture data and also has a greater impact on the fused absolute position data;
[0055] The electronic compass adopts a redundant design. When the controller is installed on the surface robot and the surrounding electromagnetic environment has little interference, the onboard electronic compass can be used. When the controller is installed on the surface robot and the surrounding electromagnetic environment has large interference, an external electronic compass can be used to adapt to different application environments. The electronic compass provides global heading information and has a greater impact on the heading data after fusion of the surface robot;
[0056] The GNSS module supports global satellite positioning systems such as GPS and Beidou, and also supports high-precision global satellite positioning systems such as DGPS and differential GPS to obtain absolute position information such as latitude and longitude data.
[0057] The laser radar provides obstacle detection information, and can provide information such as the distance, shape, and size of obstacles;
[0058] The millimeter wave radar provides obstacle detection information, which can provide information such as the distance, size and speed of the obstacle;
[0059] The communication interface is mainly used for the surface robot to communicate with the ground control system, and the motor interface is mainly used to output the control instructions of the motor after the fused data is processed, so as to achieve precise control of the surface robot.
[0060] The accelerometer and gyroscope have the advantages of complete autonomy, complete motion parameters, and strong anti-interference, but at the same time, the error accumulates over time; the GNSS device has the advantages of all-weather, high precision, and no error accumulation, but lacks attitude information and is easily interfered. In order to calibrate and compensate for the shortcomings of the two equipment through a combined filter, the lossless Kalman filter (UKF) algorithm can be used for data fusion.
[0061] The main Kalman filters are KF, EKF, and UKF. KF is mainly used in scenarios where the system equations and measurement equations are linear, EKF is used in mildly nonlinear scenarios, and UKF performs better in strong nonlinear situations. In KF, we need to obtain the state covariance to determine the Kalman gain, but due to the nonlinearity of the system, we can only get its approximate value. EKF linearizes the system through the Jacobian matrix, which will cause errors due to ignoring the high-order components of the system. It is difficult to calculate the Jacobian matrix, and the total amount of calculation is very large if it is to be accurate. UKF uses the conditional mean of random sampling instead of the mean, and uses the reproduced sample points of the estimated quantity and the measured quantity to calculate the autocovariance matrix and cross-covariance matrix of the two. 1. Establish a mathematical model for nonlinear systems:
[0062] x k+1 =f(x k )+w k
[0063] y k =h(x k )+v k
[0064] Assuming the state is a Gaussian random vector, the statistical characteristics of process noise and measurement noise are:
[0065] w k ~N(0,Q k )v k ~N(0,R k )
[0066] 2. State initialization
[0067]
[0068]
[0069] 3. Calculate Sigma Points
[0070]
[0071]
[0072]
[0073] 4. Time update equation
[0074]
[0075]
[0076]
[0077]
[0078]
[0079] 5. Measurement update equation
[0080]
[0081]
[0082] K=P xy,k P y,k -1
[0083]
[0084] According to the above UKF time and process update equations, the controller performs time updates according to the data rate received from the gyroscope. At this time, the posture, velocity, absolute position, heading and other data will be updated to achieve stable high-rate attitude information; the process update will be updated according to the update rates of the accelerometer, GNSS device, lidar and millimeter-wave radar, and the posture, velocity, absolute position and heading and other data will be updated accordingly.
[0085] The selection of the initial state value of the UKF algorithm used in the present invention is particularly important for the environmental adaptability of the controller. The surface robot equipped with the controller of the present invention can adapt to the environmental application of 0°-80°. In addition to the strict device selection of the controller carrier board, the temperature drift of each device in different environments such as high and low temperatures will bring great uncertainty to the control accuracy of the system. In order to cope with the drift of the onboard devices at different temperatures, the initial state value or network parameters of the UKF are calibrated as necessary, and the calibration method is as follows:
[0086] 1. The controller device is designed with SD card storage function. The calibration method must be carried out in a refrigerator at -15° and away from the environment with vibration interference.
[0087] 2. Make a container to place the controller. The container is covered with sponge material that absorbs moisture, and the upper cover can be easily opened. When the upper cover is closed, ensure that the interior is basically in a closed state.
[0088] 3. After the above two conditions are met, start the calibration operation: Step 1, place the controller in a container, open the upper cover, place it in a -15° refrigerator, and let it stand for 60 minutes; Step 2, open the refrigerator, quickly close the upper cover in the refrigerator, take out the container, make sure that the X-axis of the controller points to the center of the earth, put it in place, power on the controller, and let it stand for 60 minutes. During the standing process, make sure there is no vibration interference around, and the SD card records the operating status data at this time; Step 3: Repeat steps 1 and 2, but before powering on the controller, make sure that the Y-axis points to the center of the earth, and the SD card records the operating status data at this time; Step 4: Repeat steps 1 and 2, but before powering on the controller, make sure it is placed flat, and the SD card records the operating status data at this time; Step 5: Use the three recorded data and process them through the PC calibration program to calibrate the initial value of the state, the temperature drift coefficient of the accelerometer and gyroscope, and the inter-axis coupling coefficient and other parameters.
[0089] After the UKF data is fused, accurate global position and attitude information is obtained, which can more accurately obtain the operating status of the surface robot. At this time, the obstacle information detected by the laser radar or millimeter wave radar can be used to clearly determine the spatial position relationship between the surface robot and the obstacle. Using this spatial position relationship, the collision threshold β between the surface robot and the obstacle is set, and the heading of the surface robot is adjusted according to this collision threshold. Within the reasonable range of the threshold β, the surface robot can smoothly bypass the obstacle without any change in speed.
[0090] The above embodiments are only exemplary embodiments of the present invention and are not intended to limit the present invention. The protection scope of the present invention is defined by the claims. Those skilled in the art may make various modifications or equivalent substitutions to the present invention within the essence and protection scope of the present invention, and such modifications or equivalent substitutions shall also be deemed to fall within the protection scope of the present invention.
Claims
1. A multi-sensor fusion water surface robot control device based on UKF algorithm, characterized by: Including a controller, and the controller has an onboard accelerometer, a gyroscope, an electronic compass, a GNSS module, a laser radar, a millimeter wave radar, a communication interface and a motor interface; The gyroscope is an onboard sensor that provides angular acceleration information, which has a significant impact on the fused attitude data and also has an impact on the fused absolute position data; The accelerometer is an onboard sensor that provides linear acceleration information, which has a greater impact on the fused posture data and also has a greater impact on the fused absolute position data; The electronic compass adopts a redundant design. When the controller is installed on the surface robot and the surrounding electromagnetic environment interference is small, an onboard electronic compass is used. When the controller is installed on the surface robot and the surrounding electromagnetic environment interference is large, an external electronic compass is used to adapt to different application environments. The GNSS module supports GPS, Beidou global satellite positioning system, DGPS and differential GPS high-precision global satellite positioning system to obtain longitude and latitude data location information; The controller uses the UKF algorithm for data fusion. The UKF time and process update equations are obtained according to the algorithm. The controller updates the time according to the data rate received from the gyroscope. At this time, the posture, speed, absolute position, and heading data will be updated. The laser radar provides obstacle detection information, and provides information on the distance, shape, and size of obstacles; The millimeter wave radar provides obstacle detection information, providing distance, size and speed information of obstacles; The communication interface is used for the surface robot to communicate with the ground control system, and the motor interface processes the fused data and then outputs the control instructions of the motor to achieve precise control of the surface robot; The UKF algorithm needs to calculate the initial state value or network parameters during its use, and the steps for calibrating the initial state value or network parameters of UKF are as follows: S1. The controller is designed with SD card storage function, and the calibration method must be in a refrigerator at -15° and away from the environment with vibration interference; S2. Make a container for placing the controller. The container is covered with a sponge material that absorbs moisture. The upper cover can be easily opened. When the upper cover is closed, ensure that the interior is basically in a closed state. S3. After the above two conditions are met, start the calibration operation: The first step is to place the controller in a container, open the top cover, and place it in a -15° refrigerator for 60 minutes; The second step is to open the refrigerator, quickly close the upper cover in the refrigerator, take out the container, make sure the X-axis of the controller points to the center of the earth, put it in place, power on the controller, and let it stand for 60 minutes. During the standing period, make sure there is no vibration interference around, and the SD card records the operating status data at this time; Step 3: Repeat steps 1 and 2, but make sure the Y axis points to the center of the earth before powering on the controller. The SD card records the running status data at this time. Step 4: Repeat steps 1 and 2, but make sure the controller is placed flat before powering on, and the SD card will record the running status data at this time; Step 5: Using the three recorded data, the PC calibration program is used to calibrate the initial state value, the temperature drift coefficient of the accelerometer and gyroscope, and the inter-axis coupling coefficient parameters.
