Kalman smoothing with soft inequality constraints for space robot teleoperation

Abstract

Space robot teleoperation system with haptic device has two operation modes: offline teleoperation mode and online teleoperation mode. High acceleration and jerk produced by the teleoperation system may degrade the performance of trajectory tracking and cause large residual vibration. In this article, human hand is treated as a kind of sensor, and two smoothers based on the standard Kalman smoother have been proposed to limit the acceleration and jerk in joint space in a certain range. On the one hand, the offline smoother for offline teleoperation mode makes use of a smoothing algorithm proposed in the literature, which is based on the interior point techniques. On the other hand, the online smoother for online teleoperation mode is proposed and named as reduced-Q method, which is a fixed-lag smoother with adjustable measurement noise. Experimental results have shown that the methods can constrain the acceleration and jerk of the trajectories in joint space within the specified range approximately and can improve the tracking accuracy without causing too many deformations. Thus, the psychology burden of operator will be alleviated.

Introduction

The space robot has been an important research direction in the past few decades. ¹ However, it is almost impossible to realize a perfect autonomous space robot due to the limitations of the current technologies. Thus, the teleoperation system, which can assist human to teleoperate a semiautonomous space robot, is still a very important research topic. ^{2 –6} The teleoperation system has two modes by using haptic device: offline teleoperation mode and online teleoperation mode. In online mode, the operator controls the space robot based on the vision feedback directly. When there are few barriers nearby the operating path, the online teleoperation mode should be used to guarantee real-time operations. Otherwise, the offline teleoperation mode is used instead. Specifically, the operator controls the virtual model of the space robot to complete the mission and then sends the data to the real space robot after the rationality has been validated.

On account of the increasing demand that space robots are more lightweight, both link and joint flexibility effects become the main limitation for the performance of trajectory tracking. ⁷ The high acceleration and jerk can lower the precision of trajectory tracking and cause large residual vibration. ⁸ Moreover, when undertaking the teleoperation tasks, different operators have different proficiencies and cannot sense the acceleration and jerk calculated by the inverse kinematics quantitatively. The time delay of space robot teleoperation system is so large that the effect of current manipulation on the space robot is difficult to predict. Therefore, setting the limitations of acceleration and jerk is very important in both online and offline work mode of space robot teleoperation system. However, a brutal saturation on the planning data can cause a negligible deformation of the planning path. Interpolation methods such as cubic polynomial, quintic polynomial, and spline functions are widely used to deal with the robot motion planning with acceleration and jerk bounded problem. ^{9 –11} Interpolation methods are not quite fit for the trajectory planning of teleoperation system of space robot, since that planning is time-consuming and the trajectory generated also has a large deviation compared with anticipated trajectory. Some researchers propose the acceleration feedback control to improve the trajectory tracking performance. ^12,13 For online teleoperation mode, very few papers take into account the acceleration and jerk’s characteristics simultaneously. Since it is difficult to obtain the accurate dynamic model of the manipulator, some manipulators may do not have current loop. Another noteworthy topic is how to improve the measurement precision of the haptic device effectively. ¹⁴ And these methods do not take into account the constraints of kinematics of slave robot (space robot). Additional time delay method is first proposed in the teleoperation experiment of ETS-7, and it can be used to promote the safety of collision detection and reedit the command data stored in command buffer. ¹⁵ However, the time delay in the experiment is up to 10 s, and the deformation comparing with the original trajectory is too large. Despite the effort devoted at solving the constrained trajectory planning, at the best of the author’s knowledge, none of the literatures address the online smoothing methods for teleoperation system which can satisfy the limitation of the slave robot in joint space.

