A robot path planning method and system based on vision detection

By evaluating visual information and the state of the robotic arm, effective pose correction values ​​are generated and control commands are adjusted, solving the pose deviation problem caused by visual sensor vibration. This improves the accuracy and stability of robot path planning and ensures stable alignment of the end effector.

CN123060692APending Publication Date: 2026-11-10SHENZHEN FULLHOLL TECH CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202611356907.0
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2026-09-03
Publication Date
2026-11-10

AI Technical Summary

Technical Problem

During the visual closed-loop fine alignment stage of agricultural harvesting robots, high-frequency vibration of the visual sensor causes image blurring, resulting in pose estimation deviations that include vibration noise. This causes the end effector to be unable to stably converge to the pose error range allowed for harvesting.

Method used

By acquiring target visual information and robotic arm motion state information, the image quality, displacement consistency, and disturbance index are evaluated to determine the confidence level of the initial pose deviation, generate effective pose correction amounts, adjust the robotic arm control commands according to the confidence level, monitor the execution status of control commands to avoid disturbances, and initiate recovery processing.

Benefits of technology

It significantly improves the accuracy and stability of robot path planning, suppresses robotic arm vibration, ensures that the end effector converges stably to the target position, and improves work efficiency and success rate.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN123060692A_ABST
    Figure CN123060692A_ABST
Patent Text Reader

Abstract

This invention discloses a robot path planning method and system based on vision detection, relating to the field of robot path planning based on vision detection, for improving the accuracy of robot path planning. The method includes: determining the initial pose deviation of the target object, the image quality score of the target visual information, and the displacement consistency score based on target visual information; determining a disturbance index based on the robot arm motion state information; determining the confidence level of the initial pose deviation based on the disturbance index, image quality score, and displacement consistency score; generating an effective pose correction amount based on the confidence level and the initial pose deviation; generating robot arm control commands based on the effective pose correction amount and the confidence level, and adjusting the execution mode of the robot arm control commands according to the confidence level; and initiating recovery processing when continuous disturbance is detected or the convergence state is non-converged.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] This invention relates to the field of robot path planning based on vision detection, and more particularly to a robot path planning method and system based on vision detection. Background Technology

[0002] To dynamically adjust the robotic arm's trajectory based on crop growth status, existing technologies for agricultural harvesting robots typically employ vision-based path planning methods. During actual execution, a vision sensor continuously acquires images, a pose estimation algorithm calculates the deviation of the target fruit relative to the robotic arm's end effector, and generates correction commands to drive joint movement, thus achieving a closed-loop evolution process where the end effector gradually approaches the fruit. To obtain high end-effector positioning accuracy, the vision sensor is usually mounted on the robotic arm's wrist near the end effector, directly participating in the perception input stage of the closed-loop correction.

[0003] When the robotic arm, guided by a visual closed-loop system, enters the precise alignment stage close to the fruit, it needs to execute a series of short-stroke, multi-directional micro-motion adjustment commands. Under these conditions, the frequent start-stop and reversal movements of the robotic arm's wrist joint transmit high-frequency micro-vibrations to the visual sensor through structural components. Since the exposure time of the visual sensor often cannot completely freeze these high-frequency micro-vibrations, a non-negligible relative displacement occurs between the sensor and the fruit at the moment of image acquisition, resulting in directional motion blur or minor geometric distortion in the acquired image.

[0004] Once such a blurred or distorted image enters the pose estimation algorithm, the extraction position of the fruit feature points will randomly shift, causing the estimated target pose to be superimposed with noise components caused by vibration. The closed-loop controller will directly convert this visual deviation value containing spurious bias components into a correction command for the next cycle, controlling the robotic arm to fine-tune in a direction that does not exist. This unnecessary fine-tuning not only fails to eliminate the real residual deviation but also excites new vibration patterns. Existing conventional visual closed-loop control methods usually assume that the image acquired by the sensor can accurately reflect the current static deviation, lacking a mechanism for identifying and isolating the reverse interference of physical vibrations generated by the closed-loop action itself on the sensor input. This makes it easy to misjudge the pose jump caused by image blurring as a real position movement. Therefore, in the fine alignment stage of the visual closed-loop at the end of the agricultural harvesting robot arm, when the visual sensor generates high-frequency vibrations as the robotic arm makes fine adjustments, the pose estimation deviation caused by motion blur in the acquired image contains vibration noise. This noise is converted into unnecessary end-effector jitter commands through the closed loop, ultimately causing the end effector to exhibit unconverged, continuous high-frequency jitter near the fruit, making it impossible to stably reach the allowable pose error range for harvesting. Summary of the Invention

[0005] This application discloses a robot path planning method and system based on vision detection, which aims to solve the problem that in the fine alignment stage of the end effector of an agricultural harvesting robot, the high-frequency vibration of the vision sensor causes image blurring, which in turn causes the pose estimation deviation to include vibration noise. Ultimately, this results in the end effector exhibiting continuous high-frequency jitter that cannot converge near the fruit, making it impossible to stably reach the allowable pose error range for harvesting.

[0006] In a first aspect, this application discloses a robot path planning method based on vision detection, comprising the following steps: Acquire visual information about the target and motion state information of the robot's robotic arm; Based on the target visual information, the initial pose deviation of the target object, the image quality score of the target visual information, and the displacement consistency score are determined. The image quality score is used to characterize the reliability of the target visual information; the displacement consistency score is used to characterize the motion consistency of feature points in the target visual information across consecutive frames. Based on the motion state information of the robotic arm, the perturbation index is determined; the perturbation index is used to characterize the degree of physical perturbation at the moment of acquisition of visual information of the target. The confidence level of the initial pose deviation is determined based on the perturbation index, image quality score, and displacement consistency score. Based on the confidence level and the initial pose deviation, generate the effective pose correction amount; Based on the effective pose correction amount and the confidence level, control commands for the robotic arm are generated, and the execution method of the control commands is adjusted according to the confidence level to avoid inducing new disturbances. Monitor the execution status of the robotic arm control commands, verify the convergence status of the robotic arm aligned with the target object, and initiate recovery processing when continuous disturbance is detected or the convergence status is not converged; the recovery processing includes at least one of the following: retreating a preset distance, re-executing the robotic arm control commands.

[0007] Optionally, based on the confidence level and the initial pose deviation, an effective pose correction amount is generated, including: When the confidence level indicates low confidence, the consistency of the axial component of the initial pose deviation is assessed to obtain a preliminary correction vector. Based on the directional deflection angle between the initial correction vector and the historical effective trajectory direction, the initial correction vector is subjected to directional constraint reconstruction processing to obtain the effective pose correction amount.

[0008] Optionally, based on the directional deflection angle between the initial correction vector and the historical effective trajectory direction, the initial correction vector is subjected to directional constraint reconstruction processing to obtain the effective pose correction amount, including: Based on the directional deflection angle between the preliminary correction vector and the historical effective trajectory direction, determine whether the directional deflection angle is greater than the safety threshold; When the direction deflection angle is greater than the safety threshold, multiple valid trajectory direction vectors with an angle less than the preset angle threshold are selected from the pre-cached historical valid trajectory direction vectors. Determine whether the number of valid trajectory direction vectors selected meets the minimum requirement; If the minimum number requirement is met, the average direction of the selected effective trajectory direction vectors is calculated and used as the adaptive reference direction at the current moment. Project the vector corresponding to the initial pose deviation onto the adaptive reference direction and calculate the projection length; Using the adaptive reference direction as a reference, and the projection length multiplied by the attenuation coefficient as the modulus, a new orientation constraint correction vector is reconstructed to obtain the effective pose correction amount. If the number of selected vectors does not meet the minimum requirement, all axial components are set to zero, and a zero vector is output to obtain the effective pose correction amount. When the directional deflection angle is less than or equal to the safety threshold, the direction of the historical effective trajectory is used for correction to obtain the effective pose correction amount.

[0009] Optionally, based on the robotic arm's motion state information, the disturbance index is determined, including: Based on the motion state information of the robotic arm, the joint reversal frequency, theoretical acceleration jump amplitude, and actual vibration energy index are determined. The joint reversal frequency is the frequency of directional change of the joint velocity of the robotic arm. The theoretical acceleration jump amplitude is the theoretical linear acceleration and angular acceleration of the end effector mapped from the angular velocity and angular acceleration of the joints of the robotic arm. The actual vibration energy index is the root mean square value of the vibration acceleration in the vibration signal of the robotic arm. The disturbance index is obtained by weighting and fusing the joint reversal frequency, the theoretical acceleration jump amplitude, and the actual vibration energy index.

