A lying bed auxiliary feeding mechanical arm trajectory planning method, device and system based on a staged constraint RRT

Abstract

The application discloses a kind of based on stage constraint RRT's bed auxiliary feeding mechanical arm trajectory planning method, device and system, it is related to robot control technical field.The present application aims to solve the problem of controllable planning time consumption and failure rate under the scene of bed auxiliary feeding, generates and executes the mechanical arm feeding trajectory that satisfies stage heterogeneous constraint, adapts mouth dynamic pose change, has force sense safety gate and controlled retreat capability.For this, once feeding task is decomposed into four stages of taking food, transport, delivery, retreat, is managed by state machine, and stage switching is triggered by sensing event gate control instead of fixed time sequence: taking food is transported by food in fork confidence threshold trigger, delivery is retreated by in-place determination or contact force out-of-limit trigger.When state machine enters each stage, the constraint set of the stage, tolerance, time budget and speed upper limit are injected into RRT planner.Meanwhile, based on the real-time pose of the head of the person in bed, the approach axis that adapts supine / decubitus is calculated in the head coordinate system, the pre-delivery target is generated as the pose at the fixed offset from the mouth along the axis, and is updated in real time with the head pose.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, specifically to a method, device, and system for trajectory planning of a bedridden assisted feeding robotic arm based on phased constraint RRT. Background Technology

[0002] A representative solution for assisted feeding robots. The Assistive Dexterous Arm (ADA) project at the University of Washington, exemplifies this approach. A multi-degree-of-freedom collaborative robotic arm is mounted on an electric wheelchair (or fixed to a support such as a bedside table). The end effector is equipped with a fork-shaped cutlery, force sensors, and an RGB-D vision sensor. It uses a learning-based method to estimate the fork's posture for different foods to complete bite acquisition, and then delivers the food to the seated user's mouth. This type of solution focuses on feeding strategies for various foods. In the delivery phase, the user's mouth is typically treated as a static or slowly changing target, and delivery is performed according to a preset approach direction facing the seated body. Notably, recent reports of real-world home deployments of this project show that users experience limited head movement when eating in bed, leading to some delivery difficulties. This highlights the adaptability challenges of existing solutions for seated scenarios in bed-based settings.

[0003] A general solution for motion planning. Existing systems of this type generally employ a general robot motion planning framework (such as ROS / ROS 2 with MoveIt / MoveIt 2, using sampling planners like RRT and RRT-Connect from the OMPL motion planning library at the underlying level). A single feeding action is treated as a point-to-point planning problem from a single start point to an end point: given the current joint configuration of the robotic arm and the target end-effector pose, random sampling is performed in the configuration space, and the search tree is expanded to obtain a collision-free path. This path is then time-parameterized and executed by the joint controller. The execution process is generally open-loop, without end-effector force feedback or target updates during execution.

[0004] A complete feeding action in existing technologies actually includes several sub-processes with drastically different constraints, such as catching (approaching the food in the direction of insertion / scooping), transporting (preventing food spillage), and delivering (requiring safe access near the face). Single-segment point-to-point planning can only use the same constraint configuration throughout: if no end-effector posture constraint is applied, the tableware posture can be arbitrarily flipped during the transport segment; if a uniform posture constraint is applied throughout, the effective sampling space of RRT is compressed to near a low-dimensional constraint manifold, the sampling hit rate decreases significantly, leading to increased planning time and failure rate, and the catching segment may even become unsolvable due to the conflict between the posture constraint and the insertion posture.

[0005] Existing standard RRTs sample uniformly in configuration space, but do not guarantee that the tilt angle of the end cutlery relative to the direction of gravity is bounded at any point on the path. For liquid, semi-liquid, and loose foods, the tilting of the cutlery during transport directly causes food to fall off. This problem is essentially a technical issue concerning how the sampling planner samples, verifies, and projects data onto the attitude-constrained manifold; existing single-segment planning schemes do not address this specific issue.

[0006] Bedridden patients are typically positioned on a nursing turning bed, and their mouth posture continuously changes due to adjustments in bed position and spontaneous head movements. The entire trajectory generated based on a single frame of mouth posture at the planning time may become invalid during execution; and the time required for global replanning of the entire trajectory is in the hundreds of milliseconds to seconds, with an uncontrollable upper bound, causing execution delays and safety hazards in areas near the face.

[0007] The existing solution does not determine whether the food is still on the tableware before delivery, and empty fork delivery will waste a complete execution cycle; during the delivery process, it does not monitor the contact force between the tableware and the human body. When the tableware makes abnormal contact with the lips, teeth, or cheeks, the system lacks a controlled stop and retraction mechanism, and the peak contact force is not limited.

[0008] ADA-type systems assume the user is seated with their head nearly upright, and their pre-delivery posture and approach axis are preset in a near-horizontal direction. The mouth orientation and feasible approach direction of patients in a supine (supine / lateral) position differ significantly from those in a seated scenario. Using a fixed preset posture will result in the delivery segment target being unreachable, or an increased failure rate in collision verification between the planned path and the human body or bed.

[0009] The existing solution uses a single speed scaling factor for the entire trajectory after time parameterization: to ensure safety in the near-face segment, the speed is reduced throughout, which significantly lengthens the single feeding cycle time and reduces the system throughput; to pursue efficiency, the speed is increased, which causes the linear velocity of the end near the face to exceed the safety limit. Summary of the Invention

[0010] This invention provides a method, device, and system for planning the trajectory of a robotic arm for bed-assisted feeding based on phased constraint RRT. It aims to solve the problem of generating and executing a robotic arm feeding trajectory that satisfies phased heterogeneous constraints (accessibility to food, anti-spillage tilt angle during transport, and safe proximity for delivery), adapts to dynamic changes in mouth posture, and has force-sensing safety gating and controlled retraction capabilities in bed-assisted feeding scenarios, with controllable planning time and failure rate.

[0011] This invention is achieved through the following technical solution: A trajectory planning method for a bed-assisted feeding robotic arm based on phased constraint RRT, the method comprising the following steps: Step 1: Decompose a feeding task into four stages: feeding, transfer, delivery, and withdrawal. These stages are managed by a task state machine. Stage switching is triggered by sensor event gating rather than a fixed sequence: the switch from feeding to transfer is triggered when the food confidence at the fork exceeds a threshold, and the switch from delivery to withdrawal is triggered by arrival determination or contact force exceeding the limit. Step 2: When the state machine enters each stage, the independent constraint set, target tolerance, planning time budget and speed limit for that stage are injected into the RRT planner; among them, no end-effector attitude constraint is applied to the feeding stage, and the target pose is determined by the piercing direction; the transfer stage is subject to end-effector tilt cone constraint; the delivery stage is limited to performing restricted motion within the approach corridor in front of the mouth. Step 3: Based on the real-time head pose of the supine recipient, calculate the approach axis in the head coordinate system to adapt to the supine / lateral posture, and generate the pre-delivery target as the pose along the approach axis at a fixed offset distance from the mouth. The pre-delivery pose is updated in real time with the head pose.

