Feeding robot cooperative control method and device, electronic equipment and storage medium

Abstract

This application discloses a collaborative control method, device, electronic device, and storage medium for a feeding robot, relating to the field of industrial vision and intelligence technology. The method includes: extracting features from target image data to obtain the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region; transforming the three-dimensional spatial pose and three-dimensional dynamic spatial position into the robotic arm base coordinate system based on a pre-calibrated hand-eye transformation matrix to generate the corresponding target execution pose and lip tracking pose; reconstructing the overall offset of the human teaching trajectory data in joint space according to the spatial deviation between the target execution pose and the human teaching trajectory data to generate a continuous biomimetic motion trajectory; and monitoring the robotic arm end effector and lip tracking pose, as well as the user's lip opening and closing state, in real time during the movement, and controlling the robotic arm to perform feeding propulsion actions when preset feeding conditions are met, thereby improving the feeding robot's high-precision real-time tracking of the feeding position.

Description

Technical Field

[0001] This application relates to the field of industrial vision and intelligence technology, and in particular to collaborative control methods, devices, electronic devices and storage media for feeding robots. Background Technology

[0002] For patients with upper limb dysfunction or the elderly, independent eating is crucial for improving their quality of life. Most current feeding robots rely on preset trajectories or simple remote control, presenting the following challenges: First, the variety and shapes of food make it difficult for traditional visual algorithms to achieve high-precision positioning in complex environments; second, the human head and mouth undergo random displacement during eating, and the lack of real-time dynamic tracking can lead to feeding deviations and even safety hazards; finally, improper trajectory planning by the robotic arm can cause vibrations or stiff movements, affecting the user's eating experience.

[0003] Therefore, in complex environments, how to improve the high-precision real-time tracking of the dynamic position of the human mouth by feeding robots is a problem that urgently needs to be solved. Summary of the Invention

[0004] The main purpose of this application is to provide a collaborative control method, device, electronic device and storage medium for a feeding robot, which aims to solve the technical problem of how to improve the high-precision real-time tracking of the dynamic position of the human mouth by the feeding robot in complex environments.

[0005] To achieve the above objectives, this application proposes a collaborative control method for a feeding robot, which is applied to a feeding robot and includes: Acquire target image data, perform target detection and facial feature point extraction on the target image data, and obtain the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region; Based on the pre-calibrated hand-eye transformation matrix, the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region are transformed into the robot arm base coordinate system to generate the corresponding target execution pose and lip tracking pose. Acquire pre-stored human teaching trajectory data, and based on the spatial deviation between the target execution pose and the reference pose in the human teaching trajectory data, perform overall offset reconstruction of the human teaching trajectory data in joint space, and generate continuous bionic motion trajectories by combining interpolation algorithms. The robotic arm is driven to move according to the biomimetic motion trajectory. During the movement, the distance between the end of the robotic arm and the lip tracking pose, as well as the opening and closing state of the user's lips, are monitored in real time. When the distance is less than or equal to a safety threshold and the opening and closing state of the lips meets the preset opening conditions, the robotic arm is controlled to perform a feeding and pushing action.

[0006] In one embodiment, the target image data includes food container image data and user facial image data. The step of acquiring the target image data, performing target detection and facial feature point extraction on the target image data to obtain the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region includes: The food container is detected from the food container image data based on the object detection algorithm, the two-dimensional detection bounding box of the food container is extracted, and the pixel center point coordinates of the two-dimensional detection bounding box are calculated. Acquire depth image data aligned with the food container image data; define a local neighborhood in the depth image data with the center point coordinates of the pixel as the center; select the effective depth value within the local neighborhood; calculate the statistical average value of the effective depth value; and obtain the noise-resistant depth information of the food container. Based on the camera intrinsic parameter matrix, the coordinates of the pixel center point and the noise-resistant depth information are inversely projected and calculated to obtain the three-dimensional spatial pose of the food container in the camera coordinate system. Based on the facial feature point extraction algorithm, a three-dimensional topological mesh of the user's face is extracted from the user's facial image data. The key points of the lips are located from the three-dimensional topological mesh, and the three-dimensional dynamic spatial position of the user's lip region is determined based on the key points of the lips.

[0007] In one embodiment, after the steps of extracting a three-dimensional topological mesh of the user's face from the user's facial image data based on the facial feature point extraction algorithm, locating lip key points from the three-dimensional topological mesh, and determining the three-dimensional dynamic spatial position of the user's lip region based on the lip key points, the method further includes: Obtain the current 3D observation vector of the food container in the camera coordinate system and determine the current original coordinate value; Read the smoothed coordinate value saved from the previous time step; According to the exponential moving average filtering algorithm, the current original coordinate value and the previous smoothed coordinate value are weighted and fused according to a preset weight factor to generate the current smoothed coordinate value. The weight factor is used to balance response speed and data stability. The current smoothed coordinate value is used to replace the current original coordinate value as the input for subsequent coordinate mapping steps.

[0008] In one embodiment, the steps of acquiring pre-stored human teaching trajectory data, reconstructing the overall offset of the human teaching trajectory data in joint space based on the spatial deviation between the target execution pose and the reference pose in the human teaching trajectory data, and generating a continuous bionic motion trajectory using an interpolation algorithm, include: Acquire pre-stored human teaching trajectory data, which includes a discrete joint angle sequence, and the end reference pose and starting joint angle corresponding to the starting frame in the discrete joint angle sequence; Calculate the three-dimensional translational deviation vector between the target execution pose and the end effector reference pose; The three-dimensional translational deviation vector is compensated to the end reference pose to obtain the compensated initial end pose. The inverse kinematics algorithm is used to solve the compensated initial and final poses to obtain the true initial joint angles under the current physical environment. Calculate the difference between the actual starting joint angle and the starting joint angle to obtain the global constant joint offset constant; The global constant joint offset constant is superimposed onto the overall discrete joint angle sequence of the human teaching trajectory data to obtain the offset reconstruction trajectory sequence in joint space. Based on the piecewise cubic Hermitian interpolation polynomial, the joint trajectories between adjacent keyframes in the offset reconstructed trajectory sequence are interpolated and fitted to generate continuous biomimetic motion trajectories.

[0009] In one embodiment, the step of solving the compensated initial and final poses using an inverse kinematics algorithm to obtain the true initial joint angles under the current physical environment includes: Construct the positive kinematics function of the robotic arm and use the desired end-effector pose matrix as the solution objective; An inverse kinematics optimization objective function is established, which is used to minimize the weighted error between the calculation result of the forward kinematics function and the end target pose matrix, wherein the weighted error is adjusted by a mask weight matrix; The inverse kinematics optimization objective function is solved iteratively based on the Levenberg-Marquardt algorithm, and the damping factor is dynamically adjusted during the iteration process to maintain the numerical stability of the Jacobian matrix. During the iterative solution process, the condition number of the Jacobian matrix or the convergence status of the solution are monitored in real time. When it is determined that the Jacobian matrix is ​​close to singular or the current pose solution is not converged, the current abnormal iteration branch is actively intercepted and switched to the pre-calibrated non-interference safe initial guess value. The iterative solution is restarted from the pre-calibrated non-interference safe initial guess value to obtain the true starting joint angle under the current physical environment.

[0010] In one embodiment, the step of driving the robotic arm to move according to the biomimetic motion trajectory, and monitoring in real time the distance between the robotic arm end effector and the lip tracking pose, as well as the user's lip opening and closing state, and controlling the robotic arm to perform a feeding propulsion action when the distance is less than or equal to a safety threshold and the lip opening and closing state meets a preset mouth opening condition, includes: The robotic arm is driven to move according to the biomimetic motion trajectory, and the current end position of the robotic arm in the robotic arm base coordinate system is obtained in real time. The current mouth center position is determined based on the lip tracking pose. Calculate the Euclidean distance between the current end position and the current mouth center position; Determine whether the Euclidean distance is greater than a preset safe distance threshold; If the Euclidean distance is greater than the preset safe distance threshold, the smooth drive function is invoked to control the robotic arm to smoothly approach the current mouth center position; When the Euclidean distance is less than or equal to the safe distance threshold, the three-dimensional Euclidean distance between the center key point of the user's upper lip and the center key point of the lower lip is calculated in real time to determine the current lip spacing. Determine whether the current lip spacing is greater than a preset mouth opening determination threshold for multiple consecutive frames; When the current lip spacing is greater than the mouth opening threshold for multiple consecutive frames, the robotic arm is controlled to perform a feeding and pushing action.