[0010] Optionally, an image quality score for the target visual information is determined, including: Extract the target visual information edge gradients in the horizontal and vertical directions, and calculate the gradient magnitude and gradient direction; Construct an oriented gradient histogram based on the gradient magnitude and gradient direction; The variance and peak value ratio of the directional gradient histogram are calculated, and combined with the Laplacian variance, to obtain the image quality score.

[0011] Optionally, determine the displacement consistency score, including: Calculate the directional angle and magnitude change rate of the displacement vectors of feature points between consecutive frames; If the angle between the directions of the displacement vectors of a feature point between two adjacent frames is less than a set angle threshold and the rate of change of the magnitude is less than a preset threshold, then the feature point is determined to be a target feature point. The proportion of target feature points among all tracked feature points is statistically analyzed to obtain a displacement consistency score.

[0012] Optionally, the confidence level of the initial pose deviation is determined based on the perturbation index, image quality score, and displacement consistency score, including: If the first preset condition is met, the confidence level of the initial pose deviation is determined to be extremely low confidence; the first preset condition is that the perturbation index exceeds a preset perturbation threshold or the image quality score is lower than the minimum usable threshold. If the first preset condition is not met, a comprehensive confidence score is calculated; the comprehensive confidence score is directly proportional to the image quality score and the displacement consistency score, and inversely proportional to the perturbation index. If the overall confidence score is greater than the score threshold, the confidence level of the initial pose deviation is determined to be high confidence. If the overall confidence score is less than or equal to the score threshold, the confidence level of the initial pose deviation is determined to be low confidence.

[0013] Optionally, based on the effective pose correction amount and the confidence level, control commands for the robotic arm are generated, and the execution method of the control commands is adjusted according to the confidence level to avoid inducing new disturbances, including: When the confidence level indicates low confidence, evaluate the ratio of the effective pose correction modulus to the historical average correction modulus. When the ratio is within the preset effective correction range, an asymmetric S-shaped speed curve is used when generating the joint speed command of the robotic arm. Based on the confidence level and the effective pose correction modulus length, the acceleration and deceleration durations of the S-shaped velocity curve are dynamically adjusted so that the acceleration duration is less than the deceleration duration. When generating the joint angular displacement increment of the robotic arm, an attenuation coefficient that is inversely proportional to the confidence level and the effective pose correction modulus is introduced. Within each control cycle, the upper limit of the speed for the next cycle is dynamically adjusted based on the real-time speed and torque feedback of each joint of the robotic arm.

[0014] Optionally, an attenuation coefficient inversely proportional to the confidence level and the effective pose correction modulus may be introduced, including: The motion inertia state of the robotic arm is evaluated based on the end-effector velocity and acceleration, as well as the real-time torque feedback of each joint. The attenuation coefficient is determined based on the confidence level, the effective pose correction modulus, and the motion inertia state of the robotic arm.

[0015] Secondly, this application also discloses a vision-based robot path planning system, which includes: The information acquisition module is used to acquire visual information of the target and motion state information of the robot's robotic arm; The calculation module is used to determine the initial pose deviation of the target object, the image quality score of the target visual information, and the displacement consistency score based on the target visual information; the image quality score is used to characterize the reliability of the target visual information; the displacement consistency score is used to characterize the motion consistency of feature points in the target visual information across consecutive frames. The physical disturbance assessment module is used to determine the disturbance index based on the robotic arm's motion state information; the disturbance index is used to characterize the degree of physical disturbance at the moment of acquisition of target visual information; The credibility determination module is used to determine the credibility level of the initial pose deviation based on the perturbation index, image quality score, and displacement consistency score. The correction amount generation module is used to generate effective pose correction amounts based on the confidence level and the initial pose deviation. The control command generation module is used to generate robotic arm control commands based on the effective pose correction amount and the confidence level, and adjust the execution mode of the robotic arm control commands according to the confidence level to avoid inducing new disturbances. The status monitoring module is used to monitor the execution status of the robotic arm control commands, verify the convergence status of the robotic arm aligned with the target object, and initiate recovery processing when continuous disturbances are detected or the convergence status is not converged. The recovery processing includes at least one of the following: retreating a preset distance or re-executing the robotic arm control commands.

[0016] Beneficial Effects: This application provides a vision-based robot path planning method. It acquires target visual information and robotic arm motion state information, and determines the initial pose deviation, image quality score, displacement consistency score, and perturbation index of the target object based on this information. Furthermore, it determines the reliability level of the initial pose deviation according to the perturbation index, image quality score, and displacement consistency score. Subsequently, it generates an effective pose correction amount based on the reliability level and the initial pose deviation, and generates robotic arm control commands based on the effective pose correction amount and the reliability level. Simultaneously, it adjusts the execution mode of the robotic arm control commands according to the reliability level to avoid inducing new perturbations. Finally, it monitors the execution status of the robotic arm control commands, verifies the convergence state of the robotic arm aligned with the target object, and initiates recovery processing when continuous perturbation is detected or the convergence state is not converged. This method can comprehensively evaluate the reliability of visual information and the degree of physical perturbation of the robotic arm, classify the pose deviation according to reliability, and adaptively adjust the correction strategy and the execution mode of the control commands accordingly. In this way, this application effectively solves the problem in the prior art where high-frequency vibration of the vision sensor causes image blurring, which in turn leads to pose estimation errors containing vibration noise. Ultimately, this results in the end effector exhibiting continuous high-frequency jitter near the fruit, making it impossible to stably reach the allowable pose error range for picking. The technical solution of this application can significantly improve the accuracy, stability, and robustness of robot path planning. Especially in the fine alignment stage, it can effectively suppress the jitter of the robotic arm, ensuring that the end effector stably converges to the target position, thereby improving work efficiency and success rate. Attached Figure Description

[0017] Figure 1 This is a schematic diagram of a robot path planning method based on vision detection provided in an embodiment of the present invention; Figure 2 This is a schematic diagram of another robot path planning method based on vision detection provided in an embodiment of the present invention; Figure 3 This is a schematic diagram of a robot path planning system based on vision detection provided in an embodiment of the present invention. Detailed Implementation

[0018] The technical solutions of this application will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are merely some embodiments of this application, and not all embodiments. The components of this application described and shown in the accompanying drawings can generally be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely to illustrate selected embodiments of this application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.

[0019] It should be noted that similar reference numerals and letters in the following figures indicate similar items; therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures. Furthermore, in the description of this application, terms such as "first," "second," etc., are used only to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0020] To better understand the technical solution proposed in this application, some key terms involved will be explained first.

[0021] "Target visual information" refers to image or video data about a target object (such as fruit to be picked) acquired through visual sensors (such as cameras). This information is the basis for robots to perform path planning and pose correction.

[0022] "Robotic arm motion status information" refers to various physical quantity data generated by the robotic arm during its movement, such as joint angles, angular velocity, angular acceleration, torque, end effector speed and acceleration, etc. This information is used to assess the current motion status of the robotic arm and potential disturbances.

[0023] "Initial pose deviation" refers to the difference between the initial position and orientation of the target object and the end effector of the robotic arm, calculated using visual information. This is a core quantity that the robot needs to correct.

[0024] Image quality score is an indicator that measures aspects such as the sharpness, noise level, and blurriness of visual information, and is used to assess the reliability of visual information. A higher image quality score generally means more reliable visual data.

[0025] "Displacement consistency score" is an indicator that evaluates the smoothness and consistency of the motion trajectory of feature points in the visual information of a target across consecutive frames. When the robotic arm vibrates, the displacement of the feature points becomes irregular, leading to a decrease in the displacement consistency score.

[0026] The "perturbation index" is an indicator that comprehensively reflects the degree of physical disturbance experienced by a robotic arm at the moment of acquiring visual information. It may include joint reversal frequency, theoretical acceleration jump amplitude, actual vibration energy, etc., and is used to quantify the vibration level of the robotic arm.

[0027] The "confidence level" is a comprehensive assessment of the reliability of the initial pose deviation based on the perturbation index, image quality score, and displacement consistency score. It can be categorized as high confidence, low confidence, or extremely low confidence, guiding subsequent correction strategies.

[0028] The "effective pose correction amount" is the actual amount used to correct the robot arm's pose, obtained after optimization and adjustment based on the initial pose deviation and its confidence level. It may differ from the initial pose deviation to avoid introducing new disturbances.

[0029] "Robotic arm control commands" are generated based on the effective pose correction amount and sent to the robotic arm controller to drive the joint movement of the robotic arm to achieve pose correction.

