A method and system for generating robot trajectories for pick-up

Abstract

The application provides a robot motion trajectory generation method and system for picking up, comprising: obtaining a three-dimensional coordinate in a depth camera coordinate system, converting the three-dimensional coordinate to a world coordinate system to obtain an object position coordinate and an object orientation angle; obtaining a current position coordinate and a current orientation angle of the robot, determining an orientation angle deviation and a distance deviation relative to a target object according to the object position coordinate, the object orientation angle, the current position coordinate and the current orientation angle; judging whether the orientation angle deviation and a preset orientation threshold value are located in a picking up orientation range, if yes, judging whether the target object is located in a mechanical arm working distance range, if yes, executing picking up, if not, linearly approaching the target object, if not, judging whether the orientation angle deviation, an offset of a mechanical arm base to a robot center and a preset minimum turning radius are located in a turning reachable range, if yes, approaching the target object according to a circular arc trajectory, and if not, giving up picking up the target object. The accuracy of picking up success can be improved.

Description

Technical Field

[0001] This application relates to the field of robot motion control technology, and in particular to a method and system for generating robot motion trajectories for picking up objects. Background Technology

[0002] With the development of service robot and environmental robot technologies, autonomous trash-collecting robots are increasingly being applied in semi-structured outdoor environments such as parks and industrial parks. These robots are typically equipped with sensors such as depth cameras to identify and locate trash targets, and use differential drive chassis for movement, exhibiting low speed and agility. In existing technologies, the motion planning for a robot to reach a designated target point usually relies on a combination of global and local path planning. However, for short-distance, high-precision tasks like trash collection, where the target is determined in real-time by sensors, existing technologies cannot guarantee the accuracy of successful trash collection. Summary of the Invention

[0003] To improve the accuracy of successful garbage collection, this application provides a method and system for generating robot motion trajectories for garbage collection.

[0004] Firstly, a method for generating the motion trajectory of a robot for picking up objects is provided, including:

[0005] The three-dimensional coordinates of the target object in the depth camera coordinate system are obtained. Based on the installation relationship between the depth camera and the center of the robot chassis, the three-dimensional coordinates are transformed to the world coordinate system to obtain the object position coordinates and object orientation angle of the target object in the world coordinate system.

[0006] Obtain the robot's current position coordinates and current orientation angle in the world coordinate system, and determine the robot's orientation angle deviation and distance deviation relative to the target object based on the object's position coordinates, object's orientation angle, current position coordinates, and current orientation angle.

[0007] Based on the orientation angle deviation and the preset orientation threshold, it is determined whether the target object is within the robot's picking orientation range. If it is, based on the distance deviation, it is determined whether the target object is within the robot's robotic arm working distance range. If yes, the robot is triggered to perform a picking operation; otherwise, the robot is controlled to approach the target object in a straight line.

[0008] If not, the robot determines whether the target object is within the robot's turning range based on the orientation angle deviation, the offset of the robotic arm base relative to the robot's center, and the robot's preset minimum turning radius. If so, the robot is controlled to approach the target object along an arc trajectory; otherwise, the robot abandons picking up the target object.

[0009] In some embodiments, obtaining the three-dimensional coordinates of the target object in the depth camera coordinate system includes:

[0010] The system acquires real-time environmental image data and 3D point cloud data from the depth camera, performs target detection processing on the environmental image data to obtain the position information of the target object in the image, extracts the point cloud data corresponding to the target object based on the coordinate mapping relationship between the position information and the 3D point cloud data, and performs centroid calculation processing on the point cloud data to obtain the 3D coordinates of the target object in the depth camera coordinate system.

[0011] In some embodiments, the step of transforming the three-dimensional coordinates to the world coordinate system based on the mounting relationship between the depth camera and the robot chassis center to obtain the object position coordinates and object orientation angle of the target object in the world coordinate system includes:

[0012] Based on the installation relationship between the depth camera and the center of the robot chassis, a first coordinate transformation matrix is ​​determined from the depth camera coordinate system to the robot chassis base coordinate system. The first coordinate transformation matrix is ​​used to perform matrix multiplication on the three-dimensional coordinates to obtain the first position coordinates of the target object in the robot chassis base coordinate system.

[0013] Based on the initial pose correspondence between the robot chassis base coordinate system and the world coordinate system, a second coordinate transformation matrix from the robot chassis base coordinate system to the world coordinate system is determined. The second coordinate transformation matrix is ​​then used to perform matrix multiplication on the first position coordinates to obtain the object position coordinates of the target object in the world coordinate system.

[0014] The object orientation angle in the world coordinate system is obtained by taking the arctangent value after calculating the ratio of the x-axis component and the y-axis component in the object's position coordinates.

[0015] In some embodiments, determining the robot's orientation angle deviation and distance deviation relative to the target object based on the object's position coordinates, object's orientation angle, current position coordinates, and current orientation angle includes:

[0016] The difference between the object's position coordinates and the current position coordinates is calculated to obtain a relative position vector, and the magnitude of the relative position vector is calculated to obtain the distance deviation;

[0017] The object's orientation angle and the current orientation angle are compared to obtain an angle difference value. The angle difference value is then normalized to obtain an orientation angle deviation.

[0018] In some embodiments, triggering the robot to perform the picking operation includes:

[0019] The robot's current linear velocity and current angular velocity are obtained, and a stop control command is generated based on the current linear velocity and current angular velocity to control the robot to decelerate to a stationary state;

[0020] When the robot is stationary, the current joint angle data of the robotic arm is acquired, and the target node angles of each joint of the robotic arm are obtained by inverse kinematics calculation based on the object coordinate position and the current joint angle data.

[0021] Based on the target node angle, joint space trajectory planning data is generated, and a pick-up start signal is generated based on the joint space trajectory planning data to control the robotic arm to perform a pick-up action to grasp the target object according to the joint space trajectory planning data.

[0022] In some embodiments, controlling the robot to approach the target object in a straight line includes:

[0023] The current linear velocity is subjected to velocity holding processing to obtain the target linear velocity, and the current angular velocity is subjected to zeroing processing to obtain the target angular velocity;

[0024] Generate linear motion control commands based on the target linear velocity and the target angular velocity, and control the robot to move in a straight line toward the target object at the target linear velocity and the target angular velocity according to the linear motion control commands;

[0025] During the robot's movement, the real-time distance deviation between the robot and the target object is reacquired according to a preset control cycle, and the real-time distance deviation is compared with the range of the robotic arm's working distance using a threshold method.

[0026] When the real-time distance deviation meets the working distance range of the robotic arm, a stop motion control command is generated, and the robot is controlled to stop moving in accordance with the stop motion control command to trigger the execution of the picking operation.

[0027] In some embodiments, determining whether the target object is within the robot's turning radius based on the orientation angle deviation, the offset of the robotic arm base relative to the robot's center, and the robot's preset minimum turning radius includes:

[0028] The absolute value of the orientation angle deviation is used as the first judgment value. The ratio of the offset to the preset minimum turning radius is calculated to obtain the first ratio. The arcsine function is applied to the first ratio to obtain the angle threshold.

[0029] When the first judgment value is not greater than the angle threshold, it is determined that the target object is within the turning reach range of the robot;

[0030] When the first judgment value is greater than the angle threshold, it is determined that the target object is not within the robot's turning reach range.

[0031] In some embodiments, the controlled robot approaches the target object along an arc trajectory, including:

[0032] The sign function value of the orientation angle deviation is used as the steering direction indication value. The ratio of the robot's preset minimum turning radius to the robot's current linear velocity is calculated to obtain the turning angular velocity amplitude. The steering direction indication value and the turning angular velocity amplitude are multiplied to obtain the target angular velocity.

[0033] Based on the target angular velocity and the current linear velocity, a circular arc motion control command is generated, and the robot is controlled to move towards the target object along the circular arc trajectory at the current linear velocity and the target angular velocity according to the circular arc motion control command.

[0034] In some embodiments, the method further includes:

[0035] During the robot's movement, the real-time orientation angle deviation between the robot and the target object is reacquired according to a preset control cycle, and the real-time orientation angle deviation is compared with the preset orientation threshold.