[0011] In one embodiment, after the step of driving the robotic arm to move according to the biomimetic motion trajectory, and monitoring in real time the distance between the robotic arm end effector and the lip tracking pose, as well as the user's lip opening and closing state, and controlling the robotic arm to perform a feeding propulsion action when the distance is less than or equal to a safety threshold and the lip opening and closing state meets a preset mouth opening condition, the method further includes: In response to an external emergency stop command or a stop command from the software monitoring interface, the visual tracking thread's flag is immediately set to standby mode, and the closed-loop tracking control logic for the user's lip area is cut off from the data source. Obtain the angles of each joint at the moment the robotic arm triggers the stop command, as the starting pose for emergency retraction; Set the robotic arm's safe retraction zero-point folding posture as the emergency retraction target posture; Using the emergency return start pose and the emergency return target pose as boundary conditions, the joint angular velocity and angular acceleration at the start and end times are constrained to be zero. Within the preset return cycle, a smooth return trajectory interpolated by a fifth-order polynomial is generated. Control the robotic arm to move along the smooth return trajectory until it safely retracts to the zero-point folded posture.

[0012] Furthermore, to achieve the above objectives, this application also proposes a collaborative control device for a feeding robot, the collaborative control device for the feeding robot comprising: The data acquisition module is used to acquire target image data, perform target detection and facial feature point extraction on the target image data, and obtain the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region. The coordinate transformation module is used to transform the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region to the robot arm base coordinate system based on the pre-calibrated hand-eye transformation matrix, and generate the corresponding target execution pose and lip tracking pose. The trajectory reconstruction module is used to acquire pre-stored human teaching trajectory data, and based on the spatial deviation between the target execution pose and the reference pose in the human teaching trajectory data, to perform overall offset reconstruction of the human teaching trajectory data in joint space, and to generate continuous bionic motion trajectories by combining interpolation algorithms. The feeding propulsion module is used to drive the robotic arm to move according to the bionic motion trajectory. During the movement, it monitors the distance between the end of the robotic arm and the lip tracking pose and the opening and closing state of the user's lips in real time. When the distance is less than or equal to a safety threshold and the lip opening and closing state meets the preset opening conditions, it controls the robotic arm to perform the feeding propulsion action.

[0013] In addition, to achieve the above objectives, this application also proposes an electronic device comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, the computer program being configured to implement the steps of the feeding robot collaborative control method described above.

[0014] In addition, to achieve the above objectives, this application also proposes a non-transitory computer storage medium storing a computer program, which, when executed by a processor, implements the steps of the feeding robot collaborative control method described above.

[0015] One or more technical solutions proposed in this application have at least the following technical effects: (1) Acquire target image data, perform target detection and facial feature point extraction on the target image data to obtain the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region; based on the pre-calibrated hand-eye transformation matrix, transform the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region to the base coordinate system of the robotic arm to generate the corresponding target execution pose and lip tracking pose; acquire pre-stored human teaching trajectory data, and according to the spatial deviation between the target execution pose and the reference pose in the human teaching trajectory data, perform overall offset reconstruction of the human teaching trajectory data in joint space, and combine interpolation algorithm to generate continuous bionic motion trajectory; drive the robotic arm to move according to the bionic motion trajectory, and monitor the distance between the end of the robotic arm and the lip tracking pose and the opening and closing state of the user's lips in real time during the movement. When the distance is less than or equal to the safety threshold and the opening and closing state of the lips meets the preset opening conditions, control the robotic arm to perform feeding and pushing actions. By acquiring target image data containing food containers and user faces, and performing target detection and facial feature point extraction, this method solves the problem that traditional methods cannot simultaneously identify food containers and dynamically track user lips in complex environments, achieving accurate perception of the 3D spatial pose of the food container and the 3D dynamic spatial position of the user's lip region. By employing a pre-calibrated hand-eye transformation matrix to transform the pose from the camera coordinate system to the robotic arm's base coordinate system, the problem of coordinate unification between visual information and robot motion control is solved, achieving precise mapping of the target execution pose and lip tracking pose in physical space. Furthermore, by using a method that reconstructs the overall joint space offset based on the spatial deviation between the target execution pose and the reference pose in the human-taught trajectory data, combined with an interpolation algorithm to generate continuous biomimetic motion trajectories, this method overcomes the problems of stiff movements, food spillage, and poor adaptability to environmental changes inherent in traditional trajectory planning methods, achieving smooth, anthropomorphic motion that retains the natural acceleration characteristics of human scooping while adapting to different tableware positions. By employing real-time monitoring of the distance between the end effector and the lip tracking pose, as well as the user's lip opening and closing state during the movement of the robotic arm, and triggering the feeding propulsion action when safety thresholds and preset mouth opening conditions are met, the positioning deviation and safety risks caused by random displacement of the human head during feeding are solved. This achieves closed-loop safe feeding control based on human-in-the-loop intention recognition, effectively ensuring the accuracy and physical safety of the interaction process, and improving the high-precision real-time tracking of the dynamic position of the human mouth by the feeding robot in complex environments.

[0016] (2) Obtain pre-stored human teaching trajectory data, which includes a discrete joint angle sequence, and the end reference pose and starting joint angle corresponding to the starting frame in the discrete joint angle sequence; calculate the three-dimensional translational deviation vector between the target execution pose and the end reference pose; compensate the three-dimensional translational deviation vector to the end reference pose to obtain the compensated starting end pose; solve the compensated starting end pose using the inverse kinematics algorithm to obtain the real starting joint angle in the current physical environment; calculate the difference between the real starting joint angle and the starting joint angle to obtain the global constant joint offset constant; superimpose the global constant joint offset constant onto the overall discrete joint angle sequence of the human teaching trajectory data to obtain the offset reconstruction trajectory sequence in joint space; based on the piecewise cubic Hermite interpolation polynomial, interpolate and fit the joint trajectories between adjacent keyframes in the offset reconstruction trajectory sequence to generate continuous bionic motion trajectories. This method employs pre-stored human teaching trajectory data, including discrete joint angle sequences and the initial frame reference pose, to address the lack of human-like motion templates in traditional online planning and inherit the natural acceleration characteristics of humans scooping food. By calculating the three-dimensional translational deviation vector between the target execution pose and the end-effector reference pose and compensating for this deviation in the initial and end-effector poses, the problem of teaching trajectories being unusable due to changes in the placement of tableware is solved, enabling adaptive matching of the trajectory to the dynamic environment. Fitting the bias reconstruction sequence using piecewise cubic Hermitian interpolation polynomials resolves the issues of discontinuous motion and jerky movements between discrete points, ultimately generating a continuous biomimetic motion trajectory that combines human muscle memory characteristics with adaptability to the current environment, significantly improving the smoothness of the feeding action.

[0017] (3) In response to an external emergency stop command or a stop command from the software monitoring interface, immediately set the flag position of the visual tracking thread to standby state and cut off the closed-loop following control logic of the user's lip area from the data source end; obtain the joint angles of the robotic arm at the moment the stop command is triggered as the emergency return starting pose; set the safe return zero-point folding posture of the robotic arm as the emergency return target pose; use the emergency return starting pose and the emergency return target pose as boundary conditions, constrain the joint angular velocity and angular acceleration at the start and end times to be zero, and generate a smooth return trajectory with fifth-order polynomial interpolation within the preset return cycle; control the robotic arm to move to the safe return zero-point folding posture according to the smooth return trajectory. By using the initial and target poses as boundary conditions and strictly constraining the joint angular velocity and angular acceleration to be zero at the start and end times, the problem of huge mechanical shock and vibration caused by sudden stops or sudden turns is solved. The robotic arm is controlled to move strictly according to the smooth trajectory to the zero-point folding posture, ensuring that each joint of the robotic arm follows the "S"-shaped smooth velocity manifold when exiting the work space, thus comprehensively ensuring physical safety during close human-machine interaction. Attached Figure Description

[0018] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application.