[0012] Furthermore, the end-of-transfer section tilt cone constraint in step 2 is implemented through node-level verification via RRT tree expansion, specifically, Calculate the forward kinematics of the candidate nodes to obtain the tableware coordinate system orientation R(qnew), and calculate the angle between the vertical reference axis zt of the tableware and the opposite direction of gravity -g: θ(qnew)=arccos((R(qnew)·zt)·(-g)) If θ≤θmax, the node is accepted into the tree; otherwise, the pose is rotated and projected around the horizontal axis of the allowable cone boundary. The inverse kinematics of the projected end pose is used to obtain the corrected configuration. If the inverse solution exists and the joint displacement between the nearest neighbor node is less than the threshold, the corrected configuration is used as a candidate node; otherwise, this expansion is discarded. Collision checks are performed point by point on the interpolated edge paths from candidate nodes to their nearest neighbors, and the paths are passed through the last-in tree. After a node enters the tree, it attempts to connect with the target region G. The connection segment also performs the above tilt angle check and collision check. If the connection is successful, the path is backtracked, smoothed by a shortcut, and then output. The smoothed path is then rechecked point by point for θ≤θmax.

[0013] Furthermore, during execution, the mouth position is continuously monitored, and processing is performed according to a dual threshold method: Execution is not interrupted when the mouth displacement Δp < the first threshold d1; When d1≤Δp<second threshold d2, only the target region G is translated and updated. If the current trajectory endpoint still falls within the updated G, the process continues. When Δp≥d2, the current execution is stopped. The current joint state of the robotic arm is taken as the new starting point and the updated target area G is taken as the end point. Within the limited replanning time budget, only the remaining road segments are replanned.

[0014] Furthermore, the delivery segment performs a linear Cartesian motion in the reverse direction along the approach axis, and is gated by two signals: Entry Gating: Before entering the delivery segment, the food is checked for its fork state and confidence level. If the confidence level is lower than the threshold cth, entry into the delivery segment is prohibited, and the state machine transitions to re-feeding. Process gating: During execution, the contact force norm is monitored in real time. If it exceeds the safety threshold Fmax, the forward movement is immediately stopped, and the device is retracted in a controlled manner along the same approach axis to the pre-delivery pose. The pullback trajectory is pre-generated and cached when entering the delivery segment, and the time delay from the limit violation determination to the issuance of the pullback action does not exceed one control cycle.

[0015] Furthermore, the path at each stage is parameterized in time, and the speed scaling factor of each segment is set according to the minimum distance between the segment and the human body surrounding it: the transport segment far from the human body adopts a higher speed scaling, and after entering the pre-mouth area, the terminal linear velocity is limited to below the safe value vsafe.

[0016] Furthermore, the target region G is constructed with the pre-delivered pose (ppre, Rpre) as the center and with position tolerance εp and attitude tolerance εo as tolerances, for use by the planner, and is refreshed according to the pose update cycle.

[0017] A trajectory planning device for a bed-assisted feeding robotic arm based on phased constraint RRT is deployed in a robotic arm motion planning computer. The device includes... The perception access module subscribes to topics such as mouth pose, food in fork state and confidence, and contact force. It filters the mouth pose and maintains the MouthState circular buffer, and generates and outputs the GateState. The task state machine module maintains four states: food intake, transfer, delivery, and retreat. It performs state transitions based on the entry gating and exit events of each stage and sends the corresponding PhaseConfig to the motion planning module when a state is entered. The phase constraint configuration module loads and parses the constraint sets, target tolerances, time budgets, and speed scaling factors for each phase from the configuration file, and converts them into constraint and parameter fields in the planning request. The target generation module calculates the proximity axis and the pre-delivery target area in the head coordinate system based on the real-time head pose and refreshes it periodically. The motion planning module performs unconstrained RRT-Connect planning, constrained RRT planning with inclination cone constraints, and linear Cartesian planning, and completes time parameterization. The replanning trigger module performs dual-threshold displacement determination of the port position, target translation correction, and replanning scheduling of the remaining segment. Safety gate control and retraction module, which monitors the norm of contact force, cancels the current execution and issues the pre-cached retraction trajectory when the norm exceeds the limit; Execution interface module, which interacts with the manipulator controller through the FollowJointTrajectory action interface and feeds back the execution status.

[0018] Further, when the motion planning module performs constrained RRT planning with inclination cone constraints in the transfer segment, it calculates forward kinematics for each candidate node to obtain the tableware pose, verifies that the included angle θ between the vertical reference axis of the tableware and the reverse direction of gravity satisfies θ≤θmax, otherwise it projects to the allowable cone boundary and then performs inverse kinematics back-substitution; when the inverse solution exists and the joint displacement with the nearest neighbor node is less than the threshold, the corrected configuration is added to the tree; The replanning triggering module reads the filtered mouth position from the MouthState circular buffer at 30Hz, and calculates the displacement Δp relative to the reference position at the current planning time: No processing is performed when Δp<d1; Only the target area G is translated when d1≤Δp<d2; When Δp≥d2, the current execution is canceled, and with the current joint state as the starting point and the updated target area G as the end point, the remaining segment is replanned within the replanning time budget.

[0019] Further, before entering the delivery segment, the safety gate control and retraction module verifies the confidence of food on the fork in GateState; when the confidence is lower than cth, it prohibits entering the delivery segment and notifies the task state machine to switch back to foraging; during the execution of the delivery segment, it subscribes to the norm of contact force in an independent high-priority callback group, cancels the current execution immediately and issues the pre-cached retraction trajectory when the norm exceeds the limit, and the delay from the over-limit determination to the retraction issuance does not exceed one control cycle; The MouthState structure maintained by the perception access module includes Tmouth, stamp, vest and a recentposes circular buffer with a capacity of N, which is used for median filtering and double-threshold displacement determination.

[0020] A trajectory planning system for a bedridden auxiliary feeding manipulator, the system comprises, A perception layer, configured to collect mouth pose, food-on-fork status and confidence, and wrist contact force; A planning and decision-making layer, configured to run the aforementioned trajectory planning device and output joint trajectories for each stage; An execution layer, configured to receive the joint trajectories and control the manipulator to execute; Wherein the perception layer, the planning and decision-making layer and the execution layer communicate through ROS2 topics and action interfaces, and the force sensing callback is placed in an independent high-priority callback group.

[0021] The beneficial effects of the present invention are: Since the constraints of the three sub-processes of feeding, transport, and delivery are fundamentally different, this invention injects attitude constraints, target tolerance, time budget, and speed upper limit separately for each stage, ensuring that each constraint takes effect only when it is truly necessary. The mechanism is as follows: the sampling hit rate of a sampling planner decreases as the constraint dimension increases, and attitude constraints compress the effective sampling space to near the low-dimensional constraint manifold. This invention ensures that sampling in the feeding and retreat stages is conducted within the full-dimensional configuration space, with only the transport stage bearing the cost of constraint sampling. Therefore, it reduces the overall computational load required to complete a feeding task and the possibility of planning failure, and fundamentally eliminates the situation in existing schemes where a conflict between unified attitude constraints and the feeding insertion attitude leads to an unsolvable feeding stage.