[0036] When the real-time orientation angle deviation meets the preset orientation threshold, an angular velocity zeroing command is generated, and the robot's angular velocity is controlled to return to zero according to the angular velocity zeroing command to trigger the pickup operation.

[0037] Secondly, a robot motion trajectory generation system for picking up objects is provided, comprising:

[0038] The acquisition module is used to acquire the three-dimensional coordinates of the target object in the depth camera coordinate system. Based on the installation relationship between the depth camera and the center of the robot chassis, the three-dimensional coordinates are transformed to the world coordinate system to obtain the object position coordinates and object orientation angle of the target object in the world coordinate system.

[0039] The deviation module is used to obtain the robot's current position coordinates and current orientation angle in the world coordinate system, and to determine the robot's orientation angle deviation and distance deviation relative to the target object based on the object's position coordinates, object's orientation angle, current position coordinates, and current orientation angle.

[0040] The trajectory module is used to determine whether the target object is within the robot's picking-up orientation range based on the orientation angle deviation and a preset orientation threshold. If it is, the module determines whether the target object is within the robot's arm working distance range based on the distance deviation. If it is, the robot is triggered to perform the picking operation; otherwise, the robot is controlled to approach the target object in a straight line. If it is not, the module determines whether the target object is within the robot's turning reach range based on the orientation angle deviation, the offset of the arm base relative to the robot's center, and the robot's preset minimum turning radius. If it is, the robot is controlled to approach the target object along an arc trajectory; otherwise, the picking-up of the target object is abandoned.

[0041] Using the above method, this application obtains the three-dimensional coordinates of the target object in the depth camera coordinate system. Based on the installation relationship between the depth camera and the robot chassis center, the three-dimensional coordinates are transformed to the world coordinate system to obtain the object's position coordinates and orientation angle in the world coordinate system. The robot's current position coordinates and current orientation angle in the world coordinate system are obtained. Based on the object's position coordinates, orientation angle, current position coordinates, and current orientation angle, the robot's orientation angle deviation and distance deviation relative to the target object are determined. Based on the orientation angle deviation and a preset orientation threshold, it is determined whether the target object is within the robot's picking-up orientation range. If it is, based on the distance deviation, it is determined whether the target object is within the robot's robotic arm's working distance range. If so, the robot is triggered to perform a picking operation; otherwise, the robot is controlled to approach the target object in a straight line. If not, based on the orientation angle deviation, the offset of the robotic arm base relative to the robot's center, and the robot's preset minimum turning radius, it is determined whether the target object is within the robot's turning reach range. If so, the robot is controlled to approach the target object along an arc trajectory; otherwise, the picking of the target object is abandoned. This application can improve the accuracy of successful garbage picking. Attached Figure Description

[0042] Figure 1 This is a block diagram of a robot motion trajectory generation method for picking up objects, as provided in this application.

[0043] Figure 2 This is a schematic diagram of the robotic arm's working radius, the robot's minimum turning radius, and the robot's depth camera's field of view, as provided in this application.

[0044] Figure 3 This is a block diagram of a robot motion trajectory generation system for picking up objects, as provided in this application. Detailed Implementation

[0045] To better understand the purpose, technical solutions, and advantages of this application, it has been described and illustrated below with reference to the accompanying drawings and embodiments. However, those skilled in the art should understand that this application can be implemented without these details. It will be apparent to those skilled in the art that various modifications can be made to the embodiments disclosed in this application, and the general principles defined in this application can be applied to other embodiments and application scenarios without departing from the principles and scope of this application. Therefore, this application is not limited to the illustrated embodiments, but is consistent with the broadest scope claimed in this application.

[0046] The embodiments of this application will now be described in further detail with reference to the accompanying drawings.

[0047] The method for generating robot motion trajectories for picking up objects provided in this application is applicable to low-speed differential motion robots. Figure 2 This is a schematic diagram illustrating the robotic arm's working radius, the robot's minimum turning radius, and the robot's depth camera's field of view, as provided in this application. Figure 2 As shown, number 1 represents the robot body, number 2 represents the position of the robotic arm base, number 3 represents the robotic arm's working range, number 4 represents the field of view of the depth camera, number 5 represents the depth camera, and number 6 represents the robot body's minimum turning radius. The robot's minimum turning radius is Rmin, where Rmin = 1.2m. The depth camera installed at its front end for target detection has a field of view depth range of [Dmin, Dmax] and a field of view angle range of [-β, β], where Dmin = 0.05m, Dmax = 2.3m, and β = 45°. The robot's front end is equipped with a pickup device, which is a six-degree-of-freedom robotic arm. The center of the six-degree-of-freedom robotic arm's base is located at the robot's front end, and is converged to the center of the robot body by a distance Ltool, where Ltool = 0.2m. The robotic arm's working angle range is [-φ, φ], where φ = 75°. The area around the robot arm's base has a radius of [RMinWork, RMaxWork], where RMinWork = 0.1m and RMaxWork = 0.6m.

[0048] Figure 1 This is a block diagram of a method for generating the motion trajectory of a robot for picking up objects, as provided in this application. The method includes the following steps:

[0049] Step S100: Obtain the three-dimensional coordinates of the target object in the depth camera coordinate system. Based on the installation position of the depth camera and the center of the robot chassis, transform the three-dimensional coordinates to the world coordinate system to obtain the object position coordinates and object orientation angle in the world coordinate system.

[0050] Described from the control end. First, the 3D coordinates of the target object in the depth camera coordinate system need to be obtained. This includes: acquiring real-time environmental image data and 3D point cloud data collected by the depth camera; performing target detection processing on the environmental image data to obtain the target object's position information in the image; extracting the point cloud data corresponding to the target object based on the coordinate mapping relationship between the position information and the 3D point cloud data; and performing centroid calculation processing on the point cloud data to obtain the target object's 3D coordinates in the depth camera coordinate system.

[0051] First, a depth camera mounted on the robot's front end acquires real-time environmental data in front of the robot to obtain environmental image data and corresponding 3D point cloud data. The environmental image data is a GRB image, and the 3D point cloud data is a collection of point clouds containing depth information. The two are synchronized during acquisition to ensure consistency in spatial correspondence. Next, the acquired environmental image data is input into a pre-trained object detection network. This network uses a deep learning model based on the YOLOv8 architecture to detect garbage objects in the environmental image data in real time. When the object detection network detects a target object in the image, it outputs the target object's position information in the image pixel coordinate system. The position information is represented in the form of a detection box, including the center pixel coordinates of the target object in the image and the size of the detection box. Then, based on the coordinate mapping relationship between the target object's position information in the image and the 3D point cloud data, the point cloud data corresponding to the target object is extracted from the 3D point cloud data. That is, using the intrinsic parameter matrix of the depth camera, the object information of the target object in the image pixel coordinate system is mapped to the 3D space in the depth camera coordinate system, thereby locating and segmenting the point cloud clusters belonging to the target object in the 3D point cloud data. This coordinate mapping relationship is established based on the intrinsic parameters calibrated by the depth camera at launch, enabling a one-to-one correspondence between image pixels and 3D point cloud points. Finally, after extracting the point cloud data corresponding to the target object, centroid calculation is performed on this portion of the point cloud data. Specifically, the arithmetic mean of the x-axis, y-axis, and z-axis coordinates of all 3D points in the point cloud cluster corresponding to the target object in the depth camera coordinate system is calculated, and the resulting average coordinate value is used as the 3D coordinates of the target object in the depth camera coordinate system. These 3D coordinates characterize the spatial position of the target object in the depth camera coordinate system, providing accurate position input for subsequent coordinate transformations. By fully utilizing the 3D point cloud information provided by the depth camera and combining it with the detection results of the target detection network, the 3D coordinates of the target object in the depth camera coordinate system can be accurately and quickly obtained, providing a reliable data foundation for subsequent coordinate transformations and motion trajectory planning. This reduces information loss and transformation errors caused by compressing 3D information to a 2D map before planning, improving the accuracy of target localization and the success rate of the picking task.