[0019] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0020] Figure 1 This is a flowchart illustrating the first embodiment of the collaborative control method for feeding robots according to this application; Figure 2 This is a diagram showing the relationship between hand-eye calibration and spatial coordinate mapping in this application; Figure 3 This is a comparison of the depth space coordinate filtering effect based on the EMA algorithm in this application; Figure 4 This is a schematic diagram illustrating the human retrieval teaching trajectory and spatial translation compensation in this application. Figure 5 This is a graph showing the relationship between the intention to eat and the distance determination in this application. Figure 6 This is a block diagram of the biomimetic feeding and adaptive control of the feeding robot in this application; Figure 7 This is a flowchart of the state machine operation of the feeding robot in this application, which involves "finding objects, scooping up food, and feeding it". Figure 8 This is a smooth trajectory curve of the joint angle based on JTRAJ interpolation in this application; Figure 9 This is a flowchart of the inverse motion solution and safe backoff based on the LM algorithm in this application; Figure 10 This is a schematic diagram of the module structure of the collaborative control device for the feeding robot in this application.

[0021] The purpose, features, and advantages of this application will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0022] The technical solutions in the embodiments of this application will be clearly and completely described below with reference to the embodiments of this application. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of this application.

[0023] This application provides a collaborative control method for a feeding robot. Specifically, refer to... Figure 1 , Figure 1 This is a flowchart illustrating the first embodiment of the collaborative control method for a feeding robot according to this application. In this embodiment, the collaborative control method for a feeding robot includes steps S10 to S40:

[0024] Step S10: Acquire target image data, perform target detection and facial feature point extraction on the target image data to obtain the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region.

[0025] It should be noted that target image data refers to RGB images containing food containers and the user's face, along with their aligned depth images, acquired through visual sensors such as depth cameras. Target detection refers to the process of identifying and locating the food container from the image using target detection algorithms. Facial feature point extraction refers to the process of identifying and locating key points on the user's face from the image using facial feature point extraction algorithms. Food container refers to a vessel used to hold food to be fed, such as bowls, plates, dishes, etc., or cooking utensils such as pots, casseroles, and rice cooker inner pots, as well as packaging containers such as cans, cups, and jars. The following implementation will use tableware as an example. Three-dimensional spatial pose refers to the three-dimensional position coordinates (X, Y, Z) of the food container in the camera coordinate system and its spatial orientation. User lip region refers to the lip area on the user's face directly related to the eating action, including key points of the upper and lower lips. Three-dimensional dynamic spatial position refers to the real-time three-dimensional coordinates of the user's lip region in the camera coordinate system, which dynamically changes as the user's head moves.

[0026] Step S20: Based on the pre-calibrated hand-eye transformation matrix, the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region are transformed into the robot arm base coordinate system to generate the corresponding target execution pose and lip tracking pose.

[0027] It should be noted that the pre-calibrated hand-eye transformation matrix refers to the homogeneous transformation matrix obtained in advance through the hand-eye calibration process, used to transform coordinates in the camera coordinate system to the robot arm base coordinate system. The robot arm base coordinate system refers to the reference coordinate system established with the robot arm mounting base as the origin, used to describe the absolute spatial position of each component of the robot arm and target objects in the environment. The target execution pose refers to the three-dimensional spatial position and orientation of the food container in the robot arm base coordinate system after coordinate mapping, serving as the target for robot arm motion planning. The lip tracking pose refers to the three-dimensional dynamic spatial position of the user's lip area in the robot arm base coordinate system after coordinate mapping, used for real-time tracking of the user's mouth during the feeding phase.

[0028] Step S30: Obtain pre-stored human teaching trajectory data; based on the spatial deviation between the target execution pose and the reference pose in the human teaching trajectory data, perform overall offset reconstruction of the human teaching trajectory data in joint space; and combine interpolation algorithm to generate continuous bionic motion trajectory.

[0029] It should be noted that the pre-stored human teaching trajectory data refers to pre-recorded teaching action data of a human scooping food, including discrete joint angle sequences and the end-effector reference pose and starting joint angle corresponding to the starting frame. The reference pose refers to the end-effector spatial pose corresponding to the starting frame in the human teaching trajectory data, serving as a reference point for spatial deviation calculation. Spatial deviation refers to the three-dimensional translational deviation vector between the target execution pose and the reference pose in the current physical environment. Joint space is a multi-dimensional space composed of the joint angles of the robotic arm, used to describe the configuration state of the robotic arm. Overall bias reconstruction can be understood as converting the calculated spatial deviation into a constant bias in the joint space, and then superimposing this constant onto the joint angle sequence of the entire teaching trajectory, thereby achieving adaptive trajectory reconstruction. The interpolation algorithm is an algorithm that interpolates and fits discrete joint angle sequences to generate a continuous and smooth trajectory, such as a piecewise cubic Hermite interpolation polynomial. The continuous bionic motion trajectory refers to the robotic arm motion path with natural acceleration characteristics of human movements, obtained after bias reconstruction and interpolation smoothing.

[0030] Step S40: Drive the robotic arm to move according to the bionic motion trajectory. During the movement, monitor the distance between the end of the robotic arm and the lip tracking pose and the opening and closing state of the user's lips in real time. When the distance is less than or equal to the safety threshold and the opening and closing state of the lips meets the preset opening conditions, control the robotic arm to perform the feeding and pushing action.

[0031] It should be noted that the end effector of the robotic arm is the execution tool end of the feeding robot arm, which can be a gripper or a feeding spoon. The safety threshold refers to the preset upper limit of the Euclidean distance between the robotic arm end effector and the user's mouth, for example, 0.05 meters, used to determine whether the robotic arm has entered a safe feeding range. The user's lip opening state refers to the degree of opening between the user's upper and lower lips, quantified by real-time calculation of the three-dimensional spatial distance between the center key points of the upper and lower lips. The preset mouth opening condition refers to the lip opening state satisfying a lip distance greater than the preset mouth opening judgment threshold for multiple consecutive frames, used to identify the user's eating intention. The feeding propulsion action refers to the pulse command triggered after the safe distance and mouth opening conditions are met, which moves the coordinate system of the robotic arm end effector forward, delivering food into the user's mouth.

[0032] In this embodiment, by acquiring target image data containing food containers and the user's face and performing target detection and facial feature point extraction, the problem of traditional methods being unable to simultaneously identify food containers and dynamically track the user's lips in complex environments is solved, achieving accurate perception of the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region. By employing a pre-calibrated hand-eye transformation matrix to transform the pose in the camera coordinate system to the robot arm's base coordinate system, the problem of coordinate unification between visual information and robot motion control is solved, achieving precise mapping of the target execution pose and the lip tracking pose in physical space. By using a method that reconstructs the overall joint space offset based on the spatial deviation between the target execution pose and the reference pose in the human-taught trajectory data, combined with an interpolation algorithm to generate continuous biomimetic motion trajectories, the problems of stiff movements, food spillage, and poor adaptability to environmental changes in traditional trajectory planning methods are solved, achieving smooth, anthropomorphic motion that retains the natural acceleration characteristics of human scooping while adapting to different tableware positions. By employing real-time monitoring of the distance between the end effector and the lip tracking pose, as well as the user's lip opening and closing state during the movement of the robotic arm, and triggering the feeding propulsion action when safety thresholds and preset mouth opening conditions are met, the positioning deviation and safety risks caused by random displacement of the human head during feeding are solved. This achieves closed-loop safe feeding control based on human-in-the-loop intention recognition, effectively ensuring the accuracy and physical safety of the interaction process.

[0033] In one implementation, prior to step S10, the method further includes: in response to a system startup command, establishing a communication link with the robotic arm and controlling the robotic arm to safely unfold from a folded storage state to a preset visual observation posture. For example, upon system startup, the FeedingWorker thread can automatically execute the `auto_start_can` command to detect the status of the `can0` interface and configure the baud rate. Upon successful execution, the robotic arm safely unfolds from the folded state to the preset visual observation posture.

[0034] Considering that depth cameras are prone to depth holes and zero-value noise at object edges and reflective tableware surfaces, the system does not directly read the depth value of a single point. Instead, it introduces a local depth mean filter mask. Based on this, step S10 includes:

[0035] Step S101: Detect food containers from food container image data based on object detection algorithm, extract two-dimensional detection bounding boxes of food containers, and calculate the pixel center point coordinates of the two-dimensional detection bounding boxes.