[0022] This invention performs node-level verification on each candidate node during the tree expansion process of the transport segment. This verification includes forward kinematics calculation, cutlery tilt angle calculation, comparison with the allowable cone, and projection to the cone boundary or rejection if the node exceeds the limit. The smoothed shortcut path is also verified point-by-point. Because this verification applies to every accepted node and its edge interpolation points on the path, the generated transport trajectory satisfies the bounded tilt angle condition point-by-point throughout its entire length, rather than only at the start and end poses. Compared to the standard RRT's approach of uniformly sampling in configuration space and making no guarantees regarding the cutlery posture during the intermediate stages of the path, this invention ensures that the cutlery does not undergo posture flipping that could cause food to fall off during transport, making it particularly effective for liquid, semi-liquid, and loose foods.

[0023] This invention changes the pre-delivery target from a fixed pose pre-set to the seated human body to an approach axis and offset pose calculated from the real-time head pose within the head coordinate system. This allows the target to automatically adapt to the lying posture (supine / lateral) and body position adjustments. Compared to solutions that rely on seated geometric assumptions, this invention avoids target unreachability due to mismatches between the approach direction and the mouth's orientation in a lying position, as well as collision verification failures between the path and the human body or bed. This makes the solution applicable to bedridden scenarios where seated solutions are difficult to cover.

[0024] This invention replaces the single-frame target + global replanning processing method at planning time with a dual-threshold determination of mouth displacement: for small displacements, the target is translated only within the target tolerance range without interrupting execution; for large displacements, the remaining segments that have not yet been executed are replanned only, starting from the current joint state. Its mechanism is that the computational load of replanning increases with the length of the segment to be planned and the range of the configuration space it spans. The planning scale of the remaining segments is strictly smaller than the planning scale of the entire trajectory and is limited to completion within a given replanning time budget. Therefore, under the condition of continuous mouth pose changes, this invention tracks dynamic targets with a response latency significantly lower than global replanning, avoids execution lag, and ensures that target failure no longer leads to the invalidation of the entire feeding cycle.

[0025] This invention employs two independent gating signals: the entry gating, based on the food's fork state and its confidence level, prevents the state machine from entering the delivery segment and reverses to the picking stage when the condition is not met, thus eliminating the invalid execution cycle of empty fork delivery in principle; the process gating immediately stops the forward movement and retracts along the approach axis when the contact force norm exceeds the limit, eliminating the abnormal contact force before it can continue to increase, thereby constraining the peak contact force by an upper bound. Furthermore, since the retraction trajectory is pre-generated and cached at the beginning of entering the delivery segment, no planning calculations are required at the moment of exceeding the limit. The time delay from the limit determination to the issuance of the retraction action is compressed to the order of one control cycle, and the upper bound of this delay is determined by the control cycle and is independent of the current planning load. This deterministic response upper bound is not present in open-loop execution schemes.

[0026] This invention sets speed scaling factors based on the minimum distance between each path segment and the human body's surrounding area, ensuring that speed limits are only incurred in the sections closest to the human body. Compared to solutions that reduce speed throughout to ensure near-face safety, this invention shortens the total duration of a single feeding cycle while maintaining the same near-face linear speed safety level. Compared to solutions that increase speed overall for efficiency, this invention does not sacrifice speed safety in the near-face area. This resolves the conflict between near-face safety speed limits and feeding cycle efficiency.

[0027] In summary, all the technical effects described in this invention can be directly derived from the above-mentioned mechanisms of action, and can be verified through comparative tests between the Gazebo simulation environment and the FR5 real machine under single-segment point-to-point RRT-Connect and under uniform constraints and uniform speed scaling baseline throughout the process. The statistically available quantitative indicators in the tests include average planning time, planning failure rate, maximum tableware tilt angle in the transfer segment, replanning response delay, delivery success rate, peak contact force, and duration of a single feeding cycle. Attached Figure Description

[0028] Figure 1 This is the overall system architecture diagram of the present invention.

[0029] Figure 2 This is the overall flowchart of the feeding task state machine and method of the present invention.

[0030] Figure 3 This is a schematic diagram illustrating the definition of the key data structures (PhaseConfig, MouthState, GateState) of this invention.

[0031] Figure 4 This is an extended flowchart of the anti-spillage attitude constraint RRT of the transfer section of the present invention.

[0032] Figure 5 This is a flowchart of the dual-threshold residual segment replanning process for the dynamic target of the mouth in this invention.

[0033] Figure 6 This is a timing diagram of the delivery segment force / confidence dual gating and pullback mechanism of the present invention. Detailed Implementation

[0034] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods are omitted so as not to obscure the description of this application with unnecessary detail.

[0035] It should be understood that, when used in this specification and the appended claims, the terms include indicating the presence of the described feature, integral, step, operation, element and / or component, but do not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or collections thereof.

[0036] It should also be understood that the terminology used in this application specification is for the purpose of describing particular embodiments only and is not intended to limit the application. As used in this application specification and the appended claims, unless the context clearly indicates otherwise, the singular forms "one," "an," and "that" are intended to include the plural forms.

[0037] The following is in conjunction with the appendix to this application specification. Figure 1-6 The technical solutions in the embodiments of this application are clearly and completely described. 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.

[0038] Many specific details are set forth in the following description in order to provide a full understanding of this application. However, this application may also be implemented in other ways different from those described herein. Those skilled in the art can make similar extensions without departing from the spirit of this application. Therefore, this application is not limited to the specific embodiments disclosed below.

[0039] This invention decomposes a feeding task into four stages: feeding, transport, delivery, and withdrawal, all managed by a task state machine. Upon entering each stage, the state machine injects the independent constraint set, target tolerance, planning time budget, and speed limit into the RRT planner. Specifically, the feeding stage does not impose attitude constraints, and the target pose is determined by the insertion direction; the transport stage imposes an end-effector cone constraint; and the delivery stage is limited to restricted movement within the approach corridor in front of the mouth. Stage transitions are triggered by sensor event gating rather than a fixed sequence: the transition from the feeding stage to the transport stage is triggered when the food confidence at the fork exceeds a threshold, and the transition from the delivery stage to the withdrawal stage is triggered by positioning determination or exceeding the contact force limit.

[0040] In the RRT tree expansion process of the transfer section, this invention calculates the forward kinematics of each candidate node to obtain the tableware posture. The node is only accepted into the search tree if the angle between the vertical reference direction of the tableware and the opposite direction of gravity is within the allowable cone (half angle θmax). Otherwise, the posture is projected onto the boundary of the allowable cone and re-verified or directly rejected. In engineering, this is equivalent to the posture path constraint of the planner (setting tolerance for the angle of rotation around the two horizontal axes), so that the entire transfer path meets the spill prevention condition point by point, while the feeding and retreat sections are not subject to this constraint, thus maintaining sampling efficiency.