[0052] After obtaining the 3D coordinates, it is necessary to further transform these coordinates from the depth camera coordinate system to the world coordinate system to obtain the target object's position coordinates and orientation angle in the world coordinate system, providing a unified spatial reference for subsequent motion trajectory decisions. This coordinate transformation process is not a simple single coordinate transformation, but requires two levels of coordinate mapping: first, transformation from the depth camera coordinate system to the robot chassis base coordinate system, and then transformation from the robot chassis base coordinate system to the world coordinate system. Specifically, the transformation of the 3D coordinates to the world coordinate system based on the installation relationship between the depth camera and the robot chassis center to obtain the target object's position coordinates and orientation angle in the world coordinate system includes the following steps:

[0053] Step S101: Determine the first coordinate transformation matrix from the depth camera coordinate system to the robot chassis base coordinate system based on the installation relationship between the depth camera and the robot chassis center. Use the first coordinate transformation matrix to perform matrix multiplication on the three-dimensional coordinates to obtain the first position coordinates of the target object in the robot chassis base coordinate system.

[0054] Step S102: Determine the second coordinate transformation matrix from the robot chassis base coordinate system to the world coordinate system based on the initial pose correspondence between the robot chassis base coordinate system and the world coordinate system. Use the second coordinate transformation matrix to perform matrix multiplication on the first position coordinates to obtain the object position coordinates of the target object in the world coordinate system.

[0055] Step S103: After performing a ratio calculation on the x-axis and y-axis components of the object's position coordinates, the arctangent value is taken to obtain the object's orientation angle in the world coordinate system.

[0056] Since the depth camera is fixedly mounted on the front end of the robot, its relative position and attitude with the center of the robot chassis are determined through calibration at the factory. This mounting relationship can be fully described by a homogeneous transformation matrix, namely the first coordinate transformation matrix. This first coordinate transformation matrix includes the translation of the origin of the depth camera coordinate system relative to the origin of the robot chassis base coordinate system (i.e., the installation position offset, including offsets in the x, y, and z axes) and the rotation of each coordinate axis of the depth camera coordinate system relative to each coordinate axis of the robot chassis base coordinate system (i.e., the installation angle offset, reflecting the pitch, yaw, and roll installation attitude of the depth camera). In practical implementation, the obtained three-dimensional coordinates of the target object in the depth camera coordinate system are represented as a homogeneous vector. Then, this homogeneous vector is multiplied by the first coordinate transformation matrix to map the three-dimensional coordinates from the depth camera coordinate system to the robot chassis base coordinate system, obtaining the first position coordinates of the target object in the robot chassis base coordinate system. These first position coordinates reflect the spatial position of the target object relative to the center of the robot chassis and are the basic intermediate quantities for subsequent transformations to the world coordinate system.

[0057] When the robot starts, the origin of the world coordinate system coincides with the origin of the robot chassis base coordinate system, and the coordinate axes of the world coordinate system are aligned with those of the robot chassis base coordinate system. Specifically, the robot's orientation is the x-axis of the world coordinate system, leftward is the y-axis, and upward is the z-axis. As the robot moves, the position of the robot chassis base coordinate system relative to the origin of the world coordinate system and the orientation of its coordinate axes change. This change is characterized by the robot's current position coordinates and orientation angle in the world coordinate system. Based on this initial pose correspondence and the robot's real-time pose information, a second coordinate transformation matrix is ​​determined from the robot chassis base coordinate system to the world coordinate system. This second coordinate transformation matrix is ​​also a homogeneous transformation matrix, containing the translation of the origin of the robot chassis base coordinate system in the world coordinate system and the rotation of each coordinate axis of the robot chassis base coordinate system relative to each coordinate axis of the world coordinate system. In practical implementation, the obtained first position coordinates are represented as a homogeneous vector. This homogeneous vector is then multiplied by the second coordinate transformation matrix to map the first position coordinates from the robot chassis base coordinate system to the world coordinate system, thus obtaining the target object's position coordinates in the world coordinate system. These object position coordinates, with the world coordinate system as a reference, allow for unified mathematical operations with the robot's current position coordinates in the world coordinate system.

[0058] In the world coordinate system, a target object possesses not only position but also orientation attributes. The object orientation angle is defined as the angle between the direction vector pointing from the origin of the world coordinate system to the target object's position and the positive x-axis. Specifically, it is calculated by taking the x-axis and y-axis components of the obtained object position coordinates as input, calculating the ratio of the y-axis component to the x-axis component, and then performing an arctangent function to obtain the object orientation angle in the world coordinate system. The object orientation angle reflects the target object's orientation relative to the origin of the world coordinate system. In subsequent steps, it is used together with the robot's current orientation angle to calculate the robot's orientation angle deviation relative to the target object, determining whether the robot's current orientation meets the pickup conditions. By converting the target object's 3D coordinates in the depth camera coordinate system to its position coordinates and orientation angle in the world coordinate system, a complete mapping from local sensor perception data to target pose information under a globally unified spatial reference is achieved, providing an accurate and unified data foundation for subsequent judgments based on both position and orientation constraints. The coordinate transformation process involves constructing a first coordinate transformation matrix and a second coordinate transformation matrix to map the three-dimensional coordinates in the depth camera coordinate system to the robot chassis base coordinate system and the world coordinate system. Based on this, the orientation angle of the target object is independently solved, achieving a lossless and continuous transformation from perception layer data to global pose information. This ensures the accuracy and consistency of the target object's position and orientation information in the world coordinate system, providing reliable data support for subsequent picking condition judgments based on orientation angle deviation and distance deviation. This fundamentally guarantees the effectiveness of motion trajectory generation and the success rate of garbage picking tasks.

[0059] Step S200: Obtain the robot's current position coordinates and current orientation angle in the world coordinate system, and determine the robot's orientation angle deviation and distance deviation relative to the target object based on the object's position coordinates, object's orientation angle, current position coordinates, and current orientation angle.

[0060] After obtaining the object's position coordinates and orientation angle, it is also necessary to obtain the robot's own pose information in the world coordinate system, which serves as the motion reference for subsequent relative deviation calculations. Specifically, pose estimation is performed using wheeled odometry mounted on the robot chassis to obtain the robot's current position coordinates and current orientation angle in the world coordinate system. Incremental photoelectric encoders are mounted on the left and right drive wheels of the robot chassis. By collecting the encoder pulse signals, the travel distance of the left and right wheels is calculated, and the kinematic model of the robot chassis is used to integrate the travel distance to obtain the pose increment of the robot chassis base coordinate system relative to the world coordinate system. This pose increment is then accumulated to the historical pose data from the previous control cycle to recursively obtain the robot's current position coordinates and current orientation angle in the world coordinate system. The current position coordinates include x-axis and y-axis components, and the current orientation angle is the angle between the positive x-axis direction of the robot chassis base coordinate system and the positive x-axis direction of the world coordinate system. By using the wheel odometry to extrapolate the trajectory, the robot's pose information under a globally unified benchmark can be output in real time. This provides a reliable motion state benchmark for the accurate calculation of subsequent orientation angle deviation and distance deviation, ensuring the effectiveness of motion trajectory generation and the success rate of the pickup task.

[0061] After obtaining the object's position coordinates and orientation angle, to determine whether the target meets the kinematic and workspace conditions for the robot to perform a picking operation, the robot's current motion state needs to be relative to a unified spatial reference. Further, real-time pose information of the robot body in the world coordinate system needs to be obtained, and based on this, the spatial deviation of the target object relative to the robot is calculated. The spatial deviation includes the orientation angle deviation, which measures the consistency between the robot's orientation and the target direction, and the distance deviation, which measures the distance between the robot and the target. Determining the robot's orientation angle deviation and distance deviation relative to the target object based on the object's position coordinates, orientation angle, current position coordinates, and current orientation angle includes the following steps:

[0062] Step S201: Perform a difference calculation between the object's position coordinates and the current position coordinates to obtain a relative position vector, and perform a magnitude calculation on the relative position vector to obtain the distance deviation.