[0030] "Convergence state" refers to whether the pose deviation of the robotic arm's end effector gradually decreases and eventually reaches the preset accuracy range during the alignment process with the target object. Non-convergence state indicates that the robotic arm has failed to align with the target.

[0031] "Recovery processing" is an emergency measure initiated when a persistent disturbance or convergence failure is detected, designed to remove the robotic arm from its current unstable state and re-attempt to align with the target.

[0032] The following specific embodiments will provide a detailed introduction and explanation of a vision-based robot path planning method provided in this application.

[0033] Reference Figure 1 This invention provides a robot path planning method based on vision detection, comprising the following steps: S1, acquire target visual information and robot arm motion state information.

[0034] For example, target visual information can be acquired in real time using visual sensors such as industrial cameras, depth cameras, or stereo cameras mounted on the end effector or wrist of the robotic arm. These sensors can capture images or video streams of the target object. The robotic arm's motion state information can be obtained through sensors on the robotic arm itself, such as encoders, gyroscopes, and accelerometers, including joint angles, angular velocities, angular accelerations, the linear and angular velocities of the end effector, and torque feedback from each joint. This data is typically output in real time by the robotic arm controller.

[0035] S2. Based on the target visual information, determine the initial pose deviation of the target object, the image quality score of the target visual information, and the displacement consistency score.

[0036] Among them, the image quality score is used to characterize the reliability of the target visual information; the displacement consistency score is used to characterize the motion consistency of feature points in the target visual information between consecutive frames.

[0037] The initial pose deviation can be determined by inputting the acquired target visual information into a pre-trained pose estimation algorithm. This algorithm can identify the target object based on feature point matching, deep learning models (such as YOLO, Mask R-CNN, etc.) or traditional image processing methods (such as template matching, edge detection, etc.), and calculate its three-dimensional position and pose in the camera coordinate system, which is then converted into pose deviation relative to the end effector of the robotic arm.

[0038] Image quality scores can be determined using various image quality assessment algorithms. For example, the Laplacian variance of an image can be calculated to assess its sharpness; a larger variance generally indicates a sharper image. Alternatively, the texture richness and sharpness of an image can be assessed by calculating the magnitude and direction of edge gradients, constructing an oriented gradient histogram, and analyzing its variance and peak value ratio. Furthermore, a comprehensive evaluation can be performed using metrics such as signal-to-noise ratio, contrast, and brightness.

[0039] The displacement consistency score can be determined by tracking the motion of target objects or their feature points between consecutive frames. For example, optical flow methods (such as Lucas-Kanade optical flow) or feature point matching algorithms (such as SIFT, SURF, ORB, etc.) can be used to identify and track feature points in consecutive frames. Then, the directional angle and magnitude change rate of the displacement vectors of these feature points between adjacent frames are calculated. If the directional angle of the displacement vectors of most feature points is less than a certain set angle threshold, and the magnitude change rate is less than a preset threshold, then these feature points are considered to have good displacement consistency. The displacement consistency score can be defined as the proportion of feature points with good consistency among all tracked feature points.

[0040] S3. Determine the disturbance index based on the motion state information of the robotic arm.

[0041] The perturbation index is used to characterize the degree of physical perturbation at the moment the target visual information is acquired.

[0042] For example, the frequency of directional changes in joint velocity, i.e., the joint reversal frequency, can be calculated based on the real-time velocity feedback of each joint of the robotic arm. Frequent reversals usually indicate higher disturbances. Simultaneously, the theoretical acceleration jump amplitude can be calculated by mapping the joint angular velocity and angular acceleration of the robotic arm to the theoretical linear and angular acceleration of the end effector. Large jump amplitudes also indicate potential disturbances. Furthermore, vibration signals can be acquired using accelerometers mounted on the robotic arm, and the root mean square value of the vibration acceleration in the vibration signal can be calculated as an indicator of actual vibration energy. Finally, these indicators can be weighted and fused to obtain a comprehensive disturbance index.

[0043] S4. Determine the confidence level of the initial pose deviation based on the disturbance index, image quality score, and displacement consistency score.

[0044] For example, a series of thresholds and rules can be set. When the perturbation index exceeds a preset perturbation threshold or the image quality score falls below the minimum available threshold, the confidence level can be directly classified as extremely low confidence. In other cases, a comprehensive confidence score can be calculated, which is directly proportional to the image quality score and displacement consistency score, and inversely proportional to the perturbation index. For example, weighted average or fuzzy logic methods can be used for calculation. Then, based on the comparison between the comprehensive confidence score and the score threshold, the confidence level is classified as high confidence or low confidence.

[0045] S5. Generate effective pose correction amount based on the confidence level and initial pose deviation.

[0046] When the confidence level is high, the initial pose deviation can be directly used as the effective pose correction amount, or a small-scale smoothing process can be performed. When the confidence level is low, the initial pose deviation needs to be handled more cautiously. For example, the consistency of the axial component of the initial pose deviation can be judged to filter out obvious outliers and obtain a preliminary correction vector. Then, based on the directional deflection angle between the preliminary correction vector and the historical effective trajectory direction, the preliminary correction vector is reconstructed with directional constraints to obtain the effective pose correction amount. This approach aims to avoid the robotic arm making large or unstable corrections under low confidence conditions.

[0047] S6. Generate robotic arm control commands based on the effective pose correction amount and the confidence level, and adjust the execution mode of the robotic arm control commands according to the confidence level to avoid inducing new disturbances.

[0048] When the confidence level is high, conventional path planning algorithms can be used to generate joint velocity commands and joint angular displacement increments for the robotic arm. When the confidence level is low, a more conservative and robust control strategy is required. For example, when generating joint velocity commands, an asymmetric S-shaped velocity curve can be used, making the acceleration phase shorter than the deceleration phase to reduce impact and vibration. Simultaneously, the acceleration and deceleration phases of the S-shaped velocity curve can be dynamically adjusted based on the confidence level and the effective pose correction modulus. Furthermore, when generating joint angular displacement increments, an attenuation coefficient inversely proportional to the confidence level and the effective pose correction modulus can be introduced to reduce the magnitude of each correction. Within each control cycle, the upper limit of the velocity for the next cycle can be dynamically adjusted based on the real-time velocity and torque feedback of each joint to adapt to the current motion inertia state of the robotic arm.

[0049] S7. Monitor the execution status of the robotic arm control commands, verify the convergence status of the robotic arm aligned with the target object, and initiate recovery processing when continuous disturbances are detected or the convergence status is not converged.

[0050] The recovery process includes at least one of the following: retreating a preset distance or re-executing the robotic arm control command.

[0051] The execution status of the robotic arm control commands can be monitored through real-time information such as joint position, speed, and torque fed back by the robotic arm controller. Convergence verification can be achieved by continuously calculating the pose deviation between the robotic arm's end effector and the target object, and determining whether it continuously decreases within a preset error range. When the pose deviation fails to converge to the target range for an extended period, or the disturbance index remains above a certain threshold, the system will initiate recovery processing.

[0052] Recovery processing can include various strategies, such as retracting the robotic arm a preset distance to escape the current unstable area and then re-executing the robotic arm control commands, or directly re-executing the robotic arm control commands but using more conservative parameters.

[0053] Understandably, traditional methods typically assume that the images acquired by vision sensors accurately reflect the current static deviation, lacking mechanisms to identify and isolate the reverse interference from physical vibrations generated by the closed-loop motion itself on the sensor input. This leads to high-frequency micro-vibrations generated by the robotic arm's fine adjustments during the alignment phase, affecting the quality of the visual image and causing the pose estimation deviation to include vibration noise. This noise is misinterpreted as a true deviation by the closed-loop controller and converted into unnecessary end-effector jitter commands, ultimately resulting in persistent, non-converging high-frequency jitter in the end-effector near the target object.

[0054] In contrast, this application not only calculates the initial pose deviation but also further determines the image quality score, displacement consistency score, and perturbation index. The introduction of these indicators enables the system to comprehensively and objectively evaluate the effectiveness of the current visual information and the motion stability of the robotic arm. Based on these evaluation results, this application can determine the confidence level of the initial pose deviation. When the confidence level is low, the system will not blindly use the initial pose deviation directly for correction but will generate an optimized effective pose correction amount. For example, in the case of low confidence, the consistency of the axial component of the initial pose deviation can be judged, and directional constraint reconstruction processing can be performed in conjunction with the historical effective trajectory direction, thereby avoiding erroneous corrections caused by noise.

