Intelligent feeding robot based on visual and imu dual positioning

Abstract

This invention discloses an intelligent feeding robot based on dual positioning using vision and IMU (In-Vision Unit), comprising a data processor 1, a binocular camera 2, a feeding robotic arm 3, an end effector 4, and a wearable food tray 5. The wearable food tray 5 is equipped with an IMU sensor to collect head movement data. The data processor 1 calculates the spatial position of the mouth based on the head movement data and the lip shape image data collected by the binocular camera 2. The feeding robotic arm 3 automatically plans its movement path and feeding action based on this position, delivering the end effector 4 to the mouth. This invention combines visual recognition with IMU, using visual positioning to solve the problem that IMU sensors cannot obtain absolute position, eliminating the integral accumulation error of IMU sensors. At the same time, it utilizes the high precision and real-time advantages of IMU sensors to improve the feeding robot's ability to track the mouth position, thus achieving tracking capabilities comparable to pure visual positioning methods with fewer images and less hardware computing power.

Description

Technical Field

[0001] This invention relates to the field of medical devices, and in particular to an intelligent feeding robot based on dual positioning using vision and IMU. Background Technology

[0002] Stroke, multiple sclerosis, Parkinson's disease, and other illnesses render many elderly people unable to feed themselves, severely impacting their quality of life and requiring assistance with eating. To alleviate the burden of elderly care and improve the quality of life for these seniors, it is necessary to develop an assistive feeding robot to help them achieve independent eating. However, existing feeding robots generally rely on pure visual recognition for positioning. The machine learning models used in visual recognition require significant computing power, resulting in high-cost hardware platforms and difficulties in achieving high-speed real-time responses, leading to a poor user experience. Furthermore, existing feeding robots lack obstacle perception capabilities, potentially causing serious personal injury to users in the event of accidental collisions or other emergencies, posing certain safety hazards. Therefore, the accompaniment of caregivers is still required during feeding, limiting their application scenarios. Summary of the Invention

[0003] To address the aforementioned problems in existing technologies, this application provides an intelligent feeding robot based on dual positioning using vision and IMU. This not only reduces the computational power required for image processing and lowers the hardware cost of the feeding robot, but also effectively improves safety during the feeding process by utilizing obstacle avoidance path planning algorithms and admittance control algorithms, and has a better ability to cope with sudden collision events.

[0004] The technical solution of the present invention is as follows:

[0005] An intelligent feeding robot based on vision and IMU dual positioning includes a data processor 1, a binocular camera 2, a feeding robotic arm 3, and an end effector 4. The binocular camera 2 is fixedly connected to the data processor 1, acquiring lip-shape image data and sending it to the data processor 1. The data processor 1 locates and analyzes the lip shape based on the lip-shape image data and sends action commands to the feeding robotic arm 3. The bottom of the feeding robotic arm 3 is fixedly connected to the data processor 1, and the top is connected to the end effector 4. The feeding robotic arm 3 consists of at least 2 joints and is covered with a soft material. The feeding robotic arm 3 receives action commands from the data processor 1, rotates each joint according to the action commands, moves the end effector 4 to a designated position, and sends the torque, rotation angle, angular velocity, and angular acceleration of each joint to the data processor 1.

[0006] Includes a wearable food tray 5; the wearable food tray 5 is worn around the neck to catch food spilled during feeding;

[0007] The wearable meal tray 5 includes an IMU sensor inside; the IMU sensor collects head motion data and sends it to the data processor 1 via Bluetooth; the motion data includes acceleration and rotation angle, i.e., head acceleration and head rotation angle.

[0008] Data processor 1 calculates the spatial position of the mouth based on lip shape image data and motion data, as follows:

[0009] S1-1. Calculate the three-dimensional spatial coordinates of the mouth based on the lip shape image data of the binocular camera 2, i.e., the initial spatial position;

[0010] S1-2. Integrate the head acceleration to obtain the head displacement;

[0011] S1-3. Correct the initial spatial position based on the head displacement and head rotation angle to obtain the spatial position of the mouth;

[0012] The feeding steps are as follows:

[0013] S2-1: Wait for the voice command "Start feeding" or the manual remote control command; if the command is received, execute S2-2; otherwise, continue to wait.

[0014] S2-2, Binocular camera 2 acquires lip-shape image data and sends it to data processor 1;

[0015] S2-3: Data processor 1 identifies obstacle information based on lip shape image data and determines whether the mouth is open. If the mouth is open, S2-4 is executed; otherwise, S2-2 is executed.

[0016] S2-4, Data processor 1 calculates the spatial position of the exit section based on the lip shape image data;

[0017] S2-5, the feeding robotic arm 3 automatically plans the movement path and feeding action based on the spatial position of S2-4, the obstacle information of S2-3, and the pre-set restricted area;

[0018] S2-6, The feeding robotic arm 3 performs the feeding action along the motion path;

[0019] S2-7. If the spatial position of the mouth or the state of the mouth opening changes, execute S2-2; otherwise, execute S2-8.

[0020] S2-8. If all feeding actions have been completed, end the feeding process; otherwise, proceed to S2-6.

[0021] Furthermore, S2-6 includes the following steps:

[0022] S2-6-1, The feeding robotic arm 3 acquires the motion parameters of each joint, namely rotation angle, angular velocity, and angular acceleration, and calculates the actual torque value of each joint based on the above motion parameters;

[0023] S2-6-2. Calculate the expected torque value of each joint based on the dynamic model of the feeding robotic arm 3; subtract the expected torque value of each joint from the actual torque value to obtain the torque deviation value; calculate the deviation index δ based on the torque deviation value of each joint.

[0024]

[0025] Where the subscript n is the number of each joint, τ is the torque deviation value generated at each joint position under the action of external force, and w is the weight set for different joint positions;

[0026] S2-6-3. Set an error reference value δ err and a maximum error δ a According to δ and δ err δ a The difference is used to adjust the movement of the feeding robotic arm 3, as follows:

[0027] If δ≤δ err If the feeding robotic arm 3 is considered to be operating normally, the corrective action is for the feeding robotic arm 3 to continue moving along the predetermined path.

[0028] If δ err <δ≤δ a If the force of contact with the external obstacle is small, the feeding robotic arm 3 is considered to be in an abnormal state. The solution is to keep the feeding robotic arm 3 in its current position until the external obstacle is removed.

[0029] If δ a If the torque value is less than δ, the feeding robotic arm 3 is considered to be in an abnormal state and has a relatively large contact force with external obstacles. The solution is to move the feeding robotic arm 3 to a safe position so that the actual torque value of each joint remains within a safe range until the external obstacles are removed.

[0030] Furthermore, the method for calculating the safe location is as follows:

[0031] The admittance control law for the feeding robotic arm 3 is:

[0032]

[0033] Where M is the mass parameter corresponding to the acceleration term, B is the damping parameter corresponding to the velocity term, K is the elastic parameter corresponding to the displacement term, and F... env It is the interference force generated by the robot colliding with the external environment, x e It is a safe location;

[0034] The safe position x is obtained from the above formula. e acceleration:

[0035]

[0036] right Integrating, we obtain the safe position x. e :

[0037]

[0038] The beneficial technical effects of this invention are as follows:

[0039] (1) A positioning method combining vision and IMU is proposed. This method relies on the IMU sensor to track the movement of the user's head and solves the problem that the IMU sensor cannot obtain the absolute position by introducing visual positioning. At the same time, it eliminates the cumulative error of the IMU sensor due to data integration over time. Compared with the existing pure vision positioning method, this method reduces the number of images that need to be processed in the same amount of time, reduces the computing power requirements of the data processor, thereby reducing the hardware cost of the feeding robot's computing platform. It also improves the feeding robot's ability to track the mouth position during feeding by utilizing the high precision and real-time advantages of the IMU sensor.

[0040] (2) By pre-modeling obstacles in the scene and combining them with a built-in path planning algorithm, the robotic arm of the feeding robot can actively avoid various obstacles in the feeding environment, ensuring the smooth operation of the robot. By wrapping the robotic arm with soft material and incorporating an admittance control algorithm, a combination of active and passive collision avoidance is achieved. In the face of sudden accidental collisions, the safety of the user can be guaranteed to the greatest extent, while reducing the damage to the feeding robot from collisions.