[0063] Step S202: Perform a difference calculation between the object's orientation angle and the current orientation angle to obtain the angle difference value, and perform angle normalization processing on the angle difference value to obtain the orientation angle deviation.

[0064] Specifically, under a unified world coordinate system, the obtained object position coordinates The robot's current position coordinates are obtained recursively from the wheel odometry. Perform component-wise interpolation, that is, calculate the direction vector from the target object's position to the robot's position, to obtain the relative position vector. This relative position vector visually reflects the positional offset of the target object relative to the robot body in three-dimensional space. Subsequently, the modulus (i.e., Euclidean norm) of this relative position vector is calculated, and the sum of its squares is then taken as the square root to obtain the straight-line distance between the target object and the robot. This value is used as the distance deviation. This distance deviation is a scalar quantity used to quantify the spatial distance between the robot and the target object. It is one of the key indicators for subsequently determining whether the target falls within the effective working radius of the robotic arm.

[0065] Then obtain the target object's orientation angle in the world coordinate system. And the robot's current orientation angle obtained recursively through wheel odometry. Angle of orientation of the object Defined as the angle between the direction vector pointing from the origin to the target object's position and the positive X-axis in the world coordinate system; the robot's current orientation angle. Defined as the angle between the positive X-axis direction of the robot's chassis base coordinate system and the positive X-axis direction of the world coordinate system. To obtain the heading deviation that the robot needs to adjust, the difference between the object's facing angle and the current facing angle is first calculated, i.e., the angle difference is calculated. This difference represents the degree of angular deviation between the robot's current orientation and the target orientation.

[0066] Because angles have periodicity, such as +180° and -180° representing the same direction in space, the original difference... This angle may not be within the standard range of angle representations, and directly using this value may cause the robot to rotate in the wrong direction or make unnecessary detours. Therefore, the angle difference value is... Angle normalization is performed. The specific process of this normalization is to normalize the angle differences. By performing modulo operations and boundary checks, it is transformed into a standard angle range. Internally. For example, it can be calculated. To achieve this, in which The function can uniquely determine an angle based on the signs of the sine and cosine values, thus mapping angle differences of any magnitude to the aforementioned standard interval. The angle values ​​obtained after normalization are... This is the final orientation angle deviation. The absolute value of the orientation angle deviation reflects the actual angle between the robot's current orientation and the target direction. Its sign indicates the direction the robot needs to turn; a positive value indicates a left turn, and a negative value indicates a right turn, thus providing clear directional guidance for subsequent motion control. By fusing the global pose of the target object with the robot's own global pose, distance deviation (representing the distance dimension) and orientation angle deviation (representing the posture dimension) are obtained. These two deviation parameters, from the two key dimensions of distance and orientation, jointly construct a complete spatial state description of the robot relative to the target object, providing a precise numerical basis for subsequently introducing robot kinematic constraints and robotic arm operational space constraints into trajectory planning decisions. By fully utilizing the unified spatial reference under the world coordinate system, the accumulation of coordinate transformation errors is reduced, effectively improving the accuracy of deviation calculation, thereby ensuring the reliability of subsequent picking condition judgments and fundamentally improving the success rate of garbage picking tasks.

[0067] Step S300: Determine whether the target object is within the robot's picking orientation range based on the orientation angle deviation and the preset orientation threshold. If it is, determine whether the target object is within the robot's robotic arm working distance range based on the distance deviation. If yes, trigger the robot to perform the picking operation; otherwise, control the robot to approach the target object in a straight line.

[0068] After obtaining the robot's orientation angle deviation and distance deviation relative to the target object, the first-level judgment stage of the picking conditions is entered. This stage determines whether the target object is within the robot's picking orientation range based on the orientation angle deviation and a preset orientation threshold. This judgment is used to determine whether the robot's current orientation meets the alignment conditions required for the robotic arm to perform the picking operation. Specifically, a preset orientation threshold is obtained, which is an empirical angle value. Preferably, the preset orientation threshold is 5.5 degrees. This preset orientation threshold is used to quantify the maximum allowable orientation deviation when the robot performs the picking operation. Its value is related to the joint range of motion of the robotic arm and the mounting position of the base. When the absolute value of the robot's orientation angle deviation is not greater than the preset orientation threshold, it indicates that the robot's current orientation is sufficient to align with the target object, and the robotic arm can complete the grasping action of the target object within its own joint angle limits. Conversely, when the absolute value of the orientation angle deviation is greater than the preset orientation threshold, it indicates that the robot's current orientation deviates too much from the target direction, and even if the robotic arm extends, it cannot reach the location of the target object.

[0069] During the judgment process, the absolute value of the orientation angle deviation is compared with a preset orientation threshold to obtain the first-level judgment result. If the absolute value of the orientation angle deviation is not greater than the preset orientation threshold, meaning the target object is within the robot's picking orientation range, the process proceeds to the distance deviation judgment stage, which determines whether the target object is within the robot's arm working distance range based on the distance deviation. If the absolute value of the orientation angle deviation is greater than the preset orientation threshold, meaning the target object is not within the robot's picking orientation range, the process proceeds to the turning accessibility judgment stage. This stage comprehensively judges whether the target object is within the robot's turning accessibility range based on the orientation angle deviation, the offset of the robotic arm base relative to the robot's center, and the robot's preset minimum turning radius, to determine whether the robot's orientation can be adjusted through a circular motion trajectory to bring the target object into the picking orientation range. By performing a simple numerical comparison between the absolute value of the orientation angle deviation and the preset orientation threshold, a fast and accurate determination of whether the robot's current orientation meets the picking alignment conditions is achieved. This threshold-based judgment method has low computational complexity, requires no iterative optimization or complex numerical calculations, and can be completed in real time within each control cycle of the robot's embedded controller, meeting the high real-time requirements of picking tasks. In addition, this judgment condition introduces the physical operation capability of the robotic arm into the early stage of the robot's motion decision-making in the form of a preset orientation threshold. That is, when the robot's orientation deviation exceeds the threshold, it will not wait until the robot reaches the vicinity of the target to find that it cannot complete the picking. Instead, it will identify and trigger the orientation adjustment behavior in advance during the motion planning stage, reduce invalid movement and repetitive planning, and effectively improve the success rate and execution efficiency of garbage picking tasks.

[0070] If the target object is determined to be within the pickup orientation range, the distance deviation is further used to determine whether the target object is within the robot's arm operating distance range. This arm operating distance range characterizes the radial distance coverage of the effective working space of the robot arm's end effector in the base coordinate system. The arm operating distance range is predefined as the radial distance interval of an annular working area formed by the center of the robot arm base as the origin and the minimum and maximum operating radii as boundaries. The minimum operating radius depends on the kinematic limits of each joint of the robot arm and the installation dimensions of the end effector, while the maximum operating radius depends on the structural arm span length and kinematically accessible space of the robot arm. In the specific judgment process, the pre-calibrated minimum and maximum operating radii are obtained, and the distance deviation is numerically compared with the minimum and maximum operating radii respectively. If the distance deviation is greater than or equal to the minimum operating radius and less than or equal to the maximum operating radius, the target object is determined to be within the robot's arm operating distance range. If the distance deviation is less than the minimum operating radius, it indicates that the target object is too close to the robot and has fallen into the blind zone that the robot arm cannot reach due to joint angle limitations. If the distance deviation is greater than the maximum operating radius, it indicates that the target object is too far from the robot and exceeds the arm extension limit of the robot arm. By performing interval membership judgment between the distance deviation and the robot arm's operating distance range, a radial distance constraint on the robot arm's physical operation capability is further introduced on the basis that the orientation alignment condition has been met. The picking operation is only triggered when the target object simultaneously meets the orientation alignment condition and the distance interval condition. This can effectively reduce picking failures caused by the target being too close or too far, improve the success rate of the robot arm's end effector in grasping the target object, and ensure the overall effectiveness and execution reliability of the garbage picking task.