[0036] It should be noted that the 2D detection bounding box can be understood as the rectangular box output by the object detection algorithm, represented by image pixel coordinates. It defines the position and size of the food container in the 2D image and can be defined by the coordinates of the top left and bottom right corners. The pixel center point coordinates are the pixel position of the geometric center of the 2D detection bounding box in the image, which can be represented by (u, v), and serve as the reference point for subsequent depth information extraction and 3D localization.

[0037] For example, tableware can be identified using a YOLOv8 algorithm improved based on deep feature fusion, obtaining the two-dimensional detection bounding box of the target tableware in the RGB image and extracting its pixel center point coordinates. : Step S102: Obtain depth image data aligned with the food container image data. Using the pixel center point coordinates as the center, define a local neighborhood in the depth image data, select the effective depth values ​​within the local neighborhood, calculate the statistical average of the effective depth values, and obtain the noise-resistant depth information of the food container.

[0038] It's important to note that aligned depth image data can be understood as a depth map synchronized spatially and temporally with the RGB image. Each pixel corresponds to the same position in the RGB image, storing the distance information from that point to the camera. The local neighborhood refers to an N×N pixel region (e.g., 5×5 or 7×7) defined on the depth image, centered on the pixel's center point, used to mitigate single-point depth noise. Within the local neighborhood, valid distance data with depth values ​​greater than 0 are considered valid depth values, excluding invalid measurements caused by reflections, transparency, or edge occlusion. The statistical average can be understood as the arithmetic mean obtained by summing all valid depth values ​​within the local neighborhood and dividing by the number of valid points; it is used to replace single-point depth and improve anti-interference capabilities. The noise-resistant depth information is a more stable and reliable vertical distance value from the food container to the camera obtained after local neighborhood averaging filtering, effectively suppressing zero-value noise and jumps in the original depth map.

[0039] Step S103: Based on the camera intrinsic parameter matrix, the pixel center point coordinates and noise-resistant depth information are inversely projected and calculated to obtain the three-dimensional spatial pose of the food container in the camera coordinate system.

[0040] It should be noted that the camera intrinsic parameter matrix is ​​the internal parameter matrix obtained through calibration at the camera's factory, usually denoted as K. It includes the focal length and principal point coordinates, describing the projection relationship from the 3D camera coordinate system to the 2D pixel coordinate system. Inverse projection calculation is the inverse operation of the pinhole camera imaging model, a mathematical process that calculates the 2D pixel coordinates and corresponding noise-resistant depth values ​​back to the 3D spatial coordinates in the camera coordinate system. The 3D spatial pose is a complete spatial description of the food container in the camera coordinate system, including its 3D position coordinates and spatial orientation.

[0041] For example, in the aligned depth map, extract the... Centered Effective depth set of pixel neighborhood Calculate the actual physical distance after noise reduction. : Subsequently, the system calls the camera's factory-calibrated intrinsic parameter matrix. Based on the pinhole camera imaging inverse projection model, the two-dimensional pixel features and one-dimensional depth features are jointly solved to directly output the target in the camera coordinate system. The three-dimensional spatial observation vector below The corresponding formula is: in, , For camera focal length, , It is the main optical center.

[0042] Step S104: Extract the three-dimensional topological mesh of the user's face from the user's facial image data based on the facial feature point extraction algorithm, locate the key points of the lips from the three-dimensional topological mesh, and determine the three-dimensional dynamic spatial position of the user's lip region based on the key points of the lips.

[0043] It should be noted that the 3D topological mesh is a mesh structure composed of hundreds of 3D key points covering the surface of the face, output by the facial feature point extraction algorithm. Each point has corresponding 3D coordinates, describing the geometry of the facial surface. Lip key points are feature points specifically representing the contour and center position of the lips, selected from the 3D topological mesh, such as the center point of the upper lip and the center point of the lower lip. The user's lip region refers to the part of the user's face directly related to eating actions, defined by lip key points. The 3D dynamic spatial position is the real-time 3D coordinate of the user's lip region in the camera coordinate system. This position changes dynamically with the movement and rotation of the user's head and needs continuous tracking. For example, the MediaPipeFace Mesh algorithm can be used to locate lip key points. The relationship between hand-eye calibration and spatial coordinate mapping is shown in the figure below. Figure 2 As shown.

[0044] In this embodiment, a target detection algorithm is used to detect and locate the food container from the image data, and extract the two-dimensional detection bounding box and its pixel center coordinates. This solves the problem that traditional vision algorithms struggle to stably identify tableware of different shapes and materials in complex backgrounds, achieving high-precision preliminary positioning of the food container on the image plane. Furthermore, by acquiring depth image data strictly aligned with the RGB image, and using the pixel center as the core to define a local neighborhood to filter effective depth values ​​and calculate a statistical average to obtain noise-resistant depth information, the technical challenge of depth cameras easily generating depth holes and zero-value noise at object edges and reflective surfaces is solved, significantly improving the stability and reliability of depth measurement. By performing inverse projection calculation of the pixel center coordinates and noise-resistant depth information based on the camera intrinsic parameter matrix, the three-dimensional spatial pose of the food container in the camera coordinate system is obtained, solving the problem of accurate mapping from two-dimensional image features to three-dimensional physical spatial pose, providing a directly usable spatial target point for the robotic arm. Because the facial feature point extraction algorithm reconstructs a three-dimensional topological mesh from the user's facial image data and accurately locates the key points of the lips to determine the three-dimensional dynamic spatial position of the lip region, it solves the problem that the lip position drift caused by the random displacement of the user's head during eating is difficult to capture in real time. It realizes millisecond-level response tracking of the user's eating intention and dynamic target, laying the perceptual foundation for subsequent safe and accurate feeding actions.

[0045] In one implementation, step S20 includes: obtaining the three-dimensional observation vector of the food container in the camera coordinate system at the current moment; reading the homogeneous transformation matrix from the camera coordinate system to the robot arm base coordinate system; multiplying the homogeneous transformation matrix by the homogeneous coordinate form of the three-dimensional observation vector, and obtaining the absolute execution vector of the food container in the robot arm base coordinate system through matrix multiplication; and using the absolute execution vector as the data input source for subsequent kinematics calculation and trajectory reconstruction.

[0046] After step S20, the method further includes: obtaining the three-dimensional observation vector of the food container in the camera coordinate system at the current moment and determining the current original coordinate value; reading the previous smoothed coordinate value saved after smoothing at the previous moment; weighting and fusing the current original coordinate value and the previous smoothed coordinate value according to the exponential moving average filtering algorithm and a preset weight factor to generate the current smoothed coordinate value, wherein the weight factor is used to balance response speed and data stability; and replacing the current original coordinate value with the current smoothed coordinate value as the input for subsequent coordinate mapping steps.

[0047] For example, an EMA filter is introduced to smooth the transformed robot coordinates: Among them, weighting factors Setting it to 0.3 effectively balances response speed and data stability. These are the latest coordinate values ​​obtained after smoothing using the EMA filter at the current moment. These are the original coordinate values ​​that the system has just acquired at the current moment. The target pixel coordinates, which are the smoothed target pixel coordinates saved from the previous time step and then smoothed by an EMA filter, are combined with depth information to transform them into a visual observation vector in the camera coordinate system. Subsequently, the system invokes a pre-set hand-eye calibration matrix, using the core mapping formula. By mapping the visual observation vector to the robotic arm coordinate system, the absolute execution vector of the target in physical space is obtained. The execution vector This will serve as the data input source for subsequent kinematics calculations and trajectory cloning. A comparison of the depth space coordinate filtering effects based on the EMA algorithm is shown in the figure below. Figure 3 As shown.

[0048] In this embodiment, a method of reading the pre-calibrated homogeneous transformation matrix and performing a left multiplication operation on the 3D observation vector is adopted to unify the observation data in the camera coordinate system to the robot arm base coordinate system. This solves the cumulative error problem caused by the inconsistency of coordinate references, which is common in multi-sensor systems, and achieves precise alignment between visual perception information and robot physical motion control under the same spatial logic. Since the calculated absolute execution vector is used as the data input source for subsequent kinematic calculations and trajectory reconstruction, the data continuity between upper-level planning and lower-level execution is ensured, resolving motion deviations that may be caused by information transmission gaps. Furthermore, by introducing an exponential moving average filtering algorithm, the current original coordinate value and the previous smoothed coordinate value are weighted and fused according to a preset weight factor to generate the current smoothed coordinate value. This effectively suppresses coordinate jumps caused by ambient light interference or object surface reflections from the depth camera, solving the problem of visual data jitter directly causing tremors at the robot arm end. By using the smoothed coordinate value instead of the original value as the input for subsequent coordinate mapping, the system's rapid response to real displacement is preserved while significantly improving data stability. This achieves extreme smoothness in the physical execution of the feeding action, optimizing the user's feeding experience and ensuring interactive safety.