[0041] (3) By performing precise dynamic modeling of the robotic arm, it can identify abnormal values ​​in parameters such as joint torque and rotation angle during the movement process, and promptly detect situations where it comes into contact with external obstacles. At this time, the robotic arm will temporarily stop trying to move to the next target point, and instead control the torque of each joint within a safe torque range, so that the robotic arm can naturally be pushed away from the external obstacle, thereby keeping the contact force between the robotic arm and the outside world at a small value and avoiding potential human-machine injury when the robot's movement is obstructed. After the external obstacle is removed, the robotic arm will attempt to return to the original path to continue the feeding work. Attached Figure Description

[0042] Figure 1 This is a schematic diagram of the hardware structure;

[0043] Figure 2This is a schematic diagram illustrating the combination of image recognition and IMU measurement;

[0044] Figure 3 This is a feeding flowchart;

[0045] Figure 4 This is a schematic diagram of the abnormal obstacle recognition and avoidance function. Detailed Implementation

[0046] The present invention will now be described in detail with reference to the accompanying drawings and embodiments. Obviously, the described embodiments are merely some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0047] like Figure 1 As shown, this embodiment is a novel intelligent obstacle-avoidance assisted feeding robot, including a data processor 1, a binocular camera 2, a feeding robotic arm 3, an end effector 4, and a wearable food tray 5. The data processor 1 performs data calculations such as lip-sync image processing and obstacle avoidance path planning, and sends motion commands to the feeding robotic arm 3. The binocular camera 2 captures images of the user's lip movements and sends the image data to the data processor 1 for lip-sync judgment and positioning. The feeding robotic arm 3 consists of three joints, externally covered with a soft material, and can receive and execute motion commands sent by the data processor 1, and can feed back motion information such as torque, rotation angle, angular velocity, and angular acceleration of each joint to the data processor 1. The end effector 4 is used to perform food dispensing actions and adopts a modular design, allowing for the replacement of spoon-shaped feeders, tubular feeders, or other devices with different structures and functions according to different needs of the actual feeding scenario. The wearable food carrier 5 can catch food spilled during feeding. It contains an IMU sensor that can detect the user's head movement and rotation in real time, and send the collected data to the data processor 1 via Bluetooth for real-time tracking and positioning of the mouth.

[0048] The functions of the embodiment are mainly realized through four modules: feeding, mouth position positioning and tracking, collision avoidance, and abnormal obstacle recognition and avoidance.

[0049] I. Feeding

[0050] The user can initiate feeding via voice or remote control. The system first uses a binocular camera 2 to capture an image including the user's mouth. A machine learning-based image processing model determines the user's mouth shape, and feeding only proceeds after the user's mouth is confirmed to be open. While the user's mouth is open, the system calculates the spatial position of the user's mouth using the binocular camera 2 image, inputting this as the path endpoint into the path planning algorithm. Simultaneously, obstacle areas identified by the binocular camera 2 and user-predefined no-entry zones are also input as constraints into the path planning algorithm, enabling the various parts of the feeding robotic arm 3 to avoid these areas during movement. The path planning algorithm generates a path from the current position of the feeding robotic arm 3 to the user's mouth, along with the rotation curves of each joint throughout the movement. This path is discretized to obtain a series of closely spaced path points, which are then sent one by one to the feeding robotic arm 3 to initiate the feeding action. During feeding, the embodiment periodically takes and analyzes images of the user's mouth. If the user's mouth moves significantly or closes its mouth, the embodiment will readjust its movement path and ensure feeding continues even when the user's mouth is open. This process is repeated until the embodiment completes the predetermined feeding, at which point the operation ends.

[0051] II. Positioning and tracking of the mouth:

[0052] The embodiment involves having the user wear a wearable food tray 5 equipped with an IMU sensor to measure the acceleration and rotation angle of the user's head, and then integrating the acceleration to obtain the movement of the user's mouth, which has good real-time performance.

