Research on the Safety Design and Trajectory Planning for a New Dual Upper Limb Rehabilitation Robot

Abstract

The increasing utilization of upper limb rehabilitation robots in rehabilitation therapy has brought to light significant safety concerns regarding their mechanical structures and control systems. This study focuses on a six degrees of freedom (DOF) upper limb rehabilitation robot, which has been designed with an emphasis on safety through careful consideration of its mechanical structure and trajectory planning. Various parameter schemes for the shoulder joint angles were proposed, and the robotic arm’s structure was developed by analyzing the spatial motion trajectories of the shoulder joint motor. This design successfully achieves the objective of minimizing the installation space while maximizing the range of motion. Additionally, an enhanced artificial field method is introduced to facilitate the planning of self-collision avoidance trajectories for dual-arm movements. This approach effectively mitigates the risk of collisions between the robotic arm and the human body, as well as between the two robotic arms, during movement. The efficacy of this method has been validated through experimental testing.

1. Introduction

As of the present date, the global population of stroke patients aged 40 and older has reached approximately 170.4 million [1], with an estimated mortality rate of 1.96 million individuals [2]. Among the survivors, approximately 75% have experienced disabilities [3,4] and a significant loss of physical mobility [5]. Concurrently, the annual incidence of new cases of traumatic spinal cord injury ranges from 100,000 to 140,000, contributing to a cumulative total of over 2 million affected individuals [6,7]. Consequently, the implementation of rehabilitation robots is essential for enhancing the therapeutic outcomes for patients undergoing rehabilitation [8].

Currently, researchers both domestically and internationally have developed over 100 types of upper limb rehabilitation robots, highlighting the growing significance of the safety design in terms of rehabilitation robot systems. The Fraunhofer Institute in Germany has created STRING-MAN, a rope-driven parallel rehabilitation robot that utilizes seven ropes to control the patient’s torso, while additional ropes are employed to measure the torso motion [9]. Meanwhile, the University of Texas has developed the Harmony upper limb rehabilitation robot, which incorporates the glenohumeral joint rhythm phenomenon characteristic of the human shoulder joint. This robot features a shoulder with five degrees of freedom (DOF), enabling simultaneous training of both arms, enhancing limb coordination in hemiplegic patients, and preventing collisions between the robot and the human body through a parallel four-sided mechanism [10]. Zhao Xiaofeng from the Huazhong University of Science and Technology has introduced a two-arm robotic system that utilizes nonlinear model predictive cooperative control. This system is capable of executing various training modalities, including tracking, impedance, force control, and motion synchronization, by modifying the cooperative index. While it demonstrates versatility for a range of complex operational tasks, it is important to note that the rehabilitation training process is intricate and does not adequately address the safety considerations associated with the two-arm robotic system [11]. Chen Li introduced a two-arm participation–man–machine collaboration approach aimed at enhancing upper limb rehabilitation exercises for individuals with hemiplegia. This method integrates the healthy and affected limbs with a robotic system, employing a tailored control strategy to facilitate the patient’s specific movement capabilities. This integration not only enhances patient engagement during rehabilitation but also improves the precision of the movement estimation for the healthy limb, thereby optimizing the trajectory of the rehabilitation exercises. However, challenges may arise due to the substantial size of the robotic structure, which could lead to potential collisions between the robotic arms and between the arms and the patient [12]. Nanjing University of Science and Technology has engineered a 14-degree-of-freedom (DOF) double-arm exoskeleton rehabilitation robot. This design incorporates an angle for shoulder joint abduction/adduction, positioning the abduction/adduction freedom axis at an elevation of 30 degrees. This configuration aims to mitigate the mechanical complications arising from coplanarity between the shoulder joint and the forearm and wrist joints. Nonetheless, a notable limitation of this design is the restricted operational workspace of the manipulator, indicating a need for enhanced flexibility [13,14]. Diao Hui enhanced the artificial potential field method for coordinating two-arm robots, successfully achieving collision-free trajectory planning for both arms by determining the minimum distance between various components of the robot and employing a behavior-based control framework. It is suggested that the process of calculating obstacle avoidance path planning for dual-arm systems in the context of collision detection is complex and that existing collision detection algorithms are inadequate [15]. Liu Zhihui et al. from Donghua University studied the exoskeleton upper limb rehabilitation robot from the perspective of human factors engineering. Mechanical locking devices were added at each joint to limit the rotation angle within the joint limit range, and emergency switches were installed [16]. Wang Xiao et al. proposed a collision response method based on admittance control, which has good applicability for physical collisions between the end of a robotic arm and the operator [17].

However, the above research has the following shortcomings: (1) lack of in-depth research on protection strategies, resulting in secondary harm to patients; (2) rehabilitation training is cumbersome, large in volume, or comes at the expense of working space to achieve the goal of a small mechanical structure, and the applicable population is limited; and (3) the collision corresponding strategy is single, which cannot fully guarantee the man–machine safety. In this paper, a two-arm upper limb rehabilitation robot has been developed. The protection strategy in the process of human–computer interaction is deeply studied. The mechanical mechanism that satisfies the “small installation space-large activity space” is designed, and the two-arm collision avoidance trajectory is planned in real time. The feasibility and safety verification are carried out through simulation and experiment, so as to be better applied to the field of rehabilitation medicine.