When designing a teleoperation system, we can take the operator’s hand as a kind of sensor under the assumption that the haptic device has high precision. The expected trajectory with acceleration and jerk bounded approximately of slave robot in the joint space is treated as states with zero progress noises. And this kinds of limitation are known as soft constraints. Thus, filter or smoother can be used to smooth the data to be sent. Kalman filtering is a very effective and practical means of separating signal from noise, which is known to be the optimal estimator for linear systems corrupted by white noises. ¹⁶ However, in the practical application, the noises are not white mainly because the state equation cannot be modeled precisely. Thus, the incorporation of equality or inequality constraints of states can improve the performance of the Kalman filter. There are many examples of state-constrained systems in engineering applications including navigation, robotics, and target tracking. ^{17 –20} And the system model in this article is a typical target tracking problem. The main methods of Kalman filter with states constraints are described by Simon. ²¹ The basic idea of which is using the constrained numerical optimization to incorporate the equality or inequality constraints into the frame of Kalman filter. Kalman smoother has a better performance compared with Kalman filter. It can be classified into three categories: fixed-interval smoother, fixed-point smoother, and fixed-lag smoother. ²² Thus, smoothing methods with state constraints can be considered as suitable approaches for the teleoperation systems.

In this article, two smoothers are presented to smooth the data to be sent. On the one hand, the inequality-constrained fixed-interval smoother proposed by Bell et al. ²³ is used as offline smoother for the offline teleoperation mode. On the other hand, a fixed-lag Kalman smoother by using the additional time delay is proposed as online smoother for the online teleoperation mode, which can adjust smoothness online according to different operators and kinestate constraints of the slave robot.

The rest of this article is organized as follows. In “Problem statement” section, the problem statement is described. The offline smoother with soft constraints is discussed in “Offline smoothing with interior point techniques” section. “Online smoothing with additional time delay and reduced-Q method” section describes the method of online smoothing with soft constraints. Experiments and discussions are presented in “Experiment” section, followed by concluding remarks in “Conclusion” section.

Problem statement

During teleoperation tasks, the operator cannot sense the acceleration and jerk of slave robot in the joint space quantitatively. Even though some teleoperation system provides real-time display of kinestate, operators have to pay more attention to the current operation tasks. Different operators have different proficiency, and even skilled operators cannot guarantee that all operations are smooth enough. From the perspective of signal processing application, human’s hand can be taken as a kind of sensor, and the expected trajectory can be taken as ideal state with zero progress noise. Thus, smoother can be used to get rid of the operation deviation.

The smoother will have an effect in any position of the data flow, but we expect to limit acceleration and jerk calculated by the inverse kinematics of slave robot. Therefore, the data can be smoothed before sending to the slave robot, as shown in Figure 1.

Figure 1.

The data flow of teleoperation system.

The smoother will smooth each joint target points separately. And jounce, the derivative of jerk, will be taken as the progress noise. Thus, it is a target tracking problem. Then, a four-state target tracking system model of Kalman smoothing in continuous time can be expressed as

$\frac{d}{\text{d}t}\left[\begin{array}{c}{q}_i\\ {\dot{q}}_i\\ {\ddot{q}}_i\\ {\stackrel{⃛}{q}}_i\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{q}_i\\ {\dot{q}}_i\\ {\ddot{q}}_i\\ {\stackrel{⃛}{q}}_i\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]w_i(t)$

1

$z_i=q_i+v_i(t)$

2

where q_i and z_i denote the position and measurement at time t of joint i. And w_i(t) and v_i(t) denote the process noise and measurement noise, respectively. Then the discrete model can be given as follows

$x_{i,k+1}=\Phi x_{i,k}+w_{i,k}$

3

$z_{i,k}=H{x}_{i,k}+v_{i,k}$

4

$x_{i,k}=\left[\begin{array}{c}{q}_i\\ {\dot{q}}_i\\ {\ddot{q}}_i\\ {\stackrel{⃛}{q}}_i\end{array}\right],H=\left[\begin{array}{cccc}1& 0& 0& 0\end{array}\right]$

5