[0071] Once the target object simultaneously meets the picking orientation condition and the robotic arm's working distance condition, the robot is triggered to perform the picking operation. Before performing the picking operation, the robot body must first be smoothly transitioned from a moving state to a stationary state to mitigate the impact of vibrations caused by chassis movement on the positioning accuracy of the robotic arm's end effector. Specifically, the robot's current linear velocity and current angular velocity are acquired, and a stop control command is generated based on these values. This stop control command includes linear velocity deceleration and angular velocity deceleration parameters, controlling the robot chassis to decelerate to a complete stop according to a preset deceleration curve, ensuring the robot stops precisely at the picking pose in front of the target object and preventing chassis displacement or shaking during subsequent robotic arm movements. After the robot is stationary, the robotic arm's picking motion planning is triggered. Triggering the robot to perform the picking operation includes the following steps:

[0072] Step S301: Obtain the robot's current linear velocity and current angular velocity, and generate a stop control command based on the current linear velocity and current angular velocity to control the robot to decelerate to a stationary state.

[0073] Step S302: When the robot is stationary, acquire the current joint angle data of the robotic arm, and perform inverse kinematics calculation based on the object coordinate position and the current joint angle data to obtain the target node angle of each joint of the robotic arm.

[0074] Step S303: Generate joint space trajectory planning data based on the target node angle, and generate a pick-up start signal based on the joint space trajectory planning data to control the robotic arm to perform a pick-up action according to the joint space trajectory planning data to grasp the target object.

[0075] First, the current joint angle data of each joint of the robotic arm in its current pose is obtained. This data is obtained in real time from the angle sensors at each joint, representing the current kinematic configuration of the robotic arm. Then, the object position coordinates in the world coordinate system are used as the desired pose input for the robotic arm's end effector. Combined with the current joint angle data, inverse kinematics (IK) processing is performed on the robotic arm. The IK process involves constructing the desired pose matrix of the robotic arm's end effector using the object position coordinates as the desired position and a pre-set grasping posture as the desired posture. Based on the DH parameter model of the robotic arm, kinematic equations are established from the robotic arm base coordinate system to the end effector coordinate system, and the desired pose matrix is ​​used as the target solution for these kinematic equations. An iterative numerical solution method is used to solve the kinematic equations in reverse. During the iteration process, the current joint angle data is used as the initial iteration value. The Jacobian matrix is ​​used to map the end effector pose error to the velocity correction in the joint space. The desired pose is approximated through successive iterations. Iteration stops when the end effector pose error is less than a preset convergence threshold, and the target node angles of each joint are output. Inverse kinematics calculation is performed in real time when each pick-up operation is triggered. It can adaptively calculate the target configuration of each joint of the robotic arm according to the actual spatial position of the target object, ensuring that the end effector of the robotic arm accurately reaches the position of the target object.

[0076] After obtaining the target node angles of each joint of the robotic arm, joint space trajectory planning data for driving the robotic arm's movements is further generated. This data includes a sequence of joint angles that smoothly transitions from the initial angle of each joint corresponding to the current joint angle data to the target node angle. This sequence is generated using a fifth-order polynomial interpolation method to ensure that each joint meets the smoothness requirements of continuous velocity, continuous acceleration, and no exceedance of the joint's range of motion limits during movement. After the joint space trajectory planning data is generated, a pickup start signal is generated based on this data and sent to the robotic arm controller. This controller then controls the robotic arm to perform the corresponding pickup action according to the joint angle values ​​at each moment in the joint space trajectory planning data, enabling the end effector to move along a smooth trajectory to the target object's location and complete the grasping operation. By linking the target object's position coordinates with the robotic arm's inverse kinematics calculation, the robotic arm is precisely driven to complete the pickup action while the chassis is stationary. This reduces the mutual interference between chassis movement and robotic arm movement, improves the accuracy of the end effector reaching the target position and the stability of the grasping action, effectively ensuring the successful execution of the waste pickup task. After completing the pickup, the robot continues to perform subsequent motion trajectory updates or task cycles.

[0077] When the target object is determined to be within the picking orientation range but the distance deviation does not meet the robotic arm's working distance range, it indicates that the target object is already in the alignment direction in front of the robot, but the distance is too far and exceeds the effective working radius of the robotic arm. At this time, the robot does not need to adjust its orientation; it only needs to approach the target in a straight line along its current orientation until it enters the distance range within which the robotic arm can perform the picking operation. Therefore, a straight-line approach control strategy is adopted. By maintaining the robot's current orientation, maintaining its linear velocity, and zeroing its angular velocity, the robot moves in a straight line towards the target object. During the approach, the real-time distance deviation is continuously monitored at a preset control cycle. Once the robot enters the robotic arm's working distance range, it is triggered to stop and perform the picking operation. The steps for controlling the robot to approach the target object in a straight line include:

[0078] Step S304: Perform velocity holding processing on the current linear velocity to obtain the target linear velocity, and perform zeroing processing on the current angular velocity to obtain the target angular velocity.

[0079] Step S305: Generate linear motion control commands based on the target linear velocity and target angular velocity, and control the robot to move linearly towards the target object at the target linear velocity and target angular velocity according to the linear motion control commands.

[0080] Step S306: During the robot's movement, the real-time distance deviation between the robot and the target object is reacquired according to a preset control cycle, and the real-time distance deviation is compared with the range of the robotic arm's working distance using a threshold.

[0081] Step S307: When the real-time distance deviation meets the working distance range of the robotic arm, a stop motion control command is generated, and the robot is controlled to stop moving in accordance with the stop motion control command to trigger the execution of the picking operation.

[0082] Specifically, after determining that a straight-line approximation action is required, the robot reads the current measured values ​​of linear velocity and angular velocity. The linear velocity is obtained in real-time from the robot's chassis wheel-mounted odometer or inertial measurement unit, and the angular velocity is also obtained in real-time from the same sensor. To maintain stable forward movement along the current orientation, the linear velocity is not adjusted; instead, the current measured linear velocity is directly output as the target linear velocity. Simultaneously, the current angular velocity is zeroed out, meaning the target angular velocity is forcibly set to zero. Through this speed maintenance and angular velocity zeroing process, the robot ensures straight-line movement along the current orientation, avoiding heading deviations caused by residual angular velocity and guaranteeing directional consistency of the approximation path.

[0083] Based on the obtained target linear velocity and target angular velocity, a corresponding linear motion control command is generated. This control command contains linear velocity and angular velocity components, where the linear velocity component equals the target linear velocity and the angular velocity component is zero. This command is sent to the robot chassis motion controller, which drives the left and right drive wheels of the robot chassis to execute the corresponding rotational speed output according to the differential kinematics model, causing the robot body to move in a straight line along the current orientation direction, gradually shortening the spatial distance between it and the target object.

[0084] During the robot's linear movement, the coordinate acquisition and deviation calculation operations in steps S100 to S200 are executed cyclically at preset fixed control cycles T. This updates the target object's position coordinates in the world coordinate system and the robot's current position coordinates in real time, and recalculates the current real-time distance deviation accordingly. Each time a new real-time distance deviation is obtained, it is immediately compared with a pre-calibrated robotic arm operating distance range to determine if the current real-time distance deviation falls within the range greater than or equal to the minimum operating radius and less than or equal to the maximum operating radius. Through periodic online replanning, continuous tracking of the spatial relationship between the robot's real-time pose and the target object is achieved, ensuring that the moment when the distance deviation meets the pickup condition can be captured in time during the approach process.

[0085] When the threshold comparison process determines that the current real-time distance deviation meets the robot arm's working distance range, a stop motion control command is immediately generated. This stop motion control command includes linear velocity deceleration and angular velocity deceleration parameters, controlling the robot chassis to decelerate and brake according to a preset deceleration curve until the robot body is completely stationary in its current pose. After the robot is stationary, the previous picking operation is triggered. By introducing a periodic distance deviation re-check mechanism during the straight-line approach and stopping immediately when the distance deviation meets the condition, the picking failure caused by the robot exceeding the robot arm's effective working area due to inertia is mitigated. Through the closed-loop approach and real-time re-check control method, the robot gradually approaches the target object in a straight path until it enters the robot arm's working distance range while ensuring that the robot's orientation remains unchanged, solving the problem of not being able to trigger the picking operation due to the target distance being too far. The angular velocity is kept at zero throughout the straight-line approach process to ensure that the heading does not deviate, effectively reducing the interference of the approach phase on the subsequent orientation alignment accuracy and improving the stability and execution efficiency of the overall picking process.