2. Design of Upper Limb Rehabilitation Robot

The dysfunction impairment patients targeted by upper limb rehabilitation robots mainly come from the elderly, stroke, shoulder periarthritis, and other populations [18,19,20]. According to the range of motion of male joints aged 18–60, the range of motion of human joints, muscle strength, motor coordination and other activity indicators are taken into account. The joint range of motion indicators pertinent to the design of an upper limb rehabilitation robot, as derived from the combined data of males and females aged 18 to 60, are presented in

Table 1. The index parameters of each joint of the upper limb rehabilitation robot.

Name of Joint	Degree of Freedom	Reference Datum	The Range of Human Mobility	Design Consideration Range
Shoulder joint	Buckling/Stretching	Sagittal plane	$0^{\circ }~180^{\circ }$	$0^{\circ }~150^{\circ }$
	Abduction/Adduction	Coronal plane	$0^{\circ }~180^{\circ }$	$0^{\circ }~150^{\circ }$
	Internal/External rotation	Horizontal axis	$-{90}^{\circ }~90^{\circ }$	$-{80}^{\circ }{~80}^{\circ }$
Elbow joint	Buckling/Stretching	Sagittal plane	$0^{\circ }{~145}^{\circ }$	$0^{\circ }{~120}^{\circ }$
Forearm	Pronation/Supination	Horizontal axis	$-{90}^{\circ }~{90}^{\circ }$	$-{90}^{\circ }~{90}^{\circ }$
Wrist joint	Palmar flexion/Dorsiflexion	Vertical axis	${35}^{\circ }{~60}^{\circ }$	${30}^{\circ }{~60}^{\circ }$

. The comprehensive structural design of the rehabilitation robot intended for both arms/upper limbs is illustrated in Figure 1. In this design, the shoulder joint is characterized by three degrees of freedom, the elbow joint by one degree of freedom, and the wrist joint by two degrees of freedom. This configuration facilitates a range of movements, including shoulder joint flexion and extension, adduction and abduction, internal and external rotation, elbow joint flexion and extension, pronation and supination, as well as wrist palmar flexion and dorsiflexion.

The joint drive module of the rehabilitation robot is mainly composed of MAXON’s EC-flat motor and Laifu integrated harmonic reducer, as shown in Figure 2, The torque exhibited by each joint varies. The design of the entire series of robotic arms is informed by the performance parameters of each joint, thereby enhancing the overall stability and applicability of the robotic arm as a whole. To ensure that the mechanical arm does not produce any movement after power failure or emergency stop, the drive module is equipped with a BXR LE non-excitation brake between the motor and the reducer to protect the safety of the trainer. When the power is accidentally lost, the non-excitation brake will lock the reducer sleeve in time to prevent the drive module from continuing to output power, ensuring the safety and reliability of the training process. Given the unique characteristics of the range of motion and dimensions of human joints, as well as the diverse contexts and user requirements in human–computer interaction, it is posited that a mismatch between the range of motion of a robotic arm joint and the corresponding parameters of the patient may lead to diminished comfort during use. In more severe instances, such discrepancies could potentially result in secondary injuries. Therefore, the mechanical limit is designed on the joint drive module, and by processing the limited groove on the output rotating bracket, the movement of the limit block installed on the fixed bracket of the drive module is limited, so as to limit the range of motion of the joint of the robotic arm. Moreover, the rehabilitation robot is extensively utilized in the realm of human–computer interaction to enhance safety, diversity, and user comfort.

The existing upper limb rehabilitation robot has a large configuration, and it cannot be developed into a wearable robot from the perspective of kinetic energy and energy consumption [21,22,23]. The greatest difficulty in terms of the mechanical layout is the design of the shoulder joint. The shoulder joint is a composite joint, which is the most frequently used, most active and most diverse joint of the human body [24]. The activity space of the upper limb of the human body largely depends on the range of motion of the shoulder joint. However, the shoulder joint space of the human body is small, and the small volume of the shoulder joint activity structure makes it easy to collide with the human body, especially the head [25]. Based on this pain point problem, this paper establishes the human upper limb shoulder joint skeletal model as shown in Figure 3, and the relative position of the 3 DOF shoulder joint of the rehabilitation robot and the human body in the process of motion is analyzed. When the human arm is naturally drooping, the angle between the shoulder joint motor $R_1$ and $R_2$ is ${\theta }_1$, the angle between $R_2$ and $R_3$ is ${\theta }_2$, and the angle between $R_1$ and $R_3$ is ${\theta }_3$.