$\Phi =\left[\begin{array}{cccc}1& \Delta t& \frac{\Delta t^2}{2}& \frac{\Delta t^3}{6}\\ 0& 1& \Delta t& \frac{\Delta t^2}{2}\\ 0& 0& 1& \Delta t\\ 0& 0& 0& 1\end{array}\right],Q_{i,k}={\sigma }_{i,k}\left[\begin{array}{cccc}\frac{\Delta t^7}{252}& \frac{\Delta t^6}{72}& \frac{\Delta t^5}{30}& \frac{\Delta t^4}{24}\\ \frac{\Delta t^6}{72}& \frac{\Delta t^5}{20}& \frac{\Delta t^4}{8}& \frac{\Delta t^3}{6}\\ \frac{\Delta t^5}{30}& \frac{\Delta t^4}{8}& \frac{\Delta t^3}{3}& \frac{\Delta t^2}{2}\\ \frac{\Delta t^4}{24}& \frac{\Delta t^3}{6}& \frac{\Delta t^2}{2}& \Delta t\end{array}\right],R_{i,k}=r_{i,k}$

6

where k is the time step, x_i,k is the states, y_i,k is the measurement, w _{i, k} is the process noise, v_i,k is the measurement noise, Φ is the state transition matrix, H is measurement matrix, Q_i,k is the covariance matrix for the process noise, R_i,k is the covariance for the measurement noise, σ_i,k is the covariance for w_i(t), r_i,k is the covariance for v_i(t), and Δt is the sample interval. The equation (6) is the application foundation of Kalman smoother, for they are the modes of online and offline smoother.

The derivation of the standard Kalman smoother assumes that the both process noise and measurement noise are Gaussian white noise, but the process noise is colored noise in our application. There are several methods for dealing with colored process noise such as state augmentation and shaping filter. Prerequisite for application of these methods is mathematical modeling for the colored noise. However, the movement of operator’s hand mainly depends on the operation tasks and operator’s decision, so it cannot be modeled accurately. In order to improve smoother performance in this circumstances, the fading-memory method or just increasing process noise can be used directly to give more credence to the measurement in the practical application of Kalman smoother. ²⁴

Meanwhile, operators also want to constrain the acceleration and jerk within certain limits as much as possible

$\left|{\ddot{q}}_{i,k}\right|\lesssim {\text{acc}}_{i,\text{max}},\left|{\stackrel{⃛}{q}}_{i,k}\right|\lesssim {\text{jerk}}_{i,\text{max}}$

7

where acc_{i, max} and jerk_{i, max} are the limits of acceleration and jerk of the slave robot. Thus, it can be seen that the problem is a linear system model with linear soft inequality constraints according to formulas (6) and (7).

According to the operating modes of our space robot teleoperation system, two smoothing methods are presented in the following two sections.

Offline smoothing with interior point techniques

On the one hand, the time window of space robot teleoperation is short, so the offline trajectory planning should be efficient. On the other hand, the reasonableness of the planned trajectory should also be verified, which includes the collision detection, singularity detection, and rationality of planned kinestate for the slave robot. In the offline teleoperation mode, operator controls the virtual model by using the haptic device to generate the trajectory according to the task requires at first. Then, the generated trajectory will be smoothed, and the reasonableness of track will be verified. Finally, the planning data will be sent to the space robot. When smoothing the trajectory, a kind of fixed-interval smoother with inequality constraints can be used to limit its acceleration and jerk.

For the linear system model with linear inequality constraints, there are four main modifications of Kalman filter or smoother ²¹ : estimate projection, gain projection, probability density function truncation, and interior point likelihood maximization (IPLM). These methods are mainly dealing with the following quadratic programming problem within the framework of Kalman filter

${\tilde{x}}_{i,k}^{+}=\text{arg}\underset{x}{\text{min}}{(x-{\widehat{x}}_{i,k}^{+})}^{T}W(x-{\widehat{x}}_{i,k}^{+})$

8

Such that

$b_{i,k}+d{b}_{i,k}\cdot x_{i,k}\le 0$

9

where ${\widehat{x}}_{i,k}^{+}$ is the a posteriori estimation of x_i,k , b_i,k is the limitation, and db_i,k is the sign matrix. IPLM proposed by Bell et al. ²³ has a higher computational efficiency compared with other optimization algorithms, so it is chosen as offline smoothing algorithm. When applying the algorithm, the very important thing is how to select the constraints correctly. It is designed to limit the acceleration and jerk only. However, there is potential association between states. If the acceleration and jerk is presented by their difference expressions as follows