[0053] like Figure 2 As shown, the embodiment uses an image recognition method to periodically locate the mouth position. Between two adjacent image recognitions, an IMU sensor is used to perform real-time, high-precision motion measurement, and the current mouth position is calculated based on the result of the previous image recognition. This achieves mouth position location and real-time tracking at a lower image recognition frequency, reducing the computing power requirements of the computing platform.

[0054] The embodiment determines the initial position through visual recognition and positioning, which solves the problem that IMU sensors can only measure relative displacement and cannot provide the feeding robot with a specific spatial position.

[0055] The embodiment utilizes image recognition to periodically correct the IMU measurement results, removing the cumulative error of the IMU sensor caused by data integration over time, and ensuring the accuracy of mouth position tracking during long-term operation.

[0056] In summary, this embodiment combines visual recognition with IMU measurement. Visual positioning solves the problem that IMU sensors cannot obtain absolute position and eliminates the cumulative error of IMU sensors due to data integration over time. At the same time, it leverages the high precision and real-time performance of IMU sensors to improve the feeding robot's ability to track the mouth position during feeding. Thus, it can achieve tracking capabilities comparable to pure visual positioning methods with fewer images and less hardware computing power.

[0057] III. Collision Protection

[0058] The embodiment uses a path planning algorithm to avoid obstacles detected by the binocular camera 2 and pre-defined no-entry zones, effectively reducing the possibility of collisions. The control unit of the feeding robotic arm 3 employs admittance control, giving it specific stiffness characteristics. This allows it to adjust its posture for cushioning in the face of sudden, unexpected collisions and effectively control the magnitude of contact forces. The admittance control law is designed as follows:

[0059]

[0060] The corrected pose acceleration can be obtained by solving the above formula.

[0061]

[0062] right Integrating, we obtain the corrected pose x. e :

[0063]

[0064] The outer surface of the feeding robotic arm 3 is covered with a soft, elastic material to further absorb energy during collisions and maximize user safety.

[0065] IV. Identification and Avoidance of Abnormal Obstacles

[0066] When the feeding robotic arm 3 in this embodiment is obstructed (e.g., someone tries to push it away), in addition to the aforementioned anti-collision function, the embodiment also has an avoidance function to prevent the feeding robotic arm 3 from exerting prolonged and significant pressure on external obstacles (especially the human body) and causing injury. By performing dynamic modeling on the feeding robotic arm 3, the expected torque value of each joint can be calculated based on the current rotation angle, angular velocity, and angular acceleration of each joint. When the feeding robotic arm 3 is obstructed by an external force, there will be a significant deviation between the actual feedback joint torque value and the calculated expected torque value. Further weighting of the torque deviation of each joint yields a deviation index δ used to characterize the magnitude of the external contact force.

[0067]

[0068] When δ is less than or equal to the error reference value δ err When the feeding robotic arm 3 is considered to be operating normally, it continues to move along the predetermined path; when δ is greater than the error reference value δ err If δ indicates that the feeding robotic arm 3 is in an abnormal state, possibly due to obstruction, the feeding robotic arm 3 will suspend the feeding operation. err <δ≤δ a This indicates that the contact force between the feeding robotic arm 3 and the external obstacle is still at a relatively low level, and the feeding robotic arm 3 remains in its current position; if δ > δ a This indicates that the contact force between the feeding robotic arm 3 and the external obstacle is too large, exceeding the preset threshold. The feeding robotic arm 3 will immediately limit the output torque of each joint, allowing the external force to push the feeding robotic arm 3 to avoid the obstacle. After the external obstacle is removed, the deviation index δ will return to the error range. At this time, the feeding robotic arm 3 will attempt to return to the original path to continue the feeding action.

[0069] like Figure 3 , 4 As shown, the specific steps for completing one feeding cycle in this embodiment are as follows:

[0070] S1. Wait for the voice command "Start feeding" or the manual remote control command; if the command is received, execute S2, otherwise continue to wait;

[0071] S2, the binocular camera 2 acquires lip-shape image data and sends it to the data processor 1;