[0049] In one implementation, step S30 includes: Step S301: Obtain pre-stored human teaching trajectory data. The human teaching trajectory data includes a discrete joint angle sequence, as well as the end reference pose and starting joint angle corresponding to the starting frame in the discrete joint angle sequence.

[0050] For example, pre-stored human teaching trajectory data refers to the action data of a person scooping food, recorded in advance through a "teach-and-playback" method. The action data records the sequence of changes in the angles of various joints of the robotic arm over time as the human performs the feeding action. The discrete joint angle sequence indicates the specific representation of the teaching trajectory data, consisting of a series of snapshots arranged in chronological order, each snapshot recording the angle values ​​of all joints of the robotic arm at a given moment. The starting frame refers to the first data point in the discrete joint angle sequence, representing the action posture of the human at the instant they begin scooping food. The end-effector reference pose refers to the spatial position and posture of the robotic arm's end effector at the starting frame during the recording of the teaching trajectory; it serves as a reference for calculating spatial deviations. The starting joint angle indicates the specific angle values ​​corresponding to each joint of the robotic arm at the starting frame of the teaching trajectory.

[0051] Step S302: Calculate the three-dimensional translational deviation vector between the target execution pose and the end effector reference pose.

[0052] It should be noted that the target execution pose refers to the position and orientation of the food container in the robot arm's base coordinate system in the current actual scene after visual perception and coordinate mapping. The three-dimensional translational deviation vector refers to the difference between the target execution pose and the end effector reference pose in three-dimensional space. It is represented by a vector with direction and magnitude, describing how much distance needs to be moved from the "bowl position during teaching" to the "current bowl position".

[0053] Step S303: The three-dimensional translation deviation vector is compensated to the end reference pose to obtain the compensated start end pose.

[0054] It should be noted that compensation can be understood as superimposing the calculated 3D translational deviation vector onto the original end-effector reference pose. A schematic diagram of the human scooping teaching trajectory and spatial translation compensation is shown below. Figure 4 As shown.

[0055] Step S304: Solve the compensated starting and ending poses using an inverse kinematics algorithm to obtain the true starting joint angles under the current physical environment.

[0056] It should be noted that step S304 includes: constructing the forward kinematics function of the robotic arm and using the desired end-effector pose matrix as the solution objective; establishing an inverse kinematics optimization objective function, which is used to minimize the weighted error between the calculation result of the forward kinematics function and the end-effector pose matrix, wherein the weighted error is adjusted through a mask weight matrix; iteratively solving the inverse kinematics optimization objective function based on the Levenberg-Marquardt algorithm, dynamically adjusting the damping factor during the iteration process to maintain the numerical stability of the Jacobian matrix; monitoring the condition number or solution convergence status of the Jacobian matrix in real time during the iterative solution process, and actively intercepting the current abnormal iteration branch when it is determined that the Jacobian matrix is ​​close to singular or the current pose solution is not convergent, switching to the pre-calibrated non-interference safe initial guess value, and restarting the iterative solution from the pre-calibrated non-interference safe initial guess value to obtain the true starting joint angle under the current physical environment.

[0057] The kinematic function refers to the mathematical function describing the correspondence between the joint angles of the robotic arm and the spatial pose of the end effector. Given a joint angle, this function can uniquely calculate the spatial position of the end effector. The desired end effector target pose matrix refers to the compensated initial end effector pose obtained in step S303, usually represented as a 4×4 homogeneous transformation matrix. The inverse kinematics algorithm, in contrast to forward kinematics, refers to the algorithm that, given the target pose of the end effector, calculates backward which joint angles are needed to achieve that pose. The inverse kinematics optimization objective function refers to the mathematical index that needs to be minimized during the inverse kinematics solution process, usually the error between the calculated end effector pose and the target pose, including distance and orientation differences. Weighted error refers to assigning different weight coefficients to errors in different dimensions, such as position error and attitude error, in the optimization objective function to reflect the focus of control. The mask weight matrix is ​​a matrix used to adjust the weighted error, which can shield certain degrees of freedom that do not require precise control, or amplify the error influence of key degrees of freedom. The Levenberg-Marquardt (LM) algorithm is a numerical optimization algorithm for nonlinear least squares problems, used for inverse kinematics solutions, and can adaptively switch between gradient descent and the Gauss-Newton method. The damping factor is a key parameter in the LM algorithm, used to adjust the iteration step size. Increasing the damping factor makes the algorithm more stable, while decreasing it leads to faster convergence. The Jacobian matrix describes the mapping relationship between the joint space velocity of the robotic arm and the Cartesian space velocity of the end effector, reflecting the motion sensitivity of the robotic arm in its current pose. Numerical stability refers to the algorithm's ability to resist rounding errors and singularities during computation, ensuring that the calculation results do not diverge. The condition number is an important property of a matrix, used to measure how close the matrix is ​​to singularity. The larger the condition number, the more ill-conditioned the matrix, and the less stable the inversion. Approaching singularity means that the robotic arm's configuration causes it to lose its ability to move in a certain direction, such as when the elbow is fully extended. In this case, the Jacobian matrix is ​​close to non-invertible, and the inverse solution is extremely unstable. Convergence state refers to whether the iterative algorithm tends towards a stable solution. If the error no longer decreases significantly or oscillates, it is considered non-convergent. Active interception refers to the program logic proactively stopping the current erroneous calculation process when an anomaly is detected, such as near-singularity. Anomaly iteration branch refers to an erroneous solution path caused by numerical instability or near-singularity. Pre-calibrated interference-free safety initial guess values ​​refer to the initial joint angle values ​​determined through offline calibration during the system design phase to ensure that the robotic arm does not collide with the environment within the workspace; these are used for backoff in case of algorithm failure.

[0058] For example, the system constructs a digital model of the robotic arm that is mapped to the physical entity in real time at the control layer. In the underlying basic kinematic modeling stage, the system employs the conventional modified DH parameter method (MDH), using the transformation matrix between adjacent links... The basic forward kinematics expression of the lightweight six-DOF robotic arm was constructed, which is represented as follows:

[0059] To address the kinematic singularity problem within the confined space of a feeding robot, an inverse solution architecture with redundancy and fault tolerance mechanisms was designed on top of the basic model. Specific robotic arm physical parameters, such as link lengths and offsets, as shown in Table 1, were imported to instantiate the basic model.

[0060] Table 1 Based on this, the system transforms the absolute execution vector obtained from the previous visual mapping step into the target pose matrix in Cartesian space. The Levenberg-Marquardt (LM) damped least squares method is used for inverse iterative solution, with the objective function set as follows: in, For positive kinematic functions, For mask weights. To further overcome the technical bottleneck of traditional inversion algorithms being prone to crashing at feeding extrema, the system embeds a safe backoff adaptive mechanism at the bottom layer of the LM algorithm. That is, when the system determines that the Jacobian matrix is ​​close to singular or the current pose solution is not converging, the algorithm will actively intercept the anomaly and automatically switch to the pre-calibrated non-interference safe initial guess value. The iteration is restarted. This collaborative mechanism ensures the absolute numerical stability of the feeding robot arm in complex dynamic environments.

[0061] Step S305: Calculate the difference between the actual starting joint angle and the starting joint angle to obtain the global constant joint offset constant.

[0062] It should be noted that the global constant joint offset constant refers to the difference between the actual starting joint angle and the taught starting joint angle.

[0063] Step S306: The global constant joint offset constant is superimposed onto the overall discrete joint angle sequence of the human teaching trajectory data to obtain the offset reconstruction trajectory sequence in joint space.

[0064] For example, a globally constant joint offset is added as a fixed offset to the joint angles at each time point in the taught trajectory. The offset reconstructed trajectory sequence refers to the new joint angle sequence obtained after the overall superposition operation. It retains the characteristics of the taught trajectory in shape, but is spatially translated to a new target region.