According to the human body size of Chinese adults in GB 1000-88 [26], the median body size for males aged 18 to 60 years is utilized, where the height is 223 mm, and the shoulder width is 380 mm, it can be seen that the human body center to $O$ point is a = 190 mm, the neck width is b = 50 mm, and the height is c = 60 mm. The equivalent mechanism model of the human shoulder joint with three degrees of freedom is analyzed, as shown in Figure 3. The axes of the three shoulder joint motors are orthogonal to one point. At this point we set up a fixed coordinate system $O_w-X_w{Y}_w{Z}_w$, setting the parameters as follows: face coordinate is $P_0$, radius of the human head ball is $r_0$, center of the lower neck is $P_1$, radius is $r_1$, center of the upper neck is $P_2$, radius is $r_2$, $R_2$ is the distance between the shoulder joint and the axis intersection is $r_3$; and the data are shown in

Table 2. Parameter list.

Parameter	$P_0$	$P_1$	$P_2$	$r_0$	$r_1$	$r_2$	$r_3$
Numerical Value	(−190,0,150)	(−190,0,0)	(−190,0,60)	135	25	25	274

.

Based on the safety consideration of engineering practice [27], it can be seen that in the process of movement, the relative position of the $R_1$ motor and human body is fixed, and there is no collision between $R_1$ and the human head. When the $R_1$ motor runs, it drives $R_2$ to perform spatial motion. At this time, $R_2$ may collide with the human head. During rehabilitation exercise, the shoulder joint motor $R_3$ is always located on the outer side of the human arm, making it less likely to collide with the human head. To prevent collisions between the shoulder joint motor $R_2$ and the human body, the angle ${\theta }_1$, ${\theta }_2$ and ${\theta }_3$ of the three motors can be changed. The shoulder joint motor $R_3$ does not affect the motion space of $R_2$, so $R_3$ coincides with the $O_w-X_w{Y}_w{Z}_w$ axis of the world coordinate system $x_w$. The shoulder joint motor $R_1$ is the first motor of the base, and the power is large; when the installation angle is 90°, the power is minimal. Therefore, this paper only considers the installation angle between the shoulder joint motor and the other motors. Based on engineering considerations, the angle parameters are set as 90°, 60° and 45°, respectively. Combined with the numerical calculation and analysis, the human head is assumed to be a sphere, and the equation for the sphere is as follows:

$${\left(x_0+190\right)}^2+{y_0}^2+{\left(z_0-150\right)}^2={r_0}^2$$

(1)

In order to easily understand and calculate, the motor $R_2$ can be seen as a sphere rotating around the intersection point $O_w$ of the shoulder joint axis, and the sphere equation is as follows:

$$x^2+y^2+y^2={r_3}^2$$

(2)

When the shoulder joint $R_1$ motor is moving, the $R_2$ is driven to space motion and the path of the motion is a plane perpendicular to the $X_w{O}_w{Y}_w$. The space plane path equation is $y=-r_3\cos \theta (\theta =45°,60°)$, so the space plane path equation and the sphere of the $R_2$ motor motion is obtained, and the section is a circle, which can determine whether the shoulder joint collides with the human head. The result is shown in Figure 4.

It can be seen from the figure that when ${\theta }_1={\theta }_2=90°$, $R_2$ will collide with the human head. When ${\theta }_1={\theta }_2=60°$, and ${\theta }_1={\theta }_2=45°$, there will be no collision. Then, further research will be conducted on the workspace of the shoulder joint, through the MATLAB Robot Toolbox simulation analysis, when ${\theta }_1={\theta }_2=45°$, ${\theta }_3=90°$, ${\theta }_1={\theta }_2=60°$, and ${\theta }_3=90°$, for the comparison between the three degrees of freedom range of motion of the shoulder joint and the range of motion of the rehabilitation robot’s shoulder joint that is obtained, as shown in

Table 3. Range of motion table of the shoulder joint.

Shoulder Joint Freedom	Range of Activity	${\mathit{\theta }}_1$ = ${\mathit{\theta }}_2$ = 45° ${\mathit{\theta }}_3$ = 90°	${\mathit{\theta }}_1$ = ${\mathit{\theta }}_2$ = 60° ${\mathit{\theta }}_3$ = 90°	Design Consideration Range
Buckling/Stretching	$0^{\circ }~180^{\circ }$	$0^{\circ }~180^{\circ }$	$0^{\circ }{~175}^{\circ }$	$0^{\circ }~150^{\circ }$
Abduction/Adduction	$0^{\circ }~180^{\circ }$	$0^{\circ }~100{.12}^{\circ }$	$0^{\circ }~113{.29}^{\circ }$	$0^{\circ }~150^{\circ }$
Internal/External rotation	$-{90}^{\circ }~90^{\circ }$	$-{90}^{\circ }~0^{\circ }$	$-{90}^{\circ }~60{.30}^{\circ }$	$-{80}^{\circ }{~80}^{\circ }$

.

By comparing the range of motion of the shoulder joint and the motion space of the manipulator in these two cases, it shows that when the shoulder joint motor ${\theta }_1={\theta }_2=60°$, ${\theta }_3=90°$, the motion space of the robot arm is larger and there is no collision with the human head during movement.