[0041] This invention utilizes real-time coordinate transformation based on mouth marking / head posture to calculate the approach axis adapted to the supine (supine / lateral) posture within the head coordinate system. The pre-delivery target is then generated as a pose along this approach axis at a fixed offset distance from the mouth. The orientation of the tableware is jointly determined by the approach axis direction and spill-proof posture constraints. This target is updated in real-time with the head posture, replacing the fixed, near-horizontal pre-delivery pose used in seated scenarios.

[0042] During the execution of this invention, the pose of the mouth is continuously monitored: when the displacement exceeds a first threshold... d At step 1, within the target tolerance range, only online translation correction is performed on the target without interruption; when the displacement exceeds the second threshold... d At 2 o'clock, the current execution is stopped. The current joint state of the robotic arm is taken as the new starting point and the updated target area (a set of targets with position / attitude tolerance rather than a single pose point) is taken as the endpoint. Within the limited time budget, only the remaining road segment is replanned to avoid the high time consumption and execution delay caused by global replanning of the entire trajectory.

[0043] In this invention, the delivery segment performs linear Cartesian motion along the approach axis and is gated by two signals: entry gating, which prohibits food from entering the delivery segment when the food is in the fork state and its confidence level is below a threshold, and the state machine switches to re-feeding to eliminate empty fork delivery; and process gating, which immediately stops the forward movement when the contact force norm monitored in real time exceeds the safety threshold Fmax, and is controlled to retract back to the pre-delivery pose along the same approach axis, so as to limit the peak value of abnormal contact force.

[0044] This invention parameterizes the time for each stage of the path, and sets the speed scaling factor for each segment according to the minimum distance between the segment and the human body surrounding it: the transport segment far from the human body adopts a higher speed scaling, and after entering the preoral region, the terminal linear velocity is limited to below a safe value, so as to meet the safety speed limit near the face while avoiding the cycle time loss caused by the deceleration throughout the process.

[0045] like Figure 1 The diagram illustrates the overall system architecture of this embodiment of the invention. The system is deployed on a motion planning computer running ROS 2, and is divided into a perception layer, a planning decision layer, a motion planning layer, and an execution layer from top to bottom. The perception layer includes a mouth pose sensing node (publishing mouth pose markers at 30Hz in coordinate transformation / tf format), a food state sensing node (publishing the topics / feeding / foodonfork and / feeding / foodonforkconfidence), and a force sensing node (publishing the topics / feeding / contactforce and / feeding / contactforcenorm). The three data streams are filtered, the MouthState ring buffer is maintained, and the GateState gated state is generated by the perception access module. The planning decision layer includes a task state machine module, a stage constraint configuration module, a target generation module, a replanning trigger module, and a safety gating rollback module, responsible for issuing planning requests, PhaseConfig, and target regions G to the motion planning layer according to stages. The motion planning layer is based on MoveIt 2 / OMPL and includes RRT-Connect planning (with both unconstrained and tilt cone constraints), linear Cartesian planning, TOTG time parameterization, and planning scene maintenance (collisors such as human body and bed). The execution layer connects to the FR5 robotic arm controller (equivalent to the controller in Gazebo during simulation verification) via the FollowJointTrajectory motion interface, and joint states are fed back via / jointstates; the safety gating and retraction module has the highest priority cancellation / retraction channel directly to the execution interface. The data flow and control flow directions are shown by the arrows in the figure.

[0046] Data input and collection: (a) Mouth pose: Real-time pose from the mouth target marker model in the Gazebo scene during simulation verification; obtained from RGB-D camera combined with face keypoint detection during real device deployment. Both are published to / tf in the form of coordinate transformation messages (geometrymsgs / TransformStamped), with an update frequency of 30Hz.

[0047] (b) Food in fork state: The food state perception node determines the state based on the contact force characteristics of the fork tip and the relative geometric relationship between the food and the cutlery, and publishes it as a Boolean topic / feeding / foodonfork and a floating-point confidence topic / feeding / foodonforkconfidence.

[0048] (c) Contact force: from wrist force sensing (Gazebo force / torque sensor plugin in simulation), published as / feeding / contactforce (three components) and / feeding / contactforcenorm (norm scalar), with a sampling frequency of 100Hz in simulation and up to 1kHz on real devices; the QoS configuration for this topic is besteffort, keeplast (depth 1) to ensure low latency.

[0049] (d) Robotic arm status: Feedback is sent via ros2control using / jointstates(sensormsgs / JointState), and the FR5 control cycle is 8ms.

[0050] Core processing logic: (a) Data structure design (e.g.) Figure 3 To support phased planning and event gating, three core data structures are specifically designed: PhaseConfig (phase configuration structure), which includes phaseid (phase enumeration), tiltconedeg (tilt angle tolerance cone half angle θmax, only effective for the transport segment), goaltolpos / goaltolrot (target position tolerance εp / attitude tolerance εo), plantimebudget and replantimebudget (initial / replanning time budgets), velscaling (velocity scaling factor), entrygate (entry gating condition), and exitvents (exit event list). Each phase configuration is configured in YAML file format via ROS. 2. Server loading parameters; MouthState (mouth state structure), containing Tmouth (mouth pose represented by 4×4 homogeneous transformation), stamp (timestamp), vest (pose and velocity estimation), and a circular buffer of size N, recentposes, which stores the mouth pose after filtering of the most recent N frames, used for median filtering and double threshold displacement determination to suppress false triggering caused by perceived jitter; GateState (gating state structure), containing foodonfork, confidence, forcenorm, Fmax, and cth fields, serving as the unified input for entry gating and process gating.

[0051] (b) Overall execution steps, such as Figure 2 .

[0052] Step S101: System initialization, load PhaseConfig from YAML, build the planning scene, register the bed, human torso and head, dining table and plate as colliders and update with / tf; Step S102: The state machine enters the feeding stage. The target generation module calculates the piercing target pose based on the food pose in the plate, and plans and executes the feeding path using the pose-free RRT-Connect. Step S103: After the feeding action is completed, read GateState. If foodonfork is true and confidence ≥ cth (0.8 in this example), proceed to the transfer stage; otherwise, return to step S102 to feed again. Step S104: The target generation module generates the pre-delivery target area according to steps S301-S305, and the motion planning module executes RRT planning with tilt cone constraints according to steps S201-S209, and executes it after parameterization according to the velscaling time of the transfer section; during the execution, the replanning triggering module monitors the mouth displacement according to steps S401-S404 and processes it as needed. Step S105: After reaching the pre-delivery pose, enter the delivery stage and perform force-gated linear approach according to steps S501-S505; Step S106: After determining that feeding is complete (e.g., the contact force returns to the baseline after feeding characteristics appear, or the dwell time is reached), enter the withdrawal phase. First, retreat along the approach axis to the pre-delivery pose, and then return to the initial configuration with unconstrained RRT-Connect. Step S107: Decide whether to start the next feeding cycle based on the remaining food and task instructions.