[0055] Furthermore, this application adjusts the execution method of the robotic arm control commands according to the confidence level. For example, at low confidence levels, an asymmetric S-shaped velocity curve is used, and the duration of acceleration and deceleration segments is dynamically adjusted. Additionally, an attenuation coefficient is introduced to generate joint angular displacement increments. These measures aim to reduce the impact and vibration of the robotic arm and avoid inducing new disturbances. This ability to dynamically adjust the control strategy is not available in traditional methods.

[0056] Furthermore, this application introduces a monitoring mechanism for the execution status of robotic arm control commands and a verification mechanism for the convergence status, and initiates recovery processing upon detecting continuous disturbances or non-convergence. This provides the system with a robust fault-tolerance mechanism, ensuring that the robotic arm can stably complete alignment tasks even in complex environments.

[0057] In summary, this application significantly improves the intelligence and robustness of robot path planning through comprehensive optimization of visual information, robotic arm motion state, and control strategy. It effectively solves the problems of end effector jitter and convergence failure in existing technologies, and provides a more reliable technical solution for applications such as agricultural harvesting robots that require high-precision alignment.

[0058] like Figure 2 As shown, specifically based on the confidence level and the initial pose deviation, the effective pose correction amount is generated, including: S101. When the confidence level indicates low confidence, the consistency of the axial component of the initial pose deviation is judged to obtain the preliminary correction vector.

[0059] Specifically, when the system determines that the confidence level of the currently acquired initial pose deviation is low, it indicates that there is significant uncertainty in the current visual information or environment. Directly using the initial pose deviation for correction in this case may be risky. Therefore, the initial pose deviation needs to be handled more cautiously. Consistency judgment of axial components refers to analyzing the components of the initial pose deviation along each independent motion axis (e.g., X, Y, Z translation axes and rotation axes around X, Y, Z) to determine whether these components are within the expected reasonable range or whether they contradict each other. For example, it can be checked whether the magnitude and direction of each axial component are consistent with historical motion trends or physical constraints. Through this judgment, obviously abnormal or unreliable deviation components can be filtered out, thus obtaining a more reliable initial correction vector.

[0060] S102. Based on the directional deflection angle between the preliminary correction vector and the historical effective trajectory direction, the preliminary correction vector is subjected to directional constraint reconstruction processing to obtain the effective pose correction amount.

[0061] Specifically, the direction deflection angle between the preliminary correction vector and the historical effective trajectory direction can be used to determine whether the direction deflection angle is greater than the safety threshold. When the direction deflection angle is greater than the safety threshold, multiple valid trajectory direction vectors with an angle less than the preset angle threshold are selected from the pre-cached historical valid trajectory direction vectors. Determine whether the number of valid trajectory direction vectors selected meets the minimum requirement; If the minimum number requirement is met, the average direction of the selected effective trajectory direction vectors is calculated and used as the adaptive reference direction at the current moment. Project the vector corresponding to the initial pose deviation onto the adaptive reference direction and calculate the projection length; Using the adaptive reference direction as a reference, and the projection length multiplied by the attenuation coefficient as the modulus, a new orientation constraint correction vector is reconstructed to obtain the effective pose correction amount. If the number of selected vectors does not meet the minimum requirement, all axial components are set to zero, and a zero vector is output to obtain the effective pose correction amount. When the directional deflection angle is less than or equal to the safety threshold, the direction of the historical effective trajectory is used for correction to obtain the effective pose correction amount.

[0062] The direction deflection angle refers to the angle between the initial correction vector and the historical effective trajectory direction. Its magnitude reflects the degree of consistency between the current correction requirement and the historical experience direction. The safety threshold is a preset critical angle value used to distinguish whether the degree of deviation of the correction direction from historical experience requires more cautious handling. When the direction deflection angle is greater than the safety threshold, it indicates that there may be significant uncertainty or anomalies in the current situation, requiring the activation of a more refined adaptive correction mechanism.

[0063] Furthermore, when the directional deflection angle exceeds a safety threshold, the system filters from pre-cached historical valid trajectory direction vectors. These historical valid trajectory direction vectors represent successfully executed trajectory directions in the past and have high reliability. The filtering criterion is whether the angle between these historical vectors and the vector direction corresponding to the current initial pose deviation is less than a preset angle threshold. The purpose is to ensure that the selected historical data has a high correlation with the current correction scenario.

[0064] Based on this, the system determines whether the number of selected valid trajectory direction vectors meets the minimum requirement. This minimum requirement aims to ensure that the samples used for subsequent calculations have sufficient statistical significance and representativeness, avoiding deviations in the adaptive reference direction due to insufficient samples. If the minimum requirement is met, the average direction of these selected valid trajectory direction vectors is calculated and used as the adaptive reference direction for the current moment. This adaptive reference direction integrates multiple relevant historical successful experiences, enabling more robust guidance for current corrections.

[0065] Subsequently, the vector corresponding to the initial pose deviation is projected onto the adaptive reference direction, and the projection length is calculated. The projection length characterizes the magnitude of the component of the initial pose deviation in the adaptive reference direction and is used to determine the correction modulus. Using the adaptive reference direction as a reference, and multiplying the projection length by the attenuation coefficient as the modulus, a new orientation constraint correction vector is reconstructed, thus obtaining the effective pose correction amount. The introduction of the attenuation coefficient can further control the aggressiveness of the correction and improve the safety of the correction.

[0066] As a safety mechanism, if the number of selected vectors does not meet the minimum requirement, it indicates that there is a lack of sufficiently reliable historical data to generate an adaptive reference orientation. In this case, to avoid introducing uncertain corrections, all axial components will be set to zero, and the zero vector will be output as the effective pose correction amount, thereby pausing or preventing the robotic arm from making potentially unsafe movements.

[0067] When the directional deflection angle is less than or equal to the safety threshold, it indicates that the current correction requirement is highly consistent with historical experience and the risk is low. At this time, the system will use the historical effective trajectory direction for correction to obtain the effective pose correction amount, so as to maintain the continuity and efficiency of the correction.

[0068] This application's solution effectively addresses the issue that a single historical trajectory direction may not provide a sufficiently robust and safe correction direction under low-confidence conditions by introducing a judgment of the direction deflection angle and an adaptive reference direction reconstruction mechanism based on multiple historical trajectory directions. By dynamically evaluating the degree of matching between the current deviation and historical experience, and selectively employing more refined adaptive strategies or safe backoff mechanisms based on the matching results, the rationality and safety of the correction direction are ensured. In particular, when the deviation is large, constructing an adaptive reference direction by filtering and averaging multiple relevant historical trajectory directions can more accurately capture the optimal correction path in the current scenario. Simultaneously, the magnitude of the correction is controlled through projection and attenuation coefficients to avoid over-correction.

[0069] Through the above technical solution, this application provides a more intelligent and safer method for generating effective pose correction values. This method can significantly improve the robustness and reliability of robots performing visual inspection and path planning in complex or uncertain environments. Especially when the initial pose deviation has low reliability or large deviation, by adaptively adjusting the correction direction and introducing a safety backoff mechanism, the risk of new disturbances induced by inaccurate correction or instability in the robot arm's movement is effectively reduced. This ensures that the robot arm can align with the target object more smoothly and accurately, improving the overall system's operational efficiency and safety.

[0070] In some preferred embodiments, suppose a robotic arm is performing a delicate assembly task, requiring the precise insertion of a component into a target hole. At some point, due to changes in ambient light or brief sensor interference, the vision system detects a significant directional deflection angle, such as 40 degrees, between the initial pose deviation of the target hole and the robotic arm's expected historical insertion trajectory. The system first determines whether this 40-degree directional deflection angle exceeds a preset safety threshold (e.g., 30 degrees). Since 40 degrees is greater than 30 degrees, the system initiates adaptive orientation constraint reconstruction. The system filters from a pre-cached pool of valid trajectory direction vectors containing past successful insertion operations, selecting vectors whose angle with the current initial pose deviation direction is less than a preset angle threshold (e.g., 15 degrees). Assuming that eight valid trajectory direction vectors meet the criteria after filtering, and this number satisfies the minimum requirement (e.g., five), the system calculates the average direction of these eight vectors and uses it as the adaptive reference direction for the current moment. Subsequently, the vector corresponding to the initial pose deviation is projected onto this adaptive reference direction, and the projection length is calculated. Finally, using this adaptive reference direction as a baseline, and multiplying the projected length by an attenuation coefficient (e.g., 0.8) as the modulus, a new orientation constraint correction vector is reconstructed as the effective pose correction amount. This correction amount guides the robotic arm along a more robust and safer path that considers both the current deviation and incorporates multiple historical successful experiences, thus avoiding erroneous corrections that might result from directly using a single historical direction and ensuring smooth insertion of the component. If the number of selected effective trajectory orientation vectors is insufficient (e.g., only two are selected), the system outputs a zero vector and pauses the correction to avoid unsafe actions due to insufficient data.

