Application of Optimal-Jerk Trajectory Planning in Gait-balance Training Robot

To accommodate the gait and balance disorder of the elderly with age progression and the occurrence of various senile diseases, this paper proposes a novel gait balance training robot (G-Balance) based on a six degree-of-freedom parallel platform. Using the platform movement and IMU wearable sensors, two training modes, i.e., active and passive, are developed to achieve vestibular stimulation. Virtual reality technology is applied to achieve visual stimulation. In the active training mode, the elderly actively exercises to control the posture change of the platform and the switching of the virtual scene. In the passive training mode, the platform movement is combined with the virtual scene to simulate bumpy environments, such as earthquakes, to enhance the human anti-interference ability. To achieve a smooth switching of the scene, continuous speed and acceleration of the platform motion are required in some scenarios, in which a trajectory planning algorithm is applied. This paper describes the application of the trajectory planning algorithm in the balance training mode and the optimization of jerk (differential of acceleration) based on cubic spline planning, which can reduce impact on the joint and enhance stability.


Introduction
Gait and balance control is a complicated process that cannot be managed completely by any robot configuration. Different robot configurations perform training differently. Mainstream gait balance training robots have three configurations: (1) lower limb exoskeleton walking training robots, which focus on abnormal gait correction and recovery of motion control capabilities. Representative research results include Locomat [1], which is developed by the Swiss Federal Institute of Technology Zurich; WalkTrainer [2], which is developed by the Swiss Federal Institute of Technology Lausanne and SWORTEC; and ReoAmbulator, which is launched by HealthSouth of the United States, as shown in Figure 1; (2) pedal gait balance training robots, which focus on training patients' deep sensation, of which representative research results include the Haptic Walker developed by Stefan Hesse in Germany [3], the G-EO system developed in cooperation with Reha Technology in the Netherlands [4], and GaitMaster developed by the University of Tsukuba in Japan [5], as shown in Figure 2; (3) the multi-degree-offreedom motion platform gait balance training robot, which exhibits multi-degree-of-freedom spatial motion characteristics and provides the acceleration and angular velocity stimulation to the human body, with emphasis on vestibular stimulation and simulation. In this regard, the representative technology is the Computer-assisted Rehabilitation Environment (CAREN) [6] developed by Motek Medical, of which configuration is used in the G-Balance.
G-Balance, shown in Figure 3, aims to combine the simulated acceleration and angular velocity changes of the sports platform with virtual reality technology to achieve vestibular and visual stimulation such that the human gait and balance ability can be trained. The vestibular sense perceives changes in the external acceleration and angular velocity, whereas the visual sense perceives changes in the external environment [7]. In some scenarios, to achieve a seamless change in the speed and acceleration of the motion platform as the virtual scene changes, trajectory planning is performed.
Typically used trajectory equations to achieve continuous changes in acceleration and velocity include B-spline and cubic spline. Additionally, trajectory planning is transformed into a nonlinear optimization problem by introducing stroke, speed, acceleration constraints of electric motors and optimization objectives, which finally becomes the optimal trajectory planning. Based on the optimization goals, optimal trajectory planning can be categorized into three categories: optimal time trajectory planning [8], optimal energy trajectory planning [9] and optimal jerk trajectory planning [10]. In addition, these three items can combine with different weight to achieve a comprehensive optimal trajectory planning [11]. Bobrow [12] and Shin [13] were the first to investigate time-optimal trajectory planning. Their core idea is to establish dynamic equations using end-position parameters. However, the acceleration and moment of the final trajectory are discontinuous with their method. Lin et al. [14,15] introduced the spline curve and the B curve into trajectory planning to obtain a continuous acceleration of the trajectory; however, the optimal time could not be maintained as the global optimal. Energy optimal trajectory planning is often used in applications that are sensitive to energy consumption, such as industrial robotic arms. In Ref. [16], energy consumption was controlled during exercise by setting the upper limits for control signals and joint speeds. In another optimization method, jerk was used as the objective equation. Lower jerk of a system can reduce the dynamic pressure, the vibration of the mechanism, and rendering the trajectory more stable. Kyriakopoulos et al. [17] obtained the optimal jerk between two points using analytical methods. Piazzi et al. [18] used the interval optimization method to obtain the global jerk optimal trajectory, whereas Gasparetto et al. [19] combined the jerk and time into a target function, which was optimally planned to avoid the artificial determination of the operating time. In a follow-up study [20], the method was verified using the cubic spline and quintic B-spline methods. The minimum energy consumption strategy and the minimum time strategy are generally used in energy or time sensitive applications, such as industrial manipulators, which are not essential in our current study. On the contrary, lower dynamic pressure and vibration are needed in this situation. Therefore, the optimal jerk planning was selected for G-Balance.
Herein, the method to implement the optimal jerk such that a robot propagates seamlessly as the virtual scene changes, as well as the usage of the robot in the balance training mode are explained. In Section 2, the formula for solving the inverse kinematics, which provides the foundation for the subsequent trajectory planning, is presented. Section 3 presents the detailed derivation process of the cubic spline planning and the reasons for selecting it. Section 4 discusses the cubic spline planning based on the minimum jerk, where the addition of constraint equations and the planning process are elaborated. Section 5 provides the application scenarios of the trajectory planning proposed herein. Finally, the results obtained with or without optimal jerk are compared to demonstrate the superiority of the optimal jerk trajectory planning.