[0065] Step S307: Based on the piecewise cubic Hermite interpolation polynomial, interpolation fitting is performed on the joint trajectories between adjacent keyframes in the offset reconstructed trajectory sequence to generate continuous biomimetic motion trajectories.

[0066] It should be noted that the piecewise cubic Hermite interpolation polynomial (PCHIP) connects discrete data points by constructing piecewise cubic polynomials, ensuring the continuity of the first derivative at the connection points while avoiding polynomial oscillations. Adjacent keyframes refer to two data points that are temporally adjacent in a discrete joint angle sequence. Interpolation fitting refers to the process of inserting more points between known discrete points to form a continuous curve. A continuous biomimetic motion trajectory refers to the robotic arm's motion path after interpolation, where joint angles change continuously over time, velocity is smooth, and there are no abrupt changes, mimicking the gentle characteristics of human movement.

[0067] For example, step S30 aims to achieve offline reconstruction of muscle memory through inverse kinematics of a single initiation pose and a globally constant bias. Specifically, the system obtains the actual coordinates of the target in the current environment. That is, the absolute execution vector of the target, which is perceived visually and updated in real time after coordinate mapping, and compared with the reference coordinates of the human demonstration actions pre-recorded by the system. By comparison, the absolute displacement deviation vector in the three-dimensional Cartesian space is extracted. , is represented as:

[0068] The system performs spatial translation and inverse calculation only on the first frame of the teaching sequence, i.e., the starting pose. The joint angles of the first teaching frame are set as follows: Its corresponding terminal positive kinematic space coordinates are The system will account for spatial deviations. The compensation is brought back to this starting point, and the actual starting joint angle required under the current physical environment is solved using inverse kinematics (IK). This allows for the extraction of globally constant joint bias constants. The corresponding formula is:

[0069] in, The joint angle at the starting point of the teaching trajectory; The absolute three-dimensional pose of the starting point of the calculated original teaching trajectory in space; The translational deviation of the target in the actual physical space.

[0070] The system does not need to perform complex IK inverse kinematics frame by frame during runtime; instead, it directly extracts the constant bias constant from the single extraction process. The entire sequence is superimposed onto the pre-recorded discrete teaching sequence. Meanwhile, using PCHIP (Piecewise Cubic Hermite Interpolating Polynomial) piecewise cubic Hermite interpolation polynomials, a new execution trajectory is generated that combines the smoothness of human movements with the accuracy of the current position:

[0071] By transforming Cartesian translation into a global constant bias in joint space through a single spatial mapping, the computational cost and attitude change risk associated with traditional frame-by-frame inverse kinematics are fundamentally avoided.

[0072] In this embodiment, pre-stored human teaching trajectory data containing discrete joint angle sequences and the initial frame reference pose is acquired, solving the problem of traditional online planning lacking humanized action templates and inheriting the natural acceleration characteristics of humans scooping food. By calculating the three-dimensional translational deviation vector between the target execution pose and the end-effector reference pose and compensating for this deviation in the initial and end-effector poses, the problem of the teaching trajectory not being directly reusable due to changes in the placement of tableware is solved, achieving adaptive matching of the trajectory to the dynamic environment. By constructing a forward kinematics function and establishing an inverse kinematics optimization function with the goal of minimizing weighted errors, and combining the Levenberg-Marquardt algorithm to dynamically adjust the damping factor for iterative solution, the risk of numerical instability or even solution collapse when the robotic arm approaches singularities in a confined space is solved, significantly improving the robustness of the inverse kinematics solution. By monitoring the Jacobian matrix condition number during iteration and switching to a pre-calibrated, interference-free, safe initial guess value for resolving in case of anomalies, the safety hazard of the robotic arm going out of control due to algorithm failure under extreme poses is solved, ensuring that the system can return to a safe starting point under any circumstances. By calculating the difference between the actual starting joint angle and the taught starting angle to obtain a globally constant joint offset constant, and then superimposing this constant onto the entire discrete sequence, the problems of high computational cost and easy introduction of cumulative errors in traditional frame-by-frame inverse kinematics are solved, achieving efficient and consistent trajectory reconstruction. Furthermore, by fitting the offset reconstruction sequence with a piecewise cubic Hermite interpolation polynomial, the problem of discontinuous motion between discrete points and the resulting abruptness is resolved. Ultimately, a continuous biomimetic motion trajectory that combines human muscle memory characteristics with adaptability to the current environment is generated, greatly improving the smoothness of the feeding action.

[0073] In one implementation, it should be noted that during the feeding phase (FEED), the system enters a high-frequency interaction mode. Based on this, step S40 includes:

[0074] Step S401: Drive the robotic arm to move according to the bionic motion trajectory, obtain the current end position of the robotic arm in the robotic arm base coordinate system in real time, and determine the current mouth center position based on the lip tracking pose.

[0075] For example, grid data extracted in real time using the MediaPipe algorithm, and synchronously mapped to the robotic arm coordinate system via a hand-eye matrix, allows for real-time monitoring of the alignment between the robotic arm's end effector and the center of the human mouth. .

[0076] Step S402: Calculate the Euclidean distance between the current end position and the current mouth center position.

[0077] For example, the Euclidean distance between the current end position and the current mouth center position is expressed as: The current end position is represented as The current center position of the mouth is indicated as .

[0078] Step S403: Determine whether the Euclidean distance is greater than the preset safe distance threshold.

[0079] For example, the preset safe distance threshold can be .

[0080] Step S404: If the Euclidean distance is greater than the preset safe distance threshold, the smooth drive function is called to control the robotic arm to smoothly approach the current mouth center position.

[0081] For example, if The robot arm is then smoothly moved towards the mouth using the smoothing drive function, move_robot_smooth. Then, it performs opening intention recognition.

[0082] Step S405: When the Euclidean distance is less than or equal to the safe distance threshold, calculate the three-dimensional Euclidean distance between the center key point of the user's upper lip and the center key point of the lower lip in real time to determine the current lip spacing.

[0083] For example, the system accurately locates the key point at the center of the upper lip based on the three-dimensional facial topology mesh extracted by the aforementioned Media Face Mesh algorithm. Key point of the lower lip center Subsequently, the relative Euclidean distance between the two in three-dimensional space is calculated as the real-time lip spacing. :

[0084] The key point in the center of the upper lip is represented as follows: The key point in the center of the lower lip is represented as follows: .

[0085] Step S406: Determine whether the current lip spacing is greater than the preset mouth opening determination threshold for multiple consecutive frames.

[0086] It should be noted that the mouth opening determination threshold can be expressed as: The value was set at 25mm based on the eating characteristics of healthy adults.

[0087] Step S407: When the current lip distance is greater than the mouth opening determination threshold for multiple consecutive frames, control the robotic arm to perform a feeding and pushing action.

[0088] For example, when 10 consecutive frames meet At this point, the system determines that the subject has a clear intention to eat, thus triggering a feeding pulse command that moves the coordinate system of the robotic arm's end effector forward. The feeding intention recognition and distance determination curves are shown below. Figure 5 As shown, the biomimetic feeding and adaptive control block diagram of the feeding robot is as follows: Figure 6 As shown, the corresponding state machine operation flow of the feeding robot for "finding objects - scooping up - feeding" is as follows: Figure 7 As shown.

[0089] This embodiment employs a technique that drives the robotic arm based on a biomimetic motion trajectory and acquires the end-effector position and mouth center position in real time. This solves the problem of traditional open-loop control's inability to sense the user's dynamic displacement, achieving continuous locking onto the moving target. By calculating the Euclidean distance between the end-effector and the mouth and determining whether it exceeds a safe distance threshold, the lack of feeding speed control is resolved, ensuring a slow and stable approach before entering a dangerous area. By calling a smooth drive function to control the robotic arm to smoothly approach the mouth, the problem of vibration or sudden stops that easily occur when the robotic arm operates at close range is solved, significantly improving the physical comfort of the interaction. By calculating the distance between the key points of the upper and lower lip centers in real time after the distance is reached to determine the lip distance, the deficiency of being unable to confirm the user's eating intention based solely on distance judgment is solved, achieving accurate perception of physiological state. By judging whether the lip distance exceeds the mouth-opening judgment threshold for multiple consecutive frames, the problem of false triggering caused by blinking or brief facial expressions is solved, effectively filtering out instantaneous noise interference. Ultimately, once it is confirmed that the user has opened their mouth continuously, the robotic arm is controlled to perform the feeding and pushing action, realizing closed-loop safe feeding based on human intention recognition in the loop, which enhances the interactivity and reliability of the system.