[0086] Step S400: If not located, determine whether the target object is within the robot's turning reach range based on the orientation angle deviation, the offset of the robotic arm base relative to the robot's center, and the robot's preset minimum turning radius. If so, control the robot to approach the target object along an arc trajectory; otherwise, abandon picking up the target object.

[0087] If the target object is determined to be outside the robot's pickup orientation range based on the orientation angle deviation and a preset orientation threshold, further evaluation is needed to determine whether the target object can enter the pickup area through a single circular motion. This involves comprehensively judging whether the target object is within the robot's turning reachable range based on the orientation angle deviation, the offset of the robotic arm base relative to the robot's center, and the robot's preset minimum turning radius. The essence of this judgment process is to incorporate the physical installation offset of the robotic arm and the inherent minimum turning radius constraint of the differential robot into the same geometric model to determine whether, under the current orientation deviation, the robot has a feasible circular trajectory that allows it to adjust its orientation while approaching the target, ultimately causing the target object to fall within the pickup orientation range. The process of determining whether the target object is within the robot's turning reachable range based on the orientation angle deviation, the offset of the robotic arm base relative to the robot's center, and the robot's preset minimum turning radius includes the following steps:

[0088] Step S401: The absolute value of the orientation angle deviation is used as the first judgment value. The ratio of the offset to the preset minimum turning radius is calculated to obtain the first ratio. The arcsine function is performed on the first ratio to obtain the angle threshold.

[0089] Step S402: When the first judgment value is not greater than the angle threshold, it is determined that the target object is within the turning reach range of the robot.

[0090] Step S403: When the first judgment value is greater than the angle threshold, it is determined that the target object is not within the robot's turning reach range.

[0091] Obtain the pre-calibrated offset of the robotic arm base relative to the robot center. And the robot's preset minimum turning radius Offset This represents the installation distance of the robotic arm's base center along the X-axis in the robot chassis coordinate system, reflecting the degree of eccentricity of the robotic arm's working space relative to the robot body. It also includes a preset minimum turning radius. The kinematic constraint parameter inherent to differential drive robots is determined by the maximum steering curvature of the robot chassis and characterizes the minimum turning radius that the robot can achieve at any given time. Since the turning ability of a differential robot is limited by the maximum speed difference between the left and right wheels, when the radius of the planned arc trajectory is smaller than the preset minimum turning radius, the robot will be unable to travel along that arc due to wheel speed saturation. The absolute value of the orientation angle deviation is also considered. As the first judgment value. Simultaneously, the offset... With preset minimum turning radius Perform ratio calculations to obtain the first ratio. The first ratio reflects the dimensionless proportional relationship between the robotic arm's installation eccentricity and the robot's minimum turning radius. Then, an arcsine function operation is performed on the first ratio, i.e., the calculation... The angle threshold is obtained. The physical meaning of this angle threshold is that when the robot travels with the minimum turning radius, due to the distance between the robotic arm base and the robot's center... The offset results in an inherent deviation angle between the actual orientation of the robotic arm's end effector and the robot's body orientation, determined by the turning geometry. This angle threshold is the magnitude of this inherent deviation angle. This angle threshold is also affected by... and The coupling effect of the two parameters, when the offset Larger or minimum turning radius The smaller the value, the larger the angle threshold, indicating a more significant eccentricity effect of the robotic arm, and a correspondingly larger adjustable range of the end effector orientation during turning.

[0092] Then obtain the first judgment value With angle threshold Perform numerical comparisons. When When the target object's orientation angle deviation is within the maximum allowable deviation by the turning geometry, it indicates that the robot can travel along the arc trajectory with a preset minimum turning radius, allowing the end effector of the robotic arm to align with the target object at the end of the arc motion. At this point, the target object is determined to be within the robot's turning reachable range. This judgment condition unifies the target orientation angle deviation, the robotic arm installation offset, and the minimum turning radius into a single geometric judgment framework. Turning is only determined to be reachable when the target orientation deviation falls within the angle range defined by the offset and the minimum turning radius. If the target orientation deviation exceeds this range, even if the robot travels with the minimum turning radius, its arc trajectory end cannot form an effective alignment pose.

[0093] when When the current orientation angle deviation exceeds the adjustment range allowed by the turning geometry, even if the robot performs circular motion with the minimum turning radius, it cannot adjust the end effector of the robotic arm to the alignment posture while approaching the target. At this point, it is determined that the target object is not within the robot's turning reach range, and the robot cannot effectively approach and pick up the target object through a single circular motion, so it abandons picking up the target object.

[0094] By incorporating the asymmetric installation parameter—the offset of the robotic arm base relative to the robot's center—into the turning accessibility assessment, the technical bias of simplifying the accessibility between the robot and the target to a single distance threshold comparison or considering only the body's kinematic constraints is mitigated. Based on this, and combined with the curvature constraint of the robot's preset minimum turning radius and the directional constraint of the target's orientation angle deviation, a system based on... The geometric coupling criterion describes the nonlinear coupling relationship between the target spatial distribution, the physical installation of the robotic arm, and the robot's turning maneuverability in a concise mathematical form. This criterion enables precise quantitative characterization of the boundary of the turning reachable area, effectively reducing invalid picking attempts caused by the end effector's inability to align with the target object at the end of the circular motion due to the eccentric installation of the robotic arm. This improves the accuracy of motion trajectory planning and the success rate of garbage picking tasks.

[0095] After determining that the target object is within the turning radius, the robot needs to execute a circular trajectory motion to adjust its orientation while approaching the target object, ensuring that the target object falls into the robotic arm's pickup orientation range at the end of the motion. The core of this circular trajectory motion lies in unifying the robot's preset minimum turning radius, current linear velocity, and the direction and magnitude of the current orientation angular deviation to generate linear and angular velocity control quantities for driving the differential chassis to execute the circular motion, allowing the robot to smoothly approach the target object along a circular arc trajectory of equal radius. Controlling the robot to approach the target object along the circular arc trajectory includes the following steps:

[0096] Step S404: The sign function value of the orientation angle deviation is used as the steering direction indication value. The ratio of the robot's preset minimum turning radius to the robot's current linear velocity is calculated to obtain the turning angular velocity amplitude. The steering direction indication value and the turning angular velocity amplitude are multiplied to obtain the target angular velocity.

[0097] Step S405: Generate circular motion control commands based on the target angular velocity and the current linear velocity, and control the robot to move towards the target object along the circular arc trajectory at the current linear velocity and the target angular velocity according to the circular motion control commands.

[0098] Once the target object is determined to be within the turning radius, the robot enters a circular trajectory motion mode. The robot first acquires the measured value of its current linear velocity. This refers to the current linear velocity, which is obtained in real time from a wheeled odometer or inertial measurement unit and serves as the baseline for the linear velocity maintained during the circular motion. Simultaneously, the orientation angle deviation is also acquired. The sign of the orientation angle deviation directly determines the direction of the robot's circular motion.

[0099] Then the orientation angle deviation Input to symbolic function In the diagram, the sign function is used to extract the positive and negative sign information of the orientation angle deviation: when When, the value of the symbolic function is This indicates that the robot's current orientation is deviated to the left relative to the target direction, and it needs to turn to the right to achieve alignment; when When, the value of the symbolic function is This indicates that the robot's current orientation is deviated to the right relative to the target direction, and it needs to turn to the left to achieve alignment; when When, the value of the symbolic function is This indicates that there is no orientation deviation. The symbolic function value indicates the steering direction when the robot chassis performs circular motion, i.e., the steering direction indication value. This steering direction indication value is a value that only takes... , or Discrete integer values ​​can clearly indicate the steering direction in the simplest way, avoiding interference from the magnitude of the angle value in determining the steering direction.