Structure and Kinematics of G-Balance System
The structure of G-Balance, as shown in Figure 3, includes a classic six degree-of-freedom parallel platform named Stewart, a treadmill, and a safety guard like handrail. The platform was driven by six electric cylinders   with a power of 750 W. A mechanical schematic diagram of G-Balance is shown in Figure 4. The mechanism can be categorized into upper and lower platforms, which are connected by universal hinges with hinge points denoted as P i and B i (i = 1-6) respectively. The coordinates of origin P of the upper platform relative to the lower platform are expressed as follows: where P 0 is the initial position vector, and T is the translation vector. The Euler angle representation is adopted for rotation, and the corresponding rotation matrices are as follows: The position vector of the electric cylinder is expressed as where P ′ i indicates the coordinates of the upper hinge point relative to the upper platform, and L i indicates the vector of the electric cylinder. Therefore, the length of the electric cylinder is expressed as The coordinates of the upper hinge point P i and its derivative are as follows: The speed component of the upper hinge point in the direction of the electric cylinder is the speed of the electric cylinder.
Similarly, the acceleration of the electric cylinder can be obtained, which is not elaborated here because of seldom use.

Cubic Splines for Trajectory Planning
To achieve continuous speed and acceleration, the most typically used trajectory curve equations are the cubic spline and quintic B-spline equations. The higher the time power, the smoother is the motion and the less significant is the impact [17]; however, the calculation is more complicated and the planning time is longer. When simulating environmental changes in the balance training process, such as combining virtual reality technology to achieve scene switching, the robot is required to propagate smoothly. In other words, the speed and acceleration are continuous. Therefore, cubic spline planning can sufficiently fulfill these conditions. Two main modes of trajectory planning are used in multi-joint robots: trajectory planning in the workspace and joint space. The difference between these two modes is the sequence of trajectory planning and the kinematic solutions. In the trajectory planning of the workspace, the trajectory in the workspace is first obtained, and then inverse kinematics is applied to obtain the trajectory of each motor. Thus trajectory of the endpoint is visible, and movement at the end of platform is smooth; however, it may violate the joint motion constraints or cause discontinuous velocity and acceleration of driving motor. In the trajectory planning of the joint space, trajectory planning is performed in each joint space respectively. This is advantageous as each joint propagates along the ideal trajectory; however, the trajectory at the end of the mechanism cannot be displayed intuitively. To obtain the trajectory at the end of platform, forward kinematics is required through iterative method. In our training scenario, one is only required to determine whether the end reaches the final pose through a smooth motion, whereas the trajectory of the end to reach the final pose is emphasized less. In addition, the minimum jerk of the joint can both reduce the impact and increase the lifespan of the joint; hence, joint space trajectory planning was adopted. For a series of time points t i and their corresponding space position of the end of platform (t i , q i )(i = 0, 2, 3 . . . n − 2, n) (i represents the serial number of time point, note that the two missing numbers are the numbers of virtual points), we first calculated the corresponding position of each upper hinge joint t i , q ji (j represents the electric cylinder number, j = 1 ∼ 6 ) based on inverse kinematics. Each segment of the cubic spline satisfies the following equation: where Q ji (t) represents the trajectory equation of upper hinge point P j between time t i and t i+1 ; a ji , b ji , c ji , d ji are constants related to the the trajectory. In order to obtain definite initial and ending speed and acceleration, here is a simple technique. Two virtual time points were inserted between (t 0 , t 2 ) and (t n−2 , t n ) , and they are indicated as ( t 1 , q j1 ) and ( t n−1 , q j,n−1 ) . Gasparetto et al. [19,21] adopted this technique, and the values can be calculated as follows: where h i represents the time intervals, h i = t i+1 − t i , and M ji is the acceleration of upper hinge point P j at time t i , M ji = Q ji ′′ (t i ) . Assume that the initial speed and acceleration are as follows: Subsequently, because of the continuous velocity and acceleration of the cubic splines, the following formula can be derived: Because Q ji (t) is a cubic function, its second derivative exhibits a linear relationship with t. Linear interpolation is performed on the acceleration.
Because of the boundary condition Q ji (t i ) = q ji , Q j,i+1 (t i+1 ) = q j,i+1 , we can obtain the following equations by integrating Eq. (12).
Because of the continuous speed, we can obtain the following equation using Eq. (13): The formula above rewritten in the matrix form is as follows: Finally, the complete cubic spline trajectory constraint equation is obtained.