[0072] S3. Data processor 1 identifies obstacle information based on lip shape image data and determines whether the mouth is open. If the mouth is open, S4 is executed; otherwise, S2 is executed.

[0073] S4. Data processor 1 calculates the spatial position of the exit section based on the lip shape image data. Specific steps are as follows: Figure 2 As shown:

[0074] S4-1. Calculate the three-dimensional spatial coordinates of the mouth, i.e. the initial spatial position, based on the lip shape image data of the binocular camera 2.

[0075] S4-2. Receive head motion data from the IMU sensor, namely head acceleration and head rotation angle; integrate the head acceleration to obtain the head displacement.

[0076] S4-3. The initial spatial position is corrected based on the head displacement and head rotation angle to obtain the corrected spatial position; the corrected spatial position is the spatial position of the mouth.

[0077] S5, the feeding robotic arm 3 automatically plans the movement path and feeding action based on the spatial position of S4, the obstacle information of S3, and the pre-set restricted area;

[0078] S6. The feeding robotic arm 3 performs the feeding action along the motion path. The specific steps are as follows: Figure 4 As shown:

[0079] S6-1, The feeding robotic arm 3 acquires the motion parameters of each joint, namely rotation angle, angular velocity, and angular acceleration, and calculates the actual torque value of each joint based on the above motion parameters;

[0080] S6-2. Calculate the expected torque value of each joint based on the dynamic model of the feeding robotic arm 3; subtract the expected torque value of each joint from the actual torque value to obtain the torque deviation value; calculate the deviation index δ based on the torque deviation value of each joint.

[0081]

[0082] Where the subscript n is the number of each joint, τ is the torque deviation value generated at each joint position under the action of external force, and w is the weight set for different joint positions;

[0083] S6-3. Set an error reference value δ err and a maximum error δ a According to δ and δ err δ a The difference is used to adjust the movement of the feeding robotic arm 3, as follows:

[0084] If δ≤δ err If the feeding robotic arm 3 is considered to be operating normally, the corrective action is for the feeding robotic arm 3 to continue moving along the predetermined path.

[0085] If δ err <δ≤δ a If the force of contact with the external obstacle is small, the feeding robotic arm 3 is considered to be in an abnormal state. The solution is to keep the feeding robotic arm 3 in its current position until the external obstacle is removed.

[0086] If δ a If the torque value is less than δ, the feeding robotic arm 3 is considered to be in an abnormal state and has a relatively large contact force with external obstacles. The solution is to move the feeding robotic arm 3 to a safe position so that the actual torque value of each joint is kept within a safe range until the external obstacles are removed.

[0087] The method for calculating a safe location is as follows:

[0088] The admittance control law for the feeding robotic arm 3 is:

[0089]

[0090] Where M is the mass parameter corresponding to the acceleration term, B is the damping parameter corresponding to the velocity term, K is the elastic parameter corresponding to the displacement term, and F... env It is the interference force generated by the robot colliding with the external environment, x e It is a safe location;

[0091] The safe position x is obtained from the above formula. e acceleration:

[0092]

[0093] right Integrating, we obtain the safe position x. e :

[0094]

[0095] S7. If the spatial position of the mouth or the state of the mouth opening changes, execute S2; otherwise, execute S8.

[0096] S8. If all feeding actions have been completed, end the feeding process; otherwise, execute S6.

[0097] Although the embodiments of the present invention have been disclosed above, they are not limited to the applications listed in the specification and embodiments. They can be applied to various fields suitable for the present invention. For those skilled in the art, and for those of ordinary skill in the art, various changes, modifications, substitutions and variations can be made to these embodiments without departing from the principles and spirit of the present invention. Therefore, the present invention is not limited to the specific details without departing from the general concept defined by the claims and their equivalents.

Claims