3. Analysis of Safe Distance between Mechanical Arm and Human Body

When the upper limb rehabilitation robot drives the human body for rehabilitation training, the mechanical arm may collide with the human body, and the upper limbs of the human body driven by the two arms may also collide with each other. Therefore, safety design is necessary for the trajectory planning of the manipulator. The distance between the robotic arm and the human body or another robotic arm, also known as the safety distance, is the most important motion parameter. Once the safety distance exceeds the set threshold, the warning mechanism and the emergency stop mechanism will be triggered.

In order to conveniently judge whether the spatial distance between each joint or connecting rod of the robotic arm and the sitting–standing human body, and whether the spatial distance between the two robotic arms meets the requirements of the safe area, the bounding box envelope method is used to simplify the mechanism model of the upper limb rehabilitation robot [28,29]. By employing an optimization approach for the parameters of the mechanism, it is possible to prevent collisions between the robot’s shoulder joint and the human body. Additionally, the dimensions of the shoulder joints on both arms are minimized, thereby eliminating any potential interference between them. Consequently, the two segments, namely the upper arm section and the forearm section, can be simplified. The upper arm section is defined as the segment extending from the shoulder motor to the elbow joint motor, while the forearm section is characterized as the segment between the elbow joint and the wrist joint motor. For the purpose of simplification, a “capsule body” bounding box is utilized, as illustrated in Figure 5 and Figure 6.

In the figure, the two ends of the capsule body in the upper arm section are point P and point Q, respectively, where point P is the location point of the shoulder motor 3 and point Q is the location point of the elbow motor. The two ends of the capsule body in the forearm section are point Q and point W, respectively, where point Q is the location point of the elbow motor and point W is the location point of the wrist motor. The capsule body bounding box is bound with the upper arm section and the forearm section, and it moves with the manipulator. There is a certain size of excess space in the bounding box, which provides secondary protection for the human arms and robotic arms, improving the system safety.

The collision detection of the upper limb rehabilitation robot mainly detects whether the minimum distance between the robot arm and the patient is less than the set safe distance when the patient performs rehabilitation training to determine whether there is a collision. That is:

$$\left\{\begin{array}{l}{d}_{safe}>d_{min} \left(\mathrm{There}\mathrm{was}\mathrm{no}\mathrm{collision}\right)\\ d_{safe}\le d_{min} \left(\mathrm{Collision}\mathrm{occurs}\right)\end{array}\right.$$

(3)

In the formula: $d_{safe}$—the minimum safe distance between the two enclosing boxes; and $d_{min}$—the minimum distance between two bounding boxes.

In this paper, the collision detection of the sphere bounding box and capsule bounding box is needed. Before calculating the minimum distance, the secondary projection method is used to detect the collision of the bounding box preliminarily, so as to reduce unnecessary complex operations, such as square root and multiplication, and improve the efficiency of the collision detection. As shown in Figure 7, taking the sphere bounding box as an example, the two bounding boxes are projected onto three coordinate planes, respectively, and the collision is judged according to the intersection of the two bounding boxes on the three projection planes. If the two bounding boxes do not intersect on every projection plane, it is proved that there is no collision between the two bounding boxes. On the contrary, it is necessary to further judge the collision situation of the group of bounding boxes by calculating the minimum distance.

3.1. Analysis of Safe Distance between Mechanical Arm and Human Body in Standing Position

It is necessary to determine whether the spatial distance between the joints and connecting rods of the robot arm and the human body in the standing position meets the requirements of the safe area. According to the size of patients with different heights and weights, corresponding safety distances need to be set. This article takes the example of a cylinder with an equivalent length of 500 mm for the human torso and two cylinders with a diameter of 200 mm that simplify the legs to be fixed and tangent to the upper body cylinder. At the same time, it is assumed that the robot arm of the upper limb rehabilitation robot is a cylinder and the joint is a sphere.

The fixed coordinate system $O_b-x_b{y}_b{z}_b$ of the human body is set up, where $O={\left[x_b{y}_b{z}_b\right]}^T$ is the position of the center of mass of the human body, and the center of mass of the ith joint of the upper limb of the human body is $M^i={\left[x_i,y_i,z_i\right]}^T$. Because the robot arm is attached to the lateral side of the upper limb of the human body, the corresponding i the joint after offset is $\overline{M^i}={\left[\overline{x_i},\overline{y_i},\overline{z_i}\right]}^T$. Suppose $\overline{N^k}$ is the position of a point N on the $i$-link of the robot arm, then it can be known.

$$\overline{N^k}=\overline{M^{i-1}}+s(\overline{M^i}-\overline{M^{i-1}})\left(i=5,6,7,8\right)$$

(4)

The distance between the projection position $\overline{{M^i}_{xy}}$ and point $O_{xy}$ in the $x_b{o}_b{y}_b$ plane is obtained by projecting the M, N and O points to the $x_b{o}_b{y}_b$ plane:

$$D_{\overline{M_{xy}}{O}_{xy}}^i=\sqrt{{\left[\left(\overline{{M_{xy}}^i}-O_{xy}\right)\right]}^T\left[\left(\overline{{M_{xy}}^i}-O_{xy}\right)\right]}\left(i=5,6,7,8\right)$$

(5)

The distance from each joint of the robot arm to the human body in a standing position can be known, and the distance to the fixed cylinder is as follows:

$$D_{\overline{M_{xy}}{O}_{xy}}^i=\sqrt{{\left[\left(\overline{{M_{xy}}^i}-O_{xy}\right)\right]}^T\left[\left(\overline{{M_{xy}}^i}-O_{xy}\right)\right]}-250\left(i=5,6,7,8\right)$$

(6)

In order to avoid a collision between the robot arm and the patient, the two-point distance $D_{\overline{M_{xy}}{O}_{xy}}^i$ and $D_{\overline{N_{xy}}{O}_{xy}}^i$ of the rehabilitation robot arm in the rehabilitation training should always be greater than the corresponding connecting rod radius and joint radius. As shown in Figure 8, the minimum value of the distance between each link of the robot arm and the distance between the connecting rod and the sitting human body after projection in the $x_b{o}_b{y}_b$ plane is $D^i$:

$$D^i=D_{\overline{N_{xy}}{O}_{xy}}^i,s\in \left[0,1\right]\left(i=5,6,7,8\right)$$

(7)

3.2. Analysis of Safe Distance between Mechanical Arm and Human Body in Sitting Position

The upper limb rehabilitation robot designed in this paper can be trained in standing and sitting postures. Since the shape of the human body will change in the sitting posture, which further reduces the safe movement range of the upper limb rehabilitation robot arm, the above-mentioned safe distance calculation method for preventing a collision is no longer applicable to the sitting posture training state. In order to determine whether the spatial distance between each joint, as well as the connecting rod of the manipulator and the human body, is in a safe motion area, the upper body in the sitting position is simplified into a fixed cylinder with a diameter of 500 mm, and the legs are simplified into two fixed cylinders with a diameter of 200 mm, which are tangent to the upper body cylinder. At the same time, it is assumed that the manipulator of the upper limb rehabilitation robot is a cylinder and each joint is a sphere.

Suppose that the upper body cylinder of the human body is $B^1={\left[x_1,y_1,z_1\right]}^T$, the center of mass of the cylinder of the human legs is $B^2={\left[x_2,y_2,z_2\right]}^T$ and $B^3={\left[x_3,y_3,z_3\right]}^T$, and the gth joint of the human upper limb is $\overline{P^g}={\left[\overline{x_g},\overline{y_g},\overline{z_g}\right]}^T$ in the sitting position. It is assumed that $\overline{Q^g}$ is the position vector of a point on the gth connecting rod of the robot arm.

Points $\overline{P^g}$, $\overline{Q^g}$ and $B^1$, $B^2$, $B^3$ are projected onto the $x_b{o}_b{y}_b$ plane to obtain point $\overline{{P_{xy}}^g}$, point $\overline{{Q_{xy}}^g}$ and point ${\mathrm{B}}_{xy}^1$, ${\mathrm{B}}_{xy}^2$, ${\mathrm{B}}_{xy}^3$ in the $x_b{o}_b{y}_b$ plane, respectively, so that the spatial distance is simplified into the distance between two points in the plane, and the distance between point $\overline{{P_{xy}}^g}$ and point ${\mathrm{B}}_{xy}^1$ can be calculated as follows:

$$D_{\overline{P_{xy}}{B}_{xy}^1}^g=\sqrt{{\left[\left(\overline{P_{xy}^g}-{\mathrm{B}}_{xy}^1\right)\right]}^T\left[\left(\overline{P_{xy}^g}-{\mathrm{B}}_{xy}^1\right)\right]}\left(g=5,6,7,8\right)$$

(8)

It can be seen that the distance from each joint of the robot arm to the upper body cylinder in the sitting position is as follows:

$$D_{\overline{P_{xy}}{B}_{xy}^1}^g=\sqrt{{\left[\left(\overline{P_{xy}^g}-{\mathrm{B}}_{xy}^1\right)\right]}^T\left[\left(\overline{P_{xy}^g}-{\mathrm{B}}_{xy}^1\right)\right]}-250\left(g=5,6,7,8\right)$$

(9)

Similarly, the distance from a point on each link of the robot arm to the upper body cylinder in the sitting position is as follows:

$$D_{\overline{Q_{xy}}{B}_{xy}^1}^g=\sqrt{{\left[\left(\overline{Q_{xy}^g}-{\mathrm{B}}_{xy}^1\right)\right]}^T\left[\left(\overline{Q_{xy}^g}-{\mathrm{B}}_{xy}^1\right)\right]}-250\left(g=5,6,7,8\right)$$

(10)

Similarly, the distances from each joint of the robot arm to the lower body cylinder in the sitting position of the human body can be obtained as follows:

$$D_{\overline{P_{xy}}{B}_{xy}^2}^g=\sqrt{{\left[\left(\overline{P_{xy}^g}-{\mathrm{B}}_{xy}^2\right)\right]}^T\left[\left(\overline{P_{xy}^g}-{\mathrm{B}}_{xy}^2\right)\right]}-100\left(g=5,6,7,8\right)$$

(11)

$$D_{\overline{P_{xy}}{B}_{xy}^3}^g=\sqrt{{\left[\left(\overline{P_{xy}^g}-{\mathrm{B}}_{xy}^3\right)\right]}^T\left[\left(\overline{P_{xy}^g}-{\mathrm{B}}_{xy}^3\right)\right]}-100\left(g=5,6,7,8\right)$$

(12)

The distance from the point on each link of the robot arm to the lower body cylinder in the sitting position of the human body is as follows:

$$D_{\overline{Q_{xy}}{B}_{xy}^2}^g=\sqrt{{\left[\left(\overline{Q_{xy}^g}-{\mathrm{B}}_{xy}^2\right)\right]}^T\left[\left(\overline{Q_{xy}^g}-{\mathrm{B}}_{xy}^2\right)\right]}-100\left(g=5,6,7,8\right)$$

(13)

$$D_{\overline{Q_{xy}}{B}_{xy}^3}^g=\sqrt{{\left[\left(\overline{Q_{xy}^g}-{\mathrm{B}}_{xy}^3\right)\right]}^T\left[\left(\overline{Q_{xy}^g}-{\mathrm{B}}_{xy}^3\right)\right]}-100\left(g=5,6,7,8\right)$$

(14)

In order to avoid a collision between the rehabilitation robot arm and the patient, the distance between the joints and the connecting rod of the rehabilitation robot arm and the human body in the sitting position should be always greater than the corresponding connecting rod radius and joint radius during the rehabilitation training. As shown in Figure 9, the minimum value of the distance between the joints and the connecting rod of the robot arm and the sitting position of the human body after projection in the $x_b{o}_b{y}_b$ plane is $D^g$:

$$D^g=Min\left(D_{\overline{Q_{xy}}{B}_{xy}^1}^g,D_{\overline{Q_{xy}}{B}_{xy}^2}^g,D_{\overline{Q_{xy}}{B}_{xy}^3}^g\right),t=\left[0,1\right]\left(g=5,6,7,8\right)$$

(15)

The safe distance between the robot arm and the human body is detected in real time during operation. Once the set threshold is exceeded, the warning mechanism and further emergency stop mechanism will be triggered.

4. Self-Collision Avoidance Trajectory Planning

4.1. Improved Artificial Potential Field

The upper limb rehabilitation robot designed in this paper is a series linkage mechanism. The movement of the robot arm is coupled, and the movement of any joint will affect the pose of its adjacent joints. In addition, when the rehabilitation robot is in operation, there may be a collision between the robot arm and the patient’s body. If the traditional artificial potential field method is used to plan the collision avoidance path of the dual-arm upper limb rehabilitation robot, the path search space is Cartesian space, meaning each planning step needs to solve the inverse solution of the path point through inverse kinematics and screen out the minimum potential energy joint combination value. This not only greatly increases the computational complexity but may also calculate the singular value, resulting in a sudden change in the joint angle, which is not ideal in the practical application of the robot arm obstacle avoidance planning [30,31,32,33].

Therefore, this paper chooses the joint space as the search space of the collision avoidance path, takes the joint angle as the control variable, searches the adjacent joint combination of the current state through the appropriate step size, and selects a set of joint angle values with the smallest potential energy as the next pose of the robot arm. Compared with the traditional artificial potential field method, the path points obtained by searching in the joint space are reachable, and there is no need for an inverse solution, and there will be no singular points, so that the path is continuous, the calculation process is greatly simplified, and the time required for self-avoidance trajectory planning is reduced.

The gravitational potential field function of the improved robot arm can be calculated according to the angle difference between the current pose of each joint and the target pose:

$$Uatt*={\displaystyle \frac{1}{2}}{k}_a{\displaystyle \sum _{i=1}^6({\theta }_i-{\theta }_{gi})},i=1,2,\cdots ,6$$

(16)

Among them, ${\theta }_i$ is the current angle value of each joint of the robot arm, and ${\theta }_{gi}$ is the target angle value of each joint.

Aiming at the problem that the robot arm cannot reach the target point due to the obstacle, the distance $p(q_i,q_g)$ between the robotic arm and the target point is introduced as an influencing factor in the traditional repulsion field function, so that the repulsion field is more sensitive to the change of distance. At this time, when the robot arm is close to the target point, the repulsion force gradually becomes larger, but as the distance decreases, it is largely driven by the reaction force to push the robot arm. In addition, the absolute safety distance $d_{safe}$ is introduced. When the distance between the two robot arms is less than or equal to $d_{safe}$, the repulsive force is set to a maximum value to ensure the absolute safety of the patient.

At this time, the repulsion field function is expressed as follows:

$$Urep(q_i)=\left\{\begin{array}{ll}M,\hfill & p(q_i,q_{obs})\le d_{safe}\hfill \\ {\displaystyle \frac{1}{2}}{k}_r{({\displaystyle \frac{1}{p(q_i,q_{obsi})}}-{\displaystyle \frac{1}{p_o}})}^{2}p(q_i.q_{gi}),\hfill & d_{safe}<p(q_i,q_{obs})\le p_o\hfill \\ 0,\hfill & p(q_i,q_{obs})>p_o\hfill \end{array}\right.$$

(17)

The repulsive force potential field of the robot arm is as follows:

$$Urep*={\displaystyle \sum _{i=1}^{6}Urep(q_i)}$$

(18)

Then, the corresponding repulsion function becomes the following:

$$\begin{array}{l}Frep(q_i)=-\nabla Urep(q_i)\\ \begin{array}{lll} \hfill & \hfill & \hfill \end{array}=\left\{\begin{array}{ll}M,\hfill & p(q.q_{obs})\le d_{safe}\hfill \\ Frep1+Frep2,\hfill & dsafe<p(q.q_{obs})\le p_o\hfill \\ 0,\hfill & p(q.q_{obs})>p_o\hfill \end{array}\right.\end{array}$$

(19)

In the following formula:

$$\left\{\begin{array}{l}Frep1=k_r({\displaystyle \frac{1}{p(q_i,q_{obsi})}}-{\displaystyle \frac{1}{p_o}}){\displaystyle \frac{p^n(q,q_{gi})}{p^2(q_i,q_{obsi})}}\\ Frep2={\displaystyle \frac{n}{2}}{k}_r{({\displaystyle \frac{1}{p(q_i,q_{obsi})}}-{\displaystyle \frac{1}{p_o}})}^2{p}^{n-1}(q,q_{gi})\end{array}\right.$$

(20)

where the potential energy of the robot is as follows:

$$U*=Uatt*+Urep*$$

(21)

In the work of the dual-arm upper limb rehabilitation robot system, there may be a local minimum value. Therefore, a virtual obstacle is added to break the force balance and avoid the robotic arm falling into the local minimum value.

4.2. The Specific Process of the Algorithm

In this paper, the master–slave path planning method is used to plan the collision avoidance path of the dual-arm upper limb rehabilitation robot. Before the formal planning, in order to ensure the most effective rehabilitation effect, the left and right robotic arms are set according to the rehabilitation needs. When the patient’s left arm is the affected limb and the right arm is the healthy limb, the left arm is set as the main arm and the right arm is set as the slave arm. On the contrary, the right arm is set as the main arm and the left arm is set as the slave arm. When both arms are affected limbs, it can be selected and set according to the actual rehabilitation needs.

When the collision avoidance path planning is carried out, the path planning of the main arm is first carried out. At this time, the real-time pose of the main arm is known, and then the main arm is regarded as a dynamic obstacle and the collision avoidance path planning of the slave arm is carried out according to the real-time pose of the main arm. The specific algorithm flow is shown in Figure 10.

When planning the collision avoidance path, this paper selects the path of the upper limb rehabilitation exercise for the simulation test and verifies the effectiveness of the self-avoidance algorithm by comparing the simulation results’ existence or nonexistence in terms of the self-avoidance algorithm.

4.2.1. Simulation of Non-Self-Avoiding Collision Algorithm

In the experimental simulation of the non-self-avoiding collision algorithm, the left arm is set to move from the left lower initial position $q_{ls}$ = [0° 0° 40° −30° 0° 0° 0°] to the right upper position, and the end position is $q_{lf}$ = [0° −20° 30° −40° 0° 0°]; the right arm moves from the right lower initial position $q_{rs}$ = [0° 0° −60° −45° 0° 0°] to the left upper position, and the end position is $q_{rf}$ = [0° 45° −5° 55° 0° 0°]; and the left and right arms move normally. Assuming that the absolute safe distance $d_{safe}$ is 50 mm, MATLAB is used to simulate the angle changes of each joint of the right arm, as shown in Figure 11a.

The left forearm and the left upper arm are used to detect the collision of the space bounding box of the right forearm and the right upper arm, respectively, and the minimum distance between the left and right robot arm during the current motion is calculated, where ${d_{\min }}^{la-rf}$ is the minimum distance between the left forearm and the right forearm, ${d_{\min }}^{la-ru}$ is the minimum distance between the left forearm and the right upper arm, ${d_{\min }}^{lu-rf}$ is the minimum distance between the left upper arm and the right forearm, and ${d_{\min }}^{lu-ru}$ is the minimum distance between the left upper arm and the right upper arm, as shown in Figure 11b.

As shown in Figure 12, the minimum distance between the left forearm and the right forearm is less than the absolute safe distance $d_{safe}$, and the collision between the two arms is determined. It is proved that in the absence of a self-collision avoidance algorithm, the two arms cannot achieve self-collision avoidance path planning by adjusting the angle.

4.2.2. Self-Collision Avoidance Algorithm Simulation

In the simulation experiment with the self-collision avoidance algorithm, the left arm is set as the main arm, and its motion path is still moving from the lower left initial pose $q_{ls}$ = [0° −30° 60° −60° 0° 0° 0°] to the upper right, and the end pose is $q_{ls}$ = [0° 0° 40° −30° 0° 0°]; the right arm is the slave arm, which moves from the lower right initial position $q_{ls}$ = [0° −30° 60° −60° 0° 0°] to the upper left, and the end position is $q_{ls}$ = [0° −30° 60° −60° 0° 0°]. The gravitational gain coefficient $k_a$ is 10, the repulsive gain coefficient $k_r$ is 1000, the absolute safe distance $d_{safe}$ is 50 mm, the obstacle repulsive e radius $p_o$ is 60 mm, the main arm (left arm) moves normally, and the right arm avoids a collision by the self-avoidance algorithm. The simulation experiment is carried out by using MATLAB. The simulation results and the changes in the joint angles of the arm (right arm) are as shown in Figure 13a. Similarly, collision detection is performed on the left forearm and the right forearm, the left big arm and the right forearm, the right big arm and the left forearm, and the left big arm and the right big arm. The minimum distance between the rods of the left and right robot arm is shown in Figure 13b.

The simulation of the two arms by MATLAB is shown in Figure 14. After adding the self-collision avoidance algorithm, the secondary robot arm joint angle fluctuates slightly at several points. During the movement of the two arms, the minimum distance between all the rods of the two arms is always greater than the absolute safety distance $d_{safe}$.There is no collision between the two arms during the movement. It is proved that the self-collision avoidance algorithm of the improved artificial potential field method can realize the self-collision path planning of the two arms by the secondary robot arm joint angle.

5. Experimental Analysis

In order to verify the feasibility of the self-collision avoidance algorithm of the dual-arm upper limb rehabilitation robot, a self-collision avoidance trajectory tracking experiment is carried out on the prototype. The experiment is carried out in a sitting position, and the left arm is set as the main arm to make it forward and fixed. The angle of each joint is $q_{ls}$ = [0° −20° 30° −40° 0° 0°]. The right arm is set as the slave arm, and the self-avoidance movement is performed from the lower right side of the left arm to the upper left side of the left arm. The initial position is $q_{rs}$ = [0° 0° −60° 45° 0° 0°], the end position is $q_{rf}$ = [0° 45° −5° 55° 0° 0°], and the absolute safe distance is 50 mm. The experimental scene is shown in Figure 15. During the experiment, the angle of each joint of the right arm is read by the angle sensor, as shown in Figure 16a, and the minimum distance between the rods of each segment between the two arms is obtained, as shown in Figure 16b.

Figure 16 shows the angle change of each joint of the right arm in the experiment within 10 s. The change range of each joint angle of the right arm is within the angle range of each joint of the normal human body. For the forearm joint, the angle increases rapidly with the time at 0.8 s, reaches the maximum at 5 s, and then decreases rapidly with the time. At 9 s, it tends to be stable. The speed of the whole process is basically stable. When the joint limit is reached, the speed is 0, the acceleration does not change abruptly, and the movement is relatively stable, which is in line with the normal human body. The minimum distance between the two arms is always greater than the absolute safe distance of 50 mm. Since no collision occurred during the whole experiment, this proves that the dual-arm self-collision avoidance algorithm proposed in this paper has good practicability and reliability while ensuring safety. Therefore, the algorithm can be applied to the trajectory planning of dual-arm upper limb rehabilitation robots in rehabilitation training, helping patients to carry out effective rehabilitation exercises in a safe environment, so as to promote their rehabilitation effect. Although the trajectory planning strategy based on the improved artificial potential field method proposed in this study performs well in simulation and experiment, other advanced trajectory planning algorithms can be explored to further improve the efficiency and safety of the trajectory planning. At the same time, the robot should be applied to the actual clinical environment for large-scale clinical trials to verify its performance and effectiveness in a real environment.

6. Conclusions

This paper presents a novel intelligent rehabilitation robot designed for upper limb therapy. Recognizing the critical importance of safe operation during human–computer interaction, the study focuses on the safety aspects of the robot’s mechanical structure and trajectory planning. An equivalent mechanism data model is established to analyze the spatial relationship between the shoulder joint motor and the relative positioning of the human body across various parameters. This analysis facilitates the selection of a specific set of angular parameters for the shoulder joint motor, enabling the upper limb rehabilitation robot to achieve the objective of “minimal installation space—maximal activity space”.

To mitigate the risk of collisions between the robot arm and the human body, as well as between the robot arms during trajectory movements, a trajectory planning strategy based on an enhanced artificial potential field method is proposed. This strategy aims to enhance the safety of the trajectory planning process for the upper limb rehabilitation robot. Finally, the viability of the proposed trajectory planning strategy is validated through simulations and experimental results, which also demonstrate the safety of the mechanical design of the upper limb rehabilitation robot.