[0090] In one implementation, after step S40, the method further includes: responding to an external emergency stop command or a stop command from the software monitoring interface, immediately setting the flag position of the visual tracking thread to a standby state, and cutting off the closed-loop following control logic for the user's lip area from the data source; acquiring the joint angles at the moment the robotic arm triggers the stop command, as the emergency return starting pose; setting the safe retraction zero-point folding posture of the robotic arm as the emergency return target pose; using the emergency return starting pose and the emergency return target pose as boundary conditions, constraining the joint angular velocity and angular acceleration at the start and end times to be zero, and generating a smooth retraction trajectory with fifth-order polynomial interpolation within a preset retraction period; and controlling the robotic arm to move to the safe retraction zero-point folding posture according to the smooth retraction trajectory.

[0091] For example, when a stop command is triggered via a hardware emergency stop button or a software monitoring interface, the system immediately sets the flag of the visual tracking thread to standby mode and cuts off the closed-loop following control logic from the data source. To avoid severe mechanical shock to the underlying driver due to the momentary loss of control target, the system uses the JTRAJ (Joint-space Trajectory) algorithm to plan a retreat trajectory. Specifically, the angles of each joint at the instant the robotic arm triggers the stop command can be used as the current starting point. And set the zero-point folding posture for safe retraction as The system introduces strict kinematic boundary conditions, namely the joint angular velocities at the start and end times. With angular acceleration All values ​​are forced to zero within the preset rollback period. Within, a fifth-order polynomial interpolation path is generated, and the corresponding formula is:

[0092] in, Represented as the current motion time variable, This represents the preset return period. The joint angle smoothing trajectory curve based on JTRAJ interpolation is shown below. Figure 8 As shown, the flowchart for inverse motion solution and safe backoff based on the LM algorithm is as follows: Figure 9 As shown.

[0093] In this embodiment, a technique is employed to immediately set the visual tracking thread flag to standby state and disconnect the closed-loop following control logic from the data source upon response to an emergency stop or software stop command. This solves the problem of the system still attempting to follow the target in emergency situations, leading to increased danger, and achieves millisecond-level rapid disconnection of the control logic at the source. By acquiring the joint angles at the moment of triggering the stop as the emergency retraction starting pose and setting the safe retraction zero-point folding posture as the emergency retraction target pose, the problem of confusion due to the lack of a clear start and end point during emergency retraction is solved, ensuring the determinism of the retraction path. By using the starting pose and target pose as boundary conditions and strictly constraining the joint angular velocity and angular acceleration at the start and end moments to be zero, the problem of huge mechanical shock and vibration generated during emergency stops or sudden turns is solved, protecting the mechanical structure and transmission components. By using fifth-order polynomial interpolation to generate a smooth retraction trajectory within a preset retraction cycle, the problem of low-order interpolation being unable to simultaneously guarantee the continuity of position and velocity is solved, making the retraction action of the robotic arm in a confined space extremely smooth. Ultimately, the robotic arm was controlled to move strictly along the smooth trajectory to the zero-point folding posture, ensuring that each joint of the robotic arm followed an "S"-shaped smooth speed manifold when exiting the work space, thus comprehensively guaranteeing physical safety during close human-machine interaction.

[0094] Based on the same inventive concept, such as Figure 10 As shown, this application also provides a collaborative control device for a feeding robot, which includes: The data acquisition module 10 is used to acquire target image data, perform target detection and facial feature point extraction on the target image data, and obtain the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region. The coordinate transformation module 20 is used to transform the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region to the robot arm base coordinate system based on the pre-calibrated hand-eye transformation matrix, and generate the corresponding target execution pose and lip tracking pose. The trajectory reconstruction module 30 is used to acquire pre-stored human teaching trajectory data, and based on the spatial deviation between the target execution pose and the reference pose in the human teaching trajectory data, to perform overall offset reconstruction of the human teaching trajectory data in joint space, and to generate continuous bionic motion trajectory by combining interpolation algorithm. The feeding propulsion module 40 is used to drive the robotic arm to move according to the bionic motion trajectory. During the movement, it monitors the distance between the end of the robotic arm and the lip tracking pose and the opening and closing state of the user's lips in real time. When the distance is less than or equal to a safety threshold and the opening and closing state of the lips meets the preset opening conditions, it controls the robotic arm to perform the feeding propulsion action.

[0095] It should be noted that the feeding robot collaborative control device and the feeding robot collaborative control method provided in this application are based on the same application concept. Therefore, the specific implementation of this embodiment can refer to the implementation of the aforementioned feeding robot collaborative control method, and the repeated parts will not be described again.

[0096] In some embodiments, an electronic device provided in this application includes: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the computer program is configured to implement the above-described collaborative control method for a feeding robot.

[0097] Specifically, the processor may include, for example, a general-purpose microprocessor, an instruction set processor and / or an associated chipset and / or a special-purpose microprocessor (e.g., an application-specific integrated circuit (ASIC)), etc. The processor may also include onboard memory for caching purposes. The processor may be a single processing unit or multiple processing units for performing different actions of the method flow according to embodiments of this application.

[0098] Memory can be any medium capable of containing, storing, transmitting, propagating, or transmitting instructions. For example, memory can include, but is not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices, instruments, or propagation media. Specific examples of memory include: magnetic storage devices such as magnetic tape or hard disk drives (HDDs); optical storage devices such as optical discs (CD-ROMs); and also random access memory (RAM) or flash memory; and / or wired / wireless communication links.

[0099] This application also provides a non-transitory computer storage medium storing a computer program that, when executed by a processor, implements the aforementioned collaborative control method for a feeding robot. This storage medium may be included in the device / apparatus / system described in the above embodiments; or it may exist independently and not be assembled into that device / apparatus / system. The aforementioned non-transitory computer storage medium carries one or more programs, which, when executed, implement the method as described in the embodiments or implementations of this application.

[0100] According to embodiments of this application, a non-transitory computer storage medium can be a computer-readable signal medium or a computer-readable storage medium, or any combination thereof. A computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of a computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In this application, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in connection with an instruction execution system, apparatus, or device. A computer-readable signal medium can also be any storage medium other than a computer-readable storage medium that can transmit, propagate, or transfer a program for use by or in connection with an instruction execution system, apparatus, or device. The program code contained on the storage medium can be transmitted using any suitable medium, including but not limited to: wireless, wired, optical fiber, radio frequency signals, etc., or any suitable combination thereof.

[0101] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, data stored, data displayed, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of the relevant data must comply with relevant regulations.

[0102] Those skilled in the art will understand that the features described in the various embodiments of this application can be combined and / or combined in various ways, even if such combinations or combinations are not explicitly described in this application. In particular, the features described in the various embodiments of this application can be combined and / or combined in various ways without departing from the spirit and teachings of this application. All such combinations and / or combinations fall within the scope of this application. Therefore, the scope of this application should not be limited to the above embodiments. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A collaborative control method for a feeding robot, characterized in that, The feeding robot collaborative control method is applied to a feeding robot, and the feeding robot collaborative control method includes: Acquire target image data, perform target detection and facial feature point extraction on the target image data, and obtain the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region; Based on the pre-calibrated hand-eye transformation matrix, the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region are transformed into the robot arm base coordinate system to generate the corresponding target execution pose and lip tracking pose. Acquire pre-stored human teaching trajectory data, and based on the spatial deviation between the target execution pose and the reference pose in the human teaching trajectory data, perform overall offset reconstruction of the human teaching trajectory data in joint space, and generate continuous bionic motion trajectories by combining interpolation algorithms. The robotic arm is driven to move according to the biomimetic motion trajectory. During the movement, the distance between the end of the robotic arm and the lip tracking pose, as well as the opening and closing state of the user's lips, are monitored in real time. When the distance is less than or equal to a safety threshold and the opening and closing state of the lips meets the preset opening conditions, the robotic arm is controlled to perform a feeding and pushing action.