[0053] (c) Extended steps of the anti-spraying constraint RRT for the transfer section, such as Figure 4 : Step S201: Sample the configuration qrand in the joint space with a uniform distribution, and sample the target configuration with a probability pgoal bias; Step S202: Retrieve the nearest neighbor node qnear of qrand in the current search tree; Step S203: Expand from qnear to qrand in a fixed step size Δq to obtain the candidate configuration qnew; Step S204: Calculate the forward kinematics for qnew to obtain the orientation R(qnew) of the tableware coordinate system; Step S205: Calculate the angle θ(qnew) between the vertical reference axis zt of the tableware and the opposite direction of gravity -g = arccos((R(qnew)·zt)·(-g)). If θ≤θmax, then go to step S207. Step S206: Otherwise, rotate the pose around the horizontal axis of the allowable cone boundary (projection), and obtain the corrected configuration by inverse kinematics of the projected end pose; if the inverse solution exists and the joint displacement between it and qnear is less than the threshold, then use the corrected configuration as qnew and go to step S207; otherwise, discard this extension and return to step S201. Step S207: Perform collision checks on each point of the interpolation edge path from qnear to qnew (collision with objects such as the bed, human body, and desktop in the planned scene). If any point fails to pass the check, return to step S201. Step S208: Add qnew to the search tree; Step S209: Attempt to connect qnew with the target region G. The connection segment will also be checked in steps S204-S207. If the connection is successful, backtrack to obtain the path. After shortcut smoothing (the smoothed path is re-checked point by point for θ≤θmax), the transfer trajectory will be output.

[0054] It should be noted that steps S204-S206 can be equivalently implemented in engineering by issuing attitude path constraints to the planner (setting ±θmax tolerance for the rotation angle of the tableware around the two horizontal axes), and having the constraint planning framework perform verification and projection during sampling.

[0055] (d) Pre-delivery pose generation step: Step S301: Obtain the real-time transformation between the head coordinate system and the robot arm base coordinate system from / tf; Step S302: Select the local proximity direction alocal corresponding to the lying position in the head coordinate system (when lying supine, take the direction that is diagonally upward and in front of the mouth and forms a preset angle β with the normal of the face; when lying on the side, take the near-horizontal direction), transform it to the base system to obtain the proximity axis a=Rhead·alocal and normalize it; Step S303: Generate the pre-delivery position ppre = pmouth + doffset·a, where doffset is the pre-mouth offset distance (0.08m in this example); Step S304: Generate pre-delivery posture: Align the tableware feed axis with the -a direction, and then select the roll angle that minimizes the tableware tilt angle θ within the remaining degrees of freedom around the axis, so that the approach direction and the spill prevention constraint are satisfied at the same time. Step S305: Construct a target region G centered at (ppre, Rpre) and with (εp, εo) as the tolerance for the planner, and refresh it according to the pose update cycle of the mouth part.

[0056] (e) Double-threshold residual segment replanning steps, such as Figure 5 : Step S401: During execution, read the filtered mouth position pmouth(t) from the MouthState circular buffer at 30Hz, and calculate the displacement Δp=||pmouth(t)-pmouth(tplan)|| relative to the reference position of the current planning time; Step S402: If Δp<d1 (0.01m in this embodiment), no processing is performed and execution continues; Step S403: If d1≤Δp<d2 (d2 is 0.03m in this embodiment), only translate and update the target area G; if the current trajectory end point still falls within the updated G, continue execution; otherwise, process according to step S404; Step S404: If Δp≥d2, send a cancel instruction to the execution interface, read the current joint state qnow of the robotic arm, with qnow as the starting point and the updated target area G as the end point, re-plan only the remaining segment within replantimebudget (0.5s in this embodiment) while retaining the current-stage constraint configuration; if the planning succeeds, continue execution seamlessly; if it fails, stop safely and retry after the mouth pose is stabilized.

[0057] (f) Force gating step for the delivery segment, as Figure 6 : Step S501: Read GateState before entering the delivery segment: if foodonfork is false or confidence<cth, the state machine switches to the foraging stage, and empty fork delivery is prohibited; Step S502: With the pre-delivery pose as the starting point, plan a straight-line Cartesian path from the pre-delivery pose to the mouth target along the -a direction (retaining the terminal gap δ) (the pose interpolation step size is 5mm in this embodiment), perform time parameterization according to the delivery segment velscaling (corresponding to the end linear velocity not exceeding vsafe) and then issue it for execution; Step S503: During execution, the safety gating module subscribes to forcenorm at the sensor's native frequency and completes threshold determination within a single callback: if forcenorm>Fmax, send a cancel instruction to the execution interface immediately; Step S504: After the cancellation is completed, issue the pre-cached retraction trajectory immediately to retract linearly from the current pose to the pre-delivery pose along the direction; the retraction trajectory is pre-generated and cached when entering the delivery segment, eliminating planning delay at the over-limit moment and ensuring that the delay from over-limit determination to retraction execution does not exceed one control cycle; Step S505: After reaching the position normally, keep waiting for feeding, and switch to the retraction stage according to the feeding determination (contact force characteristic drops back or timeout).

[0058] (g) Segmented velocity modulation: The path at each stage is parameterized by the TOTG time-optimal trajectory generation time parameterization, and the speed and acceleration scaling factors are taken from the PhaseConfig of that stage; further, the transfer segment is divided into two sub-segments, the far end and the near end, according to the minimum distance between the path point and the human body enclosure, and high and low velscaling are used respectively. The near end segment ensures that the end linear velocity does not exceed vsafe.

[0059] State management and persistence: Each phase's PhaseConfig is persisted as a YAML file and loaded by the ROS 2 parameter server upon node startup, supporting parameter tuning without recompilation. The current state of the task state machine, the MouthState circular buffer, and the GateState reside in the node's memory. State transitions and planning requests are executed serially within a mutual exclusion callback group to avoid race conditions. Force feedback callbacks are placed in a separate high-priority callback group to ensure that limit violation judgments are not blocked by planning calculations. The planning scene is maintained by the MoveIt 2 scene monitor, and collision objects such as the human body and bed are updated with / tf. All topic data (mouth pose, force feedback, joint states, and state machine events) are recorded and persisted via rosbag2 for playback debugging and statistics of the fifth technical performance indicator.

[0060] Output and Response: The planning results are sent to the FR5 joint trajectory controller (equivalent to the ros2control in Gazebo in simulation) via the FollowJointTrajectory action interface in the form of joint trajectories (moveitmsgs / RobotTrajectory); the state machine publishes the task status and phase event topics ( / feeding / taskstate, / feeding / phaseevent) for upper-level monitoring and RViz visualization; cancellation and rollback commands triggered by force exceeding limits are directly sent to the execution interface via the highest priority channel; at the end of each feeding cycle, cycle statistics, planning time, number of replanning times, peak contact force, etc. are output and written to the log file.

