Sensor fusion for compliant robot motion controller

R k is computed in a straightforward way [3], [4]. PO should not be smaller than Qo. The design O f Q k is based on Theorem 1. The main drawback is that noise rejection decreases as N increases. Since Rnn 1 - j is a Dirac delta function, 18 is obtained. Hence, since the Kalman gains are only function of the relations between Q and R see [5] , a LKF based on 29 and 27 has always the same Kalman gains, regardless the stage i. Then, the cascade filtering of 22 represented in Figure 1 entails the same results as filtering only Y k the variables X 1 k and x k in Figure 1 are equivalent.

After the description of the robotic system, data fusion results are shown for the fz component. Similar to Theorem 1. A complete mathematical characterization of human based tasks is extremely hard, time consuming and very task dependent. Starting the design from human data avoids mathematical modeling and enables to work on the skill level, which encodes the dexterity acquired and developed through training and experience.

In the peg-in-hole task, the skill is synthesized through identification of perception-action signals from human demonstration. The skill defines an implicit representation of contact states and task trajectory, without explicit knowledge of a task model. Geometric perception signals GEPS, i. It is important to note that many approaches use just DYPS, which only give sparse geometric information about the contact state [8].

Similar to the human sense, GEPS correspond to vision and pose sense. The pose sense is 4? Peg-in-hole skill transfer system. The compliant motion controller CMC is discussed in [5]. The relative pose can be obtained by cameras mounted on the end-effector, sensing structured or unstructured features present in the task setup.