2. The method as described in claim 1, characterized in that, The target image data includes food container image data and user facial image data. The steps of acquiring the target image data, performing target detection and facial feature point extraction on the target image data, and obtaining the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region include: The food container is detected from the food container image data based on the object detection algorithm, the two-dimensional detection bounding box of the food container is extracted, and the pixel center point coordinates of the two-dimensional detection bounding box are calculated. Acquire depth image data aligned with the food container image data; define a local neighborhood in the depth image data with the center point coordinates of the pixel as the center; select the effective depth value within the local neighborhood; calculate the statistical average value of the effective depth value; and obtain the noise-resistant depth information of the food container. Based on the camera intrinsic parameter matrix, the coordinates of the pixel center point and the noise-resistant depth information are inversely projected and calculated to obtain the three-dimensional spatial pose of the food container in the camera coordinate system. Based on the facial feature point extraction algorithm, a three-dimensional topological mesh of the user's face is extracted from the user's facial image data. The key points of the lips are located from the three-dimensional topological mesh, and the three-dimensional dynamic spatial position of the user's lip region is determined based on the key points of the lips.

3. The method as described in claim 2, characterized in that, After the steps of extracting a three-dimensional topological mesh of the user's face from the user's facial image data using the facial feature point extraction algorithm, locating key lip points from the three-dimensional topological mesh, and determining the three-dimensional dynamic spatial position of the user's lip region based on the key lip points, the method further includes: Obtain the current 3D observation vector of the food container in the camera coordinate system and determine the current original coordinate value; Read the smoothed coordinate value saved from the previous time step; According to the exponential moving average filtering algorithm, the current original coordinate value and the previous smoothed coordinate value are weighted and fused according to a preset weight factor to generate the current smoothed coordinate value. The weight factor is used to balance response speed and data stability. The current smoothed coordinate value is used to replace the current original coordinate value as the input for subsequent coordinate mapping steps.

4. The method as described in claim 1, characterized in that, The steps of acquiring pre-stored human teaching trajectory data, reconstructing the overall offset of the human teaching trajectory data in joint space based on the spatial deviation between the target execution pose and the reference pose in the human teaching trajectory data, and generating a continuous bionic motion trajectory using an interpolation algorithm include: Acquire pre-stored human teaching trajectory data, which includes a discrete joint angle sequence, and the end reference pose and starting joint angle corresponding to the starting frame in the discrete joint angle sequence; Calculate the three-dimensional translational deviation vector between the target execution pose and the end effector reference pose; The three-dimensional translational deviation vector is compensated to the end reference pose to obtain the compensated initial end pose. The inverse kinematics algorithm is used to solve the compensated initial and final poses to obtain the true initial joint angles under the current physical environment. Calculate the difference between the actual starting joint angle and the starting joint angle to obtain the global constant joint offset constant; The global constant joint offset constant is superimposed onto the overall discrete joint angle sequence of the human teaching trajectory data to obtain the offset reconstruction trajectory sequence in joint space. Based on the piecewise cubic Hermitian interpolation polynomial, the joint trajectories between adjacent keyframes in the offset reconstructed trajectory sequence are interpolated and fitted to generate continuous biomimetic motion trajectories.

5. The method as described in claim 4, characterized in that, The step of solving the compensated initial and final poses using an inverse kinematics algorithm to obtain the true initial joint angles under the current physical environment includes: Construct the positive kinematics function of the robotic arm and use the desired end-effector pose matrix as the solution objective; An inverse kinematics optimization objective function is established, which is used to minimize the weighted error between the calculation result of the forward kinematics function and the end target pose matrix, wherein the weighted error is adjusted by a mask weight matrix; The inverse kinematics optimization objective function is solved iteratively based on the Levenberg-Marquardt algorithm, and the damping factor is dynamically adjusted during the iteration process to maintain the numerical stability of the Jacobian matrix. During the iterative solution process, the condition number of the Jacobian matrix or the convergence status of the solution are monitored in real time. When it is determined that the Jacobian matrix is ​​close to singular or the current pose solution is not converged, the current abnormal iteration branch is actively intercepted and switched to the pre-calibrated non-interference safe initial guess value. The iterative solution is restarted from the pre-calibrated non-interference safe initial guess value to obtain the true starting joint angle under the current physical environment.

6. The method as described in claim 1, characterized in that, The step of driving the robotic arm to move according to the biomimetic motion trajectory, and monitoring in real time the distance between the robotic arm end effector and the lip tracking pose, as well as the user's lip opening and closing state, and controlling the robotic arm to perform a feeding propulsion action when the distance is less than or equal to a safety threshold and the lip opening and closing state meets a preset mouth opening condition, includes: The robotic arm is driven to move according to the biomimetic motion trajectory, and the current end position of the robotic arm in the robotic arm base coordinate system is obtained in real time. The current mouth center position is determined based on the lip tracking pose. Calculate the Euclidean distance between the current end position and the current mouth center position; Determine whether the Euclidean distance is greater than a preset safe distance threshold; If the Euclidean distance is greater than the preset safe distance threshold, the smooth drive function is invoked to control the robotic arm to smoothly approach the current mouth center position; When the Euclidean distance is less than or equal to the safe distance threshold, the three-dimensional Euclidean distance between the center key point of the user's upper lip and the center key point of the lower lip is calculated in real time to determine the current lip spacing. Determine whether the current lip spacing is greater than a preset mouth opening determination threshold for multiple consecutive frames; When the current lip spacing is greater than the mouth opening determination threshold for multiple consecutive frames, the robotic arm is controlled to perform a feeding and pushing action.

7. The method as described in claim 1, characterized in that, The step of driving the robotic arm to move according to the biomimetic motion trajectory, and monitoring in real time the distance between the robotic arm end effector and the lip tracking pose, as well as the user's lip opening and closing state, and controlling the robotic arm to perform a feeding propulsion action when the distance is less than or equal to a safety threshold and the lip opening and closing state meets a preset mouth opening condition, further includes: In response to an external emergency stop command or a stop command from the software monitoring interface, the visual tracking thread's flag is immediately set to standby mode, and the closed-loop tracking control logic for the user's lip area is cut off from the data source. Obtain the angles of each joint at the moment the robotic arm triggers the stop command, as the emergency return starting pose; Set the robotic arm's safe retraction zero-point folding posture as the emergency retraction target posture; Using the emergency return start pose and the emergency return target pose as boundary conditions, the joint angular velocity and angular acceleration at the start and end times are constrained to be zero. Within the preset return cycle, a smooth return trajectory interpolated by a fifth-order polynomial is generated. Control the robotic arm to move along the smooth return trajectory until it safely retracts to the zero-point folded posture.

8. A collaborative control device for a feeding robot, characterized in that, The collaborative control device for the feeding robot includes: The data acquisition module is used to acquire target image data, perform target detection and facial feature point extraction on the target image data, and obtain the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region. The coordinate transformation module is used to transform the three-dimensional spatial pose of the food container and the three-dimensional dynamic spatial position of the user's lip region to the robot arm base coordinate system based on the pre-calibrated hand-eye transformation matrix, and generate the corresponding target execution pose and lip tracking pose. The trajectory reconstruction module is used to acquire pre-stored human teaching trajectory data, and based on the spatial deviation between the target execution pose and the reference pose in the human teaching trajectory data, to perform overall offset reconstruction of the human teaching trajectory data in joint space, and to generate continuous bionic motion trajectories by combining interpolation algorithms. The feeding propulsion module is used to drive the robotic arm to move according to the bionic motion trajectory. During the movement, it monitors the distance between the end of the robotic arm and the lip tracking pose and the opening and closing state of the user's lips in real time. When the distance is less than or equal to a safety threshold and the lip opening and closing state meets the preset opening conditions, it controls the robotic arm to perform the feeding propulsion action.

9. An electronic device, characterized in that, The electronic device includes: a memory, a processor, and a computer program stored in the memory and executable on the processor, the computer program being configured to implement the steps of the feeding robot collaborative control method as described in any one of claims 1 to 7.

10. A non-transitory computer storage medium, characterized in that, The non-transitory computer storage medium stores a computer program, which, when executed by a processor, implements the steps of the feeding robot collaborative control method as described in any one of claims 1 to 7.

Images