[0061] To clearly present the complete closed loop of this invention, the input, processing, and output of each stage, as well as the interface conventions between stages, are described in chronological order of data flow and control flow, starting from visual perception input and ending with force control execution and retraction. The entire link consists of four layers: a visual recognition layer, a target calculation layer, a phased trajectory planning layer, and a force control execution and safety gating layer. These four layers are interconnected by explicit data structures (PhaseConfig, MouthState, GateState, and target region G). The specific implementation of any layer can be replaced without altering the interface conventions.

[0062] Visual recognition layer. This layer undertakes two independent recognition tasks, serving the food picking and delivery processes respectively.

[0063] (a) Food Recognition and Feeding Pose Calculation (Serving the Feeding Segment). An RGB-D camera fixed to the robotic arm base or end effector acquires color and depth images of the plate area. Extrinsic parameters obtained through hand-eye calibration transform the observations from the camera coordinate system to the robotic arm base coordinate system. Food instance segmentation is performed on the color image (this example uses a lightweight instance segmentation network; a joint color-depth threshold segmentation can also be used), obtaining masks for each food block. After registering the masks with the depth image, they are back-projected into a point cloud. Principal component analysis is performed on the point cloud of each food instance to determine the position and orientation of the food block using the maximum principal axis direction, the secondary principal axis direction, and the centroid. Based on the shape characteristics of the point cloud (flatness, volume, surface normal distribution), its category attribute (block / strip / loose / liquid) is determined, and a piercing or scooping feeding strategy is selected accordingly. The output is a list of food poses {(pfood, i, Rfood, i, typei, scorei)}, where scorei is the recognition confidence. After sorting the list by confidence and accessibility, the first-ranked item is taken as the target for this food intake, which is used in step S102 to generate the piercing target pose.

[0064] (b) Face and Mouth Pose Recognition (Serving the Delivery Segment). Face region images are acquired using the same camera or another RGB-D camera facing the human body. Face detection and keypoint detection are performed. The 3D back-projection centroid of the relevant keypoints (lip contour point set) is taken as the mouth position pmouth. The facial normal nface is determined by the plane normal spanned by the facial keypoints, and the head horizontal axis is determined by the direction of the line connecting the eyes. The cross product of these two axes yields the head vertical axis, thus constructing a complete head coordinate system pose Thead. To accommodate situations where the face exhibits significant pitch or roll in a supine position, if keypoint detection fails or the confidence level is insufficient, the algorithm reverts to the registration of the depth point cloud and the head template to estimate the Thead. In simulation verification, the pose of the mouth target marker model is directly read. The obtained Thead is then written to the MouthState circular buffer (capacity N) after median filtering and published to / tf at 30Hz.

[0065] (c) Confidence transfer of recognition results. The recognition results are not used directly as geometric quantities, but the confidence scores are output and used for gating: the food recognition confidence score and the food confidence score at the fork together constitute the entry gating input of the delivery segment; when the confidence score of the mouth pose is insufficient or the jump between adjacent frames in the buffer exceeds the set range, the frame is judged as invalid and does not participate in the double threshold displacement judgment, thereby suppressing the erroneous planning caused by perceptual jitter.

[0066] II. Target Calculation Layer. The target calculation layer converts the above identification results into target regions that can be directly used by the planner. The feeding segment determines the piercing / scooping axis along the food principal axis normal or gravity direction based on the food pose and category, generating the feeding target pose, whose posture is not restricted by spillage prevention constraints; the delivery segment calculates the approach axis a and the pre-delivery pose (ppre, Rpre) according to Thead steps S301-S305, and constructs the target region G with (εp, εo) as tolerance. Both types of targets are issued in the form of target regions with tolerance rather than single pose points. This serves to preserve the margin of feasible solutions for the planner and allow subsequent target translation corrections to be completed without triggering replanning.

[0067] Phased trajectory planning layer: The task state machine injects the corresponding PhaseConfig into the planner based on the current stage. The planner then selects the planning configuration accordingly: the feeding segment is planned to the piercing target pose using RRT-Connect without attitude constraints; the transport segment is planned to the pre-delivery target region G using constrained RRT with tilt cone constraints according to steps S201-S209, where each candidate node undergoes forward kinematics calculation, tilt angle verification, and cone boundary projection, and then collision verification before entering the tree; the delivery segment no longer uses sampling planning, but instead uses a straight Cartesian path planning along the approach axis to ensure the predictability of the approach direction; the retreat segment first exits along a straight line in the reverse direction of the approach axis, and then returns to the initial configuration using unconstrained RRT-Connect. Each segment path is parameterized by TOTG time, and the velocity scaling factor is taken from the PhaseConfig of this segment and divided according to the minimum distance to the human body's bounding body. During execution, the replanning trigger module continuously compares MouthState with the reference position at the planning time of this segment, performs double threshold judgment according to steps S401-S404, and replans the remaining segments if necessary, starting from the current joint state.

[0068] Force control execution and safety gating layer: This layer will translate the planning results into a supervised execution process and assume ultimate safety responsibility.

[0069] (a) Contact force criterion for the feeding segment. During the insertion or scooping process, the axial force measured by the wrist force sensor is used as the contact criterion: when the axial force rises from the baseline and exceeds the contact threshold, it is determined that the cutlery has made contact with the food. Subsequently, it continues to move according to the preset insertion depth or scooping trajectory, and stops feeding when the axial force exceeds the upper limit of the food load to avoid piercing the plate or overloading. After the feeding action is completed, the food's fork state and its confidence level are determined by fusing the contact force characteristics (the gravity component continuously borne by the fork tip) with the relative geometric relationship between the food and cutlery, and written to the GateState.

[0070] (b) Force monitoring of the transfer section. No contact force control is applied to the transfer section; only the force norm is used as the abnormal monitoring quantity: if the force norm exceeds the limit during the transfer process where contact should not occur, it is determined to be an accidental collision, and the process is immediately stopped and switched to a safe state.

[0071] (c) Force-gated dual control of the delivery segment. The delivery segment is the link with the deepest force control intervention in the entire link, and is executed according to steps S501-S505. Before entering, the GateState is read to execute the entry gating: if foodonfork is false or the confidence level is lower than the threshold cth, entry into the delivery segment is prohibited and the feeding is turned back to avoid empty fork delivery. After entering, a straight approach is executed along the approach axis, while subscribing to the force norm in an independent high-priority callback group at the sensor's native frequency and completing the threshold determination within a single callback; once the force norm exceeds Fmax, a cancellation command is immediately sent to the execution interface, and the pre-generated cached retreat trajectory is issued, which retreats back to the pre-delivery pose along the same approach axis. Since the retreat trajectory does not require on-site planning, the delay from the limit judgment to the retreat issuance does not exceed one control cycle. After normal positioning, the system waits, and the feeding is determined to be complete based on the contact force characteristics or timeout, and then enters the retreat phase.