[0100] Then preset the minimum turning radius for the robot. With current linear velocity Perform ratio operations, that is, calculate The turning angular velocity amplitude is obtained. Based on the kinematic model of the differential drive robot, when the robot moves at a linear velocity... Turning radius When moving, its angular velocity is the ratio of its linear velocity to its turning radius. This represents the absolute value of the angular velocity required to turn with the minimum turning radius while maintaining the current linear velocity. The turning angular velocity amplitude is a non-negative scalar used to characterize the curvature of the circular trajectory. The turning direction indicator is then then... With turning angular velocity amplitude Perform the product operation to obtain the target angular velocity. ,Right now The absolute value of the target angular velocity is determined by both the current linear velocity and the minimum turning radius, ensuring that the curvature of the circular trajectory reaches the maximum value allowed by the robot's kinematics when traveling at the current linear velocity, i.e., the robot moves in a circular arc with the minimum turning radius; its sign is determined by the direction of the orientation angle deviation, ensuring that the robot turns in the direction that reduces the orientation angle deviation. Through this product operation, the discrete steering direction indication and the continuous angular velocity amplitude are fused into a unified target angular velocity, enabling the robot to move towards the target object along the circular trajectory with maximum turning capability while maintaining the current linear velocity, completing the dual objectives of orientation alignment and distance approximation in the shortest possible time.

[0101] After obtaining the target angular velocity and the current linear velocity, the robot chassis controller uses the current linear velocity and the target angular velocity as the target values ​​for the motion control command, generating the corresponding circular motion control command. This circular motion control command includes a linear velocity component. and angular velocity components The linear velocity component is equal to the measured value of the current linear velocity, and the angular velocity component is equal to the target angular velocity calculated above. After the circular motion control command is sent to the robot chassis motion controller, the left and right drive wheels of the robot chassis execute the corresponding rotational speed output according to the differential kinematics model. Due to the different rotational speeds of the left and right drive wheels, the robot body will travel along a circular arc trajectory with constant curvature: when the target angular velocity is positive, the robot turns counterclockwise; when the target angular velocity is negative, the robot turns clockwise. Throughout the circular motion, the robot's linear velocity and angular velocity remain constant, making the radius of the circular arc trajectory equal to the preset minimum turning radius. The curvature of this trajectory reaches the limit of robot kinematics, allowing the robot to adjust its orientation with the fastest angular velocity while approaching the target object.

[0102] During the robot's movement along the circular arc trajectory, the real-time orientation angle deviation and real-time distance deviation between the robot and the target object are continuously recalculated in step S200 within the control cycle T. As the circular arc motion continues, the robot's orientation gradually rotates towards the target direction, and the absolute value of the real-time orientation angle deviation gradually decreases. When the absolute value of the real-time orientation angle deviation decreases to no greater than the preset orientation threshold, it indicates that the robot has completed orientation alignment at the end of the circular arc motion, and the target object has fallen into the picking orientation range. At this time, the robot generates an angular velocity zeroing command, controls the angular velocity to zero, and switches the robot from circular arc motion to linear motion, continuing to approach the target object with the current pose until it enters the robotic arm's working distance range. By integrating the minimum turning radius, the current linear velocity, and the orientation angle deviation direction into a single target angular velocity control quantity, the robot executes circular arc trajectory motion with the minimum turning radius and completes orientation adjustment while approaching the target with a constant linear velocity. The generation of this circular motion control command does not require complex trajectory planning or iterative solutions. It can be obtained instantaneously within each control cycle using only symbolic functions and ratio calculations, which greatly reduces the computational overhead of motion control. This enables the robot to respond to changes in perceived information in real time on the embedded control platform, effectively improving the motion planning efficiency and execution success rate of garbage picking in complex environments.

[0103] By incorporating the offset of the robotic arm base relative to the robot's center and the robot's preset minimum turning radius into the turning accessibility judgment and circular trajectory motion control, an integrated circular arc approximation framework is constructed, from geometric feasibility judgment to control quantity generation. This enables the robot to execute a circular arc trajectory with the minimum turning radius, completing orientation adjustment while approaching the target object. This solves the technical problem that differential robots cannot effectively approach the target due to kinematic constraints outside the picking orientation range, and improves the success rate and execution efficiency of garbage picking tasks under complex posture conditions.

[0104] Preferably, during the robot's movement, the real-time orientation angle deviation between the robot and the target object is reacquired according to a preset control cycle, and the real-time orientation angle deviation is compared with a preset orientation threshold. When the real-time orientation angle deviation meets the preset orientation threshold, an angular velocity zeroing command is generated, and the robot's angular velocity is controlled to return to zero according to the angular velocity zeroing command to trigger the pickup operation.

[0105] During the robot's movement along the circular arc trajectory, a rolling time-domain control method can be used to achieve closed-loop execution and dynamic adaptation. Specifically, steps S100 to S200 are executed periodically according to a preset control cycle T to reacquire the real-time orientation angle deviation and real-time distance deviation between the robot and the target object within each control cycle. Within each control cycle, the absolute value of the real-time orientation angle deviation is compared with a preset orientation threshold. When the absolute value of the real-time orientation angle deviation is not greater than the preset orientation threshold, it indicates that the robot has completed orientation alignment during the circular arc movement, and the target object has fallen within the picking orientation range. At this point, an angular velocity zeroing command is generated, and the robot's angular velocity is controlled to zero according to the angular velocity zeroing command, causing the robot to switch from circular arc movement to linear movement mode, continuing to approach the target object along the current orientation at the current linear velocity. In subsequent control cycles, the real-time distance deviation is continuously monitored. When the real-time distance deviation meets the robot arm's working distance range, a stop motion control command is generated, and the robot stops moving, subsequently triggering the aforementioned picking operation. If, during the circular trajectory movement, the absolute value of the real-time orientation angle deviation consistently exceeds the preset orientation threshold until the robot reaches the preset limit distance, it indicates that the robot cannot effectively approach the target object via the circular trajectory, and the pickup of the target object is abandoned. Through the aforementioned rolling time-domain closed-loop control mechanism, the orientation alignment status is continuously monitored during the circular trajectory approach process, and the movement mode is switched in a timely manner. This allows the robot to perform circular motion with the minimum turning radius while simultaneously switching to linear approach at the moment of orientation alignment, shortening ineffective detour distances and reducing energy consumption, effectively improving the overall execution efficiency of the pickup task. Closed-loop control during the circular trajectory approach process is achieved through periodic replanning. When the real-time orientation angle deviation meets the threshold, the angular velocity is triggered to zero, and the robot switches to linear motion, ensuring that the robot completes the dual objectives of orientation adjustment and distance approach in the shortest possible time, significantly improving the success rate and execution efficiency of garbage pickup tasks under complex pose conditions.

[0106] Figure 3 This is a block diagram of a robot motion trajectory generation system for picking up objects, as provided in this application. Figure 3As shown, a robot motion trajectory generation system for picking up objects includes: an acquisition module, used to acquire the three-dimensional coordinates of a target object in the depth camera coordinate system, and transform the three-dimensional coordinates to the world coordinate system according to the installation relationship between the depth camera and the robot chassis center to obtain the object position coordinates and object orientation angle in the world coordinate system; a deviation module, used to acquire the robot's current position coordinates and current orientation angle in the world coordinate system, and determine the robot's orientation angle deviation and distance deviation relative to the target object based on the object position coordinates, object orientation angle, current position coordinates, and current orientation angle; and a trajectory module, used to determine whether the target object is within the robot's picking orientation range based on the orientation angle deviation and a preset orientation threshold. If it is, the module determines whether the target object is within the robot's robotic arm working distance range based on the distance deviation. If yes, the robot is triggered to perform a picking operation; if no, the robot is controlled to approach the target object in a straight line; if not, the module determines whether the target object is within the robot's turning reach range based on the orientation angle deviation, the offset of the robotic arm base relative to the robot center, and the robot's preset minimum turning radius. If yes, the robot is controlled to approach the target object along an arc trajectory; otherwise, the picking up of the target object is abandoned.

[0107] The other functions performed by the acquisition module, deviation module, and trajectory module, as well as the technical details of each function, are the same as or similar to the corresponding features in the previously described method for generating robot motion trajectories for picking, so they will not be repeated here.