[0071] In some of the above embodiments, a perturbation index is proposed to be determined based on the motion state information of the robotic arm, so as to characterize the degree of physical perturbation at the moment of acquisition of target visual information. Specifically, the process of determining this perturbation index can be further refined.

[0072] The above-mentioned determination of the disturbance index based on the robotic arm's motion state information includes: S201. Based on the motion state information of the robotic arm, determine the joint reversal frequency, theoretical acceleration jump amplitude, and actual vibration energy index.

[0073] Specifically, joint reversal frequency refers to the frequency at which the velocity direction of each joint of a robotic arm changes during its movement. When a robotic arm frequently changes its direction of movement, it usually indicates that its motion is unstable and may generate significant physical disturbances. This frequency can be obtained by monitoring and analyzing the joint velocity signals in real time and counting the number of velocity direction changes per unit time.

[0074] The theoretical acceleration jump amplitude can be understood as the instantaneous change in theoretical linear and angular acceleration at the end effector after kinematic mapping of the robot arm's joint angular velocity and angular acceleration. A large acceleration jump amplitude indicates that the robot arm's motion trajectory may be uneven or impacted, thus inducing vibration. This indicator can be calculated using the robot arm's kinematic model and joint sensor data.

[0075] In practical applications, the actual vibration energy index is specifically the root mean square (RMS) value of the vibration acceleration in the vibration signal generated by the robotic arm during its movement. The RMS value of vibration acceleration effectively reflects the overall vibration intensity and energy level of the robotic arm. This index can be obtained by installing an accelerometer on the robotic arm, collecting vibration data in real time, and performing calculations.

[0076] S202. The joint reversal frequency, theoretical acceleration jump amplitude and actual vibration energy index are weighted and fused to obtain the disturbance index.

[0077] For example, different weights can be assigned to these three indicators based on actual application scenarios and experience to highlight the sensitivity to specific disturbance sources. For instance, a high joint reversal frequency may indicate that the robotic arm is performing delicate operations or is in an unstable state; a large theoretical acceleration jump amplitude may foreshadow motion shock; and a high actual vibration energy index directly reflects the vibration intensity of the robotic arm. Through weighted fusion, a more robust and accurate disturbance index can be obtained.

[0078] This application's solution, by comprehensively considering the joint reversal frequency, theoretical acceleration jump amplitude, and actual vibration energy index of the robotic arm, can quantify the degree of physical disturbance of the robotic arm at the moment of visual information acquisition from multiple perspectives. The joint reversal frequency reflects the smoothness of the robotic arm's motion, the theoretical acceleration jump amplitude reveals the impact of the motion, and the actual vibration energy index directly quantifies the vibration intensity of the robotic arm. It is precisely because these indicators capture disturbance information from the perspectives of motion control, dynamic characteristics, and actual physical response that the determined disturbance index can comprehensively and accurately characterize the degree of physical disturbance at the moment of target visual information acquisition, providing a reliable basis for subsequent pose correction and control command adjustment.

[0079] Determining the image quality score for the target visual information specifically includes: S301. Extract the edge gradients of the target visual information in the horizontal and vertical directions, and calculate the gradient magnitude and gradient direction.

[0080] Specifically, extracting the edge gradients of the target visual information in the horizontal and vertical directions involves using image processing algorithms, such as the Sobel operator, Prewitt operator, or Canny operator, to perform convolution operations on the input visual information to detect boundaries of brightness or color changes in the image. This allows the calculation of the gradient values ​​for each pixel in the horizontal and vertical directions. Calculating the gradient magnitude and direction involves using mathematical formulas (e.g., magnitude = sqrt(Gx)) based on the extracted horizontal and vertical gradient values. 2 + Gy 2 The gradient magnitude (Gy, Gx) is calculated for each pixel using the gradient method `atan2(Gy, Gx)`. The gradient magnitude represents the intensity of the edge, while the gradient direction indicates the direction of the edge.

[0081] S302. Construct an oriented gradient histogram based on the gradient magnitude and gradient direction.

[0082] Specifically, this step involves dividing the image region into several cells, and within each cell, statistically analyzing the gradient distribution in different directions based on the gradient direction and magnitude of each pixel to form a histogram. This histogram reflects the texture and edge features of local image regions. Furthermore, the variance and peak value ratio of the directional gradient histogram are calculated, and combined with the Laplacian variance, to obtain an image quality score.

[0083] S303. Calculate the variance and peak value ratio of the directional gradient histogram, and combine it with the Laplacian variance to obtain the image quality score.

[0084] Among them, the variance of the histogram of oriented gradients can characterize the richness and uniformity of image texture; a larger variance usually means richer texture. The peak value ratio reflects edge information in the dominant direction of the image; a higher peak value may indicate stronger edge directionality. Laplacian variance is a commonly used image sharpness assessment metric; a larger value generally indicates a sharper image. By weighted fusion or logical judgment of these metrics, the image quality score of the target visual information can be comprehensively evaluated.

[0085] This application's solution extracts edge gradient information from an image and further analyzes its distribution characteristics in the histogram of oriented gradients (OARs). Combined with Laplacian variance, it quantifies image sharpness, texture richness, and edge features from multiple dimensions. Specifically, edge gradients reflect image detail and sharpness, the variance and peak-to-maximum ratio of the OARs provide information about image texture complexity and directionality, while Laplacian variance directly assesses image sharpness. The comprehensive application of these metrics makes image quality scoring more comprehensive and accurate, effectively characterizing the reliability of target visual information.

[0086] Determine the displacement consistency score, including: S401. Calculate the angle between the directions and the rate of change of the magnitude of the displacement vectors of feature points between consecutive frames.

[0087] In this context, feature points refer to image features that are identified and tracked in the target visual information, such as SIFT, SURF, and ORB feature points. A consecutive frame refers to a sequence of images acquired continuously by the robot vision system within a short period. A displacement vector is the vector formed by the change in position of the same feature point between two adjacent frames. The orientation angle is used to measure the stability of the feature point's motion direction between consecutive frames, while the rate of change of magnitude is used to measure the stability of the feature point's motion speed.

[0088] S402. If the angle between the directions of the displacement vectors of the feature points between two adjacent frames is less than a set angle threshold and the rate of change of the modulus is less than a preset threshold, then the feature point is determined to be the target feature point.

[0089] Specifically, the set angle threshold and preset threshold are parameters set based on actual application scenarios and experience, used to distinguish between feature points with stable motion and feature points affected by noise or abnormal motion. When the motion direction and velocity changes of a feature point are both within acceptable ranges, the feature point is considered a reliable "target feature point".

[0090] S403. Calculate the proportion of target feature points among all tracked feature points to obtain the displacement consistency score.

[0091] The higher this ratio, the better the overall motion consistency of feature points in the visual information, and the higher the reliability of the visual information.

[0092] This application's solution effectively filters out feature points exhibiting stable motion in visual information by calculating and judging the directional angle and magnitude change rate of the displacement vectors of feature points between consecutive frames. By setting angle thresholds and preset thresholds, abnormal feature point motion caused by factors such as camera shake, local deformation of the target, or environmental noise can be excluded, thereby ensuring that the identified target feature points have high motion consistency. Finally, by statistically analyzing the proportion of these target feature points, the overall consistency of feature point motion in the entire target visual information can be quantitatively evaluated, providing a reliable basis for subsequent pose deviation reliability assessment.

[0093] In some embodiments described above in this application, a reliability level for initial pose deviation is determined based on perturbation index, image quality score, and displacement consistency score. However, in practical applications, simply combining these indicators may not be sufficient to capture the true reliability of visual inspection data under complex working conditions, especially in the presence of extreme perturbations or extremely poor image quality. This could lead to misjudgment of the reliability of pose deviation, thereby affecting the accuracy and safety of subsequent robotic arm control.

[0094] In response, this application further proposes a method for determining the confidence level of the initial pose deviation, including: S501. If the first preset condition is met, the confidence level of the initial pose deviation is determined to be extremely low.

[0095] Specifically, the first preset condition refers to situations where the degree of physical perturbation (characterized by the perturbation index) is too large, exceeding a preset perturbation threshold, or when the reliability of the target visual information (characterized by the image quality score) is too low, falling below the minimum usable threshold. In these extreme cases, even if other indicators perform reasonably well, the confidence level of the initial pose deviation should be directly determined as extremely low confidence to avoid subsequent operations based on unreliable data. The preset perturbation threshold and the minimum usable threshold can be empirically set or determined experimentally based on the actual application scenario and the system's robustness requirements.