[0072] Closed-loop feedback relationship: This completes the closed loop: the visual recognition layer continuously outputs the poses of the food and mouth, along with their confidence levels; the target calculation layer converts these into a tolerance-tolerant target region; the phased planning layer generates and locally updates the trajectory based on phase constraints; and the force control execution layer monitors the planning results using both contact force and confidence level signals during execution. It can also halt execution, trigger rollback, and revert to the previous phase via a state machine transition in case of anomalies, after which the visual recognition layer provides the target again, starting a new cycle. The coupling between the three layers is achieved solely through the aforementioned explicit data structure; therefore, the visual recognition method, planner type, and force acquisition method can all be replaced as described in other feasible solutions without affecting the overall logic.

[0073] This invention proposes a feeding trajectory planning architecture that integrates a task state machine, phased constraint injection, and sensor event gating, to replace the single-segment point-to-point planning and the architecture of unified constraints / unified speed throughout the process. The architecture of this invention is specifically designed to improve the sampling efficiency and failure rate of constraint planning, as well as the safety of near-human operation, so that the costs of attitude constraints and speed limits are only incurred at the necessary stages.

[0074] This invention designs specific node-level data processing links for forward kinematics, tilt angle verification, cone boundary projection, and collision verification in the extended RRT of the transfer section, namely steps S201-S209 in the implementation. It also designs specific data structures and judgment processes for mouth position orientation ring buffer filtering and dual threshold (d1 / d2) displacement determination, namely steps S401-S404 in the implementation, to support low-time adaptation to dynamic mouth targets.

[0075] This invention defines the two sensing signals of food at the fork confidence and the contact force norm as the standard gating interface of the planning state machine, namely the entry gating / process gating, forming a specific interaction protocol between the three layers of perception, planning and execution, namely steps S501-S505 in the implementation method; and through the trajectory pre-buffering mechanism, it ensures that the force limit response delay does not exceed one control cycle.

[0076] This invention resolves the contradiction between satisfying the attitude constraints required for spill prevention / safety and the efficiency of sampling-based planning through phased injection of constraints; This invention resolves the contradiction between the real-time performance of dynamic mouth target adaptation and the high time consumption of global replanning by using a dual-threshold triggered residual segment replanning. This invention resolves the conflict between near-face safety speed limits and feeding cycle efficiency through segmented speed modulation based on human body distance.

[0077] The upper bound of the gating response delay in this invention is designed according to the FR5 control cycle (8ms); the force feedback callback adopts independent high-priority scheduling to match the sensor sampling rate; the same software interface is compatible with Gazebo simulation and FR5 real machine, which facilitates verification and deployment.

[0078] A trajectory planning system for a bed-assisted feeding robotic arm, the system comprising: a sensing access module, which subscribes to topics such as mouth pose, food in fork state and confidence, and contact force, filters the mouth pose and maintains a MouthState circular buffer, and generates and outputs GateState; The task state machine module maintains four states: food intake, transfer, delivery, and retreat. It performs state transitions based on the entrygate and exitvents of each stage and sends the corresponding PhaseConfig to the motion planning module when a state is entered. The phase constraint configuration module loads and parses the constraint sets, target tolerances, time budgets, and speed scaling factors for each phase from the configuration file, and converts them into constraint and parameter fields in the planning request. The target generation module executes steps S301-S305, which calculate the proximity axis and the pre-delivery target area based on the real-time pose of the head and refreshes it periodically. The motion planning module performs unconstrained RRT-Connect planning, constrained RRT planning with inclination cone constraints (steps S201-S209), and linear Cartesian planning, and completes time parameterization. The replanning trigger module executes steps S401-S404, including double threshold displacement determination, target translation correction, and replanning scheduling of the remaining segment. The safety gating and rollback module executes steps S501-S505, monitors the contact force norm, and cancels the current execution and sends out the pre-cached rollback trajectory when the limit is exceeded. The execution interface module interacts with the FR5 robotic arm controller or simulation controller through the FollowJointTrajectory motion interface and provides feedback on the execution status.

[0079] In the feeding embodiment of the nursing turning bed: the robotic arm adopts the Fao FR5 six-degree-of-freedom collaborative robotic arm (rated load 5kg), with a fork-shaped cutlery and wrist force sensor added to the end; the software environment is Ubuntu 22.04, ROS 2 Humble and MoveIt2, and the underlying planner is OMPL RRT-Connect; the simulation verification environment is Gazebo, and the scene includes a feeding table, a plate, food models (food1, food2), a nursing turning bed, human torso and head models (humantorso, humanhead) and mouth target markers. Parameter values: θmax=15°, doffset=0.08m, d1=0.01m, d2=0.03m, cth=0.8, Fmax=1.5N, vsafe=0.05m / s; initial planning time budget for feeding / transfer is 5s, and the budget for replanning the remaining segments is 0.5s; velscaling at the far end of the transfer segment is 0.3, and at the near end is 0.1; mouth pose update is 30Hz, force sampling simulation is 100Hz (1kHz on the actual device), and FR5 control cycle is 8ms; MouthState ring buffer capacity is N=15. During actual device deployment, the mouth pose is obtained by an RGB-D camera (such as an Intel RealSense D435i) combined with facial landmark detection and published via / tf.

[0080] Other alternatives: The sampling planner, RRT-Connect, can be replaced with variants such as RRT*, Informed RRT*, BIT*, etc., while the node verification / projection process for anti-spraying constraints (steps S204-S206) remains unchanged; attitude constraints can also be implemented by OMPL constrained manifold planning (projection method), or by a trajectory optimizer (such as TrajOpt, STOMP) to perform post-optimization on the initial path to satisfy the constraints. The contact force acquisition can be achieved by replacing the wrist force sensor with an external force estimation based on joint current, while the gating logic (steps S503-S504) remains unchanged. The mouth pose perception and mouth marking can be replaced by facial key point detection, ArUco visual marking or depth point cloud registration. The robotic arm is not limited to FR5, and the method can be adapted to other six / seven-DOF collaborative robotic arms; when using a seven-DOF robotic arm, redundant degrees of freedom can be further utilized to optimize the tilt angle of the tableware in zero space. The tableware, forks can be replaced with spoons; spoons are more sensitive to tilt angles, so θmax should be a smaller value (e.g., 8°); The remaining segment replanning can be extended to local target tracking based on model predictive control (MPC) and combined with the dual threshold triggering mechanism of the present invention; The segmented speed modulation can continuously modulate the path speed using the human distance field calculated offline, replacing the two-stage segmented scheme.

Claims

1. A method for trajectory planning of a bed-assisted feeding robotic arm based on staged-constrained RRT, characterized in that, The method includes the following steps: Step 1: Decompose a feeding task into four stages: feeding, transfer, delivery, and withdrawal. These stages are managed by a task state machine. Stage switching is triggered by sensor event gating rather than a fixed sequence: the switch from feeding to transfer is triggered when the food confidence at the fork exceeds a threshold, and the switch from delivery to withdrawal is triggered by arrival determination or contact force exceeding the limit. Step 2: When the state machine enters each stage, the independent constraint set, target tolerance, planning time budget and speed limit for that stage are injected into the RRT planner; among them, no end-effector attitude constraint is applied to the feeding stage, and the target pose is determined by the piercing direction; the transfer stage is subject to end-effector tilt cone constraint; the delivery stage is limited to performing restricted motion within the approach corridor in front of the mouth. Step 3: Based on the real-time head pose of the supine recipient, calculate the approach axis in the head coordinate system to adapt to the supine / lateral posture, and generate the pre-delivery target as the pose along the approach axis at a fixed offset distance from the mouth. The pre-delivery pose is updated in real time with the head pose.