${\ddot{q}}_{i,k}=\frac{q_{i,k}-2q_{i,k-1}+q_{i,k-2}}{\Delta t^2}$

10

${\stackrel{⃛}{q}}_{i,k}=\frac{q_{i,k}-3q_{i,k-1}+3q_{i,k-2}-q_{i,k-3}}{\Delta t^3}$

11

Such that

$\left|{\ddot{q}}_{i,k}\right|\le {\text{acc}}_{\text{max}},\left|{\stackrel{⃛}{q}}_{i,k}\right|\le {\text{jerk}}_{\text{max}}$

12

This gives that

$\{\begin{array}{c}-{\text{acc}}_{\text{max}}\cdot \Delta t^2-q_{i,k-2}+2q_{i,k-1}\le q_{i,k}\le {\text{acc}}_{\text{max}}\cdot t^2-q_{i,k-2}+2q_{i,k-1}\\ -{\text{jerk}}_{\text{max}}\cdot \Delta t^3+q_{i,k-3}-3q_{i,k-2}+3q_{i,k-1}\le q_{i,k}\le {\text{jerk}}_{\text{max}}\cdot \Delta t^3+q_{i,k-3}-3q_{i,k-2}+3q_{i,k-1}\end{array}$

13

Which means that

$\{\begin{array}{c}l{b}_{i\mathrm{,1}}=\text{max}(-{\text{acc}}_{\text{max}}\cdot t^2-q_{i,k-2}+2q_{i,k-1},-{\text{jerk}}_{\text{max}}\cdot \Delta t^3+q_{i,k-3}-3q_{i,k-2}+3q_{i,k-1})\\ u{b}_{i\mathrm{,1}}=\text{min}({\text{acc}}_{\text{max}}\cdot t^2-q_{i,k-2}+2q_{i,k-1},{\text{jerk}}_{\text{max}}\cdot \Delta t^3+q_{i,k-3}-3q_{i,k-2}+3q_{i,k-1})\\ l{b}_{i\mathrm{,1}}\le q_{i,k}\le u{b}_{i\mathrm{,1}}\end{array}$

14

The limitation of joint velocity can be calculated similarly

$\{\begin{array}{c}l{b}_{i\mathrm{,2}}=\text{max}(-{\text{acc}}_{\text{max}}\cdot \Delta t+{\dot{q}}_{i,k-1},-{\text{jerk}}_{\text{max}}\cdot \Delta t^2-{\dot{q}}_{i,k-2}+2{\dot{q}}_{i,k-1})\\ u{b}_{i\mathrm{,2}}=\text{min}({\text{acc}}_{\text{max}}\cdot \Delta t+{\dot{q}}_{i,k-1},{\text{jerk}}_{\text{max}}\cdot \Delta t^2-{\dot{q}}_{i,k-2}+2{\dot{q}}_{i,k-1})\\ l{b}_{i\mathrm{,2}}\le {\dot{q}}_{i,k}\le u{b}_{i\mathrm{,2}}\end{array}$

15

Thus, in order to meet (12), the constraints (14) and (15) must also be satisfied. However, since all points should be dynamically adjusted simultaneously during smoothing processing, equations (14) and (15) cannot be satisfied when using fixed-interval smoother. Only equation (11) can be used to constrain the process states, and the effect of that is equal to the constraints(7) which is soft constraints. The constraint (11) can be modified in the form of equation (9)

$\underset{b_{i,k}}{\underbrace{\left[\begin{array}{c}-{\text{acc}}_{\text{max}}\\ -{\text{acc}}_{\text{max}}\\ -{\text{jerk}}_{\text{max}}\\ -{\text{jerk}}_{\text{max}}\end{array}\right]}}+\underset{d{b}_{i,k}}{\underbrace{\left[\begin{array}{cccc}0& 0& 1& 0\\ 0& 0& -1& 0\\ 0& 0& 0& 1\\ 0& 0& 0& -1\end{array}\right]}}\cdot \underset{x_{i,k}}{\underbrace{\left[\begin{array}{c}{q}_{i,k}\\ {\dot{q}}_{i,k}\\ {\ddot{q}}_{i,k}\\ {\stackrel{⃛}{q}}_{i,k}\end{array}\right]}}\le \left[\begin{array}{c}0\\ 0\\ 0\\ 0\end{array}\right]$

16

When the constraints are replaced, the smoother with inequality constraints will be degenerated into the standard fixed-interval smoother.

Online smoothing with additional time delay and reduced-Q method

In the mode of online teleoperation, a 0.75 s additional time delay is added to guarantee safety. The additional time delay is composed of a first-in first-out (FIFO) queue. The command buffer and the smoother are placed between the task plan manager and the telemetry and telecommand subsystem (TM/TC). The data flow of teleoperation system is shown in Figure 2.

Figure 2.

The data flow of teleoperation system.

When collision of the three-dimensional (3-D) pre-simulator is detected, the buffer will be filled with the latest data immediately, as shown in Figure 3.

Figure 3.

The response of the buffer with collision detection.

The additional time delay can ensure that the actual space robot keeps a certain distance away when the pre-simulator collides with the bounding box.

The model with dashed lines represents the actual space robot, and the model with solid lines represents the pre-simulator. The configuration of the pre-simulator shown in Figure 4 is near the mechanical limits on joints. So it is difficult to find the right direction in which operator can move the robot away without the additional time delay.

Figure 4.

The effect of collision detection with additional time delay.

In the meantime, the data stored in the buffer can be used to construct a fixed-lag Kalman smoother. Considering the numerical stability, Biswas–Mahalanobis fixed-lag smoother (BMFLS) is used as the smoother. ²⁵ The data in the FIFO buffer are used to define a superstate vector as follows

$X_{i,k}^{\text{BMFLS}}=\left[\begin{array}{c}{q}_{i,k}\\ q_{i,k-1}\\ q_{i,k-2}\\ q_{i,k-3}\end{array}\right]$

17

The corresponding state transition and measurement matrixes are

${\Phi }^{\text{BMFLS}}=\left[\begin{array}{cccc}\Phi & 0& 0& 0\\ I& 0& 0& 0\\ 0& I& 0& 0\\ 0& 0& I& 0\end{array}\right],H^{\text{BMFLS}}=\left[\begin{array}{cccc}H& 0& 0& 0\end{array}\right]$

18

And the covariance matrixes for process noise and measurement noise are

$Q_{i,k}^{\text{BMFLS}}=\left[\begin{array}{cccc}{Q}_{i,k}& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right],R_{i,k}^{\text{BMFLS}}=R_{i,k}$

19

In fact, the implementation equations for the BMFLS are the same as those for the standard Kalman filter by using the parameters above. If viewed as sensors with different precisions, different operators mean different initial values of process noise covariance. On the other hand, the time window of space robot teleoperation system is short, so the dexterity of the operators is considered as constant, which means the covariance for process noise can be considered to be unknown constant value. Thus, the appropriate covariance can be estimated according to the constraints during the operation. In the practical application of Kalman smoother, the covariance σ_k can be used to adjust the smoothness of the trajectory, because the smoothness of trajectory will increase when the corresponding σ_k is small.

Thus, an online adjustment method for the covariance σ_k is proposed. First, the position q_i,k of joint i at time step k is smoothed to get ${\widehat{q}}_{i,k}$ , and the acceleration ${\ddot{q}}_{i,k}$ and jerk ${\stackrel{⃛}{q}}_{i,k}$ are calculated by using $\{{\widehat{q}}_{i,k}\dots {\widehat{q}}_{i,k-3}\}$ . Second, if the acceleration and jerk exceed the upper limit, ${\widehat{\sigma }}_{m,k}$ will be given by subtracting Δσ from σ_k. Third, BMFLS is used to calculate the acceleration ${\ddot{q}}_{m,k}\text{'}$ and jerk ${\stackrel{⃛}{q}}_{m,k}\text{'}$ again by using the data of first 10 s, and the constraints are judged again. The loop will be terminated when the constraints satisfied or the lower limit σ_min of σ_k is reached. The method proposed here will be called the reduced-Q method, as shown in Algorithm 1.

The σ_k recalculated will be used in the next cycle, and it is lower bounded by σ_min, so it can just satisfy a kind of soft constraints. σ_k will converge to a smaller value after every adjustment, so the trajectory will more smooth in the next operation. Δσ determines the speed of convergence, and a smaller value for it is preferred in accordance with the processor power to reduce the deformation caused by over smoothing.

Experiment

Environment of experiment

The onboard system consists of three subsystems, including robotic manipulator system, free-flying platform, and microtarget system, and the on-ground system consists of TM/TC and teleoperation system. The TM/TC is the data exchange center for onboard system and teleoperation system. Communication mode between onboard system and on-ground is the satellite communication. The relationship is shown in Figure 5.

Figure 5.

The relationship of onboard system and on-ground system.

The simulator of space robot is consisted of a 6-DOF manipulator and planar air bearing. The configuration of the manipulator can be found in the literature. ^26,27 Table 1 lists the nominal robot link parameters of the robot.

Table 1.

The nominal link parameters in Denavit-Hartenberg (DH) model for the 6-DOF manipulator.

DH joint	a (mm)	α (rad)	d (mm)	θ (rad)
1	0	−π/2	0	−π/2
2	990	0	0	0
3	0	−π/2	0	−π/2
4	0	−π/2	−780	0
5	0	−π/2	0	0
6	0	0	372	0

DH: Denavit-Hartenberg Matrix.

The manipulator can just move in the two-dimensional plane, so only three joints can be rotated. The 3-D pre-simulator is used to realize the collision detection, and solid-frame model and wire-frame model represent current planning position and position received from space robot. Offline teleoperation mode, online teleoperation mode, and their smoothing are realized by task plan manager. The Virtuose 6D35-45 (Haption Virtuose 6D35-45 technical specifications) is used as the haptic device. Its operational workspace in translation is 450 mm and minimum position resolution is 0.006 mm.And task plan manager receives the position data from haptic device at 100 Hz. API Tracker 3 (Haption Virtuose 6D35-45 technical specifications) is used as a track recorder for the movement of the manipulator’s end effector. Its measurement accuracy is 0.001 mm and measurement frequency is 100 Hz. System structure of ground verification of the space robot teleoperation is shown in Figure 6.

Figure 6.

The system structure of ground verification.

The experimental platform is shown in Figure 7.

Figure 7.

The experimental platform of space robot teleoperation system.

Firstly, the operator controls the manipulator to move along a trajectory. Then, the offline-constrained Kalman smoother (Offline-CKS) based on IPLM and the online-constrained Kalman smoother (Online-CKS) based on BMFLS with reduced-Q method are used to smooth the trajectory. Meanwhile, the fixed-interval Kalman smoother, BMFLS, and Kalman filter are used as comparative method. Communication frequency between task plan manager and central controller of space robot is 4 Hz, so Δt is set to 0.25 s. The maximum acceleration (acc_max) and maximum jerk (jerk_max) of each joint should be determined by the dynamic simulation of robot manipulator and the output power of the motors and then verified by the experiment on the planar air bearing. In this experiment, the two parameters are given according to the calibration, which are ${\text{acc}}_{\text{max}}=4$ °/s² and ${\text{jerk}}_{\text{max}}=8$ °/s³. Most textbooks and papers are focus on the theoretical derivation and simulation experiment about Kalman filter, and the noise is the Gaussian white noise whose initial values of covariance are given as the known quantity. However, in the practical engineering, the initial value of covariance can be determined by the given parameter of sensors. In this research, the sensor is the operator’s hand, and the initial value is determined by the experience operator. The process noise is generated by the operating error and operating intention, so it should be determined by the operator’s debugging, and we choose σ_k = 25. The accuracy of measuring instrument is relatively high, so the measurement noise is set as r_k = 0.01. The standard Kalman filter can also be used to smooth the trajectory with constraints by adjusting the process noise covariance. However, Kalman filter can only adjust the current point, so the covariance for the measurement noise must be turned down as much as possible. Thus, the process noise covariance of the Kalman filter can be chosen as σ_k = 1.

Results of experiment and discussion

The smoothed joint position, velocity, acceleration, and jerk of the trajectory in joint space are shown in Figures 8 and 9. It can be seen that the proposed methods can constrain the acceleration and jerk within the specified range approximately without causing too many deformations on trajectory.The tail end tracks of original data and smoothed data and the dynamic responses of the space robot simulator by these different approaches are shown in Figures 10 and 11.

Figure 8.

Joint position and velocity of the planning data. The deformations of smoothed joint position and velocity compared with the original are all very little except Kalman filtering.

Figure 9.

The acceleration and jerk of the planning data. The Offline-CKS and Online-CKS can constrain the acceleration and jerk within the limitation approximately. These two methods both achieve the expected result. Offline-CKS: offline-constrained Kalman smoother; Online-CKS: online-constrained Kalman smoother.

Figure 10.

The tail end tracks of manipulator. The deformations of the end track are all small except Kalman filtering.

Figure 11.

Dynamic responses of the space robot simulator. All these smoothing methods can increase the accuracy of dynamic response of the manipulator. And the effect of Offline-CKS is the most obvious. Offline-CKS: offline-constrained Kalman smoother.

Let x_ORG (i), y_ORG (i), x_SMT (i), and y_SMT (i) denote the Cartesian coordinate of the original and smoothed data at time i. And let the corresponding Cartesian coordinate measured by API denote as $x_{\text{ORG}}^{\text{API}}(i),y_{\text{ORG}}^{\text{API}}(i),x_{\text{SMT}}^{\text{API}}(i),\text{and}{y}_{\text{SMT}}^{\text{API}}(i)$ . We define erro_mean and erro_max as mean error and maximum error of deformation

${\text{erro}}_{\text{mean}}=\text{Mean}\left(\right\{\sqrt{{(x_{\text{ORG}}(i)-x_{\text{SMT}}(i))}^2+{(y_{\text{ORG}}(i)-y_{\text{SMT}}(i))}^2}\left\}\right)$

20

${\text{erro}}_{\text{max}}=\text{Max}\left(\right\{\sqrt{{(x_{\text{ORG}}(i)-x_{\text{SMT}}(i))}^2+{(y_{\text{ORG}}(i)-y_{\text{SMT}}(i))}^2}\left\}\right)$

21

Let e_ORG (i) and e_SMT (i) denote the response error of the original and smoothed data at time i

$e_{\text{ORG}}(i)=\sqrt{{(x_{\text{ORG}}(i)-x_{\text{ORG}}^{\text{API}}(i))}^2+{(y_{\text{ORG}}(i)-y_{\text{ORG}}^{\text{API}}(i))}^2}$

22

P is defined as improved ratio of dynamic track performance to describe the dynamic track performance improvement with different smoothing methods

$P=\frac{\text{max}(\{e_{\text{ORG}}(i)\})-\text{max}(\{e_{\text{SMT}}(i)\})}{\text{max}(\{e_{\text{ORG}}(i)\})}$

23

The error of deformation and performance improvement is shown in Table 2.

Table 2.

Deformation and performance improvement.

Methods	erromean (mm)	erromax (mm)	P(%)
Offline-CKS	1.516	6.687	38.0135
Offline-KS	1.466	5.636	33.0412
Online-CKS	2.645	10.469	33.4177
BMFLS	1.154	5.213	29.1131
Kalman filter	4.894	13.904	8.4476

Offline-CKS: offline-constrained Kalman smoother; Online-CKS: online-constrained Kalman smoother; Offline-KS: fixed-interval Kalman smoother; BMFLS: Biswas–Mahalanobis fixed-lag smoother.

The high acceleration and jerk can lower the precision of trajectory tracking, and all these methods can improve the dynamic track performance of the manipulator by reducing the acceleration and jerk as shown in Table 2 and Figure 11. Since only the constrained smoothing methods can limit the acceleration and jerk in the specified range, they have a better performance than the others. Moreover, the deformation error is expected to be as small as possible. The expected trajectory is taken as ideal states with zero progress noises in this article. Thus, from the optimal state estimation theory point of view, more information may improve the effect of estimation in most cases. As shown in the table, the deformation of Offline-CKS is smaller than Online-CKS. Because Offline-CKS can reedit more points than Online-CKS. Standard Kalman filter, as a comparison method, exhibits the worst performance in terms.

Algorithm 1. Reduced-Q method

Step 1: ${\widehat{q}}_{i,k}\leftarrow \text{BMFLS}(q_{i,k},{\sigma }_k)$ Step 2: $\{{\ddot{q}}_{i,k},{\stackrel{⃛}{q}}_{i,k}\}\leftarrow \text{calculate\hspace{0.17em}}acc\text{\hspace{0.17em}and\hspace{0.17em}}jerk\text{\hspace{0.17em}by\hspace{0.17em}using\hspace{0.17em}}\{{\widehat{q}}_{i,k},\dots {\widehat{q}}_{i,k-3}\}$ Step 3: $if({\ddot{q}}_{i,k}>{\mathrm{acc}}_{\text{max}})\left|\right|({\stackrel{⃛}{q}}_{i,k}>{\mathrm{jerk}}_{\text{max}})\mathbf{then}$ Step 4: $\mathbf{for}\mathbf{all}\text{\hspace{0.17em}joints\hspace{0.17em}}m\mathbf{do}$ Step 5: ${\widehat{\sigma }}_{m,k}\leftarrow {\sigma }_k$ Step 6: $\mathbf{while}{\widehat{\sigma }}_{m,k}>{\sigma }_{\text{min}}\mathbf{do}$ Step 7: ${\widehat{\sigma }}_{m,k}\leftarrow {\widehat{\sigma }}_{m,k}-\Delta \sigma$ Step 8: $\{{\widehat{q}}_{m,k}^{\prime }\dots {\widehat{q}}_{m,k-40}^{\prime }\}\leftarrow \text{BMFLS}(\{{\widehat{q}}_{m,k}^{\prime }\dots {\widehat{q}}_{m,k-40}^{\prime }\},{\widehat{\sigma }}_{m,k})$ Step 9: $\{{\ddot{q}}_{m,k}^{\prime },{\stackrel{⃛}{q}}_{m,k}^{\prime }\}\leftarrow \text{calculate\hspace{0.17em}}\mathrm{acc}\text{\hspace{0.17em}and\hspace{0.17em}}\mathrm{jerk}\text{\hspace{0.17em}by\hspace{0.17em}using\hspace{0.17em}}\{{\widehat{q}}_{m,k}^{\prime }\dots {\widehat{q}}_{m,k-40}^{\prime }\}$ Step 10: $\mathbf{if}({\ddot{q}}_{m,k}^{\prime }<{\mathrm{acc}}_{\text{max}})\text{\hspace{0.17em}and\hspace{0.17em}}({\stackrel{⃛}{q}}_{m,k}^{\prime }<{\mathrm{jerk}}_{\text{max}})\mathbf{then}$ Step 11:      break Step 12:     end if Step 13:    end while Step 14: $\mathbf{end}\mathbf{for}$ Step 15: ${\sigma }_k\leftarrow \text{min}({\widehat{\sigma }}_{m,k})$ Step 16: end if

Conclusion

When a space robot remote controlled by teleoperation system with haptic device, limitation of acceleration and jerk can improve precision of trajectory tracking and lower residual vibration. How to constrain the acceleration and jerk within certain limits is still an open research area facing many technical challenges. In this article, a new approach has been proposed based on the standard Kalman smoother. The operator hand was taken as a kind of position sensor, and the expected trajectories were taken as ideal states. According to the different requirements and characteristics of two operation modes of teleoperation task, offline and online smoother with soft inequality constraints had been built. IPLM was used as the offline smoother, and the reduced-Q method was used as the online smoother. The experimental results had shown that both of the two smoother can constrain the acceleration and jerk efficiently. Thus, the requirements on the precise of human operation can be reduced substantially and the mental stress of operator can also be relieved. Furthermore, the methods can also be applied in the teleoperation system of other kinds of robot, such as industrial robot and medical robot.