[0096] S502. If the first preset condition is not met, calculate a comprehensive confidence score.

[0097] The overall confidence score is directly proportional to the image quality score and displacement consistency score, and inversely proportional to the perturbation index.

[0098] For example, the overall confidence score can be calculated using methods such as weighted average or nonlinear function, where the weights of each indicator can be adjusted according to their impact on confidence.

[0099] S503. If the overall confidence score is greater than the score threshold, the confidence level of the initial pose deviation is determined to be high confidence.

[0100] S504. If the overall confidence score is less than or equal to the score threshold, the confidence level of the initial pose deviation is determined to be low confidence.

[0101] This application's solution effectively addresses the misjudgment problem that may arise from simply combining indicators by introducing a tiered confidence assessment mechanism. Specifically, firstly, by setting a first preset condition, extreme unreliable situations are quickly identified and processed. When the perturbation index is too high or the image quality score is too low, the confidence level is directly classified as extremely low confidence. This is equivalent to setting a safety threshold, which can promptly prevent the system from making decisions based on severely distorted visual information, thereby avoiding potential risks caused by unreliable data sources. Secondly, for non-extreme situations, a comprehensive confidence score is calculated. This score comprehensively considers image quality, displacement consistency, and perturbation degree, enabling a more comprehensive and refined assessment of the reliability of the initial pose deviation. This multi-factor weighted fusion approach makes confidence assessment no longer a single-dimensional judgment, but a comprehensive consideration of multiple influencing factors, thereby improving the accuracy and robustness of the assessment. Finally, by comparing the comprehensive confidence score with a score threshold, the confidence level is subdivided into high confidence and low confidence, providing a more guiding basis for subsequent pose correction and robotic arm control.

[0102] In some preferred embodiments, a specific example is given below. Suppose a robot is performing a precision assembly task, requiring precise alignment of a target object based on visual information.

[0103] Scenario 1: At a certain moment, the robot experiences severe vibration due to the activation of nearby heavy equipment, causing the detected disturbance index to far exceed the preset disturbance threshold. Alternatively, a sudden change in ambient light may result in extremely blurry visual information of the target, with the image quality score falling below the minimum usable threshold. In such cases, even if the displacement consistency score is acceptable, the confidence level of the initial pose deviation will be directly determined as extremely low confidence based on the first preset condition. The system will immediately recognize that the current visual data is unreliable, thereby avoiding robotic arm operations based on erroneous information and ensuring operational safety.

[0104] Scenario 2: At another moment, the robot is in a state of slight vibration, the perturbation index does not exceed the preset perturbation threshold, and the image quality score is also higher than the minimum usable threshold. In this case, a comprehensive confidence score is calculated. For example, if both the image quality score and displacement consistency score are at a moderate level, and the perturbation index is also at a moderate level, the comprehensive confidence score calculated after weighted fusion is slightly lower than the preset score threshold. In this situation, the confidence level of the initial pose deviation will be determined as low confidence. The system will recognize that although the current data is usable, its reliability is not high, and may trigger a more conservative robotic arm control strategy, such as reducing the movement speed or increasing the fault tolerance margin.

[0105] Scenario 3: Under ideal conditions, the robot operates smoothly with a very low perturbation index, the target visual information is clear and stable, and both the image quality score and displacement consistency score are high. In this case, the calculated overall confidence score is much higher than the score threshold. The confidence level of the initial pose deviation will be determined as high confidence. The system can confidently perform high-precision, high-efficiency robotic arm operations based on the current visual information, for example, by employing more aggressive motion curves to improve work efficiency.

[0106] In some embodiments described above, this application proposes generating robotic arm control commands based on the effective pose correction amount and the confidence level, and adjusting the execution method of the robotic arm control commands according to the confidence level to avoid inducing new disturbances. However, in its implementation, if only general adjustments are made without a refined control strategy for low confidence conditions, the movement of the robotic arm may still be unstable, such as jitter, overshoot, or response lag, which may induce new physical disturbances, affecting alignment accuracy and work efficiency. Therefore, this application further proposes a more refined and robust method for generating and executing robotic arm control commands, aiming to minimize the induction of new disturbances and ensure stable and precise operation of the robotic arm in low confidence environments through a multi-dimensional, adaptive control strategy.

[0107] Specifically, based on the effective pose correction amount and the confidence level, the above-mentioned robotic arm control commands are generated, and the execution method of the robotic arm control commands is adjusted according to the confidence level to avoid inducing new disturbances, including: S601. When the confidence level indicates low confidence, evaluate the ratio of the effective pose correction modulus to the historical average correction modulus.

[0108] Specifically, when the confidence level indicates low confidence, the ratio of the effective pose correction magnitude to the historical average correction magnitude is evaluated. This aims to preliminarily determine the degree of abnormality in the current correction. If the current correction is too large or too small compared to the historical average, it may indicate a significant deviation in visual information or a significant change in the environment, requiring the system to adopt a more cautious control strategy.

[0109] S602. When the ratio is within the preset effective correction range, an asymmetric S-shaped speed curve is used when generating the joint speed command of the robotic arm.

[0110] The aim is to optimize the response characteristics of the robotic arm while ensuring smooth motion. The S-shaped velocity curve ensures that the acceleration of the robotic arm changes smoothly during startup and shutdown, effectively reducing shock and vibration. The asymmetric design, in particular, makes the acceleration phase shorter than the deceleration phase, meaning that the robotic arm can start relatively quickly, but will decelerate to the target position in a slower and more controlled manner. This reduces the risk of overshoot or the induction of new disturbances due to excessive inertia when approaching the target.

[0111] S603. Based on the confidence level and the effective pose correction modulus length, dynamically adjust the acceleration and deceleration durations of the S-shaped velocity curve so that the acceleration duration is less than the deceleration duration.

[0112] The purpose is to achieve adaptive movement of the robotic arm. When the confidence level is low or the correction modulus is large, the system will further shorten the acceleration phase and lengthen the deceleration phase to enhance the caution of the movement and ensure that the robotic arm can approach the target more smoothly and accurately under conditions of high uncertainty.

[0113] S604. When generating the joint angular displacement increment of the robotic arm, an attenuation coefficient that is inversely proportional to the confidence level and the effective pose correction modulus is introduced.

[0114] The purpose is to finely limit each step of the robotic arm's movement. When the confidence level is low or the correction modulus is large, the attenuation coefficient will increase accordingly, reducing the actual angular displacement increment. This decomposes the large correction into smaller, safer steps, avoiding instability or disturbance caused by a single large correction.

[0115] S605. Within each control cycle, the upper limit of the speed for the next cycle is dynamically adjusted based on the real-time speed feedback and real-time torque feedback of each joint of the robotic arm.

[0116] Its purpose is to provide real-time motion status monitoring and protection mechanisms. By continuously monitoring the actual speed and torque of the joints, the system can promptly detect potential vibrations, overloads, or abnormal motion trends, and immediately reduce the speed limit, thereby preventing the robotic arm from entering an unstable working area and effectively avoiding the induction of new disturbances.

[0117] This application's solution effectively addresses the problem of potential new disturbances induced by visual information uncertainty in robotic arms by introducing multiple refined control strategies in low-confidence environments. Specifically, by evaluating the correction modulus-length ratio, the system can initially determine the anomaly of the correction, providing a basis for subsequent cautious control. The application of an asymmetric S-shaped velocity curve, combined with dynamically adjusting the duration of acceleration and deceleration phases, ensures that the robotic arm's movement responds rapidly at startup and is extremely smooth when approaching the target, significantly reducing shock and vibration. The introduction of a damping coefficient decomposes large correction amounts into small steps, ensuring safe and gradual adjustments under uncertainty. Simultaneously, the real-time speed and torque feedback mechanism enables the system to dynamically sense the robotic arm's motion state and adjust the speed limit in a timely manner, thus forming a closed-loop, adaptive disturbance avoidance mechanism that ensures the stable and precise operation of the robotic arm in complex and uncertain environments.

[0118] In some preferred embodiments, suppose a robotic arm is performing a high-precision assembly task, requiring the precise placement of a tiny part into a designated location. Due to changes in ambient light or reflections from the part's surface, the vision system's assessment of the initial pose deviation of the target part is determined to be of "low confidence."

[0119] At this point, the system will first evaluate the ratio of the currently calculated effective pose correction magnitude to the historical average correction magnitude. If this ratio indicates that the current correction is relatively large, the system will further strengthen cautious control.