1. A smart feeding robot based on vision and IMU dual positioning, comprising a data processor (1), a binocular camera (2), a feeding robotic arm (3), and an end feeder (4); the binocular camera (2) is fixedly connected to the data processor (1), collects lip shape image data and sends it to the data processor (1); the data processor (1) locates and analyzes the lip shape based on the lip shape image data, and sends action commands to the feeding robotic arm (3); the bottom of the feeding robotic arm (3) is fixedly connected to the data processor (1), and the top is connected to the end feeder (4); the feeding robotic arm (3) consists of 3 joints and is covered with a soft material; the feeding robotic arm (3) receives action commands from the data processor (1), rotates each joint according to the action commands, moves the end feeder (4) to a designated position, and sends the torque, rotation angle, angular velocity, and angular acceleration of each joint to the data processor (1); characterized in that: Includes a wearable food tray (5); the wearable food tray (5) is worn around the neck to catch food spilled during feeding; The wearable meal tray (5) includes an IMU sensor inside; the IMU sensor collects head motion data and sends it to the data processor (1) via Bluetooth; the motion data includes acceleration and rotation angle, i.e. head acceleration and head rotation angle; The data processor (1) calculates the spatial position of the mouth based on the lip shape image data and motion data, and the steps are as follows: S1-1. Calculate the three-dimensional spatial coordinates of the mouth based on the mouth shape image data of the binocular camera (2), i.e. the initial spatial position; S1-2. Integrate the head acceleration to obtain the head displacement; S1-3. Correct the initial spatial position based on the head displacement and head rotation angle to obtain the spatial position of the mouth; The feeding steps are as follows: S2-1, Wait for the "Start feeding" voice command or manual remote control command; if the command is received, execute S2-2, otherwise continue to wait; S2-2, Binocular camera (2) collects lip shape image data and sends it to data processor (1); S2-3, Data processor (1) identifies obstacle information based on lip shape image data and determines whether the mouth is open; if the mouth is open, execute S2-4, otherwise execute S2-2; S2-4, Data processor (1) Calculates the spatial position of the exit part based on the mouth shape image data; S2-5, Feeding robotic arm (3) automatically plans the movement path and feeding action based on the spatial position of S2-4, the obstacle information of S2-3 and the pre-set restricted area; S2-6, The feeding robotic arm (3) performs the feeding action along the motion path; S2-7. If the spatial position of the mouth or the state of the mouth opening changes, execute S2-2; otherwise, execute S2-8. S2-8. If all feeding actions have been completed, end the feeding process; otherwise, proceed to S2-6. 2.The intelligent feeding robot based on visual and IMU dual positioning of claim 1, wherein, S2-6 includes the following steps: S2-6-1、Feeding robotic arm (3) obtains the motion parameters of each joint, namely rotation angle, angular velocity, and angular acceleration, and calculates the actual torque value of each joint based on the above motion parameters; S2-6-2. Calculate the expected torque value of each joint based on the dynamic model of the feeding robot arm (3); subtract the expected torque value of each joint from the actual torque value to obtain the torque deviation value; calculate the deviation index based on the torque deviation value of each joint. : Subscript n These are the joint numbers. It is the torque deviation value generated at various joint positions under the action of external force. w It refers to the weights assigned to different joint positions; S2-6-3. Set an error baseline value. and a maximum error ,according to and , The difference adjustment of the feeding robotic arm (3) is as follows: if If the feeding robot arm (3) is considered to be operating normally, the handling measure is to continue the feeding robot arm (3) moving along the predetermined path; if If the feeding robot (3) is in an abnormal state, but the contact force with the external obstacle is relatively small, the solution is to keep the feeding robot (3) in its current position until the external obstacle is removed. if If the feeding robot arm (3) is in an abnormal state and the contact force with the external obstacle is relatively large, the solution is to move the feeding robot arm (3) to a safe position so that the actual torque value of each joint is kept within a safe range until the external obstacle is removed.

3. The intelligent feeding robot based on dual positioning of vision and IMU according to claim 2, characterized in that, The method for calculating the safe location is as follows: The admittance control law of the feeding robotic arm (3) is: in It is the mass parameter corresponding to the acceleration term. These are the damping parameters corresponding to the velocity term. These are the elastic parameters corresponding to the displacement term. It is the interference force generated by the robot colliding with the external environment. It is a safe location; The safe position is obtained from the above formula. acceleration: right Integrate to obtain a safe position. : 。

Images