2. The trajectory planning method of claim 1, wherein, The end-of-transfer section tilt cone constraint in step 2 is implemented through node-level verification using an extended RRT tree. Specifically, Calculate the forward kinematics of the candidate nodes to obtain the tableware coordinate system orientation R(qnew), and calculate the angle between the vertical reference axis zt of the tableware and the opposite direction of gravity -g: θ(qnew)=arccos((R(qnew)·zt)·(-g)) If θ≤θmax, the node is accepted into the tree; otherwise, the pose is rotated and projected around the horizontal axis of the allowable cone boundary. The inverse kinematics of the projected end pose is used to obtain the corrected configuration. If the inverse solution exists and the joint displacement between the nearest neighbor node is less than the threshold, the corrected configuration is used as a candidate node; otherwise, this expansion is discarded. Collision checks are performed point by point on the interpolated edge paths from candidate nodes to their nearest neighbors, and the paths are passed through the last-in tree. After a node enters the tree, it attempts to connect with the target region G. The connection segment also performs the above tilt angle check and collision check. If the connection is successful, the path is backtracked, smoothed by a shortcut, and then output. The smoothed path is then rechecked point by point for θ≤θmax.

3. The trajectory planning method of claim 1, wherein, During execution, the mouth position is continuously monitored, and processing is performed according to a dual threshold. Execution is not interrupted when the mouth displacement Δp < the first threshold d1; When d1≤Δp<second threshold d2, only the target region G is translated and updated. If the current trajectory endpoint still falls within the updated G, the process continues. When Δp≥d2, the current execution is stopped. The current joint state of the robotic arm is taken as the new starting point and the updated target area G is taken as the end point. Within the limited replanning time budget, only the remaining road segments are replanned.

4. The trajectory planning method according to claim 1, characterized in that, The delivery segment performs a linear Cartesian motion in the reverse direction along the approach axis, and is gated by two signals: Entry Gating: Before entering the delivery segment, the food is checked for its fork state and confidence level. If the confidence level is lower than the threshold cth, entry into the delivery segment is prohibited, and the state machine transitions to re-feeding. Process gating: During execution, the contact force norm is monitored in real time. If it exceeds the safety threshold Fmax, the forward movement is immediately stopped, and the device is retracted in a controlled manner along the same approach axis to the pre-delivery pose. The pullback trajectory is pre-generated and cached when entering the delivery segment, and the time delay from the limit violation determination to the issuance of the pullback action does not exceed one control cycle.

5. The trajectory planning method according to claim 1, characterized in that, Time parameterization is performed on each stage path respectively, and the velocity scaling coefficient of each segment is set according to the minimum distance between the segment and the human bounding volume: a higher velocity scaling is adopted for the transfer segment far away from the human body, and the linear velocity of the end effector is limited below the safety value vsafe after entering the pre-oral area.

6. The trajectory planning method according to claim 2, characterized in that, The target region G is constructed with the pre-delivery pose (ppre, Rpre) as the center, and the position tolerance εp and attitude tolerance εo as the tolerances, for use by the planner, and is refreshed according to the update cycle of the mouth pose.

7. A trajectory planning device for a bed-assisted feeding robotic arm based on phased constraint RRT, deployed in a robotic arm motion planning computer, characterized in that, The device includes: a perception access module, which subscribes to mouth pose, food-on-fork status and confidence, and contact force topics, filters the mouth pose, maintains a circular buffer for MouthState, and generates and outputs GateState; a task state machine module, which maintains four states of foraging, transferring, delivering and retracting, performs state transition according to the entry gating and exit events of each stage, and issues the corresponding PhaseConfig to the motion planning module when entering a state; a stage constraint configuration module, which loads and parses the constraint set, target tolerance, time budget and velocity scaling coefficient of each stage from a configuration file, and converts them into constraint and parameter fields in the planning request; a target generation module, which calculates the approach axis and the pre-delivery target region in the head coordinate system based on the real-time head pose and refreshes them periodically; a motion planning module, which performs unconstrained RRT-Connect planning, constrained RRT planning with inclination cone constraints, and linear Cartesian planning, and completes time parameterization; a replanning triggering module, which performs dual-threshold displacement judgment on mouth pose, target translation correction and residual segment replanning scheduling; a safety gating and retraction module, which monitors the norm of contact force, cancels the current execution and issues the pre-cached retraction trajectory when the norm exceeds the limit; an execution interface module, which interacts with the manipulator controller through the FollowJointTrajectory action interface and feeds back the execution status.

8. The apparatus according to claim 7, characterized in that, When the motion planning module executes constrained RRT planning with inclination cone constraints in the transfer segment, it calculates forward kinematics for each candidate node to obtain the tableware pose, checks that the included angle θ between the vertical reference axis of the tableware and the reverse direction of gravity satisfies θ ≤ θmax; if not, it projects the node onto the allowable cone boundary and then solves inverse kinematics for back-substitution, and adds the corrected configuration to the tree when the inverse solution exists and the joint displacement from the nearest neighbor node is less than the threshold; The replanning triggering module reads the filtered mouth position from the MouthState circular buffer at 30Hz, and calculates the displacement Δp relative to the reference position at the current planning time of the segment: No processing is performed when Δp < d1; Only the target region G is translated when d1 ≤ Δp < d2; When Δp ≥ d2, the current execution is canceled, and the residual segment is replanned within the replanning time budget with the current joint state as the starting point and the updated target region G as the end point.

9. The apparatus according to claim 2, characterized in that, Before entering the delivery segment, the safety gating and retraction module checks the food-on-fork confidence in the GateState, and prohibits entering the delivery segment and notifies the task state machine to switch back to the foraging state when the confidence is lower than cth; During the execution of the delivery segment, the contact force norm is subscribed to in the independent high-priority callback group. If the limit is exceeded, the current execution is immediately canceled and the pre-cached rollback trajectory is issued. The delay from the limit exceeding judgment to the rollback issuance does not exceed one control cycle. The MouthState structure maintained by the perception access module includes Tmouth, stamp, vest, and a circular buffer of capacity N called recentposes, which is used for median filtering and dual-threshold shift determination.

10. A trajectory planning system for a bed-assisted feeding robotic arm, characterized in that, The system includes, The perception layer is configured to collect information such as mouth position, food status at the fork and confidence level, and wrist contact force. The planning and decision-making layer is configured to run the trajectory planning device according to any one of claims 7-9 and output the joint trajectories at each stage; The execution layer is configured to receive the joint trajectory and control the robotic arm to execute it. The perception layer, planning and decision-making layer, and execution layer communicate with each other through the ROS2 topic and action interface, and force feedback callbacks are placed in an independent high-priority callback group.

Images