Optimization of Trajectory
By adopting cubic splines and trajectory planning, the position, velocity, and acceleration of the joint space were continuous, thereby satisfying the requirements. Additionally, the mechanism was subject to other constraints resulting from motor performance and structural limitations. The most typical ones are speed and acceleration constraints, which necessitate minimum jerk trajectory planning.
Optimal trajectory planning is a nonlinear equation optimization problem, the core of which is to obtain the constraint equation and the objective optimization equation. As presented in the previous section, an equality constraint is derived. The objective optimization equation is used to obtain the optimal jerk. Meanwhile, the speed and acceleration constraints must be satisfied.
where VC j represents the maximum speed of electric cylinder j and AC j represents the maximum acceleration of electric cylinder j . The velocity and acceleration constraint equations are infinite-state equations that are difficult to solve. According to Ref. [21], some optimizations can be achieved. The velocity of any joint is a quadratic function, and its extreme value can only appear at the endpoint or the point where the acceleration is zero. The moment at which the acceleration of each segment is zero is expressed as follows: Therefore, the maximum speed of each segment can only be one of the following: Similarly, acceleration is a linear function; therefore, only the accelerations at the two ends must fulfill the constraint conditions. Based on the analysis above, the optimal-jerk trajectory planning is transformed into a nonlinear programming problem that satisfies equality and inequality constraints, and it can be solved via sequential quadratic programming (SQP).
However, based on the derivation above, it can be concluded that the time interval yielded by this method will be infinitely long. We can utilize the method presented in Ref. [21] to add a scale factor between the minimum time and minimum jerk, as follows: Another alternative is to set a lower limit for the speed adopted in this study.
where LC j represents the minimum speed of electric cylinder j.
The lower speed limit is determined by the longest operating time set which is not the actual minimum speed cause the speed in the beginning or at the end will be zero in most cases. Using this method, we can easily control the maximum time for each run. Similarly, we can set an equality constraint on time, as expressed in Eq. (23) [18].
It is noteworthy that the time T set cannot be less than the shortest operating time, which is calculated as follows: It is difficult to identify which of the above methods is the best. Application scenarios are the most important considerations in the selection process.
A brief summary of the procedure for optimal jerk planning is as follows:

Active and Passive Training Modes
The core training function of G-Balance is to apply the characteristics of a multi-degree-of-freedom motion platform to provide vestibular acceleration and angular velocity stimulation. Simultaneously, combined with virtual reality technology, both visual stimulation and an immersive training experience can be achieved. Two training modes were proposed based on the proposed system. In the active training mode, the elderly can exert influence and control strategies on the platform. This is similar to a somatosensory game with actual physical stimulation. To capture the posture of humans, seven IMU inertial sensors were attached to the human instep, lower limbs, upper limbs, and waist to capture the motion information of the human ankle, knee, and hip joints, as shown in Fig. 5. The periodicity and symmetry of gait, gait frequency, and other gait information were closely associated with the movement of these joints, which are essential for realizing human gait assessment and simple gait training using a treadmill [22][23][24]. Meanwhile, the output of these sensors was extracted and processed as the input of the robot control system to realize active control. The flow of the control signals and one virtual training scene are shown in Figures 6 and 7, respectively. This scene is similar to a surfing game. The movement of the lower limbs of the human body corresponds to the change in the posture of the virtual person, and the posture of the platform corresponds to the posture of the surfboard. All control information is from positional changes in the human body. The human body can be simplified into a skeleton model, which is composed of thighs, calves, and trunks; hence, the position of each joint can be obtained based on the angle of each joint and the length of the limb.
Once the position of each joint is obtained, which we denote as G i herein, the center of gravity of each limb can be estimated. According to Vanhan et al. [25], a fixed proportional relationship exists between the center of gravity of each leg and the position of adjacent joints, as shown in Figure 8 and Eq. (25).
We used the weighted sum of the center of gravity of each limb and the angle of trunk tilt as the input signal of the control system, which are the position and Euler angle of the workspace, respectively. In other words, the position q i is the estimated center of gravity of the human body.
where G ci is the center of gravity of each lower limb, and µ i is the corresponding weight. The next task is to use the trajectory planning method described in Section 4 to execute trajectory planning. The sampling time interval of position q i is extremely short, and the frequency is typically 20-50 Hz. Therefore, we sampled position q i at 1 s interval as the final input position in the workspaces.
In the passive training mode, the platform is combined with a virtual environment to simulate vibration (25)    scenes and train the anti-interference ability of the human subject. This training mode is similar to an earthquake hut in a science museum. In this scenario, sensors are used to obtain the response of the human body to stimuli. The trajectory of the estimated center of gravity of the human body is used to measure a person's balance ability and training effect like the usage of center of pressure (COP) [26,27]. Unlike the twodimensional COP method, the estimated center of gravity is three dimensional. The flow of the signals in this mode is shown in Figure 9.
To achieve more realistic acceleration and angular velocity stimulation in a limited motion space, we adopted a washout algorithm [28][29][30] to achieve better vestibular stimulation, which will not be further elaborated.

Simulation Results
By performing the procedures described above for a series of positions in the workspace, the corresponding joint space positions were calculated, as shown in Tables 1 and 2. The kinematic limits of the electric cylinders were set to the same value as the models were identical.
For comparison, we set the operating time to 8 s and performed the trajectory planning of the optimal jerk and ordinary cubic splines. In the ordinary trajectory planning, we set via points at equal intervals of 1.6 s, as shown in Table 3.
The results of the simulations are presented in Figures 10, 11, 12 and 13 which show the position, velocity, acceleration of each joint, respectively, and the total jerk.
As shown in the simulation results, the trajectory, velocity, and acceleration were all continuous functions in the entire process. Additionally, the velocity and acceleration were within the limits. These results are consistent with the theoretical derivation.
The value of jerk obtained by the minimum jerk trajectory planning was 4.89 × 10 5 , which was 7.52 × 10 5 in the ordinary trajectory planning. The jerk of optimal trajectory planning was 65% of that of ordinary trajectory planning, which seemed insignificant. Moreover,      the speed and acceleration of the entire process were within the range of the kinematic limits in the ordinary trajectory planning, although the constraints were not included. This result was primarily due to small stroke of the platform, which resulted in similar planned trajectories; hence, the minimum jerk method did not offer much advantage. As the distance between the position points in the workspace increases, the minimum jerk trajectory planning gradually demonstrates its advantages. Optimal-jerk trajectory planning, however, is still worth adopting considering the position, speed, and acceleration constraints. It is noteworthy that all via points in the workspace cannot exceed the stroke limit of the robot.

Conclusions
In some scenarios, G-Balance must propagate smoothly while satisfying the speed and acceleration constraints; hence, optimal jerk trajectory planning is introduced. Herein, a detailed process of optimal-jerk trajectory planning was elaborated, and its application in active training mode using human movement as input signals was described. The following aspects are noteworthy: (1) Cubic spline trajectory planning with virtual points added at the beginning and end allowed the speed and acceleration of the starting and ending points to be controlled. (2) In optimal-jerk trajectory planning, three methods are typically used to control time: adding time with weight to the objective function, adding the minimum operating speed limit, and specifying the operating time. The method to be used depends on the specific scenario.
(3) Owing to the characteristics of cubic splines, continuous speed and acceleration constraints can be transformed into discrete constraints in a finite state, thereby simplifying the calculation process significantly.
In the virtual scene listed in this article, the usefulness of trajectory planning is not obvious cause more virtual training scenarios are being designed. Meanwhile, the entire control system, including the washout algorithm and the training mode, such as the passive training mode of G-Balance, were not described comprehensively.