Paper Title: MSMA: Multi-agent Trajectory Prediction in Connected and Autonomous Vehicle Environment with Multi-source Data Integration.


On-the-fly Point Feature Representation for Point Clouds Analysis
    Yang Xulei
    August 5, 2024
=================================================================



§ ABSTRACT 
The prediction of surrounding vehicle trajectories is crucial for collision-free path planning. In this study, we focus on a scenario where a connected and autonomous vehicle (CAV) serves as the central agent, utilizing both sensors and communication technologies to perceive its surrounding traffics consisting of autonomous vehicles (AVs), connected vehicles (CVs), and human-driven vehicles (HDVs). Our trajectory prediction task is aimed at all the detected surrounding vehicles. To effectively integrate the multi-source data from both sensor and communication technologies, we propose a deep learning framework called MSMA utilizing a cross-attention module for multi-source data fusion. Vector map data is utilized to provide contextual information. The trajectory dataset is collected in CARLA simulator with synthesized data errors introduced. Numerical experiments demonstrate that in a mixed traffic flow scenario, the integration of data from different sources enhances our understanding of the environment. This notably improves trajectory prediction accuracy, particularly in situations with a high CV market penetration rate. The code is available at: <https://github.com/xichennn/MSMA>.









§ INTRODUCTION



Predicting the trajectories of surrounding vehicles is a critical component of ensuring collision-free path planning. Autonomous vehicles (AVs) provide promising solutions to achieving safer road environment <cit.> facilitated by the technological advancement in sensors (camera, LiDAR, radar, etc) and artificial intelligence. Connected vehicle (CVs) technologies are also being deployed to improve the road safety by enhancing vehicular situational awareness through various CV options such as vehicle-to-vehicle (V2V) and vehicle-to-infrastructure (V2I) communications. With the same objective of improving road safety, CVs and AVs really should be developed in a cooperative manner. As argued in <cit.>, CV technology provides information that AV sensors can not see immediately (e.g. maneuver commands issued to other vehicles) as well as information that may be occluded by adjacent vehicles. Two examples are shown in Figure <ref>. In both scenarios, the AV failed to detect the potential hazard in advance due to its limited field of view. However, if the green vehicle could communicate its trajectory information to the AV, the accidents could be effectively avoided. We believe that the integration of AV and CV technologies can provide complementary benefits, significantly enhancing the performance and safety of the transportation system. Hereinafter, we refer to a vehicle with attributes from both sides as connected and autonomous vehicle (CAV), whereas vehicles without these attributes are termed Human-Driven Vehicles (HDVs). 



While extensive research has focused on trajectory prediction within the context of AV, trajectory prediction studies within the CAV environment have garnered comparatively less attention, primarily due to the low market penetration rate (MPR) of CVs. Gathering real-world data in the CAV environment is inherently more complex than in purely autonomous settings. Given that CAVs are still in the early stages of deployment, obtaining large-scale datasets with comprehensive trajectory information from actual CAVs navigating intricate traffic scenarios remains a limitation. Most CAV trajectory prediction studies rely on datasets like the Next Generation Simulation (NGSIM), which, regrettably, lacks CAV-specific data. Consequently, it may not fully encapsulate the distinct characteristics and behaviors exhibited by CAVs. 

Before the full-scale deployment of either connected or autonomous vehicular technology, it is anticipated that a prolonged transition period will ensue, characterized by a mixed traffic environment consisting of CVs, AVs, CAVs and HDVs. With the gradual deployment of CAV technology in real-world setting, studying the trajectory prediction in a CAV environment can help identify the limitations and robustness for trajectory prediction models that need to operate in complex and dynamic traffic environment. 

 To address this gap, we investigate a mixed traffic scenario in which a CAV serves as the central agent, as illustrated in  Figure <ref>. The CAV collects information from its surroundings using both sensors and communication technology. Typically, the communication range extends farther than the sensing range, covering approximately 300 meters for the former and 200 meters for the latter <cit.>. As a result, connected vehicles within the sensing zone provide dual observations. It is worth noting that we categorize both AVs and HDVs as non-connected vehicles (NCVs) since information regarding these two categories is solely acquired through sensors. It is crucial to recognize that data obtained from different sources inherently carry distinct types of errors, with sensor noise and communication latency being the most common ones. Leveraging the complementary nature of information from these sources holds the potential to enhance scene understanding and, importantly, mitigate errors associated with each individual source. To facilitate our study, we collected a trajectory dataset in the CARLA simulator <cit.>, introducing synthesized data errors tailored to each source to accurately reflect their inherent characteristics. 
 
 A novel deep learning framework was proposed comprising a multi-source encoder, an agent-agent and agent-lane interaction module and a multi-agent decoder. The multi-source encoder efficiently processes historical vehicle trajectories by employing source-specific temporal encoders to capture the temporal dependencies and a cross-attention fusion module for multi-source data integration. By representing each agent and lane as graph nodes, we leverage Graph attention models (GAT) to capture agent-agent and agent-lane interactions. The multi-agent decoder simultaneously predicts the trajectories of all surrounding vehicles. An Ablation study is conducted to assess the impact of multi-source data and the cross-attention fusion module on the model’s performance. Furthermore, we compare model's performance across different MPRs to unveil the synergistic benefits of sensor and communication technologies. Additionally, we illustrate the prediction results on individual scenes. Finally, a comparative study with existing research work is presented. 



The main contributions of this work lie in three-folds:

  
  * We are the first to study the impact of multi-source data for trakectory prediction in a mixed traffic flow.
  
  * A customized motion dataset with map geometry was collected in CARLA and utilized for the validation of the problem. 
  
  * A novel deep learning framework is proposed to handle the multi-source data fusion, agent-agent and agent-lane interactions, and multi-agent predictions.




§ RELATED WORK



 §.§ Publicly Available Motion Dataset

There is a plethora of open motion datasets collected by autonomous vehicle on-board sensors in urban scenarios such as Waymo  <cit.>, ArgoVerse <cit.>, ArgoVerse2 <cit.>, NuScenes <cit.>, etc., which have significantly advanced trajectory prediction research within the context of AVs. Research that utilizes these datasets often assumes a flawless data source and does not account for the inherent data errors they may encompass. The NGSIM dataset, commonly employed in the context of CAV, was collected by video cameras mounted on buildings and lacks CAV specific scenarios. Primarily, it encompasses highway driving scenarios. The V2X-Seq trajectory dataset <cit.> provides valuable multi-view trajectory information, which is closely related to our study. However, our work differs as we focus on the overlapping range of on-board sensors and communication technologies, where complementary information can be utilized. To the best of our knowledge, there are currently no publicly available motion datasets dedicated to CAV research. Therefore, we will leverage a simulated dataset generated by CARLA, affording us the flexibility to construct CAV scenarios and explore data sources featuring diverse characteristics.



 §.§ Trajectory Prediction in Connected and Autonomous Vehicle Environment



  §.§.§ Autonomous Environment

Due to the temporal-spatial characteristics of trajectory data, recurrent neural networks (RNN) with long short-term memory (LSTM) or gated recurrent unit (GRU) architectures are extensively employed to predict future trajectories taking their historic trajectory as input  <cit.>. Social-LSTM was proposed considering interactions among neighboring vehicles <cit.>. The spatial information was encoded by convolution neural networks (CNN) or graph neural networks (GNN) <cit.>. As vehicles' decisions are largely associated with the map topology, an emerging avenue of research is proposed to leverage the high-definition maps (HD maps) to assist the prediction. <cit.> rasterized the traffic scenes into birds' eye view images where map vector layers (e.g. polygons, polylines) are encoded into RGB color space. Conventional CNN are then used to model the scene. As the image-based representation is dense and computationally expensive, subsequent work proposed to use more compact and efficient vectorized representation (polygons, polylines) directly <cit.> and then model by graph convolutional networks (GCN) <cit.>, transformers <cit.>, or point cloud networks <cit.>. 



  §.§.§ Connected Environment

Trajectory prediction models proposed in CV lags behind the ones developed in AV as autonomous driving has a much higher requirement on the trajectory prediction accuracy. <cit.> first clustered the training data then employed Gaussian process regression to learn the motion pattern. <cit.> recognized three driving styles using Gaussian mixture model, then proposed a personalized joint time series model based on LSTM to predict the front vehicle trajectories. <cit.> proposed a CNN-LSTM framework utilizing 8 surrounding vehicles to predict the future trajectory of an ego-vehicle. <cit.> developed a heterogeneous context-aware graph convolutional networks which combines the information encoding of historical vehicle trajectories, scene image stream and inter-vehicle interaction patterns to predict the future trajectory of a target vehicle. 



  §.§.§ Connected and Autonomous Environment

There is limited literature in the CAV context. <cit.> proposed to utilize all the historic trajectories of surrounding CAVs to predict the longitudinal trajectory of a HDV in front of the ego-vehicle. Promising results showed that information from CAV improved the model performance even when the market penetration rate is as low as 20%. <cit.> proposed an interactive network model utilizing LSTM and CNN with a correction mechanism. The model enables a CAV to predict the trajectories of surrounding vehicles, and the trajectories can be corrected during the prediction process to improve the prediction accuracy. Both models are tested on the NGSIM dataset which contains only limited driving scenarios: highway driving (including ramp merging and double lane change) and signalized intersection scenarios. 

We will essentially follow a similar approach for modeling temporal, spatial, and contextual information, with the distinction of operating within a CAV environment where we will account for multi-source data integration.








 §.§ Multi-source Data Fusion in Cooperative Driving

There is a growing trend in leveraging multi-source data through Vehicle-to-Everything (V2X) communications to enhance model performance. However, existing research primarily focuses on cooperative perception, where perceptions from different viewpoints are fused <cit.>. Some studies have considered joint perception and motion forecasting, but the emphasis on data fusion persists mainly in the perception stage <cit.>. With the recent release of a V2X trajectory dataset <cit.>, we anticipate an increase in research directly addressing trajectory data fusion.



§ DATA PREPARATION AND SCENE REPRESENTATION



 §.§ Data Preparation


The traffic was simulated in CARLA Town03 using the Traffic Manager (TM) module, which controls vehicles in autopilot mode. The simulation interface is shown in Figure <ref>(a). The simulation lasted for 270 seconds with a sampling rate of 10Hz. The positions of the 265 populated vehicles were logged at each timestamp. 



To construct the scenario depicted in Figure <ref>, we employed the following process on the collected dataset: First, we iterated through all the vehicles designated as CAVs, one at a time. For each CAV at time t, we identified all the neighboring vehicle IDs within a 50-meter radius which is the communication range we employed. Among these IDs, we randomly assigned CV IDs according to the MPR. We also identified all the IDs that fall within a 30-meter sensing range. The trajectory data for these identified IDs were processed within a time window spanning from t-5 seconds to t+5 seconds, constituting a 10s time window. Within this window, we allocated 3s for motion history and 5s for the prediction horizon. The initial 2s were reserved for simulating latency errors. 

To simulate latency, we introduce delays ranging from 1 to 15 frames to the historical trajectories of CV IDs, shifting the ground-truth trajectories. Additionally, Gaussian noises with variances ranging from 0 to 0.5 are introduced to the historical trajectories of sensor IDs. It's important to note that for IDs found in both CV IDs and sensor IDs, we maintain dual sets of historical trajectories — one representing the delayed data and the other the noisy data. This approach allows us to account for both latency and sensor noise in the dataset.

The map of Town03 is publicly available in OpenStreetMap (OSM) XML format (shown in Figure <ref>(b)) and is processed following the Argoverse map API (Chang, 2019). We utilize the lane centerlines within a 75-meter radius of the CAV. 



 §.§ Scene Representation

Vectorized representation was employed for its efficiency <cit.>. The scenes were first transformed such that the CAVs are positioned at the origin with its heading aligned along the positive x-axis.

Specifically, each trajectory can be represented as a sequence of displacements  {Δ𝐩_t}^0_t=-(T_h-1), where Δ𝐩_t∈ℝ^2 is the 2D displacement from time step t-1 to t and T_h is the total historical time steps. Similarly, for a lane centerline composed of lane segments, each segment can be represented as Δ𝐩_l^k ∈ℝ^2, which captures the 2D displacement from the starting coordinate to the end coordinate of lane segment k.



§ METHODOLOGY



 §.§ Overview

An overview of our proposed model is illustrated in Figure <ref>. It consists of a multi-source temporal encoder, a Graph Attention Network (GAT) based agent-agent encoder, an agent-lane encoder and a multi-agent decoder. 





 §.§ Multi-source Temporal Encoder

To differentiate between data obtained from sensors and communication technologies, we employ source-specific temporal encoders for capturing temporal dependencies. While both RNNs and Transformers are suitable for handling temporal data, we use Transformer as the base model due to its efficiency in capturing long-range dependencies. Specifically, a self-attention mechanism is employed which allows the agent to focus on relevant temporal information from its own historical trajectory. 

We denote the input from agent i and source s as {Δ𝐩_t^i_s}^0_t=-(T_h-1), where s ∈{1,2,3}. Source 1 represents the input directly from CAV itself, for which we utilize ground-truth data in this context. Source 2 corresponds to input from communication technologies, where latency has been intentionally introduced. Source 3 captures input from sensor technologies which contains noise. 

A multilayer perceptron (MLP) is applied to the input to get its embedding 𝐙^i_s∈ℝ^T_h × d_h, where d_h is the embedding dimension. Following BERT <cit.> and HiVT <cit.>, we then append a learnable token 𝐳^i_s∈ℝ^d_h to the end of the embedding. Learnable positional encodings are then added to the embedding. The updated embedding 𝐙^i_s∈ℝ^(T_h+1) × d_h is then fed into the self-attention block:

     
           𝐐^i_s=𝐙^i_s𝐖^Q_s, 𝐊^i_s=𝐙^i_s𝐖^K_s, 𝐕^i_s=𝐙^i_s𝐖^V_s

           𝐙̃^i_s = softmax(𝐐^i_s𝐊^i_sT)/√(d_k)𝐕^i_s
    

where 𝐖^Q_s, 𝐖^K_s, 𝐖^V_s∈ℝ^d_h × d_k are learnable matrices for linear projection and d_k is the dimension of the transformed vectors. 1/√(d_k) implements the scaled dot product term for numerical stability of attention. Multiple heads can be used to attend to different parts of the sequence simultaneously. In addition, we apply Dropout and Layer Normalization to stabilize the training process. The updated appended token 𝐳̃^i_s∈ℝ^d_k summarizes of the temporal features. For agents with a single data source, this is the input to the subsequent agent-agent interaction module.


For agents with dual data sources, we apply across-attention to align and fuse the temporal features across the two sources. The temporal feature from sensor source is converted to the query vector and the temporal feature from communication source is used to compute the key and value vectors:


     
           𝐪̃^i_3=𝐳̃^i_3𝐖^Q_f, 𝐤̃^i_2=𝐳̃^i_2𝐖^K_f, ṽ^i_2=𝐳̃^i_2𝐖^V_f

           𝐅^i_3 = softmax(𝐪̃^i_3𝐤̃^i_2T)/√(d_k)ṽ^i_2

           α^i = sigmoid(𝐖^α[𝐳̃^i_3, 𝐅^i_3])

           Ẑ^i = α^i ⊙𝐖^self𝐳̃^i_3+(1-α^i)⊙𝐅^i_3
    


where 𝐖^Q_f, 𝐖^K_f, 𝐖^V_f, 𝐖^α, 𝐖^self∈ R^d_k × d_k are learnable matrices and ⊙ denotes element-wise product. 𝐅^i_3 represents the aligned features from the communication source in the sensor source. We then apply sigmoid to obtain a weight vector which computes the final fused representation 𝐳̂^i∈ R^d_k from the two sources. 𝐳̂^i is then the input for agents with dual sources to the next module as illustrated in Figure <ref>.



 §.§ Agent-agent Interaction

To capture agent-agent interactions, we employ the GAT architecture <cit.> to attend to neighboring features. Each agent is represented as a node within the graph. The temporal features 𝐳̃^i (𝐳̂^i for agents with dual source) we have just obtained serve as node features. The relative positions 𝐩^i_0-𝐩^j_0 between two agents i and j at the last observed step t=0 are modeled as edge attributes. GAT dynamically adjusts the weight of each edge during the message-passing process. We utilize multiple stacked graph attention layers to compute and update the edge weight e_ij. The resulting weighted information aggregation from surrounding agents of agent i can be expressed as follows:

    
       e_ij = a_G(𝐳^i𝐖^G, 𝐳^j𝐖^G, (𝐩^i_0-𝐩^j_0)𝐖^G) 

       α_ij = Softmax(e_ij) 

       𝐒^l(i) = ∑_j∈N_i α_ij𝐳^j𝐖^G
    

where W^G ∈ℝ^d_k × d_g is the shared linear transformation, d_g is the new feature dimension, the shared attentional mechanism a_G is a single-layer feedforward neural network: ℝ^d_kℝ that computes the edge weight e_ij indicating the importance of vehicle j's feature to vehicle i, and S^l is the output of the lth layer by summing up the weighted features from all the neighboring vehicles. Output from the last layer l̃ is the updated node feature, denoting as 𝐒̃^i. 



 §.§ Agent-lane Interaction

Now we consider the context information provided by lane centerlines. Each lane segment Δ𝐩_l^k ∈ℝ^2 is represented as a graph node, where the node feature is obtained by an MLP embedding ℝ^2 ℝ^d_g:

    
    𝐋 = MLP_L(Δ𝐩_l^k)


We have ϵ total segments queried within a 75m radius of the CAV. To efficiently assess relevant lane information while mitigating the computational load associated with distant or non-essential segments, we employ a 30m local radius for each traffic agent. Consequently, only lane segments falling within this local radius will be taken into account when evaluating interactions.

The edge attribute between a lane segment k and an agent i is determined by their relative position 𝐩_l^k-𝐩^i_0. If lane node k and agent node i are connected, we calculate the edge weight e_ik using graph attention layers:

     
           e_ik = a_L(𝐋^k𝐖^L,  𝐒^i𝐖^L, (𝐩_l^k-𝐩^i_0)𝐖^L)

           α_ik = Softmax(e_ik) 

           𝐌^l(i) = ∑_k∈N_i α_ik𝐋^k𝐖^L
    

where 𝐖^L ∈ℝ^d_g × d_g is the shared linear transformation, a_L is a feedforward neural network as in Eq. <ref> and 𝐌^l represents the aggregated lane information from the l-th layer. We output the node features from the last layer l̃ node feature, denoting as 𝐌̃^i, which is then concatenated with 𝐒̃^i as the final temporal-spatial embedding for agent i. 



 §.§ Multi-agent Decoder

We parameterize the distribution of future trajectories as a mixture model where each mixture component is a Gaussian distribution. The predictions are made for all agents in a single shot. For each agent, the model predicts D possible future trajectories (mixture components) and their confidence scores (mixing coefficients). The decoder has two headers: a regression head to predict the trajectory of each mode and a classification head to predict the confidence score of each mode. For agent i and its mode d, in the regression head, an MLP receives the final embedding as inputs and outputs the location μ^t_i,d∈ℝ^2 and its associated covariance σ^2^t_i,d∈ℝ^2*2 of the agent per future time step. In the classification head, we use another MLP followed by a softmax function to produce the confidence scores for each mode.


 §.§ Model Training

Since all the modules are differentiable, we can train the model end-to-end. The Loss function is defined as

    
    J = J_reg + α J_cls


Here, J_reg represents the loss from the regression head, and J_cls represents the classification loss. After experimentation, we have set α to 0.5. Given a set of D predicted trajectories, we identify the trajectory d̃ with the minimum Average Displacement Error (ADE), which is defined as the average Euclidean distance between ground-truth locations and predicted locations across all future time steps. We utilize the negative log-likelihood to calculate the regression loss:

    
    J_reg = -1/nT_f∑^n_i=1∑^T_f_t=1 logP(𝐩_t^i,pred - 𝐩_t^i,gt | μ̂^i,d̃_t,σ̂^̂2̂^i,d̃_t)


where T_f is the prediction horizon, n is the number of agents, P(|) is the probability density function of Gaussian distribution and μ̂_t^i,d̃, σ̂^̂2̂_t^i,d̃ are the mean and covariance from predicted trajectory d̃.

For J_cls, cross-entropy loss is applied:

    
    J_cls = -1/n∑^n_i=1∑^D_d=11_d logp_d


where 1_d is a binary indicator if d is the best mode, p_d is the confidence score of mode d.

The model was trained for 100 epochs using AdamW optimizer with initial learning rate 1e-3 for 10 warm-up epochs. The learning rate was then decayed every 10 epochs by a factor of 0.9. Our model consists of 4 layers for temporal encoding, 8 layers for both agent-agent and agent-lane interactions, 8 heads for all attention modules. We used 16-dimensional input embedding size and 64 for all hidden dimensions. The dropout rate was 0.1 throughout the architecture.


§ RESULTS ANALYSIS

The complete dataset was split into train, validation and test set with a ratio of 8:1:1. The standard metrics in motion predictions are adopted, including Average Displacement Error (ADE), Final Displacement Error (FDE), and Miss Rate (MR), where errors between the best predicted trajectory among the D=5 modes and the ground truth trajectory are calculated. The best here refers to the trajectory that has the minimum endpoint error. The ADE metric calculates the L2 distance across all future time steps and averages over all scored vehicles within a scenario, while FDE measures the L2 distance only at the final future time step and summarizes across all scored vehicles. MR refers to the ratio of actors in a scenario where FDE are above 2 meters.


 §.§ Ablation Study

To demonstrate the impact of cross-attention fusion, which aligns input from sensor and communication sources, we conducted two ablation studies under MPR=0.8. 



In the first study, our objective was to assess the fusion module's resilience to communication latency. This involved introducing a time delay to all historical CV trajectories, varying from 1 to 15 frames, i.e., 0.1 to 1.5 seconds. Gaussian noise was held constant at 0 for sensor trajectories. We then compared the ADE, FDE and MR metrics for all CVs under the model architectures with and without the cross-attention fusion module. The results, presented in Figure <ref>(a), clearly indicate that the inclusion of fused information from error-free sensors enhances the robustness of CVs against communication latency.

In the second study, we aim to evaluate the fusion module's ability to withstand sensor noise. We introduced Gaussian noises with varying variances, ranging from 0 to 0.5, to all historical trajectories of vehicles within the sensing range, while no latency was introduced to CV trajectories in this evaluation. The metrics comparison is shown in Figure <ref>(b), It is evident that the incorporation of fused information from communication technologies effectively mitigated the impact of sensor noises.


 §.§ Performance under Different CV Market Penetration Rates

We are also interested in assessing how varying numbers of CVs in the scenario influence the model's performance. To conduct this evaluation, models are trained under different MPRs with a fixed 1-frame delay and 0.1 noise variance. Since scenarios corresponding to different MPRs consist of varying numbers of CVs, to ensure a fair comparison, we evaluate the prediction performance using ADE exclusively for vehicles within the sensor range across different MPRs. The evaluation results are presented in Figure <ref>. We can observe a clear trend of performance improvement as the MPR increases especially at longer prediction horizons. 




To highlight the extent of improvement, we selected two scenarios for a comparative analysis of multi-agent predictions, specifically under MPR=0 and MPR=0.8. The first scenario includes vehicles navigating a roundabout, as depicted in Figure <ref>. Notably, when MPR=0, the predictions exhibit a slight deviation, as indicated by the arrows. However, a marked improvement is observed in both vehicles' predictions when they benefit from dual observations under MPR=0.8. Similar improvements are observed in the second scenario shown in Figure <ref>, where vehicles are driving in opposite directional turning lanes.








 §.§ Comparison with Existing Work

To evaluate the effectiveness of the proposed architecture MSMA, we compare with the following baselines on the same CARLA dataset:

  
  * LSTM Encoder Decoder: We use the vanilla setting with the decoder modified to accommodate 5 modes as described in this work. It predicts the future trajectory for a single agent at a time and only historical trajectory of the agent itself is utilized for prediction. 
  
  * VectorNet: We consider the first work using vectorized scene representation described in <cit.>. The default settings are employed without auxiliary graph completion. Only the central CAV’s future trajectory gets predicted thus reported. 
  
  * HiVT: We use the default setting with 64 hidden dimensions <cit.>. All the surrounding agents’ future trajectories were predicted. 


Ground truth historical trajectories are used for the baseline models as they do not handle multi-source data. MSMA with 1 frame delay and 0.1 gaussian variance under different MPRs are evaluated. ADEs across all scored agents are calculated. Table <ref> shows the displacement errors at each second and the ADEs for the models being compared. 

We note that LSTM produce the highest ADE as it lacks the information from neighboring agents and map data. While VectorNet shows slightly better performance, it struggles to capture both temporal and spatial relations effectively using a single GNN. On the other hand, HiVT significantly improves trajectory prediction accuracy by explicitly modeling both temporal information and spatial interactions using attention mechanisms. In comparison, MSMA performs even better, particularly when there is more complementary information available from higher CV MPRs.





§ CONCLUSION

In this work, we proposed a multi-source multi-agent (MSMA) encoder-decoder model for vehicle trajectory prediction in a connected and autonomous vehicle environment. Compared to existing models that rely solely on a single data source, our model incorporates the complementary information provided by both sensors and communication technologies to improve prediction accuracy. A customized CARLA data set was collected, data errors including noise and latency were introduced to accurately reflect the real-world scenarios. A cross-attention fusion module was proposed to align and integrate the perturbed data sources. Numerical experiments showcase that integrating complementary data sources helps improve model prediction performance, particularly when the market penetration rate of connected vehicles is high. 

While our method demonstrates efficacy, it has certain limitations. First, it assumes that CVs broadcast only their own trajectories, overlooking the potential benefits of cooperative driving. Enhanced collaboration could be achieved if each CV possesses information about surrounding vehicles within its communication range, and broadcasting this comprehensive information could augment the surrounding coverage. Second, the model assumes homogeneity among vehicles, and there is room for improvement by considering heterogeneous interactions through graph edges that reflect distinct vehicle characteristics. Last, despite the high-fidelity real-world scenarios provided by CARLA simulation, a real-world dataset would offer deeper insights into vehicle behaviors.




unsrt  
  




 Paper Title: A Cooperation Control Framework Based on Admittance Control and Time-varying Passive Velocity Field Control for Human--Robot Co-carrying Tasks.





 


Journal of  Class Files, Vol. 14, No. 8, August 2021
Shell et al.: A Sample Article Using IEEEtran.cls for IEEE Journals





A Cooperation Control Framework Based on Admittance Control and Time-varying Passive Velocity Field Control for Human–Robot Co-carrying Tasks

    Dang Van Trong, Sumitaka Honji, and Takahiro Wada


        
The authors are with the Graduate School of Science and Technology, Nara Institute of Science and Technology, Ikoma, Nara 630-0192, Japan (e-mail: van_trong.dang.ve6@naist.ac.jp, honji.sumitaka@naist.ac.jp, and t.wada@is.naist.jp).



    August 5, 2024
===========================================================================================================================================================================================================================================================================================================



§ ABSTRACT 
Human–robot co-carrying tasks demonstrate their potential in both industrial and everyday applications by leveraging the strengths of both parties. Effective control of robots in these tasks requires minimizing position and velocity errors to complete the shared tasks while also managing the energy level within the closed-loop systems to prevent potential dangers such as instability and unintended force exertion. However, this collaboration scenario poses numerous challenges due to varied human intentions in adapting to workspace characteristics, leading to human–robot conflicts and safety incidents. In this paper, we develop a robot controller that enables the robot partner to re-plan its path leveraging conflict information, follow co-carrying motions accurately, ensure passivity, and regular the energy of the closed-loop system. A cooperation control framework for human–robot co-carrying tasks is constructed by utilizing admittance control and time-varying Passive Velocity Field Control with a fractional exponent energy compensation control term. By measuring the interaction force, the desired trajectory of co-carrying tasks for the robot partner is first generated using admittance control. Thereafter, the new Passive Velocity Field Control with the energy compensation feature is designed to track the desired time-varying trajectory and guarantee passivity.  Furthermore, the proposed approach ensures that the system's kinetic energy converges to the desired level within a finite time interval, which is critical for time-critical applications. Numerical simulation demonstrates the efficiency of the proposed cooperation control method through four collaborative transportation scenarios.




Admittance control, cooperation control framework, human–robot co-carrying tasks, human–robot conflicts, time-varying passive velocity field control (PVFC).




§ INTRODUCTION

The field of robotics has recently witnessed numerous studies on physical human–robot interaction problems in the same workspace and mutual tasks such as handover, co-transportation, collaborative assembly, teleoperation, co-sawing, and so on <cit.>. This arises from the fact that hybrid human–robot systems are considered a potential solution as the cognitive strengths and adaptability of humans can be integrated with the high-precision and computational capability of robots to implement common tasks. Furthermore, such systems guarantee flexibility in unstructured environments,  which is difficult to achieve through robot–robot collaboration <cit.>. However, cooperation failure and increased human workload are common issues in the above human–robot collaboration scenarios <cit.>, arising from the conflicts between robot movements and variations in human motion intentions. 

In human–robot co-carrying tasks, a single object is transported to a target location identified by the human operator utilizing the mutual efforts of both the robot partner and human <cit.>. Such collaborative tasks have been conventionally applied to industrial applications and everyday life. For this task, it is important to synchronize human–robot movement and achieve safe interaction, especially when conflicts arise. To this end, the desired co-carrying trajectory for the robot needs to be re-planned in the case of human–robot conflicts. Furthermore, it is necessary to guarantee the passivity of the closed-loop system, which cannot create energy internally and can only absorb or dissipate energy from external forces,  to achieve safe collaborative co-carrying tasks among humans and robots.  Moreover, the energy level in the closed-loop system must be controlled to guarantee co-carrying performance. This implies that the co-carrying task cannot be achieved at a low energy level due to a sluggish rate and too swiftly otherwise. Existing studies on human–robot co-carrying tasks primarily concentrate on addressing specific aspects of the complete problem. Therefore, a cooperation control framework is expected to achieve the aforementioned requirements in the presence of varied human motion intentions.

To achieve safe and conflict-free collaboration between a human and a robot in co-carrying tasks, impedance control-based approaches have been adopted. Impedance control and admittance control are characterized by the generation of robot motion in compliance with external forces, rendering it well-suited for situations where the human takes the lead. Specifically, impedance control manages the interaction force of the robot arm's end-effector by utilizing the desired trajectory, while the robot's states are controlled by admittance control with the contact force input <cit.>. Based on the original impedance control proposed in <cit.>, parallel and series admittance–impedance controllers have been accordingly developed in <cit.> and <cit.> to adapt to unstructured environments.  The advanced impedance-based control frameworks in
 <cit.> successfully resolved the human–robot interaction issue under unknown system dynamics and environments.  Impedance learning controls were proposed in <cit.>, guaranteeing interaction force compliance and human workload reduction via adaptive laws. Moreover, an online adaptive admittance control in <cit.> has been proposed to ensure stable interaction and minimize human workload. The study <cit.> presents a smooth cooperation scheme for physical human–robot interaction systems by integrating admittance control and the dynamical system method. It is worth emphasizing that this framework enables the robot to not only respond to human guidance with appropriate compliant behavior but also re-plan its motion reactively. Regarding the collaborative transportation of unknown deformed objects, the study <cit.> introduced an adaptive control framework using admittance control, a reference generator, and kinematic data of the human obtained from a motion capture system. By improving the control method in <cit.>, a human–multiple robots cooperative framework <cit.> has been proposed for large deformation-agnostic objects. As seen above, impedance control <cit.> and admittance control <cit.> are useful to achieve safe motion by effectively complying with external forces in the context that the human operator always leads the movement. In fact, when the target dynamics of admittance control are modeled using a mass–damper system without incorporating a spring term, the passivity of the closed-loop system can be proven <cit.>. On the contrary, to achieve more active involvement of the robot such as in faster and less forceful co-carrying tasks, the spring term is integrated. This term requires an understanding of the human's intended motion to serve as the equilibrium point for the spring. Admittance control with mass–damper–spring features can cause human–robot conflicts if the accuracy of human intention estimation is low. By applying such controllers, the passivity of the closed-loop system cannot be ensured <cit.>.   

There have been some controllers that inherently satisfy passivity while achieving path-tracking control. Leveraging the energy-based approach, the studies in <cit.>  investigated the passive characteristics of closed-loop systems, guaranteeing that robotic systems maintain a limited energy level when interacting with a human operator and robot manipulator. In <cit.>, a passive bilateral feedforward control approach was developed for teleoperator applications, which simultaneously ensures the passivity of the closed-loop system and the robustness under measurement noise and uncertainty. The concept of a virtual energy tank has been adopted in <cit.> to limit the energy level of the system and guarantee passivity. The former study <cit.> focused on sharing human–robot authority in collaborative robotic systems and resolving delayed communication channels, meanwhile, the latter <cit.> developed a time-varying compliance controller formulation with an arbitrary number of prioritized tasks and added a constrained layer into the virtual energy tank, improving the flexibility and safety of the proposed method. In <cit.>, an interactive control method for teleoperated robotic surgery was developed by integrating the port-Hamiltonian framework and the concept of energy tanks, which enables flexible and passivity-preserving energy flow in the controlled system. Although the energy tank-based control in <cit.> underscored the crucial significance of passivity, conflicts in direction of human–robot co-carrying tasks can appear due to the radial reduction phenomenon in conventional trajectory tracking. To resolve the aforementioned problem, the Passive Velocity Field Control (PVFC) in <cit.> is appraised as a potential solution for controlling robot arms due to its adoption of the Velocity Field concept while guaranteeing the passivity of the closed-loop system. By augmenting a fictitious flywheel as an energy storage component, the PVFC in <cit.> fulfills the passive property and global exponential stability of the augmented system. In <cit.> and <cit.>, the PVFC with an energy compensation control term was designed to address the increase in kinetic energy and power flow into the patient in therapeutic applications.
The PVFC in <cit.> showed remarkable advantages in contour following issues and ensuring safe operation by maintaining the passive relationship between the robot arm under the closed-loop control system and physical environments. However, the PVFCs reported in <cit.> cannot be applied directly in human–robot co-carrying tasks considering human–robot conflicts. Indeed, the robot movement should be continuously and progressively adjusted to align with human intentions under conflicts, because the behavior of the robot arm in co-carrying tasks is determined by control inputs and human forces. This indicates that the robot requires a temporal interval to synchronize with human intentions while managing the interplay between control inputs and human forces. Therefore, to address the aforementioned issue, the purpose of the present study is to advocate integrating a reference generator with PVFC for collaboratively transporting objects.

Inspired by the above papers and analysis, the admittance control in <cit.>  is capable of generating appropriate compliant trajectories using human–robot interaction forces. Moreover, the PVFC in <cit.> is an energy-based control approach designed to ensure passivity while enabling the robot arm to perform human–robot collaboration tasks. Therefore, a robot controller combining admittance control and PVFC is investigated and applied for human–robot co-carrying tasks in our study to guarantee synchronized human–robot movement and safe interaction simultaneously. Based on this combined structure, the admittance control and PVFC are also modified and extended to enhance the continuity of the control system, flexibility, and the regulating ability of energy level. Specifically, the proposed method not only re-plans the desired co-carrying path for the robot based on conflict information but can also resolve the switching issue in <cit.> that can cause unstable behavior. Adopting human–robot interaction forces as a trigger to generate movements in <cit.>, resulting in increasing human workload; this issue is also handled by our method. Although the studies in <cit.> showed the remarkable advantages of time-invariant PVFC for rehabilitation or exercise machines, it is necessary to develop a time-varying PVFC due to varied human motion intentions in human–robot co-carrying tasks. Furthermore, we also expand the PVFC in <cit.> by supplementing a fractional exponent energy control term to maintain the energy level of the system and compensate for the kinetic energy within a finite time interval. Hence, a new cooperation control framework utilizing admittance control and a time-varying PVFC with a fractional exponent energy compensation control term is constructed for human–robot co-carrying tasks. This framework enables the robot partner to re-plan the trajectory by adopting conflict information, follow accurate co-carrying motions, guarantee passivity, and control the energy level of the closed-loop system.  In regard to the above, the main contributions of the paper are highlighted as follows. 


  * Compliance with the human–robot interaction force and adjustment of the co-carrying trajectory for the robot partner are both realized by admittance control in the case of human–robot conflicts. Moreover, the time-varying velocity field is proposed to encode the varied human motion intentions in co-carrying tasks. 

  * By augmenting a fractional exponent energy control term, the new PVFC not only deals with kinetic energy decrease and increase phenomena but also guarantees compensation capability within a finite time interval.

  * The passivity of the closed-loop system is achieved by our proposed approach when the kinetic energy exceeds the predetermined level. Furthermore, power flow into the human operator and co-transported object is controlled by designing appropriate parameters. The stability of our cooperative control framework is proven by utilizing the Lyapunov theory. 


The rest of the paper is structured in four sections. The problem of human–robot co-carrying tasks and preliminary theories are introduced in the second section. The main innovations are presented in Section III, including the cooperation control framework and its results. In Section IV, the effectiveness of the proposed method is validated through numerical simulations implemented in a co-carrying task across four different scenarios. In Section V, the advantages of the proposed method and application scope are discussed. Finally, the conclusions and future work are presented in the last section.









§ PROBLEM DEFINITION AND PRELIMINARIES



 §.§ Problem Definition


In this paper, we consider co-carrying situations where a human operator and a robot arm grab a single object firmly, which is depicted in Fig. <ref>. For this collaboration scenario, we assume that the estimated human motion intentions are somehow accessible; however, errors or measurement noises may occasionally be present in these estimations. Therefore, three interaction states of human–robot co-carrying tasks are defined as follows.


  * State 1 (Conflict in Direction): Conflict in direction occurs due to the difference in collaborative transportation trajectory (Fig. <ref>). Specifically, the estimated human intention for the robot includes errors.

  * State 2 (Harmonious Movement): 
As shown in Fig. <ref>, the co-carrying trajectory of the robot partner aligns with the human motion intention, thereby generating harmonious human–robot movement. 

  * State 3 (Conflict in Parking Location): In Fig. <ref>, the human suddenly changes his/her intention to stop at a target location, meanwhile, the robot partner continuously follows the previous co-carrying path that conflicts with the new human intention. 

According to the human–robot co-carrying task with the three interaction states, we expect to design a cooperation control framework that satisfies the following features.


  * If the estimated human motion is sufficiently accurate, the robot partner moves the co-carrying object proactively with small human–robot interaction forces. On the contrary, the robot detects conflicts and re-generates the trajectory based on the conflict information. 

  * The robot system is strictly passive with respect to the external force–velocity pair when the kinetic energy is higher than the desired one. The kinetic energy of the entire system converges to the predetermined energy level in a finite time interval. Moreover, the power flow from the robot to the human and vice versa can be reduced. 

  * In the absence of external forces, the position and velocity of the robot system converge to the human intended position and desired time-varying velocity field, respectively. 



 §.§ Preliminaries

 

<cit.> A dynamic system is strictly passive with respect to the pair of input 𝐮 and output 𝐲 if there is a positive definite storage function S( 𝐳) and a positive definite dissipative function D( 𝐳) such that the following relationship holds for all t≥ 0: 

    S( 𝐳( t ) )-S( 𝐳( 0 ) )≤∫_0^t𝐲^⊤( τ)𝐮( τ)dτ -∫_0^tD( 𝐳( τ) )dτ,

in which 𝐳 is the state vector.



<cit.> The following inequality holds for any real number that satisfies z_i,  i=1,… ,n and 0<r<1:  

    ∑_i=1^n| z_i|^r≥( ∑_i=1^n| z_i|)^r




(Cauchy–Schwarz inequality) <cit.> For arbitrary 𝐀,  𝐁∈ℝ^n,
the following inequality holds:  

    ( 𝐀^⊤𝐁)^2≤( 𝐀^⊤𝐀)( 𝐁^⊤𝐁)






§ PROPOSED METHODOLOGY




 §.§ System Description

The dynamics of a fully actuated n-link robot manipulator is presented as,

    𝐌( 𝐪)𝐪̈+𝐂( 𝐪,𝐪̇)𝐪̇=τ+τ_ext,

in which 𝐪=[ q_1   q_2   …   q_n
]^⊤∈ℝ^n is the joint displacement vector, 𝐌( 𝐪) and 𝐂( 𝐪,𝐪̇)∈ℝ^n× n denote the inertia, Coriolis and centrifugal matrices, respectively. τ and τ_ext∈ℝ^n are the control torque acting at the actuators and external torque from the interaction with the human operator, correspondingly. To guarantee the passivity of the system, the robot manipulator model in (<ref>) is augmented with a fictitious flywheel state that serves as a fictitious energy storage component. The dynamics of the fictitious flywheel is defined as:

    m_fq̈_f=τ_f,

where m_f and q_f represent the mass and angular position of the flywheel, τ_f is the control torque exerted on the flywheel. By combining the dynamic model of the robot manipulator in (<ref>) and fictitious flywheel in (<ref>), an augmented system is given as,

    𝐌^a( 𝐪^a)𝐪̈^a+𝐂^a( 𝐪^a,𝐪̇^a)𝐪̇^a=τ^a+τ_ext^a,

in which 𝐪^a=[ q_1   q_2   …   q_n   q_f

]^⊤∈ℝ^n+1 represents the joint angle vector, τ^a=[ τ^⊤   τ_f
]^⊤  and τ_ext^a=[ τ_ext^⊤    0  
]^⊤ are the augmented control input and external force vector, respectively. 𝐌^a( 𝐪^a) =[ 𝐌( 𝐪)    0_n× 1
0_1× n   m_f
]
and 𝐂^a( 𝐪^a,𝐪̇^a)=[ 𝐂( 𝐪,𝐪̇)    0_n× 1
0_1× n    0  
]  are the augmented inertia matrix and Coriolis matrix, correspondingly. 𝐌^a denotes a positive-definite symmetric matrix and ( Ṁ^a( 𝐪^a)-2𝐂^a( 𝐪^a,𝐪̇^a) ) is a skew-symmetric matrix.


 §.§ Cooperation Control Framework Design


In this section, a cooperation control framework with two parts is designed for collaboratively transporting a rigid object as shown in Fig. <ref>, including a time-varying Velocity Field Generation part and a PVFC part.

Regarding the first part, an Admittance Control with mass–damper (MD) features is presented to modify the desired co-carrying trajectory for the robot partner according to the conflict detection through the force sensor signal 𝐟_ext.

    𝐌_d( ẍ_a-ẍ̂̈_h)+𝐃_d( ẋ_a-ẋ̂̇_h)=𝐟_ext,

where 𝐌_d and 𝐃_d are inertia and damping matrices, respectively. 𝐱_a represents the desired trajectory obtained from (<ref>) and 𝐱̂_h is the estimated human motion intention. 𝐟_ext denotes the external force that is applied by the human. Based on (<ref>), the characteristics of the proposed Admittance Control are stated as follows:


  * In the case of estimating human intention precisely, the human–robot interaction force (𝐟_ext) is very small or ideally zero; therefore, the desired trajectory (𝐱_a) closely aligns with the estimated human intention (𝐱̂_h); and


  * the desired co-carrying trajectory (𝐱_a) is modified according to the interaction force (𝐟_ext) on the contrary.

By utilizing the Admittance Control output ( ẋ_a), the time-varying Velocity Field ( 𝐕( 𝐪,t ) ) of the robot manipulator in (<ref>) is created as a sliding surface. 

    {   𝐐̇( t )=𝐉^+( 𝐪)ẋ_a( t ) 
       𝐕( 𝐪,t )=𝐐̇( t )-𝐊_1( 𝐪( t )-𝐐( t ) ), 
    .

in which 𝐉^+( 𝐪)=( 𝐉^⊤( 𝐪)𝐉( 𝐪) )^-1𝐉^⊤( 𝐪) with the Jacobian matrix 𝐉( 𝐪) and 𝐐( t ) is the reference angular position. 𝐊_1 denotes the positive-definite control parameter matrix. Additionally, the desired Velocity Field V_f( 𝐪,t )  of the fictitious flywheel is calculated as:

    V_f( 𝐪,t )=√(2/m_f( E^a-1/2𝐕^⊤( 𝐪,t )𝐌( 𝐪)𝐕( 𝐪,t ) ))

Note that E^a denotes a sufficiently large positive constant, which ensures the formulation under the square root is positive. As a result, the desired time-varying Velocity Field 

( 𝐕^a( 𝐪,t )=[ 𝐕^⊤( 𝐪,t )    V_f( 𝐪,t )  
]^⊤) of the augmented system in (<ref>), which satisfies consistency and the following conservation of kinetic energy principle, is defined.

    k^a( 𝐪^a,  𝐕^a( 𝐪,t ) )=1/2𝐕^a^⊤( 𝐪,t )𝐌^a( 𝐪^a)𝐕^a( 𝐪,t ) 
                       =1/2𝐕^⊤( 𝐪,t )𝐌( 𝐪)𝐕( 𝐪,t )+1/2m_fV_f^2( 𝐪,t ) 
                       =E^a>0,

in which k^a( 𝐪^a,  𝐕^a( 𝐪,t ) ) is the kinetic energy of the desired Velocity Field. Thereafter, the actual kinetic energy (k^a( 𝐪^a,  𝐪̇^a)) of the augmented system is computed as,

    k^a( 𝐪^a,  𝐪̇^a)=1/2𝐪̇^a⊤𝐌^a( 𝐪^a)𝐪̇^a
                                     =1/2𝐪̇^⊤𝐌( 𝐪)𝐪̇+1/2m_fq̇_f^2,


Concerning the second part of the cooperation control framework, a new PVFC is designed to guarantee the passivity of the closed-loop system. The approach in <cit.> is inherited in order to control the power flow into the human operator, such that it enhances the safety of the human and co-transported object. Furthermore, we newly propose a fractional exponent control term to compensate for the kinetic energy of the augmented system in a finite time interval.
Based on (<ref>)–(<ref>), the new PVFC with a fractional exponent energy compensation control term  is developed for the τ^a( 𝐪^a,  𝐪̇^a,t ) of the augmented system in (<ref>) as, 

    τ^a( 𝐪^a,  𝐪̇^a,t )=[ 𝐆( 𝐪^a,  𝐪̇^a,t )+𝐑( 𝐪^a,  𝐪̇^a,t ) ]𝐪̇^a
                                                                               -𝐒( 𝐪^a,  𝐪̇^a)[ 𝐪̇^a]^r_1/r_2,

in which 𝐒( 𝐪^a,  𝐪̇^a)[ 𝐪̇^a]^r_1/r_2  is utilized to compensate for the kinetic energy of the augmented system within a finite time interval. Two skew-symmetric matrices 𝐆( 𝐪^a,  𝐪̇^a,t ), 𝐑( 𝐪^a,  𝐪̇^a,t ), and a diagonal matrix 𝐒( 𝐪^a,  𝐪̇^a) are designed in the following formulation.

    {   𝐆( 𝐪^a,  𝐪̇^a,t ):=1/2E^a( 𝐰𝐏^⊤-𝐏𝐰^⊤) 
       𝐑( 𝐪^a,  𝐪̇^a,t ):=κ( 𝐏𝐩^⊤-𝐩𝐏^⊤) 
       𝐒( 𝐪^a,  𝐪̇^a):=s( k^a( 𝐪^a,  𝐪̇^a)-k_d^a)𝐊_2, 
    .

where κ is control parameters and k_d^a is the desired kinetic energy level of the augmented system. Furthermore, to avoid clutter, 𝐰=𝐰( 𝐪^a,  𝐪̇^a,t ), 𝐏=𝐏( 𝐪^a,t ), and  𝐩=𝐩( 𝐪^a,  𝐪̇^a) are utilized in the paper. The inverse dynamics with the desired time-varying Velocity Field (𝐰), desired momentum (𝐏), and actual momentum (𝐩) of the augmented system in (<ref>), as well as the diagonal positive-definite matrix (𝐊_2) are defined as:

    {   𝐰=𝐌^a( 𝐪^a)𝐕̇^a( 𝐪,t )+𝐂^a( 𝐪^a,  𝐪̇^a)𝐕^a( 𝐪,t ) 
       𝐏=𝐌^a( 𝐪^a)𝐕^a( 𝐪,t ) 
       𝐩=𝐌^a( 𝐪^a)𝐪̇^a
       𝐊_2=diag( k_21,k_22,… ,k_2n,k_2f), 
    .

where the i-th term of the differential of the time-varying Velocity Field is computed as follows,

    V̇_i^a( 𝐪,t )=∑_j=1^n+1∂ V_i^a( 𝐪,t )/∂ q_j^aq̇_j^a+∂ V_i^a( 𝐪,t )/∂ t.

Additionally, the fractional exponent of the augmented velocity in (<ref>) is defined by:

    [ 𝐪̇^a]^r_1/r_2=[  sgn( q̇_1)| q̇_1|^r_1/r_2    sgn( q̇_2)| q̇_2|^r_1/r_2   …
    . 
                                                .  sgn( q̇_n)| q̇_n|^r_1/r_2    sgn( q̇_f)| q̇_f|^r_1/r_2
    ]^T,

with r_1, r_2 ( r_1<r_2) are  positive odd integers. The smooth saturation function in (<ref>) is designed to compensate for the kinetic energy of the augmented system.

    s( e )={   η_min                                                                 e<δ_1
       η_min/2( 1-cos( π/δ_1e ) )          δ_1≤ e<0 
        0                                                                            e=0
       η_max/2( 1-cos( π/δ_2e ) )         0<e≤δ_2 
       η_max                                                                  e>δ_2
    .

where e=k^a( 𝐪^a,  𝐪̇^a)-k_d^a and η_min<0, η_max>0, δ_2>0, δ_1<0 are constants.



 §.§ Main Results


 (Passivity)
Leveraging the action of control commands in (<ref>) for the augmented system in (<ref>), the kinetic energy of the system in (<ref>) satisfies:

    d/dtk^a( 𝐪^a,  𝐪̇^a)=τ_ext^a⊤𝐪̇^a
                     -s( k^a( 𝐪^a,  𝐪̇^a)-k_d^a)𝐪̇^a⊤𝐊_2[ 𝐪̇^a]^r_1/r_2,

in which r_1, r_2 ( r_1<r_2) are positive odd integers and 𝐊_2 is a positive-definite matrix. According to Definition <ref>, the closed-loop system is strictly passive with respect to the pair of the velocity output ( 𝐪̇^a) and external force input ( τ_ext^a) under the condition k^a( 𝐪^a,  𝐪̇^a)-k_d^a≥ 0.



 (Convergence time of kinetic energy)
Under the new PVFC in (<ref>) with positive odd integers ( r_1, r_2  ( r_1<r_2) ) and positive-definite matrix ( 𝐊_2), the kinetic energy of the augmented system k^a( 𝐪^a,𝐪̇^a) converges to the desired one k_d^a within a finite time interval in the absence of external disturbance ( τ_ext^a=0 ). The settling time of the system ( T ) is bounded by,

    T≤max{T_1,T_2},

where T_1 and T_2 are correspondingly the convergence times with k^a( 𝐪^a, 𝐪̇^a)≥ k_d^a and k^a( 𝐪^a,𝐪̇^a)<k_d^a, which are specifically calculated in the next subsection.  



 (Stability)
By applying the control inputs of the new time-varying PVFC in (<ref>) and (<ref>) and designing the positive-definite control parameters ( 𝐊_2,  κ),  the solutions

    {   𝐞_𝐪=𝐪( t )-𝐐( t )=0
       𝐞_𝐯=𝐪̇^a( t )-α( t )𝐕^a(𝐪^a( t ), t )=0
    .

to the augmented system in (<ref>) is Lyapunov stable in the absence of external disturbance ( τ_ext^a=0 ). The α( t ) term is defined such that:

    α( t )=√(k^a( 𝐪^a( t ),𝐪̇^a( t ) )/E^a).




 (Power flow)
When the external force ( τ_ext^a) is applied by the human operator, the power flow into the human and robot is calculated by the augmented system in (<ref>) and the control command in (<ref>) as follows:  

    P_r2h  =-s( k^a( 𝐪,𝐪^a)-k_d^a)𝐪̇^aT𝐊_2[ 𝐪̇^a]^r_1/r_2-k̇^a,

in which P_r2h >0 with k^a( 𝐪,𝐪^a)<k_d^a  is the power flow from the robot to the human; meanwhile, P_r2h <0 with k^a( 𝐪,𝐪^a)≥ k_d^a  denotes the power flowing from the human to the robot. By adjusting the control parameters ( 𝐊_2,  η_max,   η_min,  δ_1,  δ_2) in (<ref>) and (<ref>), we can reduce the power flow into the human operator, robot partner, and co-transported object.









 §.§ Proof


Differentiating the kinetic energy in (<ref>) and utilizing the augmented system in (<ref>), yields: 

    d/dtk^a( 𝐪^a,  𝐪̇^a) 
        =𝐪̇^a⊤𝐌^a𝐪̈^a+1/2𝐪̇^a⊤Ṁ^a𝐪̇^a
        =1/2𝐪̇^a⊤Ṁ^a𝐪̇^a+𝐪̇^a⊤( τ^a+τ_ext^a-𝐂^a( 𝐪^a,𝐪̇^a)𝐪̇^a).

Substituting the new time-varying PVFC in (<ref>) into (<ref>), we obtain,

    d/dtk^a( 𝐪^a,  𝐪̇^a)=1/2𝐪̇^a⊤( Ṁ^a-2𝐂^a( 𝐪^a,𝐪̇^a) )𝐪̇^a
                         +𝐪̇^a⊤𝐑( 𝐪^a,  𝐪̇^a,t )𝐪̇^a+𝐪̇^a⊤𝐆( 𝐪^a,  𝐪̇^a,t )𝐪̇^a
                         +𝐪̇^a⊤τ_ext^a-𝐪̇^a⊤𝐒( 𝐪^a,  𝐪̇^a)[ 𝐪̇^a]^r_1/r_2,

in which Ṁ^a-2𝐂^a( 𝐪^a,𝐪̇^a),  𝐆( 𝐪^a,𝐪̇^a,t ),  𝐑( 𝐪^a,𝐪̇^a,t ) are skew-symmetric matrices; therefore, equation (<ref>) becomes:

    d/dtk^a( 𝐪^a,  𝐪̇^a) 
        =𝐪̇^a⊤τ_ext^a-𝐪̇^a⊤𝐒( 𝐪^a,  𝐪̇^a)[ 𝐪̇^a]^r_1/r_2
        =τ_ext^a⊤𝐪̇^a-s( k^a( 𝐪^a,  𝐪̇^a)-k_d^a)𝐪̇^a⊤𝐊_2[ 𝐪̇^a]^r_1/r_2.

By integrating both sides of (<ref>) over the time interval [ 0,t ], we have: 

    ∫_0^t𝐪̇^a⊤τ_ext^adτ=k^a( 𝐪^a( t ),𝐪̇^a( t ) )-k^a( 𝐪^a( 0 ),𝐪̇^a( 0 ) ) 
                                                                          +∫_0^t𝐪̇^a⊤𝐒( 𝐪^a,𝐪̇^a)[ 𝐪̇^a]^r_1/r_2dτ.

Obviously, if  r_1, r_2  ( r_1<r_2) are positive odd intergers and 𝐊_2  denotes positive-definite matrix, k^a( 𝐪^a,  𝐪̇^a)>0 and ∫_0^t𝐪̇^a⊤𝐒( 𝐪^a,  𝐪̇^a)[ 𝐪̇^a]^r_1/r_2dτ>0 under k^a( 𝐪^a,𝐪̇^a)-k_d^a≥ 0. Therefore, the closed-loop system is strictly passive when the kinetic energy exceeds the predetermined level. 




According to Lyapunov's direct method in <cit.>, a Lyapunov function is selected  for examining the convergence of  the kinetic energy as follows:  

    V_1( 𝐪^a,  𝐪̇^a)=1/2( k^a( 𝐪^a,𝐪̇^a)-k_d^a)^2.

Taking the derivative of (<ref>) with respect to time and combining it with the kinetic energy in (<ref>), we have:

    V̇_1( 𝐪^a,  𝐪̇^a) 
        =( k^a( 𝐪^a,𝐪̇^a)-k_d^a)( k̇^a( 𝐪^a,𝐪̇^a)-k̇_d^a) 
        =( k^a( 𝐪^a,𝐪̇^a)-k_d^a)( 𝐪̇^a⊤𝐌^a𝐪̈^a+1/2𝐪̇^a⊤Ṁ^a𝐪̇^a).

Substituting the augmented system (<ref>) into (<ref>), yields:

    V̇_1=( k^a( 𝐪^a,𝐪̇^a)-k_d^a)(     𝐪̇^a⊤( τ^a+τ_ext^a-𝐂^a𝐪̇^a) 
                            +1/2𝐪̇^a⊤Ṁ^a𝐪̇^a
    )

Considering τ_ext^a=0 and adopting the time-varying PVFC in (<ref>), we obtain,

    V̇_1=( k^a( 𝐪^a,𝐪̇^a)-k_d^a)(    1/2𝐪̇^a⊤( Ṁ^a-2𝐂^a)𝐪̇^a
        +𝐪̇^a⊤𝐆( 𝐪^a,  𝐪̇^a,t )𝐪̇^a
        +𝐪̇^a⊤𝐑( 𝐪^a,  𝐪̇^a,t )𝐪̇^a
        -𝐪̇^a⊤𝐒( 𝐪^a,  𝐪̇^a)[ 𝐪̇^a]^r_1/r_2
    )

It is straightforward to derive the following formulation using the properties of skew-symmetric matrices.

    V̇_1=-( k^a( 𝐪^a,𝐪̇^a)-k_d^a)( 𝐪̇^a⊤𝐒( 𝐪^a,  𝐪̇^a)[ 𝐪̇^a]^r_1/r_2) 
              =-( k^a( 𝐪^a,𝐪̇^a)-k_d^a)s( e )( 𝐪̇^a⊤𝐊_2[ 𝐪̇^a]^r_1/r_2)

Based on the saturation equation (<ref>), (<ref>) is evaluated as,

    V̇_1 ≤ -λ_min( s( e ) )| k^a( 𝐪^a,𝐪̇^a)-k_d^a|L( 𝐪̇^a) 
               ≤ -λ_min( s( e ) )√(2V_1( 𝐪^a,𝐪̇^a))L( 𝐪̇^a),

in which L( 𝐪̇^a)=𝐪̇^a⊤𝐊_2[ 𝐪̇^a]^r_1/r_2 and λ_min( s( e ) )=min{| η_min|/2( 1-cos( π/δ_1e ) ),η_max/2( 1-cos( π/δ_2e ) ) }. Each term in  𝐌^a( 𝐪^a) and 𝐊_2 is a trigonometric function of the angular position or constant. Moreover,  𝐌^a( 𝐪^a) is symmetric and positive definite. 𝐊_2 is diagonal  and positive definite. Therefore, the above matrices satisfy the following inequality.

    {   k^a( 𝐪^a,  𝐪̇^a)=1/2𝐪̇^a⊤𝐌^a𝐪̇^a≤1/2λ_max( 𝐌^a)𝐪̇^a_2^2
       k^a( 𝐪^a,  𝐪̇^a)=1/2𝐪̇^a⊤𝐌^a𝐪̇^a≥1/2λ_min( 𝐌^a)𝐪̇^a_2^2, 
    .


    {   𝐪̇^a⊤𝐊_2[ 𝐪̇^a]^r_1/r_2≤λ_max( 𝐊_2)( 𝐪̇^a_r_1+r_2/r_2^)^r_1+r_2/r_2
       𝐪̇^a⊤𝐊_2[ 𝐪̇^a]^r_1/r_2≥λ_min( 𝐊_2)( 𝐪̇^a_r_1+r_2/r_2^)^r_1+r_2/r_2
    .

where λ_min( ∙) and λ_max( ∙) are the smallest and largest eigenvalues of the matrix ( ∙). Selecting the positive odd integers r_1, r_2 ( r_1<r_2) and applying (<ref>), Lemma <ref>, and (<ref>) for L( 𝐪̇^a), respectively, yields,

    L( 𝐪̇^a) ≥λ_min( 𝐊_2)( 𝐪̇^a_r_1+r_2/r_2^)^r_1+r_2/r_2
                       ≥λ_min( 𝐊_2)( 𝐪̇^a_2^2)^r_1+r_2/2r_2
                      ≥λ_min( 𝐊_2)( 2k^a( 𝐪^a,𝐪̇^a)/λ_max( 𝐌^a))^r_1+r_2/2r_2

Substituting (<ref>) into (<ref>), we obtain, 

    V̇_1≤ -λ_min( s( e ) )λ_min( 𝐊_2)√(2V_1)( 2k^a( 𝐪^a,𝐪̇^a)/λ_max( 𝐌^a))^r_1+r_2/2r_2

Thus, by designing appropriate parameters, V_1( 𝐪^a,  𝐪̇^a) is positive-definite, V̇_1( 𝐪^a,  𝐪̇^a) is negative definite, and V̇_1( 𝐪^a,  𝐪̇^a) =0 if and only if k^a( 𝐪^a,𝐪̇^a)-k_d^a=0. To compute the convergence time of the kinetic energy, the following two scenarios are considered.

For k^a( 𝐪^a, 𝐪̇^a)≥ k_d^a, combining (<ref>) with (<ref>), yields:

    V̇_1  ≤ -ψ√(2V_1)( √(2V_1)+k_d^a)^r_1+r_2/2r_2

with ψ =2^r_1+r_2/2r_2λ_min( s( e ) )λ_min( 𝐊_2)/( λ_max( 𝐌^a) )^r_1+r_2/2r_2. We rewrite (<ref>) in the following form.

    dt  ≤ -dV_1/ψ√(2V_1)( √(2V_1)+k_d^a)^r_1+r_2/2r_2
               ≤ -d( √(2V_1)+k_d^a)/ψ( √(2V_1)+k_d^a)^r_1+r_2/2r_2

Taking the integration (<ref>), we obtain:

    ∫_0^T_1dt  ≤ -∫_V_1( 0 )^0d( √(2V_1)+k_d^a)/ψ( √(2V_1)+k_d^a)^r_1+r_2/2r_2

Calculating (<ref>), the convergence time of the first scenario is presented as,

    T_1≤2r_2/ψ( r_2-r_1)( ( √(2V_1( 0 ))+k_d^a)^r_2-r_1/2r_2-( k_d^a)^r_2-r_1/2r_2).


For k^a( 𝐪^a,𝐪̇^a)<k_d^a, substituting (<ref>) into (<ref>), the second scenario is given as follows:

    V̇_1  ≤ -ψ√(2V_1)( k_d^a-√(2V_1))^r_1+r_2/2r_2

According to the same approach in (<ref>) and (<ref>), the convergence time for the second scenario is computed as follows.

    T_2≤2r_2/ψ( r_2-r_1)( ( k_d^a)^r_2-r_1/2r_2-( k_d^a-√(2V_1( 0 )))^r_2-r_1/2r_2).

Thus, the kinetic energy of the augmented system converges to the predetermined level within the finite time interval in (<ref>) and (<ref>). 

In order to prove Theorem <ref>, two supplementary lemmas are utilized.


According to the α( t ) term in (<ref>) and settling time in (<ref>), the differential of α( t ) holds in the absence of external disturbance  ( τ_ext^a=0 ).  

    t→ Tlim α̇( t )=0.



By calculating the limitation of (<ref>) over the settling time interval [ 0,T ], we obtain:

    t→ Tlim α( t )=t→ Tlim √(k^a( 𝐪^a( t ),𝐪̇^a( t ) )/E^a)=√(k_d^a/E^a).

Because the limit in (<ref>) is constant; therefore, α̇( t ) converges to zero if only t→ T. 



The following formulations hold:  

    𝐰^⊤𝐕^a( 𝐪,t )=0,


    𝐆( 𝐪^a,  𝐪̇^a,t )𝐪̇^a-α(t) 𝐰  =𝐆( 𝐪^a,  𝐪̇^a,t )𝐞_𝐯.



Inspired by the time-invariant PVFC <cit.>, Lemma <ref> is extended for our time-varying PVFC. As a result, the expressions in (<ref>) and (<ref>) are satisfied. 
 

The following Lyapunov function is selected to analyze the system stability using Lyapunov's direct method in <cit.>.

    V_2( 𝐪^a,  𝐪̇^a)=1/2𝐞_𝐪^⊤𝐞_𝐪^+1/2𝐞_𝐯^⊤𝐌^a𝐞_𝐯^>0.

Differentiating (<ref>) and adopting the augmented system in (<ref>) and Lemma <ref>, we have: 

    V̇_2( 𝐪^a,𝐪̇^a)=𝐞_𝐪^⊤ė_𝐪^+𝐞_𝐯^⊤𝐌^aė_𝐯^+1/2𝐞_𝐯^⊤Ṁ^a𝐞_𝐯^
                                  =1/2𝐞_𝐯^⊤Ṁ^a𝐞_𝐯^+𝐞_𝐪^⊤( 𝐪̇-𝐐̇) 
                                         +𝐞_𝐯^⊤( τ^a+τ_ext^a-𝐂^a( 𝐪^a,𝐪̇^a)𝐪̇^a) 
                                                                   -α( t )𝐞_𝐯^⊤𝐌^a𝐕̇^a( 𝐪^a,t )

Substituting the time-varying velocity field in (<ref>) and the new PVFC in (<ref>) into (<ref>) in absence of external force, yields: 

    V̇_2=1/2𝐞_𝐯^⊤Ṁ^a𝐞_𝐯^+𝐞_𝐪^⊤( 𝐐̇-𝐊_1( 𝐪-𝐐)-𝐐̇) 
                 +𝐞_𝐯^⊤( 𝐆( 𝐪^a,  𝐪̇^a,t )𝐪̇^a+𝐑( 𝐪^a,  𝐪̇^a,t )𝐪̇^a) 
                 -𝐞_𝐯^⊤𝐂^a( 𝐪^a,𝐪̇^a)( 𝐞_𝐯^+α( t )𝐕^a( 𝐪^a,t ) ) 
                 -𝐞_𝐯^⊤𝐒( 𝐪^a,  𝐪̇^a)[ 𝐪̇^a]^r_1/r_2-α( t )𝐞_𝐯^⊤𝐌^a𝐕̇^a( 𝐪^a,t )

We rewrite (<ref>) by using the definition in (<ref>). 

    V̇_2 =1/2𝐞_𝐯^⊤( Ṁ^a-2𝐂^a( 𝐪^a,𝐪̇^a) )𝐞_𝐯^-𝐞_𝐪^⊤𝐊_1𝐞_𝐪^
               +𝐞_𝐯^⊤𝐑( 𝐪^a,  𝐪̇^a,t )𝐪̇^a-𝐞_𝐯^⊤𝐒( 𝐪^a,  𝐪̇^a)[ 𝐪̇^a]^r_1/r_2
               +𝐞_𝐯^⊤( 𝐆( 𝐪^a,  𝐪̇^a,t )𝐪̇^a-α( t )𝐰)

Applying Lemma <ref>, we obtain:

    V̇_2=1/2𝐞_𝐯^⊤( Ṁ^a-2𝐂^a( 𝐪^a,𝐪̇^a) )𝐞_𝐯^-𝐞_𝐪^⊤𝐊_1𝐞_𝐪^
               + 𝐪̇^a⊤𝐑( 𝐪^a,  𝐪̇^a,t )𝐪̇^a
               -α( t )𝐕^a⊤( 𝐪^a,t )𝐑( 𝐪^a,  𝐪̇^a,t )𝐪̇^a 
               +𝐞_𝐯^⊤𝐆( 𝐪^a,  𝐪̇^a,t )𝐞_𝐯-𝐞_𝐯^⊤𝐒( 𝐪^a,  𝐪̇^a)[ 𝐪̇^a]^r_1/r_2

As Ṁ^a-2𝐂^a( 𝐪^a,𝐪̇^a),  𝐆( 𝐪^a,𝐪̇^a,t ),  𝐑( 𝐪^a,𝐪̇^a,t ) are skew-symmetric matrices. Equation (<ref>) is presented in the following form.

    V̇_2=-𝐞_𝐪^⊤𝐊_1𝐞_𝐪^-α( t )𝐕^a⊤( 𝐪^a,t )𝐑( 𝐪^a,  𝐪̇^a,t )𝐪̇^a 
                                                                             -𝐞_𝐯^⊤𝐒( 𝐪^a,  𝐪̇^a)[ 𝐪̇^a]^r_1/r_2

After the settling time in (<ref>), we substitute 𝐑( 𝐪^a,𝐪̇^a,t ) in (<ref>) into (<ref>). 

    V̇_2= -𝐞_𝐪^⊤𝐊_1𝐞_𝐪^
        -α( t )κ(    𝐕^a⊤( 𝐪^a,t )𝐌^a( 𝐪^a)𝐕^a( 𝐪^a,t )𝐪̇^a⊤𝐌^a( 𝐪^a)𝐪̇^a
                     -( 𝐕^a⊤( 𝐪^a,t )𝐌^a( 𝐪^a)𝐪̇^a)^2
    )

Adopting the Cauchy-Schwarz inequality in Lemma <ref>, the following formulation holds. 

    (    𝐕^a⊤( 𝐪^a,t )𝐌^a( 𝐪^a)𝐕^a( 𝐪^a,t )𝐪̇^a⊤𝐌^a( 𝐪^a)𝐪̇^a
                      -( 𝐕^a⊤( 𝐪^a,t )𝐌^a( 𝐪^a)𝐪̇^a)^2
    )≥ 0.

Thus, by designing the positive-definite control parameters ( 𝐊_2,  κ) and incorporating the conditions in  (<ref>) and (<ref>), 𝐞_𝐪=0 and 𝐞_𝐯=0 to the system is Lyapunov stable. 


Due to external force applied solely by the human operator, the power flow in co-carrying tasks is computed as follows:

    P_r2h=-𝐪̇^a⊤τ_ext^a.

By substituting the augmented system in (<ref>) and the new PVFC in (<ref>) into (<ref>), we obtain:

    P_r2h=-𝐪̇^a⊤( 𝐌^a( 𝐪^a)𝐪̈^a+𝐂^a( 𝐪^a,𝐪̇^a)𝐪̇^a) 
                          +𝐪̇^a⊤( 𝐆( 𝐪^a,  𝐪̇^a,t )𝐪̇^a+𝐑( 𝐪^a,  𝐪̇^a,t )𝐪̇^a) 
                          -𝐪̇^a⊤𝐒( 𝐪^a,  𝐪̇^a)[ 𝐪̇^a]^r_1/r_2
                   =1/2𝐪̇^a⊤( 𝐌^a-2𝐂^a( 𝐪^a,𝐪̇^a) )𝐪̇^a
                          +𝐪̇^a⊤( 𝐆( 𝐪^a,  𝐪̇^a,t )+𝐑( 𝐪^a,  𝐪̇^a,t ) )𝐪̇^a
                          -𝐪̇^a⊤𝐒( 𝐪^a,  𝐪̇^a)[ 𝐪̇^a]^r_1/r_2-d/dt( 1/2𝐪̇^aT𝐌^a𝐪̇^a)

Adopting the property of skew-symmetric matrices and combining it with the definition in (<ref>) and (<ref>), yields, 

    P_r2h=-𝐪̇^aT𝐒( 𝐪^a,  𝐪̇^a)[ 𝐪̇^a]^r_1/r_2-d/dt( 1/2𝐪̇^aT𝐌^a𝐪̇^a) 
                  =-s( k^a( 𝐪,𝐪^a)-k_d^a)𝐪̇^aT𝐊_2[ 𝐪̇^a]^r_1/r_2-k̇^a


For k^a( 𝐪^a, 𝐪̇^a)≥ k_d^a, the kinetic energy of the system increases due to the exertion of external force; therefore, k̇^a is positive. Based on the smooth
saturation function in (<ref>) and control parameter conditions ( 𝐊_2,r_1,r_2^), the first term in (<ref>) is also positive. As a result, P_r2h <0  is the power flow from the human operator to the robot partner. In the case of k^a( 𝐪^a,𝐪̇^a)<k_d^a, both k̇^a and s( k^a( 𝐪,𝐪^a)-k_d^a)𝐪̇^aT𝐊_2[ 𝐪̇^a]^r_1/r_2 in (<ref>) are negative; therefore, P_r2h >0  is the power flow from the robot to the human.

The kinetic energy of the augmented system experiences sudden increases or decreases due to the external force, resulting in the variation of k̇^a. By applying the new time-varying PVFC with the energy compensation term in (<ref>), the energy is immediately compensated, thereby reducing the magnitude of k̇^a. Meanwhile, the upper and lower bounds of the first term of (<ref>) are achieved by designing the control parameters ( 𝐊_2,r_1,r_2^) and components (η_max,   η_min,  δ_1,  δ_2) of the situation function in (<ref>). According to the aforementioned analyses, the power flow into the human operator, robot partner, and co-carrying object can be reduced. Moreover, based on (<ref>)-(<ref>), we also conclude that the power flow is zero under harmonious movement states with τ_ext^a=0. 



§ NUMERICAL SIMULATION

To validate the efficiency of the proposed method for human–robot co-carrying tasks, experiment by simulation is implemented in this section. 




 §.§ Simulation Setting


A fully actuated two-link robot manipulator is considered on a horizontal surface. The kinematics of the robot are presented in the following equation.

    𝐱( 𝐪)=[ l_1cos( q_1)+l_2cos( q_1+q_2)  
    l_1sin( q_1)+l_2sin( q_1+q_2)  
    ],

in which 𝐱=[ 
   x     y  
]^⊤ represents the position of the end-effector and the Jacobian matrix is computed as 𝐉( 𝐪)=∂𝐱/∂𝐪. The matrices of the dynamic model in (<ref>) are modeled as,

    𝐌( 𝐪)=[ M_1+M_2+2Rcos( q_2)    M_2+Rcos( q_2)  
    M_2+Rcos( q_2)    M_2
    ],  
    𝐂( 𝐪,𝐪̇)=[ 
       -Rq̇_2sin( q_2)     -R( q̇_1+q̇_2)sin( q_2)  
    
       Rq̇_1sin( q_2)     0  
    ],

where M_1=l_1^2( m_1/4+m_2)+I_1, M_2=m_2l_2^2/4+I_2, and R=m_2l_1l_2/2. The mass, length, and inertia of the two links are accordingly installed as: m_1=m_2=3.05  kg, l_1=l_2=0.4  m, and I_2=I_1=0.0414  kgm^2. The mass of the fictitious flywheel is m_f=10  kg. The initial conditions are set to 𝐪^a( 0 )=[ 
   -0.785     1.57     0  
]^⊤ rad, 𝐪̇^a( 0 )=[ 
   0.01     0.01     1  
]^⊤  rad/s, and 𝐱( 0 )=[ 
   0.5657     0  
]^⊤  m.

The human motion intention for the co-carrying task is described as follows:

    𝐱_h( t )={   [ 0.01t+0.5657,0.01t ]^⊤    t<10s  
       [ 0.6657,0.1 ]^⊤                           t≥ 10s
    ..

The human operator intends to move the co-transported object with a uniform speed ( 0.01√(2)  m/s) to the target position in the first 10 s. 
From 10 s, the human operator decides to stop at the target point ( 0.6657,0.1 ) m on the OXY surface. In this study, we assume that the external force is generated by the human operator as follows:

    𝐟_ext=[ 
       f_ext^x    f_ext^y
    ]^T
                =-𝐤_1^h( 𝐱-𝐱_h)-𝐤_2^h( ẋ-ẋ_h),

in which 𝐱 denotes the human hand position that is similar to the end-effector position. The two coefficients are designed as 𝐤_1^h=500𝐈_2× 2 and 𝐤_2^h=100𝐈_2× 2 with a unit matrix ( 𝐈_2× 2).


Additionally, to evaluate the efficiency of our proposed method, we implement human–robot co-carrying scenarios under unstructured environments. Therefore, the following scenarios according to the difference in estimated human intentions are considered, which directly affect the time-variant Velocity Field in (<ref>) and (<ref>).


  * Phase 1 (State 1: Conflict in Direction): The robot cannot measure/ estimate/ predict the human direction exactly in the initial time; therefore, the estimated human intention for the robot is given with errors in the following formulation. 

    𝐱̂_h=  {
       0.01t+0.5657+ξ_1+d  
    
       0.01t+ξ_2+d  
     .(m)     t<5s,

where ξ_1=0.1sin( t ) and ξ_2=0.1cos( t ) are estimated errors on x and y-axes, correspondingly.  d represents the estimated disturbance created by the White Noise Block on the Matlab software.

  * Phase 2 (State 2: Harmonious Movement): From 5 s to 10 s, the robot partner acquires the human motion intention precisely as,

    𝐱̂_h=  {
       0.01t+0.5657  
    
       0.01t  
     .(m)     5s≤ t<10s.


  * Phase 3 (State 3: Conflict in Parking Location): From 10 s to 15 s, the human operator suddenly changes his/her intention to stop at the point  ( 0.6657,0.1 ) m. Nonetheless, the robot is unaware of the sudden intention change and continuously follows the former trajectory in  (<ref>).

  * Phase 4 (State 2: Harmonious Movement): After 15 s, the robot obtains the accurate parking location of the co-carrying task as,

    𝐱̂_h=  {
       0.6657  
    
       0.1  
     .(m)     t≥ 15s.


Uncertainty model elements, which are unmodeled dynamics terms, are also investigated in our simulation scenario. Uncertainties are considered as 10% of the nominal dynamic matrices in (<ref>).

To validate the effectiveness of the proposed method, we compare it with three other control strategies. Specifically, the first strategy is the combination of the Admittance Control mass–damper–spring features in (<ref>) and PID Control (MDK-PID); meanwhile, the second strategy is the integration of Admittance Control mass–damper features in (<ref>) and PID Control (MD-PID). Eventually, Only Passive Velocity Field Control (O-PVFC) in <cit.> with a time-variant Velocity Field as in (<ref>), (<ref>) and the estimated human motion intentions 𝐱̂_h are analyzed and compared.

    𝐌_d( ẍ_a-ẍ̂̈_h)+𝐃_d( ẋ_a-ẋ̂̇_h)+𝐊_d( 𝐱_a-𝐱̂_h)=𝐟_ext,

in which 𝐌_d=𝐈_2× 2, 𝐃_d=14𝐈_2× 2, and 𝐊_d=100𝐈_2× 2 are the designed parameters of the Admittance Control. Furthermore, the control parameters of PID control are given as 𝐊_P=300𝐈_2× 2, 𝐊_I=10𝐈_2× 2, and 𝐊_D=400𝐈_2× 2. Finally, the Time-varying PVFC is set as follows: 𝐊_1=100𝐈_2× 2, E^a=3000  J, k_a^d=30  J, r_1=3, r_2=5, κ =2, 𝐊_2=5𝐈_3× 3, δ_1=-0.01, δ_2=0.01, η_min=-1, and η_max=1. The time duration of the numerical simulation is 20 s.



 §.§ Simulation Results


























For the human–robot co-carrying task in our proposed method, the contour-following approach is applied via the time-varying Velocity Field, which is depicted in Fig. <ref>. The approach resolved the radial reduction phenomenon observed in the conventional timed trajectory technique. In addition, Fig. <ref> also encodes the time-varying trajectory to suit co-carrying tasks and leverage human cognitive abilities in unstructured environments instead of utilizing predetermined time-invariant trajectories as in rehabilitation machines or co-drawing tasks. Specifically, the human operator has the ability to identify disruptions along the desired path and switch to an alternative route, which not only avoids disruptions, but also completes the co-carrying task. In Fig. <ref> and Fig. <ref>, the human–robot co-carrying path and velocity of the robot's end-effector are compared for three different control strategies, respectively. The Proposed Method performs harmonious interaction between the human and robot despite conflicting factors and uncertain elements. The robot passively follows the human intention in Phases 1 and 3, and proactively tracks the accurate estimated values in Phases 2 and 4. Meanwhile, the MDK-PID control encounters trajectory and velocity errors as it strives to balance between human intention and estimated values, which is displayed in Fig. <ref> and Fig. <ref>. In particular, at the 10-s mark, the human intends to stop at the target point ( 0.6657,0.1 ) m on the horizontal plane. However, in Fig. <ref>, the MD-PID  exhibits minor fluctuations around the parking location, while the MDK-PID continuously deviates from the intended human position during conflicts in parking location. As described in <cit.> for encountering stiff environments, the MDK-PID and MD-PID strategies also exhibit the chattering phenomenon in Fig. <ref>, which is characterized by rapid switching between two values. The drawbacks of the MDK-PID and MD-PID cause difficulties for the human operator during the co-carrying process and even result in damage to co-transported objects due to high chattering. In comparison with the MDK-PID and MD-PID strategies, the Proposed Method based on passivity-based control not only ensures safe operation but also achieves exact parking location and co-carrying velocity.

 
In Fig. <ref>, we focus on demonstrating the efficiency of the energy compensation mechanism within a finite time interval. Based on the computed settling time in (<ref>), the control parameters are designed to guarantee the convergence of kinetic energy of the augmented system ( k^a( 𝐪^a,  𝐪̇^a)) to the desired level ( k_d^a=30  J) within a specified timeframe. This feature presents a promising solution for time-critical applications, such as co-carrying fragile objects, in which precise control over the velocity of the robot partner and interaction force is imperative. According to (<ref>) and (<ref>), when the kinetic energy is insufficient, the robot partner's movement rate is sluggish and cannot achieve the mutual tasks. On the contrary, safety violations may occur in co-carrying tasks when the robot partner moves too swiftly. In addition to maintaining the energy level, the relationship between the kinetic energy of the robot manipulator, fictitious flywheel, and augmented system is also described in Fig. <ref>. 


The human workload for the co-carrying task is exhibited by the human force in Fig. <ref>; meanwhile, the power flow into the human operator or co-transported object in (<ref>) is also shown in Fig. <ref>. The chattering phenomenon of MDK-PID and MD-PID in Fig. <ref> and Fig. <ref> is caused by the responses of the position and velocity in (<ref>) and (<ref>). Furthermore, the volume of the human force and power of the Proposed Method is smaller than those in both the MDK-PID and MD-PID approaches in Phases 1 and 3. Meanwhile, during harmonious phases, the human force and power output of all three strategies approach zero, distinguishing them from the Admittance Model as,

    𝐌_dẍ_a+𝐃_dẋ_a=𝐟_ext.

The Admittance Model in (<ref>) utilizes 𝐟_ext as a trigger to generate movements, resulting in the presence of a certain level of human force and power even in harmonious phases. Consequently, the above Admittance Model in (<ref>) presents challenges in manipulating fragile objects that necessitate a small external force (𝐟_ext). 



Due to the significant magnitude of O-PVFC, the average data of four strategies is presented in Table <ref> instead of being depicted in Figs. <ref> - <ref>. To compare Human Force and Power Flow into Human, Table <ref> is given by adopting the following formulation as 

    Average=1/N∑_j=0^N=2000| Data_j|,

with N is computed based on the simulation duration and time step of  20 s and  0.01 s, respectively. Based on the average data in Table <ref>, it is quantitatively apparent that the values for the Proposed Method are approximately nine times smaller than those for the MDK-PID strategy and two times smaller than those for the MD-PID approach. For O-PVFC utilizing the same control parameters as those in the Proposed Method (𝐊_1=100𝐈_2× 2), the average data are not calculated because the simulation terminates abruptly at  1.52 s. This implies that the time-varying Velocity Field of the robot manipulator increases continuously under the conflict in direction phase by the following expression,

    {   𝐐̇( t )=𝐉^+( 𝐪)ẋ̂̇_h( t ) 
       𝐕( 𝐪,t )=𝐐̇( t )-𝐊_1( 𝐪( t )-𝐐( t ) ). 
    .

As a result, the Velocity Field in (<ref>) violates the existence condition of (<ref>) due to 0.5𝐕^⊤( 𝐪,t )𝐌( 𝐪)𝐕( 𝐪,t )>E^a at 1.52 s and the human–robot co-carrying task fails. By reducing the control parameter (𝐊_1=10𝐈_2× 2), O-PVFC resolved the simulation crashing problem in the case of  𝐊_1=100𝐈_2× 2; however, the substantial average magnitudes of Human Force and Power poses significant challenges for both co-carrying tasks and safe operations. Specifically, the Human Force and Power Flow into Human of the O-PVFC with 𝐊_1=10𝐈_2× 2 are roughly 147 and 115 times higher than those for the Proposed Method, respectively. In conclusion, the comparison results in Figs. <ref> – <ref> and average data in Table <ref> not only demonstrated the superiority of the Proposed Method but also satisfied the requirements for human–robot co-carrying tasks including safe operation, varied human intentions, human–robot conflict, and human workload reduction.



§ DISCUSSION


This section answers the question "Does the results of our cooperation control framework solve the problems discussed in Section II. A?" by comparing the proposed method with previous studies and analyzing its expandability to other human–robot collaboration scenarios. Finally, the limitations of the control framework are also discussed to clarify the application scope. 


 §.§ Evaluation


On the one hand, utilizing the cooperation control framework in Section III. B with the results in III. C resolved three problems <ref>–<ref> listed in Section II. A. For the first problem in <ref>, leveraging the admittance property, the robot partner proactively follows the human intention in harmonious movement scenarios, resulting in human workload reduction. Meanwhile, in noteworthy situations such as the conflict in direction and parking location, the co-carrying path is re-planned for the robot according to human–robot interaction forces. For safe operation, the results in Theorem <ref> and Lemma <ref> are presented to guarantee the passivity of the closed-loop system under k^a( 𝐪^a,𝐪̇^a)≥ k_d^a. Moreover, the power flow into the human operator, robot partner, and co-transported object can be reduced. Furthermore, energy compensation capacity within a finite time interval in Theorem <ref> is necessary for time-critical applications. Based on (<ref>) and (<ref>), if the amount of the kinetic energy is significantly small, the moving rate of the robot partner is slow and hence, the robot cannot achieve the mutual tasks. By contrast, safety in co-carrying tasks can be violated when the robot partner moves at a fast rate. Thus, the second problem <ref> was addressed by using the analysis in  Theorems <ref>, <ref>, and Lemma <ref>. Last but not least, the tracking problem of the time-varying co-carrying task in <ref> was resolved by our cooperation control framework with the result in Theorem <ref>; therefore, the human–robot co-carrying tasks were completed.

On the other hand, the numerical simulation results in Section IV. B demonstrated the effectiveness of the proposed cooperation control framework in Sections III. B, C. In Figs. <ref> and <ref>, by re-planning the co-carrying trajectory based on conflict information, the robot partner can follow human motion intentions in conflict phases. Meanwhile, in two harmonious movement phases, the robot partner proactively and accurately tracks the estimated human intentions. The control and compensation for the energy of the closed-loop system within a finite time interval are depicted in Fig. <ref> and Fig. <ref>. Through the amount of the interaction forces and power flow shown in Figs. <ref>, <ref>, and Table <ref>, the proposed method underscores the capacity of human workload reduction and human–robot power flow management, especially in human–robot conflicts phases. Thus, the above simulation results and analysis further reinforced the significance of integrating admittance control and time-varying PVFC with a fractional exponent energy compensation control term in addressing the problems in Section II. A.


 §.§ Comparison


Comparing our proposed framework with the cooperation control method in <cit.>, we see that our framework ensures continuous transfer between interaction states by utilizing human–robot interaction forces, which is formulated in (<ref>). Meanwhile, the approach in <cit.> switches between interaction states based on their exact characteristics, resulting in unstable behavior at switching points and posing challenges for practical applications due to the complexity of distinguishing between states. In <cit.>, admittance control was adopted as defined (<ref>), which consistently necessitates human forces for co-carrying movement. In contrast to this study, equation (<ref>) and Fig. <ref> demonstrated that the human forces in our method are zero in the harmonious movement phases, thereby reducing human workload. Moreover, the time-varying PVFC in (<ref>) is supplemented by the fractional exponent energy compensation control term, which is not considered in <cit.>. Our approach comprehensively investigates the variable kinetic energy instead of only focusing on k^a( 𝐪^a,𝐪̇^a)≤ k_d^a in <cit.>.  In comparison with the study with a special case ( r_1/r_2=1 ) in <cit.>, we investigate the fractional-order algorithm in our proposed framework to determine the convergence time of the kinetic energy. Furthermore, the difference between the proposed strategy and those in <cit.> is the consideration of the position error and time-varying Velocity Field, which qualifies for human–robot co-carrying tasks.




 §.§ Scalability


The human–robot co-carrying tasks is a specific application of our cooperation control framework. It is expected to be scalable for other human–robot cooperation tasks such as handover, assembly, teleoperation, sawing, and so on. To come up with the statement, similar design characteristics and technique requirements are detected for the human–robot cooperation scheme. 


  * First, human–robot cooperation control strategies are developed for robot manipulators to assist the human operator in common tasks. Therefore, determining accordant movement for the robot partner in the interaction phase is a critical function in synchronizing human–robot movements of the aforementioned scenarios. Furthermore, the energy and power in human–robot systems have to be considered to minimize the potential risk of harm to the human operator and robot. Regarding the human–robot collaboration handover, assembly, and teleoperation applications, the independent working phase without human–robot interaction would also have to be included. It is necessary to supplement an autonomous mode, which will be processed by our new PVFC.


  * Second,  human workload reduction, human–robot conflict resolution, and safety interaction are considered common technical requirements for human–robot collaboration systems. According to the results in Section III. C and analyses in Section III. E, the technical requirements are guaranteed by the proposed method. Thus, the advantages of our cooperation control framework can be extended to other human–robot collaboration applications in a straightforward manner.



 §.§ Limitation


The present study focused on the human–robot co-carrying task for a small transported object on a horizontal surface. Therefore, the effects of gravity were ignored and it was assumed that the position of the human hand was similar to the end-effector position of the robot arm. Although our cooperation control framework was developed by utilizing the aforementioned assumptions, this will only impact the results in the case of co-carrying large or heavy objects. Directly applying our proposed framework to large or heavy objects may not yield the efficiencies and performances observed in our co-carrying scenarios. We thus plan to investigate a human–robot collaboration approach for large or heavy objects in future works, discarding the above assumptions.



§ CONCLUSION AND FUTURE WORK

In this paper, a cooperation control framework using admittance control and new time-varying PVFC was proposed and applied for human–robot co-carrying tasks. Admittance control was employed to re-plan the co-carrying trajectory for the robot manipulator when conflicts in movements between a human and a robot were detected by the interaction force. A new PVFC with a fractional exponent control term was developed to guarantee the passivity of the closed-loop system, compensate for the kinetic energy within a finite time interval, reduce power flow from the robot to the human, and track the co-carrying path. The proposed method was validated by comparative simulation under four collaboration conditions, including conflict in direction and parking location, and two harmonious movement phases.

To enhance the flexibility of the cooperation control framework, a prediction method will be considered and developed for calculating future human motion intentions. Furthermore, the proposed cooperation control method needs to be extended to be applicable to other human-robot cooperation systems such as handover, assembly, teleoperation, sawing, and so on in upcoming work. In this study, experimental evaluations in a real robotic system have not been presented and investigated. Therefore, it will be necessary to validate the efficiency of the proposed approach for human-robot collaboration systems through experimental viewpoints in future work.

























 
 






IEEEtran









 
 
 
 






















 Paper Title: Diagnostic Runtime Monitoring with Martingales.





  When Audits and Recounts Distract from Election Integrity:
  The 2020 U.S. Presidential Election in Georgia
    
 Philip B. Stark10000-0002-3771-9604 

    Received ..... .., ....; accepted ......, ....
==============================================================================================================

empty
empty



§ ABSTRACT 
Machine learning systems deployed in safety-critical robotics settings must be robust to distribution shifts. However, system designers must understand the cause of a distribution shift in order to implement the appropriate intervention or mitigation strategy and prevent system failure. In this paper, we present a novel framework for diagnosing distribution shifts in a streaming fashion by deploying multiple stochastic martingales simultaneously. We show that knowledge of the underlying cause of a distribution shift can lead to proper interventions over the lifecycle of a deployed system. Our experimental framework can easily be adapted to different types of distribution shifts, models, and datasets. We find that our method outperforms existing work on diagnosing distribution shifts in terms of speed, accuracy, and flexibility, and validate the efficiency of our model in both simulated and live hardware settings. 



 




§ INTRODUCTION



Modern learning-enabled systems deployed in the real world are susceptible to failure if they encounter test-time inputs that do not follow the same distribution as the training-time inputs. However, the complexity of these learned systems leads to significant challenges in diagnosing problems that arise during operation <cit.>. Runtime monitors can alert users when issues arise, but pinpointing the underlying cause of the issue is difficult. For instance, a problem could stem from a suboptimal training process, during which the model was not exposed to a sufficiently diverse or representative dataset, leading to poor generalization capabilities. Alternatively, the problem might originate from the operational environment, where real-time conditions differ from those encountered during training (e.g. due to sensor degradation or environmental changes).




As an example, consider a camera-driven robot such as an autonomous aircraft using a PID controller to taxi along the centerline of a runway, where learned models estimate the cross-track and heading errors relative to that centerline. In this setting, problems that could cause the robot to fail include new situations that the robot has not previously encountered (e.g. the autonomous aircraft taking off from a runway at a new airport), or sensor degradation. In the case of new situations that the robot has not previously encountered, there is an offline problem (i.e. the unrepresentative training data), and we expect the distribution of both the system inputs and system outputs to change. For example, a new airport will look different than the original airport, so the input images will change; the width of the new runway will be different, so the distribution of deviations from the centerline will also change. In the case of sensor degradation, there is an online operational problem (the system that's running has changed), and we expect the distribution of system inputs to change, but not the distribution of system outputs. The input images may look grainier, but the distribution of the cross-track and heading errors will remain the same.  

In the example described above, using two runtime monitors that issue alerts in case of distribution shift — one over the system inputs and one over the system outputs — could differentiate between the two types of problems. More generally, if system designers identify that certain features correspond to specific issues, they can strategically place targeted runtime monitors over those features. This approach facilitates a more efficient troubleshooting process, since the activated monitors provide useful insights into which aspects of the system are contributing to the issue. 
By quickly detecting the cause of an issue, an appropriate intervention can then be applied. For example, in the case of a new runway at a new airport, the appropriate intervention is to collect additional data and perform weighted retraining. In the case of sensor degradation, the appropriate intervention is to replace the sensor. 

In this work, we present a framework for determining the cause of a distribution shift, using multiple martingale-based runtime monitors based on the method described in <cit.>. As in <cit.>, we focus on episodic situations (e.g. for a plane repeatedly taxiing down a runway during a continuous deployment, each taxiing sequence can be considered an episode drawn from a task distribution). Our method can be applied to any online learning setting, such as deploying autonomous vehicles, recognizing spoken language, or evaluating the performance of large language models. An overview of our system is shown in Figure <ref>.  

The contributions of our work are as follows: 1) We introduce a method that quickly alerts users when a distribution shift has occurred and enables rapid diagnosis of the underlying problem. 2) Our approach detects the cause of a distribution shift faster than prior work, and leads to better outcomes over the lifecycle of a robot. 3) We empirically evaluate our approach on photorealistic simulations of an autonomous aircraft taxiing down a runway with a camera perception module and in hardware on a free-flyer space robotics testbed for vision-based navigation. In these experiments, our method detects the cause of a distribution shift up to five times faster than prior work, and our method is effective at eliminating system failures due to causal misidentification experienced by a baseline system.




§ BACKGROUND AND RELATED WORK




 §.§ Distribution Shift Detection




  
Traditional Approaches. 
The challenge of detecting distribution shift has been explored by both the machine learning and statistics communities. Traditional approaches typically rely on statistical hypothesis testing to assess whether the test-time distribution differs from the training distribution. Most of these methods focus on covariate shift, where the distribution of input data changes while the conditional distribution of labels given inputs remains constant  <cit.>. Some methods have also been proposed for label shift, where the distribution of labels changes but the conditional distribution of inputs given labels remains the same. For example,  <cit.> propose a maximum likelihood algorithm for detecting and correcting label shift; <cit.> use an efficient weight estimator to provide a generalization bound for the label shift problem; <cit.> introduce black box predictors and a score-based test statistic for detecting label shift. However, these methods are generally designed for offline (batch) scenarios, and applying them online in a robotics setting may either compromise their statistical guarantee or lead to statistical inefficiency. Furthermore, these methods focus on detecting only a single type of distribution shift. 




  
Martingale-Based Approaches. 
A martingale (Definition <ref>) is a stochastic process (a sequence of random variables) where the conditional expectation of the next value, given all previous values, is the same as the most recent value <cit.>. Doob's Inequality (Proposition <ref>), states that the probability that a martingale grows very large is very small <cit.>.



 
A martingale is a sequence of random variables M_1, M_2, …, such that E[|M_n|] < ∞ and
E[M_n+1 | M_1, …, M_n] = M_n for all n.




For a martingale M_n indexed by an interval [0, N], and for any positive real number C, it holds that

    Pr[sup_0 ≤ n ≤ N M_n ≥ C] ≤E[max(M_N,0)]/C.



An approach for detecting distribution shift in online settings, introduced by <cit.>, uses conformal martingales to test for exchangeability. 
Other works following this approach include <cit.>. These methods apply conformal prediction to obtain p-values for each test sample, which are then used to construct a martingale. When the martingale exhibits significant growth, it indicates a probable distribution shift. <cit.> introduce different p-value calculations for different types of distribution shifts. While these works demonstrate good efficiency on lower dimensional data (e.g. on the USPS dataset <cit.>, which contains 11-dimensional feature vectors), they often struggle in more complex or higher-dimensional robotics settings, such as those involving image data. 



More recently, <cit.> introduce a learned, martingale-based runtime monitor that detects distribution shifts quickly for high-dimensional data, with guarantees limiting the number of false alarms. This method trains a neural network model to distinguish between older and more recent samples, and issues a warning when the model is consistently able to predict recency. A key component of this method is a martingale-based test statistic designed to grow if the distribution shifts. 
However, this work by default applies a martingale monitor only over the system inputs, making it impossible to distinguish between different underlying problems. In our work, we introduce a framework using multiple learned, martingale-based runtime monitors, and apply them over different feature spaces to improve problem diagnosis.



 §.§ System Fault Diagnosis

Many works leverage deep learning for system fault detection, using large models to detect one type of system fault <cit.>. However, these papers deploy large models with long training times that are impractical for live robotics systems, and they focus on diagnosing faults in specific mechanical systems in offline settings that are not easily generalizable. Our method, meanwhile, works with lightweight models suitable for online robotics settings. 

Other learned methods using simpler models for system fault detection include <cit.>.
However, these methods generally deal with low-dimensional data and require access to the true labels. Our method, meanwhile, works with high-dimensional data without access to true labels.



Some methods for fault diagnosis in online settings include <cit.>; however, these works focus on anomaly detection (i.e. experiencing a single rare event), while our work focuses on gradual distributional shifts.




§ IDENTIFYING TYPES OF DISTRIBUTION SHIFT
 



We present a framework for quickly identifying distribution shifts in episodic robotics settings that provides actionable information for targeted interventions.  Our method builds on the work of <cit.> and also leverages stochastic martingale-based runtime monitors.  Our critical contribution is to use multiple martingale-based monitors, deployed simultaneously with each monitoring a different type of distribution shift.  This enables our system to detect distribution shifts more quickly, and moreover the monitors each probe a distinct intermediate result of the robotics system.  This enables system designers to identify system components that are behaving abnormally before failure of the entire robotics system, develop appropriate interventions, and differentiate between types of distribution shift. 





 §.§ Problem Setup 

Consider an autonomous system that interacts with the environment.  The system's behavior is defined by function f: 𝒳→𝒴 which maps sensor inputs x^t ∈𝒳 at time t to action y^t ∈𝒴.  This function can be expressed as the composition of K functions, f = f_K ∘…∘ f_2 ∘ f_1, with K-1 intermediate results I_1, I_2, ... I_K-1, where I_1 = f_1(x), 
I_k = f_k(I_k-1)  for  1 < k < K, 

y = f_K(I_K-1).




In our problem setup we are given a dataset of N historical inputs sampled from a single distribution, D_orig = (X_1, X_2, ⋯, X_N).  Each X_i represents a sequence of sensor inputs recorded during an episode of the robot interacting with the environment, e.g. X_i = (x_i^1, x_i^2, …, x_i^l_i).  During the robot's deployment, in a potentially novel environment, we observe new sequences of sensor inputs D_new = (X_N+1, X_N+2, …).  Again each X_i in D_new represents a sequence of inputs recorded during an episode of the robot interacting with the environment.  

As shown in <cit.> it is possible to design a series of test functions over system inputs


    ψ^1_j: D_orig, X_N+1, ⋯, X_j↦{ T, F }
       ∀ j = N + 1, N + 2, …,

where the output T(rue) indicates that we have identified a distribution shift over the system inputs, while F(alse) indicates that we have failed to identify a distribution shift over the system inputs.

In this work we additionally aim to design test functions over the K-1 intermediate results and the final action.  These test functions will issue an output alert when a distribution shift is identified over the intermediate results or the final action.  The test functions will take the form

    ψ^k_j: D_orig, f^1 → k_j (X_N+1), ⋯, f^1 → k_j (X_j) ↦{ T, F }
       ∀ j = N + 1, N + 2, …,

where 1 ≤ k < K for test functions over the intermediate results and k=K for the test function over the output actions.  We define f^1 → k_j : 𝒳^l_j→𝒴^l_j as [That is, the domain of f^1 → k_j is the l_j-fold Cartesian product of 𝒳 and the codomain is the l_j-fold Cartesian product of 𝒴, where l_j is the length of episode j.]

    f^1 → k_j(X_j) := ( 
       f_k ∘…∘ f_2 ∘ f_1 (x^1_j), 
       f_k ∘…∘ f_2 ∘ f_1 (x^2_j), 
       ⋮
       f_k ∘…∘ f_2 ∘ f_1 (x^l_j_j)
    ).



Additionally, each test function is designed to be ϵ-sound (<cit.>), meaning that if the historical data D_orig and the new data D_new are drawn from the same distribution, then with high probability (1 - ϵ) the test will not issue a warning.  Formally, when D_new and D_orig are exchangeable, then 

    [∃ j, ψ^k_j (D_orig, f^1 → k_j (X_N+1), ⋯, f^1 → k_j (X_j) ) = T ] ≤ϵ.






 §.§ Proposed Framework 


The key intuition behind martingale-based monitors is that it is impossible for any predictor to reliably distinguish between two samples drawn from the same distribution.  Consider drawing two samples, one from D_new and the other from from D_orig.  If there has been no distribution shift (so these distributions are in fact the same), then no predictor can do better than random chance (e.g. a Bernoulli variable with parameter p = 0.5).

We adapt the martingale-based monitor defined in <cit.> to design the test functions ψ^1_j over system inputs.  This monitor relies on a neural network f^1_NN : 𝒳×𝒳→{0, 1} that is trained to predict whether two inputs come from the same distribution.  Additionally we design test functions ψ^k_j for k = 1, …, K over all K-1 intermediate results I_1, I_2, ... I_K-1 and over the final output action.  For the test functions over intermediate results, the neural network is modified to take as input two intermediate results, f^k_NN : I_k × I_k →{0, 1}.  
For the monitor over final output actions, the neural network is modified to take as input two actions, f^K_NN : 𝒴×𝒴→{0, 1}.  Note that our method is agnostic to the specific choice of model architecture (but for details about the model architectures that we used in our experiments, refer to the Appendix.

To design the test functions, let us first define the indicator variables

    Z^k(j) = 
        
            1if  f^k_NN predicts correctly for test example  j 
    
            0otherwise.

Then let us define the martingales

    M^k_n  = (e^t · S^k_n)/((q + p e^t)^n),

where S^k_n = ∑_j=1^n Z^k(j), p = q = 0.5, and we use t = 1 (Equation 4 in <cit.>).  Using these martingales we define the test functions as

    ψ^k_j( D_orig, X_N+1, ⋯, X_j) = 
        
            Tif  M^k_n  ≥ C 
    
            Fotherwise.

Using a threshold of C = 100 we are guaranteed a false positive rate of ≤ 0.01 for each test function because (Lemma 2 in <cit.>)

    Pr[sup_0 ≤ n ≤ N M^k_n  ≥ C] ≤1/C.


This allows our framework to propose a mitigation strategy tailored to the specific failure mode identified by the martingale monitor that signaled the distribution shift. 

In order to identify a particular type of distribution shift, we need to construct different martingales, each with representative inputs for our test functions. As an illustrative example, we consider distribution shifts that may occur for images taken by an external camera, as shown in Figure <ref>. During deployment, we may encounter sensor degradation, which is a shift that results in a change to the input images, so we feed in representative input features to the classifier. If a distribution shift is detected using these input features, we know that the distribution of our input images has changed, and a good mitigation strategy would be to replace the sensors. Similarly, if we encounter a new environment, such as a new airport, our output labels will change, as the plane taxis with a new trajectory. Therefore, we feed in output features to the classifier, and if we detect a distribution shift using the output features, we know that the distribution of our output labels has changed. Here, a good mitigation strategy would be to gather additional data and perform a weighted retraining. The key insight is to use knowledge of a likely type of distribution to inform the relevant chosen features.



§ EXPERIMENTS 


  
We empirically evaluate our method on photorealistic simulations of an autonomous aircraft taxiing down a runway using camera-based perception and in hardware on a free-flyer space robotics testbed for vision-based navigation. We show that our method detects distribution shifts and identifies the type of distribution shift more quickly than existing methods. We also show that the knowledge of distribution type provided by our method allows us to use targeted interventions, which leads to better results than generic interventions, and better outcomes over the robot lifecycle compared to a scheduled maintenance approach. 




 §.§ Experiment 1: Fast categorization of distribution shifts

We first demonstrate that our method is able to detect different types of distribution shifts faster than prior methods. To evaluate our method, we use data from the photorealistic X-Plane 11 flight simulator where an autonomous aircraft uses a PID controller to taxi along the centerline of the runway. 


Next, we introduce two types of distribution shifts to the data: first, we introduce Gaussian noise to the images to simulate sensor degradation, and second, we add 200 new sequences of images of the plane taxiing at a new airport, KJFK, that was not in the training dataset (see example images in Figure <ref>). During deployment, images are drawn from either the original sequences, the sequences with added noise, or the sequences from a new runway. To detect whether a shift has occurred and categorize the type of shift, we deploy two stochastic martingales simultaneously to act as runtime monitors. An alert is raised when the martingales reach a threshold of 100. If no alert is raised after 200 samples, the experiment is terminated. The experiment is repeated 100 times.



We compare our method against the method described by Vovk in <cit.>, which we will refer to as the conformal martingale (CM) method. For the CM method, we use the nearest distance nonconformity score and the ratio nonconformity score respectively for the two martingales. For each type of distribution shift, we record the number of iterations before an alert is issued by our method and the CM method. The results are summarized in Table <ref>. A representative plot showing martingale growth for our method and the CM method is shown in Figure <ref>.


In every scenario, both methods are able to detect the correct distribution shift when one exists (sensor degradation, new environment) and do not issue false alerts when there is no distribution shift, i.e., we empirically observe no false negatives or false positives. Additionally, in every scenario, our method issues an alert corresponding to the correct shift in a fewer number of iterations compared to the CM method, showing that our method is able to detect and categorize distribution shifts more quickly than existing methods. 




 §.§ Experiment 2: Impact of targeted interventions

Next, we look at how the knowledge of the type of distribution shift enables us to choose the right intervention, which leads to better performance compared to a generic intervention. To evaluate the effect of interventions, we use the same X-Plane dataset as the previous experiment. Images are sampled from the original sequences, the sequences with sensor degradation, or the sequences from a new runway (refer to example images in Figure <ref>). 


If an alert is raised, i.e., a distribution shift is detected by one of our martingales, an intervention is applied. If the alert indicates that sensor degradation has occurred, the correct targeted intervention is to replace the sensor. On the other hand, if the alert indicates that the environment has shifted, the correct targeted intervention is to perform weighted retraining to prioritize learning the most recent samples. We also evaluate a generic intervention, which is retraining the model with equal weight for all samples. After the intervention, the experiment is continued for 100 more samples. Mean squared error (MSE) of the predicted cross-track and heading distance is recorded before the distribution shift and after the intervention. We also record the MSE if no intervention is applied. The comparison of MSE for different types of interventions (over 100 trials) is shown in Table <ref>.

Unsurprisingly, the model predictions degrade after a distribution shift occurs, so if no intervention is applied, the MSE increases significantly (from 0.17 to 0.885 or 0.771, depending on the shift). A generic retraining of the model reduces the MSE somewhat. However, the greatest reduction in MSE is achieved if the type of shift is identified and the correct corresponding intervention is applied. 



 §.§ Experiment 3: System lifecycle performance compared to a scheduled maintenance approach

Next, we show that applying the correct targeted intervention for each type of distribution shift not only improves model performance in the short-term, but leads to overall better performance and greater safety over the system lifecycle. We posit that a standard approach to maintain lifelong performance, in the absence of runtime monitors for different types of distribution shifts, might be to adopt a standard maintenance schedule, where all interventions are scheduled at a fixed cadence, e.g., every γ iterations. We compare such a maintenance schedule against our method, where only targeted interventions are applied, and only when the corresponding shift has been detected.




We see that our method of only applying targeted interventions when a distribution shift is detected often achieves a lower MSE over the lifecycle as compared to the maintenance schedule, despite the maintenance schedule running every intervention at a fixed cadence. When the maintenance frequency (γ) is equal to the rate at which shifts occur (λ), our method achieves a lower MSE. Only when maintenance is carried out at double the frequency, i.e., γ is half of λ, does the maintenance schedule outperform our method in terms of lower MSE. 



More importantly, our method reduces the frequency of crashes (in fact, completely prevents crashes in this experiment) in all cases while the maintenance schedule occasionally leads to crashes, i.e., on average 3-5 crashes over 1000 iterations. This is because our method uses runtime monitors to quickly detect that a distribution shift has occurred and applies the correct intervention to prevent excessive degradation of the predictions. On the other hand, maintenance is carried out at a fixed cadence, so in some instances where a shift occurs soon after a scheduled maintenance has been completed, that shift can quickly cause errors to snowball and lead to a crash within 45 iterations, before the next scheduled maintenance can take place. Thus, we see that timely and targeted runtime monitoring leads to better performance and fewer failures over the system lifecycle. 




 §.§ Experiment 4: Tailoring custom martingales to specific types of shifts


Custom martingales can be designed to detect other specific types of shifts. As an example, we define a new martingale to detect a shift in brightness of the images, since a brightness shift is a common trigger for existing runtime monitors. This martingale uses the same CNN classifier as before, but it is trained by using the feature vector from the layer of a ResNet-50 architecture that most closely corresponds to image brightness. Specifically, we evaluate all 50 feature vectors from the intermediate layers of the ResNet-50 model and choose the feature vector that changes the most with changes in image brightness. This experiment reinforces one of the advantages of our method: system designers with insights into a likely type of distribution shift can construct a monitor to alert an issue if such a shift occurs.  




On average, all three martingales usually detect their corresponding distribution shift faster than the other two martingales. It is worth noting that a change in brightness also induces a shift in image distribution. Therefore, the brightness martingale detects the shift first 89% of the time, taking, on average, 17.92 iterations to issue an alert. The other 11% of time, however, the sensor degradation martingale detects a shift first, because the input distribution is changing as well. Similarly, a shift such as sensor degradation not only changes the distribution of input images but also that of labels, so eventually an alert is raised by the environment shift martingale as well.  Most notably, the brightness martingale only issues an alert for a shift in brightness and not for the other two shifts. This demonstrates that it is possible to design custom martingales that act as runtime monitors for specific predefined distribution shifts, allowing system designers to use their domain expertise to design the most informative runtime monitors. 





 §.§ Experiment 5: Categorizing distribution shifts on a hardware free-flyer space robotics testbed


We also demonstrate the effectiveness of our method on a hardware free-flyer space robotics testbed with input from a forward-facing Intel Realsense D455 camera mounted on the side, as pictured in Figure <ref>. The free-flyer is a cold gas thruster-actuated 2D mobile robot that floats almost frictionlessly on a smooth granite table, developed to simulate zero-g or zero-friction conditions in aerospace robotics applications <cit.>.







We introduce distributions shifts similar to the previous experiments. We simulate sensor degradation by adding Gaussian noise to the images, an example of which is shown in Figure <ref>. We simulate environment shift by changing the starting position (and therefore, the resulting trajectory) of the free-flyer, an example of which is shown in Figure <ref>. We measure how many time-steps it takes to detect a distribution shift, comparing both our method and the CM method using nearest neightbor nonconformity scores <cit.>. The results are summarized in Table <ref>.

For both the sensor degradation and environment shift, our method detects a shift faster than the CM method. Additionally, neither method issues a false positive, i.e., neither raises an alarm when a shift has not occurred. These results show that our method more rapidly warns us of different types of distribution shifts compared to existing methods and can be used for runtime monitoring in safety-critical contexts.






§ CONCLUSION



In this paper, we introduce a novel framework for diagnosing the cause of a distribution shift, and experimentally validate the speed and adaptability of our method to different types of distribution shifts, models, and datasets. We demonstrate that knowledge of the underlying cause of a distribution shift allows the system designer to induce an appropriate mitigation strategy, and applying our method outperforms a traditional maintenance schedule while reducing cost.  
 

In future work, we would like to explore combining our method with other strategies to prevent performance degradation in real-time systems <cit.>, and understanding how to combine possibly correlated monitors to achieve stronger system performance guarantees.




isrr2024_format/styles/bibtex/spbasic




§ APPENDIX





§ OVERVIEW: TYPES OF DISTRIBUTION SHIFT

    In this section, we clarify the context of our distribution shifts with respect to the well-defined mathematical notions of covariate, concept, and label shift. In a supervised machine learning setting, if X is the input and Y is the output, the training data for a model is a set of samples from the distribution P(X,Y), and the model is learning to approximate the distribution P(Y|X) <cit.>. By the definition of conditional probability,
    
    P(X,Y)  = P(Y|X)P(X) 
     
                    = P(X|Y)P(Y)
 
    In a covariate shift, P(X) changes, but P(Y|X) remains the same, In a label shift, P(Y) changes, but P(X|Y) remains the same. In a concept shift, P(Y|X), but P(X) remains the same <cit.>. However, in practice, multiple shifts may occur jointly, as a change in the P(X) distribution can affect the P(Y) distribution, and vice versa. In this paper, the distribution shifts that were induced do not necessarily fall cleanly into one of these mathematical categories. For example, the sensor degradation shift in Section <ref> has a change in the P(X) distribution (i.e. a change in the distribution of input images) which our method detects, but no additional information about the P(Y) distribution or P(Y|X) distribution. The new environment shift has a change in P(Y) (i.e. a change in the distribution of trajectory labels) which our method detects, but no additional information about the P(X) distribution or P(X|Y) distribution.

    Note that the nature of the distribution shift detection problem differs from that of the anomaly detection problem, as distribution shift detection focuses on identifying a change in the distribution of samples (and it is arguably impossible to make a distributional claim without multiple samples of evidence), whereas anomaly detection focuses on identifying a single unusual sample or rare event. Moreover, the intervention method for these two tasks differs significantly. A safety intervention or fallback should be triggered immediately (i.e. within an episode) in response to a sudden change for an unsafe anomalous event. However, interventions for distribution shifts can occur on a somewhat longer timescale (e.g. model retraining), and an alert about distributional shift should inform decision-making about successive deployments.

    The main advantage of our method is as follows: Based on which monitor signaled the distribution shift first, system designers can focus their efforts on improving specific components of the system or gathering additional data. For instance, if an issue is caused by sensor degradation, specific monitors will issue an alert and a good mitigation strategy would be to replace the sensors; if the issue stems from encountering a new environment, other monitors will issue an alert and a good mitigation strategy would be to gather additional data and perform a weighted retraining. In theory, a system designer could apply every possible mitigation strategy each time any distribution shift is encountered, but in practice doing so is costly and inefficient. Ideally, we want to find the best intervention strategy as quickly as possible, which requires that we are able to identify each particular type of distribution shift.
    


§ X-PLANE EXPERIMENT DETAILS 



In this section, we provide further details on the X-Plane experimental setup, including the model architecture that was used and the methodology with which the distribution shifts were induced. 



 §.§ Model Architecture

For all X-Plane experiments, we use the model architecture outlined in Fig <ref> with the Adam Optimizer, a batch size of 32, a fixed learning rate of 1e-4, and a binary cross-entropy loss function. As mentioned in the paper, our method is agnostic to the model used for inference, and we used a simple model to demonstrate the efficacy of our method on small models. For each type of distribution shift, we trained a model with essentially this architecture, except that the input to the model is the cross-track and heading error in the environment shift case and a feature vector of size 100,352 (2048*7*7) in the brightness shift case (this feature vector is the flattened output of one of the intermediate convolutional layers of a ResNet-50 model). The code for running these simulations will be released upon acceptance of this paper.   




 §.§ Experiment Details: Sensor Degradation

Using NASA's XPlaneConnect Python API, 1000 simulated video sequences are generated, each corresponding to a taxiing episode of the aircraft on the runway at KSVC airport <cit.>. Each taxiing sequence consists of approximately 30 images of size 200x360x3. These sequences occur at randomly initialized times between 8:00am and 10:00am. We train a classifier to distinguish the “more recent” image on these image sequences, by randomly sampling one image from each sequence. The distance to the centerline is estimated by a pre-trained DNN, using images from an outboard camera mounted on the plane. 

In the sensor degradation experiments, we gradually add Gaussian noise to a set of images in the unseen test set. In the initial experiments, we induce Gaussian noise with a kernel size of (15,15) and a σ value that gradually increases. For Experiments 1, 2, 4 and 5, we increase σ by 1 at each time step until it reaches the value of 50. With σ=50, the Gaussian noise becomes so extreme that our predictor causes the plane to go off the runway after approximately 105 timesteps. For Experiment 3, we increase σ by 2.5 until it reaches a value of 100. With σ=100, we observe a crash after approximately 45 timesteps. We chose a more significant σ value in Experiment 3 to simulate a crash if either method does not fix a shift fast enough. When the σ value reaches 50 and 100 respectively, we continuously sample images blurred with these values. See Fig. <ref> for example images.



 Note that it takes around 30 minutes to train the initial model on a Macbook Pro M1 chip on the X-Plane dataset of approximately 30,000 images for the sensor degradation shift, and around 2 minutes to retrain after each update step. Each sequence consists of approximately 30 images of size 200x360x3, with 1000 samples observed. 800 of these were used in the training set, while the other 200 were modified according to the Gaussian blur rule defined above. 
 




 §.§ Experiment Details: Environment Shift

In the environment shift experiments, we sampled 200 sequences of images from a new runway, KFJK. We induce the same sinusoidal trajectories as before, except with a larger amplitude in order to account for the larger width of the runway. 
In this case, the two linear layers of the neural network had dimension (4, 512) and (512,1), which took approximately 10 minutes to train initially on all 30,000 images, and 45 seconds for each model update. If a warning signal is not issued, the plane will crash after approximately 85 time steps as the predictor will cause the plane to taxi off the runway. 



 §.§ Experiment Details: Brightness Shift

In the brightness shift experiments, we sampled 200 sequences with the brightness of the image reduced by 50%. If a warning signal is not issued, the plane will crash after approximately 120 time steps. The model we used for this method is the same architecture as in Fig. <ref>, except that the input to the model is a feature vector from an intermediate layer of a pre-trained ResNet-50 model, rather than the image itself.
 
In order to determine which intermediate layer to use, we constructed a martingale from each intermediate layer of ResNet-50 and ran them simultaneously in order to determine which layer corresponded most closely to image brightness. We experimentally determined that Layer 49 was the fastest in detecting the brightness shift, so we used that martingale when running all of the rest of our experiments. 



 §.§ Experiment Details: No Shift

We again take 1000 video sequences of a plane taxiing down a runway using the X-Plane 11 simulator. All sequences occur without a distribution shift, with approximately 30 images of size 200x360x3 in each sequence. We run all of our martingales on these sequences to experimentally show that we issue no false positive warnings when there is no shift.



 §.§ Experiment Details: System Lifecycle

We simulate the system lifecycle by constructing sequences of 1000 iterations from the X-Plane dataset and inducing either sensor degradation or environment shift to the data according to a Poisson process with parameter λ corresponding to the expected rate of occurrence of the shift. As before, sensor degradation is simulated by adding noise to the images and environment shift is simulated by introducing images from the new runway. The shifts are large enough that if an intervention is not applied within 45 iterations after the shift occurs, the degraded model predictions cause the aircraft to deviate off the runway and crash. 









§ FREE-FLYER HARDWARE EXPERIMENTS




We collect data from multiple episodes of the free-flyer moving from a starting position to the docking target. This data consists of 9000 camera images from 30 episodes (each image is of size 360x640x3) as well as the associated ground truth relative pose of the free-flyer with respect to the docking target at each iterations, which is recorded using a motion capture system above the free-flyer testbed. These images are used to train a classifier to distinguish the “more recent" image. An example image is shown in Figure <ref>.

In these experiments, the free-flyer performs a learning-based visual servoing task that emulates autonomous spacecraft docking. Images from the onboard camera are used to guide the free-flyer from a starting point towards a static visual target (as shown in Figure <ref>), which is a pattern defined by the International Docking System Standard for spacecraft docking adapters. 

R0.5
    
    < g r a p h i c s >

    Martingale values growing to indicate a distribution shift, specifically sensor degradation, in a real-time robotics setting recorded on the free-flyer hardware. The martingale associated with our method grows much faster than the martingale associated with the CM method, demonstrating that our method is able to issue an alert faster than existing methods.
    


To collect the initial image data, we positioned the robot at or around a fixed starting point on the granite table while ensuring that the visual target stayed within the field of view of the camera. For the new environment distribution shift, we collected data by positioning the robot at a different starting point about one meter away from the original starting point. We captured 30 sequences from each starting point with around 300 images per sequence for a total of 18,000 images, as well as the corresponding x,y position of the freeflyer using the Optitrack motion capture system. For the sensor degradation experiment, we add Gaussian noise to the data from the original starting point using the same process as described above for the X-Plane experiments with σ=50. For both sets of experiments, we use the model architecture shown in Fig. <ref>, except that instead of predicting the cross-track and heading error in the environment shift case, we predict the x,y position of the free-flyer on the granite table relative to the visual servoing target. A representative plot of the martingale growth for our method and the CM method is shown in Figure <ref>.









 Paper Title: Moderating Group Conversation Dynamics with Social Robots.



















































Moderating Group Conversation Dynamics
with Social Robots
    Lucrezia Grassi^1, Carmine Tommaso Recchiuto^1, Antonio Sgorbissa^1
^1All authors are with the University of Genoa, DIBRIS, Via All'Opera Pia 13, 16145, Genoa, Italy.

Corresponding author's email: 



    August 5, 2024
=============================================================================================================================================================================================================



§ ABSTRACT 
This research investigates the impact of social robot participation in group conversations and assesses the effectiveness of various addressing policies. The study involved 300 participants, divided into groups of four, interacting with a humanoid robot serving as the moderator. The robot utilized conversation data to determine the most appropriate speaker to address. The findings indicate that the robot's addressing policy significantly influenced conversation dynamics, resulting in more balanced attention to each participant and a reduction in subgroup formation.






§ INTRODUCTION

Social robotics focuses on creating and applying robots designed to engage with humans in social environments. The use of social robots in group interactions is attracting increasing interest due to the growing need for these robots to engage with multiple users simultaneously. This includes scenarios such as robots participating in discussions with several individuals <cit.>, facilitating social interactions <cit.>, and engaging in social games <cit.>.

Robots capable of engaging with multiple people can enhance user experiences by making interactions more compelling and natural. This can lead to higher user satisfaction and improved accessibility and inclusivity, as highlighted by <cit.>. However, at present, few robots can engage with multiple users simultaneously. This is due to the many factors that need to be considered in these situations. A system described in <cit.> examines the tracking and fusion aspects of multi-party interactions but only monitors user entry and exit and can accurately identify only two users. A spoken dialogue system like the one in <cit.> can identify multiple users using data from a Kinect sensor, but its conversational abilities are limited, and it struggles with long, natural conversations involving multiple parties. Additionally, it can only engage one person at a time. Similar limitations are observed in the work of <cit.>, which aims to develop a multi-user engagement policy for managing turn-taking using the robot's gaze, head movements, and verbal communication. 

Whenever the robot interacts with multiple people, the “many minds problem” arises <cit.>. As the number of participants increases, basic conversational mechanisms such as turn-taking, speaking time, and listener feedback become more complex. Although turn-taking is a fundamental aspect of communication, researchers continue to study how speakers signal the end of their turn and indicate who they are addressing, as well as how listeners recognize when it is their turn to speak. This is accomplished through behaviors such as gaze, head orientation, and intonation, which have been studied by various scholars, including <cit.>. Speakers typically use gaze to select the next speaker, known as the “addressee” <cit.>. The selection of the addressee is a crucial issue in group conversations, requiring speakers to make quick decisions that consider the potential reactions of other participants.



Furthermore, the robot needs to comprehend the participation levels of the users it is interacting with. Dominance, a key concept in social interactions, plays a significant role in this understanding and has been extensively studied in social psychology <cit.>. Dominance can pertain to an individual's traits or their hierarchical status within a group. Indicators of dominance are categorized into vocalic and kinesic types. Vocalic indicators include factors such as speaking time, word count, and speech loudness (or energy) <cit.>. Speaking activity, especially the duration of speech, is a strong predictor of dominance. Kinesic indicators involve body movement, posture, facial expressions, and eye gaze <cit.>. Dominant individuals generally exhibit more movement and a broader range of motion than non-dominant individuals, and their gestures during speech are positively associated with dominance <cit.>.



This paper presents a study that aims to investigate and regulate the dynamics of group conversations involving a social robot. In such interactions, participants may have varying roles and personalities, leading some individuals to dominate the conversation while others may feel excluded. Ensuring balanced participation is essential to make everyone feel included and engaged.

To manage the dynamics of the conversation, particularly when it is crucial for all participants to feel like part of a cohesive group, we implemented four different control policies that leverage the concepts of dominance and communities. To validate these policies, we tested them against a baseline policy. We conducted 75 experiments involving a total of 300 participants, where a humanoid robot engaged in conversations with groups of four individuals. Figure <ref> depicts the robot interacting with the participants in one of the experiments. Throughout these experiments, the robot collected quantitative data to analyze participation levels, identify distinct subgroups (i.e., communities) of participants, and evaluate the overall conversation dynamics. This information allows the robot to act as a moderator, promoting active participation in the conversation.

The article is structured as follows. Section <ref> provides an overview of the system architecture and the developed control policies. Section <ref> describes the experimental setup and discusses the findings. Finally, Section <ref> presents the conclusions.



§ METHODOLOGY




 §.§ System Architecture

The robot's ability to converse autonomously with multiple people is enabled by CAIR (Cloud Artificial Intelligence and Robotics), a cloud software architecture specifically designed for autonomous conversation <cit.>. This system relies on a framework for knowledge representation, utilizing an ontology implemented in OWL2. Conversation topics and related sentence pieces are dynamically composed at runtime using the hierarchical structure of the knowledge base.

The CAIR server comprises a set of web services, as illustrated in Figure <ref>. The Dialogue Manager service manages the conversation and identifies the user's intent to discuss specific topics, while the Plan Manager service interprets the user's intent to direct the robot to perform particular actions. To generate appropriate responses and plans, the server uses an Ontology that includes all the topics, keywords, sentences, and plans used during interactions with users <cit.>. An additional service called Hub handles all incoming requests by forwarding them to the Dialogue Manager and Plan Manager services. Information exchange between the CAIR client and server is facilitated through the dialogue state, which tracks the conversation's history and includes discussed topics, preferred topics based on user input, and previously spoken sentences to avoid repetition.

To enable multi-party interaction, the original architecture described in <cit.> has been enhanced with two new services: the Registration service and the Audio Recorder service. The Registration service is activated when a new user initiates registration, creating a new profile ID linked to the user's voice. The Audio Recorder service starts recording audio when the Root Mean Square (RMS) of the noise exceeds a specific threshold, sending the audio to the Speech Recognition API for transcription and to the Speaker Recognition API to identify the speaker's profile ID.




The dialogue state has been expanded to support multi-party interaction, incorporating data related to the speakers along with dialogue statistics. These include a matrix tracking the number of times one speaker talked after another in successive turns, the total number of turns for each user, the average topic distance between speakers, the a priori probability that a speaker will talk, and a moving window that tracks the information related to speakers' turns. The moving window maintains the most recent conversation turns within a duration equal to M. For each turn, it records the speaker's ID, speaking time, and word count. When the total speaking time in the moving window exceeds M minutes, the earliest turn is removed, and the latest one is added (FIFO queue).



 §.§ Control Policies

The information contained in the moving window has been used to develop two policies aimed at controlling different aspects of group dynamics.



  §.§.§ Balancing Policy

The Balancing policy exploits the data from the moving window to determine which speaker to address. Its goal is to identify and engage with the user who is least active in the conversation (i.e., the submissive user). Conversation participation is quantified using a metric D_i, which considers both speaking time and word count, as these are the most significant indicators of dominance <cit.>. To calculate D_i for each speaker S_i, we measure the percentage of their speaking time (T_i) and word count (W_i) within the moving window. The metric D_i is then computed as:

    D_i = γ_1 T_i + γ_2 W_i,

where γ_1 and γ_2 are weights representing the relative importance of speaking time and word count in determining dominance. The speaker addressed by the Balancing policy is S_m, where:


    m = min_i(D_i).


There are two versions of this policy: the “hard" version (BH) and the “soft" version (BS). In the hard version, if a user other than the intended one responds, the robot ignores the response and repeats the question to the intended user. In contrast, the soft version accepts responses from any user, replies accordingly, and then readdresses the intended user with the original question.



  §.§.§ Community Policy


The Community policy is based on the hypothesis that it is possible to identify sub-groups (i.e., communities) among participants in a conversation. To identify these communities, we use the probability that one speaker talks after another, assuming that members of the same community tend to speak consecutively. This probability is derived from the matrix in the dialogue state, which tracks the frequency with which each speaker follows another. This data is represented in a matrix and then converted into an undirected graph, where nodes represent speakers and edge weights represent probabilities. The Louvain algorithm is applied to this weighted graph to find the optimal partitioning of nodes into communities <cit.>. Once the best partition is identified, the policy uses this information to address a random speaker from a different community at each turn. The goal is to maintain a single cohesive conversation and prevent speakers from dividing into sub-groups. Similar to the Balancing policy, both a hard (CH) and a soft (CS) version have been developed for the Community policy.

Finally, it is important to note that the policy to be used by the system can be selected before the start of the interaction.



§ EXPERIMENTS AND RESULTS


The experiments were conducted at the “Parini Merello" middle school in Genoa, where we brought the humanoid robot Pepper, equipped with the CAIR cloud client. Approval was obtained from the University of Genoa's ethical committee, and consent forms were signed by the parents of all participating first and second-grade students. A total of 300 students participated, divided into 75 groups of four. Each group took part in an experiment where the robot used one of the five developed policies, resulting in five types of experiments with 15 groups per experiment.

To establish a baseline, 15 groups interacted with the robot using a Neutral policy (N). The remaining groups were equally split among the other four policies. Participants registered to CAIR using their voices and then interacted freely with the robot for 15 minutes. They were unaware of the specific policy in use and could respond at their discretion. This approach ensured spontaneous and natural interactions. Each experiment required approximately 5 minutes for registration and 15 minutes for interaction with the robot. To ensure the privacy of the participants, all their personal information, including name, gender, and biometric data related to their voice, was promptly deleted after each experiment.

For the control groups (N), no specific results are expected, as this baseline represents the robot not attempting to influence the conversation dynamics. In contrast, for the first and second experimental groups (BH and BS), we anticipate that the robot will successfully “balance" participation, ensuring all participants are included equally in terms of speaking time and word count. For these experiments, we set the weights γ_1 = γ_2 = 0.5. The third and fourth experimental groups (CH and CS) introduce the concept of community, aiming to demonstrate the robot's ability to unify any emerging sub-groups among the participants.

Figure <ref> illustrates the speaking times of four participants from two separate experiments using the N policy and the BS policy, respectively. The x-axis represents the turn number, while the y-axis shows the speaking time (in seconds) within the moving window. These results exemplify how, under the N policy, a dominant speaker often emerges, with the difference between the highest and lowest speaking times widening. This occurs because the robot does not direct the conversation, allowing extroverted participants to dominate. Conversely, when the robot uses the BS policy, even its soft version, the speaking times of participants remain more balanced.

It is important to note that the number of turns varies across experiments. This variation is primarily because, although all experiments have a duration of 15 minutes, participants may speak for different amounts of time or remain silent due to initial shyness or uncertainty about what to say. Typically, the number of turns observed in the experiments ranged from about 40 to 60.







Figure <ref> shows a comparison between two experiments where the robot applied the N policy and the CH policy. The x-axis represents the turn number, while the y-axis displays the number of communities formed among participants. Under the N policy, the number of communities never drops below three, indicating that each speaker often forms a separate community. Conversely, under the CH policy, the number of communities decreases from four to mostly one throughout the experiment.

Table <ref> presents the means and standard deviations of speaking time errors, word count errors, and the number of communities across all experiments using the described policies. The error is defined as the difference at each turn between the highest and lowest values among the speakers, both for speaking times and word counts. 

To determine statistically significant differences between experiments with different policies, we used the Shapiro-Wilk test to check for normal data distribution. As some data were not normally distributed, we applied the non-parametric Mann-Whitney U test to compare the results.

We aimed to verify if the Balancing policy improved speaker participation in terms of speaking time and word count. To do this, we compared errors in speaking time and word count for the Balancing policy (BH and BS) against the baseline policy (N). Results were statistically significant with p<0.01, indicating a significant difference between the Neutral and Balancing policies. The average speaking time error for N was 40, compared to 8 for BH and 7 for BS, with lower standard deviations for BH and BS. No significant difference was found between BH and BS.

Next, we tested if the Community policy reduced the number of subgroups among speakers by comparing the average number of communities in CH and CS with those in N. All comparisons showed p<0.01, indicating significant differences when using the Community policy. We also confirmed no significant difference between CH and CS in reducing the number of communities.

Finally, we found that the Balancing and Community policies resulted in comparable outcomes regarding the average number of communities. The Mann-Whitney U test showed no significant difference among BH, BS, CH, and CS. This suggests that the Balancing policy also reduces the number of subgroups among participants. While the Balancing policy may be the most effective, selecting the best policy requires considering additional factors like the robot's preferred interaction style and user experience.






§ CONCLUSION


This paper explored the effective management of group conversations involving a social robot. Motivated by the need for systems that handle multi-party interactions, the proposed system recognizes users, engages them in dialogue, and applies policies to ensure balanced participation and foster a sense of community. This system has diverse applications in healthcare, entertainment, and education, where multi-person interaction can enhance user experience.

We implemented and tested four control policies: Balancing Hard (BH), Balancing Soft (BS), Community Hard (CH), and Community Soft (CS). These were evaluated against a baseline in 75 experiments with a humanoid robot and 300 participants grouped in sets of four. The robot moderated the conversations, applying policies to balance speaking time and word count and reduce subgroup formation. Results showed that the policies effectively managed group conversation dynamics as intended.








































  






















































plainnat




 Paper Title: VIPeR: Visual Incremental Place Recognition with Adaptive Mining and Lifelong Learning.





[
     R. Zamini[Corresponding author: rahelehzamini1361@yahoo.com] ^1,  S. Ghafouri^2  and F. Goodarzi^3

	^1  Department  of Mathematics, Faculty of Mathematical Sciences and  Computer, Kharazmi University, Thehran, Iran. 

	^2   Department of Mathematics, Faculty of Science, Arak University, Arak, 384817758, Iran.

^3   Department of Statistics, Faculty of Mathematical Sciences, University of Kashan, Kashan, Iran.

    
===================================================================================================================================================================================================================================================================================================================================================================================================================================

empty
empty



§ ABSTRACT 
Visual place recognition (VPR) is an essential component of many autonomous and augmented/virtual reality systems. It enables the systems to robustly localize themselves in large-scale environments. Existing VPR methods demonstrate attractive performance at the cost of heavy pre-training and limited generalizability. When deployed in unseen environments, these methods exhibit significant performance drops. Targeting this issue, we present VIPeR, a novel approach for visual incremental place recognition with the ability to adapt to new environments while retaining the performance of previous environments. We first introduce an adaptive mining strategy that balances the performance within a single environment and the generalizability across multiple environments. Then, to prevent catastrophic forgetting in lifelong learning, we draw inspiration from human memory systems and design a novel memory bank for our VIPeR. Our memory bank contains a sensory memory, a working memory and a long-term memory, with the first two focusing on the current environment and the last one for all previously visited environments. Additionally, we propose a probabilistic knowledge distillation to explicitly safeguard the previously learned knowledge. We evaluate our proposed VIPeR on three large-scale datasets, namely Oxford Robotcar, Nordland, and TartanAir. For comparison, we first set a baseline performance with naive finetuning. Then, several more recent lifelong learning methods are compared. Our VIPeR achieves better performance in almost all aspects with the biggest improvement of 13.65% in average performance. 







§ INTRODUCTION


Visual place recognition (VPR) aims to recognize a previously visited place by giving visual observations like images. It is a core component in simultaneous localization and mapping (SLAM) systems <cit.> to ensure the robust operation of autonomous agents and augmented/virtual reality (AR/VR) systems in large-scale environments. 
When solving the VPR task, most of the existing approaches <cit.> cast it as a retrieval task. The solution then typically involves a two-step process, with step one generating global descriptors from local features and step two matching the descriptors to the ones in a database of place-tagged descriptors.


Traditionally, VPR methods use hand-craft local features or semantic features and construct global descriptors with vector of locally aggregated descriptors (VLAD) <cit.>, graph-based descriptors <cit.>, etc. More recently, following the success of NetVLAD <cit.>, there have been notable advancements in end-to-end VPR methods in terms of accuracy <cit.> and robustness <cit.>. 
However, such a boost in performance comes at a cost. 
As the network architecture used in modern deep learning-based VPR methods getting much more complex, for example, KPConv <cit.> used in AEGIS-Net <cit.> and Transformer <cit.> used in R^2Former <cit.>, a laborious pre-training process that commonly takes days or even weeks becomes inevitable.
In addition, because the trained models have a strong independent and identical distribution (i.i.d.) assumption on the test data, the performance drops considerably when the test data is collected from an unseen environment that doesn't follow the i.i.d. assumption.







However, when deploying the VPR-equipped autonomous agents and AR/VR systems in real-world scenarios, it is difficult to collect sufficient data that covers all potential environments beforehand to pre-train the deep learning-based models. Such a gap between the advance in deep learning-based VPR methods and the practical needs of autonomous agents and AR/VR systems leads to the fact that recent practical-oriented systems still prefer the traditional VPR methods <cit.>. In this paper, we aim to close this gap by enabling the lifelong learning ability of the deep learning-based VPR method. In particular, we present VIPeR, a visual incremental place recognition method that can adapt to the newly observed environment during deployment while reserving its knowledge of previously visited environments. Fig. <ref> illustrates the core idea of the proposed VIPeR.



The idea of enabling lifelong learning for deep neural networks is not new. Also known as continual learning or incremental learning, it is designed to overcome the notorious catastrophic forgetting <cit.> when learning from sequential inputs. Despite that lifelong learning has been widely studied <cit.> and applied to many computer vision <cit.> or natural language processing <cit.> tasks, there is only a handful of works attempting to apply it to the place recognition task. 
AirLoop <cit.> and InCloud <cit.>, as the pioneers in lifelong place recognition, both tackle the catastrophic forgetting with a simple memory structure and employ knowledge distillation <cit.> to constrain the training. AirLoop further employs the memory aware synapses (MAS) <cit.> for regularization.



Although achieving promising performance, there is still plenty of room for improvement. As pointed out in CCL <cit.>, the naive triplet loss used in AirLoop <cit.> and InCloud <cit.> inevitably introduces bias when learning the global representation of various places. Such a phenomenon becomes even severe when hard-mining is applied to the training (see Table <ref>). In addition, the naive choice of a simple memory structure substantially hinders the power of the rehearsal strategy. Finally, the deterministic knowledge distillation used in AirLoop and InCloud is very sensitive to the outliers in the data, leading to potential performance degradation. 


Targeting these issues, we first introduce an adaptive mining scheme for triplet loss in metric learning. Our adaptive mining scheme can adaptively choose hard mining or soft mining, achieving a balanced performance between a single environment and multiple environments.
Focusing on lifelong learning, we then draw inspiration from human memory systems and design a novel memory bank to store memories of multiple environments. Our memory bank is made of three components. For the current environment, we have a sensory memory bank and a working memory bank. For previously visited environments, we have another long-term memory bank.
Finally, to make the model more robust against outliers, we extend the deterministic knowledge distillation used in AirLoop <cit.> and InCloud <cit.> to a probabilistic one.
We conduct extensive experiments on three large-scale datasets and our proposed VIPeR demonstrates superior performance when compared to other state-of-the-art (SoTA) lifelong place recognition methods.

In summary, our contributions are as follows: 

    
  * We introduce an adaptive mining scheme for metric learning to balance the performance within a single environment and 
    
    across multiple environments;
    
  * We design a novel, brain-inspired memory bank for rehearsal, alleviating the effects of catastrophic forgetting;
    
  * We propose a probabilistic knowledge distillation to regularize the training across different environments;
    
  * We conduct extensive experiments on three publicly available datasets and 
    demonstrate SoTA performance.
    






§ RELATED WORK




 §.§ Lifelong Learning



Lifelong learning targets the sequential learning setting and focuses on improving the adaptability and generalizability of the deep learning models. However, naive solutions like finetuning on the new tasks make the model quickly forget the knowledge about previous tasks, resulting in catastrophic forgetting <cit.>. Addressing this issue, most existing methods can be categorized into rehearsal-based approaches and regularization-based approaches <cit.>.

The core of the rehearsal-based approach is a memory buffer that either explicitly or implicitly stores samples from previous tasks. Then, when the model is trained on a new task, the samples stored in the buffer join the samples of the current task to refresh the model's memory about previous tasks. Exemplary works in this approach are iCaRL <cit.>, gradient episodic memory (GEM) <cit.>, and deep generative replay (DGR) <cit.>, which tackle the image classification task with iCaRL and GEM explicitly stores a small number of samples per class for rehearsal and DGR accompanies an additional generative model to enable implicit rehearsal.

Shifting the focus to the model itself, the regularization-based approach applies regularization on either the weight or the function. Regarding weight regularization, it commonly evaluates the importance of the model's parameters on previous tasks and mitigates the performance degradation by introducing a penalty for the loss. Within this scope, elastic weight consolidation (EWC)  <cit.> employs the Fisher information matrix to calculate the importance, while syntactic intelligence (SI)  <cit.>  and memory aware synapses (MAS) <cit.> both favor the online importance estimation. Then, in function regularization, various knowledge distillation approaches have been explored to distill the knowledge of old models to the new ones. An example work is learning without forgetting (LwF) <cit.>, which performs distillation by comparing the predictions from the output head of the old tasks to the ones of new tasks.







 §.§ Learning-based Place Recognition


NetVLAD <cit.>, as a milestone work in the learning-based VPR method, modified the VLAD <cit.> descriptor to make it differentiable and enabled end-to-end training of VPR methods. Demonstrating the representation power of the deep learning models, it inspires a plethora of works to promote the performance of deep learning-based VPR methods.

To improve the performance with image inputs, SPE-VLAD <cit.> explored the spatial pyramid structure of images to make the VLAD descriptor aware of structural information. 
Addressing the problems of appearance and viewpoint changes, Patch-NetVLAD <cit.> proposed to combine the advantages of local and global features by fusing multi-scale patch-level features that are computed from NetVLAD residuals.
Moving on to 3D inputs like point clouds, 
PointNetVLAD <cit.> is a straightforward extension of NetVLAD by replacing the 2D CNN with the PointNet <cit.> model to handle the input point clouds of fixed sizes. To empower the model to deal with inputs of random sizes, FusionVLAD <cit.> and AEGIS-Net <cit.> adopted more advanced 3D CNN with FusionVLAD focusing on cross-viewpoint VPR and AEGIS-Net concentrating on aggregating multi-level features. 

On the other hand, despite the fact that NetVLAD <cit.> is one of the most popular global descriptors, other global descriptors have also been explored. 
To start with, various forms of pooling have been employed for global descriptor generation due to their simplicity and generalizability. For example, APANet <cit.> favors sum pooling, Gordo et al. <cit.> chooses max pooling R-MAC <cit.>, MinkLoc3D <cit.> adopts generalized-mean pooling (GeM) <cit.>.
More recently, with Transformer <cit.> prevailing in a wide range of tasks, it has also been used to create global descriptors in VPR. For example, R^2Former <cit.> employs attention weights to guide the aggregations of local tokens.

Although tremendous advances have been witnessed in VPR research, most of the work tackles the task under the batch learning setting with the complete dataset available prior to the training. However, our paper investigates the VPR task under a sequential learning setting, which is addressed by a few works.

More related to our work, BioSLAM <cit.> tackles the VPR task from a lifelong learning perspective and chooses the generative replay from a rehearsal-based approach with a dual memory zone to store the previous observations. Nevertheless, AirLoop <cit.> combines a rehearsal-based approach with a regularization-based approach to mitigate the catastrophic forgetting issue with a lightweight model. Following this idea, InCloud <cit.> and CCL <cit.> extend to point cloud input with InCould introducing an angular-based knowledge distillation to relax the function regularization and CCL favoring contrast learning with InfoNCE <cit.> loss over metric learning with triplet loss <cit.>. 

Although we also build our work on AirLoop, our proposed VIPeR significantly differs from the above-mentioned methods with the adaptive mining for metric learning with triplet loss, a delicately designed three-zone memory bank for rehearsal, and probabilistic knowledge distillation (PKD) for regularization.






§ METHOD




As with many previous works on VPR, we cast the task of visual incremental place recognition as a retrieval problem. 
In particular, we define a sequence of environments ℰ = {E_0, E_1, ..., E_t, ...} with only one environment being available for training at a time. Furthermore, we define a set of images as observations for each environment E_t = {I_0, I_1, ..., I_i, ...}, which is also acquired in a sequential manner. 
Then, the VPR task is solved by incrementally constructing a database of place-tagged global descriptors 𝒢 = {g_0, ..., g_i, ... } with each one generated with g_i = f(I_i). 
f(·) here is a trainable model that consists of two sub-models, one extracts local features from input images and the other one aggregates the local features into global descriptors.
Whenever the query image I_q becomes available, the same model is used to generate a global descriptor from the query image d_q = f(I_q). Following this, by matching the query descriptor to the database descriptors, we can retrieve the top-K closest neighbors of the query image as the recognized places.
In the following, we present VIPeR to tackle visual incremental place recognition by combining metric learning with lifelong learning. An overview of VIPeR is shown in Fig. <ref>.



 §.§ Metric Learning



To train our model, we adopt metric learning with triplet loss. 
We first construct input triplet 𝒯 with an anchor image I^anc that represents a place in the environment, m positive images I^pos={I^pos_0, I^pos_1, ..., I^pos_m} that are taken from the same place, and n negative images I^neg={I^neg_0, I^neg_1, ..., I^neg_n} that are taken from different places.
Then, given the triplet 𝒯 = (I^anc, I^pos, I^neg), we drew inspiration from the naive triplet loss <cit.> and hard triplet loss <cit.>, and introduced adaptive triplet loss to train our model.





The naive triplet loss <cit.> is defined as 

    L_triplet=max( s^an_j-s^ap_i+δ ,0 ) ,

where s^an_j=sim(f(I^anc),f(I^neg_j)) is the cosine similarity between the global descriptors generated from the anchor image and a negative image sample, s^ap_i is the cosine similarity between the ones from the anchor and a positive image sample, δ is a hyper-parameter indicating the margin. When selecting the positive and negative image, the naive triplet loss opted for a random mining strategy with the sample indices i and j randomly selected. Despite that, the naive triplet loss exhibits better generalizability and is used in previous works like AirLoop <cit.> and InCloud <cit.>; it also makes the training process less effective, resulting in less discriminative global descriptors.  


The hard triplet loss <cit.>, to the contrary, goes to another extreme and favors the hard mining strategy. When choosing the sample images, the hard triplet loss focuses on the most difficult sample with the sample indices selected as i=argmin_i(s^ap_i) and j=argmax_j(s^an_j). Such a strategy largely increases the discrimination of the global descriptor and has been widely used by PointNetVLAD <cit.>, AEGIS-Net <cit.> for place recognition in a single large-scale environment. 

However, such a mining strategy inevitably introduces bias into the training, leading to performance degradation in lifelong learning settings. We later demonstrate this in Table <ref> in the ablation study.

To best join the advantages of random mining and hard mining, we propose a novel adaptive mining strategy and adjust the difficulty level of the training samples based on the fluctuations of the loss. We denote the triplet loss under the adaptive mining strategy as L_ada-triplet. Specifically, we keep tracking the changes in the loss during the training process. In the case when the loss increases in the current training step with the increment larger than a hyper-parameter T_d, we say the chosen training sample is too difficult, and we incline to a simpler sample in the following training steps. Contrarily, if the loss decreases in the current step with a decrement bigger than another hyper-parameter T_e, we determine the current training sample is too easy, and we lean towards more difficult ones. Under such a mining design, we achieve a balance between the discrimination requirements for place recognition and the generalizability requirements for handling the sequential input.




 §.§ Brain-inspired Memory Bank



To alleviate the notorious catastrophic forgetting in lifelong learning, we first investigate the rehearsal-based approach to refresh the model's memory about previous environments. Specifically, we present a brain-inspired memory bank ℬ=(M, R) with a fixed-size image storage space M and a corresponding adjacency matrix R.
Each element in the adjacency matrix R_i,j represents the relative location between place i and place j, which is then used to determine whether these two places belong to a positive or a negative pair.

Regarding the fixed-size image storage space M, we mimic the human memory system and design the architecture as a composition of a per-environment sensory memory M^sn, a per-environment working memory M^wk, and an environment-agnostic long-term memory M^lt.
Our brain-inspired memory bank is capable of improving the model's adaptability in a new environment while preserving its stability over all previous environments. We illustrate the update process of the proposed brain-inspired memory bank in Fig. <ref>. 

For any unseen environment t, the sensory memory M^sn_t, which is a first-in-first-out queue of size l^sn, is always first activated and used to store the most recently visited places. With such a design, we ensure the model stays sensitive to the latest changes in the scene. 
Whenever the sensory memory is full, the working memory M^wk_t, as a list of size l^wk, is activated to store the places coming out of the sensory memory. When the working memory is also full, we decide on whether the new place should be stored with a probability p=l^wk / num, where num is the total number of images that have passed in the current environment. This makes the working memory a perfect complement to the sensory memory since the probability of updating the working memory keeps decreasing as more images are processed, allowing the old observations to be preserved.

The long-term memory M^lt, also as a list of size l^lt, stores the samples of places from all previously visited environments to ensure its environment-agnostic property. 
When entering a new environment t, the long-term memory M_t^lt is updated following

    M_t^lt=ω M_t-1^wk+( 1-ω) M_t-1^lt ,

where M^wk_t-1 and M^lt_t-1 are the working memory and the long-term of environment t-1, ω is a hyper-parameter to control the update ratio.









  




 §.§ Relational Memory Aware Synapses

Additionally, we explore a weight regularization approach to prevent undesired weight updates when the model is adapting to the new environment.
Particularly, we follow the relational memory aware synapses (RMAS) proposed in AirLoop <cit.> to estimate the importance weight Ω^RMAS_t of each parameter θ in the environment t. Then, the changes in the parameters are penalized with a regularization loss L_RMAS. Note that for simplicity, we omit the subscript t to index the environment in the loss function. We hereby briefly review the formulation of RMAS. For more details, we refer the readers to the original AirLoop <cit.> paper. 

For environment t, the weight is approximated as

    Ω^RMAS_t≈1/N_t∑_k=1^N_t( ∂‖S̃_k, t‖_F/∂θ) ^2,

where N_t is the number of images that have been processed, ‖·‖_F is the Frobenius norm,  S̃_k, t∈ℝ^3×3 is the Gram matrix at the k-th training step. Each element in the Gram matrix is computed using the triplets we selected with adaptive mining in Section <ref>, following S̃_k, t|(0,0) = s^aa, S̃_k, t|(0,1) = s^ap_i, S̃_k, t|(0,2) = s^an_j, etc.. 


Then, the RMAS loss is computed as 


    L_RMAS=∑_θ∈Θ^n Ω^RMAS_t-1( θ_t - θ_t-1) ^2 ,

where Θ is the set of all parameters, θ_t and θ_t-1 indicate the updated parameter in the current environment t and the frozen parameter after training on environments 1, 2, ..., t-1.






 §.§ Probabilistic Knowledge Distillation

Moreover, we delve into the function regularization approach with knowledge distillation to further enhance the model's adaptability to unseen environments during lifelong learning. 
Although previous works like AirLoop <cit.> and InCloud <cit.> have also explored knowledge distillation, we believe that their performance is limited due to two reasons. First of all, when selecting memory samples for distillation, these works use images from the current environment. We claim that such a choice is counter-intuitive as the previous model hasn't seen the current environment, and its performance cannot be guaranteed. Secondly, these works opt for deterministic distillation with angular metrics, which makes the model more focused on local structures of samples.  

To select more effective samples, we propose to only use the samples from the long-term memory M^lt of our brain-inspired memory bank to perform knowledge distillation.
Besides, we present PKD to make the model pay more attention to the global distribution of the samples. To estimate the distribution of the samples in the learned feature space, we first construct the matrices H_t and H_t-1 using the model trained in the current environment t and the frozen model that has been trained on environment 1, 2, ..., t-1 respectively. Specifically, elements in the matrix are computed as



    H_Ξ|i,j=g_i, Ξ·g_j, Ξ^T/√(d)   for   Ξ∈ [t-1, t] ,

where d is the dimension of the global descriptor g. Although similar ideas can be seen in CCL <cit.>, we notice that their design with the distillation temperature hyper-parameter easily leads to a vanishing gradient during the training process. We are inspired by Transformer <cit.> and make a simple yet effective modification by replacing the distillation temperature hyper-parameter with a model-specific value √(d) to prevent the gradients vanishing issues.

Once the H matrices are computed, we can estimate the distribution with the softmax (SM) function and calculate the PKD loss with Kullback-Leibler (KL) divergence as 


    L_PKD=∑_i,jKL{SM( H_t-1|i,j) log(SM( H_t|i,j) ) } .




 §.§ Final Loss

Finally, we combine the above-mentioned loss functions into the final loss function of the following form



    L=L_ada-triplet + λ_1 L_RMAS + λ_2 L_PKD,

where λ_1 and λ_2 are hyper-parameters used to adjust the contributions of the RMAS and PKD loss functions.




§ EXPERIMENT



 §.§ Implementation Details

When choosing the VPR method, we first experiment with the same VPR method used in AirLoop <cit.>, which consists of a pre-trained VGG-19 <cit.> for local feature extraction and GeM <cit.> for global descriptor aggregation.
The hyper-parameter in GeM also follows AirLoop as p=3, and the dimension of its global descriptor is set to d=1024. Additionally, to get more discriminative global descriptors, we extend the VPR method by replacing the GeM layer with a NetVLAD layer <cit.>. We especially follow many other VPR methods like <cit.> and set the number of clusters to K=64. However, we set the dimension of global descriptors generated by NetVLAD to be the same as that of GeM descriptors. 


When forming the triplet tuple, we empirically set the number of positive and negative samples to be m=1 and n=5. And we set the margin in Eq. <ref> to be δ=1.
Regarding adaptive mining, we prevent the model from always learning with easy samples by explicitly setting the threshold for using easier samples to be higher than the one for using more difficult ones.
Especially, we choose T_d=0.02 and T_e=0.01.


As for the brain-inspired memory bank, 
we ensure a fair comparison by setting the total size of our memory bank to be the same as the queue's length in AirLoop <cit.>. Particularly, we empirically divide the total size into l^sn=500 for sensory memory, l^wk=400 for working memory, and l^lt=100 for long-term memory. In addition, we set the hyper-parameter in Eq. <ref> to be ω=0.5. 

Finally, for Eq. <ref>, we choose equal weights with hyper-parameters λ_1=1 and λ_2=1.
We train the VPR methods using the stochastic gradient descent (SGD) optimizer with a learning rate of 0.002 and a momentum of 0.9.









 §.§ Datasets and Evaluation Metrics


Datasets: We evaluate our proposed VIPeR on three large-scale, publicly available datasets, namely Oxford RobotCar <cit.>, Nordland <cit.>, and TartanAir <cit.>.

Oxford RobotCar <cit.> is a real-world autonomous driving dataset with data collected under various weather conditions across multiple urban environments in Oxford. 

For evaluation, we follow AirLoop <cit.> and select three environments under the tag “sun”, “overcast”, and “night”. For each environment, the same two sequences are selected for training and testing. 
We define two images belonging to the same place if the distance between them is less than 10m and the yaw difference is less than 15^∘.

Nordland <cit.> is also a real-world dataset with across-season data collected from a railway journey through the Nordland region of Norway. 

As AirLoop <cit.>, we also use the recommended train-test split and determine whether two images are taken from the same place if they are separated by three images or less.

TartanAir <cit.> is a photo-realistic synthetic dataset. It covers both indoor and outdoor environments and provides data collected under diverse lighting conditions, weather conditions, and times of the day.
Also following AirLoop <cit.>, we select 5 environments for evaluation with the same surface intersection-over-union (sIoU) used as the criteria to decide whether two images are from the same place. Specifically, we set sIoU<0.1 for negative, sIoU>0.7, and sIoU>0.5 for positive in training and test, respectively.





Evaluation metrics: In line with AirLoop <cit.>, we use the recall rate at 100% precision as the core metric to evaluate the model's performance on single environments. 
To better assess the performance in lifelong learning, we evaluate the model trained after each environment on every environment in the environment sequence ℰ of length T, even the unseen ones. We summarize the results in an evaluation matrix P ∈ℝ^T× T with each of its element P_i,j being the recall rate at 100% precision when evaluating the model trained after environment i in environment j.

Given the evaluation matrix, we further compute the average performance (AP), backward transfer (BWT), and forward transfer (FWT), which are introduced in <cit.>, to evaluate the overall performance, how does learning in new environments affect the previous knowledge, and how well does the model generalize to unseen environments. We refer readers to <cit.> for the detailed formulas for computation.













 §.§ Evaluation and Discussions

For comparison, we first set a baseline performance with naive finetuning on both of the VPR methods we experimented with. In addition, we also explore the performance of two generic weight regularization methods in lifelong learning, EWC <cit.> and SI <cit.>. For more recent methods, we compare our proposed VIPeR to AirLoop <cit.> and InCloud <cit.>, which are specifically designed for visual incremental place recognition. However, because AirLoop only provided the performance with the combination of VGG-19 <cit.> and GeM <cit.>, we re-train AirLoop with their official implementation to get its performance with the combination of VGG-19 and NetVLAD <cit.>. Similarly, since InCloud was only tested with LiDAR point clouds, we replaced its VPR methods with the same one that our VIPeR experimented with and re-trained the InCloud model with their official implementation.



We present the qualitative results in Fig. <ref> with VIPeR, AirLoop <cit.>, and InCloud <cit.> trained for all available environments and evaluated on the very first one to demonstrate the model's resilience to catastrophic forgetting. 
Then, we also present quantitative results in all three environments in Table <ref>.  
It can be seen that naive finetuning and generic weight regularization method EWC <cit.> and SI <cit.> exhibit inferior performance, especially in AP and BWT, indicating their inadequate abilities when fighting against catastrophic forgetting in visual incremental place recognition.
Furthermore, despite that both AirLoop <cit.> and InCloud <cit.> achieved improved performance over baseline methods by utilizing a combination of several lifelong learning methods to mitigate the catastrophic forgetting, they suffer from a notable performance drop when changing from GeM <cit.> to more discriminative NetVLAD <cit.>.

The results of our proposed VIPeR exhibit superior performance over other methods in almost all metrics.
In particular, our VIPeR prevails in terms of AP and FWT across all three datasets, which implies the model's strong performance and good generalizability towards unseen environments.
In terms of BWT, which measures the forgetness of the model, our VIPeR didn't to achieve the best performance. However, we argue this amount forgetting is acceptable as even after forgetting some knowledge of the previous visited place, our VIPeR still exhibits better recognition performance when compared to AirLoop and InCloud.
Additionally, when changing the global descriptor aggregator from GeM to NetVLAD, our VIPeR, unlike AirLoop <cit.> or InCloud <cit.>, achieves even better performance, which further demonstrates the effectiveness of presented solutions.













 §.§ Ablation Study

We further investigate the contribution of the adaptive mining, brain-inspired memory bank, and PKD loss by replacing the corresponding modules in the base model, i.e. AirLoop <cit.>, one at a time. 

Adaptive Mining: To start with, we replace the random mining with hard mining, which is commonly used in VPR tasks, and the proposed adaptive mining. We conduct experiments in all three datasets as above and present the results in Fig. <ref> and Table <ref>. 
The adaptive mining strategy used in our proposed VIPeR exhibits favorable performances in TartanAir <cit.> and Oxford RobotCar <cit.>, outperforming random mining used in the base model and hard mining used in VPR methods with batched inputs.
Although our adaptive mining does not perform as good as hard mining in Nordland <cit.> when we increasing the number of negative samples, we believe it is mainly caused by the repetitive scenes between different environments in Nordland. We argue that hard mining introduces serious bias into the learning process, which is demonstrated by the drastic performance drop in TartanAir and Oxford RobotCar.









Brain-inspired Memory Bank: Then, we replace the naive queue used in the base model with the brain-inspired memory bank in the proposed VIPeR and present the quantitative results on Oxford RobotCar <cit.> dataset in Table  <ref>. It is notable that the brain-inspired memory bank surpasses the naive queue with a large margin across all evaluation metrics. The results demonstrate the superior performance of the brain-inspired memory bank in lifelong learning with good generalizability and resilience to catastrophic forgetting.




Probabilistic Knowledge Distillation: Finally, replace the relational knowledge distillation in the base model with the PKD used in the proposed VIPeR. In addition, we also compare with performance of angular-based knowledge distillation proposed in InCloud <cit.>. 
As shown in Table <ref>, it is evident that our probabilistic distillation largely exceeds the performance of relational knowledge distillation and angular-based knowledge distillation, let alone the naive finetuning strategy.
The extraordinary results validate that our distillation approach is capable of grasping the underlying data distribution, which is a big advantage in dealing with catastrophic forgetting and the adaptability when training in unseen environments.









§ CONCLUSIONS

In this work, we propose VIPeR, a visual incremental place recognition method. We investigate
triplet mining strategies suitable for lifelong learning and find a balance between per-environment and cross-environment performance with the proposed adaptive mining. In addition, we mitigate the catastrophic forgetting issues by combining a rehearsal-based approach with a regularization-based approach. In particular, we design a novel brain-inspired memory bank to mimic the human memory system and present a probabilistic knowledge distillation.
We conduct extensive experiments on large-scale datasets, the results of which demonstrate the effectiveness of the proposed VIPeR.
As for future work, we believe there are many possible directions, like extending the VIPeR model to handle other forms of single-modality input or cross-modality input.  
















ieeetr







 Paper Title: DIABLO: A 6-DoF Wheeled Bipedal Robot Composed Entirely of Direct-Drive Joints.


Interpretable correlator Transformer for image-like quantum matter data
    Annabelle Bohrdt
    August 5, 2024
=======================================================================



§ ABSTRACT 
Wheeled bipedal robots offer the advantages of both wheeled and legged robots, combining the ability to traverse a wide range of terrains and environments with high efficiency. However, the conventional approach in existing wheeled bipedal robots involves motor-driven joints with high-ratio gearboxes. While this approach provides specific benefits, it also presents several challenges, including increased mechanical complexity, efficiency losses, noise, vibrations, and higher maintenance and lubrication requirements.

Addressing the aforementioned concerns, we developed a direct-drive wheeled bipedal robot called DIABLO, which eliminates the use of gearboxes entirely. Our robotic system is simplified as a second-order inverted pendulum, and we have designed an LQR-based balance controller to ensure stability. Additionally, we implemented comprehensive motion controller, including yaw, split-angle, height, and roll controllers. Through experiments in both simulations and real-world prototypes, we have demonstrated that our platform achieves satisfactory performance.




§ INTRODUCTION

Mobile robotics has gained increasing attention in recent years. Among the various configurations, legged robots have demonstrated reliability and robustness <cit.>. However, they are often limited in speed and face high transportation costs, particularly when carrying payloads. Conversely, purely wheeled robots do not have these drawbacks but may have limited off-road capabilities. While there are specialized configurations such as deformable wheels <cit.> and tracks designed to address these issues, they often compromise the robot's agility and speed.

Articulated wheel-legged robots offer a combination of advantages from both legged and wheeled robots. An example is ETH's quadruped ANYmal, which has a wheeled version demonstrating robust mobility <cit.>. However, one drawback of such quadrupedal wheeled robots is the presence of numerous actuators, leading to increased costs and a larger footprint. To address this concern, wheeled bipedal robots (WBRs) have gained significant research attention. WBRs leverage the benefits of both wheeled and legged systems, mitigating the challenges associated with cost and footprint by utilizing fewer actuators. However, due to their underactuated nature, designing appropriate controllers is crucial to accomplish complex tasks in challenging terrains while maintaining balance.

In this work, we present DIABLO, a 6-DoF WBR composed entirely of direct drive joints. We accurately model its dynamics using a 2D simplified model and develop essential motion controllers, including a balance controller designed using LQR. Through simulation and real-world experiments, we validate the stability of our mechanical structure and control system.


There are three contributions of this paper:

    
  * We present the mechanical, hardware, and software design of a novel WBR that is exclusively composed of direct-drive joints. This pioneering effort represents the first of its kind.
    
  * We present a method applicable to various WBR: by simplifying the complex linkage structure of the legs and treating them as simple rods. Subsequently, we treat the model as a second-order inverted pendulum and perform individual dynamic analyses on the head, rod, and wheels. Additionally, we employ LQR to devise a balance controller for this simplified model, exhibiting excellent balance performance within approximately 30 degrees of the equilibrium point.
    
  * We develop a comprehensive motion control method that enables the WBR to maintain balance even in challenging scenarios involving body tilt and non-horizontal orientation of the head (e.g., roll and pitch are not near zero as shown in Fig. <ref>B). The robustness of our system has been extensively validated through experiments conducted in both simulations and real prototypes.






§ RELATED WORK


One of the most impressive aspects of the research is the different mechanical design styles, focused on the degrees of freedom(DoF) and leg configuration problems. The most widely known WBR is the Handle robot released by Boston Dynamics <cit.>, but unfortunately, there is no related paper to introduce it in detail. The only thing known about Handle is that it uses a tandem leg configuration with a hydraulic drive. ETH's Ascento robot features a leg configuration with four linkages <cit.>, with a 1-DoF leg configuration. However, a notable limitation of the 1-DoF leg is its inability to control the head pitch angle during movement, since its joint directly controls height rather than pitch angle, making the pitch angle nearly equal to the pendulum angle<cit.>. For example, during acceleration, the head must tilt downward, and when head-mounted sensors need to detect ground information, the fixed head limits the viewing angle. SR600 <cit.>, a hydraulically driven robot designed by Harbin Institute of Technology (HIT), utilizes a series leg configuration with 2 DoFs. Ollie <cit.>, designed by Tencent RoboticX Lab, showcases impressive mobility with a parallel leg configuration which is 2-DoF as well, and it incorporates a tail joint beneath the body. The drawback of the 2-DoF leg is the inability to control the ankle joint for lateral translation movements. Hotwheel <cit.> and Nezha <cit.>, incorporates an ankle joint in its leg structure, featuring a total of 3 DoFs. Transitioning to a 3-DoF leg configuration increases the number of actuators, potentially elevating loads on joint motors and current solutions, utilizing motors with gearboxes, will cause additional joint clearance and maintenance costs. It should be noted that the above discussion is limited to the leg configuration and does not include the wheels. If the wheels are included, an additional DoF needs to be considered. After evaluating different leg configurations, we decided to remove the ankle joint and select a series 2-DoF configuration for DIABLO's leg, as it allows to control the head angle while maintaining a moderate number of actuators.

Besides the DoF and leg configuration problems, there are many control strategies to consider. Currently, available research programs focus on the robot balancing question. The reinforcement learning (RL) method has gained popularity <cit.> yet grapples with the twin challenges of neural network non-interpretability and prolonged parameter training times. As for model-based force control algorithms, a simplified model analysis <cit.> or a whole-body dynamics analysis <cit.> is generally required. After obtaining the dynamics equations of the robot, the controller design is typically carried out using LQR <cit.> or MPC <cit.>. Given the interpretability, generalizability, and ease of parameter tuning of the model-based approach in intricate environments, we've opted for this method in designing the DIABLO controller.

Additionally, given the implementation of torque control on the joints, the choice of actuator becomes crucial. It is pivotal for addressing the matching issues between the model and reality, as well as determining the upper limit of the robot's motion capabilities. As many legged robots opt for motors with gearboxes to enhance torque capacity, they often encounter issues like significant gearbox damage<cit.><cit.>. Conversely, AGVs typically employ direct-drive motors due to their high control bandwidth and minimal losses. To mitigate motor maintenance expenses and enhance joint control performance, we choose direct-drive motors as joints for DIABLO.





§ SYSTEM DESCRIPTION




 §.§ Mechanism




DIABLO features a total of 6 DoFs, with 2 DoFs dedicated to the wheels, while the remaining 4 DoFs are allocated to the hip and knee joints. All joints are efficiently powered by M1502D direct-drive motors. The implementation of direct-drive technology not only ensures a longer lifespan but also enhances system stability and improves overall reliability by eliminating transmission structure losses. The adoption of flange connections between linkages and joints simplifies the maintenance and repair processes of the robot, making it easily accessible for necessary adjustments. DIABLO can switch between crawling and standing, and the rollers under the head are used to assist the head movement during crawling.
Furthermore, the direct-drive solution eliminates friction in transmission mechanisms, resulting in higher power efficiency. 
With enhanced endurance, the robot can dedicate more time to tasks, reducing the frequency of recharging. More detailed information about the M1502D motor will be discussed in Subsection <ref>.

DIABLO's mechanical structure comprises three main components: the head, legs, and wheels. The head houses crucial components such as the battery, main control board, IMU, and more. Positioned beneath the head are four rollers (as depicted in Fig. <ref>), enabling DIABLO to switch to a crawling mode for forward movement. In this mode, the four rollers support the entire system, while the two wheels provide power, resulting in enhanced energy efficiency on horizontal terrains.



The hip motor is flanged to the knee motor, resulting in a series connection that requires less torque from the motors when the same force is applied at the foot, as compared to a parallel coaxial arrangement. The knee motor directly drives the leg components. DIABLO's leg configuration utilizes a parallelogram linkage, which plays a crucial role in distributing the forces acting on the linkages. Detailed parameters for each linkage can be found in Fig.<ref> and Tab.<ref>. It is worth noting that some linkages are designed in an arched form to accommodate practical wiring considerations in the actual robot. During the analysis, these arched linkages can be treated as straight bars. Beneath the leg, the wheel motor is flanged to the 4th rod (L_4) of the leg.

The 2-DoF leg offers the capability to adjust both the height and horizontal angle of the robot's head. This feature allows for fine-tuning the overall center of mass (CoM), ensuring balance and stability even when carrying heavy cargo. In our tests, DIABLO demonstrated the ability to carry a weight of 4 kg in a standing position and up to 80 kg in crawling mode. It can operate continuously for up to 3 hours at an ambient temperature of 34 ^∘C. On flat ground, it can easily traverse obstacles of up to 8 cm in height. The head can be adjusted to a maximum height of 20 cm, enabling the robot to cross over a wide range of obstacles. The noise level during operation is below 49 dB, ensuring a relatively quiet performance.





 §.§ Hardware





To achieve exceptional force control performance, all joints of the DIABLO robot are powered by our advanced direct-drive motor, the M1502D. This motor has undergone meticulous optimizations in terms of pole number, slot type, and the utilization of specially formulated permanent magnet materials. These optimizations ensure the motor delivers a wide range of rotation speeds and high torque output while effectively minimizing torque fluctuations.
The motor drive system employs the Field-Oriented Control (FOC) algorithm along with a high-precision angle sensor integrated into the motor itself. This combination enables precise control of motor torque, position, and speed, ensuring excellent quietness in operation. Additionally, the drive system incorporates comprehensive motor On-Board Diagnostics (OBD) monitoring mechanisms and protective functions, ensuring the motor operates safely and reliably.
For a detailed illustration of the exploded parts of the motor, please refer to Fig. <ref>, and for specific parameters, please consult Tab. <ref>.

DIABLO's motion control system is effectively managed by two main boards: a feature-rich micro-controller and a high-performance mini PC. The micro-controller board utilizes the CAN bus to establish communication with the motor drive board. It offers a wide range of I/O interfaces for serial communication, enabling functionalities such as remote control and firmware updates. Additionally, the micro-controller board integrates the BMI088, a high-performance IMU that combines a triaxial accelerometer and a triaxial gyroscope. This IMU communicates with the micro-controller board through the SPI interface, facilitating precise motion sensing and control.

On the other hand, the mini PC is equipped with a powerful 4-core ARM A53 processor, providing 5 TOPS edge inference capabilities. It supports multiple camera sensor inputs and features H.264/H.265 encoding/decoding capabilities. Communication between the micro-controller and the mini PC is established through the UART interface, enabling seamless coordination and data exchange between the two boards.



 §.§ Software




The micro-controller in DIABLO serves a dual purpose: handling peripheral communication and playing a crucial role for the motion control of DIABLO. It incorporates the ChibiOS/RT operating system and runs the basic controller, jump controller, and fall recovery controller. The basic controller includes balance controller and comprehensive motion controller, which are introduced in Sec. <ref>.
Additionally, we have developed an open-source ROS framework <cit.> on the mini PC. This framework enables users to control the robot for navigation, recognition, and other computationally intensive tasks. Its effectiveness in enhancing the robot's capabilities has been demonstrated and referenced in <cit.>.




The software framework is illustrated in Fig. <ref>. A complementary filter based state estimator is established to estimate the required states by utilizing information acquired from the IMU, motor encoder, and motor drivers.
The controller reference signal can originate from either the mini PC or the remote controller. Both the reference signal and current states are sent to the basic motion controller, jump controller, and fall recovery controller, respectively. The designed torque is calculated and sent to the motor drivers of the wheel, hip, and knee motors.
DIABLO also incorporates a battery management system (BMS) to optimize battery utilization. Furthermore, a Watchdog system is implemented to detect software anomalies and ensure system reliability and safety.
In this paper, we will provide detailed information about the basic motion controller. The jump controller and fall recovery controller are demonstrated in the attached video.
 


§ MODELING





As illustrated in Fig. <ref>, a local control frame (o_n) is positioned at the midpoint of the line connecting the two wheels. The body frame (o_b) is established relative to the local control frame. The body frame's origin is positioned at the center of the IMU, which is installed inside the head. Specifically, the IMU is situated at the midpoint of the centerline connecting the left and right hip joints. Prior to employing the Newton-Euler method for analysis, the leg linkage structure was initially simplified to a variable-length straight rod, and the model was further reduced to a 2D plane. The pitch, yaw, and roll values are obtained from the rotation of the body frame in relation to the inertia frame.

As shown in Fig. <ref>B, the CoM1 represents the center of mass of the simplified head. The position of CoM1 can be expressed as:

    _Ix_H    = _Ix + Lsinθ + lcos(β + θ)
    _Iz_H    = Lcosθ + lsin(β + θ)

where _Ix is the displacement of the o_n system relative to the o_i system in the x-direction; L is the length of the rod; θ is the pendulum angle of the rod; l is the distance from the CoM1 to the hip joint center; β is the angle between the CoM1 and the head's x-axis of rotation.

The CoM2 represents the center of mass of the simplified rod. The position of CoM2 can be expressed as:

    _Ix_L    = _Ix + L_Msinθ
    _Iz_L    = L_Mcosθ

where L_M is the distance from the CoM2 to the wheel center, and L_M satisfies the equation L_M = L - L_w. L_w is the distance from the hip joint center to CoM2.

The wheel is analyzed to obtain as following:

    F_f    = m_w_Ix+F_x2
    
        F_N    = m_wg+F_z2
    
        I_w_Ix/R   = -τ_w-F_fR

where F_f and F_N denote the horizontal and vertical component forces, respectively, at the point of contact between the ground and the wheel; F_x2 and F_z2 are the horizontal and vertical components of the force on the rod at the connection with the wheel respectively; m_w is the mass of the wheel; I_w is the inertia about the CoM of the wheel; τ_w is the torque of the wheel motor; R is the radius of the wheel; g is the gravity.

The rod is analyzed to obtain as following:

    F_x2 = m_Ld^2/dt^2(_Ix_L) + F_x1


    F_z2 = m_Ld^2/dt^2(_Iz_L)+ F_z1+ m_Lg


    I_Lθ̈ = τ_w - τ_H    +(F_z1sinθ - F_x1cosθ)L_w
       + (L-L_w)(F_z2sinθ - F_x2cosθ)

where Fx1 and Fz1 are the horizontal and vertical components
of the force on the top end of the rod respectively; m_L is the mass of the total mass of leg linkages; τ_H is the torque of the hip motor.

The head is analyzed to obtain as following:

    F_x1 = m_Hd^2/dt^2(_Ix_H)


    F_z1 = m_Hd^2/dt^2(_Iz_H) + m_Hg


    I_H(θ̈+β̈) = τ_H -F_z1lcos(θ+β)-F_x1lsin(θ+β)

where m_H is the mass of the head. I_w, I_L, and I_H  are all obtained from the SolidWorks model except I_L, which can be derived from the parallel axis theorem I_d = I + md^2.


After solving the equations of (<ref>)-(<ref>) and linearizing them around upward equilibrium, we can get the equation of motion(EoM) for the 2D simplified model:

    τ_H    = I_H(β̈ + θ̈)  
    τ_w    = I_Hβ̈ + (I_H+I_L+L^2m_H+ L^2m_L + L_w^2m_L 
       - 2LL_wm_L)θ̈+(Lm_H+Lm_L-L_wm_L)ẍ
       (L_wm_Lg-Lm_Lg-Lm_Hg)θ
    
        F_w    = (Lm_H+Lm_L-L_wm_L)θ̈+(m_H+m_L+I_w/r^2)ẍ

The EoM of the DIABLO in the 2D plane can be written as:

    aq̈ + bq = cu

where q = [γ, θ, x]^T, u=[τ_H, τ_w]^T, γ is the pitch angle of the head of DIABLO in the 2D model, and the controlled state variable q must be obtained through the transformation of joint position state variables. The corresponding relationship is given by:

    [ γ; θ; x ] = [   1   0   0   0;   1   1 1/2   0;   R   R   R   R ][   γ; σ_1; σ_2; σ_3 ]

where σ_i (i=1,2,3) stands for the angle of the hip motor, knee motor, and wheel motor, which are got by the encoders. The coefficient matrix of the equation of dynamics is deduced as follows:

    a = ∂f(EoM)/∂q̈, 
        b = ∂f(EoM)/∂q, 
        c = [  -1   0;   0  -1;   0 1/r ]

Transform the  (<ref>) to the state-space expression:

    Ẋ = AX+Bu

where 

    X   = [       γ       θ     _Ix      γ̇      θ̇    _̇İẋ   ∫γ dt ∫_Ix dt ]^T 
    u   = [ τ_H τ_w ]^T
    A   = [            0_3×3            I_3×3            0_3×2;            a^-1b          2c0_3×5; [ 1 0 0; 0 0 1 ]          2c0_3×5 ],
        B = [ 0_3×2; a^-1c; 0_2×2 ]

All states can be directly measured or analytically inferred through sensor readings, which means the output matrix of the system can be written as:

    Y = CX, C = I_8×8

The determination of the state space model involves substituting model parameters into the equations to obtain the state matrix A and control matrix 
B. The controllability matrix is of full rank, indicating that the system is controllable. Furthermore, the output matrix C is an identity matrix I_8×8, establishing the system's observability.



§ CONTROLLER DESIGN



As illustrated in Fig. <ref>, the basic motion control system of DIABLO comprises the balance controller and the comprehensive motion controller. Besides, the comprehensive motion controller consists of four key components: the height controller, roll controller, yaw controller, and split angle controller.



 §.§ Balance Controller Design

Building upon the state-space model established in the previous section, for a multi-input multi-output system, we apply LQR to our model, which takes both regulation cost function and actuator output into performance and achieves some balance between them. By tuning the state penalty matrix Q and the control penalty matrix R, we solve the Discrete-time Algebraic Riccati Equation(DARE) to determine a set of optimal feedback control gain matrices K. Thus, we design the control law as a linear combination of the system states: 

    u = -K_2×8X

The DARE is:

    P_k+1 = A^T P_k A - (A^T P_k B) (R + B^T P_k B)^-1 (B^T P_k A) + Q

As we have already obtain the continues-time state-space model, we can discretize it and find optimal solution by:

    K = (R+B^TPB)^-1B^TPA

By employing the aforementioned approach, system stability can be achieved in the vicinity of the linearized equilibrium point. To facilitate trajectory tracking for the robot, it is necessary to incorporate a reference input into the system's inputs, i.e.

    u = K(X_d - X)
    X_d = [   γ_d     0 _Ix_d     0     0     0     0     0 ]^T


Through this design, the balance controller based on LQR and 2D simplified model enables DIABLO to achieve excellent balance and straight motion.



 §.§ Comprehensive Motion Controller Design

The comprehensive motion controller elaborates on the 2D simplified model, incorporating various fundamental motion modules, including height controller, roll controller, yaw controller, and split-angle controller. These modules constitute the foundational operational capabilities of a WBR.

Through the height controller, DIABLO can adjust its leg length to adapt to the controller's demands and different terrain. According to the geometric configuration of its legs, we can deduce the relationship between the leg length and the robot height with respect to the joint position state quantities:

    L_leg = 2Lcos(σ_2/2), H = L_legcosθ

The leg length satisfies the following kinetic relationship:

    τ_knee = J^T(Δ F + m_Hg) 
    Δ F = L̈_leg,d + k_p(L_leg,d - L_leg) + k_d(L̇_leg, d - L̇_leg)

where τ_knee is the initial torque output acting on the knee motor; Δ F is the output force of the combined PD and feed-ward controller to adjust height changes in user input; L_leg,d, L̇_leg, d,L_leg,d are the desired leg length, the desired rate of leg length change, and the desired acceleration of leg length change respectively; The L_leg and the L̇_leg are the actual value of the leg length and the rate of its change respectively; the k_p, k_d are the parameters of the PD controller. 

In the roll controller, the PD controller is utilized for the control of the body roll angle, which is given as 

    Δτ_roll = k_p(ψ_d-ψ̂)+k_d(ψ̇_d-ψ̂̇̂)

where ψ̂ and ψ̂̇̂ are derived from the estimator by the sensor data after fusion solving. The Δτ_roll is integrated with the τ_knee derived from the height controller to obtain the knee motors' torque input ^lτ_knee and ^rτ_knee. By using this module, DIABLO can keep balance within a certain range of roll angles, and the head can even be non-horizontal. 

The yaw controller was devised utilizing LQR. The resulting output Δτ_w is integrated with the wheel output τ_w derived from the balance controller to get the wheel motors' torque input ^lτ_w and ^rτ_w.

The split-angle controller is to address the model mismatch issue in our balance model. As our balance model relies on a 2D simplified model derived from a 3D model, the torque difference generated by the wheel motors during steering introduces a torque on the robot along the ground normal. This torque, responsible for steering the robot, also drives the robot's legs in the opposite direction, leading to a split. 
However, such a split angle still satisfies the balance condition when merged into the 2D simplified model. This mismatch arises due to the disparity between the theoretical balance control model and the actual situation. Applying PD controller to the angular difference δθ=^rθ -^lθ, we obtain the torque output required to maintain consistent angles. This torque is then superimposed with an opposite sign onto τ_H to derive the hip motors' torque inputs ^lτ_hip and ^rτ_hip.



§ EXPERIMENT VERIFICATION




 §.§ Experiment in Simulation



In order to verify the viability of our 2D simplified model controller, which is crucial for DIABLO's ability to maintain balance and motion, we employ Matlab as our code compiler and Webots as our simulation environment.
As depicted in Fig. <ref>A, we simplify the leg structure into a single rod for the simulation. This simplified representation has been thoroughly tested and validated to achieve a stable balance in the simulation.

LQR parameters for the balance controller and PID parameters for the height controller can be conveniently tuned in the simplified model simulation, providing a basis for subsequent simulations.

We then imported the complete model into Webots for simulation, as illustrated in Fig. <ref>B. Building upon the parameters fine-tuned in the previous stage, we sequentially add the split-angle controller, roll controller and yaw controller to complete the coarse tuning of all necessary control parameters. In the simulation, we conducted velocity and angular velocity tracking tests, where the robots effectively tracked the desired signals with high accuracy.


In Fig. <ref>, DIABLO tracks a velocity target of 2 m/s for a duration of 2 seconds. Throughout the motion, the leg's pendulum angle, deduced from (<ref>), is consistently maintained within 0.5 rad, while the pitch angle of the head remains within 0.02 rad. These results unequivocally demonstrate the effectiveness of the controller design.
The successful simulation outcomes validate the robustness and reliability of our controller design, affirming its capability to achieve accurate tracking and maintain stability during dynamic motions in DIABLO.




 §.§ Experiment in Real-world Prototypes

Upon deploying the controller parameters derived from previous simulations onto the real-world prototype, only minor adjustments were required to achieve optimal performance. In the experiment, we aimed to demonstrate the superior control of the pitch angle using our balance controller in a more intuitive manner. To achieve this, we maintained the pitch angle at 1.0 rad while simultaneously tracking a desired speed of 2 m/s. As depicted in Fig <ref>, the pitch angle during the motion remains consistently within the range of 1.0 ± 0.15 rad. Simultaneously, the leg pendulum angle is within the range of -0.5 rad to 0.3 rad. These results provide strong evidence of the success of our balance controller design.
It's worth noting that the slight deviation of θ_d from 0 in this context is attributed to the mounting error of the head IMU. Despite this negligible offset, our balance controller design effectively achieves the desired pitch angle control and demonstrates excellent performance in real-world prototypes.


To evaluate the effectiveness of the height controller and roll controller, we constructed a 30° slope (approximately 0.523 rad). DIABLO's pose performance was observed as it traversed the slope on a single leg.
Fig. <ref> showcases its remarkable agility in adapting to changes in slope height while maintaining forward motion and a horizontal orientation for the head. The controller dynamically adjusts leg length for stability during operation, maintaining roll angle variation within 0.1 rad throughout traversal. This successful integration of height and roll controllers enables stable motion on slopes. 




The split-angle controller, initially designed to address model mismatch issues, typically operates with a target input set to zero during regular motion, making it difficult to observe the effectiveness of the controller. However, in specific gaits such as the spacewalk demonstrated in the Fig. <ref>, the controller's input is a non-zero sinusoidal signal.
During the spacewalk gait, DIABLO achieves a maximum split leg angle of 0.5 rad while maintaining a forward speed of 2 m/s, validating the effectiveness of the split-angle controller for special gaits.
The successful execution of the spacewalk gait, with significant split leg angles while maintaining stability and desired forward speed, highlights the efficacy of our split-angle controller. This demonstrates the controller's adaptability to specific gait patterns and confirms its role in compensating for model mismatch issues.

The conducted experiments serve to validate both the accuracy of the dynamic model presented in Section <ref> and the reliability of the controllers developed in Section <ref>.
These results provide DIABLO, the first fully direct-driven wheeled bipedal robot, with robust mobility.



§ CONCLUSION AND FUTURE WORK


This paper introduces the DIABLO platform, a WBR composed entirely of direct-drive joints. The utilization of direct-drive motors offers numerous advantages, such as eliminating transmission structure losses and achieving a high control bandwidth. By simplifying the robot to a 2D simplified model, we derived its dynamics equation, enabling a deeper understanding of its motion characteristics.
Furthermore, a model-based LQR controller has been successfully implemented on the prototype. With the assistance of the comprehensive motion controller, DIABLO demonstrates its stability and balance even in scenarios involving body tilt and non-horizontal head orientation(as shown in Fig. <ref>B and Fig. <ref>A). Through multiple experiments conducted with the prototype system, we illustrate the robot's adaptability and locomotion capabilities across various terrains.

In our future work, we intend further to explore the potential of direct-drive motors for wheel-legged robots. Besides, we plan to implement whole-body control (WBC) methods for controlling different locomotion tasks. These advancements will contribute to enhancing the versatility and performance of DIABLO, pushing the boundaries of wheeled bipedal robotics.

10

url@rmstyle






SR_Anymal
M. Tranzatto, T. Miki, M. Dharmadhikari, L. Bernreiter, M. Kulkarni, F. Mascarich, O. Andersson, S. Khattak, M. Hutter, R. Siegwart, and K. Alexis, “en-USCerberus in the darpa subterranean challenge,” en-USScience Robotics, May 2022.

Zheng_Lee_2019
C. Zheng and K. Lee, “en-USWheeler: Wheel-leg reconfigurable mechanism with passive gears for mobile robot applications,” in en-US2019 International Conference on Robotics and Automation (ICRA), May 2019.

ETH-ANYmal-legged
M. Bjelonic, R. Grandia, M. Geilinger, O. Harley, V. Medeiros, V. Pajovic, E. Jelavic, S. Coros, and M. Hutter, “en-USOffline motion libraries and online mpc for advanced mobility skills.”

ANYmal-wheeled-legged
M. Bjelonic, R. Grandia, O. Harley, C. Galliard, S. Zimmermann, and M. Hutter, “en-USWhole-body mpc and online gait sequence generation for wheeled-legged robots,” in en-USProc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst., Sep 2021.

Handle
“Boston dynamics: Introducing handle. youtube,” <https://www.youtube.com/watch?v=-7xvqQeoA8c>.

Ascento-2019
V. Klemm, A. Morra, C. Salzmann, F. Tschopp, K. Bodie, L. Gulich, N. Kung, D. Mannhart, C. Pfister, M. Vierneisel, F. Weber, R. Deuber, and R. Siegwart, “en-USAscento: A two-wheeled jumping robot,” in en-USProc. of the IEEE Intl. Conf. on Robot. and Autom., May 2019.

Ascento-2020-LQR-WBC
V. Klemm, A. Morra, L. Gulich, D. Mannhart, D. Rohr, M. Kamel, Y. de Viragh, and R. Siegwart, “en-USLqr-assisted whole-body control of a wheeled bipedal robot with kinematic loops,” en-USIEEE Robotics and Automation Letters, p. 3745–3752, Apr 2020.

AscentoPro
“This is ascento pro. youtube,” <https://www.youtube.com/watch?v=Uxt2wTI0m5o>.

SR600-HIT-1
C. Zhang, T. Liu, S. Song, and M. Q.-H. Meng, “System design and balance control of a bipedal leg-wheeled robot,” in IEEE International Conference on Robotics and Biomimetics (ROBIO), 2019, pp. 1869–1874.

sr600-hit-2
T. Liu, C. Zhang, S. Song, and M. Q.-H. Meng, “Dynamic height balance control for bipedal wheeled robot based on ros-gazebo,” in IEEE International Conference on Robotics and Biomimetics (ROBIO), 2019, pp. 1875–1880.

Ollie-2021-Nonlinear
S. Wang, L. Cui, J. Zhang, J. Lai, D. Zhang, K. Chen, Y. Zheng, Z. Zhang, and Z.-P. Jiang, “Balance control of a novel wheel-legged robot: Design and experiments,” in 2021 IEEE International Conference on Robotics and Automation (ICRA), May 2021.

Ollie-2021-RL
L. Cui, S. Wang, J. Zhang, D. Zhang, J. Lai, Y. Zheng, Z. Zhang, and Z.-P. Jiang, “Learning-based balance control of wheel-legged robots,” IEEE Robotics and Automation Letters, p. 7667–7674, Oct 2021.

Ollie-2022-RL+WBC
J. Zhang, S. Wang, H. Wang, J. Lai, Z. Bing, Y. Jiang, Y. Zheng, and Z. Zhang, “An adaptive approach to whole-body balance control of wheel-bipedal robot ollie,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Oct 2022.

Ollie-2023
J. Zhang, Z. Li, S. Wang, Y. Dai, R. Zhang, J. Lai, D. Zhang, K. Chen, J. Hu, W. Gao, J. Tang, and Y. Zheng, “Adaptive optimal output regulation for wheel-legged robot ollie: A data-driven approach,” Frontiers in Neurorobotics, Jan 2023.

HotWheel
J. Yu, Z. Zhu, J. Lu, S. Yin, and Y. Zhang, “en-USModeling and mpc-based pose tracking for wheeled bipedal robot,” en-USProc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst., Sep 2023.

Nezha_NKD
H. Chen, B. Wang, Z. Hong, C. Shen, P. M. Wensing, and W. Zhang, “en-USUnderactuated motion planning and control for jumping with wheeled-bipedal robots,” en-USIEEE Robotics and Automation Letters, p. 747–754, Apr 2021.

Learing-WBR-CCC
J. Zhang, B. Lu, Z. Liu, Y. Xie, W. Chen, and W. Jiang, “en-USA balance control method for wheeled bipedal robot based on reinforcement learning,” in en-US2023 42nd Chinese Control Conference (CCC).

MPC-simulation-only
S. Xin and S. Vijayakumar, “en-USOnline dynamic motion planning and control for wheeled biped robots,” in en-USProc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst., Mar 2020.

Mini-Cheetah
B. Katz, J. D. Carlo, and S. Kim, “en-USMini cheetah: A platform for pushing the limits of dynamic quadruped control,” in en-USProc. of the IEEE Intl. Conf. on Robot. and Autom., May 2019.

Mini-cheetah-3
G. Bledt, M. J. Powell, B. Katz, J. Di Carlo, P. M. Wensing, and S. Kim, “en-USMit cheetah 3: Design and control of a robust, dynamic quadruped robot,” in en-USIEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Oct 2018.

DIABLO_ROS
“Diablo-ros. github,” <https://github.com/DDTRobot/diablo_ros2>.

Fast-DIABLO
J. Wang, L. Xu, H. Fu, Z. Meng, C. Xu, Y. Cao, X. Lyu, and F. Gao, “Towards efficient trajectory generation for ground robots beyond 2d environment,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom., Huzhou, China, 2023, pp. 7858–7864.





 Paper Title: Collecting Larg-Scale Robotic Datasets on a High-Speed Mobile Platform.





Flat-band Fulde–Ferrell–Larkin–Ovchinnikov State from Quantum Geometry Discrepancy
    K. T. Law
    August 5, 2024
==================================================================================

empty
empty



§ ABSTRACT 
Mobile robotics datasets are essential for research on robotics, for example for research on Simultaneous Localization and Mapping (SLAM). Therefore the ShanghaiTech Mapping Robot was constructed, that features a multitude high-performance sensors and a 16-node cluster to collect all this data. That robot is based on a Clearpath Husky mobile base with a maximum speed of 1 meter per second. This is fine for indoor datasets, but to collect large-scale outdoor datasets a faster platform is needed. This system paper introduces our high-speed mobile platform for data collection. The mapping robot is secured on the rear-steered flatbed car with maximum field of view. Additionally two encoders collect odometry data from two of the car wheels and an external sensor plate houses a downlooking RGB and event camera. With this setup a dataset of more than 10km in the underground parking garage and the outside of our campus was collected and is published with this paper. 





§ INTRODUCTION


The ShanghaiTech Mapping Robot <cit.> is a powerful system to collect sensor data. It features many sensors, such as 5 stereo pairs of GS3-U3-51S5C-C 5MP RGB cameras with up to 60Hz frame rate, a Ladybug5+ omnidirectional camera system, 5 RGB-D and 5 radar sensors, 7 LiDARs, stereo infrared cameras, stereo event cameras, IMUs and an RTK GPS system. All this data is collected with a 16-node "Cluster on Wheels" <cit.>. The goal of this mapping robot is to collect universal datasets for research on robotic algorithms such as Simultaneous Localization and Mapping (SLAM) <cit.>. For that we will use SLAM Hive <cit.>, a cloud-based system for benchmarking and analysing 10s of thousands of mapping runs. 

The mobile base of the ShanghaiTech Mapping Robot is a Clearpah Husky. This robot has a maximum speed of 1m/s. This makes this system impractical for collecting large scale datasets, e.g. of the big parking garage or the campus of our school. Recognizing this challenge, this paper proposes a novel approach to enhance the mapping robot's speed and efficiency by leveraging the mobility of a small vehicle. By securing the mapping robot to a flatbed car, we aim to significantly improve its ability to cover large distances quickly and efficiently. This solution not only addresses the issue of speed but also allows for the strategic placement of sensors, such as downward-looking cameras, on this vehicle to optimize data collection quality, while maintaining the important sensor field of view in the forward direction.

The contributions of this paper are:

    
  * The design and implementation of a secure mounting mechanism for the wheeled mapping robot on the flatbed car.
    
  * The design and implementation of an external dual-wheel encoding system to record odometry data.
    
  * The design of an external downlooking sensor platform connected to the ShanghaiTech Mapping Robot.
    
  * The collection and sharing of a large-scale dataset of the ShanghaiTech underground and campus area, together with all code and CAD data.




The rest of the paper is organized as follows: Section <ref> introduces the state of the art of dataset collection before the ShanghaiTech Mapping Robot. The hardware and software we designed for the flatbed mapping robot system is described in Section <ref>. That includes the mounting system, wheel encoder setup as well as an external downlooking sensor plate. The dataset and the SLAM experiment with it are presented in Section <ref>. The paper concludes with Section <ref>.



§ STATE OF THE ART


Datasets, such as the one to be collected with our platform, play an important role in robotics. The Oxford Radar RobotCar Dataset <cit.> is a pioneering dataset designed for researching scene understanding using Millimetre-Wave FMCW scanning radar data. This dataset targets autonomous vehicle applications, leveraging the robustness of radar technology against adverse environmental conditions such as fog, rain, snow, or lens flare, which often pose challenges to other sensor modalities like vision and LiDAR. Collected in January 2019, the dataset encompasses thirty-two traversals of a central route in Oxford, covering a total distance of 280 km of urban driving.

One of the most prominent car datasets is the KITTI Dataset <cit.>. It features RGB and LiDAR data on public roads. In contrast, we aim to also collect data in places inaccessible to fully sized cars. Instead we are more interested in the mobile robotics domain. 

In the realm of high-precision mapping, the Advanced Mapping Robot and High-Resolution Dataset <cit.> introduces a fully hardware-synchronized mapping robot, which includes support for an externally synchronized tracking system to achieve super-precise timing and localization. The mapping robot is equipped with nine high-resolution cameras and two 32-beam 3D LiDARs, complemented by a professional static 3D scanner for ground truth map collection. With comprehensive sensor calibration, three distinct datasets were acquired to evaluate the performance of mapping algorithms within and between rooms.

The S3E dataset <cit.> offers a novel large-scale multimodal collection captured by a fleet of unmanned ground vehicles following four collaborative trajectory paradigms. This dataset comprises 7 outdoor and 5 indoor scenes, each exceeding 200 seconds in duration, featuring well-synchronized and calibrated high-quality stereo camera, LiDAR, and high-frequency IMU data. S3E surpasses previous datasets in terms of size, scene variability, and complexity, boasting four times the average recording time compared to the pioneering EuRoC dataset.

LiDAR-based SLAM for Robotic Mapping: State of the Art and New Frontiers <cit.> provides an exhaustive literature survey on LiDAR-based simultaneous localization and mapping (SLAM) techniques. This comprehensive review covers various LiDAR types and configurations, categorizing studies into 2D LiDAR, 3D LiDAR, and spinning-actuated LiDAR. The paper highlights the strengths and weaknesses of these systems and explores emerging trends such as multi-robot collaborative mapping and the integration of deep learning with 3D LiDAR data to enhance SLAM performance in complex environments.

Reconstructing maps using data obtained from six degrees of freedom poses significant challenges due to the asynchronous nature of ranging data reception, leading to potential mismatches in the generated point clouds <cit.>. The SegMatch method addresses this challenge by matching 3D segments, thereby enhancing localization robustness without relying on perfect segmentation or predefined objects <cit.>.

LeGO-LOAM <cit.> is a lightweight, real-time six-degree-of-freedom pose estimation method specifically designed for ground vehicles operating on varying terrain. This method reduces noise through point cloud segmentation and feature extraction, utilizing planar and edge features for a two-step Levenberg-Marquardt optimization to solve transformation problems between consecutive scans, thereby improving both accuracy and computational efficiency.







To the best of our knowledge, we are the first researchers attempting to mount a complete mapping robot on an additional faster platform in order to enhance its operation speed and range.



§ SYSTEM DESCRIPTION




An electric flatbed car has been selected as our high-speed mobile platform to accommodate our mapping robot, thus overcoming the speed limitations of the current mapping robot. Specifically, we have chosen the hydraulic lifting flatbed car model. This vehicle meets our requirements for load platform speed and endurance, with an empty car range of 30 km and a maximum speed of 25 km/h. The motor has a power rating of 1000W, which is sufficient for our needs. It mounts the robot at the front, while the driver is, with a very small footprint, sitting at the back, thus giving full field-of-view to the front and sides and only obstructing a little to the back (depending on the drivers size :/).

The lifting platform of this vehicle is beneficial for our load handling tasks, as well as for the information collection and map construction tasks performed by the robot. The size of the load platform is 1500 x 800 mm, and it has a lifting capacity of 500 kg. Additionally, the platform can be raised to a maximum height of 1600 mm above the ground.














 §.§ Mounting System


The next challenge we face is how to securely mount the mapping robot onto the hydraulic lifting flatbed car. Our proposed solution involves using a wood plate with a thickness of 21mm as the top layer of the structure. This plate will have four holes to accommodate the robot's wheels. To ensure stability, six wooden beams will be used to connect the wood plate to the flatbed structure of the car, elevating the entire assembly. Each pair of wooden beams will be reinforced with wooden strips to provide additional support. Finally, the entire structure will be firmly secured using long bolts, which will be fastened from top to bottom. This method ensures that the mapping robot is securely and stably mounted onto the hydraulic lifting flatbed car. (Fig. <ref>)





Based on the actual conditions, we adjusted the parameters and shape of the frame. As length of the hole is less than the diameter of the wheel, the elasticity of the rubber tire and the texture on the wheel's surface allow the tire to fit securely into the hole, thereby preventing the safety issues associated with tire rolling during travel. The placement process requires ensuring the vehicle is level to avoid reading errors caused by tilting. The prototype is shown in Fig. <ref>.














 §.§ Wheel Odometry


During actual driving, it is necessary to obtain the rotation angles of the vehicle's left and right wheels to calculate the travel distance and direction. We achieve this by connecting an encoder to each of the two passive back wheels and locking their rotation together, allowing the encoders to capture the relevant wheel rotation information. Due to the vehicle's structure not meeting the encoder installation requirements for this task, we need to design an additional framework to mount the encoder while ensuring it rotates co-axially with the wheel. Our approach involves installing a platform at the wheel's center, secured by four bolts originally surrounding the tire. This ensures coaxial rotation of the platform and the wheel. A flange coupling can be installed on top of the platform to lock the rotation of the encoder shaft and the platform. The setup is illustrated in Fig. <ref>.








The rotation of the wheel is measured relative to the vehicle body reference frame. To ensure accurate acquisition of the wheel rotation values through the encoder's shaft rotation, we need to securely attach the encoder to the vehicle body. We place the encoder in a bracket, which is connected to the flatbed car body using aluminum profiles (Fig. <ref>). This configuration ensures that the rotation of the encoder shaft relative to the encoder body is equal to the rotation of the wheel relative to the vehicle body. The overall final result of the flatbed car modification is shown in Fig. <ref>.







The no-name wheel encoders we utilized have 1024 pulses per rotation and are connected to a STM32F103RCT6 development board. We are using the hardware quadrature signal decoding feature of the MCU to get reliable wheel rotation speed and direction readings. The development board is connected via USB to one node of the mapping robot's cluster. There a ROS node is running which is then publishing the encoder readings as ROS messages, such that they may be recorded in ROS bagfiles and/ or used to calculate the robot motion using the odometry model. The connection diagram is shown in Fig. <ref>. All the code is shared in the link provided below.





  §.§.§ Odometry Model

We model the flatbed car as a two-wheel differential-drive robot, using the non-steered back wheels, that are connected to the drive motor via a differential gear. There are two main parameters: the distance between two wheels, denoted by L, the radius of the wheel, denoted by R. From the encoder, we get the radian that each wheel travels, multiply it by R, get d_l,t_k and d_r,t_k as the variation of travel of the left and right wheel from time t_k to time t_k+1. The odometry equation of the platform is as follows,

    x_t_k+1   = x_t_k + d_l,t_k + d_r,t_k/2cos(θ_t_k) 
     
    y_t_k+1   = y_t_k + d_l,t_k + d_r,t_k/2sin(θ_t_k) 
    θ_t_k+1   = θ_t_k + d_r,t_k - d_l,t_k/L


In order to obtain the coordinates with the origin located on the vehicle, it is necessary to perform the corresponding coordinate system transformation, given that the coordinate system for data collection has its origin positioned on the UGV. When we obtain l_0 and h_0 (the difference in distance in the y-direction and z-direction between the rear wheel center and the UGV center, respectively), we can proceed with the coordinate system transformation.The transformation equation from the vehicle coordinate system to the UGV coordinate system is as follows,

    p_u   = R_v^u p_v + t_v^u
     
    x_u   = x_v
     
    y_u   = y_v-l_0
     
    z_u   = z_v-h_0

According to our measurement, l_0 = 1.21 m, h_0 = 0.59 m.



  §.§.§ Odometry Experiment




To achieve more precise odometry, we implement 5 real-world experiments, including 3 forward, 1 backward, and 1 circle. There we drove the robot a certain distance/ diameter (see Table <ref>) and measured the true distance/ diameter using a tape measure. From Table <ref> we can see that the odometry estimates we get when applying the mesured wheel radius R and wheel baseline L have an err of several percent. 

We optimize our measured L, R with grid search approach. We calculate odometry by different L, R pairs from the product of L = [0.6, 0.8] and R = [0.15, 0.17], where each interval has 50 uniform values. The measured L = 0.7m and R = 0.1575m, and the optimized L = 0.64m and R = 0.164m, resulting in errors of typically less than one percent.





 §.§ Downlooking Sensor Plate


The ShanghaiTech Mapping Robot features a downlooking sensor plate, in order for algorithms to use this data to calculate even more precise robot motion estimates. But this plate is obstructed by the flatbed car. We have thus designed and installed an external downlooking sensor plate, featuring an event camera, an RGB camera and two non-flickering LED lights, compatible with event cameras. The plate with the devices can be seen in Fig. <ref>. The sensors are connected to the cluster of the mapping robot via USB cables. Additionally, there are hardware synchronization cables running from the IO port of each sensor to the hardware synchronization board of the mapping robot's cluster. Furthermore, there are power lines for the sensors and the LED light connected to the power distribution panel of the robot.












§ DATASET



With the ShanghaiTech Mapping Robot mounted on the flatbed car we have collected a sample dataset at the beautiful ShanghaiTech University campus. The dataset is 47 minutes and 10.21km long, with an average driving speed of 13km/h. The dataset features, from the downlooking sensor palate, an event camera EVK4 and a downlooking 5MP camera. Next to the collected wheel encoder data, we are sharing the data from the 6-lens Ladybug5+ omnidirectional camera, 6 different LiDARs, the RTK GPS and IMU data. The 884GB dataset can be downloaded here: 
<https://robotics.shanghaitech.edu.cn/static/datasets/flatbed/>.

This link also provides all the CAD files used in the project, as well as all the code for the collecting and calculating the wheel encoder odometry.

The dataset features the parking garage that is below most of the campus, as well as extensive above-ground traversals of the campus. There are also some nice overlaps with loop-closing opportunities with the ShanghaiTech Outdoor Mapping Robot dataset <cit.>.






 §.§ Dataset SLAM Experiment


From the dataset we employed LIO-SAM <cit.>, a tightly-coupled LiDAR inertial odometry algorithm, to generate a point cloud map from the front robosense 128-beam ruby LiDAR. The point cloud was downsampled to a resolution of 0.1m and has 73 million points - it is depicted in Fig. <ref>.



§ CONCLUSIONS



The paper presents a high-speed mobile platform for collecting large-scale robotic dataset. The rear-steered flatbed car allows the mapping robot to be placed at the front, maintaining a good field of view for the front-facing sensors and only obstructing a little bit of the back-view. The hydraulic lift even allows to adjust the height of the sensor platform. Furthermore we introduced dual wheel encoders, that can, as our experiments showed, provide reliable platform odometry pose estimates. Our external downlooking sensor plate offers a view of the ground, such that according algorithms may calculate even more precise robot motion estimates. The whole system is working well, as our 10km collected dataset and the point cloud map generated from it demonstrated. In the future we will collect even richer datasets with the system, utilizing all sensors on the ShanghaiTech Mapping Robot, as part of our efforts to collect a very nice ShanghaiTech Robotic Dataset. 



                                  
                                  
                                  
                                  
                                  
















§ ACKNOWLEDGMENTS

This work has been partially funded by the Shanghai Frontiers Science Center of Human-centered Artificial Intelligence.
This work was also supported by the Science and Technology Commission of Shanghai Municipality (STCSM), project 22JC1410700 "Evaluation of real-time localization and mapping algorithms for intelligent robots". The experiments of this work were supported by the core facility Platform of Computer Science and Communication, SIST, ShanghaiTech University.










unsrtnat






 Paper Title: Navigating Beyond Instructions: Vision-and-Language Navigation in Obstructed Environments.


















  The University of Queensland, CSIRO Data61
  Brisbane
  Australia
haodong.hong@uq.edu.au



  The University of Queensland
  Brisbane
  Australia
sen.wang@uq.edu.au



  The University of Queensland
  Brisbane
  Australia
helen.huang@uq.edu.au



  The University of Adelaide
  Adelaide
  Australia
qi.wu01@adelaide.edu.au


Corresponding author

  CSIRO Data61, The University of Queensland
  Brisbane
  Australia
jiajun.liu@csiro.au











§ ABSTRACT 
Real-world navigation often involves dealing with unexpected obstructions such as closed doors, moved objects, and unpredictable entities. 
However, mainstream Vision-and-Language Navigation (VLN) tasks typically assume instructions perfectly align with the fixed and predefined navigation graphs without any obstructions.
This assumption overlooks potential discrepancies in actual navigation graphs and given instructions, which can cause major failures for both indoor and outdoor agents.
To address this issue, we integrate diverse obstructions into the R2R dataset by modifying both the navigation graphs and visual observations, introducing an innovative dataset and task, R2R with UNexpected Obstructions (R2R-UNO).
R2R-UNO contains various types and numbers of path obstructions to generate instruction-reality mismatches for VLN research.
Experiments on R2R-UNO reveal that state-of-the-art VLN methods inevitably encounter significant challenges when facing such mismatches, indicating that they rigidly follow instructions rather than navigate adaptively. 
Therefore, we propose a novel method called ObVLN (Obstructed VLN), which includes a curriculum training strategy and virtual graph construction to help agents effectively adapt to obstructed environments. 
Empirical results show that ObVLN not only maintains robust performance in unobstructed scenarios but also achieves a substantial performance advantage with unexpected obstructions.   
The source code is available at <https://github.com/honghd16/ObstructedVLN>.







<ccs2012>
<concept>
<concept_id>10010147.10010178.10010224.10010225</concept_id>
<concept_desc>Computing methodologies Computer vision tasks</concept_desc>
<concept_significance>500</concept_significance>
</concept>
<concept>
<concept_id>10002951.10003227.10003251</concept_id>
<concept_desc>Information systems Multimedia information systems</concept_desc>
<concept_significance>500</concept_significance>
</concept>
</ccs2012>


[500]Computing methodologies Computer vision tasks
[500]Information systems Multimedia information systems
























Navigating Beyond Instructions: Vision-and-Language Navigation in Obstructed Environments
    Jiajun Liu
    August 5, 2024
=========================================================================================





§ INTRODUCTION






Vision-and-Language Navigation (VLN) <cit.> requires agents to follow natural language instructions to reach a specified destination.
Recently, there has been a growing interest in this area due to its great potential for real-world applications such as household robots, especially with the booming of Large Language Models (LLMs) <cit.>.
However, current VLN tasks are limited by several unrealistic assumptions, making their application remain in simulators with few real-world robotic deployments <cit.>.


One significant constraint is what we call the “perfect instruction assumption”, which implies that instructions are always perfectly aligned with the environment, neglecting real-time dynamics like unexpected obstructions.
Agents trained under this assumption excel in following instructions but lack adaptability for actual navigation where discrepancies between instructions and reality are commonly seen.
For example, as shown in <ref>, the human instructs the agent to “Walk straight down the hall” based on prior knowledge of the house. 
However, real-world environments have changed, with unforeseen obstacles like a suitcase blocking the hallway.
While humans can quickly adapt and find detours, current VLN agents struggle with these instruction-reality mismatches, often leading to navigation failures.


To address this issue, it is essential to introduce these discrepancies into VLN.
While various factors can lead to such discrepancies, this work focuses on one of the most representative and prevalent causes: obstructions. 
We propose to integrate obstructions into existing discrete VLN environments to block the path described by the instruction, resulting in an instruction-reality mismatch.
Notably, although both approaches involve obstructions, our work differs from previous work on obstacle avoidance <cit.> in terms of problem setup, navigation focus, and potential solutions (See <ref>), leading us to choose the discrete setting.




Therefore, we make various modifications to the navigation graphs and visual observations of the R2R dataset <cit.>, proposing the R2R with UNexpected Obstructions (R2R-UNO) dataset as the first VLN task to emphasize the instruction-reality mismatches.
At the graph level, we selectively block edges whose removal does not impact the overall graph connectivity in existing paths, ensuring agents can still reach the destination.
To keep visual coherence with graphs, we design an object insertion module that leverages text-to-image inpainting techniques <cit.> to integrate diverse objects into different scenes seamlessly.
Due to the instability of inpainting results, a filtering module is employed to select the high-quality ones from multiple candidates to ensure obstruction generation.
Our study using R2R-UNO reveals that state-of-the-art VLN agents <cit.> perform poorly in obstructed environments, limiting their practical applications.

Although we expect agents to perform well with mismatches, it is crucial to maintain their performance in original environments simultaneously, which represent most situations. 
However, directly training with these two types of data poses challenges for agents to learn beyond following instructions and to differentiate between mismatched and aligned scenarios. 
Empirical results show that discrepancies between these two environments can lead to imbalanced optimization, favoring one type. 
To address this, we develop the Obstructed VLN (ObVLN) method, including a curriculum training strategy <cit.> to organize the training and a novel graph construction mechanism to introduce virtual nodes for blocked edges to facilitate efficient exploration.
The overall framework including data generation and agent training is presented in <ref>.


We conduct comprehensive experiments on R2R, REVERIE <cit.>, and R2R-UNO datasets to demonstrate the significance of incorporating instruction-reality mismatches in VLN. 
Established methods, such as DUET <cit.>, struggle in obstructed settings, with a significant 30% drop in Success Rate (SR). 
The proposed object insertion and filtering modules provide crucial visual feedback aligned with navigation graph changes, proved by the improved performance of agents using inpainting images.
By employing ObVLN, agents not only maintain robust performance in original scenarios but also effectively adapt to mismatches, achieving an impressive 67% SR, marking a significant advancement.
We summarize our contributions in this paper as follows: 


  * We address the underexplored issue of instruction-reality mismatches in VLN by proposing R2R-UNO, the first VLN dataset that includes such mismatches through graph changes and diverse obstruction generation, offering a unique challenge that reflects real-world navigation complexities.

  * We highlight the lack of adaptability in current VLN methods for obstructed environments and propose ObVLN as a solution, which employs curriculum learning and virtual graph construction to enhance agent adaptability.

  * Through extensive experiments, we prove the significance of introducing R2R-UNO in VLN research and show that ObVLN performs well in both original and obstructed environments, achieving a significant 23% SR increase in R2R-UNO.
 




§ RELATED WORK

Vision-and-Language Navigation
In VLN, agents need to navigate within simulated environments like Matterport3D <cit.> following natural language instructions. 
Numerous methods <cit.> and datasets <cit.> have been developed to address various challenges in this domain.
Some works address the data scarcity problem by introducing extra sources, including synthesized instructions <cit.>, additional environments <cit.>, and predicted scenes <cit.>.
Recently, ScaleVLN <cit.> synthesized enormous high-quality instruction-trajectory pairs for HM3D <cit.> and Gibson <cit.> environments to make agent performance approach human results.
Other works <cit.> apply diverse network structures such as LSTM <cit.>, Transformer <cit.>, and Graph Neural Networks <cit.> to enhance cross-model alignment and decision-making ability.
For example, HAMT <cit.> encodes the full history information through transformers and integrates it with instructions and observations for better action prediction.
DUET <cit.> further maintains a topological map to enable agents aware of global visual representations to make global decisions instead of adjacent viewpoints.
We adopt HAMT and DUET as the evaluation models due to their superior performance and difference in whether map-based.

 Environment Changes in VLN
Many works <cit.> have proposed to modify VLN environments for different purposes, which can generally be categorized based on the changing level. 
One category involves changing the visual observations to augment data for improving agent generalization.
For example, Li et al. <cit.> utilized image captioning to obtain language descriptions of panoramas and employed generative models to produce novel views based on these descriptions.
Conversely, another category focuses on graph-level adjustments rather than the visual level to go beyond the established navigation graphs by mixing up different graphs <cit.> or substituting a fixed viewpoint with proximate locations <cit.>.
Notably, VLN-CE <cit.> abandons the graph-based navigation paradigm and allows agents to traverse continuous environments freely to enhance task realism.
Differently, our task makes changes at both visual and graph levels to block edges in the graph and generate obstacles in the views to generate instruction-reality mismatches.

 Obstacle Avoidance

Obstacle avoidance has been a long-standing research challenge in visual navigation <cit.>.
In VLN, ETPNav <cit.> applies an obstacle-avoiding controller with a trial-and-error heuristic to explicitly escape from deadlocks.
SafeVLN <cit.> employs a LiDAR-based waypoint predictor and a re-selection strategy to avoid non-navigable waypoints and obstacles. 
However, our work diverges from these works under the continual setting <cit.> in three aspects, leading us to choose the discrete setting.
Firstly, we target a broader range of situations with instruction-reality mismatches, including avoidable obstacles, closed doors, rearranged furniture, instruction errors, etc.
These scenarios can be effectively captured by graph changes but are hard to model as continuous signals.
Secondly, while obstruction avoidance focuses on assessing the properties of obstructions to avoid them, our work emphasizes changes in navigation graphs that render the instructions temporarily invalid, ignoring the nature of the obstructions.
The discrete setting decouples path planning from technicalities, such as the shape and size of obstacles, thereby focusing on high-level adaptability when instructions fail and enhancing its generality.
Finally, addressing instruction-reality mismatches involves more than just navigating around obstructions. 
It requires a complex strategy to find detours and the ability to navigate without instruction guidance, posing a significant adaptability challenge which is too hard for current continuous agents.

 Object Insertion
Inserting novel objects into target images has always been a research challenge in Computer Vision.
Early works <cit.> employ the cut-and-paste strategy to merge two pictures, which is straightforward but lacks photorealism.
With the development of neural networks, object insertion can be achieved in various ways and higher quality, including image synthesis <cit.>, neural radiance fields editing <cit.>, image generation <cit.>, etc.
In navigation, THDA <cit.> inserts 3D scans of household objects into random locations as navigation goals to augment the training data for ObjectGoal Navigation <cit.>.
In VLN, Envedit <cit.> leverages semantic image synthesis <cit.> to create objects based on modified environment semantics as augmented environments.
In this work, we uniquely take advantage of the text-to-image inpainting models <cit.> to seamlessly embed desired objects within specific view locations without influencing visual surroundings.



§ OBSTRUCTED ENVIRONMENTS



In this section, we first introduce the problem formulation of VLN on vanilla and obstructed environments, and then describe our proposed R2R-UNO dataset.



 §.§ Problem Setup
 
In VLN, the agent must follow a natural language instruction x̂=(x_1,x_2,...,x_L) with L words to navigate a simulated environment.
This environment is usually discrete with a predefined undirected navigation graph 𝒢={𝒱, ℰ} with navigable nodes 𝒱 and connectivity edges ℰ.
At each timestep t, the agent perceives a panoramic representation, comprised of N=36 views O_t={o_t^i}_i=1^N and an orientation feature that encodes heading θ and elevation ϕ information of its current node v_t. 
It then determines an action a_t to transition to one of the neighboring nodes 𝒩(v_t) by selecting the view that aligns best with the target node from the current panorama O_t.
An additional “STOP” action is available to conclude the navigation process.
For each instruction x̂, there exists a corresponding ground truth path P=<v_1, e_1, v_2, e_2, ⋯, v_n> representing the intended trajectory with n nodes for the agent. 
However, previous work presumes perfect alignment between instruction and reality, assuming that all edges <e_1, e_2, ⋯, e_n-1> are consistently accessible.
This assumption overlooks the dynamics of real-world navigation, where the graph 𝒢 may have changed.
Among various reasons for graph changes, we focus on the most representative one: obstructions.
In our obstructed environments, some specific edges E_s ⊆ P might be obstructed, making parts of the instruction inapplicable to the current scenario. 
This obstruction also leads to corresponding visual changes in the panoramic views O_t of nodes linked by these edges. 
When meeting obstructed edges E_s, the agent must find a substitute path P' to reach the final destination v_n.



 §.§ R2R-UNO



In this section, we introduce the proposed R2R-UNO dataset in detail, including the graph changes and visual modifications.





  §.§.§ Graph Changes

For a path P=<v_1, e_1, v_2, e_2, ⋯, v_n> in R2R, we first identify all redundant edges within this path, denoted as E_r ⊆ P.
A redundant edge e_i=(v_i, v_i+1) ∈ E_r is defined by the property that its removal does not affect the overall connectivity of the graph, ensuring an alternate path P' between v_i and v_i+1. 
To account for scenarios where multiple edges are concurrently obstructed, we assess the collective redundancy of combinations of these redundant edges E_r and categorize these combinations by the number of obstructed edges x into Block-x set.
Since R2R paths have 4 to 7 nodes in length, we set N_max=3 as the maximum number of obstructed edges, generating three sets Block-1,2,3, each containing individual training and evaluation splits.

With a Block-x set, consider a combination of redundant edges C=<e_i_1, e_i_2, ⋯, e_i_x>.
For each edge, we identify the shortest path <P'_1,P'_2,⋯,P'_x> between the corresponding nodes in the modified graph 𝒢={𝒱, ℰ-C} to replace C in the original path P, generating the path P as the new intended path for agents:

    P̅ = ⟨ v_1, …, v_i_j, P'_j, v_i_j+1, …, v_n ⟩,   ∀ j ∈1, 2, …, x

We refer to P̅ as the “real path” to be traversed by agents, in contrast to the “instructional path” P based on the navigation instructions x̂.
Therefore, the instruction-reality mismatch is defined as P≠ P, which demands agents actively seek alternate paths. 
To exclude excessively lengthy and impractical new routes, each Block-x set possesses a “real path” length restriction as L(P̅) ≤ 10+5 · x. 
As a result, 98.8% R2R paths are modified to be one or more new paths in R2R-UNO.
As indicated in <ref>, the R2R-UNO sets contain a greater number and length of paths than R2R, with the average path length increasing with x, denoting greater navigation difficulty.

Notably, when constructing P̅, we deliberately avoid shortening the path when P' contains future viewpoints from P, even if this leads to the loop formation in P̅.
This design aligns with practical navigation scenarios since when deviating from the instruction, it is challenging for agents to align the current viewpoint with the described path due to the absence of contextual clues. 
Therefore, it is logical and efficient for agents to seek a detour around the obstruction before attempting to re-align with the instructions.



  §.§.§ Visual Changes


To align with the graph changes, we introduce two novel modules to infuse various objects into panoramic views of nodes along redundant edges, as shown in <ref>.

The first one is the object insertion module, which employs a stable diffusion inpainting model <cit.> to approach the issue from an inpainting perspective.
Consider a redundant edge e linking nodes v_a and v_b. 
We elaborate the process of modifying the panoramic view O_a={o_a^i}_i=1^36 at node v_a as follows, which is similarly applied to O_b.
First, we localize the other node v_b within O_a to find the corresponding discrete view o_a^b and calculate the pixel coordinates (x_b, y_b) in o_a^b as follows:

    f    = H / (2 ·tan(δ / 2)) 
    
    x_b    = tan(θ_r) · f + W/2 
    
    y_b    = tan(ϕ_r) · f + H/2

W, H is the width and height of o_a^b, while θ_r and ϕ_r is the relative heading and elevation from v_a to v_b, and δ is the vertical Field of View (FoV) to calculate the focal length f.
A right trapezoid mask M_a^b is then generated, expanding from the bottom edge to around the point v_b with a specified width to cover potential areas of paths from v_a to v_b.
This is based on the observation that paths in these views either stretch from the bottom middle to the target point (x_b, y_b) or form a vertical line from the bottom to the target. 
Finally, we combine an object name obj with a predefined prompt template to formulate the final prompts pr, which, along with the view image o_a^b and mask M_a^b, serve as the input to the inpainting model to generate the inpainted view ô_a^b:

    ô_a^b = Inpainting(o_a^b, M_a^b, pr)


While inpainting models can generate visually coherent and photo-realistic images, their instability is notable, with a certain probability of failing to incorporate the desired object.
This instability often arises when the area outside the mask already contains elements similar to the intended object, such as a “chair” or “table”, which can mislead the inpainting model even with intricate prompts.
Therefore, we propose a filtering module to improve the quality of inpainted images, which evaluates multiple generated candidates with different objects to filter out higher-quality ones.
Specifically, to enrich the generated scenarios, we first carefully select ten object categories as obstructions based on their frequencies in both the Matterport3D dataset and real-world environments, including chair, table, sofa, potted plant, basket, exercise equipment, vacuum cleaner, suitcase, toy, and dog. 
We then utilize the object insertion module to produce a set of novel views as candidates, each with a different kind of object.
To reflect the inpainting quality, we employ the CLIP <cit.> model to assess the compatibility score s_c of each view-object pair.
Note that this assessment considers only the modified part of the view by combining the mask M_a^b:

    s_c = CLIP(ô_a^b ∘ M_a^b, obj)

The ∘ operator represents pixel-wise multiplication.
These scores are aggregated to form a dataset including all redundant edges within R2R. 
Visual analyses of this dataset indicate that the score distribution for each object category often takes the shape of a bimodal Gaussian distribution, with one peak 𝒩(s_c | μ_1, σ_1^2) for successful incorporation and another 𝒩(s_c | μ_2, σ_2^2) for failure:

    P(s_c) = π_1 𝒩(s_c | μ_1, σ_1^2) + π_2 𝒩(s_c | μ_2, σ_2^2)

π is the mixing coefficient, 𝒩(·| μ, σ^2) represents a Gaussian distribution with mean μ and variance σ.
Based on this insight, we train a bimodal Gaussian Mixture Model (GMM) <cit.> for each category and use them to decide which images are qualified: 

    q =  
    1    if 𝒩(s_c | μ_1, σ_1^2) > 𝒩(s_c | μ_2, σ_2^2) 
    
    0    otherwise

Finally, we randomly select one of those qualified candidates as the modified view, or, in the absence of suitable options, the choice is narrowed to the top three scorers.
This module not only largely enhances the probability of incorporating visible obstructions but also ensures the selection of contextually fitting objects.
We present the distributions of object categories and their compatibility scores in the appendix to prove the diversity of R2R-UNO.

All steps above only concern one discrete view o_a^b.
The final step is propagating the updated view ô_a^b to adjacent views o_a^j_1:8 containing overlapped areas with o_a^b to maintain consistency across the panorama.
We first calculate the transformation matrix K_i for view o_a^j_i based on SIFT features <cit.> matching.
We then apply K_i to project the inpainted part of ô_a^b onto the adjacent view and merge it with the original view to construct a coherent novel panorama:

    K_i = MATCH(SIFT(o_a^b), SIFT(o_a^j_i)) 
       ô_a^j_i = o_a^j_i· (1 - K_i * M_a^b) + K_i * ô_a^b · (K_i * M_a^b)

Here, * denotes the projection operation by pixel-wise matrix multiplication applied to image coordinates.

It is important to note that we only perform 2D inpainting for nodes linked by redundant edges, which may lead to multi-view inconsistency.
Although 3D object insertion can alleviate this problem, we stick to the 2D method for two reasons.
Firstly, current 3D techniques <cit.> mainly rely on limited pre-built object datasets <cit.>, which lack diversity and can cause visual inconsistencies in aspects like lighting and style. 
Other generative 3D methods <cit.> struggle to generate high-resolution photo-realistic images and often produce noticeable artifacts.
Secondly, the impact of multi-view inconsistency is minimal in our task, as instruction-reality mismatches are primarily defined by graph changes.  
Agents can only detect graph changes when moving to nodes alongside redundant edges, making visual modifications at other nodes irrelevant since agents should still follow instructions without detecting the mismatch.
Moreover, various environment augmentation methods <cit.> have proved that this inconsistency would not affect agent performance in real-world scenarios.



§ INSTRUCTION-REALITY MISMATCHES AND SOLUTION


In this section, we show that current VLN methods perform poorly when encountering instruction-reality mismatches and thus propose the ObVLN method to solve this problem.





 §.§ Current VLN Methods Struggle in R2R-UNO



To assess the impact of instruction-reality mismatches, we conduct zero-shot evaluations on the R2R-UNO validation unseen splits using five advanced VLN methods, known for their excellent performance under the perfect instruction assumption, including RecBERT <cit.>, HAMT <cit.>, DUET <cit.>, GELA <cit.> and ScaleVLN <cit.>.
<ref> illustrates how their success rates vary with different numbers of blocked edges.
While most methods achieve a 60%–70% success rate in original environments, the curves sharply decline to around 40% when a single edge is obstructed, with further deterioration observed in the Block-2 and Block-3 scenarios. 
ScaleVLN outperforms other methods in R2R-UNO owing to its strong generalization ability brought by extensive training data, but it still suffers from a nearly 20% performance reduction in the block-1 set of R2R-UNO.
We conclude that current VLN models are overly focused on instruction-following capabilities and lack essential basic navigation functionality to adapt to graph changes.
These findings emphasize the severity of using the perfect instruction assumption in VLN research and the urgency for its resolution.





 §.§ ObVLN

With R2R-UNO, agents can be trained in obstructed environments to better adapt to instruction-reality mismatches.
However, significant gaps exist between original and obstructed environments, such as navigation requirements, reliance on instructions, and trajectory numbers, making direct training on these two types of data compromise the performance in both environments (See <ref>).

Therefore, we propose Obstructed VLN (ObVLN), including a novel training strategy and a graph construction mechanism to organize the training and help agents deal with obstructions.
We advocate a curriculum learning strategy to treat original and obstructed environments as distinct yet complementary tasks to facilitate the training.
This strategy leverages the instruction-following skills gained in original environments as a foundation to tackle obstructed settings.
Initially, agents are trained in purely unobstructed environments.
As training progresses, the sample ratio of obstructed environments α gradually increases until it reaches a predefined maximum α_max at step c, defined as follows:

    α(t) = min(α_max, t/c·α_max)

where t is the current training step.
All training batches include instances from both settings to ensure smooth optimization.

To facilitate exploration for detours, we introduce a graph construction mechanism for graph-based methods to incorporate “virtual nodes” to represent currently inaccessible nodes due to obstructions.
This mechanism consists of three steps, with the pseudo-code in the appendix.
Firstly, when encountering an obstacle at node v_a, we estimate the location of the obstructed node v_b based on the direction of the obstructed view and a predefined distance d (3 meters in R2R-UNO).
This estimation allows us to conceptualize a virtual node v^*_b within the topological graph as a placeholder for v_b. 
Next, as the agent moves, it consistently computes its distances to v^*_b and v_a, denoted as D(v^*_b) and D(v_a), respectively. 
This distance computation is crucial for determining the proximity of the agent to the virtual and obstructed nodes.
Finally, if the distances satisfy D(v^*_b) < θ and D(v_a) < θ for a given threshold θ, and the current view indicates obstruction, we infer that the agent has arrived or moved close to v_b. 
So we connect the virtual node v^*_b and current node v_c with a zero-weight edge, indicating their alignment.
This mechanism helps agents to seek alternative routes more purposefully and increase exploration efficiency.



§ EXPERIMENTS




 §.§ Datasets and Evaluation Metrics

We mainly evaluate our methods on the widely used VLN benchmark R2R <cit.> with step-by-step instructions and our proposed R2R-UNO to focus on the challenge of instruction-reality mismatches. 
We present results on the goal-oriented benchmark REVERIE <cit.> without such mismatches in the appendix.
R2R is built on the Matterport3D dataset <cit.>, including 10,800 panoramic views from 90 building-scale scenes.
Each R2R path has three or four natural language instructions from human annotators.
R2R has four splits: train, validation seen (val seen), validation unseen (val unseen), and test unseen.
We utilize the train split for training and the val seen and val unseen splits for evaluation to align with R2R-UNO.

For evaluation, we follow previous works <cit.> to use four primary metrics in VLN: 
(1) Trajectory Length (TL): the total navigation length in meters; 
(2) Navigation Error (NE): the distance between the stop location and the target; 
(3) Success Rate (SR): the ratio of agents stopping within 3 meters of the target; 
(4) Success rate weighted by Path Length (SPL) <cit.>: SR normalized by the ratio between the length of the shortest path and the predicted path. 
Metrics related to instruction fidelity like CLS <cit.> or nDTW <cit.> are not included due to the modified trajectory with obstacles.



 §.§ Implementation Details


For R2R-UNO creation, we use the stable-diffusion-v1.5-inpainting model for object insertion and the CLIP ViT-L/14 to evaluate text-image pairs.
We use brute force and K-Nearest Neighbors matching to align the SIFT features of two adjacent views.
For methods in <ref>, we use their best models on the validation unseen split and extract the features of the obstructed environments according to their settings.
We employ HAMT <cit.> and DUET <cit.> for navigation training and follow the implementation details in their official repositories.
The obstructed environments are only used in the fine-tuning stage.
The maximum sample ratio α_max is set to 0.5 with the increasing step c as 20,000.
We increase the maximum action length to 30 for longer ground truth paths.
PREVALENT <cit.> is used as the augmented data to stabilize the training.
We utilize the AdamW optimizer <cit.> and select the best model based on the average SPL in the val unseen splits of R2R and R2R-UNO.
All models are fine-tuned for 100K iterations with a learning rate of 1e-5 and a batch size of 8 on a single NVIDIA A6000 GPU.



 §.§ Main Results

We first demonstrate that our proposed obstructed environments and ObVLN can facilitate agent adaptation to instruction-reality mismatches.
Therefore, we apply HAMT and DUET to three distinct training settings:
(1) Basic: training with R2R; 
(2) +OE: incorporating data from both the R2R and R2R-UNO;
(3) +ObVLN: using the ObVLN method on R2R and R2R-UNO.
Note that both settings 2 and 3 include all three sets of R2R-UNO.
<ref> presents the navigation performance of different models on the val seen and unseen splits of R2R and R2R-UNO.
The results show that models trained with two types of data significantly outperform those trained only on R2R in navigating obstructed scenes within the R2R-UNO dataset.
However, this improvement is accompanied by a noticeable degradation in R2R performance, like the 6% and 7% SR drop of DUET in the seen and unseen splits, respectively.
This phenomenon can be explained by the considerable TL increase in R2R, suggesting that agents are over-optimized for obstructed environments and tend to take detours even without obstructions.
Our ObVLN method effectively addresses this issue.
For HAMT, ObVLN surpasses the basic setting in R2R, achieving the best results across all four sets of R2R and R2R-UNO.
For DUET, the topological map and global action space lead agents to return to the node with the blocked edge which aligns best with the instruction.
This results in a slight loss of efficiency in terms of SPL in R2R for ObVLN.
However, it still achieves a comparable SR to the basic setting and significantly outperforms setting 2 in R2R.
Additionally, it achieves state-of-the-art results in all three sets of R2R-UNO.
The advantage of HAMT with ObVLN is less significant because HAMT is not a map-based approach, making our graph construction mechanism inapplicable.

While DUET outperforms HAMT by a 7% SR increment on R2R, this superiority becomes much more significant in obstructed environments, in which DUET achieves around 25% SR lead in the most challenging Block-3 set.
We attribute this to the topological map design in DUET, which significantly enhances exploration efficiency and serves as a critical component in finding detours. 
This advantage is consistent with the large lead of DUET on exploration-intensive, object-oriented datasets like REVERIE <cit.>.



 §.§ Ablation Study

In this section, we conduct an ablation study on several important components to show their effectiveness.







  §.§.§ Two Modules for R2R-UNO

We first evaluate the impact of the Object insertion Module (OM) and the Filtering Module (FM) by generating different Block-1 sets for training and evaluation.
Without these two modules, the obstructed environments only contain graph changes without visual modifications.
<ref> presents the performance of HAMT and DUET on the val unseen splits using different modules.
The R2R performance is consistent across all experiments due to ObVLN. 
For obstructed environments, using the object insertion module to modify the panoramic views can enhance agent navigation by providing critical visual feedback on graph changes, evidenced by performance gains.
Moreover, including the filtering module further improves the inpainting quality, leading to more successful obstruction generations and the best performance.



  §.§.§ Sampling Strategy

We explore four sampling strategies for DUET to utilize both original and obstructed environments:
(1) Path-wise, where a path is randomly selected from the combined pool of R2R and R2R-UNO paths;
(2) Task-wise, where data is sampled from original and obstructed environments in a Bernoulli mixture distribution with probabilities 1-α_t and α_t, respectively;
(3) Instruction-wise, where for each instruction, one path is randomly chosen from all possible paths.
(4) Curriculum sample (Ours), which gradually increases the sampling ratio α up to α_max by step c.
<ref> shows their performances on the the val unseen splits of R2R and R2R-UNO.
We set α_t=0.5 to match α_max and provide experiments on different α_t and α_max in the appendix.
Among all strategies, our curriculum sampling achieves dual superiority, achieving the best performance in both R2R and R2R-UNO.
Other methods experience performance drops on R2R due to the over-optimization problem.
Path-wise sampling obtains the worst performance due to path imbalance, with more training on paths with many redundant edges while ignoring those with none or less.







  §.§.§ Graph Construction Mechanism

We compare our graph construction mechanism with two baselines: the Vanilla setting that overlooks the obstruction in the graph and an idealized Oracle setting that receives the ground truth information about nodes occluded by obstructions as a performance ceiling.
<ref> shows the performance of DUET with different graph construction methods on the val unseen splits of R2R-UNO.
Our approach, incorporating virtual nodes into the graph, significantly outperforms the vanilla setting on all three sets, especially in more challenging scenarios with three obstructed edges.
As expected, the Oracle graph achieves the highest performance due to its access to accurate, unobstructed node information. 
These findings emphasize the necessity of equipping agents with obstruction-aware capabilities and the benefits of integrating obstructions into the topological map.




 §.§ Qualitative Analysis

We present some obstructed environments from R2R-UNO in <ref> comparing with corresponding original views and those generated without the filtering module. 
Our method successfully integrates various objects into specific locations within the original views, creating realistic and contextually harmonious obstructions. 
In contrast, results without the filtering module often fail to include the objects, demonstrating the critical role of this module in enhancing the inpainting reliability.
Additionally, we evaluate the generation quality using the CLIP <cit.> score and present the scores for each category in <ref>.
The filtering module consistently improves and stabilizes the generation quality, achieving higher scores and lower variance across all categories,  aligned with the examples.



§ CONCLUSION

This work introduces obstructed environments into VLN to address the prevalent issue of instruction-reality mismatches in real-world navigation. 
We present R2R-UNO, the first VLN dataset to incorporate such mismatches by integrating diverse obstructions into R2R at both the graph and visual levels through a novel object insertion and filtering module.
Using R2R-UNO, we demonstrate that current VLN methods struggle in obstructed settings and further propose the ObVLN method to help agents effectively adapt to obstructed environments.
Through these innovations, our agents achieve state-of-the-art results in R2R-UNO while maintaining robust performance in R2R.
We believe addressing the perfect instruction assumption is crucial for the practical application of VLN agents and for assessing their adaptability to navigate beyond only following instructions.







This work is supported by projects DE200101610, DP230101753 funded by Australian Research Council, and CSIRO's Science Leader project R-91559.





ACM-Reference-Format






































 Paper Title: In-Hand Singulation and Scooping Manipulation with a 5 DOF Tactile Gripper.





Aligning Multiple Knowledge Graphs in 
 a Single Pass
    Yaming Yang,
        Zhe Wang,
        Ziyu Guan^** Corresponding author,
        Wei Zhao,
        Weigang Lu,
        Xinyan Huang

Y. Yang, Z. Wang, Z. Guan, W. Zhao, and W. Lu are with the State Key Laboratory of Integrated Services Networks, School of Computer Science and Technology, Xidian University, Xi'an, China 710071.
E-mail: {yym@, zwang@stu., zyguan@, ywzhao@mail., wglu@stu.,}xidian.edu.cn
X. Huang is the Key Laboratory of Intelligent Perception and Image Understanding of Ministry of Education, the School of Artificial Intelligence, Xidian University, Xi'an, China 710071.
E-mail: xinyanh@stu.xidian.edu.cn



    August 5, 2024
=======================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================



§ ABSTRACT 

Manipulation tasks often require a high degree of dexterity, typically necessitating grippers with multiple degrees of freedom (DoF). While a robotic hand equipped with multiple fingers can execute precise and intricate manipulation tasks, the inherent redundancy stemming from its extensive DoF often adds unnecessary complexity. In this paper, we introduce the design of a tactile sensor-equipped gripper with two fingers and five DoF. We present a novel design integrating a GelSight tactile sensor, enhancing sensing capabilities and enabling finer control during specific manipulation tasks. To evaluate the gripper's performance, we conduct experiments involving two challenging tasks: 1) retrieving, singularizing, and classification of various objects embedded in granular media, and 2) executing scooping manipulations of credit cards in confined environments to achieve precise insertion. Our results demonstrate the efficiency of the proposed approach, with a high success rate for singulation and classification tasks, particularly for spherical objects at high as 94.3%, and a 100% success rate for scooping and inserting credit cards.










§ INTRODUCTION

Dexterous in-hand manipulation stands as a crucial capability for robots tasked with executing precise actions within confined workspaces. Recent advancements in research have showcased the capabilities of robotic hands to emulate precise human-like motions <cit.>, by leveraging recent strides in visual-tactile sensors, such as GelSight  <cit.>, robots are empowered to perceive object shapes and contact forces during grasping, thereby facilitating autonomous and secure object manipulation. Despite the versatility exhibited by robotic hands with high degrees of freedom, several challenges remain unsolved in the field.

First, methods employing complex finger movements in high DoF often rely on intricate control strategies. Classic model-based approaches necessitate meticulous model and controller designs, often lacking generalization capabilities. Conversely, recent learning-based methods encounter difficulties due to the high dimensionality of dexterous robotic hands, leading to inefficient exploration and the need to tackle the sim-to-real problem. In structured environments where object shapes remain unchanged, a gripper with a simple DoF may suffice to accomplish certain tasks.

Secondly, grappling with small-sized objects in a cluttered environment presents challenges, as multiple fingers may frequently collide with each other, hindering the robot's ability to maintain a balanced grasp on the object. This complexity is further amplified when tasks involve separating objects from cluttered environments, as planning and control issues may arise with the use of multi-finger grippers <cit.>.

Between the common one DoF parallel gripper, which can only accomplish simple grasping tasks, and the multi-finger robotic hands like the 16-DoF Allegro hand, the potential of in-hand manipulation using a fewer DoF gripper remains less explored. In this study, we introduce a novel gripper design based on visual-tactile sensors (Fig. <ref>), which enables scooping, in-hand manipulation, precise insertion, and classification tasks using only five degrees of freedom.




The contributions of this study are summarized as follows:



  * We introduce a novel two-finger vision-based tactile gripper with five degrees of freedom. This innovative design facilitates both rolling and linear manipulation, thereby enabling in-hand manipulation tasks.

  * We present a model-based approach that utilizes tactile information to accomplish grasping and singulation of various objects with different properties buried beneath granular media solely through touch sensing.

  * We devise a scooping manipulation maneuver for our gripper capable of handling thin objects, and extending its capabilities to perform precise insertion tasks.






§ RELATED WORKS




 §.§ Singulation with Granular Media


The process of selecting a single object from multiple items within a cluttered environment is commonly referred to as the singulation problem. Previous studies have proposed numerous solutions primarily through vision-based methods <cit.>; however, these approaches cannot be applied to handling small objects buried within granular media. In terms of object identification within granular media, some studies have focused on designing novel tactile sensors. For instance, Patel et al. utilized mechanical vibration to fluidize granular media, allowing for deeper penetration to identify different objects buried, using a GelSight sensor <cit.>. Syrymova et al. proposed a vibration-based tactile sensor for detecting foreign objects <cit.>. Addressing tactile exploration within granular materials, certain studies have utilized multi-modal sensory data, such as using BioTac to estimate contact states between robotic fingertips and objects within granular media <cit.>. More recently, researchers have proposed tactile exploration policies aimed at localizing, retrieving, and grasping buried objects using an end-to-end approach with a one DoF parallel gripper <cit.>. However, existing methods and gripper designs lack the capability to simultaneously grasp objects and complete singulation in a generalized manner. Specifically, none of the previous studies addressed the challenge of grasping objects while removing granular media impurities adhered to their surfaces.



 §.§ Scooping Manipulation


The grasping or picking of thin objects is a another active research area in robotic manipulation. Particularly challenging is the task of using a two-finger gripper, akin to human capabilities, to pick up plastic cards positioned on a flat supporting surface within constrained environments. Some gripper designs and strategies have been proposed to tackle this problem. For instance, He et al. introduced a two-DoF parallel gripper with a variable-length finger, achieving successful grasping through the finger's extending/retracting motion <cit.>. Other studies have adopted similar gripper designs featuring sharp finger tips, demonstrating the effectiveness of a scooping grasping approach <cit.>. Additionally, some two-DoF gripper designs incorporate underactuated and compliant mechanism <cit.>. However, due to finger geometry and contact constraints, many previous methods necessitate the motion of the robot arm to assist in the grasping maneuver. In environments where robot joints must be confined within a limited workspace, in-hand manipulation becomes crucial while the robot's end-effector remains fixed. Moreover, integrating a tactile sensor to perform dexterous tasks presents another challenge, particularly regarding the rounded and flat sensor surface lacking a sharp edge for scooping manipulation <cit.>.



§ METHOD



 §.§ Gripper Design

Prior studies have shown the efficacy of length-adjustable/asymmetric fingers, in tasks such as bin picking <cit.>, achieved through hand twisting motions to adjust their posture <cit.>. In contrast to other two-finger multi-DoF grippers equipped with vision-based or optical tactile sensors like <cit.>, we aim to achieve greater dexterity in in-hand manipulation by utilizing linear degrees of freedom.



The proposed gripper comprises two fingers, each consists of an Actunonix L12-R linear actuator and a Wishiot RDS3235 rotation servo, as depicted in Fig. <ref>. The linear actuator offers a travel range of 30 mm, while the working range of each rotation motor during finger contact is ± 45°. These fingers are affixed to the parallel Weiss WSG 32 gripper, thereby providing a total of five degrees of freedom. The gripper's arms are fabricated using PLA material via 3D printing, ensuring lightweight construction and reducing the overall load. Our preliminary research on robotic cable manipulation <cit.> and cloth manipulation <cit.> shows that one tactile sensor on the gripper is capable of performing complex manipulation tasks. Therefore, only one GelSight Mini tactile sensor is mounted at the fingertip of the left finger. Additionally, a replaceable flat soft dummy pad is utilized on the other finger to offer compliance, enabling adaptability in handling small objects with soft fingertips.



 §.§ Singulation: Stripping Object from Granular Media

When humans pick up small shells from the beach, the act of grasping is often followed by a series of delicate in-hand manipulations, involving the fingertips rubbing to remove sand particles that were initially adhered between the object's surface and the fingers. In this section, we outline the approach to address the challenging task of singulation, which involves stripping objects from granular media, mimicking human-like manipulation using the proposed gripper.

Stable grasping using tactile sensor: the first objective of our gripper is to be able to grasp objects within granular material, perform singulation, and subsequently conduct classification to identify the class of the objects. In a tactile-only system, identifying the object becomes extremely challenging if residual granular media remains between the sensor pad surface and the object. During the dynamic rubbing motion of the fingertips, it is essential to ensure adaptability and robustness in grasping. Being too loose may result in dropping the object, while being too tight may lead to singulation failure. To overcome this challenge, we employ the tactile-reactive Model Predictive Control (MPC) controller described in our previous work <cit.>.

Drawing upon the kinematic model and gel properties for a two-finger gripper with one degree of freedom in linear motion, the MPC model based on tactile feedback for grasping, can be formulated as:

    [[ c_n+1; p_n+1; v_n+1 ]]=[[        1        0 -K_c Δ t;        0        1      Δ t;        0        0        1 ]][[ c_n; p_n; v_n ]]+[[        0; 1/2Δ t^2;      Δ t ]] a_n

where scalars c, p, v, a refer to the contact area of grasped object, the position, velocity, and the acceleration of the gripper motion. K_c is a scalar factor in the linear assumption of the in the correlation between the contact area size c and the gripping position p.

For the cost function, we refined the original method <cit.> in our task to define the feedback vector exclusively based on the contact area from the tactile sensor. This simplifies the problem, considering that the linear and rotational motion of the gripper is confined within a 2D plane and constrained by the gripper's design degrees of freedom settings.

Denote the prediction length as N, the cost function can be formulated as follows:

    J(𝐲_n, 𝐚_n)=P 𝐞_n+N^T 𝐐𝐞_n+N+∑_k=n^n+N-1𝐞_k^T 𝐐𝐞_k+Q_a a_k^2

where

    𝐐    =[[ Q_c   0;   0 Q_v ]],



    𝐞_n     =𝐲_n-[c_desired, 0]^T, 
    𝐚_n     =[a_n, a_n+1, …, a_n+N-1]^T ∈ℝ^N .


In Equation <ref>, P is a scalar used to amplify the terminal cost, thereby accelerating convergence to move the gripper until it reaches the desired state, where the c_n converges to a desired value c_desired and  v_n converges to zero. The control error, as defined in Equation <ref>, incorporates weight coefficients Q_c, Q_v, and Q_a. Finally, an optimization problem can be formulated to compute the control inputs for the WSG gripper, enabling stable grasping during dynamic motions:

    𝐚_n^*=min J(𝐲_n, 𝐚_n),

subject to Equation <ref> and the upper and the lower limits of p_n, v_n, and  a_n. The parameters for tactile-MPC on GelSight Mini are shown in Table <ref>.


Rubbing using linear actuators: another challenge is enhancing the adaptability of the rubbing motion for objects of different dimensions, requiring varying motion ranges of linear actuators. We propose a linear state feedback controller to regulate this range. After stabilizing the WSG gripper position p_stable post-grasping, the linear actuator's movement range L is linearly correlated with the gripping width: L = k_p · p_stable + b, where k_p and b are tuned parameters. For objects smaller than 15 mm, the rotation servo retracts inward by a fixed angle to prevent them from falling.



 §.§ Scooping Manipulation and Precise Insertion

This section presents the methodologies employed to enable the proposed gripper to grasp flat objects and complete precise insertion, exemplified by a credit card.

Scooping manipulation: given the necessity of a sharp fingertip for grasping flat objects, we have adapted the design concept in <cit.> by employing a replaceable fingernail made of PLA. Leveraging the five degrees of freedom, we devised a simplified and adaptable maneuver suitable for objects of varying thicknesses. Fig. <ref> depicts the schematic of the grasping maneuver. From the free-body diagram when the card has been lifted with only three contact points with the ground the two fingers, the equivalent moment of external forces at the center of mass, M_all, can be expressed as:


    M_all   = 1/2[-(h-2d)F_R,x+lF_R,y] 
          + 1/2[hF_B,x-lF_B,y] + 1/2[(ltanθ-h)F_Lcosθ]

where F_R,x and F_R,y denote the pressure force by the fingernail and the friction force by the ground. And F_B,x and F_B,y represent the friction force and the support force from the table. At the contact point of the left finger, the card experiences the positive force F_L. Additionally, μ_1 and μ_2 are the coefficients of friction for the two contacting surfaces and the object is subject to its gravitational force mg.

For the force analysis, we select a cross-section and make the following assumptions to simplify the model: within this cross-section, the contact between the finger nail and the finger is approximated as a point contact. Furthermore, we assume that the contact point on the object receives normal pressure from the vertical edges of the object and static frictional force along the direction of the object's surface.

    F_R,y   = μ_1 · F_R,x, 
    
    F_B,x   = μ_2 · F_B,y, 
    
    F_B,x   = F_R,x - F_L·cosθ , 
    
    F_B,y   = F_L·sinθ - F_R,y + mg.


    F_R,x(1 + μ_1 μ_2) = F_L(μ_2 sinθ + cosθ) + μ_2 mg


    M_all   = 1/2 F_R,x[ (2d - h + lμ_1) ] + 1/2 F_B,y[ h μ_2 - l ] 
          + 1/2 F_L[ (l tanθ - h) cosθ] 
    
    
    
       = 1/2 F_R,x[ 2d - h + 2lμ_1 - μ_1μ_2h ] 
          + 1/2[ 1/μ_2 sinθ + cosθ( F_R,x(1 + μ_1 μ_2) - μ_2 mg ) ] 
         ·[ sinθ (h μ_2 - l) +cosθ( ltanθ - h) ] 
       = K_1 F_R,x + K_2.


By considering the relationship between static friction and normal pressure, along with the force balance of the object, we derive Equation <ref>, establishing the relationship between F_R,x and F_L as in Equation <ref>. Solving these equations yields Equation <ref>, where the friction coefficient, contact point height, length, thickness, and angle between F_L and the ground are constant. To simplify, we introduce positive coefficients K_1 and K_2. Based on the direction of force F_R,x, the equivalent torque acts in the counterclockwise direction. According to our assumption, the direction of the equivalent torque remains constant throughout this process. Consequently, the object undergoes a counterclockwise flipping motion until the flipping action is complete.



Precise grasping using tactile sensor: after successfully grasping the card and securing it between the two fingers, we further exploit the advantages of tactile sensors to showcase the versatility of our gripper through an insertion task. This task involves two objectives: 1) using in-hand manipulation to adjust the orientation of the credit card to ensure that the chip is facing downward, and 2) achieving a precise grasping position (x_d, y_d). We formulate this problem as a model-based task, leveraging tactile information to estimate the geometric properties of the card.

Our action primitives consist of a linear pushing action by the manipulator and the grasping motion by the parallel DoF. The tactile-based exploration begins once the sensor establishes stable contact. Initially, with the long edge of the credit card in contact with the tabletop, our strategy for card manipulation involves sequentially moving the gripper in a 2D plane based on the numbers printed on the card from previous steps, and then release and adjusting the gripper's next grasp to align with the edge of the credit card corresponding to the digits. We set the exploration step along the long and short edge (or pushing action change) to be s_x, s_y ∈{-2  mm,-4  mm,-8  mm}, depending on the current edge of the number on the card, as illustrated in Fig. 6. During the motion, the gripper is always perpendicular to the table. When the tactile image indicates that the last digit has reached a threshold position on the long edge of the credit card x_d, the robot arm ceases to push forward. To determine y_d on the short edge, we first employ in-hand manipulation to rotate the card by adjusting the width of the parallel DoF. Given that the card is grasped at the far right end of the long edge, rotating around this point ensures that the card flips by the gravity, allowing the short edge of the credit card is in full contact with the tabletop. Subsequently, by iteratively performing the tactile exploration action, the gripper can eventually reach the desired gripping pose.





§ EXPERIMENT



 §.§ Singulation and Classification

We employed a UR5e robot equipped with the proposed 5 DoF gripper. For this task, we assume that the initial position of all objects beneath the granular media remains constant, allowing the robot gripper to consistently grasp the object successfully. In the experimental setup, we selected rice as the granular media and placed it inside a plastic container measuring 135 × 112 × 55  mm. The depth of rice filled in the container is approximately 30  mm, as shown in Fig. <ref> (a). We selected a total of 10 objects with varying sizes, surface textures, geometric shapes, and physical properties, shown in Fig. <ref>. These objects include small items (with diameters around 10 mm), two elliptical nuts, a deformable soft foam sphere, three 3D-printed rigid balls with PLA material featuring different surface textures, and two large objects, one golf ball with a diameter of 41 mm and an artificial strawberry with an irregular geometric shape.

Fig. <ref>(b) and (c) depict sequential images and corresponding tactile images, including raw images and binary contact area images generated by thresholding the depth image for high precision. After the gripper is inserted into the rice, the tactile-MPC controller is engaged, enabling stable grasping by dynamically adjusting the grip width.Subsequently, in-hand manipulation is performed according to the proposed method to remove the rice trapped. As depicted in Fig. <ref>(c) corresponding to state 2, due to residual material, the tactile image only shows the shape of rice grains, making classification processing difficult solely relying on tactile feedback. However, once the singulation manipulation is completed, state 6 clearly depicts the features of the grasped peanut. 

The results of the singulation experiment (15 trials each) are presented in Table <ref>. The success rate is defined as the object remaining stable during the singulation process. For all objects combined, the overall success rate was 114 out of 150 attempts (76%). Specifically, for different sphere-like objects, including balls and seeds, the success rate was notably high at 99 out of 105 attempts (94.3%).


Despite most granular media being removed during the rubbing motion, minor residual may still remain. To further evaluate singulation performance, we conducted a classification of successful attempts in the first step using <cit.>, with results displayed in the confusion matrix in Fig. <ref>. The left image annotates each object's respective labels. The results indicate high classification accuracy for most objects, suggesting successful removal of attached granular media and confirming the efficacy of our method.



 §.§ Credit Card Insertion

We make the following assumptions to simplify the problem: 1) the pose of the card reader is predetermined, and 2) the initial position of the credit card is within a specified range beneath the gripper, ensuring that the gripper can always descend to grasp it. Fig. <ref>(a) shows the task setting. A 3D-printed credit card, measuring 85.5 × 54 × 0.8 mm (or 1.2 mm thick if considering raised numbers), was utilized. Additionally, a card reader for insertion was printed, featuring a hole with dimensions of 56 × 1.5 mm.





Fig. <ref>(b) and (c) depict the critical states and tactile feedback during the experiment. We directly employ the raw difference image and apply Canny detection followed by a series of morphological operations to extract the contact edge based on the numerical information for faster processing. In the initial scooping stage, the fingers' edges are parallel, leaving ample space between them to accommodate different card postures. This initial grasping step constrains the card's position, ensuring its stability relative to the fingers before each scooping action begins. During the scooping process, the GelSight fingers remain stationary, preventing the card from sliding and causing task failure, thus avoiding the complicated process of determining the appropriate angle. Upon completion of the flip, the tactile information of the card is transmitted to the GelSight. This denotes the credit card's orientation based on its numbers, distinguishing between its front and back sides. In the absence of tactile sensor feedback, the gripper executes an in-hand reorientation to flip the card, ensuring that the side with the numbers contacts the sensor. During exploration along the x-direction of the card, the robot arm continues to grasp and release until the upper contact edge (denoted by the green line) reaches x_d, as indicated in Fig. <ref>(c) from state 3 to state 4. Similarly, following the in-hand reorientation that adjusts the card to a vertical position, the robot arm proceeds with exploration along the y-direction until the desired pose y_d is achieved. Finally, the gripper is lifted to complete the insertion. Our method achieved a 100% success rate for the complete task including scooping and insertion, in 10 consecutive trials with the credit card placed in random initial pose.





§ DISCUSSION AND CONCLUSION

Several issues observed in the experimental results merit discussion. In the singulation task, the success rate for non-spherical objects was notably lower. This disparity can be attributed to the variability in width during rotation, which is less stable compared to spherical objects. Sudden changes in width may lead to objects being dropped or squeezed out, and the use of a flat sensor pad exacerbates this effect. Developing a policy using a learning-based approach may offer a potential solution for improving performance in handling these objects, a direction we defer to future work.

In this paper, we introduce a five degrees of freedom tactile gripper capable of performing dexterous in-hand manipulation tasks. We evaluated the performance of our gripper and control methods through two dexterous tasks: singulation manipulation of 10 various objects, and scooping and precision insertion of a credit card. The results underscored the efficacy of the gripper design and control strategies proposed. Overall, we achieved a 76% success rate for the rubbing motion, with a success rate exceeding 94% for sphere-shaped objects. Furthermore, the scooping manipulation exhibited a 100% success rate. In future endeavors, we aim to expand our approach to tackle more intricate manipulation tasks.



§ ACKNOWLEDGMENT

The authors would like to thank Zhengtong Xu for the invaluable discussions on the MPC controller and Guanlan Zhang for developing the early gripper prototype.


IEEEtran




 Paper Title: High-Quality, ROS Compatible Video Encoding and Decoding for High-Definition Datasets.





MUFASA: Multi-View Fusion and Adaptation Network with Spatial Awareness for Radar Object Detection
    Xiangyuan Peng1,2 Miao Tang3 Huawei Sun1,2Kay Bierzynski2Lorenzo Servadei1 Robert Wille1

    August 5, 2024
==================================================================================================

empty
empty



§ ABSTRACT 
Robotic datasets are important for scientific benchmarking and developing algorithms, for example for Simultaneous Localization and Mapping (SLAM). Modern robotic datasets feature video data of high resolution and high framerates. Storing and sharing those datasets becomes thus very costly, especially if more than one camera is used for the datasets. It is thus essential to store this video data in a compressed format. This paper investigates the use of modern video encoders for robotic datasets. We provide a software that can replay mp4 videos within ROS 1 and ROS 2 frameworks, supporting the synchronized playback in simulated time. Furthermore, the paper evaluates different encoders and their settings to find optimal configurations in terms of resulting size, quality and encoding time. Through this work we show that it is possible to store and share even highest quality video datasets within reasonable storage constraints. 






§ INTRODUCTION


Datasets are an important tool for research and development in mobile robotics. For example, for evaluating Simultaneous Localization and Mapping (SLAM) algorithms <cit.>, datasets play an important role <cit.>. Sensor technology is advancing rapidly, such that better camera sensors with higher resolutions and higher frame rates are available for use in robotic systems. While older datasets like KITTI from 2013 <cit.> use 1.4MP cameras with a frame rate of 10Hz, later datasets offer for example 5MP images with 10 Hz <cit.>. 

Our most recent mapping robot for collecting datasets features five pairs of stereo cameras, facing front, left, right, back and up, each with 5MP <cit.>, and a 16 node "Cluster on Wheels" <cit.> to collect this data with 60Hz. Additionally, that robot features other vision sensors (e.g. a 6-lens Ladybug 5+, stereo infrared, RGB-D, event camers) as well as 7 LiDARs. The datasets collected with this robot are valuable tools to evaluate the performance of SLAM algorithms. 

It is especially interesting to use this high-end data to then simulate lower quality data by resizing the images to get lower resolutions or dropping frames to simulate lower frame rates. This way, one can investigate in detail the effects of resolution and frame rate vs. accuracy, CPU and memory consumption. We have developed the SLAM Hive system <cit.> to do such benchmarking automatically in the cloud and provide an easy to use web interface for analysis of the data[SLAM Hive Demo: <https://slam-hive.net/>]. 

As discussed, high resolution and high-quality datasets are valuable for evaluating SLAM. The problem is with this data is, that it is really big. As can be seen in Table <ref>, the 11 5MP cameras with just 30Hz frame rate of the initial ShanghaiTech Mapping Robot Outdoor Dataset <cit.> has a raw data size of more than 2TB, the RGB size would be more than 7TB and even not-so-great quality 90 JPG would still be 446GB that would needed to be shared with the community. This is intractable in terms of transmission time over the internet, but also in terms of storage space for most computers. Therefore, the goal of this paper is to investigate how better compression rates can be achieved, while still maintaining excellent image quality, reasonable encoding times and small size. In order for the compressed dataset to be useful as a reference dataset for SLAM research and benchmarking, a truly excellent video quality has to be maintained. As reported in <cit.>, compression artefacts can in fact reduce the applicability of video streams to various applications such as object detection. Specifically, we do not need to encode the videos in real-time, but will do the compression in post-processing, because there are not enough computation resources even on a 16-node cluster to do the de-bayering and encoding in high-quality in real time. In the end our experiments will justify our choice of encoder and settings, which enables us to share the above mentioned video dataset with a size of just 43GB.

The contributions of this paper are:

	
  * Investigation of different high-quality compression/ encoding options for video data;
	
  * Experimental evaluations of the options based on a high-resolution, high-frame rate test video;
	
  * Development and open-sourcing [<https://github.com/STAR-Center/ros_mp4>] of a python tool to replay MP4 videos with yaml metadata, including support for synchronized replay with simulated time;
	
  * Evaluation of source frame rate effects on encoding.


The rest of the paper is organized as follows: Section <ref> introduces the background and related work. Section <ref> introduces our software framework for encoding and decoding the videos within the ROS 1 and ROS 2 middle-ware. Extensive evaluations regarding encoding algorithms and their settings vs. quality, encoding time and video size and further experiments are performed in Section <ref>  and conclusions are drawn in Section <ref>.




§ BACKGROUND






When collecting video datasets, several options for storage come to mind. In this paper we only consider color video data, as this is the most prevalent and information rich video format. High-quality computer vision cameras, such as the Grasshopper3 (GS3-U3-51S5C-C), provide raw data, that is bayered <cit.>. This means, that there are four channels of red, green (2x) and blue gray-scale values provided, typically each using one byte. After de-bayering, the image size is tripled, since every pixel now gets an RGB or YUV444 color value, interpolated over the surrounding, colored pixels, each using 3 bytes. Given the 5,013,504 pixels (2048x2448) of the 5MP camera and a 60Hz update rate, this results in 902MB per second or 3.3 TB per hour - for one camera stream. Thus compression is needed. One example frame is shown in Fig. <ref>.

Traditionally, robotics is concerned with real-time video compression for teleoperation <cit.>, robot-to-robot communication <cit.> or transmission of range data <cit.>. But the aim for this paper are datasets with no visible degradation in image quality. Lossless image compression approaches like PNG do not help much with real camera data, since the camera noise results in most pixels having different values even if they look similar. The next best option is to use JPEG. In Table <ref> we show that even very low quality jpeg values such as 75 result big file sizes. Thus real video encoders promise better video quality and smaller size.







 §.§ ROS and Video


Robotic datasets often use ROS, specifically ROS bags to store the data. Video data in ROS 1 is stored in image messages, one for each frame. The sensor_msgs/Image message type provides different encoding options, such as bayer_GBRG8, rgba8, yuv422 and OpenCV types  [<http://docs.ros.org/en/indigo/api/sensor_msgs/html/image__encodings_8h_source.html>]. The type sensor_msgs/CompressedImage, on the other hand, supports PNG, JPG and h264. ROS 2 also supports newer codecs, such as h265 [<https://github.com/ros-misc-utilities/ffmpeg_image_transport>]. 

An important feature of ROS is the time-synchronized playback with simulated time. For that ROS will take the time stamps of one bagfile and publish that time on the topic /clock. Other rosbag play instances will listen to that simulated time, if the parameter use_sim_time is set to true beforehand, and play their messages at the according, simulated times, thus achiving a synchronized playback of multiple bagfiles. 

In this project we decided to encode the video data of the dataset as mp4 files. This has several advantages: 1) Users can inspect the data very easily, by just playing them in their favorite video player (e.g. VLC). 2) We can use the newest encoders and are not held back by compatibility issues regarding provided encoders of the compressed image format. 3) Our playback tools work with ROS 1 and with ROS 2, so it is very easy to use the video dataset with either software.

To support the functionality similar to rosbag play, our software uses metadata, specifially the time stamps of the image frames, stored in an external yaml file. Using this we can replay the videos in ROS 1 and ROS 2 just as if they came from a rosbag, including support for time-synchronize playback via simulated time.

Next the different options for video codecs are presented.
 


  §.§.§ H.264/AVC


In 2004 the codec H.264/AVC was introduced <cit.>. It has numerous innovative features such as multi-reference frame motion compensation, variable block size motion compensation, and intra-frame prediction encoding.






  §.§.§ H.265/HEVC

The H.265/HEVC  codec was introduced in 2013 <cit.>. It further optimizes predictive coding techniques based on the inheritance of H.264/AVC, including more refined intra-frame and inter-frame prediction<cit.>, larger coding units<cit.>, and higher precision motion vectors<cit.>, significantly improving compression efficiency. 






  




  §.§.§ AV1

Following H.264/AVC and H.265/HEVC, the AV1<cit.> encoding standard was introduced in 2018. It features high compression efficiency and is open-source. AV1 upgrades the granularity of intra-frame directional prediction and boasts a more powerful inter-frame encoder<cit.>, significantly enhancing the accuracy of compound prediction.





 §.§ FFmpeg

FFmpeg<cit.> is an open-source multimedia framework supporting  numerous codecs, including H.264, H.265, and AV1.



Among the key parameters in FFmpeg's command line, -CRF (Constant Rate Factor) and -PRESET are crucial for controlling compression quality. CRF is a "constant quality" encoding mode that dynamically adjusts quantization parameters (QP) across frames to maintain a consistent visual quality. Users can adjust the quality of the output video by specifying a CRF value between 0 and 51, where a lower value corresponds to higher video quality and a larger file size.The -PRESET parameter balances encoding speed and compression efficiency, offering options ranging from "ultrafast" to "placebo", allowing users to choose the appropriate encoding speed based on specific application scenarios and hardware performance.




 §.§ Image Quality Measurement



To ensure the visual quality of videos, conducting detailed subjective and objective evaluations of video quality is crucial. VMAF (Video Multi-Method Assessment Fusion) is a  video quality assessment tool jointly developed by Netflix and the academic community <cit.>. 

VMAF measures video quality through integrating multiple visual quality indicators, generating objective scores that highly align with human visual perception. Based on the nuSvr algorithm of SVM, VMAF first extracts key visual features in the evaluation process, including but not limited to Visual Information Fidelity (VIF), Detail Loss Metric (DLM), and Temporal Information (TI), comprehensively considering the spatial and temporal dimensions of the video. Then, VMAF utilizes a pre-trained SVM model to combine these features with human subjective scores, assigning different weights to each video feature. A score is generated for each frame, and the final score of the video is calculated using the mean of those values. 

Table <ref> shows details of an I-frame of our test video encoded with different parameters, thus resulting in different VMAF scores. There are sill some differences in the frames visible for VMAF scores below 99.50. But for scores higher than that no difference can be made out with the naked eye. Therefore we chose 99.50 as our target VMAF score for highest-quality video encoding. 



§ ROS MP4 ENCODING AND PLAYBACK FRAMEWORK





In this section, we will delve into the detailed steps of compressing rosbag and playback. The camera image messages recorded in rosbag are essentially a series of consecutive image frames with timestamps and other metadata, which is quite similar to the characteristics of video streams. 

The first step is to separate the image frames from their associated timestamps and other metadata. The camera images recorded in rosbag may not be directly stored in common BGR formats, but may use formats like "bayer_gbrg8" or may have already undergone some form of compression during recording. In such cases, necessary format conversion and preprocessing may be required. Once the image frames are extracted, we can utilize  FFmpeg to compress these image frames into the desired video format.


 §.§ Yaml File

Apart from image frames, other metadata (such as timestamps, frame IDs, intrinsic parameters, etc.) also need to be properly preserved to ensure that we can fully restore all the original information from the rosbag messages. To achieve this, we use the YAML file format to store these metadata.  For typical visual rosbag datasets, there are two key topics: "image_raw" (or a similar name) for storing raw image data, and "camera_info" containing camera calibration and distortion information related to each frame. After analyzing the information structure of these two topics, we find that the information in the "camera_info" topic actually contains some key information corresponding to the image frames in the "image_raw" topic, such as frame_id and timestamp. Therefore, we define the structure of the YAML file as follows:








































 §.§ Play Back

To play back this compressed dataset composed of YAML file and video, we have developed a powerful script. This script not only integrates and publishes topic information corresponding to the original rosbag dataset, but also retains many of the same features as playing a rosbag. By parsing the timestamp information in the YAML file and utilizing ROS's clock mechanism, our script can accurately simulate the playback time of the dataset in a real-world environment, ensuring that the content of the published topics remains consistent with the original rosbag playback in terms of both timing and content. This high level of accuracy provides users with an experience almost identical to directly playing a rosbag. Fig. <ref> shows the different playback options available. 

















§ ENCODING EXPERIMENTS





To determine the optimal compression combination for video quality and compression efficiency for the new dataset structure, we carefully selected a 1-minute clip from the recorded dataset that contains both static and dynamic scenes as a test sample. This clip combines a 5-second static outdoor scene and a 55-second dynamic outdoor scene, which can represent the typical characteristics of the dataset. The video was recorded on the ShaghaiTech Mapping Robot with a Grasshopper3 (GS3-U3-51S5C-C) camera with 2048x2448 pixel at 60Hz frame rate. 



For evaluation we utilize the 60 second test video, but then extrapolate the results to a dataset of 14 such video streams each 2 hours long, as this is the expected dimension of our future ShanghaiTech Mapping Robot Datasets. The robot features 11 such datastreams at full frame rate plus the LadyBug5+ camera with 6 streams at half the frame rate, so in total around 11 + 6/2 = 14. The results help us to understand the computation and storage demands of a dataset compressed with the according settings.









To accurately evaluate video quality, we first converted the selected rosbag clip to YUV444 format, a format commonly used in video editing due to its ability to preserve the original quality of the video well. We used the YUV444 format video as the testing ground truth to accurately measure the quality of the compressed video. To convert from the raw Bayer patter GBRG8 to YUV444 we used the debayering algorithm R and B Interpolation, as presented in <cit.>,  to get the BGR image matrix, and then used FFmpeg to convert it to the YUV444 format.


The following experiments all utilize CPU encoding on an Intel Xeon(R) Gold 5218R CPU and 96GB of RAM. There are also encoding blocks included in many GPU cards. But those are all geared towards real-time encoding, e.g. for video streaming, and sacrifice quality for speed.
<cit.> reports that for mobile robotics, if real-time encoding is required, hardware GPU encoders are the best. But they also find that the highest quality encoding is done by software encoding. The aim for this work is to find options to encode videos in very high quality, with no visible compression artifacts. For this we require the VMAF score of the compressed videos to be at least 99.40. 


In our experiments we selected the CPU codecs x264, x265, and svtav1, and conducted extensive tests using the FFmpeg tool within the range of -crf 16 to 25 and -preset placebo to medium. Tables <ref>, <ref> and <ref> show the different encoding options and the resulting bitrates, sizes and VMAF of the 60s test video for svtav1, x264 and x265, respectively. The tables are visualized in Fig. <ref> and Fig. <ref> for VMAF vs bitrate and encoding time, respectively. 

 As shown in Fig. <ref>, our test data indicates that svtav1 consistently outperforms the other two codecs. Specifically, the VMAF scores of x264 and x265 only approach 99.50 when the -preset is set to slow and veryslow, while 55% of svtav1's VMAF scores fall between 99.40 and 99.60, significantly outperforming the other two codecs.


 60s video compression times greater than 1000 seconds would result in compression times of more than 9.7 days for our 14 stream 2h dataset, which exceeds our patience for video post-processing. Therefore, we further narrowed down our analysis to codec settings combinations with VMAF scores between 99.50 and 99.60 and preset values greater than 4, as shown in Table <ref>.

Analysis of the Table <ref> reveals a compression combination resulting in a good VMAF score: -CRF 19, -preset 6. Given that our primary concern is compression effectiveness and quality, and according to Table <ref>, there is no visual difference between VMAF scores of 99.50 and 99.55, we ultimately chose the compression combination of -CRF 22 and -preset 6. This combination achieves the best balance in terms of compression effect, quality, and time consumption.

















































































 §.§ Effect of Surce Framerate

The maximum framerate of our dataset is 60Hz. But occasionally we may also opt to collect lower frame rate data. The question this experiment is answering is: What is the best encoding setting for lower frame rates than 60Hz? The hypothesis is, that higher frame rate videos feature more similar image content, so encoding may be more efficient for those streams. On the other hand, camera data is inherently noisy, such that, at least for very high quality videos, no practical difference may be found.

To answer this question we subsample our one minute test stream by taking only every n-th frame, to get to the frame rates shown in Table <ref>. For a fixed VMAF of about 99.50, we then record the average frame sizes and encoding times per frame.

The results show that indeed higher frame rates encode each frame more efficiently. But, since the settings kind of set a goal bitrate, given our goal VMAF, for lower frame rates, lower settings can be chosen. The table shows good settings for very high quality encoding at lower frame rates with acceptable sizes and encoding times. 











 §.§ Compression Results of "ShanghaiTech Mapping Robot Outdoor Dataset"



The practical test of this paper comes from the Mapping Robot Outdoor Dataset <cit.>. This is a ground-based unmanned vehicle dataset that includes multiple sensors. In particular, we focus on the recorded data from two types of RGB cameras provided by the dataset: FLIR Grasshopper3 (GS3-U3-51S5C-C) and FLIR LadyBug5+. They provide high-resolution and high-frame-rate recorded data. We tested our dataset scheme on the data collected by these two cameras, as shown Table <ref>. For this dataset we ran with half the maximum framerate. The results show that compared with the original data, the dataset scheme using video compression technology can save 2,436.5GB of space.








§ CONCLUSIONS



This paper presented a method for encoding robotic dataset video data as mp4 files. Our software allows the replay of mp4 files within the ROS framework, as if the video was stored in a ROS bagfile, including timestamps and replay with simulated time. This enables us to utilize modern video codecs like av1 and is also user friendly, because the data can be viewed with any video player and the data is compatible with ROS 1 and ROS 2 without the need for conversion. The paper furthermore evaluated which codec and setting is the best for very high video quality without any visible compression artifacts, while maintaining reasonable compression size and encoding time and finally selected an av1 setting. This encoding is more than ten times smaller and with higher quality than JPG encoding with relatively high quality 90. We also explored the effect of source frame rate on the encoding and found that higher frame rates encode frames more efficiently due to higher similarity in the frames. With those frame rate experiments we also suggested settings for high-quality encoding with lower source frame rates. In our ShanghaiTech Mapping Robot paper we then applied those results to a real, big robotic dataset.

In the future we expect more datasets from the ShanghaiTech Mapping Robot, even with 60Hz frame, to be released to the public.






unsrt








 Paper Title: ReplanVLM: Replanning Robotic Tasks with Visual Language Models.



Paying More Attention to Image: A Training-Free Method for Alleviating Hallucination in LVLMs
    
  Shi Liu1 Kecheng Zheng1,2^† Wei Chen1^†

    
=============================================================================================

empty
empty


§ ABSTRACT 
Large language models (LLMs) have gained increasing popularity in robotic task planning due to their exceptional abilities in text analytics and generation, as well as their broad knowledge of the world. However, they fall short in decoding visual cues. LLMs have limited direct perception of the world, which leads to a deficient grasp of the current state of the world. By contrast, the emergence of visual language models (VLMs) fills this gap by integrating visual perception modules, which can enhance the autonomy of robotic task planning. Despite these advancements, VLMs still face challenges, such as the potential for task execution errors, even when provided with accurate instructions. To address such issues, this paper proposes a ReplanVLM framework for robotic task planning. In this study, we focus on error correction interventions. An internal error correction mechanism and an external error correction mechanism are presented to correct errors under corresponding phases. A replan strategy is developed to replan tasks or correct error codes when task execution fails. Experimental results on real robots and in simulation environments have demonstrated the superiority of the proposed framework, with higher success rates and robust error correction capabilities in open-world tasks. Videos of our experiments are available at https://youtu.be/NPk2pWKazJc.





§ INTRODUCTION

Large language models (LLMs) have received widespread attention for their remarkable reasoning and understanding capabilities and have been applied in various scenarios, such as human-robot interaction <cit.>, action planning <cit.>, visual target navigation <cit.>, and task planning <cit.>. With prompt engineering techniques like chain-of-thought (CoT), LLMs are empowered with powerful reasoning abilities for complex task planning and execution <cit.>. For example, Raman et al. <cit.> proposed a prompt-based strategy for extracting executable plans from an LLM. Their work introduced a corrective re-prompting technique to extract executable corrective actions to achieve the intended goal. Silver et al. <cit.> used GPT-4 to synthesize Python programs and generate task plans in the planning domain definition language. However, handling the dynamic interaction with the environment remains challenging in robotic task planning. It requires not only a precise perception of environmental information but also the prediction of the causal effects of their actions on the environment. Current LLMs have limitations in understanding the physical state of the world, such as physical forms, spatial locations, and physical properties, which are essential for robot task planning and execution. 

To overcome these challenges, recent studies tend to incorporate external vision models to improve the perception of the environment <cit.>. Under this framework, LLMs assimilate insights from vision models for task planning. Methods such as HuggingGPT <cit.> and Tidybot <cit.> have adopted this technique. Even though this strategy enables the model to understand the environment better and shows potential in robotic task planning, it suffers from serious issues. The performance of such models depends on external vision models, whereas the information provided by the vision model might be incomplete. 

On the other hand, the development of vision language models (VLMs) has provided a novel solution to the challenges faced by LLMs in environmental interaction <cit.>. VLMs integrate techniques from computer vision and natural language processing to deeply understand text information related to visual content and generate corresponding descriptions. By incorporating visual perception capabilities, VLMs can effectively decode visual cues in task planning, not only capturing environmental information but also predicting the causal impact of each action on the environment. 

Although VLMs outperform LLMs in dealing with complex scenes, long-term task planning remains challenging. It involves predicting multiple future steps, each of which may depend on the outcome of the previous step. The complexity of such scenarios requires the model to have a high degree of adaptability and predictive capability to deal with the uncertainty and diversity of the environment. To address such issues, this study proposes a ReplanVLM, a novel robotic task planning paradigm. This framework is developed based on GPT-4V. An internal error correction mechanism and an external error correction mechanism are presented to enhance the performance of the framework in error detection and correction. A replan strategy is suggested to replan tasks when task execution fails. The main contributions of this paper are summarized as follows.

(1) VLM-Enhanced ReplanVLM Framework: We propose a ReplanVLM, a robotic task planning framework based on VLMs. This framework achieves a deep understanding of the environment and task requirements. The introduction of VLMs not only enables the robot to process visual and textual information but also improves the accuracy and efficiency of task planning and execution.

(2) Internal and External Error Correction Mechanisms: We present an internal error correction mechanism to inspect codes, environments, and task requirements to prevent errors caused by hallucinations or misunderstandings. We put forward an external error correction mechanism that reevaluates the environmental state post-interaction between the robot and its surroundings to guarantee the adaptability of task execution to environmental changes.

(3) Experimental Validation and Application: Multiple experiments on real robots and in simulations are conducted to evaluate the efficacy of the proposed ReplanVLM framework for robotic task planning. Experimental results and comparative analysis show that our framework substantially reduces errors in task execution and enhances the robot's autonomy and adaptability in intricate environments. 



§ RELATED WORK

LLMs and VLMs are becoming increasingly popular in the field of robotic task planning <cit.>. Lots of studies were proposed to use LLMs and VLMs to generate task plans <cit.>. For example, Luan et al. <cit.> presented a multi-layer LLM to enhance the robot's proficiency in handling complex tasks. A VLM and a LLM were integrated to tackle the challenges associated with task planning and execution. Huang et al. <cit.> proposed a VoxPoser framework for extracting affordances and constraints for manipulation tasks in the real world. By leveraging their code-writing capabilities, LLMs were used to interact with VLMs to construct 3D value maps to enable zero-shot task trajectory generation for robots. Zhang et al. <cit.> introduced a visually-grounded symbolic planning framework. They used VLMs to detect action failures and verify action affordances towards enabling successful plan execution. Although VLMs can provide visual perception information to LLMs, it might be incomplete. For example, VLMs may only provide object names while ignoring the spatial relationship between objects, which could lead to the failure of robotic task execution or even damage equipment. 

To address this issue, some studies directly used VLMs to generate robotic task plans. Hu et al. <cit.> put forward a robotic vision-language planning (ViLA) framework. They utilized LLMs to generate a sequence of actionable steps. By integrating perceptual data directly into the reasoning and planning processes, the ViLA allows robots to understand common sense knowledge in the visual world, including spatial layouts and object attributes. Skreta et al. <cit.> employed VLMs to understand world state information to empower robots to replan in real-time in the event of action plan failures. Shirai et al. <cit.> introduced a visual-language interpreter (ViLaIn) framework for robot task planning. They used LLM and VLM to generate problem descriptions and drive symbolic planners in a language-guided framework. Wake et al. <cit.> presented a multimodal robot task planning pipeline using GPT-4V. By integrating observations from human demonstration, their proposed GPT-4V(ion) succeeded in converting human actions from videos into robot-executable programs. Even if these methods made significant advances in generating task plans, they do not have error correction mechanisms. When VLMs misunderstand instructions or environments, they may generate incorrect plans.

To bridge this gap, this paper proposes a ReplanVLM framework, which introduces internal and external error correction mechanisms to handle internal (e.g., code generation errors) and external errors (e.g., task failures due to environmental changes during execution), respectively.



§ REPLANVLM: MODEL STRUCTURE AND DETAILS



 §.§ Framework of the proposed ReplanVLM

Fig. <ref> shows an overview of the ReplanVLM framework, which is developed based on GPT-4V. To start with, the model takes textual instructions as input, e.g., “help me get an apple." It can also handle abstract requests, like “I'm a bit thirsty." In this case, the model is capable of making decisions to grab a cup or an apple based on the recognition results of environmental information. In addition to the textual instructions, the input can be a combination of images and text, e.g., “arrange the blocks in reality according to the sequence shown in the picture." After the initial input stage, a Decision Bot is introduced to plan the task based on the provided input and generate code. The code is then passed to an Inner Bot for review. If the Inner Bot finds errors (i.e., “No"), such as code mistakes or task planning issues, the Inner Bot will send the reason back to the Decision Bot to regenerate new task plans and codes. Otherwise (i.e., “Yes"), the robot executes the task plan. Once the execution is completed, an Extra Bot is suggested to compare the current environment with its initial state based on the task requirements. If the task is identified as a failure (i.e., “No"), the Extra Bot will send the failure information back to the Decision Bot to replan the task. Otherwise (i.e., “Yes"), the iteration would be terminated. 



This framework is proposed for various tasks and objects even though Fig. <ref> uses cubes as examples. In line with the framework, pseudocode (see Algorithm <ref>) is provided to illustrate the overall process, where ℓ represents the tasks to be completed, 𝐭 is a certain moment, and 𝐱_t is the state of the environment at time 𝐭.





 §.§ Internal Error Correction Mechanism

The internal error correction mechanism is proposed to mitigate the hallucination problem in LLMs during the task generation process. Hallucination is a challenging issue in the process of generating task plans. Incorrect codes may be produced even when the task planning is correct. Another significant role of the internal error correction mechanism is to utilize VLMs to predict the potential impact that their actions might have on the surrounding environment. Consider a scenario where the objective is to “grab the red block," but a yellow block rests on the red one. In some cases, the model might attempt to grab the red block directly, disregarding the yellow block above it. This action would potentially cause harm to the objects or even damage the robots. To tackle such issues, this study presents an internal error correction mechanism to prevent these problems to enhance the safety and reliability of task execution. The internal error correction mechanism involves an Inner Bot. Based on an internal feedback strategy, the internal error correction mechanism can assess and correct the task plans and codes generated by the Decision Bot. It can be conducted as follows.

Format Check: The Inner Bot first verifies whether the task plans and codes generated by the Decision Bot meet the format requirements. This step prevents the generation of unsuitable text (e.g., blanks) or pseudocode, which can occur even with correct prompts.

Matching Check: Then, Inner Bot checks the alignment of the generated plans and codes. This step helps avoid errors caused by discrepancies between codes and task plans.

Plan and Code Verification: To verify the accuracy of the task plans and codes generated by Decision Bot, Inner Bot regenerates the task plans and codes based on the environment and codebase information, and compares whether the two sets of codes have the same functionality. Such a comparison enhances the framework to detect and tolerate errors.

If any of the above three points are not satisfied, the feedback generated by the Inner Bot will be passed to the Decision Bot. Given that the Inner Bot's response might contain inaccuracies, this feedback only serves as a reference for the Decision Bot, not a decisive factor. Subsequently, the Decision Bot will regenerate plans and codes based on the known information. If their opinions remain inconsistent after five cycles, the task is deemed a failure. If all three points are satisfied, the robot will execute the task according to the plans and codes generated by the Decision Bot. Fig. <ref> illustrates an example of the internal error correction mechanism in Task 1 (details can be found in Section <ref>).





 §.§ External Error Correction Mechanism

The external error correction mechanism is presented to assess the completion status of tasks. During the task execution, robots often encounter unpredictable challenges that can result in task failure. These challenges include but are not limited to, sudden changes in the environment or failures in the robot system. In this context, the external error correction mechanism is crucial. It refers to an Extra Bot, which is utilized to determine whether the task is completed. This mechanism analyzes environmental changes, compares these changes with task requirements, identifies potential causes of task failure, and records such information for task replanning, which is conducted as follows.

The Extra Bot evaluates images of the environment taken before and after task execution. Using these images, along with task requirements, and the task plans and codes generated by the Decision Bot, it assesses whether the task has been successfully completed. If the task is deemed incomplete, the Extra Bot identifies the perceived error reasons and sends them to the Decision Bot. The Decision Bot then uses this feedback and the current environmental information to regenerate task plans and codes. If the Extra Bot confirms task completion, the ReplanVLM framework terminates.

By providing such external feedback techniques, the external error correction mechanism can not only enhance the system's ability to understand failure scenarios but also optimize task execution strategies. Fig. <ref> depicts an example of the external error correction mechanism in Task 2 (details can be found in Section <ref>).





 §.§ Object Detection

In this study, Yolov8 algorithm is used for object detection. Firstly, it identifies the object's pixel coordinates based on the bounding box in RGB images. Then, the object's pose information is gained by integrating these pixel coordinates and the depth information. Besides, this algorithm has tracking capabilities. It can accurately detect objects and give their pose information even if they move. In some tasks, such as “give_me()," the movement endpoints are predetermined. While in other tasks, like “put(`plate')," the robot dynamically chooses a suitable placement based on environmental settings.



§ EXPERIMENTS



 §.§ Experimental Platform

As illustrated in Fig. <ref>, the experimental setup consists of a 6 degrees of freedom robotic arm (i.e., JAKA Zu 7), an Intel Realsense D435i depth camera, a Rochu pneumatic gripper, and a conveyor belt. A couple of toys and simulated fruits are introduced as objects for the experiment.





 §.§ Prompt

For each component of the ReplanVLM framework, we propose different prompts. As shown in Fig. <ref>, the prompt of the Decision Bot consists of role-playing, error messages, code repository, CoT, and examples. By contrast, the prompts of the Inner Bot and Extra Bot comprise role-playing, images, and information from the Decision Bot. For each task undertaken by these components, the prompts would vary slightly to accommodate the specifics of the task, e.g., the code repository and role-playing.





 §.§ Task Design

A series of tasks are proposed to evaluate the effectiveness of the ReplanVLM framework on real robots and in simulation environments. As shown in Table <ref>, these tasks are designed based on the following criteria. 

Semantic Understanding (SU): the ability to comprehend and interpret the meaning of a language in a way that is akin to human understanding. For example, given the task “I'm a little hungry," the robot can identify from environmental clues that it should grab an apple. 

Spatial Relationship (SR): the capacity to understand the relative spatial position between objects, such as the ability to stack blocks in order according to a given picture. 

External Spatial Constraints (EC): the ability to recognize various constraints present in the environment. For instance, in the task “Help me grab the red block," the robot needs to remove the blue and yellow blocks located above the red one. 

Understanding of Special Attributes (UA): the ability to comprehend the attributes of objects, such as “kind," “evil," “dangerous," “safe," “edible," etc. 

Adaptability to Dynamic Scenes (AS): assessing the framework's adaptability to dynamic scenes.

Minimum Operation Steps (MS): the minimum operation steps (no error correction process) include robotic arm movement, gripper closure, and gripper opening.



Based on these criteria, seven tasks are proposed as follows.

Task 1: Food Demand Recognition. In this task, a variety of objects, including food, are placed on a table. This task aims to assess whether the robot can accurately understand the user's needs upon receiving the instruction “I'm hungry" and successfully identify and grasp the apple on the table.

Task 2: Grasping the Block. In this task, three colored blocks are arranged in a specific order—blue on top, yellow in the middle, and red at the bottom. Before grabbing the red block and placing it into a cardboard box, the robot needs to remove the blue and yellow blocks.

Task 3: Stacking Blocks. By providing the VLM with a picture showing the order of block stacking, the robot is required to stack the blocks in the same order.

Task 4: Sequential Arrangement on a Conveyor Belt. This task requires the robot to reorder the objects in a designated order on a moving conveyor belt. This task aims to examine the robot's response to dynamic environments, especially its ability to monitor and adjust the arrangement of objects in dynamic scenarios.

Task 5: Categorization and Transport. In this task, the robot is required to classify the fruits on the conveyor belt and place them into different plates. This task intends to test the robot's ability to recognize and transport items, assessing its efficiency in handling objects with various physical properties.

Task 6: Grabbing Invisible Objects. This task requires the robot to identify and grab a block inside a drawer. Since the visual detection system cannot directly identify the block, the robot must open the drawer and then grab the object.

Task 7: Toy Recognition. In this task, the robot is asked to identify and grab toys with given attributes, e.g., grabbing a toy with evil attributes. This task aims to evaluate the VLM's ability to recognize intricate features, such as moral attributes.

In the experiment, the robot runs ten rounds on each task. The average success rate of the ten rounds will be taken as the evaluation metric to indicate the system performance on the corresponding task.



 §.§ Ablation Experiment

To evaluate the relative importance of each module within the ReplanVLM framework, ablation studies are conducted to assess the contribution of the internal and external error correction mechanisms on the overall performance of the ReplanVLM. Accordingly, four models are developed in the ablation experiment, i.e., the ReplanVLM, the ReplanVLM without internal error correction mechanism (ReplanVLM-internal), the ReplanVLM without external error correction mechanism (ReplanVLM-external), and the ReplanVLM without error correction mechanisms (ReplanVLM-both). Each model will be tested on the tasks as presented in <ref>.



 §.§ Error Correction Experiment

Furthermore, error correction experiments are introduced to test the robustness of the ReplanVLM framework when faced with external disturbances. Specifically, this experiment focuses on the model's ability to detect and correct errors after a failed operation. In this experiment, intentional manual interference is applied to cause a failure during the robot's grabbing task. Then, the robot needs to determine whether the task is completed. If the task is deemed incomplete, a correction mechanism is triggered. An error detection rate and an error correction rate are introduced as evaluation metrics in this experiment. Likewise, the experiment will be run in ten rounds to get statistical metrics.



 §.§ Simulation Experiment

Besides the experiments on real robots, simulations are conducted to evaluate the ReplanVLM framework. CoppeliaSim robotic simulator is introduced to perform the seven tasks described in <ref>. As depicted in Fig. <ref>, a UR5 robotic arm and a RG2 gripper are set up to execute these tasks in the simulation environment.






§ RESULTS AND DISCUSSION



 §.§ Experimental Results

Table <ref> illustrates the experimental results on real robots and in simulations. As shown in Table <ref>, the proposed ReplanVLM achieves an average success rate of 94.2% on real robots. The success rate can even reach up to 100% in the task of food demand recognition (Task 1), sequential arrangement on a conveyor belt (Task 4), categorization and transport (Task 5), and toy recognition (Task 7). In task 3 (visual assistance in block stacking), the ReplanVLM framework performs the worst, with a success rate of 80%. In most of the tasks, the success rate is higher than 90%. Overall, the ReplanVLM performs well across various scenarios, especially in tasks that require the recognition of complex attributes, the discovery of potential constraints, and the use of world knowledge. The performance across diverse tasks demonstrates the effectiveness of the proposed ReplanVLM in robot task planning.



In addition, the proposed framework achieves an average success rate of 94.2% in simulations, which is identical to the results on real robots. Specifically, the success rate of Task 1, Task 2, and Task 5 can reach up to 100%, while the other four tasks get a success rate of 90%. Therefore, the ReplanVLM framework not only performs well in a single task but also maintains a high level of reliability in multiple scenarios in simulated environments. Moreover, the results of the ReplanVLM framework show a high consistency in both real-world and simulated environments.



 §.§ Comparative Experiment Analysis

To evaluate the performance of the proposed ReplanVLM, SayCan <cit.> and ProgPrompt <cit.> are introduced as baseline methods. As listed in Table <ref>, the proposed framework achieves an average success rate of 94.2%, while SayCan gets 10% and ProgPrompt gains 31.4%. Obviously, the average success rate of our framework is much higher than baseline methods. The proposed framework outperforms baseline methods on all testing tasks. Notably, both the SayCan and ProgPrompt obtain nearly zero success rates in tasks that require a deep understanding of visual information, such as scene analysis with visual constraints and handling objects with complex attributes (e.g., distinguishing between monsters and Ultraman toys). Compared with baseline methods, the proposed framework shows great potential in intricate environments, especially in scenarios that require scene understanding and decision-making capabilities.



Given the poor performance of baseline methods, the shortcomings lie in two aspects. Firstly, previous works mainly use a single-modal LLM for robot task planning. Their understanding of the environment mainly relies on external models, which makes it difficult to consider the constraints in the environment. Secondly, previous works do not have effective error detection and correction mechanisms. They cannot regenerate task plans based on previous failed experiences. By contrast, this study introduces a VLM for task planning and proposes internal and external error correction mechanisms to correct errors. Compared with previous works, our work can better understand the environment and regenerate task plans based on failed experiences. The success rate of the task planning is substantially enhanced.










 §.§ Ablation Experiment Analysis

Corresponding to the protocol of the ablation study discussed in <ref>, the results of the ablation experiment are given in Table <ref>. As illustrated in Table <ref>, the average success rates of the four methods are obtained as 94.2% (ReplanVLM), 87.1% (ReplanVLM-internal), 78.5% (ReplanVLM-external), and 68.6% (ReplanVLM-both). Specifically, the ReplanVLM outperforms the other three methods on all testing tasks. With the internal and external error correction mechanisms, the ReplanVLM achieves the highest average success rate. On the other hand, the average success rate of the ReplanVLM-internal is slightly lower than the ReplanVLM. Although the ReplanVLM-internal performs as well as the ReplanVLM on some tasks, it lags behind ReplanVLM on most tasks. In addition, the average success rate of the ReplanVLM-external is even lower than the ReplanVLM-internal, while the ReplanVLM-both performs the worst.



From the results of ablation experiments, it can be concluded that both the internal and external error correction mechanisms contribute substantially to the overall performance of the ReplanVLM framework. By removing any one of the error correction mechanisms, it will lead to a significant decrease in the success rate. By comparison, the impact of the external error correction mechanism on the overall framework is more pronounced than the internal error correction strategy. 



 §.§ Error Correction Experiment Analysis

Table <ref> shows the results of the error correction experiment. As listed in Table <ref>, the proposed framework gains an average error detection rate of 95.7%, with an average error correction rate of 80%. The error detection rate can reach up to 100% in the majority of tasks. Fig. <ref> shows an example of the correction experiment. As it can be seen from the experimental results, the ReplanVLM framework can effectively detect errors and correct them during the task execution. Although there is a fluctuation in error correction rate across different tasks, the proposed framework shows strong potential in enhancing the accuracy and stability of robot task execution, especially in intricate environments.







 §.§ Discussion

With the aid of the internal and external error correction mechanisms, the proposed ReplanVLM framework demonstrates an impressive performance on real robots and in simulation environments. It outperforms baseline methods in the comparative studies, with a better performance on all testing tasks. Either the internal error correction mechanism or the external error correction mechanism contributes significantly to the overall performance of the ReplanVLM framework.

Despite the prominent performance, this study has some limitations. First of all, this study only uses text and visual information for robot task planning. It cannot perceive other modal information such as touch and hearing. Secondly, although the LLM possesses broad world knowledge, it is difficult to handle complex manipulation tasks, such as cooking, through simple programming. Thirdly, the success rate of the task planning and execution still suffers from certain errors, such as the limitations in the VLM's visual processing and defects with the error correction mechanisms. In addition, the network latency caused by using the GPT-4V deteriorates the real-time performance of the overall system. The framework cannot apply to real-time systems.



§ CONCLUSIONS

This paper proposed a ReplanVLM framework to enhance the success rate of robots in task planning and execution. By incorporating VLM techniques in robotic task planning, the ReplanVLM framework introduces internal and external error correction mechanisms that can effectively identify and rectify errors encountered during task execution. Extensive experiments in real-world and simulated environments were conducted to evaluate the effectiveness and robustness of the proposed method in various scenarios. Experimental results and comparative analysis demonstrate that the ReplanVLM framework can reliably detect and correct errors in diverse situations. The success rate of task planning and execution is substantially enhanced.

In the future, we will introduce some other perceptual information, such as tactile information, to enhance the framework to better understand the environment so that the task plans can be generated more accurately. In addition, we will investigate the potential of multimodal LLMs in completing complex manipulation tasks. 


IEEEtran



 Paper Title: VITAL: Visual Teleoperation to Enhance Robot Learning through Human-in-the-Loop Corrections.


Bimodal Scaling Law and Size Effect In Superelastic Nanopillars
    Xian Chen
    August 5, 2024
===============================================================




§ ABSTRACT 
   Imitation Learning (IL) has emerged as a powerful approach in robotics, allowing robots to acquire new skills by mimicking human actions. Despite its potential, the data collection process for IL remains a significant challenge due to the logistical difficulties and high costs associated with obtaining high-quality demonstrations. To address these issues, we propose a low-cost visual teleoperation system for bimanual manipulation tasks, called VITAL. Our approach leverages affordable hardware and visual processing techniques to collect demonstrations, which are then augmented to create extensive training datasets for imitation learning. We enhance the generalizability and robustness of the learned policies by utilizing both real and simulated environments and human-in-the-loop corrections. We evaluated our method through several rounds of experiments in simulated and real-robot settings, focusing on tasks of varying complexity, including bottle collecting, stacking objects, and hammering. Our experimental results validate the effectiveness of our approach in learning robust robot policies from simulated data, significantly improved by human-in-the-loop corrections and real-world data integration. Additionally, we demonstrate the framework's capability to generalize to new tasks, such as setting a drink tray, showcasing its adaptability and potential for handling a wide range of real-world bimanual manipulation tasks. A video of the experiments can be found at:  
    




 





§ INTRODUCTION


Imitation Learning <cit.> has emerged as a fundamental approach in robotics, enabling robots to acquire new skills by observing and mimicking human actions <cit.>. This method holds significant promise for developing complex robotic behaviors without the need for explicitly programming the robot. However, the primary challenge with imitation learning lies in the data collection process <cit.>. On the one hand, acquiring high-quality demonstrations is costly and logistically challenging. On the other hand, the need for extensive and diverse demonstrations to train models magnifies these difficulties, as each new task or environment often requires a fresh set of demonstrations <cit.>. To address these challenges, teleoperation has been proposed as a viable solution. In recent years, there has been a growing interest in the development of bimanual teleoperation interfaces that facilitate the learning and execution of various household and industrial tasks <cit.>. 


One example of such a system is the ALOHA platform <cit.>, which has garnered considerable attention from the robotics research community. Such teleoperation platforms enable the execution of various tasks such as pushing chairs, using cabinets, wiping spills, and even cooking shrimp. Despite the advantages offered by such interfaces, there remain significant challenges that hinder their widespread adoption. One of the primary issues is that such teleoperation interfaces are hardware-specific and costly. The cost of such interfaces is approximately $30K, which makes them non-scalable and expensive for many research laboratories and practical applications. To overcome these challenges, we propose a low-cost visual teleoperation interface to collect high-quality demonstrations for long-horizon bimanual manipulation tasks.


In robot manipulation tasks, IL is often trained via in-domain demonstrations, which correspond to the data obtained directly from the deployment environment <cit.>. Although it is widely accepted that gathering direct examples from the actual robot is the most efficient way to learn long-horizon manipulation tasks, we show that collecting human demonstrations in real and digital twin environments can produce better outcomes for real-world tasks. Our approach allows the creation of a large dataset of augmented demonstrations based on a few initial demonstrations. By leveraging the flexibility and functionalities of our digital twin environment, we can enhance the number of demonstrations and the generalizability of the learned policy. These augmented demonstrations are then used to train a policy, which is fine-tuned using real-world demonstrations. Experimental results show that our approach significantly improves the performance of the robot in real-world scenarios. In summary, the main contributions of this work include:


    
  * We introduce a low-cost visual teleoperation approach that enables high-quality data collection for bimanual manipulation tasks.
    
  * We leverage a digital twin of the robot to create a vast dataset of augmented demonstrations from a limited number of initial demonstrations. This enhances the scalability and generalizability of the learned policies without requiring extensive real-world demonstration.  
    
  * We incorporate human-in-the-loop corrections to refine and improve the robot's performance in complex tasks. This method allows for interactive learning and adaptation, ensuring higher success rates in real-world applications.





























§ RELATED WORK

Robot Learning with Teleoperation: IL is a powerful technique for robot learning, enabling robots to acquire new skills by mimicking human demonstrations <cit.>. However, collecting in-domain demonstrations is often difficult and not scalable due to the need for physical interaction with the robot hardware. This process is also expensive as it requires constant access to the robot. Some works use scripted “expert” in simulation to generate demonstrations, but scaling these approaches to more complex tasks is challenging <cit.>. Other IL methods leveraged visual deep neural networks to train reactive models from images captured during demonstrations <cit.>. While these models provide greater flexibility, they necessitate collecting a large number of demonstrations to learn simple pick-and-place tasks, making them user and computationally expensive <cit.>. Other approaches utilize human activity videos to guide robot learning but often fall short in training 6-DoF manipulation policies <cit.>. These methods typically require in-domain teleoperation data to bridge the gap between video demonstrations and robotic execution <cit.>.
A common problem with these IL methods is the lack of corrective feedback, leading to the accumulation of errors over the length of the demonstration, known as the compounding errors problem <cit.>. Similar to our approach, some methods have focused on dividing demonstrations into several waypoints to address this issue <cit.>. Unlike these approaches, we aim to automatically extract the waypoints of a demonstration without adding any burden on the human user. Other IL methods used bimanual teleoperation interfaces, such as ALOHA <cit.>, for mapping joints' value from master to slave robots. However, these approaches are quite expensive and not scalable as they require four robots for bimanual manipulation: two masters and two slaves. In contrast, we propose a low-cost teleoperation interface that allows controlling robot end-effectors to perform complex bimanual tasks and collect demonstrations in both real and simulation environments. Teleoperation within a digital twin of the robot enables us to scale up the training data easily without constant access to the real robot. Additionally, we introduce a human-in-the-loop correction mechanism to repair faulty demonstrations, enhancing the robot's performance in complex tasks with minimal effort. 

Sim2Real Transfer: Some approaches, such as MimicGen <cit.> and CyberDemo <cit.>, attempt to collect demonstrations in simulation to reduce the cost of collecting demonstrations on robots. However, due to the sim-to-real gap, deploying the learned policy on a real-world robot remains challenging. Other methods use VR for teleoperation and data collection <cit.>, such as DIME <cit.>. Qin et al. <cit.> and Wang et al. <cit.> proposed tracking the user's figures through an external camera for teleoperation. Approaches like DexMV <cit.>, DexVIP <cit.>, and VideoDex <cit.> use everyday videos to learn motion priors. Due to the domain gap, these methods are not directly applicable to robots and often require another round of training.
In contrast, we collect demonstrations in the digital twin of the robot and develop data augmentation techniques at the trajectory level rather than the image data level. Most of the teleoperation interfaces have been designed either for simulation <cit.> or for real robot  <cit.> setups. Unlike these approaches, we proposed a low-cost bimanual teleoperation interface that can be used to collect demonstrations using real-robot and its simulation. We showed that policies trained on a mix of simulated and real demonstrations achieve better results than those trained solely on simulation or real-robot data. 





§ METHOD





 §.§ Collecting Demonstrations through Teleoperation

r0.4


    
    < g r a p h i c s >

    Overview of the proposed low-cost teleportation interface.
    
    


    
The architecture of our low-cost visual teleoperation system is illustrated in Figure <ref>. 

We use an external RGB-D camera to capture human motions and two Bluetooth-enabled selfie sticks for commanding the left and right grippers. The captured human motion data and gripper commands are translated into real-time motor commands for the robot arm and the gripper. In particular, we utilized the Mediapipe library for skeleton tracking, specifically focusing on 24 upper body keypoints. These keypoints are crucial for capturing the operator's movements accurately. To establish a stable huamn-world reference frame, we considered the left shoulder, right shoulder, left hip, and right hip keypoints. We define the origin of this reference frame between left and right hip keypoints. This reference frame serves as the basis for calculating the pose of each hand relative to the world frame. Given the differences in body morphology and arm length between humans and the robot, two scaling factors, α for the right arm and β for the left arm,  are applied to bridge the gap, thereby making teleoperation more intuitive and precise. For controlling the grippers of the robot's arms, we utilized two Bluetooth selfie sticks. These sticks were adapted to send commands for opening and closing the grippers. 
r0.15
    

    
    < g r a p h i c s >

    Low-cost teleportation sticks.
    

    

To track the sticks' orientation, we attached AruCo markers <cit.> to the sticks as shown in Fig. <ref>.



To efficiently manage the complexity of long-horizon tasks and reduce the compounding errors, we employ task decomposition. Each task, T, is decomposed into a sequence of subtasks, S_n, T = (S_1, S_2, …, S_n). The start and end poses of the trajectory for each subtask are defined based on the current pose of the manipulator and the pose of the object that should be grasped or placed. 
 
The detection of these subtasks is facilitated by the commands given by the user for grasping or placing objects. When a user issues a command, the system identifies the relevant subtask, captures the necessary pose information, and creates a trajectory segment for that subtask. By structuring tasks this way, we ensure that the demonstration data is organized and can be effectively used for training purposes.

To replicate the physical environment in the simulation, we build a digital twin of our robot in Gazebo. This includes accurately modeling the robot and objects to ensure consistency between the simulated and real-world environments. During the demonstration phase, we performed a task in Gazebo while recording all relevant ROS topics, including the initial state of the objects, joint values, end-effector poses, outputs from RGB-D cameras (head-mounted and on the left and right arms), commands from the instructor, activity videos with corresponding 3D skeleton data, object poses, and success or failure indicators, at their respective publication rates into a ROS bag file. The comprehensive nature of this recorded data ensures that all necessary information is captured for the next stage of the methodology, where data augmentation will be applied to enhance the dataset.






 §.§ Augmenting Demonstration in Simulation


To enhance the collected demonstration and improve the robustness of the learned policies, we perform data augmentation on the trajectory level (i.e. subtasks). Our augmentation method uses several techniques to generate a diverse set of trajectories from the initial demonstrations. First, we extract waypoints from the recorded bag file by reading the positions and orientations of the robot's end effectors. These waypoints are then used to fit a polynomial trajectory. We exclude the last few points from the polynomial fitting to ensure the end of the trajectory which represents the affordance pattern is preserved. We then apply uniform sampling to the trajectory by calculating the cumulative arc length of the waypoints to ensure uniform spacing between the sampled points. 
This method helps in creating a smooth and evenly distributed trajectory, which is then segmented into smaller parts for detailed analysis and augmentation.

We first augment the trajectory by adding Gaussian noise to the waypoints. This involves introducing random variations to the trajectory points to simulate real-world imperfections and variations. By adding noise, we can create multiple versions of the trajectory, enhancing the model's ability to generalize across different scenarios. The noise can be applied to all waypoints or selectively to preserve certain critical points, such as the trajectory's endpoint.
We also perform translations and flips on the trajectory. Translating the waypoints involves shifting the entire trajectory in the x, y, and z directions, which helps in creating varied instances of the task. Flipping the trajectory along the x-axis generates a mirrored version, providing additional diversity in the dataset and making the initial trajectory of one arm useful for the other arm. Furthermore, we sample new initial points within specified bounds and connect these points to the nearest waypoints in the existing trajectory. This process involves finding the nearest index in the trajectory and creating new paths from these initial points to the rest of the trajectory. By varying the starting points, we can simulate different initial conditions for the task. Each trajectory is annotated with an arm identifier, distinguishing between left and right arm movements. This comprehensive augmentation process significantly expands the dataset, making it thousands of times larger than the original set of demonstrations. However, augmented trajectories can sometimes be imperfect and not executable due to the robot's arm kinematics, potentially leading to task failures. To ensure the reliability of the augmented data, we validate each augmented trajectory in the digital twin environment. Only successfully executed trajectories are used for training a policy. This way we ensure that the learned policies are robust and capable of handling a wide range of real-world conditions.






 §.§ Hierarchical Policy Learning




To effectively learn and execute long-horizon tasks, we formulate the learning policy as a hierarchical policy learning problem through Behavioral Cloning (BC). This approach involves training a high-level policy to select a sequence of subtasks and low-level policies to execute each subtask. 

For a given task, we assume that we can detect the sequence of subtasks based on the demonstration by detecting commands for grasp or release (indicated by pressing the bottom of the selfie stick). These commands allow us to identify the start and end poses for each subtask, facilitating the decomposition of long-horizon tasks into a series of subtasks.

The high-level policy, π_H, is responsible for selecting the appropriate subtask, S_i, based on the current state of the environment and the task progression. This high-level policy can be implemented as a state machine or a learned model. In this work, we use a state machine that receives the current state of the robot, and the pose of the objects to infer the current subtask, π_H(o, r)→ S_i, where o shows the current state of objects and r is the state of the robot. Alternatively, a learned model could receive the task ID and the state of the objects and robot, and predict the subtask.



For the low-level policy, we propose a unified multi-subtask policy learning, π_MS, that can handle all subtasks. This policy receives the predicted subtask ID, S_i, along with the start, p_s, and end poses, p_e, and generates the appropriate trajectory conditioned on the input subtask, π_MS(S_i,p_s, p_e) → trajectory. The training process begins by loading the augmented trajectory data of a task. The trajectories are padded to ensure uniform length, which is critical for batch processing in neural networks. Our model is designed to predict the intermediate points of a trajectory given the start and end points. The high-level policy coordinates these subtasks, ensuring smooth transitions and overall task completion.










 §.§ Residual Learning with Human-in-the-Loop


r0.5
    


    < g r a p h i c s >

    Overview of the residual learning based on human-in-the-loop feedback.
    
    


Despite the robustness of the trained policy, there can be scenarios where the robot fails to complete the task due to a sim-to-real gap, pose estimation errors, or small errors in executing a trajectory. To address these issues, we incorporate a residual learning approach with human-in-the-loop corrections during the policy rollouts. When the robot encounters a failure during task execution, a human operator steps in to correct the errors. The user plays the recorded rosbag file of the experiment and provides the correction feedback whenever it is needed. The correction involves measuring the 3D delta position changes of the wrist of the left and right hands of the demonstrator relative to the initial poses of the hands. These delta changes are then scaled by empirically determined factors α (for the right hand) and β (for the left hand), both < 0.1 to avoid fast movements, and added to the robot's wrist movements. This minimal adjustment helps the robot to accomplish the task with improved accuracy. If the user observes a large error in the current prediction of the policy, they can switch the system to full teleoperation mode. In this mode, the robot will directly follow the human wrist movements to accomplish the task. 

The data from these human-in-the-loop corrections is recorded into a separate dataset, 
D'. This dataset includes both residual corrections and instances where full teleoperation was required. To fine-tune the policy, training data is sampled equally from both the original dataset D and the new dataset D'. This balanced sampling ensures that the model learns from the augmented experiences and corrections provided by the human operator, thereby improving its performance and adaptability. This iterative process of learning ensures that the policy continuously improves and adapts to real-world conditions.




§ EXPERIMENTAL SETUP



We performed multiple rounds of experiments on both simulation and real-robot settings to validate our method. Specifically, we study the following questions: (Q1) Is it feasible to train robot policies using exclusively simulation data without relying on on-robot data? 
(Q2) Which model architectures and data composition ratios are most effective for policy training? Specifically, what is the optimal balance between simulated and real-world data?
(Q3) To what extent does human-in-the-loop correction enhance policy performance when teleoperation data is limited?
(Q4) Can our framework successfully execute another complex bimanual task, such as setting a drink tray, which is somewhat similar to one of the learned tasks?



 §.§ Setup
 
Our experimental setups, both in simulation and on real robots, are illustrated in Fig. <ref> and Fig. <ref>, respectively. We developed a simulation environment in Gazebo, leveraging the ODE physics engine to closely mimic the behavior of our dual-arm robot. The hardware setup in both simulation and real robot includes an Asus Xtion camera, two Universal Robots (UR5e) equipped with Robotiq 2F-140 grippers, and an interface for managing the initiation and conclusion of experiments. For the Teleoperation interface, we use a RealSense D435 RGB-D camera to capture the motion of the user. All evaluations were performed with a PC running Ubuntu 18.04 with a 3.20 GHz Intel Xeon(R) i7, and a Quadro P5000 NVIDIA. 

r0.38


    
    < g r a p h i c s >

    Three long-horizon evaluation tasks are used. (left) Bottle Collecting; (center) Stacking Pringles; (right) Hammering.
    

    

Tasks: We designed three tasks of varying difficulty levels to test the performance of the teleoperation and the robustness of the learned policies: (i) Bottle Collecting: In this task, the robot should collect multiple bottles with different shapes from different locations. (ii) Stacking Pringles: In this task, the robot must stack Pringles cans on top of each other in various locations. This requires precise control and coordination to ensure the cans are aligned and stable. 
(iii) Hammering: The most challenging task involves the robot using a hammer to strike a cylinder. This task tests the robot's ability to handle a tool effectively. To evaluate the performance of the robot on each task, we performed 100 simulation and 10 real-robot experiments. In the case of real robot experiments, we used a 3D perception system to get the pose and label of the objects <cit.>.
r0.38


    
    < g r a p h i c s >

    (left) Our real dual-arm robot setup; (right) Output of our perception including the bounding box, pose, and label of the objects.
    

    





Baselines and Metrics: To determine the best model architecture for bimanual manipulation tasks, we evaluated multiple Behavioral Cloning Recurrent Neural Network (BC-RNN) baselines. The baselines include: LSTM (Long Short-Term Memory), GRU (Gated Recurrent Unit), and Transformer-based network. PyTorch is used as a deep learning framework. We use Mean Squared Error (MSE) as the loss function for trajectory prediction. An Adam optimizer is employed to adjust the model weights during training. Early stopping is implemented to halt training if the validation loss does not improve for a specified number of epochs. Each network has been trained for 200 epochs. We split the data into training (70%), validation (15%), and test sets (15%) to evaluate the model's performance. Details of the networks are discussed as a part of supplementary materials. The Task Success Rate is used as the primary metric for evaluation. This metric is defined as the percentage of successfully completed tasks out of the total number of attempts. 





 §.§ Results



Q1 - Feasibility of Training Policies with Simulation Data:
We conducted extensive sets of experiments across all tasks by training the policy solely on augmented simulation trajectories. For each task, we collected five successful in-domain demonstrations and augmented them to 80K demonstrations. The BC-LSTM policy trained on this augmented data was then tested on both simulated and real robots. Results are summarized in Fig. <ref>. In simulation, the robot successfully performed most tasks. However, in real-world experiments, 
r0.38


 
    < g r a p h i c s >

    Results of Q1 evaluation.
    

    

certain discrepancies such as minor inaccuracies in object grasping and placement were observed. Specifically, in the bottle collecting task, the robot achieved an average task success rate of 92% in simulation and 80% in real-robot scenarios. In the stacking objects task, the robot successfully completed 84% of the experiments in simulation but only 60% in real-world tests. Similarly, for the hammering task, the success rate was 78% in simulation and 50% in real-world experiments. The main failure in bottle collecting was due to inaccurate trajectory prediction, which led to inefficiencies in approaching, grasping, or placing the bottles. The primary reasons for failures in the stacking and hammering tasks were inaccuracies in object grasping. Specifically, when the robot approached to grasp the object, the tip of the gripper touched the object, causing it to move slightly. This offset often led to failures as objects falling during the last step of stacking or missing the target when hammering. To address these challenges, we will check the impact of the model architecture and the effect of incorporating real demonstrations in addition to digital-twin demonstrations for training the policy to address sim-to-real gap in the following subsection (Q2). Furthermore, we believe incorporating feedback mechanisms through vision sensing could provide information on the exact part of the object being grasped. This feedback would enable the system to update the trajectory accordingly by learning suitable residual corrections, thereby improving the accuracy and reliability of the task execution. 













Q2 - Impact of Model Architecture and Data Composition: To understand the influence of different model architectures and the ratio of simulated to real data, we conducted experiments with various configurations. We first trained LSTM, GRU, and Transformer-based models solely on augmented simulation demonstrations and compared their performance across the proposed tasks in the simulation environment. After identifying the best-performing architecture, we trained policies using different ratios of simulated to real-robot demonstrations, including 70%-30%, 50%-50%, 30%-70%, and 0-100% mixes. For each task, we recorded five real-world demonstrations and five simulation demonstrations, which were then augmented to create a larger dataset. The final training data was sampled from these augmented demonstrations according to the specified ratios. This approach allowed us to determine the optimal balance between simulation and real-world data for training robust and effective robot policies.
The LSTM model provided a good balance of performance and training efficiency, whereas GRU models exhibited slightly lower performance but benefited from reduced computational overhead. Transformer-based models demonstrated considerable potential for handling more complex tasks; however, they demanded significantly more computational resources (see Fig. <ref>). 
r0.38


    
    < g r a p h i c s >

    

    Results of Q2 evaluation.
    

    

Upon comparing the results, we identified that the optimal ratio of simulated to real data was 70% simulated and 30% real. This ratio offered the best generalization and performance in real-world tasks, enhancing the task success rate across all evaluated tasks. Specifically, for bottle collecting, the task success rate increased from 80% to 90%; for stacking Pringles, the success rate rose from 60% to 80% (see Fig. <ref>); and for hammering, the success rate improved from 50% to 70%. This balanced approach effectively mitigated the sim-to-real gap, thereby enhancing the robustness and reliability of the learned policies.

Q3: Effectiveness of Human-in-the-Loop Corrections:
In this round of experiments, we investigated the effectiveness of human-in-the-loop correction in scenarios when the policy failed during task execution. We recorded 5 failure experiments for each task in the simulation. We then replayed the bag file and allowed the human user to correct the robot’s movements using residual learning or full teleoperation mode. The updated demonstrations were tested to ensure they could successfully accomplish the task. These corrected demonstrations were then augmented 
r0.38


    
    < g r a p h i c s >

     

    Results of Q3 evaluation.
    

    

and used for policy fine-tuning. By comparing the results, we observed that human-in-the-loop corrections significantly improved task success rates, particularly for more complex tasks such as stacking and hammering. 

The inclusion of human corrections from dataset D', resulted in an average improvement of approximately 5% in task success rates, demonstrating the value of interactive learning. Specifically, the success rate for the hammering task increased from 78% to 86%, for the stacking task from 84% to 89%, and for the bottle collecting task from 92% to 95%. These results indicate that the policy was effectively learning from the corrections, as the number of correction feedback by the human user decreased over time. 

 

r0.38
    

    
    < g r a p h i c s >

    An example of Setting a Drink Tray task: (left) initial and (right) desired objects pose.
    

    

Q4: Capability to Execute A New Complex Bimanual Task: Setting a Drink Tray
To evaluate the generalization capabilities of our framework, we tested its ability to execute a new bimanual task that shares similarities with the learned tasks: setting a drink tray (see Fig. <ref>). This task involves placing a cup and a bottle on specific markers on a tray. The tray has two circular markers: one red for placing the cup and one blue for placing the bottle. The robot must pick up a cup and a bottle from a starting position and place them accurately on the designated red and blue markers on the tray. The policy was initially trained on the previously learned object stacking task. The trained policy was then evaluated only in simulation, as we could obtain precise positions of the markers and objects. The task was considered a success if the robot accurately placed the cup and bottle on the respective markers and a failure if the distance between the object and the marker exceeded 3 cm. We repeated this experiment 
100 times, with the objects placed in random positions each time. The robot achieved a task success rate of 87%. The primary source of error in the failure cases was slight misalignment during grasping, which led to minor placement inaccuracies, despite accurate trajectory predictions. Our approach successfully generalized to the new task, demonstrating that the learned policies could be adapted to similar but distinct tasks with minimal effort. This showcases the robustness and adaptability of our teleoperation and learning framework in handling a variety of real-world scenarios. We believe that incorporating feedback mechanisms through vision, such as detecting the exact positions of the markers and adjusting the trajectory accordingly, could further enhance the accuracy and reliability of task execution.





§ CONCLUSION AND LIMITATIONS



In this work, we presented a low-cost visual teleoperation system for bimanual manipulation tasks. Our approach leverages an RGB-D camera and visual processing techniques to collect demonstration data, which is then augmented and used to train robot policies. We employed a combination of simulated and real-world data, augmented with human-in-the-loop corrections, to improve the accuracy and reliability of the learned policies. Our system was evaluated on three tasks with differing complexity levels, demonstrating its effectiveness in both simulated and real-robot environments. The experimental results validate the effectiveness of our approach in learning robust policies from simulated data, enhanced by human-in-the-loop corrections, and real-world data integration. The framework successfully generalized to a new, complex task —setting a drink tray—demonstrating the adaptability of the learned policies to similar tasks with minimal effort. 

The main limitation of this work is that for tasks requiring a high degree of precision at certain steps (e.g., staking, hammering, etc), waypoint trajectory learning may not be sufficient, and (closed-loop) vision feedback should be considered. To address this limitation, we plan to explore the integration of visual feedback mechanisms to further enhance the accuracy and reliability of task execution. By incorporating real-time visual feedback, the system can adjust and refine its actions based on the visual context, leading to more precise and robust manipulation capabilities. 








  





§ APPENDIX






§ IMPACT OF MODEL ARCHITECTURE AND DATA COMPOSITION (Q2)




















   


































 §.§ Training data composition

We trained several policies using different ratios of simulated to real-robot demonstrations, including 70%-30%, 50%-50%, 30%-70%, and 0-100% mixes. For each task, we recorded five real-world demonstrations and five simulation demonstrations, which were then augmented to create a larger dataset. The final training data was sampled from these augmented demonstrations according to the specified ratios. This approach allowed us to determine the optimal balance between simulation and real-world data for training robust and effective robot policies.







§ VISUALIZATION OF GROUND-TRUTH AND PREDICTED TRAJECTORIES 
 


We learned a policy using a single-shot demonstration and tested its performance on the validation set of the first subtask of the bottle-collecting task. Specifically, we plot 2D/3D visualization of predicted and actual trajectories for two randomly selected samples from the validation set. The network's performance is depicted through both 2D and 3D plots in Figure <ref>. Furthermore, we tested the performance of the policy on the digital twin of the robot by randomly placing a bottle object in front of the robot, retrieving the current pose of the end effector and the pose of the object, and forwarding them as input to the learned policy. We then executed the predicted trajectory and visualized the robot's performance. 

Figure <ref> shows the performance of the robot in three experiments, highlighting its ability to follow the predicted trajectory and accurately grasp the object based on the learned policy. The visualizations include both images of the robotic arm and plots of the predicted versus actual trajectories for the X, Y, and Z axes.



§ EXPERIMENTS



 §.§ Success and failure criteria for the proposed tasks


r0.35


 
    < g r a p h i c s >

    Sucesses and failures for stacking and hammering tasks.
    
    



For the collecting objects task, success is determined by placing all objects into the basket and failure is considered if one of the objects is not placed into the basket and remains outside. Figure <ref> shows examples of successful (left) and failed (right) attempts for the stacking and hammering tasks. In the stacking task, success is achieved when objects are stacked stably. In the hammering task, success criteria are based on hitting the target object accurately.  These criteria help in evaluating the performance and reliability of the robotic system in executing these tasks. In the case of setting up a drink tray task, success is considered when the robot places all items (such as a mug and a bottle) accurately on their designated spots on the tray, and failure is defined when items are not placed correctly on the tray, or the robot fails to place all items.






 §.§ Performance of the robot in simulation: known scenarios - known objects

We show the performance of the robot in the simulated environment where the robot interacts with known objects across known scenarios in Figure <ref>. The tasks include collecting bottles (top-row), stacking objects (second-row), and hammering (lower-row), all using the same objects from the collecting demonstration. The robot's ability to follow a predicted trajectory and complete these tasks accurately showcases the robustness of the learned policy.






 §.§ Performance of the robot: known scenario - new objects


In this section, we test the robot's ability to adapt to new objects in a familiar scenario. The experiment involves using bottles that differ in weight, size, and shape from the ones used in the initial demonstration. This assessment helps determine the robot's generalization capability when faced with varying object properties while performing the same task. Despite the differences in the object's physical characteristics, the robot successfully follows the predicted trajectory and completes the collection task. The sequence of snapshots in Figure <ref> demonstrates the robot's generalization ability to complete the collecting objects task with new objects.






 §.§ Performance of the robot in simulation: new scenario - new objects


We also evaluate the robot's performance in a completely new scenario using new objects. The experiment involves setting up a drink tray with various items, testing the robot's ability to adapt to a new task and handle unfamiliar objects. For this task, we utilized the stacking object policy and adjusted the subtask trajectory end poses to accomplish the task successfully.
Figure <ref> shows the sequence of snapshots detailing the robot's performance in setting up a drink tray. This image highlights the robot's capacity to generalize from its training and successfully perform tasks in new scenarios with new objects.






































 §.§ Perception of our real robot experiments

To discuss our real robot experiments, we provide an example of our experiment setup for the collecting bottles task (Fig. <ref>), for the stacking objects task (Fig. <ref>), and for the hammering task (Fig. <ref>). The robot's perception system is capable of identifying and localizing objects in its environment, which enables it to execute the task accurately.

The experiment demonstrates the integration of perception and action, where the robot uses its sensory inputs to detect various objects to guide its movements and complete the task of bottle collection.










 §.§ Failure cases during real robot experiments


During the real robot experiments, several failure cases were observed that can be attributed to the differences between simulation and real-world conditions (See Fig. <ref>). These failures provide valuable insights into the challenges of transferring learned policies from simulation to real robots. The main factors contributing to these failures include:

Gap between simulation and real robot: The objects used during the experiments in the real world differ in shape, size, weight, and material properties compared to those used in the simulation. These discrepancies can affect the robot's ability to accurately predict and execute the required trajectories, leading to unsuccessful task completion.


Inverse kinematics solver and command frequency: The inverse kinematics (IK) solver and the frequency of sending commands to the robot differ between the simulation and the real robot. In the simulation, the IK solver and command frequency are typically more idealized and may operate at higher rates, leading to smoother motions. In contrast, the real robot's controller might introduce slight delays due to various factors (e.g., network loads), causing deviations in the robot's movement and impacting task execution.

Trajectory prediction errors: Inaccuracies in the predicted trajectory can lead to the robot's end-effector being misaligned with the object, preventing successful grasping and manipulation. Poor grasping or inadequate grip strength can cause objects to slip during manipulation. 

Execution of actions: Using an open-loop controller during the execution of actions means that the robot follows the predicted trajectories blindly without feedback corrections. In real-world scenarios, this lack of feedback can lead to accumulated errors and deviations from the intended path, causing failures in task completion. In such an open-loop scenario, accurate calibration between the robot's camera and its manipulator is crucial and calibration errors can result in incorrect positioning and unsuccessful task execution.


These factors highlight the importance of addressing the sim-to-real gap and improving the robustness of robotic systems when transitioning from simulation to real-world applications. Future work should focus on integrating closed-loop feedback controllers to mitigate some of these issues and achieve more reliable performance in real-world scenarios.






 Paper Title: CrystalTac: 3D-Printed Vision-Based Tactile Sensor Family through Rapid Monolithic Manufacturing Technique.



: Augmented Masked Autoencoder Pretraining on Public Brain MRI Data for 3D-Native Segmentation
    Asbjørn Munk1,2* Jakob Ambsdorf1,2* Sebastian Llambias1,2 Mads Nielsen1,2

    August 5, 2024
==============================================================================================




§ ABSTRACT 





Recently, vision-based tactile sensors (VBTSs) have gained popularity in robotics systems. 
The sensing mechanisms of most VBTSs can be categorised based on the type of tactile features they capture.  Each category requires specific structural designs to convert physical contact into optical information. The complex architectures of VBTSs pose challenges for traditional manufacturing techniques in terms of design flexibility, cost-effectiveness, and quality stability. Previous research has shown that monolithic manufacturing using multi-material 3D printing technology can partially address these challenges. This study introduces the CrystalTac family, a series of VBTSs designed with a unique sensing mechanism and fabricated through rapid monolithic manufacturing. Case studies on CrystalTac-type sensors demonstrate their effective performance in tasks involving tactile perception, along with impressive cost-effectiveness and design flexibility. The CrystalTac family aims to highlight the potential of monolithic manufacturing in VBTS development and inspire further research in tactile sensing and manipulation.










§ INTRODUCTION



The integration of tactile sensors with robots has drawn considerable interest in fields ranging from soft robotics <cit.>, bionic robotics <cit.>, to human-robot interface areas <cit.>. Among the different types of tactile sensors <cit.>, vision-based tactile sensors (VBTSs) <cit.> use cameras to record the physical deformation when interacting with external objects. This method has gained increasing attention due to its superior spatial resolution on tactile sensing and relatively simple structure. The rapid adoption of VBTSs is largely due to advancements in computer vision. Vision sensors, such as RGB cameras, project a 3D scene into 2D frames, converting the colour, shape, and motion information of the external environment into a distribution of pixel values. In comparison, VBTSs operate similarly to reprographic devices, scanning and mapping 2.5D features <cit.> into 2D images. These features, such as the texture geometry of the contact surface or shear force around the touched area, are termed 2.5D because their perception depth is confined to a limited range, and cannot be extended arbitrarily along the direction of light projection as 3D visual features can. Due to this limitation, VBTSs need a specific medium to convert physical information into optical signals that the camera can detect. The realisation of such a medium generally involves four steps: 
(1) establishing the correlation between tactile and optical features, often known as the tactile sensing mechanism; (2) creating the sensor architecture to embody this tactile sensing mechanism; (3) choosing appropriate techniques to manufacture the sensor's sub-components; and (4) finalizing the entire hardware assembly process based on the produced sub-components. In conclusion, the initial two steps fall under the `design' phase of VBTSs, while the final two steps comprise the `creation' phase. The design phase frequently imposes difficulties on the creation phase. This is due to the conventional method's segregation of design, manufacturing, and assembly stages. Nevertheless, in real-world applications, these stages are interconnected and must be meticulously coordinated <cit.>.








Among the three primary modules in VBTSs <cit.>-illumination and vision modules-these are typically procured from commercial electronic component suppliers. In contrast, the contact module presents the most significant challenge <cit.> (the subsequent discussion of VBTS manufacturing in this paper primarily refers to the fabrication of contact module). A VBTS contact module normally consists of six main components: the base, lens, elastomer, marker, coating, and skin. Although different types of VBTS may share similar core components, they are often produced using diverse manufacturing techniques <cit.>. This multi-dimensional variation significantly influences the process from conceptual design to final assembly. As analysed in <cit.>, the main challenges in VBTS manufacturing include complexities in design, process, time, and quality. Here, we briefly discuss how these four challenges impact the design and creation of VBTSs:


i) Design: VBTSs' structural framework and detailed constructions vary significantly, depending on the tactile sensing mechanism employed. This diversity presents a bidirectional challenge: (1) alterations in the sensor design may have a direct impact on the subsequent manufacturing stages, and (2) the constraints of existing manufacturing techniques can limit the initial sensor design.



ii) Creation: The creation of VBTSs is generally complex and involves several sub-components. The increase of manufacturing procedures necessitates more equipment, leading to substantial process and time complexities. Additionally, reliance on manual fabrication and assembly means the quality of the sensors depends on the specialised skills of the workforce. This results in a high incidence of assembly errors and variability in sensor performance.




Previous research has focused on the design and fabrication of a specific type of VBTSs <cit.>.  In <cit.>, monolithic manufacturing technology was proposed to fabricate the C-Sight tactile sensor, which leverages advancements in multi-material 3D printing. By simplifying the VBTS manufacturing into a single printing sequence, this method accelerates production and ensures greater consistency and integration of the sub-components. It is expected to substantially improve the reliability, productivity, and affordability of the newly developed tactile sensor. However, previous work of C-Sight is limited to a single kind of sensing mechanism. To better explore the potential of this technology, we summarise the mainstream VBTSs into 5 categories based on their sensing principles and then propose to develop CrystalTac-type sensor following this categorisation, also known as CrystalTac family, aiming to demonstrate the versatility of monolithic manufacturing.

The name 'CrystalTac' draws on the properties of the crystal, a mineral known for its clarity and variability in colour and texture when combined with other minerals, just like the versatile structure of the different tactile sensing mechanism-based VBTSs. The monolithic manufacturing technology should be competent for the fabrication of the CrystalTac family by adapting to different design needs, including customised sensing principles, overall dimensions, and architectural details. 

The main contributions of this paper are listed as follows:



  * We summarised the design and creation methods of known VBTSs and proposed a new categorisation method to encapsulate their typical sensing mechanisms, including IMM, MDM, MFM, and multi-mechanism fusion.


  * We developed the CrystalTac family, a series of sensors produced through monolithic manufacturing. This family includes C-Tac, C-Sight, C-SighTac, Vi-C-Tac, and Vi-C-Sight, each design based on a unique tactile sensing mechanism.



  * We conducted functional experiments to evaluate the sensing performance, cost-effectiveness, and design flexibility of CrystalTac-type sensors. Also, an optimisation upon sub-component manufacturing and several novel marker designs are introduced.




















§ BACKGROUND


Based on the four key steps for realising the `design' and `creation' of VBTSs introduced in the previous section, we review most of the known VBTSs and summarise their sensing mechanisms, manufacturing processes, and assembly methods. 




 §.§ Typical Sensing Mechanism of Known VBTSs


As shown in Fig. <ref>, the sensing mechanisms of most VBTSs <cit.> can be categorised into several distinct methods: Intensity Mapping Method (IMM), Marker Displacement Method (MDM), Modality Fusion Method (MFM), and multi-mechanism fusion. 



  §.§.§ IMM



The Intensity Mapping Method uses the pixel intensity values from the imaging frames to indicate tactile features, as illustrated in Fig. <ref>(A).
This approach leverages the high resolution of the VBTS camera frame and the changes in each pixel value over a threshold interval to reconstruct continuous features, including fine geometric textures and dense motion distributions. This change in pixel value requires the VBTS to incorporate specific structures, such as a light-conductive plate or a reflective coating. For example, the earlier optical waveguide-type sensors <cit.> usually rely on the total internal reflective (TIR) <cit.> to map the contact pattern located on the light-conductive plate surface. GelSight-type sensors <cit.>, such as OmniTact <cit.>, Digit <cit.>, GelTip <cit.>, InSight <cit.>, and Tactile Fingertip <cit.>, employ a coating layer with uniform reflective properties applied to a silicone elastomer, enabling pixel-level tactile sensing. By incorporating RGB lighting, GelSight captures multiple images under different lighting conditions within a single frame, facilitating the estimation of the overconstrained gradient of the contact surface geometry through the photometric stereo algorithm <cit.>. In contrast to intensity mapping via a light-conductive plate or a reflective coating, DTac-type sensors <cit.>, such as 9DTac <cit.>, employ a combination of translucent gel and an opaque layer, where variations in pixel darkness can indicate intensity-to-depth regressions or even estimate force/torque. Furthermore, if a depth camera is used instead of a traditional RGB camera in VBTS, each pixel intensity value will also include real physical distance information in addition to colour information. <cit.> integrates a latex inflatable film with a depth camera, achieving precise deformation measurements through stereo vision.







  §.§.§ MDM

The Marker Displacement Method, depicted in Fig. <ref>(B), translates tactile information into displacement distributions of marker patterns either positioned on the surface or embedded within the elastomer. In <cit.>, MDM is classified into 2D-MDM, 2.5D-MDM, and 3D-MDM, based on the type of tactile features represented by the pattern changes. Both 2D-MDM and 2.5D-MDM employ a single camera to capture the two-dimensional movements of markers in horizontal and vertical directions. However, 2.5D-MDM can indirectly represent corresponding contact depth information by observing the deformable characteristics of the markers, such as changes in size or shape. In contrast, 3D-MDM precisely captures the depth of markers using stereo vision. GelForce <cit.> uses two layers of spherical marker arrays to measure 3D force distributions. Inspired by the dermal papillae in the human fingertip, TacTip <cit.> designs pin-shaped markers, which amplify contact information through leverage principles. Furthermore, Yang et al.<cit.> employ marker displacements to estimate the gradient of the contact surface through the integration method. ChromaTouch <cit.> introduces two superimposed colour filters as a composite marker pattern, where contact deformation alters the markers' hue, centroid, and apparent size. Similar designs are found in <cit.>. DelTact <cit.>, Viko <cit.>, and <cit.> all utilise a dense random colour pattern for precise tracking of contact deformations. Sferrazza et al.<cit.> randomly embed spherical fluorescent green markers within an elastomer to enhance the sensing range. A multi-colour continuous marker pattern (CMP) <cit.> has been developed to improve the representation and extraction of VBTS contact information. Tac3D <cit.> employs virtual binocular vision to precisely measure the three-dimensional shape and force distribution of the contact surface using CMP.





  §.§.§ MFM

The Modality Fusion Method integrates multi-modal features beyond sole tactile input, typically incorporating vision and proximity sensing. This integration effectively mitigates the inherent limitations of a single tactile modality, such as the inability to perceive features like colour and distance. However, MFM cannot operate independently in VBTSs and needs to be combined with IMM or MDM for tactile sensing, thereby included in the concept of multi-mechanism fusion.




  §.§.§ Multi-Mechanism Fusion

The sensing mechanisms for VBTS introduced above each offer distinct advantages. However, researchers have been proactively developing multi-mechanism fusion sensors to further improve the performance of VBTSs by obtaining comprehensive sensing information.





  * IMM + MDM: GelSight-type sensors <cit.> incorporate dot markers above the reflective coating layer, aiming for force and shear detection rather than solely capturing fine textures, as demonstrated by GelSlim <cit.>, shown in Fig. <ref>(C), GelSight Wedge <cit.>, and GelStereo <cit.>. DenseTact <cit.> employs a randomised dense pattern with a coating to extract continuous tactile features. The Soft-bubble sensor<cit.> utilises a stereo camera for the tracking of shear-induced displacement through custom marker patterns. <cit.> introduced Particle Image Velocimetry (PIV) to establish a linear relationship between force and velocity. UVtac <cit.> employs a switchable ultraviolet (UV) method to decouple marker patterns and reflective membrane images, facilitating both object localisation and force estimation, with each functionality remaining independent of the other.







  * MDM + MFM: Finger Vision <cit.>, as illustrated in Fig. <ref>(D), embeds black marker patterns on a transparent elastomer to achieve multi-modality sensing. The approach proposed by <cit.> increases force resolution and sensitivity of Finger Vision by introducing whiskers. ViTacTip <cit.> also employs a transparent skin to enhance TacTip <cit.> with vision-tactile fusion capability. Similar to UVtac, SpecTac <cit.> uses UV LEDs and randomly distributed UV fluorescent markers, allowing for a switch between visual and tactile sensing modes, controlled by the activation of the UV LEDs.








  * IMM + MFM: Shimonomura et al.<cit.> employ a light-conductive plate and compound-eye camera to capture both tactile and proximity modality information. The tactile feature is obtained as an infrared image through the TIR principle, while the proximity feature is detected through stereo matching of a pair of images obtained by two cameras. See-Through-your-Skin (STS) <cit.> utilises a translucent coating and adjustable lighting to enable the transition between tactile and visual sensing modalities. Compared to STS, StereoTac <cit.> and VisTac <cit.> incorporate binocular cameras, extending 2D visual sensing into 3D areas. As displayed in Fig. <ref>(E), TIRgel <cit.> also utilises TIR through a purely transparent elastomer to visualise tactile features, achieving conversion between visual and tactile modalities via focus adjustment.







 










 §.§ Typical Manufacturing Method of Known VBTSs



Based on Table <ref>, the manufacturing methods of known VBTSs's sub-components are discussed below. The different tactile sensing mechanisms lead to variations in their manufacturing processes, where multi-mechanism fusion further complicates the procedure.







  §.§.§ Base



The base, also known as the case, mount, frame, body, bracket, or housing, typically serves as the external structure of VBTSs. It acts as the connecting element between the contact module and the illumination/vision modules. To ensure a secure fit among these modules and to achieve the required durability and compactness of the sensors, the base must be designed with an appropriate shape and constructed from a material of sufficient rigidity. 3D printing technology <cit.> is the most common option for prototyping. While commercially large-scale productions usually rely on the mould-forming method <cit.>.






  §.§.§ Lens



The lens allows the camera to capture tactile information without obstruction. Additionally, it internally supports the elastomer during interactions. The common manufacturing method employs a laser cutter to shape the acrylic board <cit.>. However, lenses with complex curved surfaces, diverging from simple planar shapes, can only be produced through mould-forming methods <cit.> or procured from commercial suppliers <cit.>. Additionally, some sensors <cit.> do not incorporate lenses but rely on their inherent structure or an internal skeleton.







  §.§.§ Elastomer



The elastomer serves as one of the primary mediums for converting tactile information into visual data, with core properties including transparency, colour, and stiffness. Mould-formed silicone <cit.> is the material most preferred for creating these elastomers, as it offers adjustable properties to suit various applications <cit.>. The preparation process involves  A/B solutions mixing, mould casting, bubble removal, and heat curing. Translucent and coloured silicone may require dyeing with pigment <cit.>. The adjustable stiffness depends on the variation of the ratio of mixed solutions <cit.>. Some VBTSs use commercial products as alternatives, modified using DIY methods <cit.>, to simplify manufacturing. There are exceptions, such as Soft-bubble <cit.>, which features an air-filled membrane design, while TacTip <cit.>, BioTacTip <cit.> and <cit.> use mixed ultra-soft gel (TechsiL RTA27905 A/B) and coloured water respectively.





  §.§.§ Marker


For MDM-type VBTSs, markers play the role of mapping tactile deformation to visual pattern distribution. Inspired by <cit.>, we classify the marker manufacturing methods into three types:




  * Surface Fabrication: Markers are prepared on the surface of premade elastomers through direct or indirect methods. The former approach combines light etching, mask templates, and stamp plates with ink printing <cit.>, or utilises plastic beads <cit.>. Similarly, SpecTac <cit.> and <cit.> manually apply fluorescent markers using a brush and UV pen, respectively. In the latter case, the complete marker patterns are printed in advance using materials such as sticker film <cit.>, transfer paper film <cit.>, or adhesive-backed templates <cit.>, and then applied to the surface of the elastomer.



  * Embedded Fabrication: To capture the deformation field at different depths, rather than solely at the contact plane, markers are embedded within the elastomer. <cit.> incorporated fluorescent markers into the solutions during the preparation of the silicone. GelForce <cit.> and <cit.> placed two marker arrays of different colours in elastomer, prepared layer by layer.




  * Integral Fabrication: 

Markers are prepared integrally with the skin in a single piece. The pin-shaped markers with the skin of TacTip were initially produced by mould casting <cit.>. Subsequently, 3D printing has been introduced for the later versions of  MDM-type VBTSs <cit.>.









  §.§.§ Skin



The skin serves as the direct contact interface with the external environment. For VBTSs designed with silicone elastomer as the main body, some lack a skin layer <cit.>, while others employ a skin for protection, which is achieved by casting a thin layer of silicone through mould-forming <cit.> or spray painting <cit.>. To filter noise, DTac <cit.> manually applies black silicone on the top surface, similar to <cit.>. Several designs implement fabric films or adhesive tape films as a skin-like layer, as demonstrated in <cit.>. For example, Finger Vision <cit.> employs a thin transparent plastic film to protect the silicone body from contaminants. Some VBTSs rely on the skin as the primary structural component. TacTip <cit.> and ChromaTouch <cit.> build the skin as the main body, with the latter also requiring the casting of a white silicone layer to filter noise. Furthermore, soft bubbles <cit.> and <cit.> both use hand-cut latex film as an outer skin for an air-filled structure.








  §.§.§ Coating
 


Coating layers are widely used in IMM-VBTSs. Unlike the skin, which is primarily intended for protection and shading, coatings are designed to convert tactile features into visual imaging. These can be categorised into two broad types: reflective coatings and controllable coatings.




  * Reflective Coating: GelSight-type sensors <cit.> incorporate metal pigments into silicone, using bronze flakes for semi-specular coatings and fine aluminum powder for matte coatings. Various manufacturing methods are employed, such as brush painting <cit.>, airbrushing <cit.>, sputtering <cit.>, and the paint dipping technique <cit.>. Similarly, <cit.> uses metal foil to create a semi-mirror coating through the gilding process.



  * Controllable Coating: Some MFM-based VBTSs control the coating transparency by adjusting internal lighting conditions. STS <cit.>, StereoTac <cit.>, and VisTac <cit.> all apply 2-3 layers of `mirror spray' to achieve a translucent layer as the controllable coating.














 §.§ Typical Assembly Method of Known VBTSs



The VBTS assembly strategy is often overlooked in most of the current research. Here, the assembly process is analyzed across three dimensions: tools, workflows, and mechanisms.





  §.§.§ Assembly Tool
 

The assembly tool consists of manual assembly and machine assembly. Due to the widespread use of traditional manufacturing methods, such as mould-forming <cit.>, manual assembly remains the prevalent practice. Machine assembly, characterised by the use of specific devices such as multi-material 3D printers, is exemplified by TacTip <cit.> and ChromaTouch <cit.>. Generally, manual assembly may result in assembly errors, while machine assembly necessitates specific hardware equipment. Most VBTSs are assembled manually or in hybrid form, with a limited number being fully assembled by machines, such as MagicTac <cit.>.





  §.§.§ Assembly Workflow
 


The assembly workflow consists of serial assembly and parallel assembly. Serial assembly involves combining components in a specific sequential order, typically determined by structural design or manufacturing requirements. For example, 9DTac <cit.> consists of a transparent layer, a translucent layer, and a black layer, thereby asking for a fixed assembly order due to its stacked construction. In contrast, parallel assembly allows components to be assembled simultaneously, as seen in <cit.>, which achieves this due to its simple structure. Although parallel assembly is more efficient, it presents challenges in the management of sensors requiring complex processes and intricate structures. The majority of VBTSs are fabricated using serial assembly, while a minority employ a hybrid of serial and parallel assembly. Purely parallel assembly is rarely used.






  §.§.§ Assembly Mechanism
 


The assembly mechanism consists of physical assembly and chemical assembly. Physical assembly relies solely on the interaction of hardware structures. For instance, <cit.> involves casting silicone into the base mould, thereby directly assembling the elastomer with the base. Chemical assembly utilises the properties of chemical reagents, such as material compatibility and adhesion. For example, coating pigments are often mixed with silicone to enhance adhesion with the elastomer, and lenses are frequently attached to the base using adhesives. Although physical assembly offers the benefits of convenience and is well-suited for modular designs, chemical assembly excels in ensuring enhanced durability and stability. Most VBTSs are produced through a hybrid of physical and chemical assembly, with only a few being made entirely through physical or chemical assembly.





In this section, we review the commonly used methods in the design and creation of VBTS, whose representative attributes can be summarised in Fig. <ref>. Each VBTS has several attributes with distinct characteristics, such as size, colour, stiffness, and manufacturing methods. This complexity corresponds to a wide range of structural variations, placing high demands on subsequent manufacturing and assembly processes. The complexities in process, time, and quality are difficult to address due to the limitations of traditional manufacturing methods. In <cit.>, a rapid monolithic manufacturing technique has been proposed which has the potential to become a standard method to simplify the design and creation of VBTS. Its feasibility has been evaluated by C-Sight, which relies on IMM. In this work, the CrystalTac family is designed as a series of sensors with different sensing mechanisms to demonstrate the adaptability of monolithic manufacturing.









§ DESIGN AND CREATION



Based on the details of monolithic manufacturing elaborated in <cit.>, this work focuses on the design and creation of the CrystalTac family. The first step involves discussing the manufacturing feasibility of CrystalTac-type sensors based on different sensing mechanisms through monolithic manufacturing, followed by the overall design of the CrystalTac family.




 §.§ Manufacturing Feasibility of Different Sensing Mechanism-based CrystalTac


From Table <ref> and Fig. <ref>, it is evident that different sensing mechanism-based VBTSs exhibit certain tendencies in terms of sub-components, each associated with specific attributes and corresponding manufacturing methods. In detail, the choice of sensing mechanism directly determines the type of sub-component, while the specific sensor design based on that mechanism influences the attributes of each sub-component. The most striking issue is the significant variation in size, colour, and stiffness requirements for the various sub-components. Here, we discuss which sensing mechanism could be applied to CrystalTac-type sensors in terms of technological and practical feasibility.






  §.§.§ Technological Feasibility



As utilised in monolithic manufacturing, PolyJet Printing (PP) <cit.> functions similarly to inkjet printing by spraying thousands of photopolymer droplets rather than ink. This process employs ultraviolet (UV) light to cure and construct parts in a layer-by-layer fashion. The combination of inkjet and photopolymerization technologies provides PP with two significant advantages:








  * High Printing Quality: Due to the small size of the ejected resin droplets, impressive micron-level spatial resolution can be achieved in both the horizontal XY-direction and vertical Z-direction. This level of precision is essential for achieving high print quality, characterised by fine resolution and a superior surface finish.


  * Multi-material Printing: By integrating multiple print heads, pp easily achieves multi-material printing with consistent print quality, whose printing capabilities encompass a wide variety of material properties. In addition to multi-colour printing, it can also produce flexible materials with varying properties.






These two advantages have considerable significance in manufacturing different sensing mechanism-based CrystalTac with complicated structure, as shown in Fig. <ref>(B-D). Referring to the typical print materials listed in <cit.>, Vero series (VB/VW/VC)[https://www.stratasys.com/en/materials/materials-catalog/polyjet-materials/vero/], Agilus30 series (AB/AW/AC)[https://www.stratasys.com/en/materials/materials-catalog/polyjet-materials/agilus30/], and support materials[https://www.stratasys.com/en/materials/materials-catalog/polyjet-materials/polyjet-support-materials/], are suitable selections for the skin, marker, elastomer, lens, and base of CrystalTac. However, due to the lack of print materials containing metal powder, reflective or controllable coatings are currently unavailable, rendering some IMM-VBTSs based on these coatings, such as GelSight, not yet feasible for CrystalTac.












  §.§.§ Practical Feasibility


As displayed in Fig. <ref>(A), the practical feasibility depends on the suitable design of each sensor, as well as the combination and assignment setups of loaded print materials. 






  * Suitable Sensor Design: As illustrated in Fig. <ref>(A.a1), the structural design of CrystalTac must be based on a defined mechanism. For example, MDM-CrystalTac requires the marker pattern to be embedded in the correct position, MFM-CrystalTac often requires minimal occlusion within the imaging path, and IMM-CrystalTac may need a multi-layer stacked structure. An irrational structural design can fail to achieve the desired sensing mechanism. However, the efficiency and flexibility of monolithic manufacturing can help researchers rapidly implement hardware iterations of a prototype design by controlling variables.



  * Combination Setup of Print Material: It should be noted that the print materials loaded in the feed box are limited, and the number of print heads is also fixed by the printer version, as shown in Fig. <ref>(A.a2). Therefore, it is necessary to consider the actual material loading situation of the current printer, especially with multi-mechanism fusion designs which require a more complex range of print materials. In general, the minimum criteria to be fulfilled include at least three print materials, of which AC, VW/VB, and support materials must be loaded. If there are spare print heads available, priority should be given to loading AW/AB or VC. Such strategies can maximise the design options for CrystalTac with limited material combinations.






  * Assignment Setup of Print Material: After determining the sensor design, it is necessary to assign the appropriate material to each sub-component based on different attributes as shown in Fig. <ref>(A.a3), considering various factors such as colour, stiffness, and shape. For example, MFM-CrystalTac requires high transparency and low distortion levels in the skin, while MDM-CrystalTac requires effective shading. It should be emphasised that although the number of print materials loaded is limited, their properties can be adjusted through fusion with each other. For instance, by mixing flexible Agilus30 materials into the rigid Vero series, materials with intermediate stiffness can be obtained, while Vero Vivid[https://www.stratasys.com/en/materials/materials-catalog/polyjet-materials/verovivid/] can be combined with a wide range of materials to achieve rich colours. These characteristics can be adjusted by changing the mixing ratio, and the entire process is automated by the printer, greatly enhancing design flexibility and ensuring production quality.







The above analysis demonstrates that monolithic manufacturing can provide sufficient capability to create the CrystalTac family with different sensing mechanisms but given certain prerequisites. As seen in Supplementary <ref>, taking into account the optimisation for key sub-components of CrystalTac, a thickness range of 2-3 mm is optimal for printed lenses, offering a balance between high structural strength and satisfactory imaging performance. Similarly, the suitable height for the printed elastomer should range between 2 and 5 mm to balance image quality and softness.
















 §.§ Overal Design of CrystalTac Family



The CrystalTac family design is introduced here, whose family tree is illustrated in Fig. <ref>. According toFig. <ref>(A), the CrystalTac family is categorised into five branches, the C-Sight using IMM mechanism, the C-Tac using MDM mechanism, the C-SighTac using IMM + MDM mechanism, the Vi-C-Sight using IMM + MFM mechanism and the Vi-C-Tac using MDM + MFM mechanism. It should be emphasised that these five CrystalTac sensors are proposed not to provide a final design version, but rather to validate its potential to implement various tactile sensing mechanisms.








  §.§.§ Contact Module







  * C-Sight: To realise the IMM mechanism, C-Sight has been designed, as described in <cit.>, inspired by DTac <cit.>. The sectional diagram in Fig. <ref>(A) shows that within the gap between C-Sight's outer skin (a6) and the clear elastomer (a3), two additional components are present: a translucent layer (a5) composed of support material and a pure white layer (a4) made of Agilus30 White. The deformation resulting from external contact alters the distance between the black skin and the white layer. Where the distance is shortened, the pixel intensity appears darker, aiding in inferring the contact depth.





  * C-Tac: To achieve the MDM mechanism, C-Tac can be embedded with 2D and 2.5D markers through different material assignments as shown in Fig. <ref>(B), such as rigid Vero and flexible Agilus30 series. The distinction between them lies in the ability to provide pseudo-depth information, achieved either through shape deformation or variations in morphological structure. Monolithic manufacturing allows for customisation of both the stiffness properties and the morphological geometry of the markers. As shown in Supplementary <ref>, three kinds of marker patterns can be employed for C-Tac, including multi-layer markers, Voronoi markers and coordinate markers, each possessing distinct characteristics compared to dot markers.





  * C-SighTac: As illustrated in Fig. <ref>(C), C-SighTac is developed based on C-Sight and C-Tac to achieve the IMM+MDM mechanism. This approach involves the structural framework of C-Sight and marker designs from C-Tac into appropriate positions. Similar to marker-enhanced GelSight <cit.>, markers may enable C-SighTac more sensitive to dynamic features, such as force.




  * Vi-C-Tac: Inspired by Finger Vision <cit.> and ViTacTip <cit.>, Vi-C-Tac uses transparent skin to replace the opaque one of C-Tac as displayed in Fig. <ref>(d), aiming to realise MDM+MFM mechanism. The internally embedded markers provide greater dynamic sensitivity to physical interaction while retaining the multi-modality sensing capability through the clear skin.








  * Vi-C-Sight: Vi-C-Sight utilises IMM+MFM mechanism, inspired by <cit.>. Using TIR principle, variations in the gradient of the elastomer surface upon the contacted object alter the trajectory of internally reflected light beams. This causes the contact texture to be mapped onto the transparent elastomer surface, resulting in imaging brightness change while still retaining background visual information. As seen in Fig. <ref>(E), Vi-C-Sight uses pure Agilus30 Clear as the elastomer material to leverage this mechanism rather than a multi-layer grid structure.













  §.§.§ Vision and Illumination Modules



To demonstrate CrystalTac's customisation flexibility for prototype designs and its adaptability for modifying mature designs, two bases with different vision and illumination modules are applied to the five proposed sensors. The first customised base is designed for C-Sight and C-SighTac, characterised by a square body with six internal white illumination sources, as detailed in <cit.>. The other base is from the well-known Digit <cit.>, which features a small black curved housing with an RGB light source. It is a popular commercial VBTS product due to its suitability for mounting on robot hands and its quick-change contact module design. So we used it as the base for C-Tac, Vi-C-Tac, and Vi-C-Sight. A description of these two bases is provided in Supplementary <ref>.










§ PERFORMANCE EVALUATION


In this section, we designed several experiments to evaluate the CrystalTac family across a variety of task targets. Initially, the performance of CrystalTac with different sensing mechanisms was verified through three functional tasks. As C-Sight has been tested on tactile reconstruction task <cit.>, it, along with the similar C-SighTac, was not further evaluated. Instead, the focus was on C-Tac, Vi-C-Tac, and Vi-C-Sight, which were selected for object recognition, object and texture hybrid recognition, and see-through-skin exploration, respectively. Additionally, two further evaluations were conducted on the manufacturing cost and customisation flexibility of CrystalTac.







 §.§ Object Recognition



The C-Tac with single-layer dot markers was used to achieve the object recognition task. This sensor features a black skin to prevent external light interference and a white marker pattern that is sensitive to mapping skin deformation. As illustrated in Fig. <ref> (A.a), six print parts were selected, including a dot, ring, sphere, curve, waves and multiple dots. The difference between the reference image and the image after contact can indicate both the location and extent of deformation. For a marker that has been displaced or deformed, its original position is indicated in red, while the final position is indicated in blue; the absence of these colours suggests that the marker remained stationary. For instance, when the hollow ring is in contact, only the peripheral markers of C-Tac are displaced, whereas the centrally located markers are the most displaced when for the sphere.


Subsequently, a total of 1200 images were collected, with 200 images per class, to train the object recognition models. A Densenet121 <cit.> model was employed for data inference, and the evaluation results are summarised in Fig. <ref>(A.b). With a test accuracy of 98.3471%, the trained model correctly identified 119 out of 121 images in the test set, demonstrating C-Tac's capability to characterise tactile information from physical contact based on the MDM mechanism.









 §.§ Object and Texture Hybrid Recognition



To evaluate the MDM+MFM mechanism provided by Vi-C-Tac, a hybrid recognition experiment was designed, with the setup illustrated in Fig. <ref> (B.a). The experiment involved three different fabrics-blue cotton, glossy chemical fibre, and rough hemp-as well as three print parts: curve, waves, and dots. By wrapping the fabric around the objects, visual information alone was insufficient to distinguish the print objects, though it allowed for capturing the fine texture of the fabric. Thereby, the markers were utilised to capture tactile features, enabling the analysis of the shape of the underlying parts. A Vi-C-Tac system with double-layer dot markers was employed. A total of nine permutations between these fabrics and parts were tested, as shown in Fig. <ref>(B.b). The enlarged local details revealed distinct textures: cotton fabric displayed clear horizontal grain, chemical fibre exhibited point textures with strong reflective properties, while the features of hemp were less distinct due to its rough characteristics. The double-layer markers were discernible within the view, with non-overlapping parts of the markers differentiated by colour contrast.











For each permutation case, 200 images were collected, resulting in a total of 1800 images for model training. By adding two separate head structures following the output of Densenet121, two classifiers for object recognition and texture recognition were implemented in a decoupled manner. The test results, shown in Fig. <ref>(B.c), indicate identification accuracies of 100% (182/182) for object recognition and 99.45% (181/182) for texture recognition. Both visual and tactile features were successfully captured by Vi-C-Tac, thanks to the MDM+MFM mechanism.















 §.§ See-through-Skin Exploration



A Vi-C-Sight was employed to conduct see-through-skin exploration, utilising its IMM+MFM mechanism. The elastomer, made up entirely of Agilus30 Clear, functions as a light-conductive plate <cit.>, enabling the total internal reflection effect to achieve the fusion of proximity-tactile sensing. Deformed areas are differentiated in brightness from undeformed areas, while other regions remain highly transparent, thereby facilitating the see-through-skin capability. As shown in Fig. <ref>(A.a), fine textures on the objects' surfaces are clearly captured. For instance, fingerprints are visible without interference from other marker patterns, allowing Vi-C-Sight to produce such clear imaging that the entire outline of the finger is recognisable against a darker background. A distinct line of demarcation marks the contact areas where textures are mapped onto the elastomer surface, akin to glass. The fabric fibres can embed themselves into the elastomer during contact, enhancing the TIR effect. This effect can be observed in the geometry of criss-cross cotton, the fibre orientation of twill denim, and the level of embroidery deformation, which can all be mapped to variations in pixel intensity. Unlike Vi-C-Tac, the MFM mechanism of Vi-C-Sight inherently couples visual and tactile information through changes in the luminance of the pixels, similar to TIRgel <cit.>.








In Fig. <ref>(A.b), an embossed print base with a human figure was wrapped in a skeleton fabric. Vi-C-Sight was used to explore the surface of these combined parts and generate an exploration map. Due to the sparse nature of the fibres, the fabric was not as visually distinctive as the opaque printed adhesive pattern also present. However, the figure pattern of the embossed print base beneath, where the two overlap, is visible in the reddish colour of the embossed areas, indicating a higher degree of deformation and a more pronounced fabric texture in the immediate vicinity.














 §.§ Manufacturing Cost Evaluation of CrystalTac Family



A similar evaluation of manufacturing costs to <cit.> has been applied to the CrystalTac family, with the results summarised in Table <ref>. For C-Tac with single-layer dot markers, Vi-C-Tac with double-layer dot markers, and Vi-C-Sight, all of which utilise a Digit base, C-Tac is slightly larger in size and volume due to the additional layer of opaque skin. C-Sight and C-SighTac, which use a customised base, have similar volumes to the others due to their rectangular shape. This difference in shape is reflected in the unit print speed (T/V) and unit material consumption cost (C/V), as C-Tac/Vi-C-Tac/Vi-C-Sight possess complex curved surfaces with overhangs, necessitating additional support materials. This requirement reduces the unit print speed and increases the unit material consumption cost. For instance, the T/V and C/V for C-Tac/Vi-C-Tac/Vi-C-Sight are approximately 11.5 min/cm^3 and 0.73 £/cm^3, respectively, which is higher compared to C-Sight/C-SighTac, with around 10 min/cm^3 and 0.685 £/cm^3. Therefore, it can be concluded that the closer the shape of a CrystalTac component is to a rectangle, the more efficient it is to print through monolithic manufacturing, thereby lowering the manufacturing cost.













As shown in Fig. <ref>(B), further evaluations have been conducted concerning different sensor capacities within a single print batch. For all five types of CrystalTac sensors, the maximum number along the X direction on the print tray is 8, while it varies in the Y direction-6 for C-Tac/Vi-C-Tac/Vi-C-Sight and 8 for C-Sight/C-SighTac-resulting in maximum capacities of 48 and 64, respectively. As the print batch capacity approaches these maximum values, both the average printing time and cost decrease sharply until reaching a batch capacity of 8, after which they stabilise at a slightly lower level. This pattern aligns with the conclusions obtained in <cit.>. The underlying reason is that when the capacity exceeds 8, additional motion overheads are required in the Y direction for the extra columns. For example, in the case of C-Tac, when the batch capacity is 48, the average print time and cost are only 9.08 minutes and £2.43, representing decreases of 87.73% and 48.05% from 74 minutes and £4.678, respectively, when the batch capacity is 1.
















 §.§ Customised Flexibility Evaluation of CrystalTac Family



As discussed, customised flexibility is an essential metric for reducing design complexity. With monolithic manufacturing, as long as the CAD model of CrystalTac is designed, it can be manufactured directly without any additional processes. This capability allows for the realisation of many complex designs to meet the requirements of specific tasks.









  §.§.§ Customised Internal Structure
 As shown in Fig. <ref>(A), for designs that only require tactile information, light robustness is enhanced by using Agilus30 Black for the outer skin, which blocks external ambient light. Additionally, the 2.5D markers made of soft material can deform to provide richer tactile information. In contrast, markers made of rigid material maintain their structure while still moving with deformation, enabling the creation of more complex geometries. Further design examples of novel markers and complicated structures can be seen in Supplementary <ref> and Supplementary <ref>.





  §.§.§ Customised External Structure
 The external structure plays a crucial role in tactile sensing, particularly in dynamic interactions. As shown in Fig. <ref>(B), the anti-slip texture can increase friction between the sensor surface and external objects, effectively reducing undesirable sliding and providing a solid foundation for robust gripping tasks. This texture also protects the contact module body by being wear-resistant, thus extending the service life and reducing usage costs. Furthermore, whiskers can enhance the sensitivity of built-in markers. By leveraging the principle of micro-leverage effect, tactile information such as shear or pressure received at the external end is magnified at the internal end.












  §.§.§ Customised Shapes and Sizes
 The ability to modify the size and shape of the contact module is also a key feature. Compared to the traditional flat shape, spherical and cylindrical surfaces perform better in in-hand manipulation tasks since they can obtain more sensing area. However, these surfaces are more difficult to manufacture, especially internal lenses, which often require complicated processes that challenge traditional manufacturing methods. As shown in Fig. <ref>(C), a hemispherical sample and two additional samples with cone-shaped and cylindrical structures were manufactured. Their built-in lenses are consistent with the overall shapes, with elastomers all 2mm thick, matching the lens thickness.











§ CONCLUSIONS AND FUTURE WORK


In this work, we introduce a sensor family named CrystalTac, comprising five branches with unique designs. Building on the previous work of C-Sight, the monolithic manufacturing technique has demonstrated its capability to address the existing challenges in the traditional design and creation process, showing potential to become a universal manufacturing technology for VBTSs. Accordingly, we summarised the sensing mechanisms of currently known VBTSs, including IMM, MDM, MFM, and multi-mechanism fusion. Based on these typical mechanisms, five types of CrystalTac sensors were fabricated using monolithic manufacturing: C-Tac, C-Sight, C-SighTac, Vi-C-Tac, and Vi-C-Sight. Subsequent functional experiments demonstrated that the CrystalTac sensors perform well and meet their design targets with unique tactile sensing mechanisms. Additionally, monolithic manufacturing has led to significant improvements in manufacturing costs and design flexibility, often constrained by conventional methods of manufacturing and assembly.



The CrystalTac family can be regarded as an initial template, with no strict parameter limitations for each sensor detail, thereby encouraging the community to view it as a foundation for further development. The significance of this work lies in demonstrating the capability of monolithic manufacturing to produce VBTSs with various tactile sensing mechanisms, providing confidence and inspiration to other researchers in the tactile robotics field. In future work, we aim to advance the new CrystalTac series by enhancing the capabilities of monolithic manufacturing in terms of production quality and efficiency, as well as multi-material printing for VBTSs. These technologies can be seamlessly integrated with tactile sensory enhancements in dexterous hands to perform tasks such as human-computer interaction or dexterous manipulation.






§ ACKNOWLEDGMENT

The authors would like to thank Andrew Stinchcombe, Ugnius Bajarunas, and Tom Barnes for their assistance and valuable advice in technique development and hardware fabrication.






 §.§ Author Contributions
 
Conceptualisation, W.F. and H.L.; hardware, W.F. and H.L.; software, W.F.; writing—original draft preparation, W.F.; writing—review and editing, H.L. and D.Z.; visualisation, W.F. and H.L.; supervision, D.Z.; project administration, D.Z. All authors have read and agreed to the published version of the manuscript.




 §.§ Funding

Wen Fan and Haoran Li are partially funded by the CSC scholarship. The authors would like to acknowledge the Royal Society Research Grant (RGS/R1/221122).





 §.§ Conflicts of Interest


The author(s) declare(s) that there is no conflict of interest regarding the publication of this article.





 §.§ Data Availability


Please contact the authors to obtain data, including the design details of the CrystalTac family.











§ SUPPLEMENTARY MATERIALS





 §.§ Optimisation on Sub-components Manufacturing of CrystalTac




Several extended experiments have been conducted to explore the optimal range of sub-component attributes in CrystalTac, including the lens and elastomer.











  §.§.§ Optimisation for CrystalTac Lens Manufacturing
 


VeroClear is highly effective for manufacturing lens components, being a nearly colourless material that exhibits dimensional stability. Its properties are comparable to those of Polymethyl methacrylate (PMMA), commonly known as acrylic. Its upgraded version, VeroUltraClear, offers 95% light transmission and improves upon VeroClear with higher clarity, transparency, and a lower yellow index. We produced a batch of lens samples using VeroClear, which demonstrated good transparency. However, there were still strip-shaped textures present on the surface of the printed lenses, as depicted in Fig. <ref>(A).

The potential cause of these textures could be the alternate superimposition of the axial movements Px and Py of the print head in the X/Y direction. When two adjacent movements, Px1 and Px2, separated by ΔPy, produce a deviation ΔPz in the vertical height Pz possibly due to a mechanical error or the state of material polymerisation-an uneven texture is created between the Px1 and Px2 trajectories. These textures complicate the processes of refraction and reflection as incident light passes through the material. Assuming that the normal incidence angles of three parallel beams on the lens surface are N1, N2, and N3, with angles of incidence θ1, θ2, and θ3, respectively, the angles of reflection for their reflected light will be θr1, θr2, and θr3. It is evident that these reflected beams are no longer parallel. Similarly, for light refraction, which follows Snell's law as shown in Eq. <ref>, the refractive indices N_lens/N_air of the two media (lens and air) are greater than 1. Therefore, the refraction angles θl1, θl2, and θl3 will be slightly smaller than θ1, θ2, and θ3, and these refracted beams will also not be parallel to each other.



    sinθ/sinθ l = N_lens/N_air




When applied to CrystalTac, the aforementioned phenomenon of printed lenses leads to two problems. Firstly, internal illumination can cause irregular reflections on the lens surface. Secondly, images captured through the lens may exhibit optical distortion, as demonstrated in Fig. <ref>(B). Both issues significantly impair the quality of the tactile data acquired by CrystalTac. The first problem can be addressed by optimising the design of the illumination system. The second issue, however, differs from common optical lens distortions such as barrel distortion, pincushion distortion, and mustache distortion, which can be corrected using distortion parameters derived from chessboard pattern calibration. This optical distortion arises from the randomly shaped surface of the lens, leading to unpredictable outcomes that cannot be corrected through simple parameter adjustments.







This issue is evidently a consequence of the operating principles of pp, which, despite having a surface finish superior to most other 3D printing technology. To avoid redundant post-processes such as mechanical polishing, two methods have been tested to determine if the imaging quality of the printed lens surface can be improved:





  * Adjusting Printing Thickness: 


As shown in Fig. <ref>(A.a), four lenses with different thicknesses were printed: 1mm, 2mm, 3mm, and 5mm. All lenses have planar dimensions of 30mm x 30mm. For the control group, four additional acrylic lenses of the same size were fabricated using laser cutting. As indicated in Fig. <ref>(A.c), by placing a QR code pattern beneath the lenses, a camera positioned above can capture the pattern's imaging through the lenses. To quantitatively analyse the optical distortion level, the Structural Similarity Index Measure (SSIM) was introduced as the metric to evaluate image similarity. This approach assesses similarity by considering a combination of factors, including image luminance, contrast, and structure. To avoid the influence of surface texture direction, each printed lens was placed over the QR pattern along both the X and Y directions, capturing two image samples. From Fig. <ref>(A.d), the similarity between images taken through printed lenses and acrylic lenses is around 86% for thicknesses less than 3mm. However, as thickness increases to 5mm, similarity decreases to 83%. This decrease is primarily attributed to the lower light transmittance of VeroClear compared to acrylic, with excessive thickness resulting in darker images through the printed lenses. The highest similarity score, approximately 87.5%, is attained with lenses 1mm thick, suggesting that some distortion may be due to surface texture. Similarly, when comparing the similarity to the actual pattern, there is an overall decrease of about 7%, indicating that acrylic lenses also contribute to some degree of distortion.
 


  * Applying Acrylic Spray: 

As illustrated in Fig. <ref>(B), applying acrylic spray can fill the uneven texture on the lens surface with liquid droplets. Once these droplets are cured, which can enhance the overall finish of the lens, offering a more convenient and efficient alternative to physical polishing using sanding equipment. In our test, all four printed lenses were placed together and a canister acrylic spray was applied 1 m above them for around 2 s. After waiting for an hour under ventilated conditions, the liquid acrylic layer on their surface was fully cured and ready for testing, with all the settings being the same as in the previous. Also from Fig. <ref>(D), the similarity between sprayed lens imaging and acrylic lens imaging is down overall by 3-5% compared to printed lens imaging without post-process. However, the similarity to the real pattern has been improved, reaching a maximum of 86% with lenses 3mm thick. The improvement in the impact of surface texture due to the application of the spray is significant, as indicated by the reduced disparity in similarity values across the X and Y directions for lenses of all thicknesses.


As illustrated in Fig. <ref>(A.b), applying acrylic spray can fill the uneven texture on the lens surface with liquid droplets. Once cured, these droplets enhance the overall finish of the lens, offering a more convenient and efficient alternative to physical polishing with sanding equipment. In our test, all four printed lenses were placed together, and a canister of acrylic spray was applied from a distance of 1m for approximately 2 seconds. After curing for an hour in ventilated conditions, the liquid acrylic layer on the surface was ready for testing, with all settings identical to the previous tests. As shown in Fig. <ref>(A.d), the similarity between images taken through sprayed lenses and acrylic lenses decreased overall by 3-5% compared to images from printed lenses without post-processing. However, the similarity to the real pattern improved, reaching a maximum of 86% with 3mm thick lenses. The improvement in the impact of surface texture due to the spray application is significant, as indicated by the reduced disparity in similarity values across the X and Y directions for lenses of all thicknesses.






In summary, altering the printing thickness and applying acrylic spray both impact the imaging quality of printed lenses. Considering various factors, a thickness range of 2-3mm is optimal for printed lenses, providing a balance between structural strength and satisfactory imaging performance. Building on this foundation, the application of acrylic spray improves the surface finish of the printed lens.







  §.§.§ Optimisation for CrystalTac Elastomer Manufacturing
 


Most VBTSs require a transparent, silicone-like elastomer as the core material to facilitate technologies such as IMM, MDM, MFM, and multi-mechanism fusion. Agilus30 Clear, due to its transparent and flexible texture, is suitable for replacing silicone. However, the Shore hardness of pure Agilus30 is 30A, which is too hard for designs requiring a softer elastomer. For instance, GelSight typically uses a Shore hardness range of 5-20A <cit.>. By incorporating the multi-layer grid structure proposed in MagicTip <cit.>, the stiffness of the printed elastomer can be further reduced. To assess the impact of this structure on imaging quality, five test samples were manufactured. These samples shared the same X/Y dimensions of 30mm x 30mm for the elastomer, but varied in height: 2mm, 5mm, 10mm, 15mm, and 20mm, as shown in Fig. <ref>(B.a). The skin layer's thickness was set to 0.5mm to balance a soft texture with stable strength. The same QR code pattern and SSIM metric used for evaluating the imaging quality of printed lenses, as depicted in Fig. <ref>(A.b), were employed.



As displayed in Fig. <ref>(B.b), as the height of the printed elastomer increases, the imaging quality decreases in two ways. Firstly, the image tone gradually becomes darker and tends toward a darker yellow. This is primarily due to the support material filled in the internal core, which has lower light transmission than the Agilus30 material. Secondly, the imaging quality deteriorates, leading to a gradual loss of detail in the pattern beneath. This trend is confirmed by Fig. <ref>(B.c), where the similarity between the printed elastomer imaging and the real pattern is 76% at a height of 2mm but gradually drops to around 52% when the height exceeds 10mm. We also found that overall height affects stiffness to a certain extent, even with the same outer skin thickness, particularly when the height is less than 5mm. This is mainly because a reduction in elastomer height reduces the number of layers in the embedded grids, and too few layers limit the range of deformation that can occur in the support core.


For CrystalTac, to optimise the imaging quality and elasticity of the printed elastomer with the multi-layer grid structure, the suitable height ranges are 2mm to 5mm, where thinner layers lead to better transparency and thicker layers result in softer stiffness.





 §.§ Marker Design for CrystalTac Family












  * Dot Marker: 
Dot markers are among the most common patterns in VBTSs <cit.>. Regarding shape, dot markers can include both cylinders with slender thicknesses and spheres with symmetrical dimensions. The overall marker array distribution can be categorised into either uniform or random spatial arrangements. Furthermore, dot markers can be transformed into pin-shaped markers by integrating them with the skin, a feasibility demonstrated in <cit.>. As shown in Fig. <ref>(A), if a rigid material such as Vero is used for dot marker manufacturing, the printed pattern will exhibit properties of a 2D marker. However, if flexible materials like Agilus30 or Tango are used, the markers have the potential to deform into various shapes. This deformation is an additional feature, categorising them within the realm of 2.5D markers. The precise pattern of the markers is dictated by the design specifications established during CAD modelling, which involves considerations of both the number and density distribution of markers, allowing for the customisation of either a uniform or a random distribution.





  * Double-layer Marker: When the elastomer contacts the object, the extent of elastic deformation decreases along the longitudinal axis. The double marker approach utilises this inhomogeneity by capturing the difference in distribution between the upper and lower pattern layers to map tactile information <cit.>. As shown in Fig. <ref>(B), this contrasting disparity typically enhances specific dynamic tactile characteristics, such as shear motion, compared to a single-layer marker configuration. The fabrication process for double-layer markers is similar to that of dot markers, with the primary distinction being the inclusion of an additional marker layer in the CAD modelling. Moreover, the construction principles of spatial structures comprising more than two layers remain consistent. By increasing the number of layers, the extreme case is that the different layers of the marker transition from a discrete state of separation to a continuous state of interconnection, resembling a top-to-bottom cylinder. By controlling the gradient distribution of colours, it should be possible to provide a more fine-grained mapping of the tactile information. All of the above variants can be easily implemented through monolithic manufacturing.






  * Voronoi Marker:  Similar to the lattice pattern in <cit.>, the Voronoi marker is designed with a continuous pattern distribution in a square layout as shown in Fig. <ref>(C). Each edge of such a square marker can be viewed as a collection of dot markers, thus improving spatial resolution compared to the standard dot marker, as investigated in continuous marker pattern (CMP) <cit.>. Using monolithic manufacturing, Voronoi markers can be seamlessly integrated into the fabrication of the skin or embedded at any specific spatial layer within the elastomer, according to a customised design. However, the marker design must adhere to geometric principles, where each tessellation cell of the Voronoi pattern should be a polygon, such as a triangle, square, or hexagon.



  * Coordinate Marker: Most marker patterns share two characteristics: each marker typically has a regular spatial shape, such as a sphere or dot, and multiple markers are grouped into an array to map force, shear, or other contact information. These characteristics imply that a single marker plays a limited role in the overall array due to the relatively low marker density. Through monolithic manufacturing, we introduce the coordinate marker, as illustrated in Fig. <ref>(d). Each individual marker in the array can independently infer the 6D pose of its localised elastomer. This capability stems from the distinct design of each marker, akin to a three-dimensional pointer within a gyroscope, making it sensitive to both rotational and translational movements. Consequently, markers characterised by complex spatial structures, as opposed to regular shapes, can be classified as stereo markers(including proposed coordinate markers). To our knowledge, the design and creation of such stereo markers are achievable exclusively through monolithic manufacturing.













 §.§ Vision and Illumination Design for CrystalTac Family











 §.§ Printing Sample of Customised CraystalTac