[0108] It should be understood that although the steps in the flowcharts in the accompanying figures are shown sequentially as indicated by the arrows, these steps are not necessarily performed in the order indicated by the arrows. Unless explicitly stated herein, there is no strict order requirement for the execution of these steps, and they can be performed in other orders.

[0109] The above description is only a partial embodiment of this application. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of this application, and these improvements and modifications should also be considered within the scope of protection of this application.

Claims

1. A method for generating the motion trajectory of a robot for picking up objects, characterized in that, The method includes: Obtain the three-dimensional coordinates of the target object in the depth camera coordinate system, and transform the three-dimensional coordinates to the world coordinate system according to the installation relationship between the depth camera and the center of the robot chassis to obtain the object position coordinates and object orientation angle of the target object in the world coordinate system; Obtain the robot's current position coordinates and current orientation angle in the world coordinate system, and determine the robot's orientation angle deviation and distance deviation relative to the target object based on the object's position coordinates, object's orientation angle, current position coordinates, and current orientation angle. Based on the orientation angle deviation and the preset orientation threshold, it is determined whether the target object is within the robot's picking orientation range. If it is, based on the distance deviation, it is determined whether the target object is within the robot's robotic arm working distance range. If yes, the robot is triggered to perform a picking operation; otherwise, the robot is controlled to approach the target object in a straight line. If not, the robot determines whether the target object is within the robot's turning range based on the orientation angle deviation, the offset of the robotic arm base relative to the robot's center, and the robot's preset minimum turning radius. If so, the robot is controlled to approach the target object along an arc trajectory; otherwise, the robot abandons picking up the target object.

2. The method according to claim 1, characterized in that, The process of obtaining the three-dimensional coordinates of the target object in the depth camera coordinate system includes: The system acquires real-time environmental image data and 3D point cloud data from the depth camera, performs target detection processing on the environmental image data to obtain the position information of the target object in the image, extracts the point cloud data corresponding to the target object based on the coordinate mapping relationship between the position information and the 3D point cloud data, and performs centroid calculation processing on the point cloud data to obtain the 3D coordinates of the target object in the depth camera coordinate system.

3. The method according to claim 2, characterized in that, The step of transforming the three-dimensional coordinates to the world coordinate system based on the installation relationship between the depth camera and the center of the robot chassis to obtain the object position coordinates and object orientation angle of the target object in the world coordinate system includes: Based on the installation relationship between the depth camera and the center of the robot chassis, a first coordinate transformation matrix is ​​determined from the depth camera coordinate system to the robot chassis base coordinate system. The first coordinate transformation matrix is ​​used to perform matrix multiplication on the three-dimensional coordinates to obtain the first position coordinates of the target object in the robot chassis base coordinate system. Based on the initial pose correspondence between the robot chassis base coordinate system and the world coordinate system, a second coordinate transformation matrix from the robot chassis base coordinate system to the world coordinate system is determined. The second coordinate transformation matrix is ​​then used to perform matrix multiplication on the first position coordinates to obtain the object position coordinates of the target object in the world coordinate system. The object orientation angle in the world coordinate system is obtained by taking the arctangent value after calculating the ratio of the x-axis component and the y-axis component in the object's position coordinates.

4. The method according to claim 3, characterized in that, The determination of the robot's orientation angle deviation and distance deviation relative to the target object based on the object's position coordinates, object's orientation angle, current position coordinates, and current orientation angle includes: The difference between the object's position coordinates and the current position coordinates is calculated to obtain a relative position vector, and the magnitude of the relative position vector is calculated to obtain the distance deviation; The object's orientation angle and the current orientation angle are compared to obtain an angle difference value. The angle difference value is then normalized to obtain an orientation angle deviation.

5. The method according to claim 1, characterized in that, The triggering of the robot to perform the picking operation includes: The robot's current linear velocity and current angular velocity are obtained, and a stop control command is generated based on the current linear velocity and current angular velocity to control the robot to decelerate to a stationary state; When the robot is stationary, the current joint angle data of the robotic arm is acquired, and the target node angles of each joint of the robotic arm are obtained by inverse kinematics calculation based on the object coordinate position and the current joint angle data. Based on the target node angle, joint space trajectory planning data is generated, and a pick-up start signal is generated based on the joint space trajectory planning data to control the robotic arm to perform a pick-up action to grasp the target object according to the joint space trajectory planning data.

6. The method according to claim 5, characterized in that, The controlled robot's linear approach to the target object includes: The current linear velocity is subjected to velocity holding processing to obtain the target linear velocity, and the current angular velocity is subjected to zeroing processing to obtain the target angular velocity; Generate linear motion control commands based on the target linear velocity and the target angular velocity, and control the robot to move in a straight line toward the target object at the target linear velocity and the target angular velocity according to the linear motion control commands; During the robot's movement, the real-time distance deviation between the robot and the target object is reacquired according to a preset control cycle, and the real-time distance deviation is compared with the range of the robotic arm's working distance using a threshold method. When the real-time distance deviation meets the working distance range of the robotic arm, a stop motion control command is generated, and the robot is controlled to stop moving in accordance with the stop motion control command to trigger the execution of the picking operation.

7. The method according to claim 1, characterized in that, The step of determining whether the target object is within the robot's turning range based on the orientation angle deviation, the offset of the robotic arm base relative to the robot's center, and the robot's preset minimum turning radius includes: The absolute value of the orientation angle deviation is used as the first judgment value. The ratio of the offset to the preset minimum turning radius is calculated to obtain the first ratio. The arcsine function is applied to the first ratio to obtain the angle threshold. When the first judgment value is not greater than the angle threshold, it is determined that the target object is within the turning reach range of the robot; When the first judgment value is greater than the angle threshold, it is determined that the target object is not within the robot's turning reach range.

8. The method according to claim 5, characterized in that, The controlled robot approaches the target object along an arc trajectory, including: The sign function value of the orientation angle deviation is used as the steering direction indication value. The ratio of the robot's preset minimum turning radius to the robot's current linear velocity is calculated to obtain the turning angular velocity amplitude. The steering direction indication value and the turning angular velocity amplitude are multiplied to obtain the target angular velocity. Based on the target angular velocity and the current linear velocity, a circular arc motion control command is generated, and the robot is controlled to move towards the target object along the circular arc trajectory at the current linear velocity and the target angular velocity according to the circular arc motion control command.

9. The method according to claim 8, characterized in that, The method further includes: During the robot's movement, the real-time orientation angle deviation between the robot and the target object is reacquired according to a preset control cycle, and the real-time orientation angle deviation is compared with the preset orientation threshold. When the real-time orientation angle deviation meets the preset orientation threshold, an angular velocity zeroing command is generated, and the robot's angular velocity is controlled to return to zero according to the angular velocity zeroing command to trigger the pickup operation.

10. A robot motion trajectory generation system for picking up objects, characterized in that, The system includes: The acquisition module is used to acquire the three-dimensional coordinates of the target object in the depth camera coordinate system. Based on the installation relationship between the depth camera and the center of the robot chassis, the three-dimensional coordinates are transformed to the world coordinate system to obtain the object position coordinates and object orientation angle of the target object in the world coordinate system. The deviation module is used to obtain the robot's current position coordinates and current orientation angle in the world coordinate system, and to determine the robot's orientation angle deviation and distance deviation relative to the target object based on the object's position coordinates, object's orientation angle, current position coordinates, and current orientation angle. The trajectory module is used to determine whether the target object is within the robot's picking-up orientation range based on the orientation angle deviation and a preset orientation threshold. If it is, the module determines whether the target object is within the robot's robotic arm working distance range based on the distance deviation. If yes, the robot is triggered to perform a picking-up operation. If no, the robot is controlled to approach the target object in a straight line. If not, the module determines whether the target object is within the robot's turning reach range based on the orientation angle deviation, the offset of the robotic arm base relative to the robot's center, and the robot's preset minimum turning radius. If yes, the robot is controlled to approach the target object along an arc trajectory. Otherwise, the picking-up of the target object is abandoned.

Images