[0120] When generating joint speed commands for the robotic arm, the system will no longer use the conventional symmetrical S-shaped speed curve, but instead adopt an asymmetrical S-shaped speed curve. For example, for a certain joint, the acceleration phase duration may be dynamically set to 0.3 seconds, while the deceleration phase duration may be set to 1.0 second. This setting allows the robotic arm to respond quickly when it starts moving, but to move at a slower and smoother speed when approaching the target position, thereby minimizing the impact and vibration caused by rapid braking or sudden stopping.

[0121] Meanwhile, when calculating the angular displacement increment of each joint of the robotic arm, the system introduces an attenuation coefficient. This attenuation coefficient is inversely proportional to the current low confidence level and the effective pose correction modulus. For example, if the calculated original angular displacement increment is 0.5 degrees, the attenuation coefficient may adjust it to 0.3 degrees, making the step size of each movement of the robotic arm smaller. This allows for a more precise and safer approach to the target under conditions of high uncertainty.

[0122] Throughout the movement, each joint of the robotic arm continuously feeds back real-time speed and torque data to the control system. If the system detects an abnormally large increase in the speed fluctuation of a joint, or a sudden rise in the torque feedback value, this may indicate an impending vibration or collision. In this case, the control system will immediately and dynamically adjust the speed limit for the next control cycle, for example, reducing the maximum permissible speed from 100 mm / s to 50 mm / s, to force the robotic arm to decelerate, thereby avoiding inducing new disturbances and ensuring the smooth completion of the assembly task.

[0123] In some embodiments described above, when generating the joint angular displacement increment of the robotic arm, an attenuation coefficient inversely proportional to the confidence level and the effective pose correction modulus is introduced. However, in its implementation, if the attenuation coefficient is determined solely by the confidence level and the effective pose correction modulus, the dynamic characteristics of the robotic arm in actual motion, such as its current state of motion inertia, may not be adequately considered. This could lead to insufficient precision in setting the attenuation coefficient under certain operating conditions, thereby affecting the smoothness and convergence of the robotic arm's correction motion, and potentially even inducing new disturbances.

[0124] In response, this application further proposes to introduce an attenuation coefficient that is inversely proportional to the confidence level and the effective pose correction modulus, including: S701. Evaluate the motion inertia state of the robotic arm based on the end-effector velocity and acceleration, as well as the real-time torque feedback of each joint.

[0125] Specifically, assessing the motion inertia state of a robotic arm involves quantifying its inertial performance in the current motion state by analyzing its dynamic characteristics. This can be calculated based on the end effector's velocity and acceleration data; for example, the equivalent mass or inertia of the end effector at the current velocity and acceleration can be derived using kinematic and dynamic models. Simultaneously, real-time torque feedback from each joint of the robotic arm provides force information at the joint level, further reflecting the load and resistance experienced by the robotic arm during actual movement, thus more accurately assessing its overall motion inertia. For example, when the end effector velocity and acceleration are high, and the joint torque feedback indicates a large load, the robotic arm's motion inertia state can be considered high.

[0126] S702. Determine the attenuation coefficient based on the confidence level, the effective pose correction modulus, and the motion inertia state of the robotic arm.

[0127] The attenuation coefficient is a multiplicative factor used to adjust the increment of angular displacement at the robotic arm joints. Its function is to appropriately scale the calculated angular displacement increment based on the current system's reliability level, the required correction magnitude, and the robotic arm's own motion inertia state when generating control commands. For example, when the reliability level is low, the effective pose correction magnitude is large, or the robotic arm's motion inertia state is high, the attenuation coefficient can be set to a smaller value to reduce the angular displacement increment, thereby reducing the robotic arm's response speed and amplitude, and avoiding new disturbances or instability caused by over-correction. Conversely, when the reliability level is high, the correction magnitude is small, and the motion inertia state is low, the attenuation coefficient can be set to a larger value to allow for a faster response.

[0128] This application's solution introduces an assessment of the robotic arm's motion inertia state, enabling a more refined and adaptive determination of the attenuation coefficient. When generating control commands for the robotic arm, not only are the reliability level of the initial pose deviation and the effective pose correction modulus length considered, but the end-effector velocity and acceleration, as well as the real-time torque feedback of each joint, are also incorporated to assess its current motion inertia state. It is precisely this consideration of the robotic arm's motion inertia state that allows the attenuation coefficient to be adjusted according to the actual dynamic characteristics of the robotic arm. For example, when the robotic arm is moving at high speed or under a large load, its motion inertia is significant. Without considering this, directly applying an attenuation coefficient based on the reliability level and correction modulus length might lead to overly aggressive corrections, inducing vibrations or instability. By assessing the motion inertia state, a suitable attenuation coefficient can be determined more accurately, thereby effectively suppressing the robotic arm's dynamic response while ensuring the correction effect and avoiding impacts or oscillations caused by excessive inertia.

[0129] In some preferred embodiments, it is assumed that the robotic arm is moving towards a target object at a high speed, and its end effector is carrying a heavy workpiece. In this case, the visual information of the target obtained by the vision inspection system may indicate a moderate initial pose deviation, which is determined to be of low confidence level based on the perturbation index, image quality score, and displacement consistency score. According to the above scheme, an attenuation coefficient needs to be introduced when generating the joint angular displacement increments of the robotic arm. Specifically, the system first evaluates the current motion inertia state of the robotic arm based on the end-effector velocity and acceleration (e.g., obtained through encoders and inertial measurement units) and the real-time torque feedback of each joint (e.g., obtained through joint torque sensors). Since the robotic arm is moving at high speed and carrying a heavy load, its motion inertia state is evaluated as high. Based on this, combined with a low confidence level and a moderate effective pose correction modulus, the system determines a relatively small attenuation coefficient. For example, considering only the confidence level and correction modulus might yield a moderate attenuation coefficient, but considering the high motion inertia, the attenuation coefficient will be further reduced. Therefore, when generating the joint angular displacement increment of the robotic arm, this small attenuation coefficient is applied to appropriately suppress the actual angular displacement increment. This suppression effectively slows down the response speed of the robotic arm, avoids excessive impact or vibration during the correction process due to its high inertia, thereby preventing the induction of new disturbances and ensuring that the robotic arm can converge smoothly and safely to the target object.

[0130] like Figure 3 As shown, this embodiment of the invention also provides a robot path planning system based on vision detection. The system includes: The information acquisition module is used to acquire visual information of the target and motion state information of the robot's robotic arm; The calculation module is used to determine the initial pose deviation of the target object, the image quality score of the target visual information, and the displacement consistency score based on the target visual information; the image quality score is used to characterize the reliability of the target visual information; the displacement consistency score is used to characterize the motion consistency of feature points in the target visual information across consecutive frames. The physical disturbance assessment module is used to determine the disturbance index based on the robotic arm's motion state information; the disturbance index is used to characterize the degree of physical disturbance at the moment of acquisition of target visual information; The credibility determination module is used to determine the credibility level of the initial pose deviation based on the perturbation index, image quality score, and displacement consistency score. The correction amount generation module is used to generate effective pose correction amounts based on the confidence level and the initial pose deviation. The control command generation module is used to generate robotic arm control commands based on the effective pose correction amount and the confidence level, and adjust the execution mode of the robotic arm control commands according to the confidence level to avoid inducing new disturbances. The status monitoring module is used to monitor the execution status of the robotic arm control commands, verify the convergence status of the robotic arm aligned with the target object, and initiate recovery processing when continuous disturbances are detected or the convergence status is not converged. The recovery processing includes at least one of the following: retreating a preset distance or re-executing the robotic arm control commands.

[0131] This application also provides a computer-readable storage medium. All or part of the processes in the above method embodiments can be executed by a computer program instructing related hardware. This program can be stored in the computer-readable storage medium, and when executed, it can include the processes of the above method embodiments. The computer-readable storage medium can be an internal storage unit of the task execution device (including a data sending end and / or a data receiving end) of any of the foregoing embodiments, such as the hard disk or memory of the task execution device. The computer-readable storage medium can also be an external storage device of the terminal device, such as a plug-in hard disk, smart media card (SMC), secure digital (SD) card, flash card, etc., equipped on the terminal device. Further, the computer-readable storage medium can include both the internal storage unit of the task execution device and an external storage device. The computer-readable storage medium is used to store the computer program and other programs and data required by the task execution device. The computer-readable storage medium can also be used to temporarily store data that has been output or will be output.

[0132] Furthermore, the functional units in the various embodiments of this application 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.