The uniqueness of the geometric information coming from sensed object features can be derived by considering the perspective n-point PnP problem. Horaud and co-authors [6]demonstrated an analytical solution to obtain the pose of a rigid object from a single camera view, for four coplanar but not collinear points. With stereo vision, only three coplanar points are needed to compute the pose [SI. The relative position of the coplanar points, the distance of the camera and the focal length determine the accuracy of the visual sensing mode.

In the experiments, the initial pose is reached by vision. The feature pattern are four circular blobs with different colors, enabling to identify the corresponding blobs at each time step.

The color segmentation and blob tracking algorithm are implemented in the DLR's cube vision system, which tracks 2 x 4 blobs in 25 [Hz]. Details about the tracking algorithm and vision system can be seen in [l]. The compliant motion skill can be generated independently from the vision cameras or from the pose sense.

This redundancy is fused to optimize the generation of skill maps from GEPS. Figure 2 gives an overview of the peg-in-hole skill transfer system. The human skill, i. The human performs the peg-in-hole task with a teach device, which records the geometric and dynamic data necessary for the training process [8]. Additionally, two vision cameras are attached to the robot, giving useful geometric information on the task evolution. ANNs are designed for each source one pose sense and two vision cameras to map geometric data into desired compliant motion signals forces and velocities.

The data fusion module is applied after the skill transfer module to perform optimal data fusion of forces and velocities. The data fusion module takes into account the active sensors at each time step. Sensor obliteration or sensor failure is represented in Figure 3.

Pose and vision results for the fx component are depicted in Figure 4. Two feedforward neural networks of size 8 x 10 x 3 are trained to represent the skill maps of the visual sense. The three outputs are f z , fi and My for one network, and vx, U, and wy for the other.

The pose sense is also trained with two feedforward neural networks of size 3 x 20 x 3. The training phase consists of geometric shifts around the nominal case the one used during the experiments , keeping the same compliant motion characteristics i. ANN weights are updated with the standard backpropagation algorithm. The ANN outputs for the vision show that the right camera issues more "texture" around the nominal case than the left camera Figures 4.

For the pose, it is very close to the desired f x Figure 4. Data fusion and soft robotics. Left Camera Data Fusion. Noise analysis referred to the output of the ANNs is necessary to design the fusion module.

Table 1 presents the. The learned noise requires a trial and error tuning of Rk to smooth the learned output, without losing the signal characteristics. Q k represents the mean square evolution of the desired quantities plus an adaptive term. The full expression for Qk considers also an adaptive term, i. ANNs for the pose and vision cameras nominal case. Figures 5 and 6 illustrate fusion experiments2. The best results are achieved when all sensors are active Figures 5.

If only the vision cameras are active Figures 5. No fusion is performed only one active sensor. The phase distortion was compensated.

Data fusion experiments f o r the fx component. The left columns represent raw data coming from the ANNs. The right columns show the fused signal vs. Data fusion experiments for the fx component. The peg-in-hole skill transfer system may be done by any geometric source acting independently or not with guaranteed performance, since all plots show similar results. Proven techniques for robust visual servo control.

Digital and Kalman 4 Conclusions Filtering. Sensor fusion for skill transfer systems. Sensor fusion for humanrobot skill transfer systems. Advanced Robotics, 14 6: Special issue on selected papers from IROS' Compliant motion control with stochastic active observers. A general filtering module associated with a data fusion architecture has been proposed. It enables to filter higher nonlinear functions, in which the first derivative is badly described by a zero mean random variable.

A bank of Kalman filters in the filtering module does not improve the results. Data fusion experiments have been presented, showing the importance of the fusion architecture. Vision and pose data have been fused to obtain human-like compliant motion behaviors. Fault tolerance has been analyzed for the peg-in-hole task. The task execution quality increases with the number and accuracy of the involved active sensors.

The fusion algorithm is not system dependent and can be generalized to other setups. An analytic solution for the perspective 4-point problem. We tie the wearable sensors to the upper limb to get motion information. And then with the help of specialized software platform, such as Choregraphe developed by Aldebaran-Robotics Company and Visual Studio, the corresponding data is relayed to the humanoid robot.

Ultimately we complete the real-time trajectory imitation of the humanoid robot. This section provides the basic concepts and notations of joint motion of NAO robot based echo state network.

Then, the basic architecture of echo state network is introduced in the paper in Section 2. Finally, the joint motion control based echo state network is provided thereafter in Section 2. NAO robot is more popular humanoid robot nowadays. NAO was developed by Aldebaran-Robotics Company with the latest technology and a variety of sensors, so it can ensure the fluency of motion. The more important feature of NAO is the embedded software. With the software, NAO can do voice synthesis, acoustic positioning, detect the visual image and shape with color, and detect obstacles based on dual-channel ultrasonic system.

Human body can be simplified as a model that can be described as in Figure 1 , which contains 12 main body joints. The purpose of this paper is to drive the robot arm joint, mapped to the human body, corresponding to the upper arm joint of the human body.

So we tied the MTi sensors on human upper limb to obtain joints movement data. And we make sure the joint of human and NAO robot is the same. As for the wearable sensors, we adopt the Xsens sensors-MTi. It is a miniature gyroscope enhanced heading measurement system that integrates MEMS inertial measurement sensors.

The internal low-power signal processor provides three-dimensional orientations of no drift and corrected 3D acceleration, 3D angular velocity, and 3D magnetic field data.

Now it is broadly used in robot fields. When starting collecting data, the initial position of the MTi sensor is marked as the coordinate direction of the current experiment.

We can use the MTi sensor directly with its function of RS, which can meet the real-time requirements of our experiment. Then we can obtain the data from the serial port with the MTi binary communication protocol using flow free-running operation or polling request mode. The initialization of the MTi sensor devices is done on the computer programming.

It is quite easy to achieve with the Xsens code examples. The MTi sensors have a variety of output modes; researchers can choose the mode according to their needs.

In this paper, we choose the Euler Angle output mode. The output format is roll, pitch, and yaw; all data elements are four-byte floating point. Because human is more flexible, the activity scope is relatively larger than robot; when the human body joint movement value exceeds the limit value of the NAO robot, the NAO robot will take the angle value as the maximum. We can complete the programming of the NAO robot motion control based on VS platform and implement this program under the chorographer.

The dynamic equation of NAT robot is given as where is the mass and inertia matrix, is the stiffness matrix, is the damping matrix, and is the NAO robot joint matrix. And the dynamic equation is described as matrix as. More formally, an ESN consisted of input units, internal units, and output units.

Then, activation of input, internal, and output units at time step is , , and , respectively. Connection weights between units are kept in four connection matrices. There are weights in the input weight matrix , weights in the internal weight matrix , weights in the output weight matrix, and weights in a matrix for connection projecting back from the output to internal units.

The activation of internal units was calculated as with being the output functions of the internal units, a sigmoid function for the experiments in this paper.

Similarly, the output was computed as with , the output functions of the output units and the concatenation of input, internal, and previous output activation vector. And the inputs of ESN were the 12 main body joints, described as. The outputs of ESN were 3 main parameters, described as. Importantly, the output neuron was equipped with random connections that project back into the reservoir.

A step teacher sequence was generated from the MGS equation and fed into the output neuron. This excited the internal neurons through the output feedback connections. After an initial transient, they started to exhibit systematic individual variations of the teacher sequence. The fact that the internal neurons display systematic variants of the exciting external signal is constitutional for ESN: Not every randomly generated RNN has this property, but it can effectively be built into a reservoir.

Figure 4 shows that the parameters of single joint are set. Taking the single joint of NAO robot as an example, the dynamic trace for passive joint is as shown in And for function 5 , in this paper, the states for shift, velocity, and acceleration in the moving joint of NAO robot are set in. In this section, brief review the steps for collecting human pose data is given in Section 3. The initial position of the human body and NAO robot: This can be achieved through Choregraphe.

In Choregraphe, processing the initialization instruction, the NAO robot will be at the zero position, as shown in Figure 5. Signal acquisition depended on MTi sensor: This system is not strictly limited by fixed motion analysis equipment. When ready, with the input validation rules, the MTi sensor begins to collect the shoulder joint data.

Joint angle calculation and processing: On the right-handed Cartesian coordinate system, rotation about , , and axis, respectively, called roll, pitch and yaw rotation. It is easy to express when the rotation is expressed as on a rotating shaft.

Combined with the above formulas, it is easy to calculate the required angle value. Data driven NAO robot: This process needs using two software systems—Choregraphe and Visual Studio.

The difference of the different components of the range can reduce the studying accuracy and speed for networks. In order to improve better studying accuracy and speed for ESN, inputting datum for sensors must be normalized firstly with function 6 and then be linear transformation: In training, the inner neurons update as follow: When , the network begins to run and the state is initialized randomly here, ,. Then DR is encouraged fully, which shows system dynamic characteristics see Algorithm 1.

Control law of joint motion.