Methods, devices, equipment and storage media for pose correction of nuclear power plant reactor pool robots
By combining a third-view camera device and an extended Kalman filter, the problem of cumulative positioning drift of the stacking pool robot was solved, achieving more accurate pose correction and safer operation.
Patent Information
- Application Number
- CN202611374754.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-09-04
- Publication Date
- 2026-11-17
AI Technical Summary
During long-term operation, the cumulative positioning drift problem of stacking robots is caused by factors such as odometer error and visual feature degradation, which is difficult to solve effectively with existing technologies.
A third-view camera is introduced to acquire robot images. Through time-progressive compensation and extended Kalman filter fusion, the pose determined by the robot's own sensors is corrected by the third-view observation pose. An adaptive observation noise covariance model is established and graded and smoothed.
It significantly reduces positioning drift, keeps positioning error within a stable range, improves robot pose accuracy and operational safety, and avoids positioning disturbances and instantaneous deviations.
Smart Images

Figure CN123118747A_ABST
Abstract
Description
Technical Field
[0001] This application belongs to the field of wiping and decontamination technology for nuclear power reactor pools, and particularly relates to a method, device, electronic equipment and computer-readable storage medium for correcting the pose of a nuclear power plant reactor pool robot. Background Technology
[0002] During operation, nuclear power plant reactor pools gradually accumulate radioactive deposits (such as metal scraps and chemical deposits). Therefore, during nuclear power plant shutdowns, refueling, and major overhauls, radioactive decontamination treatment is required on the bottom and walls of the reactor pools to ensure the long-term safe operation of the nuclear power plant.
[0003] Currently, in traditional nuclear power plant reactor pool decontamination operations, multiple workers typically need to take turns entering the radioactive environment to perform tasks such as flushing, wiping, and inspection. However, with the continuous development of intelligent inspection and remote control technologies, reactor pool robots (such as cleaning robots and scrubbing robots) are gradually being used to replace manual labor in performing decontamination and inspection tasks.
[0004] When performing tasks within a stacking pool, stacking robots typically need to operate continuously for several hours or even longer, relying on their own sensors (such as vision, laser, and inertial sensors) to determine their pose for navigation and localization. However, during long-term operation, due to limitations such as robot odometry errors and visual feature degradation, the errors generated by localization relying on their own sensors increase linearly or sublinearly over time, leading to cumulative localization drift. Summary of the Invention
[0005] This application provides a method, apparatus, electronic device, and computer-readable storage medium for correcting the pose of a nuclear power plant reactor pool robot, which can solve the problem of cumulative positioning drift caused by long-term operation of the reactor pool robot.
[0006] In a first aspect, embodiments of this application provide a method for correcting the pose of a robot in a nuclear power plant reactor pool, including: The robot images are acquired by a third-view camera device, which is used to capture images of the robot at the bottom of the pile from a third-view perspective. Map the robot pixel coordinates in the robot image to the world plane coordinate system to obtain the robot's first pose in the world plane coordinate system at the first moment; Based on the first pose, the robot's observed pose at the second moment is obtained. The first moment is earlier than the second moment. The difference between the second moment and the first moment is equal to the delay time. The delay time is the difference between the time when the fusion node receives the observed pose and the time when the robot image is acquired. The predicted pose at the second moment is corrected by using the observed pose to obtain the second pose. The predicted pose at the second moment is determined based on the sensor data of the robot's onboard sensors. The robot's pose is corrected based on the second pose.
[0007] In this embodiment, a third-view camera acquires robot images from a third-person perspective, and the robot pixel coordinates in the robot images are mapped to the world plane coordinate system to obtain the robot pose (i.e., the first pose) from the third-person perspective. Then, based on the time delay between acquiring the robot image and the fusion node receiving the observed pose, the first pose is compensated for by time progression to obtain the observed pose at the second moment. Finally, the predicted pose is corrected using the observed pose obtained from the third-person robot image to obtain a more accurate robot pose. Thus, by introducing the observed pose from the third-person perspective as an external absolute pose constraint to correct the pose determined by the robot's own sensors, positioning drift can be significantly reduced in the feature-scarce and easily disturbed environment of a nuclear power plant reactor pool, keeping the positioning error within a stable range and obtaining a more accurate robot pose. Furthermore, the observed pose from the third-person perspective does not depend on the robot's own movement process, thus solving the problem of cumulative positioning drift caused by reactor pool robot odometry errors and visual feature degradation.
[0008] In some possible implementations of the first aspect, the observed pose of the robot at the second moment is obtained based on the first pose, including: The robot's linear velocity and angular velocity at the first moment are obtained; the robot's position at the second moment is determined based on the linear velocity and position at the first moment; the robot's yaw angle at the second moment is determined based on the angular velocity and yaw angle at the first moment; wherein, the first pose includes the position and yaw angle at the first moment; the observed pose includes the position and yaw angle at the second moment.
[0009] In this implementation, considering the end-to-end delay between acquiring the third-view robot image and the fusion node receiving the observed pose, a time-compensated progressive model is used to convert the first pose at the first moment into the observed pose at the second moment to perform time correction on the observed pose, so that the time of the observed pose is consistent with the current filtering time, ensuring physical consistency during fusion calculation.
[0010] In some possible implementations of the first aspect, the predicted pose at the second time step is corrected using the observed pose to obtain the second pose, including: Based on sensor data and the motion model in the extended Kalman filter, the predicted pose at the second moment is determined; The prediction covariance at the second moment is determined based on the state Jacobian matrix, the process noise covariance, and the covariance of the previous moment at the second moment; the state Jacobian matrix is the Jacobian matrix of the motion model with respect to the state. The observation Jacobian matrix is determined based on the predicted pose and the measurement model in the extended Kalman filter; The Kalman gain is determined based on the prediction covariance, the observation Jacobian matrix, and the observation noise covariance. Based on the measurement model and predicted pose, determine the theoretical observed pose at the second moment; The second pose is determined based on the observed pose, predicted pose, theoretical observed pose, and Kalman gain.
[0011] In this implementation, the observation pose of the additional third-view perspective is used as the observation pose of the extended Kalman filter at the second moment, so as to use the observation pose to correct the predicted pose in the extended Kalman filter, thereby improving the positioning accuracy.
[0012] In some possible implementations of the first aspect, the method further includes, before determining the Kalman gain based on the prediction covariance, the observation Jacobian matrix, and the observation noise covariance: Obtain confidence scores and occlusion metrics. Confidence scores describe the confidence level of the robot detection algorithm, while occlusion metrics describe the degree of occlusion in the field of view of the third-view camera device. The observation noise covariance is determined based on the baseline observation noise, confidence sensitivity, occlusion amplification factor, confidence score, and occlusion index.
[0013] In this implementation, considering that the quality of third-view observations in the pool environment varies greatly with illumination, water film reflection, and occlusion, the observation noise covariance is adaptively calculated using confidence scores and occlusion indices. This allows the observation weights in the extended Kalman filter to be automatically adjusted according to the quality of the third-view image, thereby avoiding drastic disturbances to the extended Kalman filter fusion results caused by low-quality observations and improving the system robustness.
[0014] Compared to the approach of using a fixed observation noise covariance, this implementation improves system stability by establishing an adaptive observation noise covariance modeling mechanism.
[0015] In some possible implementations of the first aspect, the robot's pose is corrected based on the second pose, including: Determine the residual norm between the theoretical observation pose and the observed pose. The residual norm is used to characterize the degree of deviation between the actual observation pose and the theoretical observation pose. If the residual norm is less than or equal to the first threshold, the robot's pose is corrected according to the second pose. If the residual norm is greater than the second threshold, the second pose is recorded, and the step of correcting the robot's pose based on the second pose is not triggered; the second threshold is greater than the first threshold. If the residual norm is greater than the first threshold and less than or equal to the second threshold, the update amount is scaled using a scaling factor to obtain the scaled update amount. Based on the scaled update amount and the predicted pose, the scaled second pose is obtained, and then the robot's pose is corrected based on the scaled second pose. The update amount is the product of the Kalman gain and the residual, which is the difference between the observed pose and the theoretical observed pose.
[0016] In this implementation, hierarchical correction is performed based on the relationship between the residual norm and multiple thresholds. When the residual norm is greater than the first threshold and less than or equal to the second threshold, the update amount is scaled using a scaling factor before pose correction, achieving smooth correction. Hierarchical correction and smooth correction are continuous, constrained, and gradual correction processes. Compared to correction methods that simply replace pose, this embodiment avoids the problem of unstable robot chassis control caused by instantaneous pose jumps, avoids drastic changes in robot control caused by occasional errors in the third-view perspective, avoids large instantaneous deviations, thereby ensuring the robot's operational safety while also maintaining positioning accuracy.
[0017] In some possible implementations of the first aspect, the scaling factor is the ratio of a first difference to a second difference, where the first difference is the difference between the second threshold and the residual norm, and the second difference is the difference between the second threshold and the first threshold.
[0018] In some possible implementations of the first aspect, the robot's pose is corrected based on the second pose or a scaled second pose, including: Determine the pose correction amount based on the second pose or the scaled second pose; Using a coordinate system mapping correction method, a smooth offset from the map coordinate system to the odometry coordinate system is generated based on the pose correction amount, and the smooth offset is published. Based on the smooth offset, the global positioning results of the upper planning module are corrected to modify the robot's pose to a second pose or a scaled second pose. The smooth offset is subjected to time-domain low-pass filtering to ensure that the translation and angle change rates during the pose correction process are within a preset range.
[0019] In this implementation, the correction generated after fusion is not directly sent to the underlying driver as an "instantaneous absolute pose command", but is sent to the underlying driver by a coordinate system mapping correction method, which ensures the operational safety of the stack pool robot.
[0020] In some possible implementations of the first aspect, the robot's pose is corrected based on the second pose or a scaled second pose, including: Use the second pose or the scaled second pose as the reference pose of the planning module or the global target point offset; Based on the reference pose or global target point offset, local replanning is triggered to correct the robot's pose to a second pose or a scaled second pose.
[0021] In this implementation, the correction generated after fusion is not directly sent to the underlying driver as an "instantaneous absolute pose command", but is sent to the underlying driver using a planned bias correction method, which ensures the operational safety of the stack pool robot.
[0022] In some possible implementations of the first aspect, the method further includes: obtaining rasterized semantic information based on images captured by a third-view camera device; and performing map registration between the rasterized semantic information and a raster map based on a registration transformation matrix, so as to map the rasterized semantic information to the corresponding raster in the raster map.
[0023] In this implementation, the rasterized semantic information such as pool wall boundaries, obstacle locations, and pollution point distribution given by the third-view image can also be mapped to the corresponding grids in the raster map, so as to utilize the raster semantic information of the third view to achieve global consistency constraints.
[0024] Secondly, embodiments of this application provide a pose correction device for a nuclear power plant reactor pool robot, comprising: The third-view image acquisition module is used to acquire robot images captured by the third-view camera device, which is used to capture images of the robot at the bottom of the stack pool from a third-view perspective. The mapping module is used to map the robot pixel coordinates in the robot image to the world plane coordinate system, so as to obtain the robot's first pose in the world plane coordinate system at the first moment. The time compensation module is used to obtain the robot's observed pose at the second moment based on the first pose. The first moment is earlier than the second moment. The difference between the second moment and the first moment is equal to the delay time. The delay time is the difference between the time when the fusion node receives the observed pose and the time when the robot image is acquired. The pose correction module is used to correct the predicted pose at the second time step using the observed pose, and obtain the second pose. The predicted pose at the second time step is determined based on the sensor data of the robot's onboard sensors. The correction module is used to correct the robot's pose based on the second pose.
[0025] Thirdly, embodiments of this application provide an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the method as described in any of the first aspects above.
[0026] Fourthly, embodiments of this application provide a computer-readable storage medium storing a computer program that, when executed by a processor, implements the method described in any of the first aspects above.
[0027] Fifthly, embodiments of this application provide a computer program product that, when run on an electronic device, causes the electronic device to perform the method described in any one of the first aspects above.
[0028] It is understood that the beneficial effects of the second to fifth aspects mentioned above can be found in the relevant descriptions in the first aspect mentioned above, and will not be repeated here. Attached Figure Description
[0029] To more clearly illustrate the technical solutions in the embodiments of this application, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0030] Figure 1 A schematic block diagram of a nuclear power plant reactor pool robot pose correction system provided in an embodiment of this application; Figure 2 A schematic flowchart of a nuclear power plant reactor pool robot pose correction method provided in an embodiment of this application; Figure 3 A schematic flowchart illustrating the EKF filter fusion correction process provided in this application embodiment; Figure 4 A schematic flowchart illustrating the gating judgment and smoothing correction process provided in this application embodiment; Figure 5 A schematic block diagram of the structure of the nuclear power plant reactor pool robot pose correction device provided in the embodiments of this application; Figure 6 A schematic block diagram of the structure of an electronic device provided in an embodiment of this application. Detailed Implementation
[0031] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods have been omitted so as not to obscure the description of this application with unnecessary detail.
[0032] It should be understood that, when used in this application specification and the appended claims, the term "comprising" indicates the presence of the described features, integrals, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or a collection thereof.
[0033] It should also be understood that the term “and / or” as used in this application specification and the appended claims means any combination of one or more of the associated listed items and all possible combinations, and includes such combinations.
[0034] As used in this application specification and the appended claims, the term "if" may be interpreted, depending on the context, as "when," "once," "in response to determination," or "in response to detection." Similarly, the phrase "if determined" or "if detected [the described condition or event]" may be interpreted, depending on the context, as meaning "once determined," "in response to determination," "once detected [the described condition or event]," or "in response to detection [the described condition or event]."
[0035] Furthermore, in the description of this application and the appended claims, the terms "first," "second," "third," etc., are used only to distinguish descriptions and should not be construed as indicating or implying relative importance.
[0036] References to "one embodiment" or "some embodiments" as described in this specification mean that one or more embodiments of this application include a specific feature, structure, or characteristic described in connection with that embodiment. Therefore, the phrases "in one embodiment," "in some embodiments," "in other embodiments," "in still other embodiments," etc., appearing in different parts of this specification do not necessarily refer to the same embodiment, but rather mean "one or more, but not all, embodiments," unless otherwise specifically emphasized. The terms "comprising," "including," "having," and variations thereof mean "including but not limited to," unless otherwise specifically emphasized.
[0037] In related technologies, when a pooling robot operates at the bottom of a pool, it typically relies on onboard sensors for navigation and localization. Onboard sensors refer to sensors mounted on the pooling robot, which may include, but are not limited to, cameras, radar, and inertial measurement units (IMUs). More specifically, the cameras on the pooling robot may include both front and rear cameras. For example, the pooling robot performs multi-sensor data fusion using radar, cameras, and IMUs, and determines its own pose using Simultaneous Localization and Mapping (SLAM) algorithms, then navigates and localizes based on its pose.
[0038] However, during the research process, this application found that the bottom of the stacking pool is mostly a smooth stainless steel structure with few texture features and complex light reflection, making it difficult for the vehicle-mounted vision system to stably extract features. Furthermore, stacking pool robots typically need to operate continuously for several hours or even longer during stacking pool cleaning operations. During long-term operation, due to limitations such as robot odometer errors and visual feature degradation, the errors generated by positioning relying on its own sensors increase linearly or sublinearly over time, leading to cumulative positioning drift. Because the stacking pool environment is characterized by feature scarcity, glare, and occlusion, the vehicle-mounted vision system struggles to stably extract features, thus making it impossible to rely on the vehicle-mounted vision system to eliminate the cumulative positioning drift problem.
[0039] To address the aforementioned issues, this application's embodiments introduce a third-view observation pose as an external absolute pose constraint to correct the pose determined by the robot's own sensors. This significantly reduces positioning drift in the feature-scarce and easily disturbed nuclear power plant reactor pool environment, keeping the positioning error within a stable range and obtaining a more accurate robot pose.
[0040] The third-person perspective observation pose does not depend on the robot's own movement process, thus solving the problem of cumulative positioning drift caused by odometry errors and visual feature degradation in stacked robots.
[0041] Please see Figure 1 This is a schematic block diagram of a nuclear power plant reactor pool robot pose correction system provided in the embodiments of this application. The system may include a third-view observation subsystem 110, a local perception and motion estimation subsystem 120, and a fusion and correction subsystem 130.
[0042] The third-view observation subsystem 110 provides the third-view observation pose to the fusion and correction subsystem 130, while the local perception and motion estimation subsystem 120 provides the pose predicted by its own sensors to the fusion and correction subsystem 130. The fusion and correction subsystem 130 uses the third-view observation pose to correct the pose predicted by its own sensors and outputs the corrected pose.
[0043] The third-view observation subsystem 110 may include a fixed or movable third-view camera positioned at a high altitude. This third-view camera is not mounted on the stacking robot, but rather above the stacking pool or along its edge, for a high-altitude view of the stacking pool. Thus, when the stacking robot is working at the bottom of the stacking pool, the third-view camera captures an aerial image of the robot.
[0044] Reactor pool robots can include, but are not limited to, cleaning robots and wiping robots. Cleaning robots are used to clean the reactor pool. For example, the end effector of a cleaning robot's robotic arm carries a foam gun capable of spraying foam and a high-pressure water gun capable of spraying high-pressure water. The foam gun and high-pressure water gun can be used to complete rinsing and cleaning operations. Wiping robots are used to wipe the cleaned areas. Typically, the wiping robot carries a squeegee and a wiping brush, enabling it to scrape water, suck up contaminants, and wipe away residual wastewater. By jointly scheduling cleaning robots and wiping robots to perform decontamination work, the cleaning and wiping operations of the reactor pool can be achieved.
[0045] After acquiring robot images from a third-person perspective, the robot's pose in the world coordinate system (i.e., the third-person observation pose) can be obtained through processing such as distortion correction, robot detection, and planar projection. The third-person observation pose is a fully covered top-down observation source.
[0046] After obtaining the observation pose from a third-view camera, raster semantic information can be obtained from the images captured by the third-view camera. Raster semantic information can include, for example, semantic information such as pool wall boundaries, obstacle locations, and the distribution of contamination points.
[0047] The third-view observation subsystem 110 can provide the third-view observation pose and raster semantic information to the fusion and correction subsystem 130. The third-view observation pose and raster semantic information can be used as global constraints to correct the local SLAM trajectory, correct the raster map, and smooth the pose deviation.
[0048] In practical applications, the third-view observation subsystem 110 can provide the third-view observation pose to the fusion and correction subsystem 130 at a low frequency (e.g., 1~10Hz).
[0049] The local perception and motion estimation subsystem 120 may include one or more sensors mounted on the stacking robot itself. For example, it may include sensors such as vehicle-mounted cameras, LiDAR, odometry, and IMUs mounted on the stacking robot. Sensor data is collected by the stacking robot's own sensors, and SLAM calculations are performed based on the sensor data for navigation and localization.
[0050] In specific applications, the local perception and motion estimation subsystem 120 can predict the robot's pose based on sensor data from multiple sensors and provide the predicted pose to the fusion and correction subsystem 130.
[0051] The fusion and correction subsystem 130 is used to correct the pose predicted by its own sensors using the observed pose from a third-person perspective, thereby obtaining a corrected robot pose. In specific applications, the fusion and correction subsystem 130 can perform Extended Kalman Filter (EKF) fusion based on the predicted pose and the observed pose to obtain the corrected pose.
[0052] Furthermore, the fusion and correction subsystem 130 can also use raster semantic information to correct corresponding rasters in the raster map. For example, pool wall boundary semantic information can be mapped to corresponding rasters in the raster map to correct these rasters to pool wall boundary rasters.
[0053] The fusion and correction subsystem 130 can also use the observation pose from a third-person perspective to correct the local SLAM trajectory.
[0054] In practical applications, the fusion and correction subsystem 130 can use processes such as time alignment, observation confidence modeling, gating judgment and smooth fusion to apply the observation pose and grid semantic information obtained by the third-view camera to the local SLAM and the global map in a hierarchical manner.
[0055] Please see Figure 2 This is a schematic flowchart of a nuclear power plant reactor pool robot pose correction method provided in an embodiment of this application. The method may include the following steps: Step S201: Obtain robot images captured by a third-view camera device. The third-view camera device is used to capture images of the robot at the bottom of the pool from a third-view perspective.
[0056] The third-view camera device can specifically be an external camera, which is not mounted on the stacking robot but rather positioned high above the stacking pool or along its edge, for a high-altitude view of the pool. When the stacking robot is working at the bottom of the pool, images of the robot can be obtained by taking high-altitude photos using the third-view camera device. "Robot image" can refer to any image including the robot.
[0057] In practical applications, the intrinsic and extrinsic parameters of the third-view camera device are pre-calibrated. Intrinsic parameters may include distortion coefficients and intrinsic parameter matrices. Extrinsic parameters may include the transformation matrix projecting the image coordinate system to the world plane coordinate system.
[0058] When calibrating the third-view camera, first fix the camera at the edge of the stack pool or at an elevated position, and define the world plane coordinate system W so that the bottom surface of the stack pool is approximately a plane Z=0; then, perform precise calibration on the third-view camera to obtain the camera intrinsic parameter matrix K (including focal length). With the main point The radial / tangential distortion parameters are obtained; simultaneously, the rotation matrix of the third-view camera relative to the world coordinate system is obtained through extrinsic parameter calibration. With translation vector .
[0059] The calibration method for third-person view cameras can be offline checkerboard calibration or online self-calibration based on fiducial (AprilTag). In addition, the calibration results provide confidence intervals (e.g., the mean and variance of the calibration residuals), which are used as a benchmark term when calculating the measurement noise covariance (or observation noise covariance) in subsequent calculations.
[0060] When capturing robot images using a third-person perspective camera, a timestamp for each robot image is also recorded. The timestamp of the robot image acquisition is used to characterize the time when the image was acquired.
[0061] Step S202: Map the robot pixel coordinates in the robot image to the world plane coordinate system to obtain the robot's first pose in the world plane coordinate system at the first moment.
[0062] In practical applications, after acquiring robot images using a third-person camera, distortion correction can be performed on the robot images first. Then, a robot detection algorithm can be used to detect whether a robot exists in the image and to determine the robot's pixel coordinates. Robot pixel coordinates refer to the coordinates of the robot's location within the robot image.
[0063] Robot pixel coordinates It can be the pixel position of the vehicle body center or reference point (i.e., fiducial).
[0064] In some embodiments, when performing robot detection on a robot image using a robot detection algorithm, the confidence level (or confidence score) of the robot detection algorithm can also be output. For example, if the robot detection algorithm is YOLO, the output is the detection confidence score of the YOLO algorithm. The confidence score of the robot detection algorithm can be used to calculate the subsequent observation noise covariance (or measurement covariance).
[0065] After obtaining the robot's pixel coordinates, the robot's pixel coordinates can be mapped from the image coordinate system to the world plane coordinate system using the pre-calibrated intrinsic and extrinsic parameters of the third-view camera, thereby obtaining the robot's first pose in the world plane coordinate system at the first moment.
[0066] In some embodiments, a homography matrix can be used. Project robot pixel coordinates onto the world plane coordinate system The details are as follows:
[0067] in, Rotation matrix The first two columns, This represents the translation vector in the corresponding basis. Let be the scale factor. Find... Later:
[0068] pass The robot's pixel coordinates can be obtained Projecting onto the world plane coordinate system, we obtain the first pose in the world plane coordinate system at the first moment. .
[0069] In other embodiments, the robot pixel coordinate system can also be projected onto the world plane coordinate system using the Perspective-n-Point (PnP) method.
[0070] If multiple known geometric keypoints (such as front / rear or corner markers) can be detected on the robot image, the PnP method can be used to solve for 6DoF (6 degrees of freedom) in combination with depth or known height, and then the pose can be projected onto Z=0 to obtain the first pose.
[0071] The first pose can include the robot's coordinates (X, Y) and yaw angle in the world coordinate system. Yaw angle The estimate can be obtained by regressing the direction of the vehicle's major axis, the direction of the reference point, or the relative positions of multiple feature points in the image. If PnP provides the transformation from the third-view camera to the world coordinate system... Then use (Depend on The robot's pose can be obtained by converting the coordinates to the world coordinate system. (i.e., the first pose).
[0072] Step S203: Based on the first pose, obtain the robot's observed pose at the second moment. The first moment is earlier than the second moment. The difference between the second moment and the first moment is equal to the delay time. The delay time is the difference between the time when the fusion node receives the observed pose and the time when the robot image is acquired.
[0073] In this embodiment of the application, the first moment is The second moment is The delay time is Robot image acquisition time This can be obtained through the timestamps of the robot images. The timestamps of the robot images can be obtained by hardware logging from the camera. The time it takes for the fusion node to receive the third-person view's observed pose is... Delay time A fusion node can refer to a node used for EKF fusion, such as the fusion and correction subsystem 130. After receiving the observation pose from the third-view perspective, the fusion node uses the observation pose from the third-view perspective to correct the predicted pose.
[0074] There is an end-to-end delay between the acquisition of robot images by the third-person camera and the receipt of the observed pose from the third-person perspective by the fusion node. In order to make the first moment (i.e. The observed robot pose is used for the second time step (i.e., the fusion time step). To correct the robot's pose, the first pose at the first moment needs to be time-compensated to obtain the observed pose at the second moment. This observed pose at the second moment is then used as the observed pose from a third-person perspective to correct the predicted pose at the second moment, thus obtaining the corrected robot pose at the second moment.
[0075] In some embodiments, during the process of obtaining the robot's observed pose at the second moment based on the first pose, the robot's linear velocity at the first moment can be obtained first. and angular velocity Then, based on the linear velocity at the first moment... and the robot's position at the first moment Determine the robot's position at the second moment. Based on the angular velocity at the first moment And the robot's yaw angle at the first moment Determine the robot's yaw angle at the second moment. The first pose includes the position and yaw angle at the first moment; the observed pose. This includes the position at the second moment and the yaw angle at the second moment.
[0076] in, , This can be obtained using an odometer or IMU. and .
[0077] In one implementation, a time-progression formula based on a kinematic model is established: .
[0078] In another implementation, if velocity cannot be obtained directly, an average velocity can be fitted from several odometer or IMU data points. For more precise compensation, a second-order model (with added acceleration 'a') can be used to calculate the position at the second moment. At this point, the second-order model can be: .
[0079] It should be noted that, If the stability is less than 200 ms, uniform speed compensation can be used; If the latency is greater than 500 ms, the system architecture can be optimized or a factor graph can be used to connect historical poses to accurately handle the latency.
[0080] It is worth noting that, considering the end-to-end delay between acquiring the third-person robot image and receiving the observed pose at the fusion node, a time-compensated progressive model (i.e., a time-progressive formula based on the kinematic model) is used to convert the first pose at the first moment into the observed pose at the second moment, so as to perform time correction on the observed pose, making the time of the observed pose consistent with the current filtering time, ensuring physical consistency during fusion calculation, and ensuring timing consistency through time delay compensation.
[0081] Step S204: Using the observed pose, correct the predicted pose at the second moment to obtain the second pose. The predicted pose at the second moment is determined based on the sensor data of the robot's onboard sensors.
[0082] Robot-mounted sensors refer to sensors installed on the stack-pool robot, which may include, but are not limited to, cameras, radar, and IMUs. Using data from radar and IMUs, the predicted pose at a second time step can be obtained. Then, EKF fusion is performed based on the observed pose to correct the predicted pose, obtaining the second pose. The second pose is the corrected robot pose.
[0083] Step S205: Correct the robot's pose based on the second pose.
[0084] In this embodiment, by introducing a third-person perspective observation pose as an external absolute pose constraint, the pose determined by the robot's own sensors is corrected. This significantly reduces positioning drift in the feature-scarce and easily disturbed environment of a nuclear power plant reactor pool, keeping the positioning error within a stable range and obtaining a more accurate robot pose. Furthermore, the third-person perspective observation pose does not depend on the robot's own movement process, thus solving the problem of cumulative positioning drift caused by odometry errors and visual feature degradation in the reactor pool.
[0085] Based on the various embodiments described above, see also the embodiments described in this application. Figure 3 The diagram shown illustrates a flowchart of the EKF filter fusion correction process. The specific process of correcting the predicted pose at the second time step using the observed pose to obtain the second pose may include the following steps: Step S301: Determine the predicted pose at the second moment based on the sensor data and the motion model in the extended Kalman filter.
[0086] For example, the motion model (or state equation) in EKF can be a discrete-time nonlinear motion model, which can be: . For control inputs, such as wheel speed or speed commands. Let the state variable be... (in Linear velocity or expandable to ). , This represents the process noise covariance. The process noise covariance originates from the uncertainty of the odometer or IMU. The values can be, for example, positional variance of 0.01–0.1 m^2 and angular variance of (1–5°)^2. The value can be determined according to the equipment calibration; no specific limit is imposed here. The odometer's cumulative error and IMU noise should be taken into account initially; the initial settings can be adjusted accordingly. .
[0087] Using the third-person viewpoint pose as a measure of the robot's state The measurement (i.e., observation pose).
[0088] The predicted pose at the second time step is obtained through the prediction step in EKF. . Specifically, . yes Position at any given moment yes Control input at each moment. Based on the pose and control input of the previous moment, and using angular velocity and linear velocity acquired by sensors such as odometers or IMUs, the following is predicted: The pose at that moment (i.e., the second pose).
[0089] Step S302: Determine the prediction covariance for the second time step based on the state Jacobian matrix, the process noise covariance, and the covariance of the previous time step. The state Jacobian matrix is the Jacobian matrix of the motion model with respect to the state.
[0090] For example, the state Jacobian matrix The time preceding the second moment is... At this point, the predicted covariance at the second time step... . yes The predictive covariance at time. yes The transpose of .
[0091] Step S303: Determine the observation Jacobian matrix based on the predicted pose and the measurement model in the extended Kalman filter.
[0092] The measurement model in EKF can also be called the observation equation. For example, the measurement model in EKF is: . , It is the observation noise covariance. It refers to the normal distribution.
[0093] In the EKF update step, it is necessary to calculate the observation Jacobian matrix (i.e., the measurement Jacobian matrix). Observation Jacobian matrix .
[0094] Step S304: Determine the Kalman gain based on the prediction covariance, the observation Jacobian matrix, and the observation noise covariance.
[0095] Among them, Kalman gain . yes The transpose of .
[0096] Step S305: Determine the theoretical observation pose at the second moment based on the measurement model and the predicted pose.
[0097] Step S306: Determine the second pose based on the observed pose, predicted pose, theoretical observed pose, and Kalman gain.
[0098] In the EKF update step, state and covariance updates can be performed based on the Kalman gain. Specifically, the second pose... . This is the observation pose at the second moment. It is the theoretical observation pose at the second moment. This is the deviation between the measured pose and the predicted pose. The updated covariance. .
[0099] In this embodiment, the observation pose of an additional third-view perspective is used as the observation pose of the extended Kalman filter at the second moment, so as to use the observation pose to correct the predicted pose in the extended Kalman filter, thereby improving the positioning accuracy.
[0100] In some embodiments of this application, the observation noise covariance It can be a fixed value. That is, in multi-sensor fusion, a fixed observation noise covariance is used.
[0101] However, during the research process, this application found that in the stack pool environment, the quality of third-view observations varies greatly with illumination, water film reflection and occlusion. Therefore, using a fixed observation noise covariance may cause low-quality observations to cause severe disturbances to the fusion results, resulting in poor system robustness.
[0102] In other embodiments of this application, an adaptive observation noise covariance modeling mechanism is established so that the measurement weights are automatically adjusted according to the image quality, thereby avoiding low-quality observations from dominating the fusion, improving the fusion stability and making the system more robust.
[0103] In other words, before determining the Kalman gain based on the prediction covariance, the observation Jacobian matrix, and the observation noise covariance, the confidence score can be obtained first. and occlusion index The confidence score describes the confidence level of the robot detection algorithm, while the occlusion index describes the degree of occlusion in the field of view of the third-view camera device. . It is an occlusion index or visibility index, where 1 indicates an unobstructed and complete field of view. It can be determined by methods such as the proportion of the detection box, the number of key points, or image entropy estimation.
[0104] Then, the observation noise covariance is determined based on the baseline observation noise, confidence sensitivity, occlusion amplification factor, confidence score, and occlusion index.
[0105] For example, observation noise covariance .
[0106] in: ; ; .
[0107] and This is the baseline observation noise. For example, m, . This represents the confidence level sensitivity (typically 1–2). This is the occlusion magnification factor (typically 0.5–2).
[0108] In this embodiment, the observation pose from a third-person perspective is considered as a noisy measurement. Its observation noise covariance It can reflect image quality, detection confidence, and occlusion status.
[0109] When the confidence level is high and there is no obstruction, the observation noise covariance is close to the baseline value; when the confidence level is low or there is severe obstruction, the observation noise covariance increases rapidly, thereby automatically reducing the weight of the observation during filter updates and avoiding erroneous corrections.
[0110] In this embodiment, considering that the third-view observation quality in the reactor pool environment varies greatly with illumination, water film reflection, and occlusion, the observation noise covariance is adaptively calculated using confidence scores and occlusion indices, thus proposing a method based on detection confidence. With occlusion rate The dynamic observation noise covariance model enables the observation weights in the extended Kalman filter to be automatically adjusted according to the quality of the third-view image, thereby avoiding the drastic disturbance of the extended Kalman filter fusion result caused by low-quality observations and improving the robustness of the system.
[0111] Compared to the approach of using fixed observation noise covariance, this implementation improves system stability and robustness by establishing an adaptive observation noise covariance modeling mechanism.
[0112] Based on the various embodiments described above, see also the embodiments described in this application. Figure 4 The diagram shown illustrates a flowchart of the gating judgment and smoothing correction process. The specific process of correcting the robot's pose based on the second pose may include the following steps: Step S401: Determine the residual norm between the theoretical observation pose and the observed pose. The residual norm is used to characterize the degree of deviation between the actual observation pose and the theoretical observation pose.
[0113] For example, residual norm In practical applications, the residual norm can be calculated by using the normalized innovation squared (NIS) or a simple Euclidean difference.
[0114] when When the value is large, directly applying the corrected second pose will introduce significant jumps in the robot's pose. To avoid drastic changes in robot control caused by occasional errors in the third-view perspective, a two-level gating and smoothing strategy is designed. The two-level gating refers to a first threshold and a second threshold, which are used to select different processing methods. For example, the first threshold is... The second threshold is .For example, m, ~1.0 m.
[0115] Step S402: If the residual norm is less than or equal to the first threshold, then the robot's pose is corrected according to the second pose.
[0116] when If the deviation is small, the pose can be updated directly using the conventional EKF update method, that is, the robot can be controlled by the second pose to correct the pose.
[0117] Step S403: If the residual norm is greater than the second threshold, then record the second pose and do not trigger the step of correcting the robot's pose based on the second pose; the second threshold is greater than the first threshold.
[0118] when If the deviation is too large, directly updating the pose will result in a large jump in pose. In this case, do not force an update, but only record the current second pose and trigger manual confirmation or perform a weak constraint update.
[0119] Step S404: If the residual norm is greater than the first threshold and less than or equal to the second threshold, the update amount is scaled using a scaling factor to obtain the scaled update amount. Based on the scaled update amount and the predicted pose, the scaled second pose is obtained, and then the robot's pose is corrected based on the scaled second pose. The update amount is the product of the Kalman gain and the residual, and the residual is the difference between the observed pose and the theoretical observed pose.
[0120] when If the deviation is moderate, then it indicates a moderate deviation. In this case, do not directly... As the pose is sent down, pose correction is performed using a smooth fusion method.
[0121] Update volume scaling factor At this point, the scaled second pose . This is the updated amount after scaling. It predicts the pose.
[0122] In some embodiments, the scaling factor is the ratio of a first difference to a second difference, where the first difference is the difference between a second threshold and the residual norm, and the second difference is the difference between the second threshold and the first threshold.
[0123] Follow Monotonically decreasing, for example: ; pass Follow Monotonic reduction allows for more conservative fusion when the deviation is large.
[0124] In addition, to prevent discontinuities in velocity and angular velocity, it is also possible to... Implement rate limits (max position correction per second, max yaw rate correction). For example, you can set a maximum linear velocity correction of 0.1 m / s and a maximum angular velocity correction of 5° / s.
[0125] In contrast, if a forced global correction is performed directly based on the second pose, it may cause a significant jump in the robot's instantaneous pose, affecting the stability of the chassis control and potentially leading to danger and poor safety.
[0126] In this embodiment, hierarchical correction is performed based on the relationship between the residual norm and multiple thresholds. When the residual norm is greater than the first threshold and less than or equal to the second threshold, the update amount is scaled using a scaling factor before pose correction, achieving smooth correction. Hierarchical correction and smooth correction are continuous and constrained progressive correction processes. This avoids the problem of unstable robot chassis control caused by instantaneous pose jumps, avoids drastic changes in robot control caused by occasional errors in the third-view perspective, avoids large instantaneous deviations, and thus ensures the robot's operational safety while also maintaining positioning accuracy.
[0127] Based on the various embodiments described above, in some embodiments of this application, during the process of correcting the robot's pose according to the second pose or the scaled second pose, the pose correction amount can be determined according to the second pose or the scaled second pose; a coordinate system (Transform Framework, TF) mapping correction method is used to generate a smooth offset from the map coordinate system to the odometry coordinate system according to the pose correction amount, and the smooth offset is published; the global positioning result of the upper-layer planning module is corrected according to the smooth offset to correct the robot's pose to the second pose or the scaled second pose; the smooth offset is subjected to time-domain low-pass filtering to ensure that the translation rate and angle rate of change during the pose correction process are within a preset range.
[0128] In practical applications, a tiny smooth offset between map and odom is generated based on the pose correction amount. and publish By continuously releasing small... This allows the upper-level planning module to correct the global position without altering the original sensor data stream for local real-time obstacle avoidance. (Smoothing offset) Low-pass filtering is applied in time to ensure that the linear displacement and angle changes of each correction are within an acceptable rate range.
[0129] By using TF mapping correction, the global pose seen by the upper-level planning module is automatically corrected, and the pose correction process is smooth and without jumps. In this way, the correction generated after fusion is not directly sent to the lower-level driver as an "instantaneous absolute pose command", but is sent to the lower-level driver using coordinate system mapping correction, so as to achieve smooth and safe pose correction and ensure the operational safety of the stacked robot.
[0130] Based on the various embodiments described above, in other embodiments of this application, during the process of correcting the robot's pose according to the second pose or the scaled second pose, the second pose or the scaled second pose can be used as the reference pose or global target point offset of the planning module; based on the reference pose or the global target point offset, local replanning is triggered to correct the robot's pose to the second pose or the scaled second pose.
[0131] In this embodiment, the correction generated after fusion is not directly sent to the underlying driver as an "instantaneous absolute pose command". Instead, it is sent to the underlying driver using a planned bias correction method to achieve smooth and safe pose correction and ensure the operational safety of the stacked robot.
[0132] In this embodiment, a rollback mechanism can also be set during pose correction. Rollback is triggered when a conflict occurs. For example, if a strong conflict with the onboard sensor of the stack-pool robot is detected within a preset time period (e.g., 1-3 seconds) after correction, the system should automatically roll back the most recent correction and generate an alarm or enter manual confirmation mode. All pose correction and rollback events can be logged and uploaded to the operation interface for maintenance personnel to view.
[0133] Based on the various embodiments described above, in some embodiments of this application, rasterized semantic information can also be obtained from images captured by a third-view camera device; and the rasterized semantic information can be map-registered with a raster map according to a registration transformation matrix, so as to map the rasterized semantic information to the corresponding raster in the raster map.
[0134] In addition to providing the observation position from a third-person perspective, the third-person camera device can also provide rasterized semantic information. This rasterized semantic information can include, for example, pool wall boundaries, obstacle locations, and the distribution of contamination points. The rasterized semantic information is registered and aligned with the raster map to map the corresponding rasters in the map. In this way, the raster map can be globally corrected based on the rasterized semantic information provided by the third-person camera device. For example, based on the rasterized semantic information, it can be determined which rasters in the raster map represent pool wall boundaries and which represent obstacle locations.
[0135] In practical applications, a point-to-point registration and alignment method can be adopted. Specifically, several reference points with known locations are arranged along the pool edge or at key positions. These reference points can be observed by the pool-stacking robot and the third-view camera. The pixel positions of the reference points on the image acquired by the third-view camera are detected, and combined with the calibration of the third-view camera, the precise extrinsic parameters from the third-view camera to the world plane coordinate system are calculated. This allows the rasterized semantic information in the image to be directly projected onto the world map plane coordinate system, obtaining a mapping transformation. (Translation + Rotation). Mapping Transformation It can be used for map correction or TF correction with rigid constraints.
[0136] If no reference point is available, rasterized semantic information can be mapped onto a raster map based on feature or point cloud registration alignment methods. Specifically, images captured by a third-view camera are acquired, boundary lines or semantic rasters are extracted from these images, and these boundary lines or semantic rasters are projected onto the raster map coordinate system. Then, the optimal registration transformation T is solved using iterative closest point (ICP) or boundary-based RANSAC. At this point, .
[0137] Boundary lines or semantic grids are denoted as . Projection points in images captured by a third-person camera device This corresponds to a point in the raster map. If the registration residual is below a threshold (e.g., root mean square residual < 0.05–0.2 m), the transformation is accepted and a smoothing correction is applied to the global map or a local window map; otherwise, it is rejected or a local correction is performed. Different regions can be assigned weights during registration. For example, higher weights can be applied to pool walls or known structures to enhance registration robustness.
[0138] In this embodiment of the application, the rasterized semantic information such as pool wall boundaries, obstacle locations, and pollution point distribution given by the third-view image can also be mapped to the corresponding grids in the raster map, so as to utilize the raster semantic information of the third view to achieve global consistency constraints.
[0139] As can be seen from the above, the embodiments of this application, by introducing a third-person perspective observation pose to correct the predicted pose, can solve the problem of cumulative positioning drift caused by the long-term operation of the pool robot. The more accurate positioning of the pool robot allows its working trajectory to be highly consistent with the preset cleaning path, thereby achieving near 100% area coverage, significantly reducing the risk of repetitive work and omissions, and improving the cleaning efficiency of the pool.
[0140] Furthermore, the more accurate positioning of the reactor pool robot can reduce unnecessary movement and correction operations, extend the service life of key components, and reduce maintenance frequency and replacement costs. It can also improve the cleaning coverage and positioning accuracy, thereby ensuring the cleaning quality of the reactor pool, reducing potential radiation risks, improving the decontamination quality of the reactor pool, and thus enhancing the overall safety level of the nuclear power plant.
[0141] It should be understood that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.
[0142] Corresponding to the nuclear power plant reactor pool robot pose correction method described in the above embodiments, Figure 5 The diagram shows a schematic block diagram of the nuclear power plant reactor pool robot pose correction device provided in the embodiments of this application. For ease of explanation, only the parts related to the embodiments of this application are shown.
[0143] Reference Figure 5 The device includes: The third-view image acquisition module 510 is used to acquire robot images captured by the third-view camera device, which is used to capture images of the robot at the bottom of the stack pool from a third-view perspective. The mapping module 520 is used to map the robot pixel coordinates in the robot image to the world plane coordinate system to obtain the robot's first pose in the world plane coordinate system at the first moment. The time compensation module 530 is used to obtain the robot's observed pose at the second moment based on the first pose. The first moment is earlier than the second moment. The difference between the second moment and the first moment is equal to the delay time. The delay time is the difference between the time when the fusion node receives the observed pose and the time when the robot image is acquired. The pose correction module 540 is used to correct the predicted pose at the second time step using the observed pose step, and obtain the second pose step. The predicted pose step at the second time step is determined based on the sensor data of the robot's onboard sensors. The correction module 550 is used to correct the robot's pose based on the second pose.
[0144] In some possible implementations, the time compensation module 530 is specifically used for: The robot's linear velocity and angular velocity at the first moment are obtained; the robot's position at the second moment is determined based on the linear velocity and position at the first moment; the robot's yaw angle at the second moment is determined based on the angular velocity and yaw angle at the first moment; wherein, the first pose includes the position and yaw angle at the first moment; the observed pose includes the position and yaw angle at the second moment.
[0145] In some possible implementations, the pose correction module 540 is specifically used to: determine the predicted pose at the second time step based on sensor data and the motion model in the extended Kalman filter; determine the predicted covariance at the second time step based on the state Jacobian matrix, the process noise covariance, and the covariance of the previous time step; the state Jacobian matrix is the Jacobian matrix of the motion model with respect to the state; determine the observation Jacobian matrix based on the predicted pose and the measurement model in the extended Kalman filter; determine the Kalman gain based on the predicted covariance, the observation Jacobian matrix, and the observation noise covariance; determine the theoretical observation pose at the second time step based on the measurement model and the predicted pose; and determine the second pose based on the observation pose, the predicted pose, the theoretical observation pose, and the Kalman gain.
[0146] In some possible implementations, the device also includes a dynamic calculation module for observation noise covariance, used to: obtain confidence scores and occlusion indices, whereby the confidence scores describe the confidence level of the robot detection algorithm and the occlusion indices describe the degree of occlusion in the field of view of the third-view camera device; and determine the observation noise covariance based on the benchmark observation noise, confidence sensitivity, occlusion magnification factor, confidence scores, and occlusion indices.
[0147] In some possible implementations, the correction module 550 is specifically used to: determine the residual norm between the theoretical observed pose and the observed pose, where the residual norm characterizes the degree of deviation between the actual observed pose and the theoretical observed pose; if the residual norm is less than or equal to a first threshold, then the robot's pose is corrected based on the second pose; if the residual norm is greater than a second threshold, then the second pose is recorded, and the step of correcting the robot's pose based on the second pose is not triggered; the second threshold is greater than the first threshold; if the residual norm is greater than the first threshold and less than or equal to the second threshold, then the update amount is scaled using a scaling factor to obtain a scaled update amount, and a scaled second pose is obtained based on the scaled update amount and the predicted pose, and then the robot's pose is corrected based on the scaled second pose; the update amount is the product of the Kalman gain and the residual, where the residual is the difference between the observed pose and the theoretical observed pose.
[0148] In some possible implementations, the scaling factor is the ratio of a first difference to a second difference, where the first difference is the difference between the second threshold and the residual norm, and the second difference is the difference between the second threshold and the first threshold.
[0149] In some possible implementations, the correction module 550 is specifically used to: determine the pose correction amount based on the second pose or the scaled second pose; generate a smooth offset from the map coordinate system to the odometry coordinate system based on the pose correction amount using a coordinate system mapping correction method, and publish the smooth offset; correct the global positioning results of the upper-level planning module based on the smooth offset, so as to correct the robot's pose to the second pose or the scaled second pose; and perform time-domain low-pass filtering on the smooth offset to ensure that the translation rate and angle rate of change during the pose correction process are within a preset range.
[0150] In some possible implementations, the correction module 550 is specifically used to: use the second pose or the scaled second pose as the reference pose or global target point offset of the planning module; and trigger local replanning based on the reference pose or global target point offset to correct the robot's pose to the second pose or the scaled second pose.
[0151] In some possible implementations, the device also includes a map registration and semantic raster fusion module, used to: obtain rasterized semantic information based on images captured by a third-view camera device; and perform map registration between the rasterized semantic information and the raster map based on a registration transformation matrix, so as to map the rasterized semantic information to the corresponding raster in the raster map.
[0152] It should be noted that the information interaction and execution process between the above-mentioned devices / modules are based on the same concept as the method embodiments of this application. For details on their specific functions and technical effects, please refer to the method embodiments section, and they will not be repeated here.
[0153] Figure 6 This is a schematic block diagram of the structure of an electronic device provided in an embodiment of this application. Figure 6 As shown, the electronic device 6 of this embodiment includes: at least one processor 60 ( Figure 6 (Only one is shown in the diagram), memory 61, and computer program 62 stored in said memory 61 and executable on said at least one processor 60, wherein said processor 60 executes said computer program 62 to implement the steps in any of the above method embodiments.
[0154] The electronic device may include, but is not limited to, a processor 60 and a memory 61. Those skilled in the art will understand that... Figure 6This is merely an example of electronic device 6 and does not constitute a limitation on electronic device 6. It may include more or fewer components than shown, or combine certain components, or different components, such as input / output devices, network access devices, etc.
[0155] The processor 60 may be a Central Processing Unit (CPU), or it may be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor.
[0156] In some embodiments, the memory 61 may be an internal storage unit of the electronic device 6, such as a hard disk or memory of the electronic device 6. In other embodiments, the memory 61 may be an external storage device of the electronic device 6, such as a plug-in hard disk, smart media card (SMC), secure digital (SD) card, flash card, etc., equipped on the electronic device 6. Furthermore, the memory 61 may include both internal and external storage units of the electronic device 6. The memory 61 is used to store the operating system, applications, bootloader, data, and other programs, such as the program code of the computer program. The memory 61 can also be used to temporarily store data that has been output or will be output.
[0157] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is merely an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.
[0158] Furthermore, the specific names of each functional unit and module are merely for ease of differentiation and are not intended to limit the scope of protection of this application. The specific working processes of the units and modules in the above system can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.
[0159] This application also provides an electronic device, which includes: at least one processor, a memory, and a computer program stored in the memory and executable on the at least one processor, wherein the processor executes the computer program to implement the steps in any of the above method embodiments.
[0160] This application also provides a computer-readable storage medium storing a computer program that, when executed by a processor, can implement the steps in the above-described method embodiments.
[0161] This application provides a computer program product that, when run on an electronic device, enables the electronic device to implement the steps described in the various method embodiments above.
[0162] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments of this application can be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include at least: any entity or device capable of carrying computer program code to a photographing device / terminal device, a recording medium, a computer memory, a read-only memory (ROM), a random access memory (RAM), an electrical carrier signal, a telecommunication signal, and a software distribution medium. Examples include USB flash drives, portable hard drives, magnetic disks, or optical disks. In some jurisdictions, according to legislation and patent practice, computer-readable media cannot be electrical carrier signals or telecommunication signals.
[0163] In the above embodiments, the descriptions of each embodiment have different focuses. For parts that are not described in detail or recorded in a certain embodiment, please refer to the relevant descriptions of other embodiments.
[0164] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.
[0165] In the embodiments provided in this application, it should be understood that the disclosed devices, electronic devices, and methods can be implemented in other ways. For example, the device / electronic device embodiments described above are merely illustrative. For instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the displayed or discussed mutual couplings or direct couplings or communication connections may be through some interfaces; indirect couplings or communication connections between devices or units may be electrical, mechanical, or other forms.
[0166] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.
[0167] The above-described embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be included within the protection scope of this application.
Claims
1. A method for correcting the pose of a robot in a nuclear power plant reactor pool, characterized in that, include: The robot image is acquired by a third-view camera device, which is used to acquire images of the robot at the bottom of the pool from a third-view perspective. Map the robot pixel coordinates in the robot image to the world plane coordinate system to obtain the robot's first pose in the world plane coordinate system at the first moment; Based on the first pose, the observation pose of the robot at the second moment is obtained. The first moment is earlier than the second moment. The difference between the second moment and the first moment is equal to the delay time. The delay time is the difference between the time when the fusion node receives the observation pose and the time when the robot image is acquired. Using the observed pose, the predicted pose at the second moment is corrected to obtain the second pose, which is determined based on the sensor data of the robot's onboard sensor. The robot's pose is corrected based on the second pose.
2. The method as described in claim 1, characterized in that, Based on the first pose, the observed pose of the robot at the second moment is obtained, including: Obtain the linear velocity and angular velocity of the robot at the first moment; The position of the robot at the second moment is determined based on the linear velocity at the first moment and the position of the robot at the first moment. Based on the angular velocity at the first moment and the yaw angle of the robot at the first moment, determine the yaw angle of the robot at the second moment; The first pose includes the position at the first moment and the yaw angle at the first moment; the observed pose includes the position at the second moment and the yaw angle at the second moment.
3. The method as described in claim 1, characterized in that, Using the observed pose, the predicted pose at the second time moment is corrected to obtain the second pose, including: The predicted pose at the second moment is determined based on the sensor data and the motion model in the extended Kalman filter. The prediction covariance at the second moment is determined based on the state Jacobian matrix, the process noise covariance, and the covariance of the previous moment before the second moment; the state Jacobian matrix is the Jacobian matrix of the motion model with respect to the state. The observation Jacobian matrix is determined based on the predicted pose and the measurement model in the extended Kalman filter; The Kalman gain is determined based on the predicted covariance, the observed Jacobian matrix, and the observed noise covariance. Based on the measurement model and the predicted pose, the theoretical observed pose at the second moment is determined; The second pose is determined based on the observed pose, the predicted pose, the theoretical observed pose, and the Kalman gain.
4. The method as described in claim 3, characterized in that, Before determining the Kalman gain based on the predicted covariance, the observed Jacobian matrix, and the observed noise covariance, the method further includes: Obtain a confidence score and an occlusion index. The confidence score is used to characterize the confidence level of the robot detection algorithm, and the occlusion index is used to characterize the degree of occlusion in the field of view of the third-view camera device. The observation noise covariance is determined based on the baseline observation noise, confidence sensitivity, occlusion amplification factor, the confidence score, and the occlusion index.
5. The method as described in claim 3, characterized in that, The robot's pose is corrected based on the second pose, including: Determine the residual norm between the theoretical observation pose and the observed pose, wherein the residual norm is used to characterize the degree of deviation between the actual observation pose and the theoretical observation pose; If the residual norm is less than or equal to the first threshold, then the robot's pose is corrected according to the second pose; If the residual norm is greater than the second threshold, the second pose is recorded, and the step of correcting the robot's pose based on the second pose is not triggered; the second threshold is greater than the first threshold. If the residual norm is greater than the first threshold and less than or equal to the second threshold, the update amount is scaled using a scaling factor to obtain a scaled update amount. Based on the scaled update amount and the predicted pose, a scaled second pose is obtained, and then the robot's pose is corrected based on the scaled second pose. The update amount is the product of the Kalman gain and the residual, where the residual is the difference between the observed pose and the theoretical observed pose.
6. The method as described in claim 5, characterized in that, The scaling factor is the ratio of a first difference to a second difference, where the first difference is the difference between the second threshold and the residual norm, and the second difference is the difference between the second threshold and the first threshold.
7. The method as described in claim 5, characterized in that, Correcting the robot's pose based on the second pose or the scaled second pose includes: Determine the pose correction amount based on the second pose or the scaled second pose; Using a coordinate system mapping correction method, a smooth offset from the map coordinate system to the odometry coordinate system is generated based on the pose correction amount, and the smooth offset is published. Based on the smooth offset, the global positioning result of the planning module is corrected to modify the robot's pose to the second pose or the scaled second pose. The smooth offset is subjected to time-domain low-pass filtering to ensure that the translation rate and angle rate of change during the pose correction process are within a preset range.
8. The method as described in claim 5, characterized in that, Correcting the robot's pose based on the second pose or the scaled second pose includes: The second pose or the scaled second pose is determined as the reference pose of the planning module or the global target point offset; Based on the reference pose or the global target point offset, the planning module is triggered to perform local replanning to correct the robot's pose to the second pose or the scaled second pose.
9. The method according to any one of claims 1 to 6, characterized in that, The method further includes: Based on the images captured by the third-view camera device, rasterized semantic information is obtained; Based on the registration transformation matrix, the rasterized semantic information is registered with the raster map to map the rasterized semantic information to the corresponding raster in the raster map.
10. A pose correction device for a nuclear power plant reactor pool robot, characterized in that, include: The third-view image acquisition module is used to acquire robot images captured by the third-view camera device, which is used to capture images of the robot at the bottom of the pile from a third-view perspective. The mapping module is used to map the robot pixel coordinates in the robot image to the world plane coordinate system to obtain the first pose of the robot in the world plane coordinate system at the first moment. The time compensation module is used to obtain the observation pose of the robot at a second moment based on the first pose. The first moment is earlier than the second moment. The difference between the second moment and the first moment is equal to the delay time. The delay time is the difference between the time when the fusion node receives the observation pose and the time when the robot image is acquired. The pose correction module is used to correct the predicted pose at the second moment using the observed pose to obtain the second pose, wherein the predicted pose at the second moment is determined based on the sensor data of the robot's onboard sensor. The correction module is used to correct the pose of the robot based on the second pose.
11. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the method as described in any one of claims 1 to 9.
12. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by a processor, it implements the method as described in any one of claims 1 to 9.