[0133] 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 readable storage medium. Based on this understanding, the technical solutions of the embodiments of this application, essentially, or the parts that contribute to the prior art, or all or part of the technical solutions, can be embodied in the form of a software product. This software product is stored in a storage medium and includes several instructions to cause a device (which may be a microcontroller, chip, etc.) or processor to execute all or part of the steps of the methods of the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, ROM, RAM, magnetic disks, or optical disks.

[0134] The above are merely specific embodiments of this application, but the scope of protection of this application is not limited thereto. Any changes or substitutions within the technical scope disclosed in this application should be covered within the scope of protection of this application.

Claims

1. A robot path planning method based on vision detection, characterized in that, include: Acquire visual information about the target and motion state information of the robot's robotic arm; Based on the target visual information, the initial pose deviation of the target object, the image quality score of the target visual information, and the displacement consistency score are determined; the image quality score is used to characterize the reliability of the target visual information; the displacement consistency score is used to characterize the motion consistency of feature points in the target visual information across consecutive frames. Based on the robotic arm's motion state information, a perturbation index is determined; the perturbation index is used to characterize the degree of physical perturbation at the moment the target visual information is acquired. The confidence level of the initial pose deviation is determined based on the disturbance index, the image quality score, and the displacement consistency score. Based on the confidence level and the initial pose deviation, an effective pose correction amount is generated; Based on the effective pose correction amount and the confidence level, control commands for the robotic arm are generated, and the execution mode of the control commands is adjusted according to the confidence level to avoid inducing new disturbances. Monitor the execution status of the robotic arm control commands, verify the convergence status of the robotic arm aligned with the target object, and initiate recovery processing when continuous disturbance is detected or the convergence status is not converged; the recovery processing includes at least one of the following: retreating a preset distance, re-executing the robotic arm control commands.

2. The robot path planning method based on vision detection according to claim 1, characterized in that, The step of generating an effective pose correction amount based on the confidence level and the initial pose deviation includes: When the confidence level indicates low confidence, the consistency of the axial component of the initial pose deviation is assessed to obtain a preliminary correction vector. Based on the directional deflection angle between the preliminary correction vector and the historical effective trajectory direction, the preliminary correction vector is subjected to directional constraint reconstruction processing to obtain the effective pose correction amount.

3. The robot path planning method based on vision detection according to claim 2, characterized in that, The step of performing orientation constraint reconstruction processing on the preliminary correction vector based on the orientation deflection angle between the preliminary correction vector and the historical effective trajectory direction to obtain the effective pose correction amount includes: Based on the directional deflection angle between the preliminary correction vector and the historical effective trajectory direction, determine whether the directional deflection angle is greater than the safety threshold; When the direction deflection angle is greater than the safety threshold, multiple valid trajectory direction vectors with an angle less than a preset angle threshold corresponding to the vector direction of the initial pose deviation are selected from the pre-cached historical valid trajectory direction vectors. Determine whether the number of valid trajectory direction vectors selected meets the minimum requirement; If the minimum quantity requirement is met, the average direction of the selected effective trajectory direction vectors is calculated and used as the adaptive reference direction at the current moment. Project the vector corresponding to the initial pose deviation onto the adaptive reference direction and calculate the projection length; Using the adaptive reference direction as a reference, and multiplying the projection length by the attenuation coefficient as the modulus, a new orientation constraint correction vector is reconstructed to obtain the effective pose correction amount. If the number of selected vectors does not meet the minimum requirement, then all axial components are set to zero and a zero vector is output to obtain the effective pose correction amount. When the directional deflection angle is less than or equal to the safety threshold, the effective pose correction is obtained by following the historical effective trajectory direction.

4. The robot path planning method based on vision detection according to claim 1, characterized in that, The step of determining the disturbance index based on the robotic arm's motion state information includes: Based on the robotic arm's motion state information, the joint reversal frequency, theoretical acceleration jump amplitude, and actual vibration energy index are determined; the joint reversal frequency is the frequency of directional change of the robotic arm's joint velocity; the theoretical acceleration jump amplitude is the theoretical linear acceleration and angular acceleration of the robotic arm's joints mapped to the end effector; the actual vibration energy index is the root mean square value of the vibration acceleration in the robotic arm's vibration signal. The disturbance index is obtained by weighting and fusing the joint reversal frequency, the theoretical acceleration jump amplitude, and the actual vibration energy index.

5. The robot path planning method based on vision detection according to claim 1, characterized in that, Determining the image quality score of the target visual information includes: Extract the edge gradients of the target visual information in the horizontal and vertical directions, and calculate the gradient magnitude and gradient direction; Based on the gradient magnitude and gradient direction, construct an oriented gradient histogram; The variance and peak value ratio of the directional gradient histogram are calculated, and the image quality score is obtained by combining the Laplacian variance.

6. The robot path planning method based on vision detection according to claim 1, characterized in that, Determine the displacement consistency score, including: Calculate the directional angle and magnitude change rate of the displacement vectors of feature points between consecutive frames; If the angle between the directions of the displacement vectors of a feature point between two adjacent frames is less than a set angle threshold, and the rate of change of the modulus is less than a preset threshold, then the feature point is determined to be a target feature point. The proportion of the target feature point among all tracked feature points is counted to obtain the displacement consistency score.

7. The robot path planning method based on vision detection according to claim 1, characterized in that, The step of determining the confidence level of the initial pose deviation based on the perturbation index, the image quality score, and the displacement consistency score includes: If the first preset condition is met, the confidence level of the initial pose deviation is determined to be extremely low confidence; the first preset condition is that the perturbation index exceeds a preset perturbation threshold or the image quality score is lower than the minimum usable threshold. If the first preset condition is not met, a comprehensive confidence score is calculated; the comprehensive confidence score is directly proportional to the image quality score and the displacement consistency score, and inversely proportional to the perturbation index. If the overall confidence score is greater than the score threshold, then the confidence level of the initial pose deviation is determined to be high confidence. If the overall confidence score is less than or equal to the score threshold, then the confidence level of the initial pose deviation is determined to be low confidence.

8. The robot path planning method based on vision detection according to claim 7, characterized in that, The step of generating robotic arm control commands based on the effective pose correction amount and the confidence level, and adjusting the execution mode of the robotic arm control commands according to the confidence level to avoid inducing new disturbances, includes: When the confidence level indicates low confidence, evaluate the ratio of the effective pose correction modulus to the historical average correction modulus. When the ratio is within a preset effective correction range, an asymmetric S-shaped velocity curve is used when generating the joint speed command of the robotic arm. Based on the confidence level and the effective pose correction modulus, the acceleration and deceleration durations of the S-shaped velocity curve are dynamically adjusted so that the acceleration duration is less than the deceleration duration. When generating the joint angular displacement increment of the robotic arm, an attenuation coefficient that is inversely proportional to the confidence level and the effective pose correction modulus is introduced. Within each control cycle, the upper limit of the speed for the next cycle is dynamically adjusted based on the real-time speed and torque feedback of each joint of the robotic arm.

9. The robot path planning method based on vision detection according to claim 8, characterized in that, The introduced attenuation coefficient, which is inversely proportional to the confidence level and the effective pose correction magnitude, includes: The motion inertia state of the robotic arm is evaluated based on the end-effector velocity and acceleration, as well as the real-time torque feedback of each joint of the robotic arm. The attenuation coefficient is determined based on the confidence level, the effective pose correction modulus, and the motion inertia state of the robotic arm.

10. A robot path planning system based on vision detection, characterized in that, The system includes: The information acquisition module is used to acquire visual information of the target and motion state information of the robot's robotic arm; The calculation module is used to determine the initial pose deviation of the target object, the image quality score of the target visual information, and the displacement consistency score based on the target visual information; the image quality score is used to characterize the reliability of the target visual information; the displacement consistency score is used to characterize the motion consistency of feature points in the target visual information across consecutive frames. The physical disturbance assessment module is used to determine the disturbance index based on the motion state information of the robotic arm; the disturbance index is used to characterize the degree of physical disturbance at the moment of acquisition of the target visual information. The credibility determination module is used to determine the credibility level of the initial pose deviation based on the disturbance index, the image quality score, and the displacement consistency score. The correction amount generation module is used to generate an effective pose correction amount based on the confidence level and the initial pose deviation. The control command generation module is used to generate robotic arm control commands based on the effective pose correction amount and the confidence level, and adjust the execution mode of the robotic arm control commands according to the confidence level to avoid inducing new disturbances. The status monitoring module is used to monitor the execution status of the robotic arm control commands, verify the convergence status of the robotic arm aligned with the target object, and initiate recovery processing when continuous disturbance is detected or the convergence status is not converged; the recovery processing includes at least one of the following: retreating a preset distance, re-executing the robotic arm control commands.